5 #ifndef __eigenpy_quaternion_hpp__ 6 #define __eigenpy_quaternion_hpp__ 11 #include <Eigen/Geometry> 15 namespace boost {
namespace python {
namespace converter {
18 template<
typename Quaternion>
19 struct rvalue_from_python_data<
Eigen::QuaternionBase<Quaternion> const &>
25 template <
class Quaternion>
26 struct implicit<Quaternion,
Eigen::QuaternionBase<Quaternion> >
29 typedef Eigen::QuaternionBase<Quaternion>
Target;
37 return implicit_rvalue_convertible_from_python(obj, registered<Source>::converters)
41 static void construct(PyObject* obj, rvalue_from_python_stage1_data*
data)
43 void* storage = ((rvalue_from_python_storage<Target>*)data)->storage.bytes;
45 arg_from_python<Source> get_source(obj);
46 bool convertible = get_source.convertible();
47 BOOST_VERIFY(convertible);
49 new (storage) Source(get_source());
52 data->convertible = storage;
66 std::ostringstream oss; oss <<
"Index " << index <<
" out of range " << imin <<
".."<< imax <<
".";
75 template<
typename Scalar,
int Options>
84 static inline bool isApprox(
const Quaternion &
self,
const Quaternion & other,
85 const Scalar & prec = Eigen::NumTraits<Scalar>::dummy_precision())
87 return self.isApprox(other,prec);
91 template<
typename Quaternion>
93 :
public bp::def_visitor< QuaternionVisitor<Quaternion> >
97 typedef typename QuaternionBase::Scalar
Scalar;
99 typedef typename QuaternionBase::Vector3
Vector3;
100 typedef typename Eigen::Matrix<Scalar,4,1>
Vector4;
101 typedef typename QuaternionBase::Matrix3
Matrix3;
103 typedef typename QuaternionBase::AngleAxisType
AngleAxis;
109 template<class PyClass>
110 void visit(PyClass& cl)
const 113 .def(bp::init<>(bp::arg(
"self"),
"Default constructor"))
114 .def(bp::init<Matrix3>((bp::arg(
"self"),bp::arg(
"R")),
115 "Initialize from rotation matrix.\n" 116 "\tR : a rotation matrix 3x3."))
117 .def(bp::init<Vector4>((bp::arg(
"self"),bp::arg(
"vec4")),
118 "Initialize from a vector 4D.\n" 119 "\tvec4 : a 4D vector representing quaternion coefficients in the order xyzw."))
120 .def(bp::init<AngleAxis>((bp::arg(
"self"),bp::arg(
"aa")),
121 "Initialize from an angle axis.\n" 122 "\taa: angle axis object."))
123 .def(bp::init<Quaternion>((bp::arg(
"self"),bp::arg(
"quat")),
124 "Copy constructor.\n" 125 "\tquat: a quaternion."))
126 .def(
"__init__",bp::make_constructor(&QuaternionVisitor::FromTwoVectors,
127 bp::default_call_policies(),
128 (bp::arg(
"u"),bp::arg(
"v"))),
129 "Initialize from two vectors u and v")
130 .def(bp::init<Scalar,Scalar,Scalar,Scalar>
131 ((bp::arg(
"self"),bp::arg(
"w"),bp::arg(
"x"),bp::arg(
"y"),bp::arg(
"z")),
132 "Initialize from coefficients.\n\n" 133 "... note:: The order of coefficients is *w*, *x*, *y*, *z*. " 134 "The [] operator numbers them differently, 0...4 for *x* *y* *z* *w*!"))
137 &QuaternionVisitor::getCoeff<0>,
138 &QuaternionVisitor::setCoeff<0>,
"The x coefficient.")
140 &QuaternionVisitor::getCoeff<1>,
141 &QuaternionVisitor::setCoeff<1>,
"The y coefficient.")
143 &QuaternionVisitor::getCoeff<2>,
144 &QuaternionVisitor::setCoeff<2>,
"The z coefficient.")
146 &QuaternionVisitor::getCoeff<3>,
147 &QuaternionVisitor::setCoeff<3>,
"The w coefficient.")
151 isApproxQuaternion_overload(bp::args(
"self",
"other",
"prec"),
152 "Returns true if *this is approximately equal to other, within the precision determined by prec."))
155 .def(
"coeffs",(
const Vector4 & (Quaternion::*)()
const)&Quaternion::coeffs,
157 "Returns a vector of the coefficients (x,y,z,w)",
158 bp::return_internal_reference<>())
161 "Returns an equivalent 3x3 rotation matrix. Similar to toRotationMatrix.")
162 .def(
"toRotationMatrix",&Quaternion::toRotationMatrix,
164 "Returns an equivalent 3x3 rotation matrix.")
166 .def(
"setFromTwoVectors",&setFromTwoVectors,((bp::arg(
"self"),bp::arg(
"a"),bp::arg(
"b"))),
167 "Set *this to be the quaternion which transforms a into b through a rotation." 168 ,bp::return_self<>())
169 .def(
"conjugate",&Quaternion::conjugate,
171 "Returns the conjugated quaternion.\n" 172 "The conjugate of a quaternion represents the opposite rotation.")
173 .def(
"inverse",&Quaternion::inverse,
175 "Returns the quaternion describing the inverse rotation.")
176 .def(
"setIdentity",&Quaternion::setIdentity,
178 "Set *this to the idendity rotation.",bp::return_self<>())
179 .def(
"norm",&Quaternion::norm,
181 "Returns the norm of the quaternion's coefficients.")
182 .def(
"normalize",&Quaternion::normalize,
184 "Normalizes the quaternion *this.")
185 .def(
"normalized",&Quaternion::normalized,
187 "Returns a normalized copy of *this.")
188 .def(
"squaredNorm",&Quaternion::squaredNorm,
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 corresponds to the cosine of half the angle between the two rotations.")
195 .def(
"_transformVector",&Quaternion::_transformVector,
196 (bp::arg(
"self"),bp::arg(
"vector")),
197 "Rotation of a vector by a quaternion.")
200 "Returns a vector expression of the imaginary part (x,y,z).")
201 .def(
"angularDistance",
203 &Quaternion::template angularDistance<Quaternion>,
204 "Returns the angle (in radian) between two rotations.")
205 .def(
"slerp",&slerp,bp::args(
"self",
"t",
"other"),
206 "Returns the spherical linear interpolation between the two quaternions *this and other at the parameter t in [0;1].")
209 .def(bp::self * bp::self)
210 .def(bp::self *= bp::self)
211 .def(bp::self * bp::other<Vector3>())
212 .def(
"__eq__",&QuaternionVisitor::__eq__)
213 .def(
"__ne__",&QuaternionVisitor::__ne__)
214 .def(
"__abs__",&Quaternion::norm)
215 .def(
"__len__",&QuaternionVisitor::__len__).staticmethod(
"__len__")
216 .def(
"__setitem__",&QuaternionVisitor::__setitem__)
217 .def(
"__getitem__",&QuaternionVisitor::__getitem__)
218 .def(
"assign",&assign<Quaternion>,
219 bp::args(
"self",
"quat"),
220 "Set *this from an quaternion quat and returns a reference to *this.",
222 .def(
"assign",(Quaternion & (Quaternion::*)(
const AngleAxis &))&Quaternion::operator=,
223 bp::args(
"self",
"aa"),
224 "Set *this from an angle-axis aa and returns a reference to *this.",
226 .def(
"__str__",&print)
227 .def(
"__repr__",&print)
232 .def(
"FromTwoVectors",&FromTwoVectors,
234 "Returns the quaternion which transforms a into b through a rotation.",
235 bp::return_value_policy<bp::manage_new_object>())
236 .staticmethod(
"FromTwoVectors")
237 .def(
"Identity",&Quaternion::Identity,
238 "Returns a quaternion representing an identity rotation.")
239 .staticmethod(
"Identity")
248 static Scalar
getCoeff(Quaternion &
self) {
return self.coeffs()[i]; }
251 {
return self.setFromTwoVectors(a,b); }
253 template<
typename OtherQuat>
254 static Quaternion &
assign(Quaternion &
self,
const OtherQuat &
quat)
255 {
return self =
quat; }
259 Quaternion*
q(
new Quaternion); q->setFromTwoVectors(u,v);
263 static bool __eq__(
const Quaternion & u,
const Quaternion &
v)
265 return u.coeffs() == v.coeffs();
268 static bool __ne__(
const Quaternion& u,
const Quaternion&
v)
276 return self.coeffs()[idx];
282 self.coeffs()[idx] =
value;
286 static Vector3
vec(
const Quaternion &
self) {
return self.vec(); }
288 static std::string
print(
const Quaternion &
self)
290 std::stringstream ss;
291 ss <<
"(x,y,z,w) = " <<
self.coeffs().transpose() << std::endl;
296 static Quaternion
slerp(
const Quaternion &
self,
const Scalar t,
const Quaternion & other)
297 {
return self.slerp(t,other); }
303 bp::class_<Quaternion>(
"Quaternion",
304 "Quaternion representing rotation.\n\n" 305 "Supported operations " 306 "('q is a Quaternion, 'v' is a Vector3): " 307 "'q*q' (rotation composition), " 309 "'q*v' (rotating 'v' by 'q'), " 310 "'q==q', 'q!=q', 'q[0..3]'.",
315 bp::implicitly_convertible<Quaternion,QuaternionBase >();
323 #endif // ifndef __eigenpy_quaternion_hpp__
Eigen::QuaternionBase< Quaternion > Target
#define EIGENPY_RVALUE_FROM_PYTHON_DATA_INIT(type)
Eigen::Matrix< Scalar, 4, 1 > Vector4
boost::python::object matrix()
static Quaternion slerp(const Quaternion &self, const Scalar t, const Quaternion &other)
static void * convertible(PyObject *obj)
static void setCoeff(Quaternion &self, Scalar value)
Quaternion::Coefficients Coefficients
static bool __eq__(const Quaternion &u, const Quaternion &v)
static Quaternion & setFromTwoVectors(Quaternion &self, const Vector3 &a, const Vector3 &b)
QuaternionBase::Vector3 Vector3
QuaternionBase::Scalar Scalar
static bool isApprox(const Quaternion &self, const Quaternion &other, const Scalar &prec=Eigen::NumTraits< Scalar >::dummy_precision())
Eigen::QuaternionBase< Quaternion > QuaternionBase
static Quaternion * FromTwoVectors(const Vector3 &u, const Vector3 &v)
static Scalar __getitem__(const Quaternion &self, int idx)
Allows a template specialization.
Eigen::Quaternion< Scalar, Options > Quaternion
static Vector3 vec(const Quaternion &self)
void expose()
Call the expose function of a given type T.
static std::string print(const Quaternion &self)
static void __setitem__(Quaternion &self, int idx, const Scalar value)
ExceptionIndex(int index, int imin, int imax)
static Quaternion & assign(Quaternion &self, const OtherQuat &quat)
QuaternionBase::AngleAxisType AngleAxis
static bool __ne__(const Quaternion &u, const Quaternion &v)
QuaternionBase::Matrix3 Matrix3
static Scalar getCoeff(Quaternion &self)
static void construct(PyObject *obj, rvalue_from_python_stage1_data *data)