angle-axis.hpp 4.05 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
30
      AngleAxisVisitor<AngleAxis>::expose();
    }
  };
Nicolas Mansard's avatar
Nicolas Mansard committed
31

jcarpent's avatar
jcarpent committed
32
  template<typename AngleAxis>
Nicolas Mansard's avatar
Nicolas Mansard committed
33
  class AngleAxisVisitor
34
  : public bp::def_visitor< AngleAxisVisitor<AngleAxis> >
Nicolas Mansard's avatar
Nicolas Mansard committed
35
36
  {

jcarpent's avatar
jcarpent committed
37
38
39
40
41
42
    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
43
44
45
46
47
48
  public:

    template<class PyClass>
    void visit(PyClass& cl) const 
    {
      cl
jcarpent's avatar
jcarpent committed
49
50
51
52
53
54
55
56
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
82
83
84
85
86
87
88
89
      .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.")
      .def("isApprox",(bool (AngleAxis::*)(const AngleAxis &))&AngleAxis::isApprox,
           "Returns true if *this is approximately equal to other.")
      .def("isApprox",(bool (AngleAxis::*)(const AngleAxis &, const Scalar prec))&AngleAxis::isApprox,
           bp::args("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)
      ;
Nicolas Mansard's avatar
Nicolas Mansard committed
90
    }
jcarpent's avatar
jcarpent committed
91
    
Nicolas Mansard's avatar
Nicolas Mansard committed
92
93
  private:

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

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

jcarpent's avatar
jcarpent committed
100
    static bool __eq__(const AngleAxis & u, const AngleAxis & v)
Justin Carpentier's avatar
Justin Carpentier committed
101
102
    { return u.axis() == v.axis() && v.angle() == u.angle(); }
    
jcarpent's avatar
jcarpent committed
103
104
    static bool __ne__(const AngleAxis & u, const AngleAxis & v)
    { return !__eq__(u,v); }
Nicolas Mansard's avatar
Nicolas Mansard committed
105

jcarpent's avatar
jcarpent committed
106
    static std::string print(const AngleAxis & self)
Nicolas Mansard's avatar
Nicolas Mansard committed
107
    {
jcarpent's avatar
jcarpent committed
108
109
110
111
112
      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
113
114
115
116
117
118
    }

  public:

    static void expose()
    {
jcarpent's avatar
jcarpent committed
119
120
121
122
      bp::class_<AngleAxis>("AngleAxis",
                            "AngleAxis representation of rotations.\n\n",
                            bp::no_init)
      .def(AngleAxisVisitor<AngleAxis>());
Nicolas Mansard's avatar
Nicolas Mansard committed
123
124
125
126
127
128
    }

  };

} // namespace eigenpy

129
#endif //ifndef __eigenpy_angle_axis_hpp__