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