Merge pull request #325 from ManifoldFR/wj/move-std-vector

Copy std-vector and std-map from Pinocchio

Author: jcarpent

.pre-commit-config.yaml

@@ -5,7 +5,7 @@ repos:
     rev: v15.0.4
     hooks:
     -   id: clang-format
-        args: [--style=Google]
+        args: ['--style={BasedOnStyle: Google, SortIncludes: false}']
 -   repo: https://github.com/pre-commit/pre-commit-hooks
     rev: v4.4.0
     hooks:

CMakeLists.txt

@@ -135,6 +135,7 @@ set(${PROJECT_NAME}_HEADERS
     include/eigenpy/exception.hpp
     include/eigenpy/scalar-conversion.hpp
     include/eigenpy/expose.hpp
+    include/eigenpy/copyable.hpp
     include/eigenpy/details.hpp
     include/eigenpy/fwd.hpp
     include/eigenpy/eigen-allocator.hpp
@@ -154,6 +155,9 @@ set(${PROJECT_NAME}_HEADERS
     include/eigenpy/user-type.hpp
     include/eigenpy/ufunc.hpp
     include/eigenpy/register.hpp
+    include/eigenpy/std-map.hpp
+    include/eigenpy/std-vector.hpp
+    include/eigenpy/pickle-vector.hpp
     include/eigenpy/stride.hpp
     include/eigenpy/swig.hpp
     include/eigenpy/version.hpp)
@@ -194,6 +198,7 @@ set(${PROJECT_NAME}_SOURCES
     src/angle-axis.cpp
     src/quaternion.cpp
     src/geometry-conversion.cpp
+    src/std-vector.cpp
     src/version.cpp)
 
 add_library(${PROJECT_NAME} SHARED ${${PROJECT_NAME}_SOURCES}

cmake

@@ -0,0 +1,30 @@
+//
+// Copyright (c) 2016-2021 CNRS INRIA
+//
+
+#ifndef __eigenpy_utils_copyable_hpp__
+#define __eigenpy_utils_copyable_hpp__
+
+#include <boost/python.hpp>
+
+namespace eigenpy {
+
+namespace bp = boost::python;
+
+///
+/// \brief Add the Python method copy to allow a copy of this by calling the
+/// copy constructor.
+///
+template <class C>
+struct CopyableVisitor : public bp::def_visitor<CopyableVisitor<C> > {
+  template <class PyClass>
+  void visit(PyClass& cl) const {
+    cl.def("copy", &copy, bp::arg("self"), "Returns a copy of *this.");
+  }
+
+ private:
+  static C copy(const C& self) { return C(self); }
+};
+}  // namespace eigenpy
+
+#endif  // ifndef __eigenpy_utils_copyable_hpp__

include/eigenpy/copyable.hpp

@@ -232,6 +232,22 @@ struct EigenAllocator {
 };
 
 #if EIGEN_VERSION_AT_LEAST(3, 2, 0)
+/// @brief Check if we need to allocate @tparam MatType to convert @param
+/// pyArray.
+/// @details do not allocate if:
+/// want row-major & data C-contiguous OR
+/// want col-major & data F-contiguous OR
+/// you want a compile-time vector
+/// in these cases, data layout fits desired view layout
+template <typename MatType>
+inline bool is_arr_layout_compatible_with_mat_type(PyArrayObject *pyArray) {
+  bool is_array_C_cont = PyArray_IS_C_CONTIGUOUS(pyArray);
+  bool is_array_F_cont = PyArray_IS_F_CONTIGUOUS(pyArray);
+  return (MatType::IsRowMajor && is_array_C_cont) ||
+         (!MatType::IsRowMajor && is_array_F_cont) ||
+         MatType::IsVectorAtCompileTime;
+}
+
 template <typename MatType, int Options, typename Stride>
 struct EigenAllocator<Eigen::Ref<MatType, Options, Stride> > {
   typedef Eigen::Ref<MatType, Options, Stride> RefType;
@@ -255,16 +271,9 @@ struct EigenAllocator<Eigen::Ref<MatType, Options, Stride> > {
     const int pyArray_type_code = EIGENPY_GET_PY_ARRAY_TYPE(pyArray);
     const int Scalar_type_code = Register::getTypeCode<Scalar>();
     if (pyArray_type_code != Scalar_type_code) need_to_allocate |= true;
-    if ((MatType::IsRowMajor && (PyArray_IS_C_CONTIGUOUS(pyArray) &&
-                                 !PyArray_IS_F_CONTIGUOUS(pyArray))) ||
-        (!MatType::IsRowMajor && (PyArray_IS_F_CONTIGUOUS(pyArray) &&
-                                  !PyArray_IS_C_CONTIGUOUS(pyArray))) ||
-        MatType::IsVectorAtCompileTime ||
-        (PyArray_IS_F_CONTIGUOUS(pyArray) &&
-         PyArray_IS_C_CONTIGUOUS(pyArray)))  // no need to allocate
-      need_to_allocate |= false;
-    else
-      need_to_allocate |= true;
+    bool incompatible_layout =
+        !is_arr_layout_compatible_with_mat_type<MatType>(pyArray);
+    need_to_allocate |= incompatible_layout;
     if (Options !=
         Eigen::Unaligned)  // we need to check whether the memory is correctly
                            // aligned and composed of a continuous segment
@@ -365,16 +374,9 @@ struct EigenAllocator<const Eigen::Ref<const MatType, Options, Stride> > {
     const int Scalar_type_code = Register::getTypeCode<Scalar>();
 
     if (pyArray_type_code != Scalar_type_code) need_to_allocate |= true;
-    if ((MatType::IsRowMajor && (PyArray_IS_C_CONTIGUOUS(pyArray) &&
-                                 !PyArray_IS_F_CONTIGUOUS(pyArray))) ||
-        (!MatType::IsRowMajor && (PyArray_IS_F_CONTIGUOUS(pyArray) &&
-                                  !PyArray_IS_C_CONTIGUOUS(pyArray))) ||
-        MatType::IsVectorAtCompileTime ||
-        (PyArray_IS_F_CONTIGUOUS(pyArray) &&
-         PyArray_IS_C_CONTIGUOUS(pyArray)))  // no need to allocate
-      need_to_allocate |= false;
-    else
-      need_to_allocate |= true;
+    bool incompatible_layout =
+        !is_arr_layout_compatible_with_mat_type<MatType>(pyArray);
+    need_to_allocate |= incompatible_layout;
     if (Options !=
         Eigen::Unaligned)  // we need to check whether the memory is correctly
                            // aligned and composed of a continuous segment

include/eigenpy/eigen-allocator.hpp

@@ -0,0 +1,46 @@
+//
+// Copyright (c) 2019-2020 CNRS INRIA
+//
+
+#ifndef __eigenpy_utils_pickle_vector_hpp__
+#define __eigenpy_utils_pickle_vector_hpp__
+
+#include <boost/python.hpp>
+#include <boost/python/stl_iterator.hpp>
+#include <boost/python/tuple.hpp>
+
+namespace eigenpy {
+///
+/// \brief Create a pickle interface for the std::vector
+///
+/// \tparam VecType Vector Type to pickle
+///
+template <typename VecType>
+struct PickleVector : boost::python::pickle_suite {
+  static boost::python::tuple getinitargs(const VecType&) {
+    return boost::python::make_tuple();
+  }
+
+  static boost::python::tuple getstate(boost::python::object op) {
+    return boost::python::make_tuple(
+        boost::python::list(boost::python::extract<const VecType&>(op)()));
+  }
+
+  static void setstate(boost::python::object op, boost::python::tuple tup) {
+    if (boost::python::len(tup) > 0) {
+      VecType& o = boost::python::extract<VecType&>(op)();
+      boost::python::stl_input_iterator<typename VecType::value_type> begin(
+          tup[0]),
+          end;
+      while (begin != end) {
+        o.push_back(*begin);
+        ++begin;
+      }
+    }
+  }
+
+  static bool getstate_manages_dict() { return true; }
+};
+}  // namespace eigenpy
+
+#endif  // ifndef __eigenpy_utils_pickle_vector_hpp__

include/eigenpy/pickle-vector.hpp

@@ -294,8 +294,8 @@ class QuaternionVisitor
     return q;
   }
 
-  static Quaternion* FromTwoVectors(const Eigen::Ref<Vector3> u,
-                                    const Eigen::Ref<Vector3> v) {
+  static Quaternion* FromTwoVectors(const Eigen::Ref<const Vector3> u,
+                                    const Eigen::Ref<const Vector3> v) {
     Quaternion* q(new Quaternion);
     q->setFromTwoVectors(u, v);
     return q;
@@ -308,12 +308,12 @@ class QuaternionVisitor
 
   static Quaternion* DefaultConstructor() { return new Quaternion; }
 
-  static Quaternion* FromOneVector(const Eigen::Ref<Vector4> v) {
+  static Quaternion* FromOneVector(const Eigen::Ref<const Vector4> v) {
     Quaternion* q(new Quaternion(v[3], v[0], v[1], v[2]));
     return q;
   }
 
-  static Quaternion* FromRotationMatrix(const Eigen::Ref<Matrix3> R) {
+  static Quaternion* FromRotationMatrix(const Eigen::Ref<const Matrix3> R) {
     Quaternion* q(new Quaternion(R));
     return q;
   }

include/eigenpy/quaternion.hpp

@@ -0,0 +1,64 @@
+/// Copyright (c) 2016-2022 CNRS INRIA
+/// This file was taken from Pinocchio (header
+/// <pinocchio/bindings/python/utils/std-vector.hpp>)
+///
+
+#ifndef __eigenpy_utils_map_hpp__
+#define __eigenpy_utils_map_hpp__
+
+#include <boost/python/suite/indexing/map_indexing_suite.hpp>
+
+namespace eigenpy {
+namespace details {
+template <typename Container>
+struct overload_base_get_item_for_std_map
+    : public boost::python::def_visitor<
+          overload_base_get_item_for_std_map<Container> > {
+  typedef typename Container::value_type value_type;
+  typedef typename Container::value_type::second_type data_type;
+  typedef typename Container::key_type key_type;
+  typedef typename Container::key_type index_type;
+
+  template <class Class>
+  void visit(Class& cl) const {
+    cl.def("__getitem__", &base_get_item);
+  }
+
+ private:
+  static boost::python::object base_get_item(
+      boost::python::back_reference<Container&> container, PyObject* i_) {
+    namespace bp = ::boost::python;
+
+    index_type idx = convert_index(container.get(), i_);
+    typename Container::iterator i = container.get().find(idx);
+    if (i == container.get().end()) {
+      PyErr_SetString(PyExc_KeyError, "Invalid key");
+      bp::throw_error_already_set();
+    }
+
+    typename bp::to_python_indirect<data_type&,
+                                    bp::detail::make_reference_holder>
+        convert;
+    return bp::object(bp::handle<>(convert(i->second)));
+  }
+
+  static index_type convert_index(Container& /*container*/, PyObject* i_) {
+    namespace bp = ::boost::python;
+    bp::extract<key_type const&> i(i_);
+    if (i.check()) {
+      return i();
+    } else {
+      bp::extract<key_type> i(i_);
+      if (i.check()) return i();
+    }
+
+    PyErr_SetString(PyExc_TypeError, "Invalid index type");
+    bp::throw_error_already_set();
+    return index_type();
+  }
+};
+
+}  // namespace details
+}  // namespace eigenpy
+
+#endif  // ifndef __eigenpy_utils_map_hpp__