Blender  V3.3
frames.inl
Go to the documentation of this file.
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., 51 Franklin Street, *
24  * Fifth Floor, Boston, MA 02110-1301, 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 
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 
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 
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 
147 {
148  data[0] *= arg;
149  data[1] *= arg;
150  data[2] *= arg;
151  return *this;
152 }
153 
154 Vector Vector::Zero()
155 {
156  return Vector(0,0,0);
157 }
158 
159 double Vector::operator()(int index) const {
160  FRAMES_CHECKI((0<=index)&&(index<=2));
161  return data[index];
162 }
163 
164 double& Vector::operator () (int index)
165 {
166  FRAMES_CHECKI((0<=index)&&(index<=2));
167  return data[index];
168 }
169 
170 IMETHOD Vector Normalize(const Vector& a, double eps)
171 {
172  double l=a.Norm();
173  return (l<eps) ? Vector(0.0,0.0,0.0) : a/l;
174 }
175 
177 // Complexity : 24M+18A
178 {
179  Wrench tmp;
180  tmp.force = M*arg.force;
181  tmp.torque = M*arg.torque + p*tmp.force;
182  return tmp;
183 }
184 
185 Wrench Frame::Inverse(const Wrench& arg) const
186 {
187  Wrench tmp;
188  tmp.force = M.Inverse(arg.force);
189  tmp.torque = M.Inverse(arg.torque-p*arg.force);
190  return tmp;
191 }
192 
193 
194 
195 Wrench Rotation::Inverse(const Wrench& arg) const
196 {
197  return Wrench(Inverse(arg.force),Inverse(arg.torque));
198 }
199 
200 Twist Rotation::Inverse(const Twist& arg) const
201 {
202  return Twist(Inverse(arg.vel),Inverse(arg.rot));
203 }
204 
205 Wrench Wrench::Zero()
206 {
207  return Wrench(Vector::Zero(),Vector::Zero());
208 }
209 
210 
211 void Wrench::ReverseSign()
212 {
213  torque.ReverseSign();
214  force.ReverseSign();
215 }
216 
217 Wrench Wrench::RefPoint(const Vector& v_base_AB) const
218  // Changes the reference point of the Wrench.
219  // The vector v_base_AB is expressed in the same base as the twist
220  // The vector v_base_AB is a vector from the old point to
221  // the new point.
222 {
223  return Wrench(this->force,
224  this->torque+this->force*v_base_AB
225  );
226 }
227 
228 
230 {
231  torque-=arg.torque;
232  force -=arg.force;
233  return *this;
234 }
235 
237 {
238  torque+=arg.torque;
239  force +=arg.force;
240  return *this;
241 }
242 
243 double& Wrench::operator()(int i)
244 {
245  // assert((0<=i)&&(i<6)); done by underlying routines
246  if (i<3)
247  return force(i);
248  else
249  return torque(i-3);
250 }
251 
252 double Wrench::operator()(int i) const
253 {
254  // assert((0<=i)&&(i<6)); done by underlying routines
255  if (i<3)
256  return force(i);
257  else
258  return torque(i-3);
259 }
260 
261 
262 Wrench operator*(const Wrench& lhs,double rhs)
263 {
264  return Wrench(lhs.force*rhs,lhs.torque*rhs);
265 }
266 
267 Wrench operator*(double lhs,const Wrench& rhs)
268 {
269  return Wrench(lhs*rhs.force,lhs*rhs.torque);
270 }
271 
272 Wrench operator/(const Wrench& lhs,double rhs)
273 {
274  return Wrench(lhs.force/rhs,lhs.torque/rhs);
275 }
276 
277 // addition of Wrench's
278 Wrench operator+(const Wrench& lhs,const Wrench& rhs)
279 {
280  return Wrench(lhs.force+rhs.force,lhs.torque+rhs.torque);
281 }
282 
283 Wrench operator-(const Wrench& lhs,const Wrench& rhs)
284 {
285  return Wrench(lhs.force-rhs.force,lhs.torque-rhs.torque);
286 }
287 
288 // unary -
289 Wrench operator-(const Wrench& arg)
290 {
291  return Wrench(-arg.force,-arg.torque);
292 }
293 
294 Twist Frame::operator * (const Twist& arg) const
295 // Complexity : 24M+18A
296 {
297  Twist tmp;
298  tmp.rot = M*arg.rot;
299  tmp.vel = M*arg.vel+p*tmp.rot;
300  return tmp;
301 }
302 Twist Frame::Inverse(const Twist& arg) const
303 {
304  Twist tmp;
305  tmp.rot = M.Inverse(arg.rot);
306  tmp.vel = M.Inverse(arg.vel-p*arg.rot);
307  return tmp;
308 }
309 
310 Twist Twist::Zero()
311 {
312  return Twist(Vector::Zero(),Vector::Zero());
313 }
314 
315 
316 void Twist::ReverseSign()
317 {
318  vel.ReverseSign();
319  rot.ReverseSign();
320 }
321 
322 Twist Twist::RefPoint(const Vector& v_base_AB) const
323  // Changes the reference point of the twist.
324  // The vector v_base_AB is expressed in the same base as the twist
325  // The vector v_base_AB is a vector from the old point to
326  // the new point.
327  // Complexity : 6M+6A
328 {
329  return Twist(this->vel+this->rot*v_base_AB,this->rot);
330 }
331 
333 {
334  vel-=arg.vel;
335  rot -=arg.rot;
336  return *this;
337 }
338 
340 {
341  vel+=arg.vel;
342  rot +=arg.rot;
343  return *this;
344 }
345 
346 double& Twist::operator()(int i)
347 {
348  // assert((0<=i)&&(i<6)); done by underlying routines
349  if (i<3)
350  return vel(i);
351  else
352  return rot(i-3);
353 }
354 
355 double Twist::operator()(int i) const
356 {
357  // assert((0<=i)&&(i<6)); done by underlying routines
358  if (i<3)
359  return vel(i);
360  else
361  return rot(i-3);
362 }
363 
364 
365 Twist operator*(const Twist& lhs,double rhs)
366 {
367  return Twist(lhs.vel*rhs,lhs.rot*rhs);
368 }
369 
370 Twist operator*(double lhs,const Twist& rhs)
371 {
372  return Twist(lhs*rhs.vel,lhs*rhs.rot);
373 }
374 
375 Twist operator/(const Twist& lhs,double rhs)
376 {
377  return Twist(lhs.vel/rhs,lhs.rot/rhs);
378 }
379 
380 // addition of Twist's
381 Twist operator+(const Twist& lhs,const Twist& rhs)
382 {
383  return Twist(lhs.vel+rhs.vel,lhs.rot+rhs.rot);
384 }
385 
386 Twist operator-(const Twist& lhs,const Twist& rhs)
387 {
388  return Twist(lhs.vel-rhs.vel,lhs.rot-rhs.rot);
389 }
390 
391 // unary -
392 Twist operator-(const Twist& arg)
393 {
394  return Twist(-arg.vel,-arg.rot);
395 }
396 
398 {
399  M=R;
400  p=Vector::Zero();
401 }
402 
404 {
405  M = Rotation::Identity();
406  p = V;
407 }
408 
409 Frame::Frame(const Rotation & R, const Vector & V)
410 {
411  M = R;
412  p = V;
413 }
414 
415  Frame operator *(const Frame& lhs,const Frame& rhs)
416 // Complexity : 36M+36A
417 {
418  return Frame(lhs.M*rhs.M,lhs.M*rhs.p+lhs.p);
419 }
420 
421 Vector Frame::operator *(const Vector & arg) const
422 {
423  return M*arg+p;
424 }
425 
426 Vector Frame::Inverse(const Vector& arg) const
427 {
428  return M.Inverse(arg-p);
429 }
430 
431 Frame Frame::Inverse() const
432 {
433  return Frame(M.Inverse(),-M.Inverse(p));
434 }
435 
436 
438 {
439  M = arg.M;
440  p = arg.p;
441  return *this;
442 }
443 
444 Frame::Frame(const Frame & arg) :
445  p(arg.p),M(arg.M)
446 {}
447 
448 
450 {
451  data[0] = -data[0];
452  data[1] = -data[1];
453  data[2] = -data[2];
454 }
455 
456 
457 
458 Vector operator-(const Vector & arg)
459 {
460  Vector tmp;
461  tmp.data[0]=-arg.data[0];
462  tmp.data[1]=-arg.data[1];
463  tmp.data[2]=-arg.data[2];
464  return tmp;
465 }
466 
468 // a 3D vector where the 2D vector v is put in the XY plane
469 {
470  data[0]=v(0);
471  data[1]=v(1);
472  data[2]=0;
473 
474 }
476 // a 3D vector where the 2D vector v is put in the YZ plane
477 {
478  data[1]=v(0);
479  data[2]=v(1);
480  data[0]=0;
481 
482 }
483 
485 // a 3D vector where the 2D vector v is put in the ZX plane
486 {
487  data[2]=v(0);
488  data[0]=v(1);
489  data[1]=0;
490 
491 }
492 
493 
494 
495 
496 
497 double& Rotation::operator()(int i,int j) {
498  FRAMES_CHECKI((0<=i)&&(i<=2)&&(0<=j)&&(j<=2));
499  return data[i*3+j];
500 }
501 
502 double Rotation::operator()(int i,int j) const {
503  FRAMES_CHECKI((0<=i)&&(i<=2)&&(0<=j)&&(j<=2));
504  return data[i*3+j];
505 }
506 
507 Rotation::Rotation( double Xx,double Yx,double Zx,
508  double Xy,double Yy,double Zy,
509  double Xz,double Yz,double Zz)
510 {
511  data[0] = Xx;data[1]=Yx;data[2]=Zx;
512  data[3] = Xy;data[4]=Yy;data[5]=Zy;
513  data[6] = Xz;data[7]=Yz;data[8]=Zz;
514 }
515 
516 
517 Rotation::Rotation(const Vector& x,const Vector& y,const Vector& z)
518 {
519  data[0] = x.data[0];data[3] = x.data[1];data[6] = x.data[2];
520  data[1] = y.data[0];data[4] = y.data[1];data[7] = y.data[2];
521  data[2] = z.data[0];data[5] = z.data[1];data[8] = z.data[2];
522 }
523 
525  int count=9;
526  while (count--) data[count] = arg.data[count];
527  return *this;
528 }
529 
531 // Complexity : 9M+6A
532  return Vector(
533  data[0]*v.data[0] + data[1]*v.data[1] + data[2]*v.data[2],
534  data[3]*v.data[0] + data[4]*v.data[1] + data[5]*v.data[2],
535  data[6]*v.data[0] + data[7]*v.data[1] + data[8]*v.data[2]
536  );
537 }
538 
540  // Transformation of the base to which the twist is expressed.
541  // look at Frame*Twist for a transformation that also transforms
542  // the velocity reference point.
543  // Complexity : 18M+12A
544 {
545  return Twist((*this)*arg.vel,(*this)*arg.rot);
546 }
547 
549  // Transformation of the base to which the wrench is expressed.
550  // look at Frame*Twist for a transformation that also transforms
551  // the force reference point.
552 {
553  return Wrench((*this)*arg.force,(*this)*arg.torque);
554 }
555 
557  return Rotation(1,0,0,0,1,0,0,0,1);
558 }
559 // *this = *this * ROT(X,angle)
561 {
562  double cs = cos(angle);
563  double sn = sin(angle);
564  double x1,x2,x3;
565  x1 = cs* (*this)(0,1) + sn* (*this)(0,2);
566  x2 = cs* (*this)(1,1) + sn* (*this)(1,2);
567  x3 = cs* (*this)(2,1) + sn* (*this)(2,2);
568  (*this)(0,2) = -sn* (*this)(0,1) + cs* (*this)(0,2);
569  (*this)(1,2) = -sn* (*this)(1,1) + cs* (*this)(1,2);
570  (*this)(2,2) = -sn* (*this)(2,1) + cs* (*this)(2,2);
571  (*this)(0,1) = x1;
572  (*this)(1,1) = x2;
573  (*this)(2,1) = x3;
574 }
575 
577 {
578  double cs = cos(angle);
579  double sn = sin(angle);
580  double x1,x2,x3;
581  x1 = cs* (*this)(0,0) - sn* (*this)(0,2);
582  x2 = cs* (*this)(1,0) - sn* (*this)(1,2);
583  x3 = cs* (*this)(2,0) - sn* (*this)(2,2);
584  (*this)(0,2) = sn* (*this)(0,0) + cs* (*this)(0,2);
585  (*this)(1,2) = sn* (*this)(1,0) + cs* (*this)(1,2);
586  (*this)(2,2) = sn* (*this)(2,0) + cs* (*this)(2,2);
587  (*this)(0,0) = x1;
588  (*this)(1,0) = x2;
589  (*this)(2,0) = x3;
590 }
591 
593 {
594  double cs = cos(angle);
595  double sn = sin(angle);
596  double x1,x2,x3;
597  x1 = cs* (*this)(0,0) + sn* (*this)(0,1);
598  x2 = cs* (*this)(1,0) + sn* (*this)(1,1);
599  x3 = cs* (*this)(2,0) + sn* (*this)(2,1);
600  (*this)(0,1) = -sn* (*this)(0,0) + cs* (*this)(0,1);
601  (*this)(1,1) = -sn* (*this)(1,0) + cs* (*this)(1,1);
602  (*this)(2,1) = -sn* (*this)(2,0) + cs* (*this)(2,1);
603  (*this)(0,0) = x1;
604  (*this)(1,0) = x2;
605  (*this)(2,0) = x3;
606 }
607 
608 
610  double cs=cos(angle);
611  double sn=sin(angle);
612  return Rotation(1,0,0,0,cs,-sn,0,sn,cs);
613 }
615  double cs=cos(angle);
616  double sn=sin(angle);
617  return Rotation(cs,0,sn,0,1,0,-sn,0,cs);
618 }
620  double cs=cos(angle);
621  double sn=sin(angle);
622  return Rotation(cs,-sn,0,sn,cs,0,0,0,1);
623 }
624 
625 
626 
627 
628 void Frame::Integrate(const Twist& t_this,double samplefrequency)
629 {
630  double n = t_this.rot.Norm()/samplefrequency;
631  if (n<epsilon) {
632  p += M*(t_this.vel/samplefrequency);
633  } else {
634  (*this) = (*this) *
635  Frame ( Rotation::Rot( t_this.rot, n ),
636  t_this.vel/samplefrequency
637  );
638  }
639 }
640 
642 {
643  Rotation tmp(*this);
644  tmp.SetInverse();
645  return tmp;
646 }
647 
649  return Vector(
650  data[0]*v.data[0] + data[3]*v.data[1] + data[6]*v.data[2],
651  data[1]*v.data[0] + data[4]*v.data[1] + data[7]*v.data[2],
652  data[2]*v.data[0] + data[5]*v.data[1] + data[8]*v.data[2]
653  );
654 }
655 
656 void Rotation::setValue(float* oglmat)
657 {
658  data[0] = *oglmat++; data[3] = *oglmat++; data[6] = *oglmat++; oglmat++;
659  data[1] = *oglmat++; data[4] = *oglmat++; data[7] = *oglmat++; oglmat++;
660  data[2] = *oglmat++; data[5] = *oglmat++; data[8] = *oglmat;
661  Ortho();
662 }
663 
664 void Rotation::getValue(float* oglmat) const
665 {
666  *oglmat++ = (float)data[0]; *oglmat++ = (float)data[3]; *oglmat++ = (float)data[6]; *oglmat++ = 0.f;
667  *oglmat++ = (float)data[1]; *oglmat++ = (float)data[4]; *oglmat++ = (float)data[7]; *oglmat++ = 0.f;
668  *oglmat++ = (float)data[2]; *oglmat++ = (float)data[5]; *oglmat++ = (float)data[8]; *oglmat++ = 0.f;
669  *oglmat++ = 0.f; *oglmat++ = 0.f; *oglmat++ = 0.f; *oglmat = 1.f;
670 }
671 
673 {
674  double tmp;
675  tmp = data[1];data[1]=data[3];data[3]=tmp;
676  tmp = data[2];data[2]=data[6];data[6]=tmp;
677  tmp = data[5];data[5]=data[7];data[7]=tmp;
678 }
679 
680 
681 
682 
683 
684 
685 
686 double Frame::operator()(int i,int j) {
687  FRAMES_CHECKI((0<=i)&&(i<=3)&&(0<=j)&&(j<=3));
688  if (i==3) {
689  if (j==3)
690  return 1.0;
691  else
692  return 0.0;
693  } else {
694  if (j==3)
695  return p(i);
696  else
697  return M(i,j);
698 
699  }
700 }
701 
702 double Frame::operator()(int i,int j) const {
703  FRAMES_CHECKI((0<=i)&&(i<=3)&&(0<=j)&&(j<=3));
704  if (i==3) {
705  if (j==3)
706  return 1;
707  else
708  return 0;
709  } else {
710  if (j==3)
711  return p(i);
712  else
713  return M(i,j);
714 
715  }
716 }
717 
718 
721 }
722 
723 
724 void Frame::setValue(float* oglmat)
725 {
726  M.setValue(oglmat);
727  p.data[0] = oglmat[12];
728  p.data[1] = oglmat[13];
729  p.data[2] = oglmat[14];
730 }
731 
732 void Frame::getValue(float* oglmat) const
733 {
734  M.getValue(oglmat);
735  oglmat[12] = (float)p.data[0];
736  oglmat[13] = (float)p.data[1];
737  oglmat[14] = (float)p.data[2];
738 }
739 
740 void Vector::Set2DPlane(const Frame& F_someframe_XY,const Vector2& v_XY)
741 // a 3D vector where the 2D vector v is put in the XY plane of the frame
742 // F_someframe_XY.
743 {
744 Vector tmp_XY;
745 tmp_XY.Set2DXY(v_XY);
746 tmp_XY = F_someframe_XY*(tmp_XY);
747 }
748 
749 
750 
751 
752 
753 
754 
755 
756 
757 //============ 2 dimensional version of the frames objects =============
759 {
760  data[0] = arg.data[0];
761  data[1] = arg.data[1];
762 }
763 
764 IMETHOD Vector2::Vector2(double x,double y)
765 {
766  data[0]=x;data[1]=y;
767 }
768 
769 IMETHOD Vector2::Vector2(double* xy)
770 {
771  data[0]=xy[0];data[1]=xy[1];
772 }
773 
775 {
776  data[0]=xy[0];data[1]=xy[1];
777 }
778 
780 {
781  data[0] = arg.data[0];
782  data[1] = arg.data[1];
783  return *this;
784 }
785 
786 IMETHOD void Vector2::GetValue(double* xy) const
787 {
788  xy[0]=data[0];xy[1]=data[1];
789 }
790 
791 IMETHOD Vector2 operator +(const Vector2 & lhs,const Vector2& rhs)
792 {
793  return Vector2(lhs.data[0]+rhs.data[0],lhs.data[1]+rhs.data[1]);
794 }
795 
796 IMETHOD Vector2 operator -(const Vector2 & lhs,const Vector2& rhs)
797 {
798  return Vector2(lhs.data[0]-rhs.data[0],lhs.data[1]-rhs.data[1]);
799 }
800 
801 IMETHOD Vector2 operator *(const Vector2& lhs,double rhs)
802 {
803  return Vector2(lhs.data[0]*rhs,lhs.data[1]*rhs);
804 }
805 
806 IMETHOD Vector2 operator *(double lhs,const Vector2& rhs)
807 {
808  return Vector2(lhs*rhs.data[0],lhs*rhs.data[1]);
809 }
810 
811 IMETHOD Vector2 operator /(const Vector2& lhs,double rhs)
812 {
813  return Vector2(lhs.data[0]/rhs,lhs.data[1]/rhs);
814 }
815 
817 {
818  data[0]+=arg.data[0];
819  data[1]+=arg.data[1];
820  return *this;
821 }
822 
824 {
825  data[0]-=arg.data[0];
826  data[1]-=arg.data[1];
827  return *this;
828 }
829 
831  return Vector2(0,0);
832 }
833 
834 IMETHOD double Vector2::operator()(int index) const {
835  FRAMES_CHECKI((0<=index)&&(index<=1));
836  return data[index];
837 }
838 
839 IMETHOD double& Vector2::operator () (int index)
840 {
841  FRAMES_CHECKI((0<=index)&&(index<=1));
842  return data[index];
843 }
845 {
846  data[0] = -data[0];
847  data[1] = -data[1];
848 }
849 
850 
852 {
853  return Vector2(-arg.data[0],-arg.data[1]);
854 }
855 
856 
858 // projects v in its XY plane, and sets *this to these values
859 {
860  data[0]=v(0);
861  data[1]=v(1);
862 }
864 // projects v in its XY plane, and sets *this to these values
865 {
866  data[0]=v(1);
867  data[1]=v(2);
868 }
870 // projects v in its XY plane, and sets *this to these values
871 {
872  data[0]=v(2);
873  data[1]=v(0);
874 }
875 
876 IMETHOD void Vector2::Set3DPlane(const Frame& F_someframe_XY,const Vector& v_someframe)
877 // projects v in the XY plane of F_someframe_XY, and sets *this to these values
878 // expressed wrt someframe.
879 {
880  Vector tmp = F_someframe_XY.Inverse(v_someframe);
881  data[0]=tmp(0);
882  data[1]=tmp(1);
883 }
884 
885 
886 
888  c=arg.c;s=arg.s;
889  return *this;
890 }
891 
893  return Vector2(v.data[0]*c-v.data[1]*s,v.data[0]*s+v.data[1]*c);
894 }
895 
896 IMETHOD double Rotation2::operator()(int i,int j) const {
897  FRAMES_CHECKI((0<=i)&&(i<=1)&&(0<=j)&&(j<=1));
898  if (i==j) return c;
899  if (i==0)
900  return s;
901  else
902  return -s;
903 }
904 
905 
906 IMETHOD Rotation2 operator *(const Rotation2& lhs,const Rotation2& rhs) {
907  return Rotation2(lhs.c*rhs.c-lhs.s*rhs.s,lhs.s*rhs.c+lhs.c*rhs.s);
908 }
909 
911  s=-s;
912 }
913 
915  return Rotation2(c,-s);
916 }
917 
919  return Vector2(v.data[0]*c+v.data[1]*s,-v.data[0]*s+v.data[1]*c);
920 }
921 
923  return Rotation2(1,0);
924 }
925 
927 {
928  c = 1;
929  s = 0;
930 }
931 
933  c=cos(angle);s=sin(angle);
934 }
935 
937  return Rotation2(cos(angle),sin(angle));
938 }
939 
940 IMETHOD double Rotation2::GetRot() const {
941  return atan2(s,c);
942 }
943 
944 
946 }
947 
949 {
950  M=R;
951  p=Vector2::Zero();
952 }
953 
955 {
957  p = V;
958 }
959 
961 {
962  M = R;
963  p = V;
964 }
965 
966 IMETHOD Frame2 operator *(const Frame2& lhs,const Frame2& rhs)
967 {
968  return Frame2(lhs.M*rhs.M,lhs.M*rhs.p+lhs.p);
969 }
970 
972 {
973  return M*arg+p;
974 }
975 
977 {
978  return M.Inverse(arg-p);
979 }
980 
982 {
983  M.SetIdentity();
984  p = Vector2::Zero();
985 }
986 
988 {
989  M.SetInverse();
990  p = M*p;
991  p.ReverseSign();
992 }
993 
994 
996 {
997  Frame2 tmp(*this);
998  tmp.SetInverse();
999  return tmp;
1000 }
1001 
1003 {
1004  M = arg.M;
1005  p = arg.p;
1006  return *this;
1007 }
1008 
1010  p(arg.p), M(arg.M)
1011 {}
1012 
1013 
1014 IMETHOD double Frame2::operator()(int i,int j) {
1015  FRAMES_CHECKI((0<=i)&&(i<=2)&&(0<=j)&&(j<=2));
1016  if (i==2) {
1017  if (j==2)
1018  return 1;
1019  else
1020  return 0;
1021  } else {
1022  if (j==2)
1023  return p(i);
1024  else
1025  return M(i,j);
1026 
1027  }
1028 }
1029 
1030 IMETHOD double Frame2::operator()(int i,int j) const {
1031  FRAMES_CHECKI((0<=i)&&(i<=2)&&(0<=j)&&(j<=2));
1032  if (i==2) {
1033  if (j==2)
1034  return 1;
1035  else
1036  return 0;
1037  } else {
1038  if (j==2)
1039  return p(i);
1040  else
1041  return M(i,j);
1042 
1043  }
1044 }
1045 
1046 // Scalar products.
1047 
1048 IMETHOD double dot(const Vector& lhs,const Vector& rhs) {
1049  return rhs(0)*lhs(0)+rhs(1)*lhs(1)+rhs(2)*lhs(2);
1050 }
1051 
1052 IMETHOD double dot(const Twist& lhs,const Wrench& rhs) {
1053  return dot(lhs.vel,rhs.force)+dot(lhs.rot,rhs.torque);
1054 }
1055 
1056 IMETHOD double dot(const Wrench& rhs,const Twist& lhs) {
1057  return dot(lhs.vel,rhs.force)+dot(lhs.rot,rhs.torque);
1058 }
1059 
1060 
1061 
1062 
1063 
1064 // Equality operators
1065 
1066 
1067 
1068 IMETHOD bool Equal(const Vector& a,const Vector& b,double eps) {
1069  return (Equal(a.data[0],b.data[0],eps)&&
1070  Equal(a.data[1],b.data[1],eps)&&
1071  Equal(a.data[2],b.data[2],eps) );
1072  }
1073 
1074 
1075 IMETHOD bool Equal(const Frame& a,const Frame& b,double eps) {
1076  return (Equal(a.p,b.p,eps)&&
1077  Equal(a.M,b.M,eps) );
1078 }
1079 
1080 IMETHOD bool Equal(const Wrench& a,const Wrench& b,double eps) {
1081  return (Equal(a.force,b.force,eps)&&
1082  Equal(a.torque,b.torque,eps) );
1083 }
1084 
1085 IMETHOD bool Equal(const Twist& a,const Twist& b,double eps) {
1086  return (Equal(a.rot,b.rot,eps)&&
1087  Equal(a.vel,b.vel,eps) );
1088 }
1089 
1090 IMETHOD bool Equal(const Vector2& a,const Vector2& b,double eps) {
1091  return (Equal(a.data[0],b.data[0],eps)&&
1092  Equal(a.data[1],b.data[1],eps) );
1093  }
1094 
1095 IMETHOD bool Equal(const Rotation2& a,const Rotation2& b,double eps) {
1096  return ( Equal(a.c,b.c,eps) && Equal(a.s,b.s,eps) );
1097 }
1098 
1099 IMETHOD bool Equal(const Frame2& a,const Frame2& b,double eps) {
1100  return (Equal(a.p,b.p,eps)&&
1101  Equal(a.M,b.M,eps) );
1102 }
1103 
1104 IMETHOD void SetToZero(Vector& v) {
1105  v=Vector::Zero();
1106 }
1107 IMETHOD void SetToZero(Twist& v) {
1108  SetToZero(v.rot);
1109  SetToZero(v.vel);
1110 }
1111 IMETHOD void SetToZero(Wrench& v) {
1112  SetToZero(v.force);
1113  SetToZero(v.torque);
1114 }
1115 
1117  v = Vector2::Zero();
1118 }
1119 
1120 
1122 // The following defines the operations
1123 // diff
1124 // addDelta
1125 // random
1126 // posrandom
1127 // on all the types defined in this library.
1128 // (mostly for uniform integration, differentiation and testing).
1129 // Defined as functions because double is not a class and a method
1130 // would brake uniformity when defined for a double.
1132 
1133 
1134 
1135 
1136 
1137 
1143 IMETHOD Rotation Rot(const Vector& axis_a_b) {
1144  // The formula is
1145  // V.(V.tr) + st*[V x] + ct*(I-V.(V.tr))
1146  // can be found by multiplying it with an arbitrary vector p
1147  // and noting that this vector is rotated.
1148  Vector rotvec = axis_a_b;
1149  double angle = rotvec.Normalize(1E-10);
1150  double ct = ::cos(angle);
1151  double st = ::sin(angle);
1152  double vt = 1-ct;
1153  return Rotation(
1154  ct + vt*rotvec(0)*rotvec(0),
1155  -rotvec(2)*st + vt*rotvec(0)*rotvec(1),
1156  rotvec(1)*st + vt*rotvec(0)*rotvec(2),
1157  rotvec(2)*st + vt*rotvec(1)*rotvec(0),
1158  ct + vt*rotvec(1)*rotvec(1),
1159  -rotvec(0)*st + vt*rotvec(1)*rotvec(2),
1160  -rotvec(1)*st + vt*rotvec(2)*rotvec(0),
1161  rotvec(0)*st + vt*rotvec(2)*rotvec(1),
1162  ct + vt*rotvec(2)*rotvec(2)
1163  );
1164  }
1165 
1166 IMETHOD Vector diff(const Vector& a,const Vector& b,double dt) {
1167  return (b-a)/dt;
1168 }
1169 
1202 IMETHOD Vector diff(const Rotation& R_a_b1,const Rotation& R_a_b2,double dt) {
1203  Rotation R_b1_b2(R_a_b1.Inverse()*R_a_b2);
1204  return R_a_b1 * R_b1_b2.GetRot() / dt;
1205 }
1206 IMETHOD Twist diff(const Frame& F_a_b1,const Frame& F_a_b2,double dt) {
1207  return Twist(
1208  diff(F_a_b1.p,F_a_b2.p,dt),
1209  diff(F_a_b1.M,F_a_b2.M,dt)
1210  );
1211 }
1212 IMETHOD Twist diff(const Twist& a,const Twist& b,double dt) {
1213  return Twist(diff(a.vel,b.vel,dt),diff(a.rot,b.rot,dt));
1214 }
1215 
1216 IMETHOD Wrench diff(const Wrench& a,const Wrench& b,double dt) {
1217  return Wrench(
1218  diff(a.force,b.force,dt),
1219  diff(a.torque,b.torque,dt)
1220  );
1221 }
1222 
1223 
1224 IMETHOD Vector addDelta(const Vector& a,const Vector&da,double dt) {
1225  return a+da*dt;
1226 }
1227 
1228 IMETHOD Rotation addDelta(const Rotation& a,const Vector&da,double dt) {
1229  return a*Rot(a.Inverse(da)*dt);
1230 }
1231 IMETHOD Frame addDelta(const Frame& a,const Twist& da,double dt) {
1232  return Frame(
1233  addDelta(a.M,da.rot,dt),
1234  addDelta(a.p,da.vel,dt)
1235  );
1236 }
1237 IMETHOD Twist addDelta(const Twist& a,const Twist&da,double dt) {
1238  return Twist(addDelta(a.vel,da.vel,dt),addDelta(a.rot,da.rot,dt));
1239 }
1240 IMETHOD Wrench addDelta(const Wrench& a,const Wrench&da,double dt) {
1241  return Wrench(addDelta(a.force,da.force,dt),addDelta(a.torque,da.torque,dt));
1242 }
1243 
1244 
1282 IMETHOD void random(Vector& a) {
1283  random(a[0]);
1284  random(a[1]);
1285  random(a[2]);
1286 }
1287 IMETHOD void random(Twist& a) {
1288  random(a.rot);
1289  random(a.vel);
1290 }
1291 IMETHOD void random(Wrench& a) {
1292  random(a.torque);
1293  random(a.force);
1294 }
1295 
1296 IMETHOD void random(Rotation& R) {
1297  double alfa;
1298  double beta;
1299  double gamma;
1300  random(alfa);
1301  random(beta);
1302  random(gamma);
1303  R = Rotation::EulerZYX(alfa,beta,gamma);
1304 }
1305 
1307  random(F.M);
1308  random(F.p);
1309 }
1310 
1311 IMETHOD void posrandom(Vector& a) {
1312  posrandom(a[0]);
1313  posrandom(a[1]);
1314  posrandom(a[2]);
1315 }
1316 IMETHOD void posrandom(Twist& a) {
1317  posrandom(a.rot);
1318  posrandom(a.vel);
1319 }
1320 IMETHOD void posrandom(Wrench& a) {
1321  posrandom(a.torque);
1322  posrandom(a.force);
1323 }
1324 
1325 IMETHOD void posrandom(Rotation& R) {
1326  double alfa;
1327  double beta;
1328  double gamma;
1329  posrandom(alfa);
1330  posrandom(beta);
1331  posrandom(gamma);
1332  R = Rotation::EulerZYX(alfa,beta,gamma);
1333 }
1334 
1336  random(F.M);
1337  random(F.p);
1338 }
1339 
1340 
1341 
1342 
1343 IMETHOD bool operator==(const Frame& a,const Frame& b ) {
1344 #ifdef KDL_USE_EQUAL
1345  return Equal(a,b);
1346 #else
1347  return (a.p == b.p &&
1348  a.M == b.M );
1349 #endif
1350 }
1351 
1352 IMETHOD bool operator!=(const Frame& a,const Frame& b) {
1353  return !operator==(a,b);
1354 }
1355 
1356 IMETHOD bool operator==(const Vector& a,const Vector& b) {
1357 #ifdef KDL_USE_EQUAL
1358  return Equal(a,b);
1359 #else
1360  return (a.data[0]==b.data[0]&&
1361  a.data[1]==b.data[1]&&
1362  a.data[2]==b.data[2] );
1363 #endif
1364  }
1365 
1366 IMETHOD bool operator!=(const Vector& a,const Vector& b) {
1367  return !operator==(a,b);
1368 }
1369 
1370 IMETHOD bool operator==(const Twist& a,const Twist& b) {
1371 #ifdef KDL_USE_EQUAL
1372  return Equal(a,b);
1373 #else
1374  return (a.rot==b.rot &&
1375  a.vel==b.vel );
1376 #endif
1377 }
1378 
1379 IMETHOD bool operator!=(const Twist& a,const Twist& b) {
1380  return !operator==(a,b);
1381 }
1382 
1383 IMETHOD bool operator==(const Wrench& a,const Wrench& b ) {
1384 #ifdef KDL_USE_EQUAL
1385  return Equal(a,b);
1386 #else
1387  return (a.force==b.force &&
1388  a.torque==b.torque );
1389 #endif
1390 }
1391 
1392 IMETHOD bool operator!=(const Wrench& a,const Wrench& b) {
1393  return !operator==(a,b);
1394 }
1395 IMETHOD bool operator!=(const Rotation& a,const Rotation& b) {
1396  return !operator==(a,b);
1397 }
1398 
typedef float(TangentPoint)[2]
_GL_VOID GLfloat value _GL_VOID_RET _GL_VOID const GLuint GLboolean *residences _GL_BOOL_RET _GL_VOID GLsizei GLfloat GLfloat GLfloat GLfloat const GLubyte *bitmap _GL_VOID_RET _GL_VOID GLenum const void *lists _GL_VOID_RET _GL_VOID const GLdouble *equation _GL_VOID_RET _GL_VOID GLdouble GLdouble blue _GL_VOID_RET _GL_VOID GLfloat GLfloat blue _GL_VOID_RET _GL_VOID GLint GLint blue _GL_VOID_RET _GL_VOID GLshort GLshort blue _GL_VOID_RET _GL_VOID GLubyte GLubyte blue _GL_VOID_RET _GL_VOID GLuint GLuint blue _GL_VOID_RET _GL_VOID GLushort GLushort blue _GL_VOID_RET _GL_VOID GLbyte GLbyte GLbyte alpha _GL_VOID_RET _GL_VOID GLdouble GLdouble GLdouble alpha _GL_VOID_RET _GL_VOID GLfloat GLfloat GLfloat alpha _GL_VOID_RET _GL_VOID GLint GLint GLint alpha _GL_VOID_RET _GL_VOID GLshort GLshort GLshort alpha _GL_VOID_RET _GL_VOID GLubyte GLubyte GLubyte alpha _GL_VOID_RET _GL_VOID GLuint GLuint GLuint alpha _GL_VOID_RET _GL_VOID GLushort GLushort GLushort alpha _GL_VOID_RET _GL_VOID GLenum mode _GL_VOID_RET _GL_VOID GLint GLsizei GLsizei GLenum type _GL_VOID_RET _GL_VOID GLsizei GLenum GLenum const void *pixels _GL_VOID_RET _GL_VOID const void *pointer _GL_VOID_RET _GL_VOID GLdouble v _GL_VOID_RET _GL_VOID GLfloat v _GL_VOID_RET _GL_VOID GLint GLint i2 _GL_VOID_RET _GL_VOID GLint j _GL_VOID_RET _GL_VOID GLfloat param _GL_VOID_RET _GL_VOID GLint param _GL_VOID_RET _GL_VOID GLdouble GLdouble GLdouble GLdouble GLdouble zFar _GL_VOID_RET _GL_UINT GLdouble *equation _GL_VOID_RET _GL_VOID GLenum GLint *params _GL_VOID_RET _GL_VOID GLenum GLfloat *v _GL_VOID_RET _GL_VOID GLenum GLfloat *params _GL_VOID_RET _GL_VOID GLfloat *values _GL_VOID_RET _GL_VOID GLushort *values _GL_VOID_RET _GL_VOID GLenum GLfloat *params _GL_VOID_RET _GL_VOID GLenum GLdouble *params _GL_VOID_RET _GL_VOID GLenum GLint *params _GL_VOID_RET _GL_VOID GLsizei const void *pointer _GL_VOID_RET _GL_VOID GLsizei const void *pointer _GL_VOID_RET _GL_BOOL GLfloat param _GL_VOID_RET _GL_VOID GLint param _GL_VOID_RET _GL_VOID GLenum GLfloat param _GL_VOID_RET _GL_VOID GLenum GLint param _GL_VOID_RET _GL_VOID GLushort pattern _GL_VOID_RET _GL_VOID GLdouble GLdouble GLint GLint const GLdouble *points _GL_VOID_RET _GL_VOID GLdouble GLdouble GLint GLint GLdouble GLdouble GLint GLint const GLdouble *points _GL_VOID_RET _GL_VOID GLdouble GLdouble u2 _GL_VOID_RET _GL_VOID GLdouble GLdouble GLint GLdouble GLdouble v2 _GL_VOID_RET _GL_VOID GLenum GLfloat param _GL_VOID_RET _GL_VOID GLenum GLint param _GL_VOID_RET _GL_VOID GLenum mode _GL_VOID_RET _GL_VOID GLdouble GLdouble nz _GL_VOID_RET _GL_VOID GLfloat GLfloat nz _GL_VOID_RET _GL_VOID GLint GLint nz _GL_VOID_RET _GL_VOID GLshort GLshort nz _GL_VOID_RET _GL_VOID GLsizei const void *pointer _GL_VOID_RET _GL_VOID GLsizei const GLfloat *values _GL_VOID_RET _GL_VOID GLsizei const GLushort *values _GL_VOID_RET _GL_VOID GLint param _GL_VOID_RET _GL_VOID const GLuint const GLclampf *priorities _GL_VOID_RET _GL_VOID GLdouble y _GL_VOID_RET _GL_VOID GLfloat y _GL_VOID_RET _GL_VOID GLint y _GL_VOID_RET _GL_VOID GLshort y _GL_VOID_RET _GL_VOID GLdouble GLdouble z _GL_VOID_RET _GL_VOID GLfloat GLfloat z _GL_VOID_RET _GL_VOID GLint GLint z _GL_VOID_RET _GL_VOID GLshort GLshort z _GL_VOID_RET _GL_VOID GLdouble GLdouble z
_GL_VOID GLfloat value _GL_VOID_RET _GL_VOID const GLuint GLboolean *residences _GL_BOOL_RET _GL_VOID GLsizei GLfloat GLfloat GLfloat GLfloat const GLubyte *bitmap _GL_VOID_RET _GL_VOID GLenum const void *lists _GL_VOID_RET _GL_VOID const GLdouble *equation _GL_VOID_RET _GL_VOID GLdouble GLdouble blue _GL_VOID_RET _GL_VOID GLfloat GLfloat blue _GL_VOID_RET _GL_VOID GLint GLint blue _GL_VOID_RET _GL_VOID GLshort GLshort blue _GL_VOID_RET _GL_VOID GLubyte GLubyte blue _GL_VOID_RET _GL_VOID GLuint GLuint blue _GL_VOID_RET _GL_VOID GLushort GLushort blue _GL_VOID_RET _GL_VOID GLbyte GLbyte GLbyte alpha _GL_VOID_RET _GL_VOID GLdouble GLdouble GLdouble alpha _GL_VOID_RET _GL_VOID GLfloat GLfloat GLfloat alpha _GL_VOID_RET _GL_VOID GLint GLint GLint alpha _GL_VOID_RET _GL_VOID GLshort GLshort GLshort alpha _GL_VOID_RET _GL_VOID GLubyte GLubyte GLubyte alpha _GL_VOID_RET _GL_VOID GLuint GLuint GLuint alpha _GL_VOID_RET _GL_VOID GLushort GLushort GLushort alpha _GL_VOID_RET _GL_VOID GLenum mode _GL_VOID_RET _GL_VOID GLint y
_GL_VOID GLfloat value _GL_VOID_RET _GL_VOID const GLuint GLboolean *residences _GL_BOOL_RET _GL_VOID GLsizei GLfloat GLfloat GLfloat GLfloat const GLubyte *bitmap _GL_VOID_RET _GL_VOID GLenum const void *lists _GL_VOID_RET _GL_VOID const GLdouble *equation _GL_VOID_RET _GL_VOID GLdouble GLdouble blue _GL_VOID_RET _GL_VOID GLfloat GLfloat blue _GL_VOID_RET _GL_VOID GLint GLint blue _GL_VOID_RET _GL_VOID GLshort GLshort blue _GL_VOID_RET _GL_VOID GLubyte GLubyte blue _GL_VOID_RET _GL_VOID GLuint GLuint blue _GL_VOID_RET _GL_VOID GLushort GLushort blue _GL_VOID_RET _GL_VOID GLbyte GLbyte GLbyte alpha _GL_VOID_RET _GL_VOID GLdouble GLdouble GLdouble alpha _GL_VOID_RET _GL_VOID GLfloat GLfloat GLfloat alpha _GL_VOID_RET _GL_VOID GLint GLint GLint alpha _GL_VOID_RET _GL_VOID GLshort GLshort GLshort alpha _GL_VOID_RET _GL_VOID GLubyte GLubyte GLubyte alpha _GL_VOID_RET _GL_VOID GLuint GLuint GLuint alpha _GL_VOID_RET _GL_VOID GLushort GLushort GLushort alpha _GL_VOID_RET _GL_VOID GLenum mode _GL_VOID_RET _GL_VOID GLint GLsizei GLsizei GLenum type _GL_VOID_RET _GL_VOID GLsizei GLenum GLenum const void *pixels _GL_VOID_RET _GL_VOID const void *pointer _GL_VOID_RET _GL_VOID GLdouble v _GL_VOID_RET _GL_VOID GLfloat v _GL_VOID_RET _GL_VOID GLint GLint i2 _GL_VOID_RET _GL_VOID GLint j _GL_VOID_RET _GL_VOID GLfloat param _GL_VOID_RET _GL_VOID GLint param _GL_VOID_RET _GL_VOID GLdouble GLdouble GLdouble GLdouble GLdouble zFar _GL_VOID_RET _GL_UINT GLdouble *equation _GL_VOID_RET _GL_VOID GLenum GLint *params _GL_VOID_RET _GL_VOID GLenum GLfloat *v _GL_VOID_RET _GL_VOID GLenum GLfloat *params _GL_VOID_RET _GL_VOID GLfloat *values _GL_VOID_RET _GL_VOID GLushort *values _GL_VOID_RET _GL_VOID GLenum GLfloat *params _GL_VOID_RET _GL_VOID GLenum GLdouble *params _GL_VOID_RET _GL_VOID GLenum GLint *params _GL_VOID_RET _GL_VOID GLsizei const void *pointer _GL_VOID_RET _GL_VOID GLsizei const void *pointer _GL_VOID_RET _GL_BOOL GLfloat param _GL_VOID_RET _GL_VOID GLint param _GL_VOID_RET _GL_VOID GLenum GLfloat param _GL_VOID_RET _GL_VOID GLenum GLint param _GL_VOID_RET _GL_VOID GLushort pattern _GL_VOID_RET _GL_VOID GLdouble GLdouble GLint GLint const GLdouble *points _GL_VOID_RET _GL_VOID GLdouble GLdouble GLint GLint GLdouble GLdouble GLint GLint const GLdouble *points _GL_VOID_RET _GL_VOID GLdouble GLdouble u2 _GL_VOID_RET _GL_VOID GLdouble GLdouble GLint GLdouble GLdouble v2 _GL_VOID_RET _GL_VOID GLenum GLfloat param _GL_VOID_RET _GL_VOID GLenum GLint param _GL_VOID_RET _GL_VOID GLenum mode _GL_VOID_RET _GL_VOID GLdouble GLdouble nz _GL_VOID_RET _GL_VOID GLfloat GLfloat nz _GL_VOID_RET _GL_VOID GLint GLint nz _GL_VOID_RET _GL_VOID GLshort GLshort nz _GL_VOID_RET _GL_VOID GLsizei const void *pointer _GL_VOID_RET _GL_VOID GLsizei const GLfloat *values _GL_VOID_RET _GL_VOID GLsizei const GLushort *values _GL_VOID_RET _GL_VOID GLint param _GL_VOID_RET _GL_VOID const GLuint const GLclampf *priorities _GL_VOID_RET _GL_VOID GLdouble y _GL_VOID_RET _GL_VOID GLfloat y _GL_VOID_RET _GL_VOID GLint y _GL_VOID_RET _GL_VOID GLshort y _GL_VOID_RET _GL_VOID GLdouble GLdouble z _GL_VOID_RET _GL_VOID GLfloat GLfloat z _GL_VOID_RET _GL_VOID GLint GLint z _GL_VOID_RET _GL_VOID GLshort GLshort z _GL_VOID_RET _GL_VOID GLdouble GLdouble GLdouble w _GL_VOID_RET _GL_VOID GLfloat GLfloat GLfloat w _GL_VOID_RET _GL_VOID GLint GLint GLint w _GL_VOID_RET _GL_VOID GLshort GLshort GLshort w _GL_VOID_RET _GL_VOID GLdouble GLdouble x2
struct Frame Frame
Definition: frames.hpp:261
__forceinline avxi & operator-=(avxi &a, const avxi &b)
Definition: avxi.h:394
__forceinline avxi & operator+=(avxi &a, const avxi &b)
Assignment Operators.
Definition: avxi.h:385
__forceinline avxi & operator*=(avxi &a, const avxi &b)
Definition: avxi.h:403
ATTR_WARN_UNUSED_RESULT const BMLoop * l
ATTR_WARN_UNUSED_RESULT const BMVert * v
btGeneric6DofConstraint & operator=(btGeneric6DofConstraint &other)
SIMD_FORCE_INLINE btVector3 operator()(const btVector3 &x) const
Return the transform of the vector.
Definition: btTransform.h:90
SIMD_FORCE_INLINE btScalar angle(const btVector3 &v) const
Return the angle between this and another vector.
Definition: btVector3.h:356
void SetIdentity()
Definition: frames.inl:981
double operator()(int i, int j)
Definition: frames.inl:1014
Vector2 operator*(const Vector2 &arg)
Definition: frames.inl:971
Frame2 & operator=(const Frame2 &arg)
Definition: frames.inl:1002
Rotation2 M
Orientation of the Frame.
Definition: frames.hpp:1037
Vector2 p
origine of the Frame
Definition: frames.hpp:1036
void SetInverse()
Definition: frames.inl:987
Frame2 Inverse() const
Definition: frames.inl:995
represents a frame transformation in 3D space (rotation + translation)
Definition: frames.hpp:526
static Frame Identity()
Definition: frames.inl:719
Rotation M
Orientation of the Frame.
Definition: frames.hpp:529
void setValue(float *oglmat)
Definition: frames.inl:724
void Integrate(const Twist &t_this, double frequency)
Definition: frames.inl:628
void getValue(float *oglmat) const
Definition: frames.inl:732
Vector p
origine of the Frame
Definition: frames.hpp:528
Frame Inverse() const
Gives back inverse transformation of a Frame.
Definition: frames.inl:431
double operator()(int i, int j)
Definition: frames.inl:686
double GetRot() const
Gets the angle (in radians)
Definition: frames.inl:940
Vector2 operator*(const Vector2 &v) const
Definition: frames.inl:892
double operator()(int i, int j) const
Access to elements 0..1,0..1, bounds are checked when NDEBUG is not set.
Definition: frames.inl:896
static Rotation2 Rot(double angle)
The Rot... static functions give the value of the appropriate rotation matrix bac.
Definition: frames.inl:936
void SetInverse()
Definition: frames.inl:910
Rotation2()
Default constructor does NOT initialise to Zero().
Definition: frames.hpp:996
static Rotation2 Identity()
Definition: frames.inl:922
void SetIdentity()
Definition: frames.inl:926
Rotation2 Inverse() const
Definition: frames.inl:914
void SetRot(double angle)
The SetRot.. functions set the value of *this to the appropriate rotation matrix.
Definition: frames.inl:932
Rotation2 & operator=(const Rotation2 &arg)
Definition: frames.inl:887
represents rotations in 3 dimensional space.
Definition: frames.hpp:299
void DoRotZ(double angle)
Definition: frames.inl:592
Rotation Inverse() const
Gives back the inverse rotation matrix of *this.
Definition: frames.inl:641
Rotation & operator=(const Rotation &arg)
Definition: frames.inl:524
Vector operator*(const Vector &v) const
Definition: frames.inl:530
static Rotation EulerZYX(double Alfa, double Beta, double Gamma)
Definition: frames.hpp:435
static Rotation Identity()
Gives back an identity rotaton matrix.
Definition: frames.inl:556
static Rotation RotX(double angle)
The Rot... static functions give the value of the appropriate rotation matrix back.
Definition: frames.inl:609
void setValue(float *oglmat)
Definition: frames.inl:656
static Rotation Rot(const Vector &rotaxis, double angle)
Definition: frames.cpp:250
void DoRotX(double angle)
Definition: frames.inl:560
double data[9]
Definition: frames.hpp:301
static Rotation RotY(double angle)
The Rot... static functions give the value of the appropriate rotation matrix back.
Definition: frames.inl:614
void getValue(float *oglmat) const
Definition: frames.inl:664
void SetInverse()
Sets the value of *this to its inverse.
Definition: frames.inl:672
void Ortho()
Definition: frames.cpp:169
static Rotation RotZ(double angle)
The Rot... static functions give the value of the appropriate rotation matrix back.
Definition: frames.inl:619
double & operator()(int i, int j)
Access to elements 0..2,0..2, bounds are checked when NDEBUG is not set.
Definition: frames.inl:497
void DoRotY(double angle)
Definition: frames.inl:576
represents both translational and rotational velocities.
Definition: frames.hpp:679
Vector rot
The rotational velocity of that point.
Definition: frames.hpp:682
Vector vel
The velocity of that point.
Definition: frames.hpp:681
2D version of Vector
Definition: frames.hpp:916
static Vector2 Zero()
Definition: frames.inl:830
void ReverseSign()
Definition: frames.inl:844
void Set3DZX(const Vector &v)
projects v in its ZX plane, and sets *this to these values
Definition: frames.inl:869
Vector2 & operator=(const Vector2 &arg)
Definition: frames.inl:779
void GetValue(double *xy) const
store vector components in array
Definition: frames.inl:786
void Set3DXY(const Vector &v)
projects v in its XY plane, and sets *this to these values
Definition: frames.inl:857
Vector2()
Does not initialise to Zero().
Definition: frames.hpp:920
Vector2 & operator+=(const Vector2 &arg)
Definition: frames.inl:816
void Set3DYZ(const Vector &v)
projects v in its YZ plane, and sets *this to these values
Definition: frames.inl:863
Vector2 & operator-=(const Vector2 &arg)
Definition: frames.inl:823
double operator()(int index) const
Access to elements, range checked when NDEBUG is not set, from 0..1.
Definition: frames.inl:834
void Set3DPlane(const Frame &F_someframe_XY, const Vector &v_someframe)
Definition: frames.inl:876
A concrete implementation of a 3 dimensional vector class.
Definition: frames.hpp:143
void Set2DPlane(const Frame &F_someframe_XY, const Vector2 &v_XY)
a 3D vector where the 2D vector v_XY is put in the XY plane of the frame F_someframe_XY.
Definition: frames.inl:740
double Norm() const
Definition: frames.cpp:115
void ReverseSign()
Reverses the sign of the Vector object itself.
Definition: frames.inl:449
void Set2DZX(const Vector2 &v)
a 3D vector where the 2D vector v is put in the ZX plane
Definition: frames.inl:484
double data[3]
Definition: frames.hpp:145
static Vector Zero()
Definition: frames.inl:154
void Set2DXY(const Vector2 &v)
a 3D vector where the 2D vector v is put in the XY plane
Definition: frames.inl:467
void Set2DYZ(const Vector2 &v)
a 3D vector where the 2D vector v is put in the YZ plane
Definition: frames.inl:475
represents both translational and rotational acceleration.
Definition: frames.hpp:835
Vector force
Force that is applied at the origin of the current ref frame.
Definition: frames.hpp:837
Vector torque
Torque that is applied at the origin of the current ref frame.
Definition: frames.hpp:838
#define rot(x, k)
Vector operator/(const Vector &lhs, double rhs)
Definition: frames.inl:109
IMETHOD Vector operator-(const Vector &lhs, const Vector &rhs)
Definition: frames.inl:74
IMETHOD bool operator!=(const Frame &a, const Frame &b)
Definition: frames.inl:1352
IMETHOD double dot(const Vector &lhs, const Vector &rhs)
Definition: frames.inl:1048
IMETHOD Vector operator+(const Vector &lhs, const Vector &rhs)
Definition: frames.inl:65
Vector operator*(const Vector &lhs, double rhs)
Definition: frames.inl:91
IMETHOD Vector Normalize(const Vector &a, double eps)
Definition: frames.inl:170
IMETHOD Rotation Rot(const Vector &axis_a_b)
Definition: frames.inl:1143
int count
#define M
#define F
#define R
static unsigned a[3]
Definition: RandGen.cpp:78
Point2 Vector2
Definition: FitCurve.h:52
IMETHOD Vector diff(const Vector &a, const Vector &b, double dt=1)
bool operator==(const Rotation &a, const Rotation &b)
Definition: frames.cpp:377
INLINE Rall1d< T, V, S > cos(const Rall1d< T, V, S > &arg)
Definition: rall1d.h:319
INLINE Rall1d< T, V, S > operator-(const Rall1d< T, V, S > &lhs, const Rall1d< T, V, S > &rhs)
Definition: rall1d.h:234
IMETHOD void random(doubleVel &F)
Definition: framevel.hpp:44
void SetToZero(Jacobian &jac)
Definition: jacobian.cpp:81
IMETHOD Vector addDelta(const Vector &a, const Vector &da, double dt=1)
Rotation operator*(const Rotation &lhs, const Rotation &rhs)
Definition: frames.cpp:177
IMETHOD void posrandom(doubleVel &F)
Definition: framevel.hpp:48
INLINE Rall1d< T, V, S > operator+(const Rall1d< T, V, S > &lhs, const Rall1d< T, V, S > &rhs)
Definition: rall1d.h:227
INLINE Rall1d< T, V, S > operator/(const Rall1d< T, V, S > &lhs, const Rall1d< T, V, S > &rhs)
Definition: rall1d.h:215
double epsilon
default precision while comparing with Equal(..,..) functions. Initialized at 0.0000001.
Definition: utility.cpp:22
INLINE Rall1d< T, V, S > sin(const Rall1d< T, V, S > &arg)
Definition: rall1d.h:311
IMETHOD bool Equal(const VectorAcc &, const VectorAcc &, double=epsilon)
INLINE Rall1d< T, V, S > atan2(const Rall1d< T, V, S > &y, const Rall1d< T, V, S > &x)
Definition: rall1d.h:429
static const pxr::TfToken st("st", pxr::TfToken::Immortal)
static const pxr::TfToken b("b", pxr::TfToken::Immortal)
const btScalar eps
Definition: poly34.cpp:11
ccl_device_inline float beta(float x, float y)
Definition: util/math.h:775
#define FRAMES_CHECKI(a)
Definition: utility.h:54
#define IMETHOD
Definition: utility.h:43
CCL_NAMESPACE_BEGIN struct Window V
int xy[2]
Definition: wm_draw.c:135