quaternion.hpp 12.4 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
  };

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

97
    typedef typename QuaternionBase::Scalar Scalar;
98
    typedef typename Quaternion::Coefficients Coefficients;
99
100
101
    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
102

103
    typedef typename QuaternionBase::AngleAxisType AngleAxis;
104
105
    
    BOOST_PYTHON_FUNCTION_OVERLOADS(isApproxQuaternion_overload,call<Quaternion>::isApprox,2,3)
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_internal_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
220
221
           bp::args("self","quat"),
           "Set *this from an quaternion quat and returns a reference to *this.",
           bp::return_self<>())
222
      .def("assign",(Quaternion & (Quaternion::*)(const AngleAxis &))&Quaternion::operator=,
223
224
225
           bp::args("self","aa"),
           "Set *this from an angle-axis aa and returns a reference to *this.",
           bp::return_self<>())
226
227
228
      .def("__str__",&print)
      .def("__repr__",&print)
      
229
230
231
//      .def("FromTwoVectors",&Quaternion::template FromTwoVectors<Vector3,Vector3>,
//           bp::args("a","b"),
//           "Returns the quaternion which transform a into b through a rotation.")
232
233
      .def("FromTwoVectors",&FromTwoVectors,
           bp::args("a","b"),
Justin Carpentier's avatar
Justin Carpentier committed
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
238
      .def("Identity",&Quaternion::Identity,
           "Returns a quaternion representing an identity rotation.")
239
240
      .staticmethod("Identity")
      ;
Nicolas Mansard's avatar
Nicolas Mansard committed
241
242
    }
  private:
243
244
245
246
    
    template<int i>
    static void setCoeff(Quaternion & self, Scalar value) { self.coeffs()[i] = value; }
    
247
248
249
    template<int i>
    static Scalar getCoeff(Quaternion & self) { return self.coeffs()[i]; }
    
250
251
252
253
254
255
256
    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; }

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

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

285
286
287
288
289
290
291
292
293
294
    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();
    }
295
296
297
    
    static Quaternion slerp(const Quaternion & self, const Scalar t, const Quaternion & other)
    { return self.slerp(t,other); }
Nicolas Mansard's avatar
Nicolas Mansard committed
298
299
300
301
302

  public:

    static void expose()
    {
303
304
305
306
307
308
309
310
311
      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)
312
313
314
315
      .def(QuaternionVisitor<Quaternion>());
      
      // Cast to Eigen::QuaternionBase and vice-versa
      bp::implicitly_convertible<Quaternion,QuaternionBase >();
316
//      bp::implicitly_convertible<QuaternionBase,Quaternion >();
Nicolas Mansard's avatar
Nicolas Mansard committed
317
318
319
320
321
322
    }

  };

} // namespace eigenpy

323
#endif // ifndef __eigenpy_quaternion_hpp__