angle-axis.hpp

/*
 * Copyright 2014-2020 CNRS INRIA
 */

#ifndef __eigenpy_angle_axis_hpp__
#define __eigenpy_angle_axis_hpp__

#include "eigenpy/eigenpy.hpp"

#include <Eigen/Core>
#include <Eigen/Geometry>

namespace eigenpy
{

  namespace bp = boost::python;
  
  template<typename AngleAxis> class AngleAxisVisitor;
  
  template<typename Scalar>
  struct call< Eigen::AngleAxis<Scalar> >
  {
    typedef Eigen::AngleAxis<Scalar> AngleAxis;
    
    static inline void expose()
    {
      AngleAxisVisitor<AngleAxis>::expose();
    }
      
    static inline bool isApprox(const AngleAxis & self, const AngleAxis & other,
                                const Scalar & prec = Eigen::NumTraits<Scalar>::dummy_precision())
    {
      return self.isApprox(other,prec);
    }
  };

  template<typename AngleAxis>
  class AngleAxisVisitor
  : public bp::def_visitor< AngleAxisVisitor<AngleAxis> >
  {

    typedef typename AngleAxis::Scalar Scalar;
    typedef typename AngleAxis::Vector3 Vector3;
    typedef typename AngleAxis::Matrix3 Matrix3;
    
    typedef typename Eigen::Quaternion<Scalar,0> Quaternion;
    typedef Eigen::RotationBase<AngleAxis,3> RotationBase;
    
    BOOST_PYTHON_FUNCTION_OVERLOADS(isApproxAngleAxis_overload,call<AngleAxis>::isApprox,2,3)
    
  public:

    template<class PyClass>
    void visit(PyClass& cl) const 
    {
      cl
      .def(bp::init<>(bp::arg("self"),"Default constructor"))
      .def(bp::init<Scalar,Vector3>
           ((bp::arg("self"),bp::arg("angle"),bp::arg("axis")),
            "Initialize from angle and axis."))
      .def(bp::init<Matrix3>
           ((bp::arg("self"),bp::arg("rotation matrix")),
            "Initialize from a rotation matrix"))
      .def(bp::init<Quaternion>((bp::arg("self"),bp::arg("quaternion")),
                                "Initialize from a quaternion."))
      .def(bp::init<AngleAxis>((bp::arg("self"),bp::arg("copy")),
                               "Copy constructor."))
      
      /* --- Properties --- */
      .add_property("axis",
                    bp::make_function((Vector3 & (AngleAxis::*)())&AngleAxis::axis,
                                      bp::return_internal_reference<>()),
                    &AngleAxisVisitor::setAxis,"The rotation axis.")
      .add_property("angle",
                    (Scalar (AngleAxis::*)()const)&AngleAxis::angle,
                    &AngleAxisVisitor::setAngle,"The rotation angle.")
      
      /* --- Methods --- */
      .def("inverse",&AngleAxis::inverse,
           bp::arg("self"),
           "Return the inverse rotation.")
      .def("fromRotationMatrix",&AngleAxis::template fromRotationMatrix<Matrix3>,
           (bp::arg("self"),bp::arg("rotation matrix")),
           "Sets *this from a 3x3 rotation matrix",
           bp::return_self<>())
      .def("toRotationMatrix",
//           bp::arg("self"),
           &AngleAxis::toRotationMatrix,
           "Constructs and returns an equivalent 3x3 rotation matrix.")
      .def("matrix",&AngleAxis::matrix,
           bp::arg("self"),
           "Returns an equivalent rotation matrix.")
      
      .def("isApprox",
           &call<AngleAxis>::isApprox,
           isApproxAngleAxis_overload(bp::args("self","other","prec"),
                                      "Returns true if *this is approximately equal to other, within the precision determined by prec."))
      
      /* --- Operators --- */
      .def(bp::self * bp::other<Vector3>())
      .def(bp::self * bp::other<Quaternion>())
      .def(bp::self * bp::self)
      .def("__eq__",&AngleAxisVisitor::__eq__)
      .def("__ne__",&AngleAxisVisitor::__ne__)
      
      .def("__str__",&print)
      .def("__repr__",&print)
      ;
    }
    
  private:

    static void setAxis(AngleAxis & self, const Vector3 & axis)
    { self.axis() = axis; }

    static void setAngle( AngleAxis & self, const Scalar & angle)
    { self.angle() = angle; }

    static bool __eq__(const AngleAxis & u, const AngleAxis & v)
    { return u.axis() == v.axis() && v.angle() == u.angle(); }
    
    static bool __ne__(const AngleAxis & u, const AngleAxis & v)
    { return !__eq__(u,v); }

    static std::string print(const AngleAxis & self)
    {
      std::stringstream ss;
      ss << "angle: " << self.angle() << std::endl;
      ss << "axis: " << self.axis().transpose() << std::endl;
      
      return ss.str();
    }

  public:

    static void expose()
    {
      bp::class_<AngleAxis>("AngleAxis",
                            "AngleAxis representation of a rotation.\n\n",
                            bp::no_init)
      .def(AngleAxisVisitor<AngleAxis>());
      
      // Cast to Eigen::RotationBase
      bp::implicitly_convertible<AngleAxis,RotationBase>();
    }

  };

} // namespace eigenpy

#endif //ifndef __eigenpy_angle_axis_hpp__