quaternion.hpp 10.9 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
22
23
24
25
26
27
28
29

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;

30
31
  template<typename QuaternionDerived> class QuaternionVisitor;
  
32
33
  template<typename Scalar, int Options>
  struct call< Eigen::Quaternion<Scalar,Options> >
34
  {
35
36
    typedef Eigen::Quaternion<Scalar,Options> Quaternion;
    static inline void expose()
37
    {
38
39
      QuaternionVisitor<Quaternion>::expose();
    }
Justin Carpentier's avatar
Justin Carpentier committed
40
41
42
43
44
45
    
    static inline bool isApprox(const Quaternion & self, const Quaternion & other,
                                const Scalar & prec = Eigen::NumTraits<Scalar>::dummy_precision())
    {
      return self.isApprox(other,prec);
    }
46
47
  };

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

Nicolas Mansard's avatar
Nicolas Mansard committed
50
51
  template<typename Quaternion>
  class QuaternionVisitor
52
  :  public bp::def_visitor< QuaternionVisitor<Quaternion> >
Nicolas Mansard's avatar
Nicolas Mansard committed
53
54
55
  {
    typedef Eigen::QuaternionBase<Quaternion> QuaternionBase;

56
    typedef typename QuaternionBase::Scalar Scalar;
57
    typedef typename Quaternion::Coefficients Coefficients;
58
59
60
    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
61

62
    typedef typename QuaternionBase::AngleAxisType AngleAxis;
Nicolas Mansard's avatar
Nicolas Mansard committed
63
64
65
66
67
68
69

  public:

    template<class PyClass>
    void visit(PyClass& cl) const 
    {
      cl
70
71
      .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
72
73
                             "Initialize from a vector 4D.\n"
                             "\tvec4 : a 4D vector representing quaternion coefficients in the order xyzw."))
74
      .def(bp::init<Matrix3>((bp::arg("self"),bp::arg("R")),
Justin Carpentier's avatar
Justin Carpentier committed
75
76
                             "Initialize from rotation matrix.\n"
                             "\tR : a rotation matrix 3x3."))
77
      .def(bp::init<AngleAxis>((bp::arg("self"),bp::arg("aa")),
Justin Carpentier's avatar
Justin Carpentier committed
78
79
                               "Initialize from an angle axis.\n"
                               "\taa: angle axis object."))
80
      .def(bp::init<Quaternion>((bp::arg("self"),bp::arg("quat")),
Justin Carpentier's avatar
Justin Carpentier committed
81
82
                                "Copy constructor.\n"
                                "\tquat: a quaternion."))
83
      .def("__init__",bp::make_constructor(&QuaternionVisitor::FromTwoVectors,
84
                                           bp::default_call_policies(),
Justin Carpentier's avatar
Justin Carpentier committed
85
                                           (bp::arg("u"),bp::arg("v"))),
Justin Carpentier's avatar
Justin Carpentier committed
86
           "Initialize from two vectors u and v")
87
      .def(bp::init<Scalar,Scalar,Scalar,Scalar>
88
           ((bp::arg("self"),bp::arg("w"),bp::arg("x"),bp::arg("y"),bp::arg("z")),
89
90
91
92
93
            "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",
94
                    &QuaternionVisitor::getCoeff<0>,
95
96
                    &QuaternionVisitor::setCoeff<0>,"The x coefficient.")
      .add_property("y",
97
                    &QuaternionVisitor::getCoeff<1>,
98
99
                    &QuaternionVisitor::setCoeff<1>,"The y coefficient.")
      .add_property("z",
100
                    &QuaternionVisitor::getCoeff<2>,
101
102
                    &QuaternionVisitor::setCoeff<2>,"The z coefficient.")
      .add_property("w",
103
                    &QuaternionVisitor::getCoeff<3>,
104
105
                    &QuaternionVisitor::setCoeff<3>,"The w coefficient.")
      
Justin Carpentier's avatar
Justin Carpentier committed
106
107
      .def("isApprox",
           &call<Quaternion>::isApprox,
108
           isApproxQuaternion_overload(bp::args("self","other","prec"),
Justin Carpentier's avatar
Justin Carpentier committed
109
                                       "Returns true if *this is approximately equal to other, within the precision determined by prec."))
110
111
112
      
      /* --- Methods --- */
      .def("coeffs",(const Vector4 & (Quaternion::*)()const)&Quaternion::coeffs,
113
114
           bp::arg("self"),
           "Returns a vector of the coefficients (x,y,z,w)",
115
           bp::return_value_policy<bp::copy_const_reference>())
116
117
118
119
      .def("matrix",&Quaternion::matrix,
           bp::arg("self"),
           "Returns an equivalent 3x3 rotation matrix. Similar to toRotationMatrix.")
      .def("toRotationMatrix",&Quaternion::toRotationMatrix,
120
//           bp::arg("self"), // Bug in Boost.Python
121
           "Returns an equivalent 3x3 rotation matrix.")
122
      
123
124
      .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."
125
           ,bp::return_self<>())
126
127
128
129
130
131
132
133
134
135
136
137
138
139
140
141
142
143
144
145
146
147
148
149
150
      .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"
151
           "Geometrically speaking, the dot product of two unit quaternions corresponds to the cosine of half the angle between the two rotations.")
152
153
154
155
156
157
158
      .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",
159
//           (bp::arg("self"),bp::arg("other")), // Bug in Boost.Python
160
161
162
           &Quaternion::template angularDistance<Quaternion>,
           "Returns the angle (in radian) between two rotations.")
      .def("slerp",&slerp,bp::args("self","t","other"),
163
164
165
166
167
168
169
170
171
172
173
174
175
           "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>,
176
           bp::args("self","quat"),"Set *this from an quaternion quat and returns a reference to *this.",bp::return_self<>())
177
      .def("assign",(Quaternion & (Quaternion::*)(const AngleAxis &))&Quaternion::operator=,
178
           bp::args("self","aa"),"Set *this from an angle-axis aa and returns a reference to *this.",bp::return_self<>())
179
180
181
      .def("__str__",&print)
      .def("__repr__",&print)
      
182
183
184
//      .def("FromTwoVectors",&Quaternion::template FromTwoVectors<Vector3,Vector3>,
//           bp::args("a","b"),
//           "Returns the quaternion which transform a into b through a rotation.")
185
186
      .def("FromTwoVectors",&FromTwoVectors,
           bp::args("a","b"),
Justin Carpentier's avatar
Justin Carpentier committed
187
           "Returns the quaternion which transforms a into b through a rotation.",
188
           bp::return_value_policy<bp::manage_new_object>())
189
      .staticmethod("FromTwoVectors")
190
191
      .def("Identity",&Quaternion::Identity,
           "Returns a quaternion representing an identity rotation.")
192
193
      .staticmethod("Identity")
      ;
Nicolas Mansard's avatar
Nicolas Mansard committed
194
195
    }
  private:
196
197
198
199
    
    template<int i>
    static void setCoeff(Quaternion & self, Scalar value) { self.coeffs()[i] = value; }
    
200
201
202
    template<int i>
    static Scalar getCoeff(Quaternion & self) { return self.coeffs()[i]; }
    
203
204
205
206
207
208
209
    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; }

210
    static Quaternion* FromTwoVectors(const Vector3& u, const Vector3& v)
Nicolas Mansard's avatar
Nicolas Mansard committed
211
    { 
212
      Quaternion* q(new Quaternion); q->setFromTwoVectors(u,v);
Nicolas Mansard's avatar
Nicolas Mansard committed
213
214
215
      return q; 
    }
  
Justin Carpentier's avatar
Justin Carpentier committed
216
    static bool __eq__(const Quaternion & u, const Quaternion & v)
Nicolas Mansard's avatar
Nicolas Mansard committed
217
    {
Justin Carpentier's avatar
Justin Carpentier committed
218
      return u.coeffs() == v.coeffs();
Nicolas Mansard's avatar
Nicolas Mansard committed
219
    }
220
221
    
    static bool __ne__(const Quaternion& u, const Quaternion& v)
Nicolas Mansard's avatar
Nicolas Mansard committed
222
223
224
225
    {
      return !__eq__(u,v); 
    }

226
    static Scalar __getitem__(const Quaternion & self, int idx)
Nicolas Mansard's avatar
Nicolas Mansard committed
227
    { 
228
229
      if((idx<0) || (idx>=4)) throw eigenpy::ExceptionIndex(idx,0,3);
      return self.coeffs()[idx];
Nicolas Mansard's avatar
Nicolas Mansard committed
230
231
    }
  
232
    static void __setitem__(Quaternion& self, int idx, const Scalar value)
Nicolas Mansard's avatar
Nicolas Mansard committed
233
    { 
234
235
      if((idx<0) || (idx>=4)) throw eigenpy::ExceptionIndex(idx,0,3);
      self.coeffs()[idx] = value;
Nicolas Mansard's avatar
Nicolas Mansard committed
236
237
    }

238
239
240
241
242
243
244
245
246
247
    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();
    }
248
249
250
    
    static Quaternion slerp(const Quaternion & self, const Scalar t, const Quaternion & other)
    { return self.slerp(t,other); }
Nicolas Mansard's avatar
Nicolas Mansard committed
251
252
253
254
255

  public:

    static void expose()
    {
256
257
258
259
260
261
262
263
264
      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)
265
266
267
268
269
      .def(QuaternionVisitor<Quaternion>());
      
      // Cast to Eigen::QuaternionBase and vice-versa
      bp::implicitly_convertible<Quaternion,QuaternionBase >();
      bp::implicitly_convertible<QuaternionBase,Quaternion >();
Nicolas Mansard's avatar
Nicolas Mansard committed
270
271
272
273
274
275
    }

  };

} // namespace eigenpy

276
#endif // ifndef __eigenpy_quaternion_hpp__