5 #ifndef __eigenpy_quaternion_hpp__
6 #define __eigenpy_quaternion_hpp__
17 template <
typename Quaternion>
18 struct rvalue_from_python_data<
Eigen::QuaternionBase<Quaternion> const&>
23 template <
class Quaternion>
24 struct implicit<Quaternion,
Eigen::QuaternionBase<Quaternion> > {
26 typedef Eigen::QuaternionBase<Quaternion>
Target;
33 return implicit_rvalue_convertible_from_python(
34 obj, registered<Source>::converters)
39 static void construct(PyObject* obj, rvalue_from_python_stage1_data*
data) {
40 void* storage = ((rvalue_from_python_storage<Target>*)
data)->storage.bytes;
42 arg_from_python<Source> get_source(obj);
43 bool convertible = get_source.convertible();
44 BOOST_VERIFY(convertible);
46 new (storage)
Source(get_source());
49 data->convertible = storage;
62 std::ostringstream oss;
63 oss <<
"Index " << index <<
" out of range " << imin <<
".." << imax <<
".";
68 template <
typename QuaternionDerived>
71 template <
typename Scalar,
int Options>
78 const Scalar& prec = Eigen::NumTraits<Scalar>::dummy_precision()) {
79 return self.isApprox(other, prec);
83 template <
typename Quaternion>
84 class QuaternionVisitor
85 :
public bp::def_visitor<QuaternionVisitor<Quaternion> > {
88 typedef typename QuaternionBase::Scalar
Scalar;
90 typedef typename QuaternionBase::Vector3
Vector3;
92 typedef typename QuaternionBase::Matrix3
Matrix3;
94 typedef typename QuaternionBase::AngleAxisType
AngleAxis;
100 template <class PyClass>
101 void visit(PyClass& cl)
const {
104 bp::default_call_policies(), (bp::arg(
"R"))),
105 "Initialize from rotation matrix.\n"
106 "\tR : a rotation matrix 3x3.")
109 bp::default_call_policies(), (bp::arg(
"aa"))),
110 "Initialize from an angle axis.\n"
111 "\taa: angle axis object.")
114 bp::default_call_policies(),
116 "Copy constructor.\n"
117 "\tquat: a quaternion.")
120 bp::default_call_policies(),
121 (bp::arg(
"u"), bp::arg(
"v"))),
122 "Initialize from two vectors u and v")
125 bp::default_call_policies(),
127 "Initialize from a vector 4D.\n"
128 "\tvec4 : a 4D vector representing quaternion coefficients in the "
132 "Default constructor")
134 bp::make_constructor(
136 bp::default_call_policies(),
137 (bp::arg(
"w"), bp::arg(
"x"), bp::arg(
"y"), bp::arg(
"z"))),
138 "Initialize from coefficients.\n\n"
139 "... note:: The order of coefficients is *w*, *x*, *y*, *z*. "
140 "The [] operator numbers them differently, 0...4 for *x* *y* *z* "
143 .add_property(
"x", &QuaternionVisitor::getCoeff<0>,
144 &QuaternionVisitor::setCoeff<0>,
"The x coefficient.")
145 .add_property(
"y", &QuaternionVisitor::getCoeff<1>,
146 &QuaternionVisitor::setCoeff<1>,
"The y coefficient.")
147 .add_property(
"z", &QuaternionVisitor::getCoeff<2>,
148 &QuaternionVisitor::setCoeff<2>,
"The z coefficient.")
149 .add_property(
"w", &QuaternionVisitor::getCoeff<3>,
150 &QuaternionVisitor::setCoeff<3>,
"The w coefficient.")
153 isApproxQuaternion_overload(
154 bp::args(
"self",
"other",
"prec"),
155 "Returns true if *this is approximately equal to other, "
156 "within the precision determined by prec."))
160 (
const Vector4& (Quaternion::*)()
const) & Quaternion::coeffs,
161 bp::arg(
"self"),
"Returns a vector of the coefficients (x,y,z,w)",
162 bp::return_internal_reference<>())
163 .def(
"matrix", &Quaternion::matrix, bp::arg(
"self"),
164 "Returns an equivalent 3x3 rotation matrix. Similar to "
166 .def(
"toRotationMatrix", &Quaternion::toRotationMatrix,
168 "Returns an equivalent rotation matrix.")
171 ((bp::arg(
"self"), bp::arg(
"a"), bp::arg(
"b"))),
172 "Set *this to be the quaternion which transforms a into b through "
175 .def(
"conjugate", &Quaternion::conjugate, bp::arg(
"self"),
176 "Returns the conjugated quaternion.\n"
177 "The conjugate of a quaternion represents the opposite rotation.")
178 .def(
"inverse", &Quaternion::inverse, bp::arg(
"self"),
179 "Returns the quaternion describing the inverse rotation.")
180 .def(
"setIdentity", &Quaternion::setIdentity, bp::arg(
"self"),
181 "Set *this to the identity rotation.", bp::return_self<>())
182 .def(
"norm", &Quaternion::norm, bp::arg(
"self"),
183 "Returns the norm of the quaternion's coefficients.")
184 .def(
"normalize", &Quaternion::normalize, bp::arg(
"self"),
185 "Normalizes the quaternion *this.", bp::return_self<>())
186 .def(
"normalized", &normalized, bp::arg(
"self"),
187 "Returns a normalized copy of *this.",
188 bp::return_value_policy<bp::manage_new_object>())
189 .def(
"squaredNorm", &Quaternion::squaredNorm, bp::arg(
"self"),
190 "Returns the squared norm of the quaternion's coefficients.")
191 .def(
"dot", &Quaternion::template dot<Quaternion>,
192 (bp::arg(
"self"), bp::arg(
"other")),
193 "Returns the dot product of *this with an other Quaternion.\n"
194 "Geometrically speaking, the dot product of two unit quaternions "
195 "corresponds to the cosine of half the angle between the two "
197 .def(
"_transformVector", &Quaternion::_transformVector,
198 (bp::arg(
"self"), bp::arg(
"vector")),
199 "Rotation of a vector by a quaternion.")
200 .def(
"vec", &
vec, bp::arg(
"self"),
201 "Returns a vector expression of the imaginary part (x,y,z).")
202 .def(
"angularDistance",
205 &Quaternion::template angularDistance<Quaternion>,
206 "Returns the angle (in radian) between two rotations.")
207 .def(
"slerp", &
slerp, bp::args(
"self",
"t",
"other"),
208 "Returns the spherical linear interpolation between the two "
209 "quaternions *this and other at the parameter t in [0;1].")
212 .def(bp::self * bp::self)
213 .def(bp::self *= bp::self)
214 .def(bp::self * bp::other<Vector3>())
217 .def(
"__abs__", &Quaternion::norm)
221 .def(
"assign", &assign<Quaternion>, bp::args(
"self",
"quat"),
222 "Set *this from an quaternion quat and returns a reference to "
227 (Quaternion & (Quaternion::*)(
const AngleAxis&)) &
228 Quaternion::operator=,
229 bp::args(
"self",
"aa"),
230 "Set *this from an angle-axis aa and returns a reference to *this.",
232 .def(
"__str__", &
print)
233 .def(
"__repr__", &
print)
241 "Returns the quaternion which transforms a into b through a "
243 bp::return_value_policy<bp::manage_new_object>())
244 .staticmethod(
"FromTwoVectors")
246 "Returns a quaternion representing an identity rotation.",
247 bp::return_value_policy<bp::manage_new_object>())
248 .staticmethod(
"Identity");
252 static Quaternion* normalized(
const Quaternion&
self) {
253 return new Quaternion(
self.normalized());
258 self.coeffs()[i] =
value;
263 return self.coeffs()[i];
268 return self.setFromTwoVectors(
a, b);
271 template <
typename OtherQuat>
272 static Quaternion&
assign(Quaternion&
self,
const OtherQuat&
quat) {
277 Quaternion*
q(
new Quaternion);
283 Quaternion*
q(
new Quaternion(w,
x,
y, z));
288 Quaternion*
q(
new Quaternion(aa));
293 const Eigen::Ref<const Vector3>
v) {
294 Quaternion*
q(
new Quaternion);
295 q->setFromTwoVectors(u,
v);
300 Quaternion*
q(
new Quaternion(other));
307 Quaternion*
q(
new Quaternion(
v[3],
v[0],
v[1],
v[2]));
312 Quaternion*
q(
new Quaternion(
R));
316 static bool __eq__(
const Quaternion& u,
const Quaternion&
v) {
317 return u.coeffs() ==
v.coeffs();
320 static bool __ne__(
const Quaternion& u,
const Quaternion&
v) {
326 return self.coeffs()[idx];
331 self.coeffs()[idx] =
value;
335 static Vector3 vec(
const Quaternion&
self) {
return self.vec(); }
337 static std::string
print(
const Quaternion&
self) {
338 std::stringstream ss;
339 ss <<
"(x,y,z,w) = " <<
self.coeffs().transpose() << std::endl;
345 const Quaternion& other) {
346 return self.slerp(
t, other);
351 #if PY_MAJOR_VERSION == 3 && PY_MINOR_VERSION == 6
354 typedef ::boost::python::detail::not_specified HolderType;
357 bp::class_<Quaternion, HolderType>(
359 "Quaternion representing rotation.\n\n"
360 "Supported operations "
361 "('q is a Quaternion, 'v' is a Vector3): "
362 "'q*q' (rotation composition), "
364 "'q*v' (rotating 'v' by 'q'), "
365 "'q==q', 'q!=q', 'q[0..3]'.",
371 bp::implicitly_convertible<Quaternion, QuaternionBase>();
378 #endif // ifndef __eigenpy_quaternion_hpp__