angle-axis.hpp 4.25 KB
Newer Older
1
/*
2
3
 * Copyright 2014-2019, CNRS
 * Copyright 2018-2019, INRIA
4
5
6
7
 */

#ifndef __eigenpy_angle_axis_hpp__
#define __eigenpy_angle_axis_hpp__
Nicolas Mansard's avatar
Nicolas Mansard committed
8

9
10
#include "eigenpy/fwd.hpp"

11
#include <boost/python.hpp>
Nicolas Mansard's avatar
Nicolas Mansard committed
12
13
#include <Eigen/Core>
#include <Eigen/Geometry>
14

Nicolas Mansard's avatar
Nicolas Mansard committed
15
16
17
18
namespace eigenpy
{

  namespace bp = boost::python;
19
20
21
  
  template<typename AngleAxis> class AngleAxisVisitor;
  
22
23
  template<typename Scalar>
  struct call< Eigen::AngleAxis<Scalar> >
24
  {
25
26
    typedef Eigen::AngleAxis<Scalar> AngleAxis;
    static inline void expose()
27
    {
28
29
      AngleAxisVisitor<AngleAxis>::expose();
    }
Justin Carpentier's avatar
Justin Carpentier committed
30
31
32
33
34
35
      
    static inline bool isApprox(const AngleAxis & self, const AngleAxis & other,
                                const Scalar & prec = Eigen::NumTraits<Scalar>::dummy_precision())
    {
      return self.isApprox(other,prec);
    }
36
  };
Nicolas Mansard's avatar
Nicolas Mansard committed
37

Justin Carpentier's avatar
Justin Carpentier committed
38
39
  BOOST_PYTHON_FUNCTION_OVERLOADS(isApproxAngleAxis_overload,call<Eigen::AngleAxisd>::isApprox,2,3)

jcarpent's avatar
jcarpent committed
40
  template<typename AngleAxis>
Nicolas Mansard's avatar
Nicolas Mansard committed
41
  class AngleAxisVisitor
42
  : public bp::def_visitor< AngleAxisVisitor<AngleAxis> >
Nicolas Mansard's avatar
Nicolas Mansard committed
43
44
  {

jcarpent's avatar
jcarpent committed
45
46
47
48
49
50
    typedef typename AngleAxis::Scalar Scalar;
    typedef typename AngleAxis::Vector3 Vector3;
    typedef typename AngleAxis::Matrix3 Matrix3;
    
    typedef typename Eigen::Quaternion<Scalar,0> Quaternion;
    
Nicolas Mansard's avatar
Nicolas Mansard committed
51
52
53
54
55
56
  public:

    template<class PyClass>
    void visit(PyClass& cl) const 
    {
      cl
jcarpent's avatar
jcarpent committed
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
      .def(bp::init<>("Default constructor"))
      .def(bp::init<Scalar,Vector3>
           ((bp::arg("angle"),bp::arg("axis")),
            "Initialize from angle and axis."))
      .def(bp::init<Matrix3>
           ((bp::arg("rotationMatrix")),
            "Initialize from a rotation matrix"))
      .def(bp::init<Quaternion>(bp::arg("quaternion"),"Initialize from a quaternion."))
      .def(bp::init<AngleAxis>(bp::arg("copy"),"Copy constructor."))
      
      /* --- Properties --- */
      .add_property("axis",
                    bp::make_function((const Vector3 & (AngleAxis::*)()const)&AngleAxis::axis,
                                      bp::return_value_policy<bp::copy_const_reference>()),
                    &AngleAxisVisitor::setAxis,"The rotation axis.")
      .add_property("angle",
                    (Scalar (AngleAxis::*)()const)&AngleAxis::angle,
                    &AngleAxisVisitor::setAngle,"The rotation angle.")
      
      /* --- Methods --- */
      .def("inverse",&AngleAxis::inverse,"Return the inverse rotation.")
      .def("fromRotationMatrix",&AngleAxis::template fromRotationMatrix<Matrix3>,
           bp::arg("Sets *this from a 3x3 rotation matrix."),bp::return_self<>())
      .def("toRotationMatrix",&AngleAxis::toRotationMatrix,"Constructs and returns an equivalent 3x3 rotation matrix.")
      .def("matrix",&AngleAxis::matrix,"Returns an equivalent rotation matrix.")
Justin Carpentier's avatar
Justin Carpentier committed
82
83
84
85
86
      
      .def("isApprox",
           &call<AngleAxis>::isApprox,
           isApproxAngleAxis_overload(bp::args("other","prec"),
                                      "Returns true if *this is approximately equal to other, within the precision determined by prec."))
jcarpent's avatar
jcarpent committed
87
88
89
90
91
92
93
94
95
96
97
      
      /* --- 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)
      ;
Nicolas Mansard's avatar
Nicolas Mansard committed
98
    }
jcarpent's avatar
jcarpent committed
99
    
Nicolas Mansard's avatar
Nicolas Mansard committed
100
101
  private:

jcarpent's avatar
jcarpent committed
102
103
    static void setAxis(AngleAxis & self, const Vector3 & axis)
    { self.axis() = axis; }
Nicolas Mansard's avatar
Nicolas Mansard committed
104

jcarpent's avatar
jcarpent committed
105
106
    static void setAngle( AngleAxis & self, const Scalar & angle)
    { self.angle() = angle; }
Nicolas Mansard's avatar
Nicolas Mansard committed
107

jcarpent's avatar
jcarpent committed
108
    static bool __eq__(const AngleAxis & u, const AngleAxis & v)
Justin Carpentier's avatar
Justin Carpentier committed
109
110
    { return u.axis() == v.axis() && v.angle() == u.angle(); }
    
jcarpent's avatar
jcarpent committed
111
112
    static bool __ne__(const AngleAxis & u, const AngleAxis & v)
    { return !__eq__(u,v); }
Nicolas Mansard's avatar
Nicolas Mansard committed
113

jcarpent's avatar
jcarpent committed
114
    static std::string print(const AngleAxis & self)
Nicolas Mansard's avatar
Nicolas Mansard committed
115
    {
jcarpent's avatar
jcarpent committed
116
117
118
119
120
      std::stringstream ss;
      ss << "angle: " << self.angle() << std::endl;
      ss << "axis: " << self.axis().transpose() << std::endl;
      
      return ss.str();
Nicolas Mansard's avatar
Nicolas Mansard committed
121
122
123
124
125
126
    }

  public:

    static void expose()
    {
jcarpent's avatar
jcarpent committed
127
128
129
130
      bp::class_<AngleAxis>("AngleAxis",
                            "AngleAxis representation of rotations.\n\n",
                            bp::no_init)
      .def(AngleAxisVisitor<AngleAxis>());
Nicolas Mansard's avatar
Nicolas Mansard committed
131
132
133
134
135
136
    }

  };

} // namespace eigenpy

137
#endif //ifndef __eigenpy_angle_axis_hpp__