doxygen: intern/itasc tagged
[blender.git] / intern / itasc / kdl / framevel.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 pair (value,derivative) and define operations on that pair
6  *      this classes are usefull for automatic differentiation ( <-> symbolic diff , <-> numeric diff)
7  *      Defines VectorVel, RotationVel, FrameVel.  Look at Frames.h for details on how to work
8  *      with Frame objects.
9  *  \author
10  *      Erwin Aertbelien, Div. PMA, Dep. of Mech. Eng., K.U.Leuven
11  *
12  *  \version
13  *      ORO_Geometry V0.2
14  *
15  *  \par History
16  *      - $log$
17  *
18  *  \par Release
19  *      $Id: framevel.hpp 19905 2009-04-23 13:29:54Z ben2610 $
20  *      $Name:  $
21  ****************************************************************************/
22
23 #ifndef KDL_FRAMEVEL_H
24 #define KDL_FRAMEVEL_H
25
26 #include "utilities/utility.h"
27 #include "utilities/rall1d.h"
28 #include "utilities/traits.h"
29
30 #include "frames.hpp"
31
32
33
34 namespace KDL {
35
36 typedef Rall1d<double> doubleVel;
37
38 IMETHOD doubleVel diff(const doubleVel& a,const doubleVel& b,double dt=1.0) {
39         return doubleVel((b.t-a.t)/dt,(b.grad-a.grad)/dt);
40 }
41
42 IMETHOD doubleVel addDelta(const doubleVel& a,const doubleVel&da,double dt=1.0) {
43         return doubleVel(a.t+da.t*dt,a.grad+da.grad*dt);
44 }
45
46 IMETHOD void random(doubleVel& F) {
47         random(F.t);
48         random(F.grad);
49 }
50 IMETHOD void posrandom(doubleVel& F) {
51         posrandom(F.t);
52         posrandom(F.grad);
53 }
54
55 }
56
57 template <>
58 struct Traits<KDL::doubleVel> {
59         typedef double valueType;
60         typedef KDL::doubleVel derivType;
61 };
62
63 namespace KDL {
64
65 class TwistVel;
66 class VectorVel;
67 class FrameVel;
68 class RotationVel;
69
70 class VectorVel
71 // = TITLE
72 //     An VectorVel is a Vector and its first derivative
73 // = CLASS TYPE
74 //     Concrete
75 {
76 public:
77     Vector p;       // position vector
78     Vector v;       // velocity vector
79 public:
80     VectorVel():p(),v(){}
81     VectorVel(const Vector& _p,const Vector& _v):p(_p),v(_v) {}
82     explicit VectorVel(const Vector& _p):p(_p),v(Vector::Zero()) {}
83
84     Vector value() const { return p;}
85     Vector deriv() const { return v;}
86
87     IMETHOD VectorVel& operator = (const VectorVel& arg);
88     IMETHOD VectorVel& operator = (const Vector& arg);
89     IMETHOD VectorVel& operator += (const VectorVel& arg);
90     IMETHOD VectorVel& operator -= (const VectorVel& arg);
91     IMETHOD static VectorVel Zero();
92     IMETHOD void ReverseSign();
93     IMETHOD doubleVel Norm() const;
94     IMETHOD friend VectorVel operator + (const VectorVel& r1,const VectorVel& r2);
95     IMETHOD friend VectorVel operator - (const VectorVel& r1,const VectorVel& r2);
96     IMETHOD friend VectorVel operator + (const Vector& r1,const VectorVel& r2);
97     IMETHOD friend VectorVel operator - (const Vector& r1,const VectorVel& r2);
98     IMETHOD friend VectorVel operator + (const VectorVel& r1,const Vector& r2);
99     IMETHOD friend VectorVel operator - (const VectorVel& r1,const Vector& r2);
100     IMETHOD friend VectorVel operator * (const VectorVel& r1,const VectorVel& r2);
101     IMETHOD friend VectorVel operator * (const VectorVel& r1,const Vector& r2);
102     IMETHOD friend VectorVel operator * (const Vector& r1,const VectorVel& r2);
103     IMETHOD friend VectorVel operator * (const VectorVel& r1,double r2);
104     IMETHOD friend VectorVel operator * (double r1,const VectorVel& r2);
105     IMETHOD friend VectorVel operator * (const doubleVel& r1,const VectorVel& r2);
106     IMETHOD friend VectorVel operator * (const VectorVel& r2,const doubleVel& r1);
107     IMETHOD friend VectorVel operator*(const Rotation& R,const VectorVel& x);
108
109     IMETHOD friend VectorVel operator / (const VectorVel& r1,double r2);
110     IMETHOD friend VectorVel operator / (const VectorVel& r2,const doubleVel& r1);
111     IMETHOD friend void SetToZero(VectorVel& v);
112
113
114     IMETHOD friend bool Equal(const VectorVel& r1,const VectorVel& r2,double eps=epsilon);
115     IMETHOD friend bool Equal(const Vector& r1,const VectorVel& r2,double eps=epsilon);
116     IMETHOD friend bool Equal(const VectorVel& r1,const Vector& r2,double eps=epsilon);
117     IMETHOD friend VectorVel operator - (const VectorVel& r);
118     IMETHOD friend doubleVel dot(const VectorVel& lhs,const VectorVel& rhs);
119     IMETHOD friend doubleVel dot(const VectorVel& lhs,const Vector& rhs);
120     IMETHOD friend doubleVel dot(const Vector& lhs,const VectorVel& rhs);
121 };
122
123
124
125 class RotationVel
126 // = TITLE
127 //     An RotationVel is a Rotation and its first derivative, a rotation vector
128 // = CLASS TYPE
129 //     Concrete
130 {
131 public:
132     Rotation R; // Rotation matrix
133     Vector   w; // rotation vector
134 public:
135     RotationVel():R(),w() {}
136     explicit RotationVel(const Rotation& R_):R(R_),w(Vector::Zero()){}
137     RotationVel(const Rotation& R_,const Vector& _w):R(R_),w(_w){}
138
139
140     Rotation value() const { return R;}
141     Vector   deriv() const { return w;}
142
143
144     IMETHOD RotationVel& operator = (const RotationVel& arg);
145     IMETHOD RotationVel& operator = (const Rotation& arg);
146         IMETHOD VectorVel UnitX() const;
147         IMETHOD VectorVel UnitY() const;
148         IMETHOD VectorVel UnitZ() const;
149     IMETHOD static RotationVel Identity();
150     IMETHOD RotationVel Inverse() const;
151     IMETHOD VectorVel Inverse(const VectorVel& arg) const;
152     IMETHOD VectorVel Inverse(const Vector& arg) const;
153     IMETHOD VectorVel operator*(const VectorVel& arg) const;
154     IMETHOD VectorVel operator*(const Vector& arg) const;
155     IMETHOD void DoRotX(const doubleVel& angle);
156     IMETHOD void DoRotY(const doubleVel& angle);
157     IMETHOD void DoRotZ(const doubleVel& angle);
158     IMETHOD static RotationVel RotX(const doubleVel& angle);
159     IMETHOD static RotationVel RotY(const doubleVel& angle);
160     IMETHOD static RotationVel RotZ(const doubleVel& angle);
161     IMETHOD static RotationVel Rot(const Vector& rotvec,const doubleVel& angle);
162     // rotvec has arbitrary norm
163     // rotation around a constant vector !
164     IMETHOD static RotationVel Rot2(const Vector& rotvec,const doubleVel& angle);
165     // rotvec is normalized.
166     // rotation around a constant vector !
167     IMETHOD friend RotationVel operator* (const RotationVel& r1,const RotationVel& r2);
168     IMETHOD friend RotationVel operator* (const Rotation& r1,const RotationVel& r2);
169     IMETHOD friend RotationVel operator* (const RotationVel& r1,const Rotation& r2);
170     IMETHOD friend bool Equal(const RotationVel& r1,const RotationVel& r2,double eps=epsilon);
171     IMETHOD friend bool Equal(const Rotation& r1,const RotationVel& r2,double eps=epsilon);
172     IMETHOD friend bool Equal(const RotationVel& r1,const Rotation& r2,double eps=epsilon);
173
174     IMETHOD TwistVel Inverse(const TwistVel& arg) const;
175     IMETHOD TwistVel Inverse(const Twist& arg) const;
176     IMETHOD TwistVel operator * (const TwistVel& arg) const;
177     IMETHOD TwistVel operator * (const Twist& arg) const;
178 };
179
180
181
182
183 class FrameVel
184 // = TITLE
185 //     An FrameVel is a Frame and its first derivative, a Twist vector
186 // = CLASS TYPE
187 //     Concrete
188 // = CAVEATS
189 //
190 {
191 public:
192     RotationVel M;
193     VectorVel   p;
194 public:
195     FrameVel(){}
196
197     explicit FrameVel(const Frame& T_):
198         M(T_.M),p(T_.p) {}
199
200     FrameVel(const Frame& T_,const Twist& _t):
201         M(T_.M,_t.rot),p(T_.p,_t.vel) {}
202
203     FrameVel(const RotationVel& _M,const VectorVel& _p):
204         M(_M),p(_p) {}
205
206
207     Frame value() const { return Frame(M.value(),p.value());}
208     Twist deriv() const { return Twist(p.deriv(),M.deriv());}
209
210
211     IMETHOD FrameVel& operator = (const Frame& arg);
212     IMETHOD FrameVel& operator = (const FrameVel& arg);
213     IMETHOD static FrameVel Identity();
214     IMETHOD FrameVel Inverse() const;
215     IMETHOD VectorVel Inverse(const VectorVel& arg) const;
216     IMETHOD VectorVel operator*(const VectorVel& arg) const;
217     IMETHOD VectorVel operator*(const Vector& arg) const;
218     IMETHOD VectorVel Inverse(const Vector& arg) const;
219     IMETHOD Frame GetFrame() const;
220     IMETHOD Twist GetTwist() const;
221     IMETHOD friend FrameVel operator * (const FrameVel& f1,const FrameVel& f2);
222     IMETHOD friend FrameVel operator * (const Frame& f1,const FrameVel& f2);
223     IMETHOD friend FrameVel operator * (const FrameVel& f1,const Frame& f2);
224     IMETHOD friend bool Equal(const FrameVel& r1,const FrameVel& r2,double eps=epsilon);
225     IMETHOD friend bool Equal(const Frame& r1,const FrameVel& r2,double eps=epsilon);
226     IMETHOD friend bool Equal(const FrameVel& r1,const Frame& r2,double eps=epsilon);
227
228     IMETHOD TwistVel  Inverse(const TwistVel& arg) const;
229     IMETHOD TwistVel  Inverse(const Twist& arg) const;
230     IMETHOD TwistVel operator * (const TwistVel& arg) const;
231     IMETHOD TwistVel operator * (const Twist& arg) const;
232 };
233
234
235
236
237
238 //very similar to Wrench class.
239 class TwistVel
240 // = TITLE
241 // This class represents a TwistVel. This is a velocity and rotational velocity together
242 {
243 public:
244     VectorVel vel;
245     VectorVel rot;
246 public:
247
248 // = Constructors
249     TwistVel():vel(),rot() {};
250     TwistVel(const VectorVel& _vel,const VectorVel& _rot):vel(_vel),rot(_rot) {};
251     TwistVel(const Twist& p,const Twist& v):vel(p.vel, v.vel), rot( p.rot, v.rot) {};
252     TwistVel(const Twist& p):vel(p.vel), rot( p.rot) {};
253
254     Twist value() const {
255         return Twist(vel.value(),rot.value());
256     }
257     Twist deriv() const {
258         return Twist(vel.deriv(),rot.deriv());
259     }
260 // = Operators
261      IMETHOD TwistVel& operator-=(const TwistVel& arg);
262      IMETHOD TwistVel& operator+=(const TwistVel& arg);
263
264 // = External operators
265      IMETHOD friend TwistVel operator*(const TwistVel& lhs,double rhs);
266      IMETHOD friend TwistVel operator*(double lhs,const TwistVel& rhs);
267      IMETHOD friend TwistVel operator/(const TwistVel& lhs,double rhs);
268
269      IMETHOD friend TwistVel operator*(const TwistVel& lhs,const doubleVel& rhs);
270      IMETHOD friend TwistVel operator*(const doubleVel& lhs,const TwistVel& rhs);
271      IMETHOD friend TwistVel operator/(const TwistVel& lhs,const doubleVel& rhs);
272
273      IMETHOD friend TwistVel operator+(const TwistVel& lhs,const TwistVel& rhs);
274      IMETHOD friend TwistVel operator-(const TwistVel& lhs,const TwistVel& rhs);
275      IMETHOD friend TwistVel operator-(const TwistVel& arg);
276      IMETHOD friend void SetToZero(TwistVel& v);
277
278
279 // = Zero
280      static IMETHOD TwistVel Zero();
281
282 // = Reverse Sign
283      IMETHOD void ReverseSign();
284
285 // = Change Reference point
286      IMETHOD TwistVel RefPoint(const VectorVel& v_base_AB);
287      // Changes the reference point of the TwistVel.
288      // The VectorVel v_base_AB is expressed in the same base as the TwistVel
289      // The VectorVel v_base_AB is a VectorVel from the old point to
290      // the new point.
291      // Complexity : 6M+6A
292
293      // = Equality operators
294      // do not use operator == because the definition of Equal(.,.) is slightly
295      // different.  It compares whether the 2 arguments are equal in an eps-interval
296      IMETHOD friend bool Equal(const TwistVel& a,const TwistVel& b,double eps=epsilon);
297      IMETHOD friend bool Equal(const Twist& a,const TwistVel& b,double eps=epsilon);
298      IMETHOD friend bool Equal(const TwistVel& a,const Twist& b,double eps=epsilon);
299
300 // = Conversion to other entities
301      IMETHOD Twist GetTwist() const;
302      IMETHOD Twist GetTwistDot() const;
303 // = Friends
304     friend class RotationVel;
305     friend class FrameVel;
306
307 };
308
309 IMETHOD VectorVel diff(const VectorVel& a,const VectorVel& b,double dt=1.0) {
310         return VectorVel(diff(a.p,b.p,dt),diff(a.v,b.v,dt));
311 }
312
313 IMETHOD VectorVel addDelta(const VectorVel& a,const VectorVel&da,double dt=1.0) {
314         return VectorVel(addDelta(a.p,da.p,dt),addDelta(a.v,da.v,dt));
315 }
316 IMETHOD VectorVel diff(const RotationVel& a,const RotationVel& b,double dt = 1.0) {
317         return VectorVel(diff(a.R,b.R,dt),diff(a.w,b.w,dt));
318 }
319
320 IMETHOD RotationVel addDelta(const RotationVel& a,const VectorVel&da,double dt=1.0) {
321         return RotationVel(addDelta(a.R,da.p,dt),addDelta(a.w,da.v,dt));
322 }
323
324 IMETHOD TwistVel diff(const FrameVel& a,const FrameVel& b,double dt=1.0) {
325         return TwistVel(diff(a.M,b.M,dt),diff(a.p,b.p,dt));
326 }
327
328 IMETHOD FrameVel addDelta(const FrameVel& a,const TwistVel& da,double dt=1.0) {
329         return FrameVel(
330                         addDelta(a.M,da.rot,dt),
331                         addDelta(a.p,da.vel,dt)
332                    );
333 }
334
335 IMETHOD void random(VectorVel& a) {
336         random(a.p);
337         random(a.v);
338 }
339 IMETHOD void random(TwistVel& a) {
340         random(a.vel);
341         random(a.rot);
342 }
343
344 IMETHOD void random(RotationVel& R) {
345         random(R.R);
346         random(R.w);
347 }
348
349 IMETHOD void random(FrameVel& F) {
350         random(F.M);
351         random(F.p);
352 }
353 IMETHOD void posrandom(VectorVel& a) {
354         posrandom(a.p);
355         posrandom(a.v);
356 }
357 IMETHOD void posrandom(TwistVel& a) {
358         posrandom(a.vel);
359         posrandom(a.rot);
360 }
361
362 IMETHOD void posrandom(RotationVel& R) {
363         posrandom(R.R);
364         posrandom(R.w);
365 }
366
367 IMETHOD void posrandom(FrameVel& F) {
368         posrandom(F.M);
369         posrandom(F.p);
370 }
371
372 #ifdef KDL_INLINE
373 #include "framevel.inl"
374 #endif
375
376 } // namespace
377
378 #endif
379
380
381
382