quaternion.hpp 6.03 KB
Newer Older
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
/*
 * Copyright 2014, Nicolas Mansard, LAAS-CNRS
 *
 * This file is part of eigenpy.
 * eigenpy is free software: you can redistribute it and/or
 * modify it under the terms of the GNU Lesser General Public License
 * as published by the Free Software Foundation, either version 3 of
 * the License, or (at your option) any later version.
 * eigenpy is distributed in the hope that it will be
 * useful, but WITHOUT ANY WARRANTY; without even the implied warranty
 * of MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
 * GNU Lesser General Public License for more details.  You should
 * have received a copy of the GNU Lesser General Public License along
 * with eigenpy.  If not, see <http://www.gnu.org/licenses/>.
 */

#ifndef __eigenpy_quaternion_hpp__
#define __eigenpy_quaternion_hpp__
Nicolas Mansard's avatar
Nicolas Mansard committed
19
20
21

#include <Eigen/Core>
#include <Eigen/Geometry>
jcarpent's avatar
jcarpent committed
22

23
#include "eigenpy/exception.hpp"
Nicolas Mansard's avatar
Nicolas Mansard committed
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41

namespace eigenpy
{

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

  template<>
  struct UnalignedEquivalent<Eigen::Quaterniond>
  {
42
43
44
45
46
#ifndef EIGENPY_ALIGNED
    typedef Eigen::Quaternion<Eigen::Quaterniond::Scalar,Eigen::DontAlign> type;
#else
    typedef Eigen::Quaterniond type;
#endif
Nicolas Mansard's avatar
Nicolas Mansard committed
47
48
49
50
51
52
53
54
55
56
57
  };

  namespace bp = boost::python;

  template<typename Quaternion>
  class QuaternionVisitor
    :  public boost::python::def_visitor< QuaternionVisitor<Quaternion> >
  {
    typedef Eigen::QuaternionBase<Quaternion> QuaternionBase;
    typedef typename eigenpy::UnalignedEquivalent<Quaternion>::type QuaternionUnaligned;

58
    typedef typename QuaternionBase::Scalar Scalar;
Nicolas Mansard's avatar
Nicolas Mansard committed
59
    typedef typename QuaternionUnaligned::Coefficients Coefficients;
60
61
#ifndef EIGENPY_ALIGNED
    typedef Eigen::Matrix<Scalar,3,1,Eigen::DontAlign> Vector3;
Nicolas Mansard's avatar
Nicolas Mansard committed
62
    typedef Eigen::Matrix<Scalar,3,3,Eigen::DontAlign> Matrix3;
63
64
65
66
#else
    typedef Eigen::Matrix<Scalar,3,1,0> Vector3;
    typedef Eigen::Matrix<Scalar,3,3,0> Matrix3;
#endif
Nicolas Mansard's avatar
Nicolas Mansard committed
67
68
69
70
71

    typedef Eigen::AngleAxis<Scalar> AngleAxisUnaligned;

  public:

Nicolas Mansard's avatar
Nicolas Mansard committed
72
    /* Conversion from C++ to Python. */
Nicolas Mansard's avatar
Nicolas Mansard committed
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
92
93
94
95
96
97
98
99
100
101
102
103
104
105
106
107
108
109
110
111
112
113
114
115
116
117
118
119
120
121
122
123
124
125
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
151
152
153
154
155
156
157
158
159
160
161
162
163
164
165
166
167
168
169
170
171
172
173
174
175
176
177
178
179
180
    static PyObject* convert(Quaternion const& q)
    {
      QuaternionUnaligned qx = q;
      return boost::python::incref(boost::python::object(qx).ptr());
    }

    template<class PyClass>
    void visit(PyClass& cl) const 
    {
      cl
	.def(bp::init<Matrix3>((bp::arg("matrixRotation")),"Initialize from rotation matrix."))
	.def(bp::init<AngleAxisUnaligned>((bp::arg("angleaxis")),"Initialize from angle axis."))
	.def(bp::init<QuaternionUnaligned>((bp::arg("clone")),"Copy constructor."))
	.def("__init__",bp::make_constructor(&QuaternionVisitor::fromTwoVectors,
					     bp::default_call_policies(),
					     (bp::arg("u"),bp::arg("v"))),"Initialize from two vector u,v")
	.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*!"))

	/* --- Methods --- */
	.def("coeffs",&QuaternionVisitor::coeffs)
	.def("matrix",&QuaternionUnaligned::toRotationMatrix)
      
	.def("setFromTwoVectors",&QuaternionVisitor::setFromTwoVectors,((bp::arg("u"),bp::arg("v"))))
	.def("conjugate",&QuaternionUnaligned::conjugate)
	.def("inverse",&QuaternionUnaligned::inverse)
	.def("norm",&QuaternionUnaligned::norm)
	.def("normalize",&QuaternionUnaligned::normalize)
	.def("normalized",&QuaternionUnaligned::normalized)
	.def("apply",&QuaternionUnaligned::_transformVector)

	/* --- 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__",&QuaternionUnaligned::norm)
	.def("__len__",&QuaternionVisitor::__len__).staticmethod("__len__")
	.def("__setitem__",&QuaternionVisitor::__setitem__)
	.def("__getitem__",&QuaternionVisitor::__getitem__)
	;
    }
  private:

    static QuaternionUnaligned* fromTwoVectors(const Vector3& u, const Vector3& v)
    { 
      QuaternionUnaligned* q(new QuaternionUnaligned); q->setFromTwoVectors(u,v); 
      return q; 
    }
  
    static Coefficients coeffs(const QuaternionUnaligned& self)
    {
      return self.coeffs(); 
    }

    static void setFromTwoVectors(QuaternionUnaligned& self, const Vector3& u, const Vector3& v)
    { 
      self.setFromTwoVectors(u,v);
    }

    static bool __eq__(const QuaternionUnaligned& u, const QuaternionUnaligned& v)
    {
      return u.isApprox(v,1e-9);
    }
    static bool __ne__(const QuaternionUnaligned& u, const QuaternionUnaligned& v)
    {
      return !__eq__(u,v); 
    }

    static double __getitem__(const QuaternionUnaligned & self, int idx)
    { 
      if((idx<0) || (idx>4)) throw eigenpy::ExceptionIndex(idx,0,4);
      if(idx==0) return self.x(); 
      else if(idx==1) return self.y(); 
      else if(idx==2) return self.z();
      else return self.w(); 
    }
  
    static void __setitem__(QuaternionUnaligned& self, int idx, double value)
    { 
      if((idx<0) || (idx>4)) throw eigenpy::ExceptionIndex(idx,0,4);
      if(idx==0) self.x()=value; 
      else if(idx==1) self.y()=value;
      else if(idx==2) self.z()=value;
      else self.w()=value;
    }

    static int __len__() {    return 4;  }

  public:

    static void expose()
    {
      bp::class_<QuaternionUnaligned>("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::init<>())
	.def(QuaternionVisitor<Quaternion>())
	;
181
182
   
#ifndef EIGENPY_ALIGNED
Nicolas Mansard's avatar
Nicolas Mansard committed
183
      bp::to_python_converter< Quaternion,QuaternionVisitor<Quaternion> >();
184
#endif
Nicolas Mansard's avatar
Nicolas Mansard committed
185
186
187
188
189
190
    }

  };

} // namespace eigenpy

191
#endif // ifndef __eigenpy_quaternion_hpp__