quaternion.hpp 11 KB
Newer Older
1
/*
2
3
 * Copyright 2014-2019, CNRS
 * Copyright 2018-2019, INRIA
4
5
6
7
 */

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

9
10
#include "eigenpy/fwd.hpp"

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

14
#include "eigenpy/exception.hpp"
Nicolas Mansard's avatar
Nicolas Mansard committed
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30

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;

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

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

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

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

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

  public:

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

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

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

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

  public:

    static void expose()
    {
257
258
259
260
261
262
263
264
265
      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)
266
267
268
269
270
      .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
271
272
273
274
275
276
    }

  };

} // namespace eigenpy

277
#endif // ifndef __eigenpy_quaternion_hpp__