.. _program_listing_file__tmp_ws_src_eigenpy_include_eigenpy_quaternion.hpp:

Program Listing for File quaternion.hpp
=======================================

|exhale_lsh| :ref:`Return to documentation for file <file__tmp_ws_src_eigenpy_include_eigenpy_quaternion.hpp>` (``/tmp/ws/src/eigenpy/include/eigenpy/quaternion.hpp``)

.. |exhale_lsh| unicode:: U+021B0 .. UPWARDS ARROW WITH TIP LEFTWARDS

.. code-block:: cpp

   /*
    * 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>());
   
       // Cast to Eigen::QuaternionBase and vice-versa
       bp::implicitly_convertible<Quaternion, QuaternionBase>();
       //      bp::implicitly_convertible<QuaternionBase,Quaternion >();
     }
   };
   
   }  // namespace eigenpy
   
   #endif  // ifndef __eigenpy_quaternion_hpp__