Merge of itasc branch. Project files, scons and cmake should be working. Makefile...
[blender.git] / intern / itasc / kdl / frameacc.hpp
1 /*****************************************************************************
2  * \file
3  *      This file contains the definition of classes for a
4  *      Rall Algebra of (subset of) the classes defined in frames,
5  *      i.e. classes that contain a set (value,derivative,2nd derivative)
6  *      and define operations on that set
7  *      this classes are usefull for automatic differentiation ( <-> symbolic diff ,
8  *      <-> numeric diff).
9  *      Defines VectorAcc, RotationAcc, FrameAcc, doubleAcc.
10  *      Look at the corresponding classes Vector Rotation Frame Twist and
11  *      Wrench for the semantics of the methods.
12  *
13  *      It also contains the 2nd derivative <-> RFrames.h
14  *
15  *  \author
16  *      Erwin Aertbelien, Div. PMA, Dep. of Mech. Eng., K.U.Leuven
17  *
18  *  \version
19  *      ORO_Geometry V0.2
20  *
21  *  \par History
22  *      - $log$
23  *
24  *  \par Release
25  *      $Id: frameacc.hpp 19905 2009-04-23 13:29:54Z ben2610 $
26  *      $Name:  $
27  ****************************************************************************/
28
29 #ifndef RRFRAMES_H
30 #define RRFRAMES_H
31
32
33 #include "utilities/rall2d.h"
34 #include "frames.hpp"
35
36
37
38 namespace KDL {
39
40 class TwistAcc;
41 typedef Rall2d<double,double,double> doubleAcc;
42
43
44 class VectorAcc
45 {
46 public:
47     Vector p;   //!< position vector
48     Vector v;   //!< velocity vector
49     Vector dv;  //!< acceleration vector
50 public:
51     VectorAcc():p(),v(),dv() {}
52     explicit VectorAcc(const Vector& _p):p(_p),v(Vector::Zero()),dv(Vector::Zero()) {}
53     VectorAcc(const Vector& _p,const Vector& _v):p(_p),v(_v),dv(Vector::Zero()) {}
54     VectorAcc(const Vector& _p,const Vector& _v,const Vector& _dv):
55         p(_p),v(_v),dv(_dv) {}
56     IMETHOD VectorAcc& operator = (const VectorAcc& arg);
57     IMETHOD VectorAcc& operator = (const Vector& arg);
58     IMETHOD VectorAcc& operator += (const VectorAcc& arg);
59     IMETHOD VectorAcc& operator -= (const VectorAcc& arg);
60     IMETHOD static VectorAcc Zero();
61     IMETHOD void ReverseSign();
62     IMETHOD doubleAcc Norm();
63     IMETHOD friend VectorAcc operator + (const VectorAcc& r1,const VectorAcc& r2);
64     IMETHOD friend VectorAcc operator - (const VectorAcc& r1,const VectorAcc& r2);
65     IMETHOD friend VectorAcc operator + (const Vector& r1,const VectorAcc& r2);
66     IMETHOD friend VectorAcc operator - (const Vector& r1,const VectorAcc& r2);
67     IMETHOD friend VectorAcc operator + (const VectorAcc& r1,const Vector& r2);
68     IMETHOD friend VectorAcc operator - (const VectorAcc& r1,const Vector& r2);
69     IMETHOD friend VectorAcc operator * (const VectorAcc& r1,const VectorAcc& r2);
70     IMETHOD friend VectorAcc operator * (const VectorAcc& r1,const Vector& r2);
71     IMETHOD friend VectorAcc operator * (const Vector& r1,const VectorAcc& r2);
72     IMETHOD friend VectorAcc operator * (const VectorAcc& r1,double r2);
73     IMETHOD friend VectorAcc operator * (double r1,const VectorAcc& r2);
74     IMETHOD friend VectorAcc operator * (const doubleAcc& r1,const VectorAcc& r2);
75     IMETHOD friend VectorAcc operator * (const VectorAcc& r2,const doubleAcc& r1);
76     IMETHOD friend VectorAcc operator*(const Rotation& R,const VectorAcc& x);
77
78     IMETHOD friend VectorAcc operator / (const VectorAcc& r1,double r2);
79     IMETHOD friend VectorAcc operator / (const VectorAcc& r2,const doubleAcc& r1);
80
81
82     IMETHOD friend bool Equal(const VectorAcc& r1,const VectorAcc& r2,double eps=epsilon);
83     IMETHOD friend bool Equal(const Vector& r1,const VectorAcc& r2,double eps=epsilon);
84     IMETHOD friend bool Equal(const VectorAcc& r1,const Vector& r2,double eps=epsilon);
85     IMETHOD friend VectorAcc operator - (const VectorAcc& r);
86     IMETHOD friend doubleAcc dot(const VectorAcc& lhs,const VectorAcc& rhs);
87     IMETHOD friend doubleAcc dot(const VectorAcc& lhs,const Vector& rhs);
88     IMETHOD friend doubleAcc dot(const Vector& lhs,const VectorAcc& rhs);
89 };
90
91
92
93 class RotationAcc
94 {
95 public:
96     Rotation R;     //!< rotation matrix
97     Vector   w;     //!< angular velocity vector
98     Vector   dw;    //!< angular acceration vector
99 public:
100     RotationAcc():R(),w() {}
101     explicit RotationAcc(const Rotation& _R):R(_R),w(Vector::Zero()){}
102     RotationAcc(const Rotation& _R,const Vector& _w,const Vector& _dw):
103         R(_R),w(_w),dw(_dw) {}
104     IMETHOD RotationAcc& operator = (const RotationAcc& arg);
105     IMETHOD RotationAcc& operator = (const Rotation& arg);
106     IMETHOD static RotationAcc Identity();
107     IMETHOD RotationAcc Inverse() const;
108     IMETHOD VectorAcc Inverse(const VectorAcc& arg) const;
109     IMETHOD VectorAcc Inverse(const Vector& arg) const;
110     IMETHOD VectorAcc operator*(const VectorAcc& arg) const;
111     IMETHOD VectorAcc operator*(const Vector& arg) const;
112
113     //  Rotations
114     // The SetRot.. functions set the value of *this to the appropriate rotation matrix.
115     // The Rot... static functions give the value of the appropriate rotation matrix back.
116     // The DoRot... functions apply a rotation R to *this,such that *this = *this * R.
117     // IMETHOD void DoRotX(const doubleAcc& angle);
118     // IMETHOD void DoRotY(const doubleAcc& angle);
119     // IMETHOD void DoRotZ(const doubleAcc& angle);
120     // IMETHOD static RRotation RotX(const doubleAcc& angle);
121     // IMETHOD static RRotation RotY(const doubleAcc& angle);
122     // IMETHOD static RRotation RotZ(const doubleAcc& angle);
123
124     // IMETHOD void SetRot(const Vector& rotaxis,const doubleAcc& angle);
125     // Along an arbitrary axes.  The norm of rotvec is neglected.
126     // IMETHOD static RotationAcc Rot(const Vector& rotvec,const doubleAcc& angle);
127     // rotvec has arbitrary norm
128     // rotation around a constant vector !
129     // IMETHOD static RotationAcc Rot2(const Vector& rotvec,const doubleAcc& angle);
130     // rotvec is normalized.
131     // rotation around a constant vector !
132
133     IMETHOD friend RotationAcc operator* (const RotationAcc& r1,const RotationAcc& r2);
134     IMETHOD friend RotationAcc operator* (const Rotation& r1,const RotationAcc& r2);
135     IMETHOD friend RotationAcc operator* (const RotationAcc& r1,const Rotation& r2);
136     IMETHOD friend bool Equal(const RotationAcc& r1,const RotationAcc& r2,double eps=epsilon);
137     IMETHOD friend bool Equal(const Rotation& r1,const RotationAcc& r2,double eps=epsilon);
138     IMETHOD friend bool Equal(const RotationAcc& r1,const Rotation& r2,double eps=epsilon);
139     IMETHOD TwistAcc Inverse(const TwistAcc& arg) const;
140     IMETHOD TwistAcc Inverse(const Twist& arg) const;
141     IMETHOD TwistAcc operator * (const TwistAcc& arg) const;
142     IMETHOD TwistAcc operator * (const Twist& arg) const;
143 };
144
145
146
147
148 class FrameAcc
149 {
150 public:
151     RotationAcc M;   //!< Rotation,angular velocity, and angular acceleration of frame.
152     VectorAcc   p;   //!< Translation, velocity and acceleration of origin.
153 public:
154     FrameAcc(){}
155     explicit FrameAcc(const Frame& _T):M(_T.M),p(_T.p) {}
156     FrameAcc(const Frame& _T,const Twist& _t,const Twist& _dt):
157         M(_T.M,_t.rot,_dt.rot),p(_T.p,_t.vel,_dt.vel) {}
158     FrameAcc(const RotationAcc& _M,const VectorAcc& _p):M(_M),p(_p) {}
159
160     IMETHOD FrameAcc& operator = (const FrameAcc& arg);
161     IMETHOD FrameAcc& operator = (const Frame& arg);
162     IMETHOD static FrameAcc Identity();
163     IMETHOD FrameAcc Inverse() const;
164     IMETHOD VectorAcc Inverse(const VectorAcc& arg) const;
165     IMETHOD VectorAcc operator*(const VectorAcc& arg) const;
166     IMETHOD VectorAcc operator*(const Vector& arg) const;
167     IMETHOD VectorAcc Inverse(const Vector& arg) const;
168     IMETHOD Frame GetFrame() const;
169     IMETHOD Twist GetTwist() const;
170     IMETHOD Twist GetAccTwist() const;
171     IMETHOD friend FrameAcc operator * (const FrameAcc& f1,const FrameAcc& f2);
172     IMETHOD friend FrameAcc operator * (const Frame& f1,const FrameAcc& f2);
173     IMETHOD friend FrameAcc operator * (const FrameAcc& f1,const Frame& f2);
174     IMETHOD friend bool Equal(const FrameAcc& r1,const FrameAcc& r2,double eps=epsilon);
175     IMETHOD friend bool Equal(const Frame& r1,const FrameAcc& r2,double eps=epsilon);
176     IMETHOD friend bool Equal(const FrameAcc& r1,const Frame& r2,double eps=epsilon);
177
178     IMETHOD TwistAcc  Inverse(const TwistAcc& arg) const;
179     IMETHOD TwistAcc  Inverse(const Twist& arg) const;
180     IMETHOD TwistAcc operator * (const TwistAcc& arg) const;
181     IMETHOD TwistAcc operator * (const Twist& arg) const;
182 };
183
184
185
186
187
188
189
190
191 //very similar to Wrench class.
192 class TwistAcc
193 {
194 public:
195     VectorAcc vel;       //!< translational velocity and its 1st and 2nd derivative
196     VectorAcc rot;       //!< rotational velocity and its 1st and 2nd derivative
197 public:
198
199      TwistAcc():vel(),rot() {};
200      TwistAcc(const VectorAcc& _vel,const VectorAcc& _rot):vel(_vel),rot(_rot) {};
201
202      IMETHOD TwistAcc& operator-=(const TwistAcc& arg);
203      IMETHOD TwistAcc& operator+=(const TwistAcc& arg);
204
205      IMETHOD friend TwistAcc operator*(const TwistAcc& lhs,double rhs);
206      IMETHOD friend TwistAcc operator*(double lhs,const TwistAcc& rhs);
207      IMETHOD friend TwistAcc operator/(const TwistAcc& lhs,double rhs);
208
209      IMETHOD friend TwistAcc operator*(const TwistAcc& lhs,const doubleAcc& rhs);
210      IMETHOD friend TwistAcc operator*(const doubleAcc& lhs,const TwistAcc& rhs);
211      IMETHOD friend TwistAcc operator/(const TwistAcc& lhs,const doubleAcc& rhs);
212
213      IMETHOD friend TwistAcc operator+(const TwistAcc& lhs,const TwistAcc& rhs);
214      IMETHOD friend TwistAcc operator-(const TwistAcc& lhs,const TwistAcc& rhs);
215      IMETHOD friend TwistAcc operator-(const TwistAcc& arg);
216
217      IMETHOD friend void SetToZero(TwistAcc& v);
218
219      static IMETHOD TwistAcc Zero();
220
221      IMETHOD void ReverseSign();
222
223      IMETHOD TwistAcc RefPoint(const VectorAcc& v_base_AB);
224      // Changes the reference point of the RTwist.
225      // The RVector v_base_AB is expressed in the same base as the RTwist
226      // The RVector v_base_AB is a RVector from the old point to
227      // the new point.
228      // Complexity : 6M+6A
229
230      IMETHOD friend bool Equal(const TwistAcc& a,const TwistAcc& b,double eps=epsilon);
231      IMETHOD friend bool Equal(const Twist& a,const TwistAcc& b,double eps=epsilon);
232      IMETHOD friend bool Equal(const TwistAcc& a,const Twist& b,double eps=epsilon);
233
234
235      IMETHOD Twist GetTwist() const;
236      IMETHOD Twist GetTwistDot() const;
237
238     friend class RotationAcc;
239     friend class FrameAcc;
240
241 };
242
243
244
245
246
247
248
249 #ifdef KDL_INLINE
250 #include "frameacc.inl"
251 #endif
252
253 }
254
255
256
257
258
259 #endif