quaternion.hpp 9.97 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"
15
#include "eigenpy/registration.hpp"
Nicolas Mansard's avatar
Nicolas Mansard committed
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31

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;

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

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

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

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

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

  public:

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

180
    static Quaternion* FromTwoVectors(const Vector3& u, const Vector3& v)
Nicolas Mansard's avatar
Nicolas Mansard committed
181
    { 
182
      Quaternion* q(new Quaternion); q->setFromTwoVectors(u,v);
Nicolas Mansard's avatar
Nicolas Mansard committed
183
184
185
      return q; 
    }
  
Justin Carpentier's avatar
Justin Carpentier committed
186
    static bool __eq__(const Quaternion & u, const Quaternion & v)
Nicolas Mansard's avatar
Nicolas Mansard committed
187
    {
Justin Carpentier's avatar
Justin Carpentier committed
188
      return u.coeffs() == v.coeffs();
Nicolas Mansard's avatar
Nicolas Mansard committed
189
    }
190
191
    
    static bool __ne__(const Quaternion& u, const Quaternion& v)
Nicolas Mansard's avatar
Nicolas Mansard committed
192
193
194
195
    {
      return !__eq__(u,v); 
    }

196
    static Scalar __getitem__(const Quaternion & self, int idx)
Nicolas Mansard's avatar
Nicolas Mansard committed
197
    { 
198
199
      if((idx<0) || (idx>=4)) throw eigenpy::ExceptionIndex(idx,0,3);
      return self.coeffs()[idx];
Nicolas Mansard's avatar
Nicolas Mansard committed
200
201
    }
  
202
    static void __setitem__(Quaternion& self, int idx, const Scalar value)
Nicolas Mansard's avatar
Nicolas Mansard committed
203
    { 
204
205
      if((idx<0) || (idx>=4)) throw eigenpy::ExceptionIndex(idx,0,3);
      self.coeffs()[idx] = value;
Nicolas Mansard's avatar
Nicolas Mansard committed
206
207
    }

208
209
210
211
212
213
214
215
216
217
    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();
    }
218
219
220
    
    static Quaternion slerp(const Quaternion & self, const Scalar t, const Quaternion & other)
    { return self.slerp(t,other); }
Nicolas Mansard's avatar
Nicolas Mansard committed
221
222
223
224
225

  public:

    static void expose()
    {
226
227
228
229
230
231
232
233
234
235
236
      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)
      .def(QuaternionVisitor<Quaternion>())
      ;
237
   
Nicolas Mansard's avatar
Nicolas Mansard committed
238
239
240
241
242
243
    }

  };

} // namespace eigenpy

244
#endif // ifndef __eigenpy_quaternion_hpp__