Skip to content

Commit c7fdc96

Browse files
jcarpentjcarpent
committed
[C++] Update bindings of Quaternion class
- Update API to follow Eigen conventions - Add Doc - Remove unaligned conversions
1 parent af32ca4 commit c7fdc96

File tree

1 file changed

+127
-104
lines changed

1 file changed

+127
-104
lines changed

‎src/quaternion.hpp‎

Lines changed: 127 additions & 104 deletions
Original file line numberDiff line numberDiff line change
@@ -28,160 +28,183 @@ namespace eigenpy
2828
class ExceptionIndex : public Exception
2929
{
3030
public:
31-
int index;
3231
ExceptionIndex(int index,int imin,int imax) : Exception("")
3332
{
3433
std::ostringstream oss; oss << "Index " << index << " out of range " << imin << ".."<< imax <<".";
3534
message = oss.str();
3635
}
3736
};
3837

39-
template<>
40-
struct UnalignedEquivalent<Eigen::Quaterniond>
41-
{
42-
#ifndef EIGENPY_ALIGNED
43-
typedef Eigen::Quaternion<Eigen::Quaterniond::Scalar,Eigen::DontAlign> type;
44-
#else
45-
typedef Eigen::Quaterniond type;
46-
#endif
47-
};
48-
4938
namespace bp = boost::python;
5039

5140
template<typename Quaternion>
5241
class QuaternionVisitor
5342
: public boost::python::def_visitor< QuaternionVisitor<Quaternion> >
5443
{
5544
typedef Eigen::QuaternionBase<Quaternion> QuaternionBase;
56-
typedef typename eigenpy::UnalignedEquivalent<Quaternion>::type QuaternionUnaligned;
5745

5846
typedef typename QuaternionBase::Scalar Scalar;
59-
typedef typename QuaternionUnaligned::Coefficients Coefficients;
60-
#ifndef EIGENPY_ALIGNED
61-
typedef Eigen::Matrix<Scalar,3,1,Eigen::DontAlign> Vector3;
62-
typedef Eigen::Matrix<Scalar,3,3,Eigen::DontAlign> Matrix3;
63-
#else
64-
typedef Eigen::Matrix<Scalar,3,1,0> Vector3;
65-
typedef Eigen::Matrix<Scalar,3,3,0> Matrix3;
66-
#endif
47+
typedef typename Quaternion::Coefficients Coefficients;
48+
typedef Eigen::Matrix<Scalar,3,1> Vector3;
49+
typedef Eigen::Matrix<Scalar,4,1> Vector4;
50+
typedef Eigen::Matrix<Scalar,3,3> Matrix3;
6751

68-
typedef Eigen::AngleAxis<Scalar> AngleAxisUnaligned;
52+
typedef Eigen::AngleAxis<Scalar> AngleAxis;
6953

7054
public:
7155

72-
/* Conversion from C++ to Python. */
73-
static PyObject* convert(Quaternion const& q)
74-
{
75-
QuaternionUnaligned qx = q;
76-
return boost::python::incref(boost::python::object(qx).ptr());
77-
}
78-
7956
template<class PyClass>
8057
void visit(PyClass& cl) const
8158
{
8259
cl
83-
.def(bp::init<Matrix3>((bp::arg("matrixRotation")),"Initialize from rotation matrix."))
84-
.def(bp::init<AngleAxisUnaligned>((bp::arg("angleaxis")),"Initialize from angle axis."))
85-
.def(bp::init<QuaternionUnaligned>((bp::arg("clone")),"Copy constructor."))
86-
.def("__init__",bp::make_constructor(&QuaternionVisitor::fromTwoVectors,
87-
bp::default_call_policies(),
88-
(bp::arg("u"),bp::arg("v"))),"Initialize from two vector u,v")
89-
.def(bp::init<Scalar,Scalar,Scalar,Scalar>
90-
((bp::arg("w"),bp::arg("x"),bp::arg("y"),bp::arg("z")),
91-
"Initialize from coefficients.\n\n"
92-
"... note:: The order of coefficients is *w*, *x*, *y*, *z*. "
93-
"The [] operator numbers them differently, 0...4 for *x* *y* *z* *w*!"))
94-
95-
/* --- Methods --- */
96-
.def("coeffs",&QuaternionVisitor::coeffs)
97-
.def("matrix",&QuaternionUnaligned::toRotationMatrix)
60+
.def(bp::init<>("Default constructor"))
61+
.def(bp::init<Matrix3>((bp::arg("matrixRotation")),"Initialize from rotation matrix."))
62+
.def(bp::init<AngleAxis>((bp::arg("angleaxis")),"Initialize from angle axis."))
63+
.def(bp::init<Quaternion>((bp::arg("clone")),"Copy constructor."))
64+
.def("__init__",bp::make_constructor(&QuaternionVisitor::fromTwoVectors,
65+
bp::default_call_policies(),
66+
(bp::arg("u"),bp::arg("v"))),"Initialize from two vector u,v")
67+
.def(bp::init<Scalar,Scalar,Scalar,Scalar>
68+
((bp::arg("w"),bp::arg("x"),bp::arg("y"),bp::arg("z")),
69+
"Initialize from coefficients.\n\n"
70+
"... note:: The order of coefficients is *w*, *x*, *y*, *z*. "
71+
"The [] operator numbers them differently, 0...4 for *x* *y* *z* *w*!"))
72+
73+
.add_property("x",
74+
(Scalar (Quaternion::*)()const)&Quaternion::x,
75+
&QuaternionVisitor::setCoeff<0>,"The x coefficient.")
76+
.add_property("y",
77+
(Scalar (Quaternion::*)()const)&Quaternion::y,
78+
&QuaternionVisitor::setCoeff<1>,"The y coefficient.")
79+
.add_property("z",
80+
(Scalar (Quaternion::*)()const)&Quaternion::z,
81+
&QuaternionVisitor::setCoeff<2>,"The z coefficient.")
82+
.add_property("w",
83+
(Scalar (Quaternion::*)()const)&Quaternion::w,
84+
&QuaternionVisitor::setCoeff<3>,"The w coefficient.")
85+
86+
.def("isApprox",(bool (Quaternion::*)(const Quaternion &))&Quaternion::template isApprox<Quaternion>,
87+
"Returns true if *this is approximately equal to other.")
88+
.def("isApprox",(bool (Quaternion::*)(const Quaternion &, const Scalar prec))&Quaternion::template isApprox<Quaternion>,
89+
"Returns true if *this is approximately equal to other, within the precision determined by prec..")
90+
91+
/* --- Methods --- */
92+
.def("coeffs",(const Vector4 & (Quaternion::*)()const)&Quaternion::coeffs,
93+
bp::return_value_policy<bp::copy_const_reference>())
94+
.def("matrix",&Quaternion::matrix,"Returns an equivalent rotation matrix")
95+
.def("toRotationMatrix ",&Quaternion::toRotationMatrix,"Returns an equivalent 3x3 rotation matrix.")
96+
97+
.def("setFromTwoVectors",&setFromTwoVectors,((bp::arg("a"),bp::arg("b"))),"Set *this to be the quaternion which transform a into b through a rotation."
98+
,bp::return_self<>())
99+
.def("conjugate",&Quaternion::conjugate,"Returns the conjugated quaternion. The conjugate of a quaternion represents the opposite rotation.")
100+
.def("inverse",&Quaternion::inverse,"Returns the quaternion describing the inverse rotation.")
101+
.def("setIdentity",&Quaternion::setIdentity,bp::return_self<>(),"Set *this to the idendity rotation.")
102+
.def("norm",&Quaternion::norm,"Returns the norm of the quaternion's coefficients.")
103+
.def("normalize",&Quaternion::normalize,"Normalizes the quaternion *this.")
104+
.def("normalized",&Quaternion::normalized,"Returns a normalized copy of *this.")
105+
.def("squaredNorm",&Quaternion::squaredNorm,"Returns the squared norm of the quaternion's coefficients.")
106+
.def("dot",&Quaternion::template dot<Quaternion>,bp::arg("other"),"Returns the dot product of *this with other"
107+
"Geometrically speaking, the dot product of two unit quaternions corresponds to the cosine of half the angle between the two rotations.")
108+
.def("_transformVector",&Quaternion::_transformVector,bp::arg("vector"),"Rotation of a vector by a quaternion.")
109+
.def("vec",&vec,"Returns a vector expression of the imaginary part (x,y,z).")
110+
.def("angularDistance",&Quaternion::template angularDistance<Quaternion>,"Returns the angle (in radian) between two rotations.")
111+
.def("slerp",&Quaternion::template slerp<Quaternion>,bp::args("t","other"),
112+
"Returns the spherical linear interpolation between the two quaternions *this and other at the parameter t in [0;1].")
113+
114+
/* --- Operators --- */
115+
.def(bp::self * bp::self)
116+
.def(bp::self *= bp::self)
117+
.def(bp::self * bp::other<Vector3>())
118+
.def("__eq__",&QuaternionVisitor::__eq__)
119+
.def("__ne__",&QuaternionVisitor::__ne__)
120+
.def("__abs__",&Quaternion::norm)
121+
.def("__len__",&QuaternionVisitor::__len__).staticmethod("__len__")
122+
.def("__setitem__",&QuaternionVisitor::__setitem__)
123+
.def("__getitem__",&QuaternionVisitor::__getitem__)
124+
.def("assign",&assign<Quaternion>,
125+
bp::arg("quat"),"Set *this from an quaternion quat and returns a reference to *this.",bp::return_self<>())
126+
.def("assign",(Quaternion & (Quaternion::*)(const AngleAxis &))&Quaternion::operator=,
127+
bp::arg("aa"),"Set *this from an angle-axis aa and returns a reference to *this.",bp::return_self<>())
128+
.def("__str__",&print)
129+
.def("__repr__",&print)
130+
131+
.def("FromTwoVectors",&Quaternion::template FromTwoVectors<Vector3,Vector3>,
132+
bp::args("a","b"),
133+
"Returns the quaternion which transform a into b through a rotation.")
134+
.staticmethod("FromTwoVectors")
135+
.def("Identity",&Quaternion::Identity,"Returns a quaternion representing an identity rotation.")
136+
.staticmethod("Identity")
98137

99-
.def("setFromTwoVectors",&QuaternionVisitor::setFromTwoVectors,((bp::arg("u"),bp::arg("v"))))
100-
.def("conjugate",&QuaternionUnaligned::conjugate)
101-
.def("inverse",&QuaternionUnaligned::inverse)
102-
.def("norm",&QuaternionUnaligned::norm)
103-
.def("normalize",&QuaternionUnaligned::normalize)
104-
.def("normalized",&QuaternionUnaligned::normalized)
105-
.def("apply",&QuaternionUnaligned::_transformVector)
106-
107-
/* --- Operators --- */
108-
.def(bp::self * bp::self)
109-
.def(bp::self *= bp::self)
110-
.def(bp::self * bp::other<Vector3>())
111-
.def("__eq__",&QuaternionVisitor::__eq__)
112-
.def("__ne__",&QuaternionVisitor::__ne__)
113-
.def("__abs__",&QuaternionUnaligned::norm)
114-
.def("__len__",&QuaternionVisitor::__len__).staticmethod("__len__")
115-
.def("__setitem__",&QuaternionVisitor::__setitem__)
116-
.def("__getitem__",&QuaternionVisitor::__getitem__)
117-
;
138+
139+
;
118140
}
119141
private:
120-
121-
static QuaternionUnaligned* fromTwoVectors(const Vector3& u, const Vector3& v)
142+
143+
template<int i>
144+
static void setCoeff(Quaternion & self, Scalar value) { self.coeffs()[i] = value; }
145+
146+
static Quaternion & setFromTwoVectors(Quaternion & self, const Vector3 & a, const Vector3 & b)
147+
{ return self.setFromTwoVectors(a,b); }
148+
149+
template<typename OtherQuat>
150+
static Quaternion & assign(Quaternion & self, const OtherQuat & quat)
151+
{ return self = quat; }
152+
153+
static Quaternion* fromTwoVectors(const Vector3& u, const Vector3& v)
122154
{
123-
QuaternionUnaligned* q(new QuaternionUnaligned); q->setFromTwoVectors(u,v);
155+
Quaternion* q(new Quaternion); q->setFromTwoVectors(u,v);
124156
return q;
125157
}
126158

127-
static Coefficients coeffs(const QuaternionUnaligned& self)
128-
{
129-
return self.coeffs();
130-
}
131-
132-
static void setFromTwoVectors(QuaternionUnaligned& self, const Vector3& u, const Vector3& v)
133-
{
134-
self.setFromTwoVectors(u,v);
135-
}
136-
137-
static bool __eq__(const QuaternionUnaligned& u, const QuaternionUnaligned& v)
159+
static bool __eq__(const Quaternion& u, const Quaternion& v)
138160
{
139161
return u.isApprox(v,1e-9);
140162
}
141-
static bool __ne__(const QuaternionUnaligned& u, const QuaternionUnaligned& v)
163+
164+
static bool __ne__(const Quaternion& u, const Quaternion& v)
142165
{
143166
return !__eq__(u,v);
144167
}
145168

146-
static double __getitem__(const QuaternionUnaligned & self, int idx)
169+
static Scalar __getitem__(const Quaternion & self, int idx)
147170
{
148-
if((idx<0) || (idx>4)) throw eigenpy::ExceptionIndex(idx,0,4);
149-
if(idx==0) return self.x();
150-
else if(idx==1) return self.y();
151-
else if(idx==2) return self.z();
152-
else return self.w();
171+
if((idx<0) || (idx>=4)) throw eigenpy::ExceptionIndex(idx,0,3);
172+
return self.coeffs()[idx];
153173
}
154174

155-
static void __setitem__(QuaternionUnaligned& self, int idx, double value)
175+
static void __setitem__(Quaternion& self, int idx, const Scalar value)
156176
{
157-
if((idx<0) || (idx>4)) throw eigenpy::ExceptionIndex(idx,0,4);
158-
if(idx==0) self.x()=value;
159-
else if(idx==1) self.y()=value;
160-
else if(idx==2) self.z()=value;
161-
else self.w()=value;
177+
if((idx<0) || (idx>=4)) throw eigenpy::ExceptionIndex(idx,0,3);
178+
self.coeffs()[idx] = value;
162179
}
163180

164-
static int __len__() { return 4; }
181+
static int __len__() { return 4; }
182+
static Vector3 vec(const Quaternion & self) { return self.vec(); }
183+
184+
static std::string print(const Quaternion & self)
185+
{
186+
std::stringstream ss;
187+
ss << "(x,y,z,w) = " << self.coeffs().transpose() << std::endl;
188+
189+
return ss.str();
190+
}
165191

166192
public:
167193

168194
static void expose()
169195
{
170-
bp::class_<QuaternionUnaligned>("Quaternion",
171-
"Quaternion representing rotation.\n\n"
172-
"Supported operations "
173-
"('q is a Quaternion, 'v' is a Vector3): "
174-
"'q*q' (rotation composition), "
175-
"'q*=q', "
176-
"'q*v' (rotating 'v' by 'q'), "
177-
"'q==q', 'q!=q', 'q[0..3]'.",
178-
bp::init<>())
179-
.def(QuaternionVisitor<Quaternion>())
180-
;
196+
bp::class_<Quaternion>("Quaternion",
197+
"Quaternion representing rotation.\n\n"
198+
"Supported operations "
199+
"('q is a Quaternion, 'v' is a Vector3): "
200+
"'q*q' (rotation composition), "
201+
"'q*=q', "
202+
"'q*v' (rotating 'v' by 'q'), "
203+
"'q==q', 'q!=q', 'q[0..3]'.",
204+
bp::no_init)
205+
.def(QuaternionVisitor<Quaternion>())
206+
;
181207

182-
#ifndef EIGENPY_ALIGNED
183-
bp::to_python_converter< Quaternion,QuaternionVisitor<Quaternion> >();
184-
#endif
185208
}
186209

187210
};

0 commit comments

Comments
 (0)