angle-axis.hpp 4.68 KB
Newer Older
1
/*
Justin Carpentier's avatar
Justin Carpentier committed
2
 * Copyright 2014-2020 CNRS INRIA
3
4
5
6
 */

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

Justin Carpentier's avatar
Justin Carpentier committed
8
#include "eigenpy/eigenpy.hpp"
9

Nicolas Mansard's avatar
Nicolas Mansard committed
10
11
#include <Eigen/Core>
#include <Eigen/Geometry>
12

Nicolas Mansard's avatar
Nicolas Mansard committed
13
14
15
16
namespace eigenpy
{

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

jcarpent's avatar
jcarpent committed
37
  template<typename AngleAxis>
Nicolas Mansard's avatar
Nicolas Mansard committed
38
  class AngleAxisVisitor
39
  : public bp::def_visitor< AngleAxisVisitor<AngleAxis> >
Nicolas Mansard's avatar
Nicolas Mansard committed
40
41
  {

jcarpent's avatar
jcarpent committed
42
43
44
45
46
    typedef typename AngleAxis::Scalar Scalar;
    typedef typename AngleAxis::Vector3 Vector3;
    typedef typename AngleAxis::Matrix3 Matrix3;
    
    typedef typename Eigen::Quaternion<Scalar,0> Quaternion;
47
    typedef Eigen::RotationBase<AngleAxis,3> RotationBase;
jcarpent's avatar
jcarpent committed
48
    
49
50
    BOOST_PYTHON_FUNCTION_OVERLOADS(isApproxAngleAxis_overload,call<AngleAxis>::isApprox,2,3)
    
Nicolas Mansard's avatar
Nicolas Mansard committed
51
52
53
54
55
56
  public:

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

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

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

jcarpent's avatar
jcarpent committed
119
    static bool __eq__(const AngleAxis & u, const AngleAxis & v)
Justin Carpentier's avatar
Justin Carpentier committed
120
121
    { return u.axis() == v.axis() && v.angle() == u.angle(); }
    
jcarpent's avatar
jcarpent committed
122
123
    static bool __ne__(const AngleAxis & u, const AngleAxis & v)
    { return !__eq__(u,v); }
Nicolas Mansard's avatar
Nicolas Mansard committed
124

jcarpent's avatar
jcarpent committed
125
    static std::string print(const AngleAxis & self)
Nicolas Mansard's avatar
Nicolas Mansard committed
126
    {
jcarpent's avatar
jcarpent committed
127
128
129
130
131
      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
132
133
134
135
136
137
    }

  public:

    static void expose()
    {
jcarpent's avatar
jcarpent committed
138
      bp::class_<AngleAxis>("AngleAxis",
139
                            "AngleAxis representation of a rotation.\n\n",
jcarpent's avatar
jcarpent committed
140
141
                            bp::no_init)
      .def(AngleAxisVisitor<AngleAxis>());
142
      
143
      // Cast to Eigen::RotationBase
144
      bp::implicitly_convertible<AngleAxis,RotationBase>();
Nicolas Mansard's avatar
Nicolas Mansard committed
145
146
147
148
149
150
    }

  };

} // namespace eigenpy

151
#endif //ifndef __eigenpy_angle_axis_hpp__