Merge of itasc branch. Project files, scons and cmake should be working. Makefile...
[blender.git] / intern / itasc / kdl / framevel.inl
1 /*****************************************************************************
2  * \file  
3  *      provides inline functions of rframes.h
4  *       
5  *  \author 
6  *      Erwin Aertbelien, Div. PMA, Dep. of Mech. Eng., K.U.Leuven
7  *
8  *  \version 
9  *      ORO_Geometry V0.2
10  *
11  *  \par History
12  *      - $log$
13  *
14  *  \par Release
15  *      $Id: framevel.inl 19905 2009-04-23 13:29:54Z ben2610 $
16  *      $Name:  $ 
17  ****************************************************************************/
18
19
20 // Methods and operators related to FrameVelVel
21 // They all delegate most of the work to RotationVelVel and VectorVelVel
22 FrameVel& FrameVel::operator = (const FrameVel& arg) {
23     M=arg.M;
24     p=arg.p;
25     return *this;
26 }
27
28 FrameVel FrameVel::Identity() {
29     return FrameVel(RotationVel::Identity(),VectorVel::Zero());
30 }
31
32
33 FrameVel operator *(const FrameVel& lhs,const FrameVel& rhs)
34 {
35     return FrameVel(lhs.M*rhs.M,lhs.M*rhs.p+lhs.p);
36 }
37 FrameVel operator *(const FrameVel& lhs,const Frame& rhs)
38 {
39     return FrameVel(lhs.M*rhs.M,lhs.M*rhs.p+lhs.p);
40 }
41 FrameVel operator *(const Frame& lhs,const FrameVel& rhs)
42 {
43     return FrameVel(lhs.M*rhs.M , lhs.M*rhs.p+lhs.p );
44 }
45
46 VectorVel FrameVel::operator *(const VectorVel & arg) const
47 {
48     return M*arg+p;
49 }
50 VectorVel FrameVel::operator *(const Vector & arg) const
51 {
52     return M*arg+p;
53 }
54
55 VectorVel FrameVel::Inverse(const VectorVel& arg) const
56 {
57     return M.Inverse(arg-p);
58 }
59
60 VectorVel FrameVel::Inverse(const Vector& arg) const
61 {
62     return M.Inverse(arg-p);
63 }
64
65 FrameVel FrameVel::Inverse() const
66 {
67     return FrameVel(M.Inverse(),-M.Inverse(p));
68 }
69
70 FrameVel& FrameVel::operator = (const Frame& arg) {
71     M = arg.M;
72     p = arg.p;
73     return *this;
74 }
75 bool Equal(const FrameVel& r1,const FrameVel& r2,double eps) {
76     return (Equal(r1.M,r2.M,eps) && Equal(r1.p,r2.p,eps));
77 }
78 bool Equal(const Frame& r1,const FrameVel& r2,double eps) {
79     return (Equal(r1.M,r2.M,eps) && Equal(r1.p,r2.p,eps));
80 }
81 bool Equal(const FrameVel& r1,const Frame& r2,double eps) {
82     return (Equal(r1.M,r2.M,eps) && Equal(r1.p,r2.p,eps));
83 }
84
85 Frame FrameVel::GetFrame() const {
86     return Frame(M.R,p.p);
87 }
88
89 Twist FrameVel::GetTwist() const {
90     return Twist(p.v,M.w);
91 }
92
93
94 RotationVel operator* (const RotationVel& r1,const RotationVel& r2) {
95     return RotationVel( r1.R*r2.R, r1.w + r1.R*r2.w );
96 }
97
98 RotationVel operator* (const Rotation& r1,const RotationVel& r2) {
99     return RotationVel( r1*r2.R, r1*r2.w );
100 }
101
102 RotationVel operator* (const RotationVel& r1,const Rotation& r2) {
103     return RotationVel( r1.R*r2, r1.w );
104 }
105
106 RotationVel& RotationVel::operator = (const RotationVel& arg) {
107         R=arg.R;
108         w=arg.w;
109         return *this;
110     }
111 RotationVel& RotationVel::operator = (const Rotation& arg) {
112     R=arg;
113     w=Vector::Zero();
114     return *this;
115 }
116
117 VectorVel   RotationVel::UnitX() const {
118         return VectorVel(R.UnitX(),w*R.UnitX()); 
119 }
120
121 VectorVel   RotationVel::UnitY() const {
122         return VectorVel(R.UnitY(),w*R.UnitY()); 
123 }
124
125 VectorVel   RotationVel::UnitZ() const {
126         return VectorVel(R.UnitZ(),w*R.UnitZ()); 
127 }
128
129
130
131 RotationVel RotationVel::Identity() {
132     return RotationVel(Rotation::Identity(),Vector::Zero());
133 }
134
135 RotationVel RotationVel::Inverse() const {
136     return RotationVel(R.Inverse(),-R.Inverse(w));
137 }
138
139 VectorVel RotationVel::Inverse(const VectorVel& arg) const {
140     Vector tmp=R.Inverse(arg.p);
141     return VectorVel(tmp,
142                     R.Inverse(arg.v-w*arg.p)
143                     );
144 }
145
146 VectorVel RotationVel::Inverse(const Vector& arg) const {
147     Vector tmp=R.Inverse(arg);
148     return VectorVel(tmp,
149                     R.Inverse(-w*arg)
150                     );
151 }
152
153
154 VectorVel RotationVel::operator*(const VectorVel& arg) const {
155     Vector tmp=R*arg.p;
156     return VectorVel(tmp,w*tmp+R*arg.v);
157 }
158
159 VectorVel RotationVel::operator*(const Vector& arg) const {
160     Vector tmp=R*arg;
161     return VectorVel(tmp,w*tmp);
162 }
163
164
165 // = Rotations
166 // The Rot... static functions give the value of the appropriate rotation matrix back.
167 // The DoRot... functions apply a rotation R to *this,such that *this = *this * R.
168
169 void RotationVel::DoRotX(const doubleVel& angle) {
170     w+=R*Vector(angle.grad,0,0);
171     R.DoRotX(angle.t);
172 }
173 RotationVel RotationVel::RotX(const doubleVel& angle) {
174     return RotationVel(Rotation::RotX(angle.t),Vector(angle.grad,0,0));
175 }
176
177 void RotationVel::DoRotY(const doubleVel& angle) {
178     w+=R*Vector(0,angle.grad,0);
179     R.DoRotY(angle.t);
180 }
181 RotationVel RotationVel::RotY(const doubleVel& angle) {
182     return RotationVel(Rotation::RotX(angle.t),Vector(0,angle.grad,0));
183 }
184
185 void RotationVel::DoRotZ(const doubleVel& angle) {
186     w+=R*Vector(0,0,angle.grad);
187     R.DoRotZ(angle.t);
188 }
189 RotationVel RotationVel::RotZ(const doubleVel& angle) {
190     return RotationVel(Rotation::RotZ(angle.t),Vector(0,0,angle.grad));
191 }
192
193
194 RotationVel RotationVel::Rot(const Vector& rotvec,const doubleVel& angle) 
195 // rotvec has arbitrary norm
196 // rotation around a constant vector !
197 {
198     Vector v(rotvec);
199         v.Normalize();
200     return RotationVel(Rotation::Rot2(v,angle.t),v*angle.grad);
201 }
202
203 RotationVel RotationVel::Rot2(const Vector& rotvec,const doubleVel& angle) 
204     // rotvec is normalized.
205 {
206     return RotationVel(Rotation::Rot2(rotvec,angle.t),rotvec*angle.grad);
207 }
208
209
210 VectorVel operator + (const VectorVel& r1,const VectorVel& r2) {
211     return VectorVel(r1.p+r2.p,r1.v+r2.v);
212 }
213
214 VectorVel operator - (const VectorVel& r1,const VectorVel& r2) {
215     return VectorVel(r1.p-r2.p,r1.v-r2.v);
216 }
217
218 VectorVel operator + (const VectorVel& r1,const Vector& r2) {
219     return VectorVel(r1.p+r2,r1.v);
220 }
221
222 VectorVel operator - (const VectorVel& r1,const Vector& r2) {
223     return VectorVel(r1.p-r2,r1.v);
224 }
225
226 VectorVel operator + (const Vector& r1,const VectorVel& r2) {
227     return VectorVel(r1+r2.p,r2.v);
228 }
229
230 VectorVel operator - (const Vector& r1,const VectorVel& r2) {
231     return VectorVel(r1-r2.p,-r2.v);
232 }
233
234 // unary -
235 VectorVel operator - (const VectorVel& r) {
236     return VectorVel(-r.p,-r.v);
237 }
238
239 void SetToZero(VectorVel& v){
240     SetToZero(v.p);
241     SetToZero(v.v);
242 }
243
244 // cross prod.
245 VectorVel operator * (const VectorVel& r1,const VectorVel& r2) {
246     return VectorVel(r1.p*r2.p, r1.p*r2.v+r1.v*r2.p);
247 }
248
249 VectorVel operator * (const VectorVel& r1,const Vector& r2) {
250     return VectorVel(r1.p*r2, r1.v*r2);
251 }
252
253 VectorVel operator * (const Vector& r1,const VectorVel& r2) {
254     return VectorVel(r1*r2.p, r1*r2.v);
255 }
256
257
258
259 // scalar mult.
260 VectorVel operator * (double r1,const VectorVel& r2) {
261     return VectorVel(r1*r2.p, r1*r2.v);
262 }
263
264 VectorVel operator * (const VectorVel& r1,double r2) {
265     return VectorVel(r1.p*r2, r1.v*r2);
266 }
267
268
269
270 VectorVel operator * (const doubleVel& r1,const VectorVel& r2) {
271     return VectorVel(r1.t*r2.p, r1.t*r2.v + r1.grad*r2.p);
272 }
273
274 VectorVel operator * (const VectorVel& r2,const doubleVel& r1) {
275     return VectorVel(r1.t*r2.p, r1.t*r2.v + r1.grad*r2.p);
276 }
277
278 VectorVel operator / (const VectorVel& r1,double r2) {
279     return VectorVel(r1.p/r2, r1.v/r2);
280 }
281
282 VectorVel operator / (const VectorVel& r2,const doubleVel& r1) {
283     return VectorVel(r2.p/r1.t, r2.v/r1.t - r2.p*r1.grad/r1.t/r1.t);
284 }
285
286 VectorVel operator*(const Rotation& R,const VectorVel& x) {
287     return VectorVel(R*x.p,R*x.v);
288 }
289
290 VectorVel& VectorVel::operator = (const VectorVel& arg) {
291     p=arg.p;
292     v=arg.v;
293     return *this;
294 }
295 VectorVel& VectorVel::operator = (const Vector& arg) {
296     p=arg;
297     v=Vector::Zero();
298     return *this;
299 }
300 VectorVel& VectorVel::operator += (const VectorVel& arg) {
301     p+=arg.p;
302     v+=arg.v;
303     return *this;
304 }
305 VectorVel& VectorVel::operator -= (const VectorVel& arg) {
306     p-=arg.p;
307     v-=arg.v;
308     return *this;
309 }
310
311 VectorVel VectorVel::Zero() {
312     return VectorVel(Vector::Zero(),Vector::Zero());
313 }
314 void VectorVel::ReverseSign() {
315     p.ReverseSign();
316     v.ReverseSign();
317 }
318 doubleVel VectorVel::Norm() const {
319     double n = p.Norm();
320     return doubleVel(n,dot(p,v)/n);
321 }
322
323 bool Equal(const VectorVel& r1,const VectorVel& r2,double eps) {
324     return (Equal(r1.p,r2.p,eps) && Equal(r1.v,r2.v,eps));
325 }
326 bool Equal(const Vector& r1,const VectorVel& r2,double eps) {
327     return (Equal(r1,r2.p,eps) && Equal(Vector::Zero(),r2.v,eps));
328 }
329 bool Equal(const VectorVel& r1,const Vector& r2,double eps) {
330     return (Equal(r1.p,r2,eps) && Equal(r1.v,Vector::Zero(),eps));
331 }
332
333 bool Equal(const RotationVel& r1,const RotationVel& r2,double eps) {
334     return (Equal(r1.w,r2.w,eps) && Equal(r1.R,r2.R,eps));
335 }
336 bool Equal(const Rotation& r1,const RotationVel& r2,double eps) {
337     return (Equal(Vector::Zero(),r2.w,eps) && Equal(r1,r2.R,eps));
338 }
339 bool Equal(const RotationVel& r1,const Rotation& r2,double eps) {
340     return (Equal(r1.w,Vector::Zero(),eps) && Equal(r1.R,r2,eps));
341 }
342 bool Equal(const TwistVel& a,const TwistVel& b,double eps) {
343         return (Equal(a.rot,b.rot,eps)&&
344                 Equal(a.vel,b.vel,eps)  );
345 }
346 bool Equal(const Twist& a,const TwistVel& b,double eps) {
347         return (Equal(a.rot,b.rot,eps)&&
348                 Equal(a.vel,b.vel,eps)  );
349 }
350 bool Equal(const TwistVel& a,const Twist& b,double eps) {
351         return (Equal(a.rot,b.rot,eps)&&
352                 Equal(a.vel,b.vel,eps)  );
353 }
354
355
356
357 IMETHOD doubleVel dot(const VectorVel& lhs,const VectorVel& rhs) {
358     return doubleVel(dot(lhs.p,rhs.p),dot(lhs.p,rhs.v)+dot(lhs.v,rhs.p));
359 }
360 IMETHOD doubleVel dot(const VectorVel& lhs,const Vector& rhs) {
361     return doubleVel(dot(lhs.p,rhs),dot(lhs.v,rhs));
362 }
363 IMETHOD doubleVel dot(const Vector& lhs,const VectorVel& rhs) {
364     return doubleVel(dot(lhs,rhs.p),dot(lhs,rhs.v));
365 }
366
367
368
369
370
371
372
373
374
375
376
377
378 TwistVel TwistVel::Zero()
379 {
380     return TwistVel(VectorVel::Zero(),VectorVel::Zero());
381 }
382
383
384 void TwistVel::ReverseSign()
385 {   
386     vel.ReverseSign();
387     rot.ReverseSign();
388 }
389
390 TwistVel TwistVel::RefPoint(const VectorVel& v_base_AB)
391      // Changes the reference point of the TwistVel.
392      // The VectorVel v_base_AB is expressed in the same base as the TwistVel
393      // The VectorVel v_base_AB is a VectorVel from the old point to
394      // the new point.
395      // Complexity : 6M+6A
396 {
397     return TwistVel(this->vel+this->rot*v_base_AB,this->rot);
398 }
399
400 TwistVel& TwistVel::operator-=(const TwistVel& arg)
401 {
402     vel-=arg.vel;
403     rot -=arg.rot;
404     return *this;
405 }
406
407 TwistVel& TwistVel::operator+=(const TwistVel& arg)
408 {
409     vel+=arg.vel;
410     rot +=arg.rot;
411     return *this;
412 }
413
414
415 TwistVel operator*(const TwistVel& lhs,double rhs)
416 {
417     return TwistVel(lhs.vel*rhs,lhs.rot*rhs);
418 }
419
420 TwistVel operator*(double lhs,const TwistVel& rhs)
421 {
422     return TwistVel(lhs*rhs.vel,lhs*rhs.rot);
423 }
424
425 TwistVel operator/(const TwistVel& lhs,double rhs)
426 {
427     return TwistVel(lhs.vel/rhs,lhs.rot/rhs);
428 }
429
430
431 TwistVel operator*(const TwistVel& lhs,const doubleVel& rhs)
432 {
433     return TwistVel(lhs.vel*rhs,lhs.rot*rhs);
434 }
435
436 TwistVel operator*(const doubleVel& lhs,const TwistVel& rhs)
437 {
438     return TwistVel(lhs*rhs.vel,lhs*rhs.rot);
439 }
440
441 TwistVel operator/(const TwistVel& lhs,const doubleVel& rhs)
442 {
443     return TwistVel(lhs.vel/rhs,lhs.rot/rhs);
444 }
445
446
447
448 // addition of TwistVel's
449 TwistVel operator+(const TwistVel& lhs,const TwistVel& rhs)
450 {
451     return TwistVel(lhs.vel+rhs.vel,lhs.rot+rhs.rot);
452 }
453
454 TwistVel operator-(const TwistVel& lhs,const TwistVel& rhs)
455 {
456     return TwistVel(lhs.vel-rhs.vel,lhs.rot-rhs.rot);
457 }
458
459 // unary -
460 TwistVel operator-(const TwistVel& arg) 
461 {
462     return TwistVel(-arg.vel,-arg.rot);
463 }
464
465 void SetToZero(TwistVel& v)
466 {
467    SetToZero(v.vel);
468    SetToZero(v.rot);
469 }
470
471
472
473
474
475 TwistVel RotationVel::Inverse(const TwistVel& arg) const
476 {
477     return TwistVel(Inverse(arg.vel),Inverse(arg.rot));
478 }
479
480 TwistVel RotationVel::operator * (const TwistVel& arg) const
481 {
482     return TwistVel((*this)*arg.vel,(*this)*arg.rot);
483 }
484
485 TwistVel RotationVel::Inverse(const Twist& arg) const
486 {
487     return TwistVel(Inverse(arg.vel),Inverse(arg.rot));
488 }
489
490 TwistVel RotationVel::operator * (const Twist& arg) const
491 {
492     return TwistVel((*this)*arg.vel,(*this)*arg.rot);
493 }
494
495
496 TwistVel FrameVel::operator * (const TwistVel& arg) const
497 {
498     TwistVel tmp;
499     tmp.rot = M*arg.rot;
500     tmp.vel = M*arg.vel+p*tmp.rot;
501     return tmp;
502 }
503
504 TwistVel FrameVel::operator * (const Twist& arg) const
505 {
506     TwistVel tmp;
507     tmp.rot = M*arg.rot;
508     tmp.vel = M*arg.vel+p*tmp.rot;
509     return tmp;
510 }
511
512 TwistVel FrameVel::Inverse(const TwistVel& arg) const
513 {
514     TwistVel tmp;
515     tmp.rot =  M.Inverse(arg.rot);
516     tmp.vel = M.Inverse(arg.vel-p*arg.rot);
517     return tmp;
518 }
519
520 TwistVel FrameVel::Inverse(const Twist& arg) const
521 {
522     TwistVel tmp;
523     tmp.rot =  M.Inverse(arg.rot);
524     tmp.vel = M.Inverse(arg.vel-p*arg.rot);
525     return tmp;
526 }
527
528 Twist TwistVel::GetTwist() const {
529     return Twist(vel.p,rot.p);
530 }
531
532 Twist TwistVel::GetTwistDot() const {
533     return Twist(vel.v,rot.v);
534 }