Blender  V3.3
frames.hpp
Go to the documentation of this file.
1 /***************************************************************************
2  frames.hpp `- description
3  -------------------------
4  begin : June 2006
5  copyright : (C) 2006 Erwin Aertbelien
6  email : firstname.lastname@mech.kuleuven.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 
115 #ifndef KDL_FRAMES_H
116 #define KDL_FRAMES_H
117 
118 
119 #include "utilities/kdl-config.h"
120 #include "utilities/utility.h"
121 
123 
124 namespace KDL {
125 
126 
127 
128 class Vector;
129 class Rotation;
130 class Frame;
131 class Wrench;
132 class Twist;
133 class Vector2;
134 class Rotation2;
135 class Frame2;
136 
137 
138 
142 class Vector
143 {
144 public:
145  double data[3];
147  inline Vector() {data[0]=data[1]=data[2] = 0.0;}
148 
150  inline Vector(double x,double y, double z);
151 
153  inline Vector(double* xyz);
154 
156  inline Vector(float* xyz);
157 
159  inline Vector(const Vector& arg);
160 
162  inline void GetValue(double* xyz) const;
163 
165  inline Vector& operator = ( const Vector& arg);
166 
168  inline double operator()(int index) const;
169 
171  inline double& operator() (int index);
172 
174  double operator[] ( int index ) const
175  {
176  return this->operator() ( index );
177  }
178 
180  double& operator[] ( int index )
181  {
182  return this->operator() ( index );
183  }
184 
185  inline double x() const;
186  inline double y() const;
187  inline double z() const;
188  inline void x(double);
189  inline void y(double);
190  inline void z(double);
191 
193  inline void ReverseSign();
194 
195 
197  inline Vector& operator-=(const Vector& arg);
198 
199 
201  inline Vector& operator +=(const Vector& arg);
202 
204  inline Vector& operator *=(double arg);
205 
207  inline friend Vector operator*(const Vector& lhs,double rhs);
209  inline friend Vector operator*(double lhs,const Vector& rhs);
211 
212  inline friend Vector operator/(const Vector& lhs,double rhs);
213  inline friend Vector operator+(const Vector& lhs,const Vector& rhs);
214  inline friend Vector operator-(const Vector& lhs,const Vector& rhs);
215  inline friend Vector operator*(const Vector& lhs,const Vector& rhs);
216  inline friend Vector operator-(const Vector& arg);
217  inline friend double dot(const Vector& lhs,const Vector& rhs);
218 
221  inline friend void SetToZero(Vector& v);
222 
224  inline static Vector Zero();
225 
231  double Normalize(double eps=epsilon);
232 
234  double Norm() const;
235 
236 
237 
239  inline void Set2DXY(const Vector2& v);
241  inline void Set2DYZ(const Vector2& v);
243  inline void Set2DZX(const Vector2& v);
245  inline void Set2DPlane(const Frame& F_someframe_XY,const Vector2& v_XY);
246 
247 
250  inline friend bool Equal(const Vector& a,const Vector& b,double eps);
251 
253  inline friend Vector Normalize(const Vector& a, double eps);
254 
256  inline friend bool operator==(const Vector& a,const Vector& b);
258  inline friend bool operator!=(const Vector& a,const Vector& b);
259 
260  friend class Rotation;
261  friend class Frame;
262 };
263  inline Vector Normalize(const Vector&, double eps=epsilon);
264 
298 class Rotation
299 {
300 public:
301  double data[9];
302 
303  inline Rotation() {
304  *this = Identity();
305  }
306  inline Rotation(double Xx,double Yx,double Zx,
307  double Xy,double Yy,double Zy,
308  double Xz,double Yz,double Zz);
309  inline Rotation(const Vector& x,const Vector& y,const Vector& z);
310  // default copy constructor is sufficient
311 
312  inline void setValue(float* oglmat);
313  inline void getValue(float* oglmat) const;
314 
315  inline Rotation& operator=(const Rotation& arg);
316 
319  inline Vector operator*(const Vector& v) const;
320 
322  inline double& operator()(int i,int j);
323 
325  inline double operator() (int i,int j) const;
326 
327  friend Rotation operator *(const Rotation& lhs,const Rotation& rhs);
328 
330  inline void SetInverse();
331 
333  inline Rotation Inverse() const;
334 
336  inline Vector Inverse(const Vector& v) const;
337 
339  inline Wrench Inverse(const Wrench& arg) const;
340 
342  inline Twist Inverse(const Twist& arg) const;
343 
345  inline static Rotation Identity();
346 
347 
348 // = Rotations
350  inline static Rotation RotX(double angle);
352  inline static Rotation RotY(double angle);
354  inline static Rotation RotZ(double angle);
357  inline void DoRotX(double angle);
360  inline void DoRotY(double angle);
363  inline void DoRotZ(double angle);
364 
368  // @see Rot2 if you want to handle this error in another way.
369  static Rotation Rot(const Vector& rotaxis,double angle);
370 
372  static Rotation Rot2(const Vector& rotvec,double angle);
373 
374  // make sure the matrix is a pure rotation (no scaling)
375  void Ortho();
376 
379  Vector GetRot() const;
380 
383  Vector2 GetXZRot() const;
384 
393  double GetRotAngle(Vector& axis,double eps=epsilon) const;
394 
395 
400  static Rotation EulerZYZ(double Alfa,double Beta,double Gamma);
401 
411  void GetEulerZYZ(double& alfa,double& beta,double& gamma) const;
412 
413 
417  static Rotation RPY(double roll,double pitch,double yaw);
418 
426  void GetRPY(double& roll,double& pitch,double& yaw) const;
427 
428 
435  inline static Rotation EulerZYX(double Alfa,double Beta,double Gamma) {
436  return RPY(Gamma,Beta,Alfa);
437  }
438 
450  inline void GetEulerZYX(double& Alfa,double& Beta,double& Gamma) const {
451  GetRPY(Gamma,Beta,Alfa);
452  }
453 
458  inline Twist operator * (const Twist& arg) const;
459 
464  inline Wrench operator * (const Wrench& arg) const;
465 
467  inline Vector UnitX() const {
468  return Vector(data[0],data[3],data[6]);
469  }
470 
472  inline void UnitX(const Vector& X) {
473  data[0] = X(0);
474  data[3] = X(1);
475  data[6] = X(2);
476  }
477 
479  inline Vector UnitY() const {
480  return Vector(data[1],data[4],data[7]);
481  }
482 
484  inline void UnitY(const Vector& X) {
485  data[1] = X(0);
486  data[4] = X(1);
487  data[7] = X(2);
488  }
489 
491  inline Vector UnitZ() const {
492  return Vector(data[2],data[5],data[8]);
493  }
494 
496  inline void UnitZ(const Vector& X) {
497  data[2] = X(0);
498  data[5] = X(1);
499  data[8] = X(2);
500  }
501 
504 
505 
507  friend bool operator==(const Rotation& a,const Rotation& b);
509  friend bool operator!=(const Rotation& a,const Rotation& b);
510 
511  friend class Frame;
512 };
513  bool operator==(const Rotation& a,const Rotation& b);
514 
515 
516 
526 class Frame {
527 public:
530 
531 public:
532 
533  inline Frame(const Rotation& R,const Vector& V);
534 
536  explicit inline Frame(const Vector& V);
538  explicit inline Frame(const Rotation& R);
539 
540  inline void setValue(float* oglmat);
541  inline void getValue(float* oglmat) const;
542 
543  inline Frame() {}
545  inline Frame(const Frame& arg);
546 
548  //\TODO should be formulated as a constructor
549  void Make4x4(double* d);
550 
553  inline double operator()(int i,int j);
554 
557  inline double operator() (int i,int j) const;
558 
559  // = Inverse
561  inline Frame Inverse() const;
562 
564  inline Vector Inverse(const Vector& arg) const;
565 
567  inline Wrench Inverse(const Wrench& arg) const;
568 
570  inline Twist Inverse(const Twist& arg) const;
571 
573  inline Frame& operator = (const Frame& arg);
574 
577  inline Vector operator * (const Vector& arg) const;
578 
585  inline Wrench operator * (const Wrench& arg) const;
586 
593  inline Twist operator * (const Twist& arg) const;
594 
596  inline friend Frame operator *(const Frame& lhs,const Frame& rhs);
597 
599  inline static Frame Identity();
600 
604  inline void Integrate(const Twist& t_this,double frequency);
605 
606  /*
607  // DH_Craig1989 : constructs a transformationmatrix
608  // T_link(i-1)_link(i) with the Denavit-Hartenberg convention as
609  // described in the Craigs book: Craig, J. J.,Introduction to
610  // Robotics: Mechanics and Control, Addison-Wesley,
611  // isbn:0-201-10326-5, 1986.
612  //
613  // Note that the frame is a redundant way to express the information
614  // in the DH-convention.
615  // \verbatim
616  // Parameters in full : a(i-1),alpha(i-1),d(i),theta(i)
617  //
618  // axis i-1 is connected by link i-1 to axis i numbering axis 1
619  // to axis n link 0 (immobile base) to link n
620  //
621  // link length a(i-1) length of the mutual perpendicular line
622  // (normal) between the 2 axes. This normal runs from (i-1) to
623  // (i) axis.
624  //
625  // link twist alpha(i-1): construct plane perpendicular to the
626  // normal project axis(i-1) and axis(i) into plane angle from
627  // (i-1) to (i) measured in the direction of the normal
628  //
629  // link offset d(i) signed distance between normal (i-1) to (i)
630  // and normal (i) to (i+1) along axis i joint angle theta(i)
631  // signed angle between normal (i-1) to (i) and normal (i) to
632  // (i+1) along axis i
633  //
634  // First and last joints : a(0)= a(n) = 0
635  // alpha(0) = alpha(n) = 0
636  //
637  // PRISMATIC : theta(1) = 0 d(1) arbitrarily
638  //
639  // REVOLUTE : theta(1) arbitrarily d(1) = 0
640  //
641  // Not unique : if intersecting joint axis 2 choices for normal
642  // Frame assignment of the DH convention : Z(i-1) follows axis
643  // (i-1) X(i-1) is the normal between axis(i-1) and axis(i)
644  // Y(i-1) follows out of Z(i-1) and X(i-1)
645  //
646  // a(i-1) = distance from Z(i-1) to Z(i) along X(i-1)
647  // alpha(i-1) = angle between Z(i-1) to Z(i) along X(i-1)
648  // d(i) = distance from X(i-1) to X(i) along Z(i)
649  // theta(i) = angle between X(i-1) to X(i) along X(i)
650  // \endverbatim
651  */
652  static Frame DH_Craig1989(double a,double alpha,double d,double theta);
653 
654  // DH : constructs a transformationmatrix T_link(i-1)_link(i) with
655  // the Denavit-Hartenberg convention as described in the original
656  // publictation: Denavit, J. and Hartenberg, R. S., A kinematic
657  // notation for lower-pair mechanisms based on matrices, ASME
658  // Journal of Applied Mechanics, 23:215-221, 1955.
659 
660  static Frame DH(double a,double alpha,double d,double theta);
661 
662 
665  inline friend bool Equal(const Frame& a,const Frame& b,double eps);
666 
668  inline friend bool operator==(const Frame& a,const Frame& b);
670  inline friend bool operator!=(const Frame& a,const Frame& b);
671 };
672 
679 class Twist {
680 public:
683 public:
684 
686  Twist():vel(),rot() {};
687 
688  Twist(const Vector& _vel,const Vector& _rot):vel(_vel),rot(_rot) {};
689 
690  inline Twist& operator-=(const Twist& arg);
691  inline Twist& operator+=(const Twist& arg);
693  inline double& operator()(int i);
694 
697  inline double operator()(int i) const;
698 
699  double operator[] ( int index ) const
700  {
701  return this->operator() ( index );
702  }
703 
704  double& operator[] ( int index )
705  {
706  return this->operator() ( index );
707  }
708 
709  inline friend Twist operator*(const Twist& lhs,double rhs);
710  inline friend Twist operator*(double lhs,const Twist& rhs);
711  inline friend Twist operator/(const Twist& lhs,double rhs);
712  inline friend Twist operator+(const Twist& lhs,const Twist& rhs);
713  inline friend Twist operator-(const Twist& lhs,const Twist& rhs);
714  inline friend Twist operator-(const Twist& arg);
715  inline friend double dot(const Twist& lhs,const Wrench& rhs);
716  inline friend double dot(const Wrench& rhs,const Twist& lhs);
717  inline friend void SetToZero(Twist& v);
718 
719 
721  static inline Twist Zero();
722 
724  inline void ReverseSign();
725 
732  inline Twist RefPoint(const Vector& v_base_AB) const;
733 
734 
737  inline friend bool Equal(const Twist& a,const Twist& b,double eps);
738 
740  inline friend bool operator==(const Twist& a,const Twist& b);
742  inline friend bool operator!=(const Twist& a,const Twist& b);
743 
744 // = Friends
745  friend class Rotation;
746  friend class Frame;
747 
748 };
749 
757 /*
758 class AccelerationTwist {
759 public:
760  Vector trans; //!< The translational acceleration of that point
761  Vector rot; //!< The rotational acceleration of that point.
762 public:
763 
765  AccelerationTwist():trans(),rot() {};
766 
767  AccelerationTwist(const Vector& _trans,const Vector& _rot):trans(_trans),rot(_rot) {};
768 
769  inline AccelerationTwist& operator-=(const AccelerationTwist& arg);
770  inline AccelerationTwist& operator+=(const AccelerationTwist& arg);
772  inline double& operator()(int i);
773 
776  inline double operator()(int i) const;
777 
778  double operator[] ( int index ) const
779  {
780  return this->operator() ( index );
781  }
782 
783  double& operator[] ( int index )
784  {
785  return this->operator() ( index );
786  }
787 
788  inline friend AccelerationTwist operator*(const AccelerationTwist& lhs,double rhs);
789  inline friend AccelerationTwist operator*(double lhs,const AccelerationTwist& rhs);
790  inline friend AccelerationTwist operator/(const AccelerationTwist& lhs,double rhs);
791  inline friend AccelerationTwist operator+(const AccelerationTwist& lhs,const AccelerationTwist& rhs);
792  inline friend AccelerationTwist operator-(const AccelerationTwist& lhs,const AccelerationTwist& rhs);
793  inline friend AccelerationTwist operator-(const AccelerationTwist& arg);
794  //inline friend double dot(const AccelerationTwist& lhs,const Wrench& rhs);
795  //inline friend double dot(const Wrench& rhs,const AccelerationTwist& lhs);
796  inline friend void SetToZero(AccelerationTwist& v);
797 
798 
800  static inline AccelerationTwist Zero();
801 
803  inline void ReverseSign();
804 
811  inline AccelerationTwist RefPoint(const Vector& v_base_AB) const;
812 
813 
816  inline friend bool Equal(const AccelerationTwist& a,const AccelerationTwist& b,double eps=epsilon);
817 
819  inline friend bool operator==(const AccelerationTwist& a,const AccelerationTwist& b);
821  inline friend bool operator!=(const AccelerationTwist& a,const AccelerationTwist& b);
822 
823 // = Friends
824  friend class Rotation;
825  friend class Frame;
826 
827 };
828 */
834 class Wrench
835 {
836 public:
839 public:
840 
842  Wrench():force(),torque() {};
843  Wrench(const Vector& _force,const Vector& _torque):force(_force),torque(_torque) {};
844 
845 // = Operators
846  inline Wrench& operator-=(const Wrench& arg);
847  inline Wrench& operator+=(const Wrench& arg);
848 
850  inline double& operator()(int i);
851 
854  inline double operator()(int i) const;
855 
856  double operator[] ( int index ) const
857  {
858  return this->operator() ( index );
859  }
860 
861  double& operator[] ( int index )
862  {
863  return this->operator() ( index );
864  }
865 
867  inline friend Wrench operator*(const Wrench& lhs,double rhs);
869  inline friend Wrench operator*(double lhs,const Wrench& rhs);
871  inline friend Wrench operator/(const Wrench& lhs,double rhs);
872 
873  inline friend Wrench operator+(const Wrench& lhs,const Wrench& rhs);
874  inline friend Wrench operator-(const Wrench& lhs,const Wrench& rhs);
875 
877  inline friend Wrench operator-(const Wrench& arg);
878 
881  inline friend void SetToZero(Wrench& v);
882 
884  static inline Wrench Zero();
885 
887  inline void ReverseSign();
888 
895  inline Wrench RefPoint(const Vector& v_base_AB) const;
896 
897 
900  inline friend bool Equal(const Wrench& a,const Wrench& b,double eps);
901 
903  inline friend bool operator==(const Wrench& a,const Wrench& b);
905  inline friend bool operator!=(const Wrench& a,const Wrench& b);
906 
907  friend class Rotation;
908  friend class Frame;
909 
910 
911 };
912 
913 
915 class Vector2
916 {
917  double data[2];
918 public:
920  Vector2() {data[0]=data[1] = 0.0;}
921  inline Vector2(double x,double y);
922  inline Vector2(const Vector2& arg);
923  inline Vector2(double* xyz);
924  inline Vector2(float* xyz);
925 
926  inline Vector2& operator = ( const Vector2& arg);
927 
929  inline double operator()(int index) const;
930 
932  inline double& operator() (int index);
933 
935  inline void GetValue(double* xy) const;
936 
937  inline void ReverseSign();
938  inline Vector2& operator-=(const Vector2& arg);
939  inline Vector2& operator +=(const Vector2& arg);
940 
941 
942  inline friend Vector2 operator*(const Vector2& lhs,double rhs);
943  inline friend Vector2 operator*(double lhs,const Vector2& rhs);
944  inline friend Vector2 operator/(const Vector2& lhs,double rhs);
945  inline friend Vector2 operator+(const Vector2& lhs,const Vector2& rhs);
946  inline friend Vector2 operator-(const Vector2& lhs,const Vector2& rhs);
947  inline friend Vector2 operator*(const Vector2& lhs,const Vector2& rhs);
948  inline friend Vector2 operator-(const Vector2& arg);
949  inline friend void SetToZero(Vector2& v);
950 
952  inline static Vector2 Zero();
953 
959  double Normalize(double eps=epsilon);
960 
962  inline double Norm() const;
963 
965  inline void Set3DXY(const Vector& v);
966 
968  inline void Set3DYZ(const Vector& v);
969 
971  inline void Set3DZX(const Vector& v);
972 
976  inline void Set3DPlane(const Frame& F_someframe_XY,const Vector& v_someframe);
977 
978 
981  inline friend bool Equal(const Vector2& a,const Vector2& b,double eps);
982 
983  friend class Rotation2;
984 };
985 
986 
990 {
991  double s,c;
994 public:
996  Rotation2() {c=1.0;s=0.0;}
997 
998  explicit Rotation2(double angle_rad):s(sin(angle_rad)),c(cos(angle_rad)) {}
999 
1000  Rotation2(double ca,double sa):s(sa),c(ca){}
1001 
1002  inline Rotation2& operator=(const Rotation2& arg);
1003  inline Vector2 operator*(const Vector2& v) const;
1005  inline double operator() (int i,int j) const;
1006 
1007  inline friend Rotation2 operator *(const Rotation2& lhs,const Rotation2& rhs);
1008 
1009  inline void SetInverse();
1010  inline Rotation2 Inverse() const;
1011  inline Vector2 Inverse(const Vector2& v) const;
1012 
1013  inline void SetIdentity();
1014  inline static Rotation2 Identity();
1015 
1016 
1018  inline void SetRot(double angle);
1019 
1021  inline static Rotation2 Rot(double angle);
1022 
1024  inline double GetRot() const;
1025 
1028  inline friend bool Equal(const Rotation2& a,const Rotation2& b,double eps);
1029 };
1030 
1033 class Frame2
1034  {
1035 public:
1038 
1039 public:
1040 
1041  inline Frame2(const Rotation2& R,const Vector2& V);
1042  explicit inline Frame2(const Vector2& V);
1043  explicit inline Frame2(const Rotation2& R);
1044  inline Frame2(void);
1045  inline Frame2(const Frame2& arg);
1046  inline void Make4x4(double* d);
1047 
1050  inline double operator()(int i,int j);
1051 
1054  inline double operator() (int i,int j) const;
1055 
1056  inline void SetInverse();
1057  inline Frame2 Inverse() const;
1058  inline Vector2 Inverse(const Vector2& arg) const;
1059  inline Frame2& operator = (const Frame2& arg);
1060  inline Vector2 operator * (const Vector2& arg);
1061  inline friend Frame2 operator *(const Frame2& lhs,const Frame2& rhs);
1062  inline void SetIdentity();
1063  inline void Integrate(const Twist& t_this,double frequency);
1064  inline static Frame2 Identity() {
1065  Frame2 tmp;
1066  tmp.SetIdentity();
1067  return tmp;
1068  }
1069  inline friend bool Equal(const Frame2& a,const Frame2& b,double eps);
1070 };
1071 
1072 inline bool Equal(const Vector&, const Vector&, double = epsilon);
1073  bool Equal(const Rotation&, const Rotation&, double = epsilon);
1074 inline bool Equal(const Frame&, const Frame&, double = epsilon);
1075 inline bool Equal(const Twist&, const Twist&, double = epsilon);
1076 inline bool Equal(const Wrench&, const Wrench&, double = epsilon);
1077 inline bool Equal(const Vector2&, const Vector2&, double = epsilon);
1078 inline bool Equal(const Rotation2&, const Rotation2&, double = epsilon);
1079 inline bool Equal(const Frame2&, const Frame2&, double = epsilon);
1080 
1081 IMETHOD Vector diff(const Vector& a,const Vector& b,double dt=1);
1082 IMETHOD Vector diff(const Rotation& R_a_b1,const Rotation& R_a_b2,double dt=1);
1083 IMETHOD Twist diff(const Frame& F_a_b1,const Frame& F_a_b2,double dt=1);
1084 IMETHOD Twist diff(const Twist& a,const Twist& b,double dt=1);
1085 IMETHOD Wrench diff(const Wrench& W_a_p1,const Wrench& W_a_p2,double dt=1);
1086 IMETHOD Vector addDelta(const Vector& a,const Vector&da,double dt=1);
1087 IMETHOD Rotation addDelta(const Rotation& a,const Vector&da,double dt=1);
1088 IMETHOD Frame addDelta(const Frame& a,const Twist& da,double dt=1);
1089 IMETHOD Twist addDelta(const Twist& a,const Twist&da,double dt=1);
1090 IMETHOD Wrench addDelta(const Wrench& a,const Wrench&da,double dt=1);
1091 #ifdef KDL_INLINE
1092 // #include "vector.inl"
1093 // #include "wrench.inl"
1094  //#include "rotation.inl"
1095  //#include "frame.inl"
1096  //#include "twist.inl"
1097  //#include "vector2.inl"
1098  //#include "rotation2.inl"
1099  //#include "frame2.inl"
1100 #include "frames.inl"
1101 #endif
1102 
1103 
1104 
1105 }
1106 
1107 
1108 #endif
_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
#define X
Definition: GeomUtils.cpp:199
ATTR_WARN_UNUSED_RESULT const BMVert * v
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
static Frame2 Identity()
Definition: frames.hpp:1064
Frame2 & operator=(const Frame2 &arg)
Definition: frames.inl:1002
void Integrate(const Twist &t_this, double frequency)
Rotation2 M
Orientation of the Frame.
Definition: frames.hpp:1037
friend bool Equal(const Frame2 &a, const Frame2 &b, double eps)
Vector2 p
origine of the Frame
Definition: frames.hpp:1036
void SetInverse()
Definition: frames.inl:987
void Make4x4(double *d)
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 Make4x4(double *d)
Reads data from an double array.
Definition: frames.cpp:39
void Integrate(const Twist &t_this, double frequency)
Definition: frames.inl:628
Frame & operator=(const Frame &arg)
Normal copy-by-value semantics.
Definition: frames.inl:437
void getValue(float *oglmat) const
Definition: frames.inl:732
friend bool operator==(const Frame &a, const Frame &b)
The literal equality operator==(), also identical.
static Frame DH_Craig1989(double a, double alpha, double d, double theta)
Definition: frames.cpp:53
Vector operator*(const Vector &arg) const
Definition: frames.inl:421
friend bool Equal(const Frame &a, const Frame &b, double eps)
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
friend bool operator!=(const Frame &a, const Frame &b)
The literal inequality operator!=().
static Frame DH(double a, double alpha, double d, double theta)
Definition: frames.cpp:70
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
Rotation2(double ca, double sa)
Definition: frames.hpp:1000
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
friend bool Equal(const Rotation2 &a, const Rotation2 &b, double eps)
void SetIdentity()
Definition: frames.inl:926
Rotation2(double angle_rad)
Definition: frames.hpp:998
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
Vector GetRot() const
Definition: frames.cpp:297
void GetRPY(double &roll, double &pitch, double &yaw) const
Definition: frames.cpp:207
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
Vector UnitY() const
Access to the underlying unitvectors of the rotation matrix.
Definition: frames.hpp:479
void UnitZ(const Vector &X)
Access to the underlying unitvectors of the rotation matrix.
Definition: frames.hpp:496
Vector UnitZ() const
Access to the underlying unitvectors of the rotation matrix.
Definition: frames.hpp:491
Vector UnitX() const
Access to the underlying unitvectors of the rotation matrix.
Definition: frames.hpp:467
double GetRotAngle(Vector &axis, double eps=epsilon) const
Definition: frames.cpp:354
static Rotation EulerZYX(double Alfa, double Beta, double Gamma)
Definition: frames.hpp:435
static Rotation Rot2(const Vector &rotvec, double angle)
Along an arbitrary axes. rotvec should be normalized.
Definition: frames.cpp:273
static Rotation Identity()
Gives back an identity rotaton matrix.
Definition: frames.inl:556
void GetEulerZYZ(double &alfa, double &beta, double &gamma) const
Definition: frames.cpp:233
static Rotation RotX(double angle)
The Rot... static functions give the value of the appropriate rotation matrix back.
Definition: frames.inl:609
Vector2 GetXZRot() const
Definition: frames.cpp:330
void setValue(float *oglmat)
Definition: frames.inl:656
friend bool operator==(const Rotation &a, const Rotation &b)
The literal equality operator==(), also identical.
Definition: frames.cpp:377
friend bool operator!=(const Rotation &a, const Rotation &b)
The literal inequality operator!=()
void UnitX(const Vector &X)
Access to the underlying unitvectors of the rotation matrix.
Definition: frames.hpp:472
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 EulerZYZ(double Alfa, double Beta, double Gamma)
Definition: frames.cpp:220
void GetEulerZYX(double &Alfa, double &Beta, double &Gamma) const
Definition: frames.hpp:450
void UnitY(const Vector &X)
Access to the underlying unitvectors of the rotation matrix.
Definition: frames.hpp:484
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
static Rotation RPY(double roll, double pitch, double yaw)
Definition: frames.cpp:195
represents both translational and rotational velocities.
Definition: frames.hpp:679
friend Twist operator*(const Twist &lhs, double rhs)
Twist(const Vector &_vel, const Vector &_rot)
Definition: frames.hpp:688
Twist & operator+=(const Twist &arg)
Definition: frames.inl:339
double & operator()(int i)
index-based access to components, first vel(0..2), then rot(3..5)
Definition: frames.inl:346
friend double dot(const Twist &lhs, const Wrench &rhs)
friend double dot(const Wrench &rhs, const Twist &lhs)
friend bool Equal(const Twist &a, const Twist &b, double eps)
friend bool operator==(const Twist &a, const Twist &b)
The literal equality operator==(), also identical.
Vector rot
The rotational velocity of that point.
Definition: frames.hpp:682
friend Twist operator-(const Twist &lhs, const Twist &rhs)
Twist()
The default constructor initialises to Zero via the constructor of Vector.
Definition: frames.hpp:686
friend void SetToZero(Twist &v)
friend Twist operator/(const Twist &lhs, double rhs)
friend Twist operator-(const Twist &arg)
void ReverseSign()
Reverses the sign of the twist.
Definition: frames.inl:316
Twist RefPoint(const Vector &v_base_AB) const
Definition: frames.inl:322
friend Twist operator*(double lhs, const Twist &rhs)
friend Twist operator+(const Twist &lhs, const Twist &rhs)
Twist & operator-=(const Twist &arg)
Definition: frames.inl:332
double operator[](int index) const
Definition: frames.hpp:699
Vector vel
The velocity of that point.
Definition: frames.hpp:681
static Twist Zero()
Definition: frames.inl:310
friend bool operator!=(const Twist &a, const Twist &b)
The literal inequality operator!=().
2D version of Vector
Definition: frames.hpp:916
static Vector2 Zero()
Definition: frames.inl:830
friend Vector2 operator-(const Vector2 &arg)
friend Vector2 operator-(const Vector2 &lhs, const Vector2 &rhs)
void ReverseSign()
Definition: frames.inl:844
friend Vector2 operator*(const Vector2 &lhs, double rhs)
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
double Normalize(double eps=epsilon)
Definition: frames.cpp:102
Vector2(float *xyz)
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
friend Vector2 operator/(const Vector2 &lhs, double rhs)
Vector2(double *xyz)
double Norm() const
Definition: frames.cpp:87
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
friend void SetToZero(Vector2 &v)
friend Vector2 operator*(const Vector2 &lhs, const Vector2 &rhs)
friend Vector2 operator+(const Vector2 &lhs, const Vector2 &rhs)
Vector2 & operator-=(const Vector2 &arg)
Definition: frames.inl:823
friend bool Equal(const Vector2 &a, const Vector2 &b, double eps)
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
friend Vector2 operator*(double lhs, const Vector2 &rhs)
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
friend double dot(const Vector &lhs, const Vector &rhs)
void GetValue(double *xyz) const
store vector components in array
Definition: frames.inl:51
double Norm() const
Definition: frames.cpp:115
void ReverseSign()
Reverses the sign of the Vector object itself.
Definition: frames.inl:449
friend void SetToZero(Vector &v)
Vector & operator-=(const Vector &arg)
subtracts a vector from the Vector object itself
Definition: frames.inl:137
friend Vector operator+(const Vector &lhs, const Vector &rhs)
friend Vector operator/(const Vector &lhs, double rhs)
Scalar division is defined.
Vector & operator*=(double arg)
Multiply by a scalar.
Definition: frames.inl:146
friend bool operator==(const Vector &a, const Vector &b)
The literal equality operator==(), also identical.
friend bool Equal(const Vector &a, const Vector &b, double eps)
double z() const
Definition: frames.inl:85
double operator[](int index) const
Equivalent to double operator()(int index) const.
Definition: frames.hpp:174
double y() const
Definition: frames.inl:84
friend Vector Normalize(const Vector &a, double eps)
return a normalized vector
friend Vector operator-(const Vector &arg)
void Set2DZX(const Vector2 &v)
a 3D vector where the 2D vector v is put in the ZX plane
Definition: frames.inl:484
Vector & operator+=(const Vector &arg)
Adds a vector from the Vector object itself.
Definition: frames.inl:128
friend Vector operator*(const Vector &lhs, const Vector &rhs)
Vector & operator=(const Vector &arg)
Assignment operator. The normal copy by value semantics.
Definition: frames.inl:57
double Normalize(double eps=epsilon)
Definition: frames.cpp:145
Vector()
Does not initialise the Vector to zero. use Vector::Zero() or SetToZero for that.
Definition: frames.hpp:147
double data[3]
Definition: frames.hpp:145
double x() const
Definition: frames.inl:83
void y(double)
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
double operator()(int index) const
Access to elements, range checked when NDEBUG is not set, from 0..2.
Definition: frames.inl:159
friend Vector operator*(const Vector &lhs, double rhs)
Scalar multiplication is defined.
friend Vector operator-(const Vector &lhs, const Vector &rhs)
void z(double)
void x(double)
friend bool operator!=(const Vector &a, const Vector &b)
The literal inequality operator!=().
void Set2DYZ(const Vector2 &v)
a 3D vector where the 2D vector v is put in the YZ plane
Definition: frames.inl:475
friend Vector operator*(double lhs, const Vector &rhs)
Scalar multiplication is defined.
represents both translational and rotational acceleration.
Definition: frames.hpp:835
friend bool Equal(const Wrench &a, const Wrench &b, double eps)
Wrench & operator+=(const Wrench &arg)
Definition: frames.inl:236
friend Wrench operator+(const Wrench &lhs, const Wrench &rhs)
Wrench & operator-=(const Wrench &arg)
Definition: frames.inl:229
Vector force
Force that is applied at the origin of the current ref frame.
Definition: frames.hpp:837
friend bool operator==(const Wrench &a, const Wrench &b)
The literal equality operator==(), also identical.
friend Wrench operator/(const Wrench &lhs, double rhs)
Scalar division.
Wrench()
Does initialise force and torque to zero via the underlying constructor of Vector.
Definition: frames.hpp:842
static Wrench Zero()
Definition: frames.inl:205
Wrench(const Vector &_force, const Vector &_torque)
Definition: frames.hpp:843
double & operator()(int i)
index-based access to components, first force(0..2), then torque(3..5)
Definition: frames.inl:243
friend Wrench operator-(const Wrench &lhs, const Wrench &rhs)
friend Wrench operator-(const Wrench &arg)
An unary - operator.
friend bool operator!=(const Wrench &a, const Wrench &b)
The literal inequality operator!=().
Vector torque
Torque that is applied at the origin of the current ref frame.
Definition: frames.hpp:838
double operator[](int index) const
Definition: frames.hpp:856
Wrench RefPoint(const Vector &v_base_AB) const
Definition: frames.inl:217
friend Wrench operator*(const Wrench &lhs, double rhs)
Scalar multiplication.
friend Wrench operator*(double lhs, const Wrench &rhs)
Scalar multiplication.
friend void SetToZero(Wrench &v)
void ReverseSign()
Reverses the sign of the current Wrench.
Definition: frames.inl:211
@ Vector
Vector data type.
#define R
static unsigned a[3]
Definition: RandGen.cpp:78
Point2 Vector2
Definition: FitCurve.h:52
Definition: chain.cpp:27
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
Vector Normalize(const Vector &, double eps=epsilon)
IMETHOD Vector addDelta(const Vector &a, const Vector &da, double dt=1)
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)
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 IMETHOD
Definition: utility.h:43
CCL_NAMESPACE_BEGIN struct Window V
int xy[2]
Definition: wm_draw.c:135