quaternion.hpp

Go to the documentation of this file.

/*
 * 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__