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


eigenpy
Author(s): Justin Carpentier, Nicolas Mansard
autogenerated on Sat Nov 2 2024 02:14:45