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