amino  1.0-beta2
Lightweight Robot Utility Library
tf.hpp
Go to the documentation of this file.
1 /* -*- mode: C++; c-basic-offset: 4 -*- */
2 /* ex: set shiftwidth=4 tabstop=4 expandtab: */
3 /*
4  * Copyright (c) 2013, Georgia Tech Research Corporation
5  * All rights reserved.
6  *
7  * Author(s): Neil T. Dantam <ntd@gatech.edu>
8  * Georgia Tech Humanoid Robotics Lab
9  * Under Direction of Prof. Mike Stilman <mstilman@cc.gatech.edu>
10  *
11  *
12  * This file is provided under the following "BSD-style" License:
13  *
14  *
15  * Redistribution and use in source and binary forms, with or
16  * without modification, are permitted provided that the following
17  * conditions are met:
18  *
19  * * Redistributions of source code must retain the above copyright
20  * notice, this list of conditions and the following disclaimer.
21  *
22  * * Redistributions in binary form must reproduce the above
23  * copyright notice, this list of conditions and the following
24  * disclaimer in the documentation and/or other materials provided
25  * with the distribution.
26  *
27  * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND
28  * CONTRIBUTORS "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES,
29  * INCLUDING, BUT NOT LIMITED TO, THE IMPLIED WARRANTIES OF
30  * MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
31  * DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR
32  * CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL,
33  * SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT
34  * LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF
35  * USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
36  * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
37  * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
38  * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
39  * POSSIBILITY OF SUCH DAMAGE.
40  *
41  */
42 
43 #ifndef AA_TF_HPP
44 #define AA_TF_HPP
45 
59 namespace amino {
60 
61 
62 
66 struct Vec3 : aa_tf_vec3 {
67  Vec3() {}
68 
72  Vec3( const struct aa_tf_duqu *S ) : aa_tf_vec3(from_duqu(S->data)) {}
73 
77  Vec3( const struct aa_tf_duqu &S ) : aa_tf_vec3(from_duqu(S.data)) {}
78 
82  Vec3( const struct aa_tf_tfmat *T ) : aa_tf_vec3(from_tfmat(T->data)) {}
83 
87  Vec3( const struct aa_tf_tfmat &T ) : aa_tf_vec3(from_tfmat(T.data)) {}
88 
92  Vec3( double _x, double _y, double _z ) : aa_tf_vec3(from_xyz(_x,_y,_z)) {}
93 
94 
98  Vec3( const double *_xyz ) : aa_tf_vec3(from_vec3(_xyz)) {}
99 
103  static aa_tf_vec3 from_xyz( double x, double y, double z ) {
104  aa_tf_vec3 V;
105  V.x = x;
106  V.y = y;
107  V.z = z;
108  return V;
109  }
110 
114  static aa_tf_vec3 from_vec3( const double a_x[3] ) {
115  aa_tf_vec3 V;
116  memcpy( V.data, a_x, 3*sizeof(V.data[0]) );
117  return V;
118  }
119 
123  static aa_tf_vec3 from_duqu( const double S[8] ) {
124  aa_tf_vec3 V;
125  aa_tf_duqu_trans( S, V.data );
126  return V;
127  }
128 
132  static aa_tf_vec3 from_tfmat( const double T[12] ) {
133  return from_vec3( T+9 );
134  }
135 };
136 
137 /*------------------------------*/
138 /*-------- ORIENTATIONS --------*/
139 /*------------------------------*/
140 
144 struct XAngle {
145  double value;
146 
148  XAngle(double v) : value(v) {}
149 };
150 
154 struct YAngle {
155  double value;
156 
158  YAngle(double v) : value(v) {}
159 };
160 
164 struct ZAngle {
165  double value;
166 
168  ZAngle(double v) : value(v) {}
169 };
170 
174 struct Quat : aa_tf_quat {
178  Quat() : aa_tf_quat(from_xyzw(0,0,0,1)) {}
179 
183  Quat( const aa_tf_quat *p ) : aa_tf_quat(from_quat(p->data)) {}
184 
188  Quat( const aa_tf_quat &p ) : aa_tf_quat(from_quat(p.data)) {}
189 
193  Quat( const aa_tf_rotmat *p ) : aa_tf_quat(from_rotmat(p->data)) {}
194 
198  Quat( const aa_tf_rotmat &p ) : aa_tf_quat(from_rotmat(p.data)) {}
199 
203  Quat( const aa_tf_axang *p ) : aa_tf_quat(from_axang(p->data)) {}
204 
208  Quat( const aa_tf_axang &p ) : aa_tf_quat(from_axang(p.data)) {}
209 
213  Quat( const XAngle &p ) : aa_tf_quat(from_xangle(p.value)) {}
214 
218  Quat( const YAngle &p ) : aa_tf_quat(from_yangle(p.value)) {}
219 
223  Quat( const ZAngle &p ) : aa_tf_quat(from_zangle(p.value)) {}
224 
225 
229  static aa_tf_quat from_quat( const double x[4] ) {
230  aa_tf_quat y;
231  memcpy(y.data, x, 4*sizeof(y.data[0]));
232  return y;
233  }
234 
238  static aa_tf_quat from_xyzw( double x, double y, double z, double w ) {
239  aa_tf_quat q;
240  q.x = x;
241  q.y = y;
242  q.z = z;
243  q.w = w;
244  return q;
245  }
246 
250  static aa_tf_quat from_rotmat( const double x[9] ) {
251  aa_tf_quat y;
252  aa_tf_rotmat2quat(x, y.data);
253  return y;
254  }
255 
259  static aa_tf_quat from_axang( const double x[4] ) {
260  aa_tf_quat y;
261  aa_tf_axang2quat(x, y.data);
262  return y;
263  }
264 
268  static aa_tf_quat from_axang( const double a[3], double angle ) {
269  double x[4] = {a[0], a[1], a[2], angle};
270  return from_axang(x);
271  }
272 
276  static aa_tf_quat from_rotvec( const double x[3] ) {
277  aa_tf_quat y;
278  aa_tf_rotvec2quat(x, y.data);
279  return y;
280  }
281 
285  static aa_tf_quat from_xangle( const double v ) {
286  aa_tf_quat y;
287  aa_tf_xangle2quat(v, y.data);
288  return y;
289  }
290 
294  static aa_tf_quat from_yangle( const double v ) {
295  aa_tf_quat y;
296  aa_tf_yangle2quat(v, y.data);
297  return y;
298  }
299 
303  static aa_tf_quat from_zangle( const double v ) {
304  aa_tf_quat y;
305  aa_tf_zangle2quat(v, y.data);
306  return y;
307  }
308 };
309 
317  RotMat() : aa_tf_rotmat( from_rotmat(1,0,0, 0,1,0, 0,0,1) )
318  {}
319 
323  RotMat( const aa_tf_quat *p ) : aa_tf_rotmat(from_quat(p->data)) {}
324 
328  RotMat( const aa_tf_quat &p ) : aa_tf_rotmat(from_quat(p.data)) {}
329 
333  RotMat( const aa_tf_rotmat *p ) : aa_tf_rotmat(from_rotmat(p->data)) {}
334 
338  RotMat( const aa_tf_rotmat &p ) : aa_tf_rotmat(from_rotmat(p.data)) {}
339 
343  RotMat( const aa_tf_axang *p ) : aa_tf_rotmat(from_axang(p->data)) {}
344 
348  RotMat( const aa_tf_axang &p ) : aa_tf_rotmat(from_axang(p.data)) {}
349 
353  RotMat( const XAngle &p ) : aa_tf_rotmat(from_xangle(p.value)) {}
354 
358  RotMat( const YAngle &p ) : aa_tf_rotmat(from_yangle(p.value)) {}
359 
363  RotMat( const ZAngle &p ) : aa_tf_rotmat(from_zangle(p.value)) {}
364 
368  RotMat( double r11, double r12, double r13,
369  double r21, double r22, double r23,
370  double r31, double r32, double r33 ) :
371  aa_tf_rotmat(from_rotmat(r11, r12, r13,
372  r21, r22, r23,
373  r31, r32, r33))
374  {}
375 
376 
380  static aa_tf_rotmat from_rotmat( double r11, double r12, double r13,
381  double r21, double r22, double r23,
382  double r31, double r32, double r33 )
383  {
384  aa_tf_rotmat R;
385  R.data[0] = r11;
386  R.data[1] = r21;
387  R.data[2] = r31;
388 
389  R.data[3] = r12;
390  R.data[4] = r22;
391  R.data[5] = r32;
392 
393  R.data[6] = r13;
394  R.data[7] = r23;
395  R.data[8] = r33;
396  return R;
397  }
398 
399 
403  static aa_tf_rotmat from_quat( const double x[4] ) {
404  aa_tf_rotmat y;
405  aa_tf_quat2rotmat(x, y.data);
406  return y;
407  }
408 
412  static aa_tf_rotmat from_rotmat( const double x[9] ) {
413  aa_tf_rotmat y;
414  memcpy(y.data, x, 9*sizeof(y.data[0]));
415  return y;
416  }
417 
421  static aa_tf_rotmat from_axang( const double x[4] ) {
422  aa_tf_rotmat y;
423  aa_tf_axang2rotmat(x, y.data);
424  return y;
425  }
426 
430  static aa_tf_rotmat from_rotvec( const double x[3] ) {
431  aa_tf_rotmat y;
433  return y;
434  }
435 
439  static aa_tf_rotmat from_xangle( const double v ) {
440  aa_tf_rotmat y;
442  return y;
443  }
444 
448  static aa_tf_rotmat from_yangle( const double v ) {
449  aa_tf_rotmat y;
451  return y;
452  }
453 
457  static aa_tf_rotmat from_zangle( const double v ) {
458  aa_tf_rotmat y;
460  return y;
461  }
462 };
463 
468 
472  AxisAngle() : aa_tf_axang(from_axang(0,0,1,0))
473  {}
474 
478  AxisAngle( const aa_tf_quat *p ) : aa_tf_axang(from_quat(p->data)) {}
479 
483  AxisAngle( const aa_tf_quat &p ) : aa_tf_axang(from_quat(p.data)) {}
484 
488  AxisAngle( const aa_tf_rotmat *p ) : aa_tf_axang(from_rotmat(p->data)) {}
489 
493  AxisAngle( const aa_tf_rotmat &p ) : aa_tf_axang(from_rotmat(p.data)) {}
494 
498  AxisAngle( const aa_tf_axang *p ) : aa_tf_axang(from_axang(p->data)) {}
499 
503  AxisAngle( const aa_tf_axang &p ) : aa_tf_axang(from_axang(p.data)) {}
504 
508  AxisAngle( double x, double y, double z, double theta ) :
509  aa_tf_axang(from_axang(x,y,z,theta))
510  {}
511 
515  AxisAngle( const double *_axis, double _angle ) :
516  aa_tf_axang(from_axang(_axis,_angle))
517  {}
518 
522  static aa_tf_axang from_quat( const double x[4] ) {
523  aa_tf_axang y;
524  aa_tf_quat2axang(x, y.data);
525  return y;
526  }
527 
531  static aa_tf_axang from_rotmat( const double x[9] ) {
532  aa_tf_axang y;
533  aa_tf_rotmat2axang(x, y.data);
534  return y;
535  }
536 
540  static aa_tf_axang from_axang( double x, double y, double z, double theta ) {
541  double n = sqrt( x*x + y*y + z*z );
542  double a[4] = {x/n, y/n, z/n, theta};
543  return from_axang(a);
544  }
545 
549  static aa_tf_axang from_axang( const double x[4] ) {
550  aa_tf_axang y;
551  memcpy(y.data, x, 4*sizeof(y.data[0]));
552  return y;
553  }
554 
558  static aa_tf_axang from_axang( const double axis[3], double angle ) {
559  aa_tf_axang y;
560  y.axis.x = axis[0];
561  y.axis.y = axis[1];
562  y.axis.z = axis[2];
563  y.angle = angle;
564  return y;
565  }
566 
570  static aa_tf_axang from_rotvec( const double x[3] ) {
571  aa_tf_axang y;
572  aa_tf_rotvec2axang(x, y.data);
573  return y;
574  }
575 };
576 
577 /*-----------------------------*/
578 /*------ TRANSFORMATIONS ------*/
579 /*-----------------------------*/
580 
588  DualQuat() : aa_tf_duqu(from_xyzw(0,0,0,1, 0,0,0,0))
589  {}
590 
594  DualQuat(const double *S) : aa_tf_duqu(from_duqu(S)) {}
595 
599  DualQuat(const struct aa_tf_duqu *S) : aa_tf_duqu(from_duqu(S->data)) {}
600 
604  DualQuat(const struct aa_tf_duqu &S) : aa_tf_duqu(from_duqu(S.data)) {}
605 
609  DualQuat(const struct aa_tf_qv *S) : aa_tf_duqu(from_qv(S->r.data, S->v.data)) {}
610 
614  DualQuat(const struct aa_tf_qv &S) : aa_tf_duqu(from_qv(S.r.data, S.v.data)) {}
615 
619  DualQuat(const struct aa_tf_quat *r, const struct aa_tf_vec3 *v) :
620  aa_tf_duqu(from_qv(r->data, v->data))
621  {}
622 
626  DualQuat(const struct aa_tf_quat &r, const struct aa_tf_vec3 &v) :
627  aa_tf_duqu(from_qv(r.data, v.data))
628  {}
629 
633  DualQuat(const struct aa_tf_tfmat *T) : aa_tf_duqu(from_tfmat(T->data)) {}
634 
638  DualQuat(const struct aa_tf_tfmat &T) : aa_tf_duqu(from_tfmat(T.data)) {}
639 
643  DualQuat(const XAngle &r, const struct aa_tf_vec3 &v) : aa_tf_duqu(from_xxyz(r.value, v.x, v.y, v.z)) {}
644 
648  DualQuat(const YAngle &r, const struct aa_tf_vec3 &v) : aa_tf_duqu(from_yxyz(r.value, v.x, v.y, v.z)) {}
649 
653  DualQuat(const ZAngle &r, const struct aa_tf_vec3 &v) : aa_tf_duqu(from_zxyz(r.value, v.x, v.y, v.z)) {}
654 
658  DualQuat(const struct aa_tf_vec3 &v) : aa_tf_duqu(from_xyz(v.x, v.y, v.z)) {}
659 
663  DualQuat(const aa_tf_axang &r, const struct aa_tf_vec3 &v) :
664  aa_tf_duqu(from_qv(Quat::from_axang(r.data).data,
665  v.data))
666  {}
667 
672  {
673  DualQuat S;
674  aa_tf_duqu_conj(this->data, S.data);
675  return S;
676  }
677 
682  aa_tf_vec3 V;
683  aa_tf_duqu_trans( this->data, V.data );
684  return V;
685  }
686 
690  static aa_tf_duqu from_xyzw( double x_real, double y_real, double z_real, double w_real,
691  double x_dual, double y_dual, double z_dual, double w_dual ) {
692  DualQuat S;
693  S.real = Quat::from_xyzw(x_real,y_real,z_real,w_real);
694  S.dual = Quat::from_xyzw(x_dual,y_dual,z_dual,w_dual);
695  return S;
696  }
697 
701  static aa_tf_duqu from_duqu( const double s[8] ) {
702  DualQuat S;
703  memcpy(S.data, s, 8*sizeof(s[0]));
704  return S;
705  }
706 
710  static aa_tf_duqu from_qv(const double q[4], const double v[3]) {
711  DualQuat S;
712  aa_tf_qv2duqu(q,v,S.data);
713  return S;
714  }
715 
719  static aa_tf_duqu from_tfmat(const double T[12] ) {
720  DualQuat S;
721  aa_tf_tfmat2duqu(T,S.data);
722  return S;
723  }
724 
728  static aa_tf_duqu from_xxyz(double theta, double x, double y, double z) {
729  DualQuat S;
730  aa_tf_xxyz2duqu(theta, x, y, z, S.data);
731  return S;
732  }
733 
737  static aa_tf_duqu from_yxyz(double theta, double x, double y, double z) {
738  DualQuat S;
739  aa_tf_yxyz2duqu(theta, x, y, z, S.data);
740  return S;
741  }
742 
746  static aa_tf_duqu from_zxyz(double theta, double x, double y, double z) {
747  DualQuat S;
748  aa_tf_zxyz2duqu(theta, x, y, z, S.data);
749  return S;
750  }
751 
755  static aa_tf_duqu from_xyz(double x, double y, double z) {
756  DualQuat S;
757  aa_tf_xyz2duqu(x, y, z, S.data);
758  return S;
759  }
760 
764  static aa_tf_duqu from_dx(double *s, double *dx )
765  {
766  DualQuat S;
767  aa_tf_duqu_vel2diff( s, dx, S.data );
768  return S;
769  }
770 };
771 
772 
776 struct QuatTran : aa_tf_qv {
780  QuatTran() : aa_tf_qv(from_xyzw(0,0,0,1, 0,0,0))
781  {}
782 
786  QuatTran(const struct aa_tf_qv *S) : aa_tf_qv(from_qv(S->r.data, S->v.data)) {}
787 
791  QuatTran(const struct aa_tf_qv &S) : aa_tf_qv(from_qv(S.r.data, S.v.data)) {}
792 
796  QuatTran(const struct aa_tf_quat *_r, const struct aa_tf_vec3 *_v) :
797  aa_tf_qv(from_qv(_r->data, _v->data))
798  {}
799 
803  QuatTran(const struct aa_tf_quat &_r, const struct aa_tf_vec3 &_v) :
804  aa_tf_qv(from_qv(_r.data, _v.data))
805  {}
806 
810  QuatTran(const struct aa_tf_duqu *S) : aa_tf_qv(from_duqu(S->data)) {}
811 
815  QuatTran(const struct aa_tf_duqu &S) : aa_tf_qv(from_duqu(S.data)) {}
816 
820  QuatTran(const struct aa_tf_tfmat *T) : aa_tf_qv(from_tfmat(T->data)) {}
821 
825  QuatTran(const struct aa_tf_tfmat &T) : aa_tf_qv(from_tfmat(T.data)) {}
826 
831  aa_tf_qv qv;
832  aa_tf_qutr_conj(this->data, qv.data);
833  return qv;
834  }
835 
839  static aa_tf_qv from_xyzw( double q_x, double q_y, double q_z, double q_w,
840  double t_x, double t_y, double t_z )
841  {
842  aa_tf_qv qv;
843  qv.r = Quat::from_xyzw(q_x, q_y, q_z, q_w);
844  qv.v = Vec3::from_xyz(t_x, t_y, t_z);
845  return qv;
846  }
847 
851  static aa_tf_qv from_qv(const double a_r[4], const double a_v[3])
852  {
853  aa_tf_qv qv;
854  memcpy(qv.r.data, a_r, 4*sizeof(qv.r.data[0]));
855  memcpy(qv.v.data, a_v, 3*sizeof(qv.v.data[0]));
856  return qv;
857  }
858 
862  static aa_tf_qv from_qv(const double e[7])
863  {
864  return from_qv(e, e+4);
865  }
866 
870  static aa_tf_qv from_duqu(const double s[8]) {
871  aa_tf_qv qv;
872  aa_tf_duqu2qv( s, qv.r.data, qv.v.data );
873  return qv;
874  }
875 
879  static aa_tf_qv from_tfmat(const double t[12]) {
880  aa_tf_qv qv;
881  aa_tf_tfmat2duqu( t, qv.data );
882  return qv;
883  }
884 };
885 
889 struct TfMat : aa_tf_tfmat {
894 
898  TfMat(const struct aa_tf_tfmat *T) : aa_tf_tfmat(from_tfmat(T->data)) {}
899 
903  TfMat(const struct aa_tf_tfmat &T) : aa_tf_tfmat(from_tfmat(T.data)) {}
904 
908  TfMat(const struct aa_tf_duqu *S) : aa_tf_tfmat(from_duqu(S->data)) {}
909 
913  TfMat(const struct aa_tf_duqu &S) : aa_tf_tfmat(from_duqu(S.data)) {}
914 
918  TfMat(const struct aa_tf_qv *S) : aa_tf_tfmat(from_qv(S->r.data, S->v.data)) {}
919 
923  TfMat(const struct aa_tf_qv &S) : aa_tf_tfmat(from_qv(S.r.data, S.v.data)) {}
924 
928  TfMat(const struct aa_tf_quat *_r, const struct aa_tf_vec3 *_v) :
929  aa_tf_tfmat(from_qv(_r->data, _v->data))
930  {}
931 
935  TfMat(const struct aa_tf_quat &_r, const struct aa_tf_vec3 &_v) :
936  aa_tf_tfmat(from_qv(_r.data, _v.data))
937  {}
938 
942  static aa_tf_tfmat from_duqu( const double s[8] ) {
943  aa_tf_tfmat T;
944  aa_tf_duqu2tfmat(s, T.data);
945  return T;
946  }
947 
951  static aa_tf_tfmat from_qv(const double q[4], const double v[3]) {
952  aa_tf_tfmat T;
953  aa_tf_qv2tfmat(q,v,T.data);
954  return T;
955  }
956 
960  static aa_tf_tfmat from_tfmat(const double t[12] ) {
961  aa_tf_tfmat T;
962  memcpy(T.data, t, 12*sizeof(T.data[0]));
963  return T;
964  }
965 };
966 
967 /*---- OPERATORS -----*/
968 
969 
973 static inline struct aa_tf_vec3
974 operator+(const struct aa_tf_vec3 &a, const struct aa_tf_vec3 &b) {
975  struct aa_tf_vec3 c;
976  c.x = a.x + b.x;
977  c.y = a.y + b.y;
978  c.z = a.z + b.z;
979  return c;
980 }
981 
985 static inline struct aa_tf_vec3
986 operator/(const struct aa_tf_vec3 &a, double b ) {
987  struct aa_tf_vec3 c;
988  c.x = a.x / b;
989  c.y = a.y / b;
990  c.z = a.z / b;
991  return c;
992 }
993 
994 
998 static inline struct aa_tf_rotmat
999 operator*(const struct aa_tf_rotmat &a, const struct aa_tf_rotmat &b) {
1000  struct aa_tf_rotmat c;
1001  aa_tf_9mul(a.data, b.data, c.data);
1002  return c;
1003 }
1004 
1008 static inline struct aa_tf_quat
1009 operator*(const struct aa_tf_quat &a, const struct aa_tf_quat &b) {
1010  struct aa_tf_quat c;
1011  aa_tf_qmul(a.data, b.data, c.data);
1012  return c;
1013 }
1014 
1018 static inline struct aa_tf_tfmat
1019 operator*(const struct aa_tf_tfmat &a, const struct aa_tf_tfmat &b) {
1020  struct aa_tf_tfmat c;
1021  aa_tf_12chain(a.data, b.data, c.data);
1022  return c;
1023 }
1024 
1028 static inline struct aa_tf_duqu
1029 operator*(const struct aa_tf_duqu &a, const struct aa_tf_duqu &b) {
1030  struct aa_tf_duqu c;
1031  aa_tf_duqu_mul(a.data, b.data, c.data);
1032  return c;
1033 }
1034 
1042 static inline struct aa_tf_qv
1043 operator*(const struct aa_tf_qv &a, const struct aa_tf_qv &b) {
1044  struct aa_tf_qv c;
1045  aa_tf_qv_chain(a.r.data, a.v.data,
1046  b.r.data, b.v.data,
1047  c.r.data, c.v.data);
1048  return c;
1049 }
1050 
1051 };
1052 
1053 #endif //AA_MEM_HPP
RotMat(const aa_tf_quat &p)
Construct a rotation matrix from a unit quaternion.
Definition: tf.hpp:328
static aa_tf_axang from_axang(double x, double y, double z, double theta)
Create an axis angle from individual components.
Definition: tf.hpp:540
AA_API void aa_tf_duqu_mul(const double d1[AA_RESTRICT 8], const double d2[AA_RESTRICT 8], double d3[AA_RESTRICT 8])
Dual quaternion multiplication.
QuatTran(const struct aa_tf_duqu *S)
Construct from a dual quaternion.
Definition: tf.hpp:810
QuatTran(const struct aa_tf_qv &S)
Construct from another quaternion-translation.
Definition: tf.hpp:791
static aa_tf_quat from_zangle(const double v)
Create a quaternion object rotation about z.
Definition: tf.hpp:303
double y
y component
Definition: tf.h:221
AA_API void aa_tf_quat2rotmat(const double quat[AA_RESTRICT 4], double rotmat[AA_RESTRICT 9])
convert quaternion to rotation matrix
AA_API void aa_tf_yxyz2duqu(double theta, double x, double y, double z, double d[AA_RESTRICT 8])
Convert y angle and translation to dual quaternion.
TfMat(const struct aa_tf_quat &_r, const struct aa_tf_vec3 &_v)
Construct a transformation matrix from a quaternion-translation.
Definition: tf.hpp:935
static aa_tf_duqu from_dx(double *s, double *dx)
Create a dual quaternion derivative from a dual quaternion and the spatial velocity.
Definition: tf.hpp:764
RotMat(const aa_tf_axang &p)
Construct a rotation matrix from an axis-angle.
Definition: tf.hpp:348
Quat(const aa_tf_rotmat &p)
Construct from a rotation matrix.
Definition: tf.hpp:198
AA_API void aa_tf_xyz2duqu(double x, double y, double z, double d[AA_RESTRICT 8])
Pure translation dual quaternion.
Vec3(const struct aa_tf_tfmat &T)
Extract the translation part of a transformation matrix.
Definition: tf.hpp:87
AA_API void aa_tf_duqu2tfmat(const double d[AA_RESTRICT 8], double T[AA_RESTRICT 12])
Convert dual quaternion to transformation matrix.
QuatTran(const struct aa_tf_qv *S)
Construct from another quaternion-translation.
Definition: tf.hpp:786
AxisAngle(const aa_tf_axang &p)
Construct an axis-angle from another axis-angle.
Definition: tf.hpp:503
DualQuat(const struct aa_tf_duqu *S)
Construct from another dual quaternion.
Definition: tf.hpp:599
static aa_tf_vec3 from_vec3(const double a_x[3])
Create a Vec3 from array.
Definition: tf.hpp:114
RotMat(const aa_tf_quat *p)
Construct a rotation matrix from a unit quaternion.
Definition: tf.hpp:323
double value
The angle value in radians.
Definition: tf.hpp:145
static aa_tf_rotmat from_xangle(const double v)
Create a rotation matrix from a rotation about X.
Definition: tf.hpp:439
static aa_tf_vec3 from_tfmat(const double T[12])
Create a Vec3 from translation part of transformation matrix.
Definition: tf.hpp:132
AxisAngle()
Construct an idenity axis-angle.
Definition: tf.hpp:472
void aa_tf_qutr_conj(const double a[7], double c[7])
quaternion-translation conjugate
AA_API void aa_tf_9mul(const double R0[AA_RESTRICT 9], const double R1[AA_RESTRICT 9], double R[AA_RESTRICT 9])
Multiply two rotation matrices.
A transformation matrix object.
Definition: tf.hpp:889
double value
The angle value in radians.
Definition: tf.hpp:165
aa_tf_qv conj()
Return the conjugate (inverse).
Definition: tf.hpp:830
static aa_tf_duqu from_zxyz(double theta, double x, double y, double z)
Create a dual quaternion an Z axis rotation and translation components.
Definition: tf.hpp:746
AxisAngle(const aa_tf_quat *p)
Construct an axis-angle from a unit quaternion.
Definition: tf.hpp:478
TfMat(const struct aa_tf_duqu *S)
Construct a transformation matrix from a dual quaternion.
Definition: tf.hpp:908
YAngle(double v)
Construct with the given angle.
Definition: tf.hpp:158
static aa_tf_duqu from_xyzw(double x_real, double y_real, double z_real, double w_real, double x_dual, double y_dual, double z_dual, double w_dual)
Create a dual quaternion from components.
Definition: tf.hpp:690
double data[8]
data array
Definition: tf.h:321
Memory layout for a vector of length 3.
Definition: tf.h:158
QuatTran(const struct aa_tf_quat &_r, const struct aa_tf_vec3 &_v)
Construct from another quaternion-translation.
Definition: tf.hpp:803
AA_API void aa_tf_qmul(const double a[AA_RESTRICT 4], const double b[AA_RESTRICT 4], double c[AA_RESTRICT 4])
Quaternion multiplication.
Vec3(const struct aa_tf_duqu &S)
Extract the translation part of a unit dual quaternion.
Definition: tf.hpp:77
QuatTran(const struct aa_tf_tfmat &T)
Construct from a transformation matrix.
Definition: tf.hpp:825
static aa_tf_duqu from_qv(const double q[4], const double v[3])
Create a dual quaternion from a rotation quaternion and a translation vector.
Definition: tf.hpp:710
TfMat(const struct aa_tf_duqu &S)
Construct a transformation matrix from a dual quaternion.
Definition: tf.hpp:913
static const double aa_tf_ident[12]
Identity transformation matrix array.
Definition: tf.h:460
static aa_tf_axang from_rotvec(const double x[3])
Create an axis angle from a rotation vector.
Definition: tf.hpp:570
A rotation about the Y axis.
Definition: tf.hpp:154
AA_API void aa_tf_tfmat2duqu(const double T[AA_RESTRICT 12], double d[AA_RESTRICT 8])
Convert transformation matrix to dual quaternion.
TfMat(const struct aa_tf_tfmat &T)
Construct a transformation matrix from another transformation matrix.
Definition: tf.hpp:903
Memory layout for a transformation matrix.
Definition: tf.h:262
DualQuat(const YAngle &r, const struct aa_tf_vec3 &v)
Construct from a rotation about Y and a translation vector.
Definition: tf.hpp:648
static aa_tf_quat from_rotmat(const double x[9])
Create a quaternion object a rotation matrix.
Definition: tf.hpp:250
RotMat(const aa_tf_rotmat *p)
Construct a rotation matrix from another rotation matrix.
Definition: tf.hpp:333
static aa_tf_duqu from_xyz(double x, double y, double z)
Create a dual quaternion from and identity rotation and translation components.
Definition: tf.hpp:755
DualQuat(const double *S)
Construct from another dual quaternion.
Definition: tf.hpp:594
double value
The angle value in radians.
Definition: tf.hpp:155
double x
x component
Definition: tf.h:161
An axis-angle object.
Definition: tf.hpp:467
static aa_tf_rotmat from_rotmat(double r11, double r12, double r13, double r21, double r22, double r23, double r31, double r32, double r33)
Create a rotation matrix from individual components.
Definition: tf.hpp:380
static aa_tf_tfmat from_tfmat(const double t[12])
Construct a transformation matrix from another transformation matrix.
Definition: tf.hpp:960
static aa_tf_quat from_axang(const double x[4])
Create a quaternion object an axis-angle.
Definition: tf.hpp:259
Vec3(const struct aa_tf_tfmat *T)
Extract the translation part of a transformation matrix.
Definition: tf.hpp:82
Quat(const aa_tf_axang &p)
Construct from an axix angle rotation.
Definition: tf.hpp:208
Quat(const aa_tf_axang *p)
Construct from an axix angle rotation.
Definition: tf.hpp:203
AA_API void aa_tf_duqu_vel2diff(const double d[AA_RESTRICT 8], const double dx[AA_RESTRICT 6], double dd[AA_RESTRICT 8])
Dual quaternion derivative from velocity.
Quat(const XAngle &p)
Construct from a rotation about the x axis.
Definition: tf.hpp:213
QuatTran()
Construct an idenity object.
Definition: tf.hpp:780
A dual quaternion object.
Definition: tf.hpp:584
static aa_tf_quat from_rotvec(const double x[3])
Create a quaternion object a rotation vector.
Definition: tf.hpp:276
AA_API void aa_tf_axang2rotmat(const double ra[AA_RESTRICT 4], double R[AA_RESTRICT 9])
convert axis angle to rotation matrix
double data[3]
data array
Definition: tf.h:165
Vec3(const double *_xyz)
Construct from array.
Definition: tf.hpp:98
AxisAngle(const double *_axis, double _angle)
Construct an axis-angle from an axis array and an angle.
Definition: tf.hpp:515
double data[7]
data array
Definition: tf.h:306
RotMat(const ZAngle &p)
Construct a rotation matrix from a rotation about Z.
Definition: tf.hpp:363
A vector of length 3.
Definition: tf.hpp:66
QuatTran(const struct aa_tf_quat *_r, const struct aa_tf_vec3 *_v)
Construct from another quaternion-translation.
Definition: tf.hpp:796
static aa_tf_duqu from_duqu(const double s[8])
Create a dual quaternion from another dual quaternion.
Definition: tf.hpp:701
TfMat(const struct aa_tf_qv *S)
Construct a transformation matrix from a quaternion-translation.
Definition: tf.hpp:918
A quaternion object.
Definition: tf.hpp:174
DualQuat()
Construct an identity dual quaternion.
Definition: tf.hpp:588
static aa_tf_qv from_duqu(const double s[8])
Create a quaternion-translation from a dual quaternion.
Definition: tf.hpp:870
double x
x component
Definition: tf.h:220
static aa_tf_rotmat from_quat(const double x[4])
Create a rotation matrix from a unit quaternion.
Definition: tf.hpp:403
double data[4]
data array
Definition: tf.h:232
TfMat(const struct aa_tf_quat *_r, const struct aa_tf_vec3 *_v)
Construct a transformation matrix from a quaternion-translation.
Definition: tf.hpp:928
TfMat()
Construct an identity transformation matrix.
Definition: tf.hpp:893
TfMat(const struct aa_tf_tfmat *T)
Construct a transformation matrix from another transformation matrix.
Definition: tf.hpp:898
AA_API void aa_tf_rotmat2axang(const double R[AA_RESTRICT 9], double ra[AA_RESTRICT 4])
convert rotation matrix to axis angle
DualQuat(const struct aa_tf_qv &S)
Construct from a quaternion-vector.
Definition: tf.hpp:614
AA_API void aa_tf_zangle2rotmat(double theta_z, double R[AA_RESTRICT 9])
Angle about z axis.
double y
y component
Definition: tf.h:162
AA_API void aa_tf_yangle2quat(double theta_y, double q[AA_RESTRICT 4])
Unit quaternion for angle about y axis.
RotMat(const YAngle &p)
Construct a rotation matrix from a rotation about Y.
Definition: tf.hpp:358
AA_API void aa_tf_qv2duqu(const double q[AA_RESTRICT 4], const double v[AA_RESTRICT 3], double d[AA_RESTRICT 8])
Convert orientation unit quaternion and translation vector to dual quaternion.
DualQuat(const struct aa_tf_duqu &S)
Construct from another dual quaternion.
Definition: tf.hpp:604
Memory layout for a rotation matrix.
Definition: tf.h:174
AA_API void aa_tf_rotvec2rotmat(const double rv[AA_RESTRICT 3], double R[AA_RESTRICT 9])
convert rotatoin vector to rotation matrix
AA_API void aa_tf_yangle2rotmat(double theta_y, double R[AA_RESTRICT 9])
Angle about y axis.
static aa_tf_qv from_tfmat(const double t[12])
Create a quaternion-translation from a transformation matrix.
Definition: tf.hpp:879
DualQuat(const struct aa_tf_qv *S)
Construct from a quaternion-vector.
Definition: tf.hpp:609
double z
z component
Definition: tf.h:222
A rotation matrix object.
Definition: tf.hpp:313
DualQuat(const ZAngle &r, const struct aa_tf_vec3 &v)
Construct from a rotation about Z and a translation vector.
Definition: tf.hpp:653
double z
z component
Definition: tf.h:163
AxisAngle(double x, double y, double z, double theta)
Construct an axis-angle from individual components.
Definition: tf.hpp:508
AA_API void aa_tf_zxyz2duqu(double theta, double x, double y, double z, double d[AA_RESTRICT 8])
Convert z angle and translation to dual quaternion.
AxisAngle(const aa_tf_rotmat &p)
Construct an axis-angle from a rotation matrix.
Definition: tf.hpp:493
AxisAngle(const aa_tf_axang *p)
Construct an axis-angle from another axis-angle.
Definition: tf.hpp:498
Quat(const YAngle &p)
Construct from a rotation about the Y axis.
Definition: tf.hpp:218
amino namespace
Definition: amino.hpp:60
AA_API void aa_tf_rotvec2quat(const double rotvec[AA_RESTRICT 3], double q[AA_RESTRICT 4])
covert rotation vector to quaternion
static aa_tf_duqu from_xxyz(double theta, double x, double y, double z)
Create a dual quaternion an X axis rotation and translation components.
Definition: tf.hpp:728
Memory layout for a quaternion, x,y,z,w order.
Definition: tf.h:217
XAngle(double v)
Construct with the given angle.
Definition: tf.hpp:148
static aa_tf_rotmat from_axang(const double x[4])
Create a rotation matrix from an axis-angle.
Definition: tf.hpp:421
static aa_tf_tfmat from_duqu(const double s[8])
Construct a transformation matrix from a dual quaternion.
Definition: tf.hpp:942
QuatTran(const struct aa_tf_tfmat *T)
Construct from a transformation matrix.
Definition: tf.hpp:820
static aa_tf_axang from_quat(const double x[4])
Create an axis angle from a unit quaternion.
Definition: tf.hpp:522
static aa_tf_quat from_quat(const double x[4])
Create a quaternion object from a quaternion array.
Definition: tf.hpp:229
double data[12]
data array
Definition: tf.h:268
Quat(const ZAngle &p)
Construct from a rotation about the Z axis.
Definition: tf.hpp:223
AA_API void aa_tf_xangle2quat(double theta_x, double q[AA_RESTRICT 4])
Unit quaternion for angle about x axis.
AxisAngle(const aa_tf_rotmat *p)
Construct an axis-angle from a rotation matrix.
Definition: tf.hpp:488
RotMat(double r11, double r12, double r13, double r21, double r22, double r23, double r31, double r32, double r33)
Construct a rotation matrix from individual components.
Definition: tf.hpp:368
static aa_tf_rotmat from_yangle(const double v)
Create a rotation matrix from a rotation about Y.
Definition: tf.hpp:448
Memory layout for a dual quaternion .
Definition: tf.h:315
Quat(const aa_tf_rotmat *p)
Construct from a rotation matrix.
Definition: tf.hpp:193
DualQuat(const struct aa_tf_quat *r, const struct aa_tf_vec3 *v)
Construct from a quaternion-vector.
Definition: tf.hpp:619
static aa_tf_qv from_qv(const double e[7])
Create a quaternion-translation from a rotation quaternion and a translation vector.
Definition: tf.hpp:862
AA_API void aa_tf_xangle2rotmat(double theta_x, double R[AA_RESTRICT 9])
Angle about x axis.
AA_API void aa_tf_xxyz2duqu(double theta, double x, double y, double z, double d[AA_RESTRICT 8])
Convert x angle and translation to dual quaternion.
static aa_tf_tfmat from_qv(const double q[4], const double v[3])
Construct a transformation matrix from a rotation quaternion and translation vector.
Definition: tf.hpp:951
static aa_tf_qv from_qv(const double a_r[4], const double a_v[3])
Create a quaternion-translation from a rotation quaternion and a translation vector.
Definition: tf.hpp:851
Vec3(double _x, double _y, double _z)
Construct from individual components.
Definition: tf.hpp:92
static aa_tf_vec3 from_duqu(const double S[8])
Create a Vec3 from translation part of unit dual quation.
Definition: tf.hpp:123
DualQuat(const struct aa_tf_tfmat &T)
Construct from a transformation matrix.
Definition: tf.hpp:638
static aa_tf_rotmat from_rotvec(const double x[3])
Create a rotation matrix from a rotation vector.
Definition: tf.hpp:430
static aa_tf_axang from_rotmat(const double x[9])
Create an axis angle from a rotation matrix.
Definition: tf.hpp:531
static aa_tf_duqu from_yxyz(double theta, double x, double y, double z)
Create a dual quaternion an Y axis rotation and translation components.
Definition: tf.hpp:737
DualQuat conj()
Return the conjugate of this.
Definition: tf.hpp:671
RotMat(const XAngle &p)
Construct a rotation matrix from a rotation about X.
Definition: tf.hpp:353
AA_API void aa_tf_duqu_conj(const double d[AA_RESTRICT 8], double dconj[AA_RESTRICT 8])
Dual quaternion conjugate.
static aa_tf_quat from_yangle(const double v)
Create a quaternion object rotation about y.
Definition: tf.hpp:294
static aa_tf_axang from_axang(const double x[4])
Create an axis angle from an axis angle array.
Definition: tf.hpp:549
TfMat(const struct aa_tf_qv &S)
Construct a transformation matrix from a quaternion-translation.
Definition: tf.hpp:923
AA_API void aa_tf_axang2quat(const double axang[AA_RESTRICT 4], double q[AA_RESTRICT 4])
axis-angle to quaternion.
static aa_tf_vec3 from_xyz(double x, double y, double z)
Create a Vec3 from components.
Definition: tf.hpp:103
RotMat(const aa_tf_axang *p)
Construct a rotation matrix from an axis-angle.
Definition: tf.hpp:343
A rotation about the X axis.
Definition: tf.hpp:144
RotMat()
Construct an identity rotation matrix.
Definition: tf.hpp:317
static aa_tf_qv from_xyzw(double q_x, double q_y, double q_z, double q_w, double t_x, double t_y, double t_z)
Create a quaternion-translation from components.
Definition: tf.hpp:839
AA_API void aa_tf_zangle2quat(double theta_z, double q[AA_RESTRICT 4])
Unit quaternion for angle about z axis.
Quat(const aa_tf_quat *p)
Construct from another quaternion.
Definition: tf.hpp:183
Axis-Angle rotation.
Definition: tf.h:188
aa_tf_vec3 translation()
Return the translation part of a unit dual quaternion.
Definition: tf.hpp:681
Vec3(const struct aa_tf_duqu *S)
Extract the translation part of a unit dual quaternion.
Definition: tf.hpp:72
AxisAngle(const aa_tf_quat &p)
Construct an axis-angle from a unit quaternion.
Definition: tf.hpp:483
DualQuat(const XAngle &r, const struct aa_tf_vec3 &v)
Construct from a rotation about X and a translation vector.
Definition: tf.hpp:643
double w
w component
Definition: tf.h:223
Quat()
Construct an identity quaternion.
Definition: tf.hpp:178
AA_API void aa_tf_quat2axang(const double q[AA_RESTRICT 4], double axang[AA_RESTRICT 4])
Quaternion to axis-angle.
AA_API void aa_tf_12chain(const double T1[AA_RESTRICT 12], const double T2[AA_RESTRICT 12], double T[AA_RESTRICT 12])
chain two transforms
Quat(const aa_tf_quat &p)
Construct from another quaternion.
Definition: tf.hpp:188
AA_API void aa_tf_duqu_trans(const double d[AA_RESTRICT 8], double v[AA_RESTRICT 3])
Extract dual quaternion translation vector.
QuatTran(const struct aa_tf_duqu &S)
Construct from a dual quaternion.
Definition: tf.hpp:815
static aa_tf_quat from_xangle(const double v)
Create a quaternion object rotation about x.
Definition: tf.hpp:285
A rotation quaternion and translation vector object.
Definition: tf.hpp:776
AA_API void aa_tf_qv2tfmat(const double q[AA_RESTRICT 4], const double v[AA_RESTRICT 3], double T[AA_RESTRICT 12])
Convert orientation unit quaternion and translation vector to transformation matrix.
Memory layout for a Transformation as rotation quaternion and translation vector. ...
Definition: tf.h:300
static aa_tf_rotmat from_rotmat(const double x[9])
Create a rotation matrix object from a rotation matrix array.
Definition: tf.hpp:412
DualQuat(const struct aa_tf_tfmat *T)
Construct from a transformation matrix.
Definition: tf.hpp:633
DualQuat(const aa_tf_axang &r, const struct aa_tf_vec3 &v)
Construct from an axis-angle rotation and a translation vector.
Definition: tf.hpp:663
A rotation about the Z axis.
Definition: tf.hpp:164
DualQuat(const struct aa_tf_quat &r, const struct aa_tf_vec3 &v)
Construct from a quaternion-vector.
Definition: tf.hpp:626
AA_API void aa_tf_qv_chain(const double q0[AA_RESTRICT 4], const double v0[AA_RESTRICT 3], const double q1[AA_RESTRICT 4], const double v1[AA_RESTRICT 3], double q[AA_RESTRICT 4], double v[AA_RESTRICT 3])
chain two transforms
RotMat(const aa_tf_rotmat &p)
Construct a rotation matrix from another rotation matrix.
Definition: tf.hpp:338
static aa_tf_quat from_axang(const double a[3], double angle)
Create a quaternion object an axis-angle.
Definition: tf.hpp:268
static aa_tf_quat from_xyzw(double x, double y, double z, double w)
Create a quaternion object from components.
Definition: tf.hpp:238
DualQuat(const struct aa_tf_vec3 &v)
Construct from an identity rotation and a translation vector.
Definition: tf.hpp:658
static aa_tf_duqu from_tfmat(const double T[12])
Create a dual quaternion from a transformation matrix.
Definition: tf.hpp:719
AA_API void aa_tf_duqu2qv(const double d[AA_RESTRICT 8], double q[AA_RESTRICT 4], double v[AA_RESTRICT 3])
Convert dual quaternion to orientation unit quaternion and translation vector.
AA_API void aa_tf_rotvec2axang(const double rotvec[AA_RESTRICT 3], double axang[AA_RESTRICT 4])
convert rotation vector to axis-angle
AA_API void aa_tf_rotmat2quat(const double rotmat[AA_RESTRICT 9], double quat[AA_RESTRICT 4])
convert rotation matrix to quaternion
double data[9]
data array
Definition: tf.h:181
ZAngle(double v)
Construct with the given angle.
Definition: tf.hpp:168
static aa_tf_axang from_axang(const double axis[3], double angle)
Create an axis angle from an axis array and an angle.
Definition: tf.hpp:558
static aa_tf_rotmat from_zangle(const double v)
Create a rotation matrix from a rotation about Z.
Definition: tf.hpp:457