quaternion.hpp
Go to the documentation of this file.
1 /*
2  * Copyright 2014-2021 CNRS INRIA
3  */
4 
5 #ifndef __eigenpy_quaternion_hpp__
6 #define __eigenpy_quaternion_hpp__
7 
8 #include "eigenpy/eigenpy.hpp"
9 
10 #include <Eigen/Core>
11 #include <Eigen/Geometry>
12 
13 #include "eigenpy/exception.hpp"
14 
15 namespace boost { namespace python { namespace converter {
16 
18  template<typename Quaternion>
19  struct rvalue_from_python_data<Eigen::QuaternionBase<Quaternion> const &>
20  : rvalue_from_python_data_eigen<Quaternion const &>
21  {
22  EIGENPY_RVALUE_FROM_PYTHON_DATA_INIT(Quaternion const &)
23  };
24 
25  template <class Quaternion>
26  struct implicit<Quaternion, Eigen::QuaternionBase<Quaternion> >
27  {
28  typedef Quaternion Source;
29  typedef Eigen::QuaternionBase<Quaternion> Target;
30 
31  static void* convertible(PyObject* obj)
32  {
33  // Find a converter which can produce a Source instance from
34  // obj. The user has told us that Source can be converted to
35  // Target, and instantiating construct() below, ensures that
36  // at compile-time.
37  return implicit_rvalue_convertible_from_python(obj, registered<Source>::converters)
38  ? obj : 0;
39  }
40 
41  static void construct(PyObject* obj, rvalue_from_python_stage1_data* data)
42  {
43  void* storage = ((rvalue_from_python_storage<Target>*)data)->storage.bytes;
44 
45  arg_from_python<Source> get_source(obj);
46  bool convertible = get_source.convertible();
47  BOOST_VERIFY(convertible);
48 
49  new (storage) Source(get_source());
50 
51  // record successful construction
52  data->convertible = storage;
53  }
54  };
55 
56 }}} // namespace boost::python::converter
57 
58 namespace eigenpy
59 {
60 
61  class ExceptionIndex : public Exception
62  {
63  public:
64  ExceptionIndex(int index,int imin,int imax) : Exception("")
65  {
66  std::ostringstream oss; oss << "Index " << index << " out of range " << imin << ".."<< imax <<".";
67  message = oss.str();
68  }
69  };
70 
71  namespace bp = boost::python;
72 
73  template<typename QuaternionDerived> class QuaternionVisitor;
74 
75  template<typename Scalar, int Options>
76  struct call< Eigen::Quaternion<Scalar,Options> >
77  {
78  typedef Eigen::Quaternion<Scalar,Options> Quaternion;
79  static inline void expose()
80  {
82  }
83 
84  static inline bool isApprox(const Quaternion & self, const Quaternion & other,
85  const Scalar & prec = Eigen::NumTraits<Scalar>::dummy_precision())
86  {
87  return self.isApprox(other,prec);
88  }
89  };
90 
91  template<typename Quaternion>
92  class QuaternionVisitor
93  : public bp::def_visitor< QuaternionVisitor<Quaternion> >
94  {
95  typedef Eigen::QuaternionBase<Quaternion> QuaternionBase;
96 
97  typedef typename QuaternionBase::Scalar Scalar;
98  typedef typename Quaternion::Coefficients Coefficients;
99  typedef typename QuaternionBase::Vector3 Vector3;
100  typedef typename Eigen::Matrix<Scalar,4,1> Vector4;
101  typedef typename QuaternionBase::Matrix3 Matrix3;
102 
103  typedef typename QuaternionBase::AngleAxisType AngleAxis;
104 
105  BOOST_PYTHON_FUNCTION_OVERLOADS(isApproxQuaternion_overload,call<Quaternion>::isApprox,2,3)
106 
107  public:
108 
109  template<class PyClass>
110  void visit(PyClass& cl) const
111  {
112  cl
113  .def(bp::init<>(bp::arg("self"),"Default constructor"))
114  .def(bp::init<Matrix3>((bp::arg("self"),bp::arg("R")),
115  "Initialize from rotation matrix.\n"
116  "\tR : a rotation matrix 3x3."))
117  .def(bp::init<Vector4>((bp::arg("self"),bp::arg("vec4")),
118  "Initialize from a vector 4D.\n"
119  "\tvec4 : a 4D vector representing quaternion coefficients in the order xyzw."))
120  .def(bp::init<AngleAxis>((bp::arg("self"),bp::arg("aa")),
121  "Initialize from an angle axis.\n"
122  "\taa: angle axis object."))
123  .def(bp::init<Quaternion>((bp::arg("self"),bp::arg("quat")),
124  "Copy constructor.\n"
125  "\tquat: a quaternion."))
126  .def("__init__",bp::make_constructor(&QuaternionVisitor::FromTwoVectors,
127  bp::default_call_policies(),
128  (bp::arg("u"),bp::arg("v"))),
129  "Initialize from two vectors u and v")
130  .def(bp::init<Scalar,Scalar,Scalar,Scalar>
131  ((bp::arg("self"),bp::arg("w"),bp::arg("x"),bp::arg("y"),bp::arg("z")),
132  "Initialize from coefficients.\n\n"
133  "... note:: The order of coefficients is *w*, *x*, *y*, *z*. "
134  "The [] operator numbers them differently, 0...4 for *x* *y* *z* *w*!"))
135 
136  .add_property("x",
137  &QuaternionVisitor::getCoeff<0>,
138  &QuaternionVisitor::setCoeff<0>,"The x coefficient.")
139  .add_property("y",
140  &QuaternionVisitor::getCoeff<1>,
141  &QuaternionVisitor::setCoeff<1>,"The y coefficient.")
142  .add_property("z",
143  &QuaternionVisitor::getCoeff<2>,
144  &QuaternionVisitor::setCoeff<2>,"The z coefficient.")
145  .add_property("w",
146  &QuaternionVisitor::getCoeff<3>,
147  &QuaternionVisitor::setCoeff<3>,"The w coefficient.")
148 
149  .def("isApprox",
151  isApproxQuaternion_overload(bp::args("self","other","prec"),
152  "Returns true if *this is approximately equal to other, within the precision determined by prec."))
153 
154  /* --- Methods --- */
155  .def("coeffs",(const Vector4 & (Quaternion::*)()const)&Quaternion::coeffs,
156  bp::arg("self"),
157  "Returns a vector of the coefficients (x,y,z,w)",
158  bp::return_internal_reference<>())
159  .def("matrix",&Quaternion::matrix,
160  bp::arg("self"),
161  "Returns an equivalent 3x3 rotation matrix. Similar to toRotationMatrix.")
162  .def("toRotationMatrix",&Quaternion::toRotationMatrix,
163 // bp::arg("self"), // Bug in Boost.Python
164  "Returns an equivalent 3x3 rotation matrix.")
165 
166  .def("setFromTwoVectors",&setFromTwoVectors,((bp::arg("self"),bp::arg("a"),bp::arg("b"))),
167  "Set *this to be the quaternion which transforms a into b through a rotation."
168  ,bp::return_self<>())
169  .def("conjugate",&Quaternion::conjugate,
170  bp::arg("self"),
171  "Returns the conjugated quaternion.\n"
172  "The conjugate of a quaternion represents the opposite rotation.")
173  .def("inverse",&Quaternion::inverse,
174  bp::arg("self"),
175  "Returns the quaternion describing the inverse rotation.")
176  .def("setIdentity",&Quaternion::setIdentity,
177  bp::arg("self"),
178  "Set *this to the idendity rotation.",bp::return_self<>())
179  .def("norm",&Quaternion::norm,
180  bp::arg("self"),
181  "Returns the norm of the quaternion's coefficients.")
182  .def("normalize",&Quaternion::normalize,
183  bp::arg("self"),
184  "Normalizes the quaternion *this.")
185  .def("normalized",&Quaternion::normalized,
186  bp::arg("self"),
187  "Returns a normalized copy of *this.")
188  .def("squaredNorm",&Quaternion::squaredNorm,
189  bp::arg("self"),
190  "Returns the squared norm of the quaternion's coefficients.")
191  .def("dot",&Quaternion::template dot<Quaternion>,
192  (bp::arg("self"),bp::arg("other")),
193  "Returns the dot product of *this with an other Quaternion.\n"
194  "Geometrically speaking, the dot product of two unit quaternions corresponds to the cosine of half the angle between the two rotations.")
195  .def("_transformVector",&Quaternion::_transformVector,
196  (bp::arg("self"),bp::arg("vector")),
197  "Rotation of a vector by a quaternion.")
198  .def("vec",&vec,
199  bp::arg("self"),
200  "Returns a vector expression of the imaginary part (x,y,z).")
201  .def("angularDistance",
202 // (bp::arg("self"),bp::arg("other")), // Bug in Boost.Python
203  &Quaternion::template angularDistance<Quaternion>,
204  "Returns the angle (in radian) between two rotations.")
205  .def("slerp",&slerp,bp::args("self","t","other"),
206  "Returns the spherical linear interpolation between the two quaternions *this and other at the parameter t in [0;1].")
207 
208  /* --- Operators --- */
209  .def(bp::self * bp::self)
210  .def(bp::self *= bp::self)
211  .def(bp::self * bp::other<Vector3>())
212  .def("__eq__",&QuaternionVisitor::__eq__)
213  .def("__ne__",&QuaternionVisitor::__ne__)
214  .def("__abs__",&Quaternion::norm)
215  .def("__len__",&QuaternionVisitor::__len__).staticmethod("__len__")
216  .def("__setitem__",&QuaternionVisitor::__setitem__)
217  .def("__getitem__",&QuaternionVisitor::__getitem__)
218  .def("assign",&assign<Quaternion>,
219  bp::args("self","quat"),
220  "Set *this from an quaternion quat and returns a reference to *this.",
221  bp::return_self<>())
222  .def("assign",(Quaternion & (Quaternion::*)(const AngleAxis &))&Quaternion::operator=,
223  bp::args("self","aa"),
224  "Set *this from an angle-axis aa and returns a reference to *this.",
225  bp::return_self<>())
226  .def("__str__",&print)
227  .def("__repr__",&print)
228 
229 // .def("FromTwoVectors",&Quaternion::template FromTwoVectors<Vector3,Vector3>,
230 // bp::args("a","b"),
231 // "Returns the quaternion which transform a into b through a rotation.")
232  .def("FromTwoVectors",&FromTwoVectors,
233  bp::args("a","b"),
234  "Returns the quaternion which transforms a into b through a rotation.",
235  bp::return_value_policy<bp::manage_new_object>())
236  .staticmethod("FromTwoVectors")
237  .def("Identity",&Quaternion::Identity,
238  "Returns a quaternion representing an identity rotation.")
239  .staticmethod("Identity")
240  ;
241  }
242  private:
243 
244  template<int i>
245  static void setCoeff(Quaternion & self, Scalar value) { self.coeffs()[i] = value; }
246 
247  template<int i>
248  static Scalar getCoeff(Quaternion & self) { return self.coeffs()[i]; }
249 
250  static Quaternion & setFromTwoVectors(Quaternion & self, const Vector3 & a, const Vector3 & b)
251  { return self.setFromTwoVectors(a,b); }
252 
253  template<typename OtherQuat>
254  static Quaternion & assign(Quaternion & self, const OtherQuat & quat)
255  { return self = quat; }
256 
257  static Quaternion* FromTwoVectors(const Vector3& u, const Vector3& v)
258  {
259  Quaternion* q(new Quaternion); q->setFromTwoVectors(u,v);
260  return q;
261  }
262 
263  static bool __eq__(const Quaternion & u, const Quaternion & v)
264  {
265  return u.coeffs() == v.coeffs();
266  }
267 
268  static bool __ne__(const Quaternion& u, const Quaternion& v)
269  {
270  return !__eq__(u,v);
271  }
272 
273  static Scalar __getitem__(const Quaternion & self, int idx)
274  {
275  if((idx<0) || (idx>=4)) throw eigenpy::ExceptionIndex(idx,0,3);
276  return self.coeffs()[idx];
277  }
278 
279  static void __setitem__(Quaternion& self, int idx, const Scalar value)
280  {
281  if((idx<0) || (idx>=4)) throw eigenpy::ExceptionIndex(idx,0,3);
282  self.coeffs()[idx] = value;
283  }
284 
285  static int __len__() { return 4; }
286  static Vector3 vec(const Quaternion & self) { return self.vec(); }
287 
288  static std::string print(const Quaternion & self)
289  {
290  std::stringstream ss;
291  ss << "(x,y,z,w) = " << self.coeffs().transpose() << std::endl;
292 
293  return ss.str();
294  }
295 
296  static Quaternion slerp(const Quaternion & self, const Scalar t, const Quaternion & other)
297  { return self.slerp(t,other); }
298 
299  public:
300 
301  static void expose()
302  {
303  bp::class_<Quaternion>("Quaternion",
304  "Quaternion representing rotation.\n\n"
305  "Supported operations "
306  "('q is a Quaternion, 'v' is a Vector3): "
307  "'q*q' (rotation composition), "
308  "'q*=q', "
309  "'q*v' (rotating 'v' by 'q'), "
310  "'q==q', 'q!=q', 'q[0..3]'.",
311  bp::no_init)
313 
314  // Cast to Eigen::QuaternionBase and vice-versa
315  bp::implicitly_convertible<Quaternion,QuaternionBase >();
316 // bp::implicitly_convertible<QuaternionBase,Quaternion >();
317  }
318 
319  };
320 
321 } // namespace eigenpy
322 
323 #endif // ifndef __eigenpy_quaternion_hpp__
#define EIGENPY_RVALUE_FROM_PYTHON_DATA_INIT(type)
Eigen::Matrix< Scalar, 4, 1 > Vector4
Definition: quaternion.hpp:100
boost::python::object matrix()
Definition: bnpy.cpp:20
static Quaternion slerp(const Quaternion &self, const Scalar t, const Quaternion &other)
Definition: quaternion.hpp:296
Definition: complex.cpp:7
static void setCoeff(Quaternion &self, Scalar value)
Definition: quaternion.hpp:245
Quaternion::Coefficients Coefficients
Definition: quaternion.hpp:98
static bool __eq__(const Quaternion &u, const Quaternion &v)
Definition: quaternion.hpp:263
static Quaternion & setFromTwoVectors(Quaternion &self, const Vector3 &a, const Vector3 &b)
Definition: quaternion.hpp:250
QuaternionBase::Vector3 Vector3
Definition: quaternion.hpp:99
data
Definition: setup.in.py:38
QuaternionBase::Scalar Scalar
Definition: quaternion.hpp:97
static bool isApprox(const Quaternion &self, const Quaternion &other, const Scalar &prec=Eigen::NumTraits< Scalar >::dummy_precision())
Definition: quaternion.hpp:84
Eigen::QuaternionBase< Quaternion > QuaternionBase
Definition: quaternion.hpp:95
static Quaternion * FromTwoVectors(const Vector3 &u, const Vector3 &v)
Definition: quaternion.hpp:257
static Scalar __getitem__(const Quaternion &self, int idx)
Definition: quaternion.hpp:273
Allows a template specialization.
Definition: expose.hpp:16
Eigen::Quaternion< Scalar, Options > Quaternion
Definition: quaternion.hpp:78
static Vector3 vec(const Quaternion &self)
Definition: quaternion.hpp:286
void expose()
Call the expose function of a given type T.
Definition: expose.hpp:25
static std::string print(const Quaternion &self)
Definition: quaternion.hpp:288
static void __setitem__(Quaternion &self, int idx, const Scalar value)
Definition: quaternion.hpp:279
ExceptionIndex(int index, int imin, int imax)
Definition: quaternion.hpp:64
static Quaternion & assign(Quaternion &self, const OtherQuat &quat)
Definition: quaternion.hpp:254
QuaternionBase::AngleAxisType AngleAxis
Definition: quaternion.hpp:103
static bool __ne__(const Quaternion &u, const Quaternion &v)
Definition: quaternion.hpp:268
QuaternionBase::Matrix3 Matrix3
Definition: quaternion.hpp:101
static Scalar getCoeff(Quaternion &self)
Definition: quaternion.hpp:248
Definition: python.py:1
static void construct(PyObject *obj, rvalue_from_python_stage1_data *data)
Definition: quaternion.hpp:41


eigenpy
Author(s): Justin Carpentier, Nicolas Mansard
autogenerated on Sat Apr 17 2021 02:37:59