quaternion.hpp 12.3 KB
Newer Older
1
/*
Justin Carpentier's avatar
Justin Carpentier committed
2
 * Copyright 2014-2020 CNRS INRIA
3
4
5
6
 */

#ifndef __eigenpy_quaternion_hpp__
#define __eigenpy_quaternion_hpp__
Nicolas Mansard's avatar
Nicolas Mansard committed
7

Justin Carpentier's avatar
Justin Carpentier committed
8
#include "eigenpy/eigenpy.hpp"
9

Nicolas Mansard's avatar
Nicolas Mansard committed
10
11
#include <Eigen/Core>
#include <Eigen/Geometry>
jcarpent's avatar
jcarpent committed
12

13
#include "eigenpy/exception.hpp"
Nicolas Mansard's avatar
Nicolas Mansard committed
14

15
16
17
18
19
20
21
namespace boost { namespace python { namespace converter {

  /// \brief Template specialization of rvalue_from_python_data
  template<typename Quaternion>
  struct rvalue_from_python_data<Eigen::QuaternionBase<Quaternion> const &>
  : rvalue_from_python_data_eigen<Quaternion const &>
  {
22
    EIGENPY_RVALUE_FROM_PYTHON_DATA_INIT(Quaternion const &)
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
  };

  template <class Quaternion>
  struct implicit<Quaternion, Eigen::QuaternionBase<Quaternion> >
  {
    typedef Quaternion Source;
    typedef Eigen::QuaternionBase<Quaternion> Target;
    
    static void* convertible(PyObject* obj)
    {
      // Find a converter which can produce a Source instance from
      // obj. The user has told us that Source can be converted to
      // Target, and instantiating construct() below, ensures that
      // at compile-time.
      return implicit_rvalue_convertible_from_python(obj, registered<Source>::converters)
      ? obj : 0;
    }
    
    static void construct(PyObject* obj, rvalue_from_python_stage1_data* data)
    {
      void* storage = ((rvalue_from_python_storage<Target>*)data)->storage.bytes;
      
      arg_from_python<Source> get_source(obj);
      bool convertible = get_source.convertible();
      BOOST_VERIFY(convertible);
      
      new (storage) Source(get_source());
      
      // record successful construction
      data->convertible = storage;
    }
  };

}}} // namespace boost::python::converter

Nicolas Mansard's avatar
Nicolas Mansard committed
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
namespace eigenpy
{

  class ExceptionIndex : public Exception
  {
  public:
    ExceptionIndex(int index,int imin,int imax) : Exception("")
    {
      std::ostringstream oss; oss << "Index " << index << " out of range " << imin << ".."<< imax <<".";
      message = oss.str();
    }
  };

  namespace bp = boost::python;

73
74
  template<typename QuaternionDerived> class QuaternionVisitor;
  
75
76
  template<typename Scalar, int Options>
  struct call< Eigen::Quaternion<Scalar,Options> >
77
  {
78
79
    typedef Eigen::Quaternion<Scalar,Options> Quaternion;
    static inline void expose()
80
    {
81
82
      QuaternionVisitor<Quaternion>::expose();
    }
Justin Carpentier's avatar
Justin Carpentier committed
83
84
85
86
87
88
    
    static inline bool isApprox(const Quaternion & self, const Quaternion & other,
                                const Scalar & prec = Eigen::NumTraits<Scalar>::dummy_precision())
    {
      return self.isApprox(other,prec);
    }
89
90
  };

Justin Carpentier's avatar
Justin Carpentier committed
91
92
  BOOST_PYTHON_FUNCTION_OVERLOADS(isApproxQuaternion_overload,call<Eigen::Quaterniond>::isApprox,2,3)

Nicolas Mansard's avatar
Nicolas Mansard committed
93
94
  template<typename Quaternion>
  class QuaternionVisitor
95
  :  public bp::def_visitor< QuaternionVisitor<Quaternion> >
Nicolas Mansard's avatar
Nicolas Mansard committed
96
97
98
  {
    typedef Eigen::QuaternionBase<Quaternion> QuaternionBase;

99
    typedef typename QuaternionBase::Scalar Scalar;
100
    typedef typename Quaternion::Coefficients Coefficients;
101
102
103
    typedef typename QuaternionBase::Vector3 Vector3;
    typedef typename Eigen::Matrix<Scalar,4,1> Vector4;
    typedef typename QuaternionBase::Matrix3 Matrix3;
Nicolas Mansard's avatar
Nicolas Mansard committed
104

105
    typedef typename QuaternionBase::AngleAxisType AngleAxis;
Nicolas Mansard's avatar
Nicolas Mansard committed
106
107
108
109
110
111
112

  public:

    template<class PyClass>
    void visit(PyClass& cl) const 
    {
      cl
113
114
      .def(bp::init<>(bp::arg("self"),"Default constructor"))
      .def(bp::init<Vector4>((bp::arg("self"),bp::arg("vec4")),
Justin Carpentier's avatar
Justin Carpentier committed
115
116
                             "Initialize from a vector 4D.\n"
                             "\tvec4 : a 4D vector representing quaternion coefficients in the order xyzw."))
117
      .def(bp::init<Matrix3>((bp::arg("self"),bp::arg("R")),
Justin Carpentier's avatar
Justin Carpentier committed
118
119
                             "Initialize from rotation matrix.\n"
                             "\tR : a rotation matrix 3x3."))
120
      .def(bp::init<AngleAxis>((bp::arg("self"),bp::arg("aa")),
Justin Carpentier's avatar
Justin Carpentier committed
121
122
                               "Initialize from an angle axis.\n"
                               "\taa: angle axis object."))
123
      .def(bp::init<Quaternion>((bp::arg("self"),bp::arg("quat")),
Justin Carpentier's avatar
Justin Carpentier committed
124
125
                                "Copy constructor.\n"
                                "\tquat: a quaternion."))
126
      .def("__init__",bp::make_constructor(&QuaternionVisitor::FromTwoVectors,
127
                                           bp::default_call_policies(),
Justin Carpentier's avatar
Justin Carpentier committed
128
                                           (bp::arg("u"),bp::arg("v"))),
Justin Carpentier's avatar
Justin Carpentier committed
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
133
134
135
136
            "Initialize from coefficients.\n\n"
            "... note:: The order of coefficients is *w*, *x*, *y*, *z*. "
            "The [] operator numbers them differently, 0...4 for *x* *y* *z* *w*!"))
      
      .add_property("x",
137
                    &QuaternionVisitor::getCoeff<0>,
138
139
                    &QuaternionVisitor::setCoeff<0>,"The x coefficient.")
      .add_property("y",
140
                    &QuaternionVisitor::getCoeff<1>,
141
142
                    &QuaternionVisitor::setCoeff<1>,"The y coefficient.")
      .add_property("z",
143
                    &QuaternionVisitor::getCoeff<2>,
144
145
                    &QuaternionVisitor::setCoeff<2>,"The z coefficient.")
      .add_property("w",
146
                    &QuaternionVisitor::getCoeff<3>,
147
148
                    &QuaternionVisitor::setCoeff<3>,"The w coefficient.")
      
Justin Carpentier's avatar
Justin Carpentier committed
149
150
      .def("isApprox",
           &call<Quaternion>::isApprox,
151
           isApproxQuaternion_overload(bp::args("self","other","prec"),
Justin Carpentier's avatar
Justin Carpentier committed
152
                                       "Returns true if *this is approximately equal to other, within the precision determined by prec."))
153
154
155
      
      /* --- Methods --- */
      .def("coeffs",(const Vector4 & (Quaternion::*)()const)&Quaternion::coeffs,
156
157
           bp::arg("self"),
           "Returns a vector of the coefficients (x,y,z,w)",
158
           bp::return_value_policy<bp::copy_const_reference>())
159
160
161
162
      .def("matrix",&Quaternion::matrix,
           bp::arg("self"),
           "Returns an equivalent 3x3 rotation matrix. Similar to toRotationMatrix.")
      .def("toRotationMatrix",&Quaternion::toRotationMatrix,
163
//           bp::arg("self"), // Bug in Boost.Python
164
           "Returns an equivalent 3x3 rotation matrix.")
165
      
166
167
      .def("setFromTwoVectors",&setFromTwoVectors,((bp::arg("self"),bp::arg("a"),bp::arg("b"))),
           "Set *this to be the quaternion which transforms a into b through a rotation."
168
           ,bp::return_self<>())
169
170
171
172
173
174
175
176
177
178
179
180
181
182
183
184
185
186
187
188
189
190
191
192
193
      .def("conjugate",&Quaternion::conjugate,
           bp::arg("self"),
           "Returns the conjugated quaternion.\n"
           "The conjugate of a quaternion represents the opposite rotation.")
      .def("inverse",&Quaternion::inverse,
           bp::arg("self"),
           "Returns the quaternion describing the inverse rotation.")
      .def("setIdentity",&Quaternion::setIdentity,
           bp::arg("self"),
           "Set *this to the idendity rotation.",bp::return_self<>())
      .def("norm",&Quaternion::norm,
           bp::arg("self"),
           "Returns the norm of the quaternion's coefficients.")
      .def("normalize",&Quaternion::normalize,
           bp::arg("self"),
           "Normalizes the quaternion *this.")
      .def("normalized",&Quaternion::normalized,
           bp::arg("self"),
           "Returns a normalized copy of *this.")
      .def("squaredNorm",&Quaternion::squaredNorm,
           bp::arg("self"),
           "Returns the squared norm of the quaternion's coefficients.")
      .def("dot",&Quaternion::template dot<Quaternion>,
           (bp::arg("self"),bp::arg("other")),
           "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
196
197
198
199
200
201
      .def("_transformVector",&Quaternion::_transformVector,
           (bp::arg("self"),bp::arg("vector")),
           "Rotation of a vector by a quaternion.")
      .def("vec",&vec,
           bp::arg("self"),
           "Returns a vector expression of the imaginary part (x,y,z).")
      .def("angularDistance",
202
//           (bp::arg("self"),bp::arg("other")), // Bug in Boost.Python
203
204
205
           &Quaternion::template angularDistance<Quaternion>,
           "Returns the angle (in radian) between two rotations.")
      .def("slerp",&slerp,bp::args("self","t","other"),
206
207
208
209
210
211
212
213
214
215
216
217
218
           "Returns the spherical linear interpolation between the two quaternions *this and other at the parameter t in [0;1].")

      /* --- Operators --- */
      .def(bp::self * bp::self)
      .def(bp::self *= bp::self)
      .def(bp::self * bp::other<Vector3>())
      .def("__eq__",&QuaternionVisitor::__eq__)
      .def("__ne__",&QuaternionVisitor::__ne__)
      .def("__abs__",&Quaternion::norm)
      .def("__len__",&QuaternionVisitor::__len__).staticmethod("__len__")
      .def("__setitem__",&QuaternionVisitor::__setitem__)
      .def("__getitem__",&QuaternionVisitor::__getitem__)
      .def("assign",&assign<Quaternion>,
219
           bp::args("self","quat"),"Set *this from an quaternion quat and returns a reference to *this.",bp::return_self<>())
220
      .def("assign",(Quaternion & (Quaternion::*)(const AngleAxis &))&Quaternion::operator=,
221
           bp::args("self","aa"),"Set *this from an angle-axis aa and returns a reference to *this.",bp::return_self<>())
222
223
224
      .def("__str__",&print)
      .def("__repr__",&print)
      
225
226
227
//      .def("FromTwoVectors",&Quaternion::template FromTwoVectors<Vector3,Vector3>,
//           bp::args("a","b"),
//           "Returns the quaternion which transform a into b through a rotation.")
228
229
      .def("FromTwoVectors",&FromTwoVectors,
           bp::args("a","b"),
Justin Carpentier's avatar
Justin Carpentier committed
230
           "Returns the quaternion which transforms a into b through a rotation.",
231
           bp::return_value_policy<bp::manage_new_object>())
232
      .staticmethod("FromTwoVectors")
233
234
      .def("Identity",&Quaternion::Identity,
           "Returns a quaternion representing an identity rotation.")
235
236
      .staticmethod("Identity")
      ;
Nicolas Mansard's avatar
Nicolas Mansard committed
237
238
    }
  private:
239
240
241
242
    
    template<int i>
    static void setCoeff(Quaternion & self, Scalar value) { self.coeffs()[i] = value; }
    
243
244
245
    template<int i>
    static Scalar getCoeff(Quaternion & self) { return self.coeffs()[i]; }
    
246
247
248
249
250
251
252
    static Quaternion & setFromTwoVectors(Quaternion & self, const Vector3 & a, const Vector3 & b)
    { return self.setFromTwoVectors(a,b); }
    
    template<typename OtherQuat>
    static Quaternion & assign(Quaternion & self, const OtherQuat & quat)
    { return self = quat; }

253
    static Quaternion* FromTwoVectors(const Vector3& u, const Vector3& v)
Nicolas Mansard's avatar
Nicolas Mansard committed
254
    { 
255
      Quaternion* q(new Quaternion); q->setFromTwoVectors(u,v);
Nicolas Mansard's avatar
Nicolas Mansard committed
256
257
258
      return q; 
    }
  
Justin Carpentier's avatar
Justin Carpentier committed
259
    static bool __eq__(const Quaternion & u, const Quaternion & v)
Nicolas Mansard's avatar
Nicolas Mansard committed
260
    {
Justin Carpentier's avatar
Justin Carpentier committed
261
      return u.coeffs() == v.coeffs();
Nicolas Mansard's avatar
Nicolas Mansard committed
262
    }
263
264
    
    static bool __ne__(const Quaternion& u, const Quaternion& v)
Nicolas Mansard's avatar
Nicolas Mansard committed
265
266
267
268
    {
      return !__eq__(u,v); 
    }

269
    static Scalar __getitem__(const Quaternion & self, int idx)
Nicolas Mansard's avatar
Nicolas Mansard committed
270
    { 
271
272
      if((idx<0) || (idx>=4)) throw eigenpy::ExceptionIndex(idx,0,3);
      return self.coeffs()[idx];
Nicolas Mansard's avatar
Nicolas Mansard committed
273
274
    }
  
275
    static void __setitem__(Quaternion& self, int idx, const Scalar value)
Nicolas Mansard's avatar
Nicolas Mansard committed
276
    { 
277
278
      if((idx<0) || (idx>=4)) throw eigenpy::ExceptionIndex(idx,0,3);
      self.coeffs()[idx] = value;
Nicolas Mansard's avatar
Nicolas Mansard committed
279
280
    }

281
282
283
284
285
286
287
288
289
290
    static int __len__() {  return 4;  }
    static Vector3 vec(const Quaternion & self) { return self.vec(); }
    
    static std::string print(const Quaternion & self)
    {
      std::stringstream ss;
      ss << "(x,y,z,w) = " << self.coeffs().transpose() << std::endl;
      
      return ss.str();
    }
291
292
293
    
    static Quaternion slerp(const Quaternion & self, const Scalar t, const Quaternion & other)
    { return self.slerp(t,other); }
Nicolas Mansard's avatar
Nicolas Mansard committed
294
295
296
297
298

  public:

    static void expose()
    {
299
300
301
302
303
304
305
306
307
      bp::class_<Quaternion>("Quaternion",
                             "Quaternion representing rotation.\n\n"
                             "Supported operations "
                             "('q is a Quaternion, 'v' is a Vector3): "
                             "'q*q' (rotation composition), "
                             "'q*=q', "
                             "'q*v' (rotating 'v' by 'q'), "
                             "'q==q', 'q!=q', 'q[0..3]'.",
                             bp::no_init)
308
309
310
311
      .def(QuaternionVisitor<Quaternion>());
      
      // Cast to Eigen::QuaternionBase and vice-versa
      bp::implicitly_convertible<Quaternion,QuaternionBase >();
312
//      bp::implicitly_convertible<QuaternionBase,Quaternion >();
Nicolas Mansard's avatar
Nicolas Mansard committed
313
314
315
316
317
318
    }

  };

} // namespace eigenpy

319
#endif // ifndef __eigenpy_quaternion_hpp__