Program Listing for File quaternion.hpp
↰ Return to documentation for file (include/eigenpy/quaternion.hpp)
/*
* Copyright 2014-2023 CNRS INRIA
*/
#ifndef __eigenpy_quaternion_hpp__
#define __eigenpy_quaternion_hpp__
#include "eigenpy/eigenpy.hpp"
#include "eigenpy/exception.hpp"
#include "eigenpy/eigen-from-python.hpp"
namespace boost {
namespace python {
namespace converter {
template <typename Quaternion>
struct rvalue_from_python_data<Eigen::QuaternionBase<Quaternion> const&>
: ::eigenpy::rvalue_from_python_data<Quaternion const&> {
EIGENPY_RVALUE_FROM_PYTHON_DATA_INIT(Quaternion const&)
};
template <class Quaternion>
struct implicit<Quaternion, Eigen::QuaternionBase<Quaternion> > {
typedef Quaternion Source;
typedef Eigen::QuaternionBase<Quaternion> Target;
static void* convertible(PyObject* obj) {
// Find a converter which can produce a Source instance from
// obj. The user has told us that Source can be converted to
// Target, and instantiating construct() below, ensures that
// at compile-time.
return implicit_rvalue_convertible_from_python(
obj, registered<Source>::converters)
? obj
: 0;
}
static void construct(PyObject* obj, rvalue_from_python_stage1_data* data) {
void* storage = ((rvalue_from_python_storage<Target>*)data)->storage.bytes;
arg_from_python<Source> get_source(obj);
bool convertible = get_source.convertible();
BOOST_VERIFY(convertible);
new (storage) Source(get_source());
// record successful construction
data->convertible = storage;
}
};
} // namespace converter
} // namespace python
} // namespace boost
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();
}
};
template <typename QuaternionDerived>
class QuaternionVisitor;
template <typename Scalar, int Options>
struct call<Eigen::Quaternion<Scalar, Options> > {
typedef Eigen::Quaternion<Scalar, Options> Quaternion;
static inline void expose() { QuaternionVisitor<Quaternion>::expose(); }
static inline bool isApprox(
const Quaternion& self, const Quaternion& other,
const Scalar& prec = Eigen::NumTraits<Scalar>::dummy_precision()) {
return self.isApprox(other, prec);
}
};
template <typename Quaternion>
class QuaternionVisitor
: public bp::def_visitor<QuaternionVisitor<Quaternion> > {
typedef Eigen::QuaternionBase<Quaternion> QuaternionBase;
typedef typename QuaternionBase::Scalar Scalar;
typedef typename Quaternion::Coefficients Coefficients;
typedef typename QuaternionBase::Vector3 Vector3;
typedef Coefficients Vector4;
typedef typename QuaternionBase::Matrix3 Matrix3;
typedef typename QuaternionBase::AngleAxisType AngleAxis;
BOOST_PYTHON_FUNCTION_OVERLOADS(isApproxQuaternion_overload,
call<Quaternion>::isApprox, 2, 3)
public:
template <class PyClass>
void visit(PyClass& cl) const {
cl.def("__init__",
bp::make_constructor(&QuaternionVisitor::FromRotationMatrix,
bp::default_call_policies(), (bp::arg("R"))),
"Initialize from rotation matrix.\n"
"\tR : a rotation matrix 3x3.")
.def("__init__",
bp::make_constructor(&QuaternionVisitor::FromAngleAxis,
bp::default_call_policies(), (bp::arg("aa"))),
"Initialize from an angle axis.\n"
"\taa: angle axis object.")
.def("__init__",
bp::make_constructor(&QuaternionVisitor::FromOtherQuaternion,
bp::default_call_policies(),
(bp::arg("quat"))),
"Copy constructor.\n"
"\tquat: a quaternion.")
.def("__init__",
bp::make_constructor(&QuaternionVisitor::FromTwoVectors,
bp::default_call_policies(),
(bp::arg("u"), bp::arg("v"))),
"Initialize from two vectors u and v")
.def("__init__",
bp::make_constructor(&QuaternionVisitor::FromOneVector,
bp::default_call_policies(),
(bp::arg("vec4"))),
"Initialize from a vector 4D.\n"
"\tvec4 : a 4D vector representing quaternion coefficients in the "
"order xyzw.")
.def("__init__",
bp::make_constructor(&QuaternionVisitor::DefaultConstructor),
"Default constructor")
.def("__init__",
bp::make_constructor(
&QuaternionVisitor::FromCoefficients,
bp::default_call_policies(),
(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", &QuaternionVisitor::getCoeff<0>,
&QuaternionVisitor::setCoeff<0>, "The x coefficient.")
.add_property("y", &QuaternionVisitor::getCoeff<1>,
&QuaternionVisitor::setCoeff<1>, "The y coefficient.")
.add_property("z", &QuaternionVisitor::getCoeff<2>,
&QuaternionVisitor::setCoeff<2>, "The z coefficient.")
.add_property("w", &QuaternionVisitor::getCoeff<3>,
&QuaternionVisitor::setCoeff<3>, "The w coefficient.")
.def("isApprox", &call<Quaternion>::isApprox,
isApproxQuaternion_overload(
bp::args("self", "other", "prec"),
"Returns true if *this is approximately equal to other, "
"within the precision determined by prec."))
/* --- Methods --- */
.def("coeffs",
(const Vector4& (Quaternion::*)() const) & Quaternion::coeffs,
bp::arg("self"), "Returns a vector of the coefficients (x,y,z,w)",
bp::return_internal_reference<>())
.def("matrix", &Quaternion::matrix, bp::arg("self"),
"Returns an equivalent 3x3 rotation matrix. Similar to "
"toRotationMatrix.")
.def("toRotationMatrix", &Quaternion::toRotationMatrix,
// bp::arg("self"), // Bug in Boost.Python
"Returns an equivalent rotation matrix.")
.def("setFromTwoVectors", &setFromTwoVectors,
((bp::arg("self"), bp::arg("a"), bp::arg("b"))),
"Set *this to be the quaternion which transforms a into b through "
"a rotation.",
bp::return_self<>())
.def("conjugate", &Quaternion::conjugate, bp::arg("self"),
"Returns the conjugated quaternion.\n"
"The conjugate of a quaternion represents the opposite rotation.")
.def("inverse", &Quaternion::inverse, bp::arg("self"),
"Returns the quaternion describing the inverse rotation.")
.def("setIdentity", &Quaternion::setIdentity, bp::arg("self"),
"Set *this to the identity rotation.", bp::return_self<>())
.def("norm", &Quaternion::norm, bp::arg("self"),
"Returns the norm of the quaternion's coefficients.")
.def("normalize", &Quaternion::normalize, bp::arg("self"),
"Normalizes the quaternion *this.", bp::return_self<>())
.def("normalized", &normalized, bp::arg("self"),
"Returns a normalized copy of *this.",
bp::return_value_policy<bp::manage_new_object>())
.def("squaredNorm", &Quaternion::squaredNorm, bp::arg("self"),
"Returns the squared norm of the quaternion's coefficients.")
.def("dot", &Quaternion::template dot<Quaternion>,
(bp::arg("self"), bp::arg("other")),
"Returns the dot product of *this with an other Quaternion.\n"
"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("self"), bp::arg("vector")),
"Rotation of a vector by a quaternion.")
.def("vec", &vec, bp::arg("self"),
"Returns a vector expression of the imaginary part (x,y,z).")
.def("angularDistance",
// (bp::arg("self"),bp::arg("other")), // Bug in
// Boost.Python
&Quaternion::template angularDistance<Quaternion>,
"Returns the angle (in radian) between two rotations.")
.def("slerp", &slerp, bp::args("self", "t", "other"),
"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__)
.def("__setitem__", &QuaternionVisitor::__setitem__)
.def("__getitem__", &QuaternionVisitor::__getitem__)
.def("assign", &assign<Quaternion>, bp::args("self", "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::args("self", "aa"),
"Set *this from an angle-axis aa and returns a reference to *this.",
bp::return_self<>())
.def("__str__", &print)
.def("__repr__", &print)
// .def("FromTwoVectors",&Quaternion::template
// FromTwoVectors<Vector3,Vector3>,
// bp::args("a","b"),
// "Returns the quaternion which transform a into b through a
// rotation.")
.def("FromTwoVectors", &FromTwoVectors, bp::args("a", "b"),
"Returns the quaternion which transforms a into b through a "
"rotation.",
bp::return_value_policy<bp::manage_new_object>())
.staticmethod("FromTwoVectors")
.def("Identity", &Identity,
"Returns a quaternion representing an identity rotation.",
bp::return_value_policy<bp::manage_new_object>())
.staticmethod("Identity");
}
private:
static Quaternion* normalized(const Quaternion& self) {
return new Quaternion(self.normalized());
}
template <int i>
static void setCoeff(Quaternion& self, Scalar value) {
self.coeffs()[i] = value;
}
template <int i>
static Scalar getCoeff(Quaternion& self) {
return self.coeffs()[i];
}
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;
}
static Quaternion* Identity() {
Quaternion* q(new Quaternion);
q->setIdentity();
return q;
}
static Quaternion* FromCoefficients(Scalar w, Scalar x, Scalar y, Scalar z) {
Quaternion* q(new Quaternion(w, x, y, z));
return q;
}
static Quaternion* FromAngleAxis(const AngleAxis& aa) {
Quaternion* q(new Quaternion(aa));
return q;
}
static Quaternion* FromTwoVectors(const Eigen::Ref<const Vector3> u,
const Eigen::Ref<const Vector3> v) {
Quaternion* q(new Quaternion);
q->setFromTwoVectors(u, v);
return q;
}
static Quaternion* FromOtherQuaternion(const Quaternion& other) {
Quaternion* q(new Quaternion(other));
return q;
}
static Quaternion* DefaultConstructor() { return new Quaternion; }
static Quaternion* FromOneVector(const Eigen::Ref<const Vector4> v) {
Quaternion* q(new Quaternion(v[3], v[0], v[1], v[2]));
return q;
}
static Quaternion* FromRotationMatrix(const Eigen::Ref<const Matrix3> R) {
Quaternion* q(new Quaternion(R));
return q;
}
static bool __eq__(const Quaternion& u, const Quaternion& v) {
return u.coeffs() == v.coeffs();
}
static bool __ne__(const Quaternion& u, const Quaternion& v) {
return !__eq__(u, v);
}
static Scalar __getitem__(const Quaternion& self, int idx) {
if ((idx < 0) || (idx >= 4)) throw eigenpy::ExceptionIndex(idx, 0, 3);
return self.coeffs()[idx];
}
static void __setitem__(Quaternion& self, int idx, const Scalar value) {
if ((idx < 0) || (idx >= 4)) throw eigenpy::ExceptionIndex(idx, 0, 3);
self.coeffs()[idx] = value;
}
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();
}
static Quaternion slerp(const Quaternion& self, const Scalar t,
const Quaternion& other) {
return self.slerp(t, other);
}
public:
static void expose() {
#if PY_MAJOR_VERSION == 3 && PY_MINOR_VERSION == 6
typedef EIGENPY_SHARED_PTR_HOLDER_TYPE(Quaternion) HolderType;
#else
typedef ::boost::python::detail::not_specified HolderType;
#endif
bp::class_<Quaternion, HolderType>(
"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>())
.def(IdVisitor<Quaternion>());
// Cast to Eigen::QuaternionBase and vice-versa
bp::implicitly_convertible<Quaternion, QuaternionBase>();
// bp::implicitly_convertible<QuaternionBase,Quaternion >();
}
};
} // namespace eigenpy
#endif // ifndef __eigenpy_quaternion_hpp__