Merge of itasc branch. Project files, scons and cmake should be working. Makefile...
[blender.git] / intern / itasc / kdl / frames.inl
1 /***************************************************************************
2                         frames.inl -  description
3                        -------------------------
4     begin                : June 2006
5     copyright            : (C) 2006 Erwin Aertbelien
6     email                : firstname.lastname@mech.kuleuven.ac.be
7
8  History (only major changes)( AUTHOR-Description ) :
9  
10  ***************************************************************************
11  *   This library is free software; you can redistribute it and/or         *
12  *   modify it under the terms of the GNU Lesser General Public            *
13  *   License as published by the Free Software Foundation; either          *
14  *   version 2.1 of the License, or (at your option) any later version.    *
15  *                                                                         *
16  *   This library is distributed in the hope that it will be useful,       *
17  *   but WITHOUT ANY WARRANTY; without even the implied warranty of        *
18  *   MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the GNU     *
19  *   Lesser General Public License for more details.                       *
20  *                                                                         *
21  *   You should have received a copy of the GNU Lesser General Public      *
22  *   License along with this library; if not, write to the Free Software   *
23  *   Foundation, Inc., 59 Temple Place,                                    *
24  *   Suite 330, Boston, MA  02111-1307  USA                                *
25  *                                                                         *
26  ***************************************************************************/
27
28
29 IMETHOD Vector::Vector(const Vector & arg)
30 {
31     data[0] = arg.data[0];
32     data[1] = arg.data[1];
33     data[2] = arg.data[2];
34 }
35
36 IMETHOD Vector::Vector(double x,double y, double z)
37 {
38         data[0]=x;data[1]=y;data[2]=z;
39 }
40
41 IMETHOD Vector::Vector(double* xyz)
42 {
43         data[0]=xyz[0];data[1]=xyz[1];data[2]=xyz[2];
44 }
45
46 IMETHOD Vector::Vector(float* xyz)
47 {
48         data[0]=xyz[0];data[1]=xyz[1];data[2]=xyz[2];
49 }
50
51 IMETHOD void Vector::GetValue(double* xyz) const
52 {
53         xyz[0]=data[0];xyz[1]=data[1];xyz[2]=data[2];
54 }
55
56
57 IMETHOD Vector& Vector::operator =(const Vector & arg)
58 {
59     data[0] = arg.data[0];
60     data[1] = arg.data[1];
61     data[2] = arg.data[2];
62     return *this;
63 }
64
65 IMETHOD Vector operator +(const Vector & lhs,const Vector& rhs)
66 {
67     Vector tmp;
68     tmp.data[0] = lhs.data[0]+rhs.data[0];
69     tmp.data[1] = lhs.data[1]+rhs.data[1];
70     tmp.data[2] = lhs.data[2]+rhs.data[2];
71     return tmp;
72 }
73
74 IMETHOD Vector operator -(const Vector & lhs,const Vector& rhs)
75 {
76     Vector tmp;
77     tmp.data[0] = lhs.data[0]-rhs.data[0];
78     tmp.data[1] = lhs.data[1]-rhs.data[1];
79     tmp.data[2] = lhs.data[2]-rhs.data[2];
80     return tmp;
81 }
82
83 IMETHOD double Vector::x() const { return data[0]; }
84 IMETHOD double Vector::y() const { return data[1]; }
85 IMETHOD double Vector::z() const { return data[2]; }
86
87 IMETHOD void Vector::x( double _x ) { data[0] = _x; }
88 IMETHOD void Vector::y( double _y ) { data[1] = _y; }
89 IMETHOD void Vector::z( double _z ) { data[2] = _z; }
90
91 Vector operator *(const Vector& lhs,double rhs) 
92 {
93     Vector tmp;
94     tmp.data[0] = lhs.data[0]*rhs;
95     tmp.data[1] = lhs.data[1]*rhs;
96     tmp.data[2] = lhs.data[2]*rhs;
97     return tmp;
98 }
99
100 Vector operator *(double lhs,const Vector& rhs) 
101 {
102     Vector tmp;
103     tmp.data[0] = lhs*rhs.data[0];
104     tmp.data[1] = lhs*rhs.data[1];
105     tmp.data[2] = lhs*rhs.data[2];
106     return tmp;
107 }
108
109 Vector operator /(const Vector& lhs,double rhs) 
110 {
111     Vector tmp;
112     tmp.data[0] = lhs.data[0]/rhs;
113     tmp.data[1] = lhs.data[1]/rhs;
114     tmp.data[2] = lhs.data[2]/rhs;
115     return tmp;
116 }
117
118 Vector operator *(const Vector & lhs,const Vector& rhs)
119 // Complexity : 6M+3A
120 {
121     Vector tmp;
122     tmp.data[0] = lhs.data[1]*rhs.data[2]-lhs.data[2]*rhs.data[1];
123     tmp.data[1] = lhs.data[2]*rhs.data[0]-lhs.data[0]*rhs.data[2];
124     tmp.data[2] = lhs.data[0]*rhs.data[1]-lhs.data[1]*rhs.data[0];
125     return tmp;
126 }
127
128 Vector& Vector::operator +=(const Vector & arg)
129 // Complexity : 3A
130 {
131     data[0]+=arg.data[0];
132     data[1]+=arg.data[1];
133     data[2]+=arg.data[2];
134     return *this;
135 }
136
137 Vector& Vector::operator -=(const Vector & arg)
138 // Complexity : 3A
139 {
140     data[0]-=arg.data[0];
141     data[1]-=arg.data[1];
142     data[2]-=arg.data[2];
143     return *this;
144 }
145
146 Vector Vector::Zero()
147 {
148     return Vector(0,0,0);
149 }
150
151 double Vector::operator()(int index) const {
152     FRAMES_CHECKI((0<=index)&&(index<=2));
153     return data[index];
154 }
155
156 double& Vector::operator () (int index)
157 {
158     FRAMES_CHECKI((0<=index)&&(index<=2));
159     return data[index];
160 }
161
162 IMETHOD Vector Normalize(const Vector& a, double eps)
163 {
164         double l=a.Norm();
165         return (l<eps) ? Vector(0.0,0.0,0.0) : a/l;
166 }
167
168 Wrench Frame::operator * (const Wrench& arg) const
169 // Complexity : 24M+18A
170 {
171     Wrench tmp;
172     tmp.force  = M*arg.force;
173     tmp.torque = M*arg.torque + p*tmp.force;
174     return tmp;
175 }
176
177 Wrench Frame::Inverse(const Wrench& arg) const
178 {
179     Wrench tmp;
180     tmp.force =  M.Inverse(arg.force);
181     tmp.torque = M.Inverse(arg.torque-p*arg.force);
182     return tmp;
183 }
184
185
186
187 Wrench Rotation::Inverse(const Wrench& arg) const
188 {
189     return Wrench(Inverse(arg.force),Inverse(arg.torque));
190 }
191
192 Twist Rotation::Inverse(const Twist& arg) const
193 {
194     return Twist(Inverse(arg.vel),Inverse(arg.rot));
195 }
196
197 Wrench Wrench::Zero()
198 {
199     return Wrench(Vector::Zero(),Vector::Zero());
200 }
201
202
203 void Wrench::ReverseSign()
204 {   
205     torque.ReverseSign();
206     force.ReverseSign();
207 }
208
209 Wrench Wrench::RefPoint(const Vector& v_base_AB) const
210      // Changes the reference point of the Wrench.
211      // The vector v_base_AB is expressed in the same base as the twist
212      // The vector v_base_AB is a vector from the old point to
213      // the new point.
214 {
215     return Wrench(this->force,
216                   this->torque+this->force*v_base_AB
217                   );
218 }
219
220
221 Wrench& Wrench::operator-=(const Wrench& arg)
222 {
223     torque-=arg.torque;
224     force -=arg.force;
225     return *this;
226 }
227
228 Wrench& Wrench::operator+=(const Wrench& arg)
229 {
230     torque+=arg.torque;
231     force +=arg.force;
232     return *this;
233 }
234
235 double& Wrench::operator()(int i)
236 {
237     // assert((0<=i)&&(i<6)); done by underlying routines
238     if (i<3) 
239         return force(i);
240     else
241         return torque(i-3);
242 }
243
244 double Wrench::operator()(int i) const
245 {
246     // assert((0<=i)&&(i<6)); done by underlying routines
247     if (i<3) 
248         return force(i);
249     else
250         return torque(i-3);
251 }
252
253
254 Wrench operator*(const Wrench& lhs,double rhs)
255 {
256     return Wrench(lhs.force*rhs,lhs.torque*rhs);
257 }
258
259 Wrench operator*(double lhs,const Wrench& rhs)
260 {
261     return Wrench(lhs*rhs.force,lhs*rhs.torque);
262 }
263
264 Wrench operator/(const Wrench& lhs,double rhs)
265 {
266     return Wrench(lhs.force/rhs,lhs.torque/rhs);
267 }
268
269 // addition of Wrench's
270 Wrench operator+(const Wrench& lhs,const Wrench& rhs)
271 {
272     return Wrench(lhs.force+rhs.force,lhs.torque+rhs.torque);
273 }
274
275 Wrench operator-(const Wrench& lhs,const Wrench& rhs)
276 {
277     return Wrench(lhs.force-rhs.force,lhs.torque-rhs.torque);
278 }
279
280 // unary -
281 Wrench operator-(const Wrench& arg) 
282 {
283     return Wrench(-arg.force,-arg.torque);
284 }
285
286 Twist Frame::operator * (const Twist& arg) const
287 // Complexity : 24M+18A
288 {
289     Twist tmp;
290     tmp.rot = M*arg.rot;
291     tmp.vel = M*arg.vel+p*tmp.rot;
292     return tmp;
293 }
294 Twist Frame::Inverse(const Twist& arg) const
295 {
296     Twist tmp;
297     tmp.rot =  M.Inverse(arg.rot);
298     tmp.vel = M.Inverse(arg.vel-p*arg.rot);
299     return tmp;
300 }
301
302 Twist Twist::Zero()
303 {
304     return Twist(Vector::Zero(),Vector::Zero());
305 }
306
307
308 void Twist::ReverseSign()
309 {   
310     vel.ReverseSign();
311     rot.ReverseSign();
312 }
313
314 Twist Twist::RefPoint(const Vector& v_base_AB) const
315      // Changes the reference point of the twist.
316      // The vector v_base_AB is expressed in the same base as the twist
317      // The vector v_base_AB is a vector from the old point to
318      // the new point.
319      // Complexity : 6M+6A
320 {
321     return Twist(this->vel+this->rot*v_base_AB,this->rot);
322 }
323
324 Twist& Twist::operator-=(const Twist& arg)
325 {
326     vel-=arg.vel;
327     rot -=arg.rot;
328     return *this;
329 }
330
331 Twist& Twist::operator+=(const Twist& arg)
332 {
333     vel+=arg.vel;
334     rot +=arg.rot;
335     return *this;
336 }
337
338 double& Twist::operator()(int i)
339 {
340     // assert((0<=i)&&(i<6)); done by underlying routines
341     if (i<3) 
342         return vel(i);
343     else
344         return rot(i-3);
345 }
346
347 double Twist::operator()(int i) const
348 {
349     // assert((0<=i)&&(i<6)); done by underlying routines
350     if (i<3) 
351         return vel(i);
352     else
353         return rot(i-3);
354 }
355
356
357 Twist operator*(const Twist& lhs,double rhs)
358 {
359     return Twist(lhs.vel*rhs,lhs.rot*rhs);
360 }
361
362 Twist operator*(double lhs,const Twist& rhs)
363 {
364     return Twist(lhs*rhs.vel,lhs*rhs.rot);
365 }
366
367 Twist operator/(const Twist& lhs,double rhs)
368 {
369     return Twist(lhs.vel/rhs,lhs.rot/rhs);
370 }
371
372 // addition of Twist's
373 Twist operator+(const Twist& lhs,const Twist& rhs)
374 {
375     return Twist(lhs.vel+rhs.vel,lhs.rot+rhs.rot);
376 }
377
378 Twist operator-(const Twist& lhs,const Twist& rhs)
379 {
380     return Twist(lhs.vel-rhs.vel,lhs.rot-rhs.rot);
381 }
382
383 // unary -
384 Twist operator-(const Twist& arg) 
385 {
386     return Twist(-arg.vel,-arg.rot);
387 }
388
389 Frame::Frame(const Rotation & R)
390 {
391     M=R;
392     p=Vector::Zero();
393 }
394
395 Frame::Frame(const Vector & V)
396 {
397     M = Rotation::Identity();
398     p = V;
399 }
400
401 Frame::Frame(const Rotation & R, const Vector & V)
402 {
403     M = R;
404     p = V;
405 }
406
407  Frame operator *(const Frame& lhs,const Frame& rhs)
408 // Complexity : 36M+36A
409 {
410     return Frame(lhs.M*rhs.M,lhs.M*rhs.p+lhs.p);
411 }
412
413 Vector Frame::operator *(const Vector & arg) const
414 {
415     return M*arg+p;
416 }
417
418 Vector Frame::Inverse(const Vector& arg) const
419 {
420     return M.Inverse(arg-p);
421 }
422
423 Frame Frame::Inverse() const
424 {
425     return Frame(M.Inverse(),-M.Inverse(p));
426 }
427
428
429 Frame& Frame::operator =(const Frame & arg)
430
431     M = arg.M;
432     p = arg.p;
433     return *this;
434 }
435
436 Frame::Frame(const Frame & arg) :
437     p(arg.p),M(arg.M)
438 {}
439
440
441 void Vector::ReverseSign()
442 {
443     data[0] = -data[0];
444     data[1] = -data[1];
445     data[2] = -data[2];
446 }
447
448
449
450 Vector operator-(const Vector & arg)
451 {
452     Vector tmp;
453     tmp.data[0]=-arg.data[0];
454     tmp.data[1]=-arg.data[1];
455     tmp.data[2]=-arg.data[2];
456     return tmp;
457 }
458
459 void Vector::Set2DXY(const Vector2& v)
460 // a 3D vector where the 2D vector v is put in the XY plane
461 {
462     data[0]=v(0);
463     data[1]=v(1);
464     data[2]=0;
465
466 }
467 void Vector::Set2DYZ(const Vector2& v)
468 // a 3D vector where the 2D vector v is put in the YZ plane
469 {
470     data[1]=v(0);
471     data[2]=v(1);
472     data[0]=0;
473
474 }
475
476 void Vector::Set2DZX(const Vector2& v)
477 // a 3D vector where the 2D vector v is put in the ZX plane
478 {
479     data[2]=v(0);
480     data[0]=v(1);
481     data[1]=0;
482
483 }
484
485
486
487
488
489 double& Rotation::operator()(int i,int j) {
490     FRAMES_CHECKI((0<=i)&&(i<=2)&&(0<=j)&&(j<=2));
491     return data[i*3+j];
492 }
493
494 double Rotation::operator()(int i,int j) const {
495     FRAMES_CHECKI((0<=i)&&(i<=2)&&(0<=j)&&(j<=2));
496     return data[i*3+j];
497 }
498
499 Rotation::Rotation( double Xx,double Yx,double Zx,
500                             double Xy,double Yy,double Zy,
501                             double Xz,double Yz,double Zz)
502 {
503     data[0] = Xx;data[1]=Yx;data[2]=Zx;
504     data[3] = Xy;data[4]=Yy;data[5]=Zy;
505     data[6] = Xz;data[7]=Yz;data[8]=Zz;
506 }
507
508
509 Rotation::Rotation(const Vector& x,const Vector& y,const Vector& z) 
510 {
511     data[0] = x.data[0];data[3] = x.data[1];data[6] = x.data[2];
512     data[1] = y.data[0];data[4] = y.data[1];data[7] = y.data[2];
513     data[2] = z.data[0];data[5] = z.data[1];data[8] = z.data[2];
514 }
515
516 Rotation& Rotation::operator=(const Rotation& arg) {
517     int count=9;
518     while (count--) data[count] = arg.data[count];
519     return *this;
520 }
521
522 Vector Rotation::operator*(const Vector& v) const {
523 // Complexity : 9M+6A
524     return Vector(
525      data[0]*v.data[0] + data[1]*v.data[1] + data[2]*v.data[2],
526      data[3]*v.data[0] + data[4]*v.data[1] + data[5]*v.data[2],
527      data[6]*v.data[0] + data[7]*v.data[1] + data[8]*v.data[2] 
528     );
529 }
530
531 Twist Rotation::operator * (const Twist& arg) const
532      // Transformation of the base to which the twist is expressed.
533      // look at Frame*Twist for a transformation that also transforms
534      // the velocity reference point.
535      // Complexity : 18M+12A
536 {
537     return Twist((*this)*arg.vel,(*this)*arg.rot);
538 }
539
540 Wrench Rotation::operator * (const Wrench& arg) const
541      // Transformation of the base to which the wrench is expressed.
542      // look at Frame*Twist for a transformation that also transforms
543      // the force reference point.
544 {
545     return Wrench((*this)*arg.force,(*this)*arg.torque);
546 }
547
548 Rotation Rotation::Identity() {
549         return Rotation(1,0,0,0,1,0,0,0,1);
550 }
551 // *this = *this * ROT(X,angle)
552 void Rotation::DoRotX(double angle)
553 {
554     double cs = cos(angle);
555     double sn = sin(angle);
556     double x1,x2,x3;
557     x1  = cs* (*this)(0,1) + sn* (*this)(0,2);
558     x2  = cs* (*this)(1,1) + sn* (*this)(1,2);
559     x3  = cs* (*this)(2,1) + sn* (*this)(2,2);
560     (*this)(0,2) = -sn* (*this)(0,1) + cs* (*this)(0,2);
561     (*this)(1,2) = -sn* (*this)(1,1) + cs* (*this)(1,2);
562     (*this)(2,2) = -sn* (*this)(2,1) + cs* (*this)(2,2);
563     (*this)(0,1) = x1;
564     (*this)(1,1) = x2;
565     (*this)(2,1) = x3;
566 }
567
568 void Rotation::DoRotY(double angle)
569 {
570     double cs = cos(angle);
571     double sn = sin(angle);
572     double x1,x2,x3;
573     x1  = cs* (*this)(0,0) - sn* (*this)(0,2);
574     x2  = cs* (*this)(1,0) - sn* (*this)(1,2);
575     x3  = cs* (*this)(2,0) - sn* (*this)(2,2);
576     (*this)(0,2) = sn* (*this)(0,0) + cs* (*this)(0,2);
577     (*this)(1,2) = sn* (*this)(1,0) + cs* (*this)(1,2);
578     (*this)(2,2) = sn* (*this)(2,0) + cs* (*this)(2,2);
579     (*this)(0,0) = x1;
580     (*this)(1,0) = x2;
581     (*this)(2,0) = x3;
582 }
583
584 void Rotation::DoRotZ(double angle)
585 {
586     double cs = cos(angle);
587     double sn = sin(angle);
588     double x1,x2,x3;
589     x1  = cs* (*this)(0,0) + sn* (*this)(0,1);
590     x2  = cs* (*this)(1,0) + sn* (*this)(1,1);
591     x3  = cs* (*this)(2,0) + sn* (*this)(2,1);
592     (*this)(0,1) = -sn* (*this)(0,0) + cs* (*this)(0,1);
593     (*this)(1,1) = -sn* (*this)(1,0) + cs* (*this)(1,1);
594     (*this)(2,1) = -sn* (*this)(2,0) + cs* (*this)(2,1);
595     (*this)(0,0) = x1;
596     (*this)(1,0) = x2;
597     (*this)(2,0) = x3;
598 }
599
600
601 Rotation Rotation::RotX(double angle) {
602     double cs=cos(angle);
603     double sn=sin(angle);
604     return Rotation(1,0,0,0,cs,-sn,0,sn,cs);
605 }
606 Rotation Rotation::RotY(double angle) {
607     double cs=cos(angle);
608     double sn=sin(angle);
609     return Rotation(cs,0,sn,0,1,0,-sn,0,cs);
610 }
611 Rotation Rotation::RotZ(double angle) {
612     double cs=cos(angle);
613     double sn=sin(angle);
614     return Rotation(cs,-sn,0,sn,cs,0,0,0,1);
615 }
616
617
618
619
620 void Frame::Integrate(const Twist& t_this,double samplefrequency)
621 {
622     double n = t_this.rot.Norm()/samplefrequency;
623     if (n<epsilon) {
624         p += M*(t_this.vel/samplefrequency);
625     } else {
626         (*this) = (*this) * 
627             Frame ( Rotation::Rot( t_this.rot, n ),
628                     t_this.vel/samplefrequency
629                 );
630     }
631 }
632
633 Rotation Rotation::Inverse() const
634 {
635     Rotation tmp(*this);
636     tmp.SetInverse();
637     return tmp;
638 }
639
640 Vector Rotation::Inverse(const Vector& v) const {
641     return Vector(
642      data[0]*v.data[0] + data[3]*v.data[1] + data[6]*v.data[2],
643      data[1]*v.data[0] + data[4]*v.data[1] + data[7]*v.data[2],
644      data[2]*v.data[0] + data[5]*v.data[1] + data[8]*v.data[2] 
645     );
646 }
647
648 void Rotation::setValue(float* oglmat)
649 {
650     data[0] = *oglmat++; data[3] = *oglmat++; data[6] = *oglmat++; oglmat++;
651     data[1] = *oglmat++; data[4] = *oglmat++; data[7] = *oglmat++; oglmat++;
652     data[2] = *oglmat++; data[5] = *oglmat++; data[8] = *oglmat;
653         Ortho();
654 }
655
656 void Rotation::getValue(float* oglmat) const
657 {
658         *oglmat++ = (float)data[0]; *oglmat++ = (float)data[3]; *oglmat++ = (float)data[6]; *oglmat++ = 0.f;
659         *oglmat++ = (float)data[1]; *oglmat++ = (float)data[4]; *oglmat++ = (float)data[7]; *oglmat++ = 0.f;
660         *oglmat++ = (float)data[2]; *oglmat++ = (float)data[5]; *oglmat++ = (float)data[8]; *oglmat++ = 0.f;
661         *oglmat++ = 0.f;            *oglmat++ = 0.f;            *oglmat++ = 0.f;            *oglmat   = 1.f;
662 }
663
664 void Rotation::SetInverse()
665 {
666     double tmp;
667     tmp = data[1];data[1]=data[3];data[3]=tmp;
668     tmp = data[2];data[2]=data[6];data[6]=tmp;
669     tmp = data[5];data[5]=data[7];data[7]=tmp;
670 }
671
672
673
674
675
676
677
678 double Frame::operator()(int i,int j) {
679     FRAMES_CHECKI((0<=i)&&(i<=3)&&(0<=j)&&(j<=3));
680     if (i==3) {
681         if (j==3)
682             return 1.0;
683         else
684             return 0.0;
685     } else {
686         if (j==3) 
687             return p(i);
688         else
689             return M(i,j);
690
691     }
692 }
693
694 double Frame::operator()(int i,int j) const {
695     FRAMES_CHECKI((0<=i)&&(i<=3)&&(0<=j)&&(j<=3));
696         if (i==3) {
697         if (j==3)
698             return 1;
699         else
700             return 0;
701     } else {
702         if (j==3) 
703             return p(i);
704         else
705             return M(i,j);
706
707     }
708 }
709
710
711 Frame Frame::Identity() {
712     return Frame(Rotation::Identity(),Vector::Zero());
713 }
714
715
716 void Frame::setValue(float* oglmat)
717 {
718         M.setValue(oglmat);
719         p.data[0] = oglmat[12];
720         p.data[1] = oglmat[13];
721         p.data[2] = oglmat[14];
722 }
723
724 void Frame::getValue(float* oglmat) const
725 {
726         M.getValue(oglmat);
727         oglmat[12] = (float)p.data[0];
728         oglmat[13] = (float)p.data[1];
729         oglmat[14] = (float)p.data[2];
730 }
731
732 void Vector::Set2DPlane(const Frame& F_someframe_XY,const Vector2& v_XY)
733 // a 3D vector where the 2D vector v is put in the XY plane of the frame
734 // F_someframe_XY.
735 {
736 Vector tmp_XY;
737 tmp_XY.Set2DXY(v_XY);
738 tmp_XY = F_someframe_XY*(tmp_XY);
739 }
740
741
742
743
744
745
746
747
748
749 //============ 2 dimensional version of the frames objects =============
750 IMETHOD Vector2::Vector2(const Vector2 & arg)
751 {
752     data[0] = arg.data[0];
753     data[1] = arg.data[1];
754 }
755
756 IMETHOD Vector2::Vector2(double x,double y)
757 {
758         data[0]=x;data[1]=y;
759 }
760
761 IMETHOD Vector2::Vector2(double* xy)
762 {
763         data[0]=xy[0];data[1]=xy[1];
764 }
765
766 IMETHOD Vector2::Vector2(float* xy)
767 {
768         data[0]=xy[0];data[1]=xy[1];
769 }
770
771 IMETHOD Vector2& Vector2::operator =(const Vector2 & arg)
772 {
773     data[0] = arg.data[0];
774     data[1] = arg.data[1];
775     return *this;
776 }
777
778 IMETHOD void Vector2::GetValue(double* xy) const
779 {
780         xy[0]=data[0];xy[1]=data[1];
781 }
782
783 IMETHOD Vector2 operator +(const Vector2 & lhs,const Vector2& rhs)
784 {
785     return Vector2(lhs.data[0]+rhs.data[0],lhs.data[1]+rhs.data[1]);
786 }
787
788 IMETHOD Vector2 operator -(const Vector2 & lhs,const Vector2& rhs)
789 {
790     return Vector2(lhs.data[0]-rhs.data[0],lhs.data[1]-rhs.data[1]);
791 }
792
793 IMETHOD Vector2 operator *(const Vector2& lhs,double rhs) 
794 {
795     return Vector2(lhs.data[0]*rhs,lhs.data[1]*rhs);
796 }
797
798 IMETHOD Vector2 operator *(double lhs,const Vector2& rhs) 
799 {
800     return Vector2(lhs*rhs.data[0],lhs*rhs.data[1]);
801 }
802
803 IMETHOD Vector2 operator /(const Vector2& lhs,double rhs) 
804 {
805     return Vector2(lhs.data[0]/rhs,lhs.data[1]/rhs);
806 }
807
808 IMETHOD Vector2& Vector2::operator +=(const Vector2 & arg)
809 {
810     data[0]+=arg.data[0];
811     data[1]+=arg.data[1];
812     return *this;
813 }
814
815 IMETHOD Vector2& Vector2::operator -=(const Vector2 & arg)
816 {
817     data[0]-=arg.data[0];
818     data[1]-=arg.data[1];
819     return *this;
820 }
821
822 IMETHOD Vector2 Vector2::Zero() {
823     return Vector2(0,0);
824 }
825
826 IMETHOD double Vector2::operator()(int index) const {
827     FRAMES_CHECKI((0<=index)&&(index<=1));
828     return data[index];
829 }
830
831 IMETHOD double& Vector2::operator () (int index)
832 {
833     FRAMES_CHECKI((0<=index)&&(index<=1));
834     return data[index];
835 }
836 IMETHOD void Vector2::ReverseSign()
837 {
838     data[0] = -data[0];
839     data[1] = -data[1];
840 }
841
842
843 IMETHOD Vector2 operator-(const Vector2 & arg)
844 {
845     return Vector2(-arg.data[0],-arg.data[1]);
846 }
847
848
849 IMETHOD void Vector2::Set3DXY(const Vector& v)
850 // projects v in its XY plane, and sets *this to these values
851 {
852     data[0]=v(0);
853     data[1]=v(1);
854 }
855 IMETHOD void Vector2::Set3DYZ(const Vector& v)
856 // projects v in its XY plane, and sets *this to these values
857 {
858     data[0]=v(1);
859     data[1]=v(2);
860 }
861 IMETHOD void Vector2::Set3DZX(const Vector& v)
862 // projects v in its XY plane, and and sets *this to these values
863 {
864     data[0]=v(2);
865     data[1]=v(0);
866 }
867
868 IMETHOD void Vector2::Set3DPlane(const Frame& F_someframe_XY,const Vector& v_someframe) 
869 // projects v in the XY plane of F_someframe_XY, and sets *this to these values
870 // expressed wrt someframe.
871 {
872     Vector tmp = F_someframe_XY.Inverse(v_someframe);
873     data[0]=tmp(0);
874     data[1]=tmp(1);
875 }
876
877
878
879 IMETHOD Rotation2& Rotation2::operator=(const Rotation2& arg) {
880     c=arg.c;s=arg.s;
881     return *this;
882 }
883
884 IMETHOD Vector2 Rotation2::operator*(const Vector2& v) const {
885     return Vector2(v.data[0]*c-v.data[1]*s,v.data[0]*s+v.data[1]*c);
886 }
887
888 IMETHOD double Rotation2::operator()(int i,int j) const {
889     FRAMES_CHECKI((0<=i)&&(i<=1)&&(0<=j)&&(j<=1));
890     if (i==j) return c;
891     if (i==0) 
892         return s;
893     else
894         return -s;
895 }
896
897
898 IMETHOD Rotation2 operator *(const Rotation2& lhs,const Rotation2& rhs) {
899     return Rotation2(lhs.c*rhs.c-lhs.s*rhs.s,lhs.s*rhs.c+lhs.c*rhs.s);
900 }
901
902 IMETHOD void Rotation2::SetInverse() {
903     s=-s;
904 }
905
906 IMETHOD Rotation2 Rotation2::Inverse() const {
907     return Rotation2(c,-s);
908 }
909
910 IMETHOD Vector2 Rotation2::Inverse(const Vector2& v) const {
911     return Vector2(v.data[0]*c+v.data[1]*s,-v.data[0]*s+v.data[1]*c);
912 }
913
914 IMETHOD Rotation2 Rotation2::Identity() {
915     return Rotation2(1,0);
916 }
917      
918 IMETHOD void Rotation2::SetIdentity()
919 {
920         c = 1;
921         s = 0;
922 }
923
924 IMETHOD void Rotation2::SetRot(double angle) {
925     c=cos(angle);s=sin(angle);
926 }
927
928 IMETHOD Rotation2 Rotation2::Rot(double angle) {
929     return Rotation2(cos(angle),sin(angle));
930 }
931
932 IMETHOD double Rotation2::GetRot() const {
933     return atan2(s,c);
934 }
935
936
937 IMETHOD Frame2::Frame2() {
938 }
939
940 IMETHOD Frame2::Frame2(const Rotation2 & R)
941 {
942     M=R;
943     p=Vector2::Zero();
944 }
945
946 IMETHOD Frame2::Frame2(const Vector2 & V)
947 {
948     M = Rotation2::Identity();
949     p = V;
950 }
951
952 IMETHOD Frame2::Frame2(const Rotation2 & R, const Vector2 & V)
953 {
954     M = R;
955     p = V;
956 }
957
958 IMETHOD Frame2 operator *(const Frame2& lhs,const Frame2& rhs)
959 {
960     return Frame2(lhs.M*rhs.M,lhs.M*rhs.p+lhs.p);
961 }
962
963 IMETHOD Vector2 Frame2::operator *(const Vector2 & arg)
964 {
965     return M*arg+p;
966 }
967
968 IMETHOD Vector2 Frame2::Inverse(const Vector2& arg) const
969 {
970     return M.Inverse(arg-p);
971 }
972
973 IMETHOD void Frame2::SetIdentity()
974 {
975         M.SetIdentity();
976         p = Vector2::Zero();
977 }
978
979 IMETHOD void Frame2::SetInverse()
980 {
981     M.SetInverse();
982     p = M*p;
983     p.ReverseSign();
984 }
985
986
987 IMETHOD Frame2 Frame2::Inverse() const
988 {
989     Frame2 tmp(*this);
990     tmp.SetInverse();
991     return tmp;
992 }
993
994 IMETHOD Frame2& Frame2::operator =(const Frame2 & arg)
995
996     M = arg.M;
997     p = arg.p;
998     return *this;
999 }
1000
1001 IMETHOD Frame2::Frame2(const Frame2 & arg) :
1002     p(arg.p), M(arg.M)
1003 {}
1004
1005
1006 IMETHOD double Frame2::operator()(int i,int j) {
1007     FRAMES_CHECKI((0<=i)&&(i<=2)&&(0<=j)&&(j<=2));
1008     if (i==2) {
1009         if (j==2)
1010             return 1;
1011         else
1012             return 0;
1013     } else {
1014         if (j==2) 
1015             return p(i);
1016         else
1017             return M(i,j);
1018
1019     }
1020 }
1021
1022 IMETHOD double Frame2::operator()(int i,int j) const {
1023     FRAMES_CHECKI((0<=i)&&(i<=2)&&(0<=j)&&(j<=2));
1024     if (i==2) {
1025         if (j==2)
1026             return 1;
1027         else
1028             return 0;
1029     } else {
1030         if (j==2) 
1031             return p(i);
1032         else
1033             return M(i,j);
1034
1035     }
1036 }
1037
1038 // Scalar products.
1039
1040 IMETHOD double dot(const Vector& lhs,const Vector& rhs) {
1041     return rhs(0)*lhs(0)+rhs(1)*lhs(1)+rhs(2)*lhs(2);
1042 }
1043
1044 IMETHOD double dot(const Twist& lhs,const Wrench& rhs) {
1045     return dot(lhs.vel,rhs.force)+dot(lhs.rot,rhs.torque);
1046 }
1047
1048 IMETHOD double dot(const Wrench& rhs,const Twist& lhs) {
1049     return dot(lhs.vel,rhs.force)+dot(lhs.rot,rhs.torque);
1050 }
1051
1052
1053
1054
1055
1056 // Equality operators
1057
1058
1059
1060 IMETHOD bool Equal(const Vector& a,const Vector& b,double eps) {
1061         return (Equal(a.data[0],b.data[0],eps)&&
1062                 Equal(a.data[1],b.data[1],eps)&&
1063                 Equal(a.data[2],b.data[2],eps)   );
1064      }
1065      
1066
1067 IMETHOD bool Equal(const Frame& a,const Frame& b,double eps) {
1068         return (Equal(a.p,b.p,eps)&&
1069                 Equal(a.M,b.M,eps)   );
1070 }
1071
1072 IMETHOD bool Equal(const Wrench& a,const Wrench& b,double eps) {
1073         return (Equal(a.force,b.force,eps)&&
1074                 Equal(a.torque,b.torque,eps)  );
1075 }
1076
1077 IMETHOD bool Equal(const Twist& a,const Twist& b,double eps) {
1078         return (Equal(a.rot,b.rot,eps)&&
1079                 Equal(a.vel,b.vel,eps)  );
1080 }
1081
1082 IMETHOD bool Equal(const Vector2& a,const Vector2& b,double eps) {
1083         return (Equal(a.data[0],b.data[0],eps)&&
1084                 Equal(a.data[1],b.data[1],eps)   );
1085      }
1086      
1087 IMETHOD bool Equal(const Rotation2& a,const Rotation2& b,double eps) {
1088     return ( Equal(a.c,b.c,eps) && Equal(a.s,b.s,eps) );
1089 }
1090
1091 IMETHOD bool Equal(const Frame2& a,const Frame2& b,double eps) {
1092         return (Equal(a.p,b.p,eps)&&
1093                 Equal(a.M,b.M,eps)   );
1094 }
1095
1096 IMETHOD void SetToZero(Vector& v) {
1097     v=Vector::Zero();
1098 }
1099 IMETHOD void SetToZero(Twist& v) {
1100     SetToZero(v.rot);
1101     SetToZero(v.vel);
1102 }
1103 IMETHOD void SetToZero(Wrench& v) {
1104     SetToZero(v.force);
1105     SetToZero(v.torque);
1106 }
1107
1108 IMETHOD void SetToZero(Vector2& v) {
1109     v = Vector2::Zero();
1110 }
1111
1112
1113 ////////////////////////////////////////////////////////////////
1114 // The following defines the operations
1115 //   diff
1116 //   addDelta
1117 //   random
1118 //   posrandom
1119 // on all the types defined in this library.
1120 // (mostly for uniform integration, differentiation and testing).
1121 // Defined as functions because double is not a class and a method
1122 // would brake uniformity when defined for a double.
1123 ////////////////////////////////////////////////////////////////
1124
1125
1126
1127
1128
1129
1130 /**
1131  * axis_a_b is a rotation vector, its norm is a rotation angle
1132  * axis_a_b rotates the a frame towards the b frame.
1133  * This routine returns the rotation matrix R_a_b
1134  */
1135 IMETHOD Rotation Rot(const Vector& axis_a_b) {
1136     // The formula is 
1137     // V.(V.tr) + st*[V x] + ct*(I-V.(V.tr))
1138     // can be found by multiplying it with an arbitrary vector p
1139     // and noting that this vector is rotated.
1140         Vector rotvec = axis_a_b;
1141         double angle = rotvec.Normalize(1E-10);
1142     double ct = ::cos(angle);
1143     double st = ::sin(angle);
1144     double vt = 1-ct;
1145     return Rotation(
1146         ct            +  vt*rotvec(0)*rotvec(0), 
1147         -rotvec(2)*st +  vt*rotvec(0)*rotvec(1), 
1148         rotvec(1)*st  +  vt*rotvec(0)*rotvec(2),
1149         rotvec(2)*st  +  vt*rotvec(1)*rotvec(0),
1150         ct            +  vt*rotvec(1)*rotvec(1),
1151         -rotvec(0)*st +  vt*rotvec(1)*rotvec(2),
1152         -rotvec(1)*st +  vt*rotvec(2)*rotvec(0),
1153         rotvec(0)*st  +  vt*rotvec(2)*rotvec(1),
1154         ct            +  vt*rotvec(2)*rotvec(2)
1155         );
1156     }
1157
1158 IMETHOD Vector diff(const Vector& a,const Vector& b,double dt) {
1159         return (b-a)/dt;
1160 }
1161
1162 /**
1163  * \brief diff operator for displacement rotational velocity.
1164  *
1165  * The Vector arguments here represent a displacement rotational velocity.  i.e. a rotation
1166  * around a fixed axis for a certain angle.  For this representation you cannot use diff() but
1167  * have to use diff_displ().
1168  *
1169  * \TODO represent a displacement twist and displacement rotational velocity with another
1170  *       class, instead of Vector and Twist.
1171  * \warning do not confuse displacement rotational velocities and velocities
1172  * \warning do not confuse displacement twist and twist.
1173  *
1174 IMETHOD Vector diff_displ(const Vector& a,const Vector& b,double dt) {
1175         return diff(Rot(a),Rot(b),dt);
1176 }*/
1177
1178 /**
1179  * \brief diff operator for displacement twist.
1180  *
1181  * The Twist arguments here represent a displacement twist.  i.e. a rotation
1182  * around a fixed axis for a certain angle.  For this representation you cannot use diff() but
1183  * have to use diff_displ().
1184  *
1185  * \warning do not confuse displacement rotational velocities and velocities
1186  * \warning do not confuse displacement twist and twist.
1187  *
1188
1189 IMETHOD Twist diff_displ(const Twist& a,const Twist& b,double dt) {
1190         return Twist(diff(a.vel,b.vel,dt),diff(Rot(a.rot),Rot(b.rot),dt));
1191 }
1192 */
1193
1194 IMETHOD Vector diff(const Rotation& R_a_b1,const Rotation& R_a_b2,double dt) {
1195         Rotation R_b1_b2(R_a_b1.Inverse()*R_a_b2);
1196         return R_a_b1 * R_b1_b2.GetRot() / dt;
1197 }
1198 IMETHOD Twist diff(const Frame& F_a_b1,const Frame& F_a_b2,double dt) {
1199         return Twist(
1200                         diff(F_a_b1.p,F_a_b2.p,dt),
1201                         diff(F_a_b1.M,F_a_b2.M,dt)
1202                         );
1203 }
1204 IMETHOD Twist diff(const Twist& a,const Twist& b,double dt) {
1205         return Twist(diff(a.vel,b.vel,dt),diff(a.rot,b.rot,dt));
1206 }
1207
1208 IMETHOD Wrench diff(const Wrench& a,const Wrench& b,double dt) {
1209         return Wrench(
1210                         diff(a.force,b.force,dt),
1211                         diff(a.torque,b.torque,dt)
1212                         );
1213 }
1214
1215
1216 IMETHOD Vector addDelta(const Vector& a,const Vector&da,double dt) {
1217         return a+da*dt;
1218 }
1219
1220 IMETHOD Rotation addDelta(const Rotation& a,const Vector&da,double dt) {
1221         return a*Rot(a.Inverse(da)*dt);
1222 }
1223 IMETHOD Frame addDelta(const Frame& a,const Twist& da,double dt) {
1224         return Frame(
1225                         addDelta(a.M,da.rot,dt),
1226                         addDelta(a.p,da.vel,dt)
1227                    );
1228 }
1229 IMETHOD Twist addDelta(const Twist& a,const Twist&da,double dt) {
1230         return Twist(addDelta(a.vel,da.vel,dt),addDelta(a.rot,da.rot,dt));
1231 }
1232 IMETHOD Wrench addDelta(const Wrench& a,const Wrench&da,double dt) {
1233         return Wrench(addDelta(a.force,da.force,dt),addDelta(a.torque,da.torque,dt));
1234 }
1235
1236
1237 /**
1238  * \brief addDelta operator for displacement rotational velocity.
1239  *
1240  * The Vector arguments here represent a displacement rotational velocity.  i.e. a rotation
1241  * around a fixed axis for a certain angle.  For this representation you cannot use diff() but
1242  * have to use diff_displ().
1243  *
1244  * \param a  : displacement rotational velocity
1245  * \param da : rotational velocity 
1246  * \return   displacement rotational velocity
1247  *
1248  * \warning do not confuse displacement rotational velocities and velocities
1249  * \warning do not confuse displacement twist and twist.
1250  *
1251 IMETHOD Vector addDelta_displ(const Vector& a,const Vector&da,double dt) {
1252     return getRot(addDelta(Rot(a),da,dt)); 
1253 }*/
1254
1255 /**
1256  * \brief addDelta operator for displacement twist.
1257  *
1258  * The Vector arguments here represent a displacement rotational velocity.  i.e. a rotation
1259  * around a fixed axis for a certain angle.  For this representation you cannot use diff() but
1260  * have to use diff_displ().
1261  *
1262  * \param a  : displacement twist 
1263  * \param da : twist 
1264  * \return   displacement twist 
1265  *
1266  * \warning do not confuse displacement rotational velocities and velocities
1267  * \warning do not confuse displacement twist and twist.
1268  *
1269 IMETHOD Twist addDelta_displ(const Twist& a,const Twist&da,double dt) {
1270     return Twist(addDelta(a.vel,da.vel,dt),addDelta_displ(a.rot,da.rot,dt)); 
1271 }*/
1272
1273
1274 IMETHOD void random(Vector& a) {
1275         random(a[0]);
1276         random(a[1]);
1277         random(a[2]);
1278 }
1279 IMETHOD void random(Twist& a) {
1280         random(a.rot);
1281         random(a.vel);
1282 }
1283 IMETHOD void random(Wrench& a) {
1284         random(a.torque);
1285         random(a.force);
1286 }
1287
1288 IMETHOD void random(Rotation& R) {
1289         double alfa;
1290         double beta;
1291         double gamma;
1292         random(alfa);
1293         random(beta);
1294         random(gamma);
1295         R = Rotation::EulerZYX(alfa,beta,gamma);
1296 }
1297
1298 IMETHOD void random(Frame& F) {
1299         random(F.M);
1300         random(F.p);
1301 }
1302
1303 IMETHOD void posrandom(Vector& a) {
1304         posrandom(a[0]);
1305         posrandom(a[1]);
1306         posrandom(a[2]);
1307 }
1308 IMETHOD void posrandom(Twist& a) {
1309         posrandom(a.rot);
1310         posrandom(a.vel);
1311 }
1312 IMETHOD void posrandom(Wrench& a) {
1313         posrandom(a.torque);
1314         posrandom(a.force);
1315 }
1316
1317 IMETHOD void posrandom(Rotation& R) {
1318         double alfa;
1319         double beta;
1320         double gamma;
1321         posrandom(alfa);
1322         posrandom(beta);
1323         posrandom(gamma);
1324         R = Rotation::EulerZYX(alfa,beta,gamma);
1325 }
1326
1327 IMETHOD void posrandom(Frame& F) {
1328         random(F.M);
1329         random(F.p);
1330 }
1331
1332
1333
1334
1335 IMETHOD bool operator==(const Frame& a,const Frame& b ) {
1336 #ifdef KDL_USE_EQUAL
1337     return Equal(a,b);
1338 #else
1339         return (a.p == b.p &&
1340                 a.M == b.M );
1341 #endif
1342 }
1343
1344 IMETHOD bool operator!=(const Frame& a,const Frame& b) {
1345     return !operator==(a,b);
1346 }
1347
1348 IMETHOD bool operator==(const Vector& a,const Vector& b) {
1349 #ifdef KDL_USE_EQUAL
1350     return Equal(a,b);
1351 #else
1352         return (a.data[0]==b.data[0]&&
1353                 a.data[1]==b.data[1]&&
1354                 a.data[2]==b.data[2] );
1355 #endif
1356      }
1357
1358 IMETHOD bool operator!=(const Vector& a,const Vector& b) {
1359     return !operator==(a,b);
1360 }
1361
1362 IMETHOD bool operator==(const Twist& a,const Twist& b) {
1363 #ifdef KDL_USE_EQUAL
1364     return Equal(a,b);
1365 #else
1366         return (a.rot==b.rot &&
1367                 a.vel==b.vel  );
1368 #endif
1369 }
1370
1371 IMETHOD bool operator!=(const Twist& a,const Twist& b) {
1372     return !operator==(a,b);
1373 }
1374
1375 IMETHOD bool operator==(const Wrench& a,const Wrench& b ) {
1376 #ifdef KDL_USE_EQUAL
1377     return Equal(a,b);
1378 #else
1379     return (a.force==b.force &&
1380             a.torque==b.torque );
1381 #endif
1382 }
1383
1384 IMETHOD bool operator!=(const Wrench& a,const Wrench& b) {
1385     return !operator==(a,b);
1386 }
1387 IMETHOD bool operator!=(const Rotation& a,const Rotation& b) {
1388     return !operator==(a,b);
1389 }
1390