amino  1.0-beta2
Lightweight Robot Utility Library
tf.h
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) 2010-2014, Georgia Tech Research Corporation
5  * Copyright (c) 2015, Rice University
6  * All rights reserved.
7  *
8  * Author(s): Neil T. Dantam <ntd@gatech.edu>
9  * Georgia Tech Humanoid Robotics Lab
10  * Under Direction of Prof. Mike Stilman <mstilman@cc.gatech.edu>
11  *
12  *
13  * This file is provided under the following "BSD-style" License:
14  *
15  *
16  * Redistribution and use in source and binary forms, with or
17  * without modification, are permitted provided that the following
18  * conditions are met:
19  *
20  * * Redistributions of source code must retain the above copyright
21  * notice, this list of conditions and the following disclaimer.
22  *
23  * * Redistributions in binary form must reproduce the above
24  * copyright notice, this list of conditions and the following
25  * disclaimer in the documentation and/or other materials provided
26  * with the distribution.
27  *
28  * * Neither the name of copyright holder the names of its
29  * contributors may be used to endorse or promote products derived
30  * from this software without specific prior written permission.
31  *
32  * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND
33  * CONTRIBUTORS "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES,
34  * INCLUDING, BUT NOT LIMITED TO, THE IMPLIED WARRANTIES OF
35  * MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
36  * DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR
37  * CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL,
38  * SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT
39  * LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF
40  * USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
41  * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
42  * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
43  * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
44  * POSSIBILITY OF SUCH DAMAGE.
45  *
46  */
47 
48 #ifndef AMINO_TF_H
49 #define AMINO_TF_H
50 
60 #include <float.h>
61 
62 #ifdef __cplusplus
63 extern "C" {
64 #endif
65 
66 /**********/
67 /* Series */
68 /**********/
69 
78 #define AA_TF_DEF_SERIES(name, a0, a1, a2) \
79  static inline double \
80  aa_tf_ ## name ##_series2(double theta2) \
81  { \
82  return aa_horner3( theta2, a0, a1, a2 ); \
83  } \
84  static inline double \
85  aa_tf_ ## name ## _series(double theta) \
86  { \
87  return aa_tf_ ## name ## _series2(theta*theta); \
88  } \
89 
90 AA_TF_DEF_SERIES( sinc, 1., -1./6, 1./120 )
91 AA_TF_DEF_SERIES( cos, 1., -1./2, 1./24 )
92 AA_TF_DEF_SERIES( invsinc, 1., 1./6, 7./360 )
93 
101 static inline void
102 aa_tf_sinccos2( double theta2, double *sc, double *c )
103 {
104  if( theta2 < sqrt(DBL_EPSILON) ) {
105  *sc = aa_tf_sinc_series2(theta2);
106  *c = aa_tf_cos_series2(theta2);
107  } else {
108  double theta = sqrt(theta2);
109  double s = sin(theta);
110  *c = cos(theta);
111  *sc = s / theta;
112  }
113 }
114 
118 static inline void
119 aa_tf_sinccos( double theta, double *sc, double *c )
120 {
121  if( theta*theta < sqrt(DBL_EPSILON) ) {
122  *sc = aa_tf_sinc_series(theta);
123  *c = aa_tf_cos_series(theta);
124  } else {
125  double s = sin(theta);
126  *c = cos(theta);
127  *sc = s / theta;
128  }
129 }
130 
134 static inline double
135 aa_tf_sinc( double theta )
136 {
137  if( fabs(theta) < sqrt(sqrt(DBL_EPSILON)) ) {
138  return aa_tf_sinc_series(theta);
139  } else {
140  return sin(theta)/theta;
141  }
142 }
143 
144 /*********/
145 /* Types */
146 /*********/
147 
149 #define AA_TF_X 0
150 #define AA_TF_Y 1
152 #define AA_TF_Z 2
154 
158 typedef struct aa_tf_vec3 {
159  union {
160  struct {
161  double x;
162  double y;
163  double z;
164  };
165  double data[3];
166  };
167 } aa_tf_vec3_t;
168 
174 typedef struct aa_tf_rotmat {
175  union {
176  struct {
177  struct aa_tf_vec3 col0;
178  struct aa_tf_vec3 col1;
179  struct aa_tf_vec3 col2;
180  };
181  double data[9];
182  };
184 
188 typedef struct aa_tf_axang {
189  union {
190  struct {
191  union {
192  struct aa_tf_vec3 axis;
193  double v[3];
194  };
195  double angle;
196  };
197  double data[4];
198  };
199 } aa_tf_axang_t;
200 
202 #define AA_TF_QUAT_V 0
203 #define AA_TF_QUAT_XYZ AA_TF_QUAT_V
205 #define AA_TF_QUAT_X (AA_TF_QUAT_V + AA_TF_X)
207 #define AA_TF_QUAT_Y (AA_TF_QUAT_V + AA_TF_Y)
209 #define AA_TF_QUAT_Z (AA_TF_QUAT_V + AA_TF_Z)
211 #define AA_TF_QUAT_W 3
213 
217 typedef struct aa_tf_quat {
218  union {
219  struct {
220  double x;
221  double y;
222  double z;
223  double w;
224  };
225  struct {
226  union {
227  struct aa_tf_vec3 vec;
228  double v[3];
229  };
230  double scalar;
231  };
232  double data[4];
233  };
234 } aa_tf_quat_t;
235 
239 typedef struct aa_tf_eulerzyx {
240  union {
241  struct {
242  double y;
243  double p;
244  double r;
245  };
246  double data[3];
247  };
249 
250 
262 typedef struct aa_tf_tfmat {
263  union {
264  struct {
265  double R[9];
267  };
268  double data[12];
269  };
270 } aa_tf_tfmat_t;
271 
275 #define AA_TF_TFMAT_R 0
276 
280 #define AA_TF_TFMAT_V 9
281 
285 #define AA_TF_TFMAT_X 9
286 
290 #define AA_TF_TFMAT_Y 10
291 
295 #define AA_TF_TFMAT_Z 11
296 
300 typedef struct aa_tf_qv {
301  union {
302  struct {
303  aa_tf_quat_t r;
304  aa_tf_vec3_t v;
305  };
306  double data[7];
307  };
308 } aa_tf_qv_t;
309 
310 
311 
315 typedef struct aa_tf_duqu {
316  union {
317  struct {
318  aa_tf_quat_t real;
319  aa_tf_quat_t dual;
320  };
321  double data[8];
322  };
323 } aa_tf_duqu_t;
324 
328 struct aa_tf_dx {
329  union {
330  struct {
331  double dv[3];
332  double omega[3];
333  };
334  double data[6];
335  };
336 };
337 
339 #define AA_TF_DX_V 0
340 #define AA_TF_DX_W 3
342 
344 struct aa_tf_qv_dx {
345  union {
346  struct {
347  struct aa_tf_qv tf;
348  struct aa_tf_dx dx;
349  };
350  double data[13];
351  };
352 };
353 
355 #define AA_TF_QUTR_Q 0
356 #define AA_TF_QUTR_QX (AA_TF_QUTR_Q + AA_TF_QUAT_X)
358 #define AA_TF_QUTR_QY (AA_TF_QUTR_Q + AA_TF_QUAT_Y)
360 #define AA_TF_QUTR_QZ (AA_TF_QUTR_Q + AA_TF_QUAT_Z)
362 #define AA_TF_QUTR_QW (AA_TF_QUTR_Q + AA_TF_QUAT_W)
364 
366 #define AA_TF_QUTR_T 4
367 #define AA_TF_QUTR_TX (AA_TF_QUTR_T + AA_TF_X)
369 #define AA_TF_QUTR_TY (AA_TF_QUTR_T + AA_TF_Y)
371 #define AA_TF_QUTR_TZ (AA_TF_QUTR_T + AA_TF_Z)
373 
375 #define AA_TF_DUQU_REAL 0
376 #define AA_TF_DUQU_DUAL 4
378 
380 #define AA_TF_DUQU_REAL_W (AA_TF_DUQU_REAL + AA_TF_QUAT_W)
381 #define AA_TF_DUQU_REAL_XYZ (AA_TF_DUQU_REAL + AA_TF_QUAT_XYZ)
383 #define AA_TF_DUQU_REAL_X (AA_TF_DUQU_REAL + AA_TF_QUAT_X)
385 #define AA_TF_DUQU_REAL_Y (AA_TF_DUQU_REAL + AA_TF_QUAT_Y)
387 #define AA_TF_DUQU_REAL_Z (AA_TF_DUQU_REAL + AA_TF_QUAT_Z)
389 
391 #define AA_TF_DUQU_DUAL_W (AA_TF_DUQU_DUAL + AA_TF_QUAT_W)
392 #define AA_TF_DUQU_DUAL_XYZ (AA_TF_DUQU_DUAL + AA_TF_QUAT_XYZ)
394 #define AA_TF_DUQU_DUAL_X (AA_TF_DUQU_DUAL + AA_TF_QUAT_X)
396 #define AA_TF_DUQU_DUAL_Y (AA_TF_DUQU_DUAL + AA_TF_QUAT_Y)
398 #define AA_TF_DUQU_DUAL_Z (AA_TF_DUQU_DUAL + AA_TF_QUAT_Z)
400 
402 #define AA_TF_EPSILON .0001
403 
404 
405 
409 #define AA_TF_IDENT_INITIALIZER {1,0,0, 0,1,0, 0,0,1, 0,0,0}
410 
414 #define AA_TF_ROTMAT_IDENT_INITIALIZER {1,0,0, 0,1,0, 0,0,1}
415 
419 #define AA_TF_QUAT_IDENT_INITIALIZER {0,0,0,1}
420 
424 #define AA_TF_DUQU_IDENT_INITIALIZER {0,0,0,1, 0,0,0,0}
425 
429 #define AA_TF_QUTR_IDENT_INITIALIZER {0,0,0,1, 0,0,0}
430 
434 #define AA_TF_AXANG_IDENT_INITIALIZER {1,0,0,0}
435 
439 #define AA_TF_ROTVEC_IDENT_INITIALIZER {0,0,0}
440 
444 #define AA_TF_VEC_IDENT_INITIALIZER {0,0,0}
445 
447 #define AA_TF_IDENT ( (double[12]) AA_TF_IDENT_INITIALIZER )
448 #define AA_TF_ROTMAT_IDENT ( (double[9] AA_TF_ROTMAT_IDENT_INITIALIZER )
450 #define AA_TF_QUAT_IDENT ( (double[4]) AA_TF_QUAT_IDENT_INITIALIZER )
452 #define AA_TF_AXANG_IDENT ( (double[4]) AA_TF_AXANG_IDENT_INITIALIZER )
454 #define AA_TF_ROTVEC_IDENT ( (double[3]) AA_TF_ROTVEC_IDENT_INITIALIZER )
456 
460 static const double aa_tf_ident[12] = AA_TF_IDENT_INITIALIZER;
461 
465 static const double aa_tf_tfmat_ident[12] = AA_TF_IDENT_INITIALIZER;
466 
471 
476 
481 
486 
491 
496 
501 
505 static const double aa_tf_vec_x[3] = {1, 0, 0};
506 
510 static const double aa_tf_vec_y[3] = {0, 1, 0};
511 
515 static const double aa_tf_vec_z[3] = {0, 0, 1};
516 
517 /**************/
518 /* Transforms */
519 /**************/
520 
522 AA_API void aa_tf_12( const double T[AA_RESTRICT 12],
523  const double p0[AA_RESTRICT 3],
524  double p1[AA_RESTRICT 3] );
526 AA_API void aa_tf_93( const double R[AA_RESTRICT 9],
527  const double v[AA_RESTRICT 3],
528  const double p0[AA_RESTRICT 3],
529  double p1[AA_RESTRICT 4] );
531 AA_API void aa_tf_tf_qv( const double quat[AA_RESTRICT 4],
532  const double v[AA_RESTRICT 3],
533  const double p0[AA_RESTRICT 3],
534  double p1[AA_RESTRICT 4] );
535 
537 AA_API void aa_tf_9( const double R[AA_RESTRICT 9],
538  const double p0[AA_RESTRICT 3],
539  double p1[AA_RESTRICT 4] );
540 
542 AA_API void aa_tf_12inv( const double T[AA_RESTRICT 12],
543  double Ti[AA_RESTRICT 12] );
545 AA_API void aa_tf_93inv( const double R[AA_RESTRICT 9],
546  const double v[AA_RESTRICT 3],
547  double Ri[AA_RESTRICT 9], double vi[AA_RESTRICT 3] );
553 AA_API void aa_tf_qv_conj( const double q[AA_RESTRICT 4], const double v[AA_RESTRICT 3],
554  double qc[AA_RESTRICT 4], double vc[AA_RESTRICT 3] );
555 
557 AA_API void aa_tf_12chain( const double T1[AA_RESTRICT 12],
558  const double T2[AA_RESTRICT 12],
559  double T[AA_RESTRICT 12] );
560 
562 AA_API void aa_tf_v12chain( double T[AA_RESTRICT 12] ,
563  const double T1[AA_RESTRICT 12],
564  const double T2[AA_RESTRICT 12],
565  ... );
566 
568 AA_API void aa_tf_93chain( const double R0[AA_RESTRICT 9],
569  const double v0[AA_RESTRICT 3],
570  const double R1[AA_RESTRICT 9],
571  const double v1[AA_RESTRICT 3],
572  double R[AA_RESTRICT 9], double v[AA_RESTRICT 3] );
574 AA_API void aa_tf_qv_chain( const double q0[AA_RESTRICT 4],
575  const double v0[AA_RESTRICT 3],
576  const double q1[AA_RESTRICT 4],
577  const double v1[AA_RESTRICT 3],
578  double q[AA_RESTRICT 4],
579  double v[AA_RESTRICT 3] );
580 
582 AA_API void aa_tf_93rel( const double R1[AA_RESTRICT 9],
583  const double v1[AA_RESTRICT 3],
584  const double R2[AA_RESTRICT 9],
585  const double v2[AA_RESTRICT 3],
586  double Rrel[AA_RESTRICT 9],
587  double vrel[AA_RESTRICT 3] );
588 
590 AA_API void aa_tf_12rel( const double T1[AA_RESTRICT 12],
591  const double T2[AA_RESTRICT 12],
592  double Trel[AA_RESTRICT 12] );
593 
594 /************/
595 /* Matrices */
596 /************/
597 
615 AA_API void aa_tf_skewsym_scal2( double a, double b, const double u[3], double R[9] );
616 
620 AA_API void
621 aa_tf_skewsym_scal_c( const double u[AA_RESTRICT 3],
622  const double a[AA_RESTRICT 3], const double b[AA_RESTRICT 3],
623  double R[9] );
624 
625 
626 /* Multiply */
627 
631 AA_API void aa_tf_rotmat_mul( const double R0[AA_RESTRICT 9],
632  const double R1[AA_RESTRICT 9],
633  double R[AA_RESTRICT 9] );
634 
638 AA_API void aa_tf_tfmat_mul( const double T0[AA_RESTRICT 12],
639  const double T1[AA_RESTRICT 12],
640  double T[AA_RESTRICT 12] );
641 
645 AA_API void aa_tf_tfmat2_mul( const double R0[AA_RESTRICT 9],
646  const double v0[AA_RESTRICT 3],
647  const double R1[AA_RESTRICT 9],
648  const double v1[AA_RESTRICT 3],
649  double R[AA_RESTRICT 9], double v[AA_RESTRICT 3] );
650 
651 
652 /* Inverting Multiply */
658 AA_API void aa_tf_rotmat_imul( const double R0[AA_RESTRICT 9],
659  const double R1[AA_RESTRICT 9],
660  double R[AA_RESTRICT 9] );
661 
667 AA_API void aa_tf_rotmat_muli( const double R0[AA_RESTRICT 9],
668  const double R1[AA_RESTRICT 9],
669  double R[AA_RESTRICT 9] );
670 
676 AA_API void aa_tf_tfmat_imul( const double T0[AA_RESTRICT 12],
677  const double T1[AA_RESTRICT 12],
678  double T[AA_RESTRICT 12] );
679 
685 AA_API void aa_tf_tfmat_muli( const double T0[AA_RESTRICT 12],
686  const double T1[AA_RESTRICT 12],
687  double T[AA_RESTRICT 12] );
688 
694 AA_API void aa_tf_tfmat2_imul( const double R0[AA_RESTRICT 9],
695  const double v0[AA_RESTRICT 3],
696  const double R1[AA_RESTRICT 9],
697  const double v1[AA_RESTRICT 3],
698  double R[AA_RESTRICT 9], double v[AA_RESTRICT 3] );
699 
705 AA_API void aa_tf_tfmat2_muli( const double R0[AA_RESTRICT 9],
706  const double v0[AA_RESTRICT 3],
707  const double R1[AA_RESTRICT 9],
708  const double v1[AA_RESTRICT 3],
709  double R[AA_RESTRICT 9], double v[AA_RESTRICT 3] );
710 
711 
715 AA_API void
716 aa_tf_rotmat_normalize( double R[AA_RESTRICT 9] );
717 
718 /* Transform */
719 
723 AA_API void aa_tf_rotmat_rot( const double R[AA_RESTRICT 9],
724  const double p0[AA_RESTRICT 3],
725  double p1[AA_RESTRICT 3] );
726 
730 AA_API void aa_tf_tfmat_tf( const double T[AA_RESTRICT 12],
731  const double p0[AA_RESTRICT 3],
732  double p1[AA_RESTRICT 3] );
733 
737 AA_API void aa_tf_tfmat2_tf( const double R[AA_RESTRICT 9],
738  const double v[AA_RESTRICT 3],
739  const double p0[AA_RESTRICT 3],
740  double p1[AA_RESTRICT 4] );
741 
745 AA_API void aa_tf_rotmat_inv1( double R[AA_RESTRICT 9] );
746 
750 AA_API void aa_tf_rotmat_inv2( const double R[AA_RESTRICT 9],
751  double Ri[AA_RESTRICT 9] );
752 
756 AA_API void aa_tf_tfmat_inv1( double T[AA_RESTRICT 12] );
757 
761 AA_API void aa_tf_tfmat_inv2( const double T[AA_RESTRICT 12],
762  double Ti[AA_RESTRICT 12] );
763 
764 
768 AA_API void
769 aa_tf_tfmat_normalize( double T[AA_RESTRICT 12] );
770 
772 AA_API int aa_tf_isrotmat( const double R[AA_RESTRICT 9] );
773 
774 
776 AA_API void aa_tf_9mul( const double R0[AA_RESTRICT 9],
777  const double R1[AA_RESTRICT 9],
778  double R[AA_RESTRICT 9] );
780 AA_API void aa_tf_9rot( const double R[AA_RESTRICT 9],
781  const double p0[AA_RESTRICT 3],
782  double p1[AA_RESTRICT 3] );
783 
785 AA_API void aa_tf_9rel( const double R1[AA_RESTRICT 9], const double R2[AA_RESTRICT 9],
786  double Ri[AA_RESTRICT 9] );
787 
789 AA_API void aa_tf_rotmat_exp_aa( const double aa[AA_RESTRICT 4], double R[AA_RESTRICT 9] );
790 
792 AA_API void aa_tf_rotmat_expv( const double rv[AA_RESTRICT 3], double R[AA_RESTRICT 9] );
793 
795 AA_API void aa_tf_rotmat_lnv( const double R[AA_RESTRICT 9], double v[AA_RESTRICT 3] );
796 
798 AA_API void aa_tf_tfmat_expv( const double v[AA_RESTRICT 6], double T[AA_RESTRICT 12] );
799 
801 AA_API void aa_tf_tfmat_lnv( const double T[AA_RESTRICT 12], double v[AA_RESTRICT 6] );
802 
804 AA_API void aa_tf_rotmat_vel2diff( const double R[AA_RESTRICT 9],
805  const double w[AA_RESTRICT 3], double dR[AA_RESTRICT 9] );
806 
808 AA_API void aa_tf_rotmat_diff2vel( const double R[AA_RESTRICT 9],
809  const double dR[AA_RESTRICT 9], double w[AA_RESTRICT 3] );
810 
812 AA_API void aa_tf_rotmat_svel( const double R0[AA_RESTRICT 9],
813  const double w[AA_RESTRICT 3], double dt,
814  double R1[AA_RESTRICT 9] );
815 
816 
817 
819 AA_API void aa_tf_tfmat_svel( const double T0[AA_RESTRICT 12],
820  const double w[AA_RESTRICT 3], double dt,
821  double T1[AA_RESTRICT 12] );
822 
824 AA_API void aa_tf_v9mul( double R[AA_RESTRICT 9],
825  const double R1[AA_RESTRICT 9], const double R2[AA_RESTRICT 9], ... );
826 
827 
831 AA_API void
832 aa_tf_rotmat_xy( const double x_axis[AA_RESTRICT 3],
833  const double y_axis[AA_RESTRICT 3],
834  double R[AA_RESTRICT 9] );
838 AA_API void
839 aa_tf_rotmat_yz( const double y_axis[AA_RESTRICT 3],
840  const double z_axis[AA_RESTRICT 3],
841  double R[AA_RESTRICT 9] );
842 
846 AA_API void
847 aa_tf_rotmat_zx( const double z_axis[AA_RESTRICT 3],
848  const double x_axis[AA_RESTRICT 3],
849  double R[AA_RESTRICT 9] );
850 
851 
852 /***********/
853 /* Vectors */
854 /***********/
855 
861 #define AA_TF_DOTX(a,b) \
862  ((a)[0]*(b)[0] + (a)[1]*(b)[1] + (a)[2]*(b)[2])
863 
867 static inline double
868 AA_TF_VDOT( const double a[AA_RESTRICT 3], const double b[AA_RESTRICT 3] )
869 {
870  return AA_TF_DOTX(a,b);
871 }
872 
876 static inline float
877 AA_TF_VDOTF( const float a[AA_RESTRICT 3], const float b[AA_RESTRICT 3] )
878 {
879  return AA_TF_DOTX(a,b);
880 }
881 
882 
886 AA_API double
887 aa_tf_vdot( const double a[AA_RESTRICT 3], const double b[AA_RESTRICT 3] );
888 
892 AA_API float
893 aa_tf_vdotf( const float a[AA_RESTRICT 3], const float b[AA_RESTRICT 3] );
894 
900 #define AA_TF_CROSSX( a, b, c ) \
901  (c)[0] = (a)[1]*(b)[2] - (a)[2]*(b)[1]; \
902  (c)[1] = (a)[2]*(b)[0] - (a)[0]*(b)[2]; \
903  (c)[2] = (a)[0]*(b)[1] - (a)[1]*(b)[0]; \
904 
905 
909 static inline void
910 AA_TF_CROSS( const double a[AA_RESTRICT 3], const double b[AA_RESTRICT 3], double c[AA_RESTRICT 3] )
911 {
912  AA_TF_CROSSX(a,b,c);
913 }
914 
918 static inline void
919 AA_TF_CROSSF( const float a[AA_RESTRICT 3], const float b[AA_RESTRICT 3], float c[AA_RESTRICT 3] )
920 {
921  AA_TF_CROSSX(a,b,c);
922 }
923 
927 AA_API void aa_tf_cross( const double a[AA_RESTRICT 3], const double b[AA_RESTRICT 3],
928  double c[AA_RESTRICT 3] ) ;
929 
930 
934 AA_API void aa_tf_crossf( const float a[AA_RESTRICT 3], const float b[AA_RESTRICT 3],
935  float c[AA_RESTRICT 3] ) ;
936 
940 AA_API void aa_tf_vnormalize( double v[AA_RESTRICT 3] );
941 
942 
946 AA_API void aa_tf_vnormalizef( float v[AA_RESTRICT 3] );
947 
948 
949 /***************/
950 /* Quaternions */
951 /***************/
952 
956 static inline double
957 AA_TF_QDOT( const double a[AA_RESTRICT 4], const double b[AA_RESTRICT 4] )
958 {
959  return a[0]*b[0] + a[1]*b[1] + a[2]*b[2] + a[3]*b[3];
960 }
961 
965 double
966 aa_tf_qdot( const double a[AA_RESTRICT 4], const double b[AA_RESTRICT 4] );
967 
971 AA_API void aa_tf_qnormalize( double q[AA_RESTRICT 4] );
972 
973 
975 AA_API double aa_tf_qnorm( const double q[AA_RESTRICT 4] );
976 
981 AA_API void aa_tf_qminimize( double q[AA_RESTRICT 4] );
982 
987 AA_API void aa_tf_qminimize2( const double q[AA_RESTRICT 4], double qmin[AA_RESTRICT 4] );
988 
991 AA_API void aa_tf_qnormalize2( const double q[AA_RESTRICT 4], double qnorm[AA_RESTRICT 4] );
992 
994 AA_API void aa_tf_qconj( const double q[AA_RESTRICT 4],
995  double r[AA_RESTRICT 4] );
996 
997 
999 AA_API void aa_tf_qexp( const double q[AA_RESTRICT 4],
1000  double r[AA_RESTRICT 4] );
1001 
1002 
1004 AA_API void aa_tf_qln( const double q[AA_RESTRICT 4],
1005  double r[AA_RESTRICT 4] );
1006 
1013 AA_API void aa_tf_qduln( const double q[AA_RESTRICT 4],
1014  const double dq[AA_RESTRICT 4],
1015  double dln[AA_RESTRICT 3] );
1016 
1024 AA_API void aa_tf_qdulnj( const double q[AA_RESTRICT 4],
1025  const double dq[AA_RESTRICT 4],
1026  double dln[AA_RESTRICT 3] );
1027 
1034 AA_API void aa_tf_qdpexp( const double e[AA_RESTRICT 3],
1035  const double de[AA_RESTRICT 3],
1036  double dq[AA_RESTRICT 4] );
1037 
1045 AA_API void aa_tf_qdpexpj( const double e[AA_RESTRICT 3],
1046  const double de[AA_RESTRICT 3],
1047  double dq[AA_RESTRICT 4] );
1048 
1053 double aa_tf_qangle( const double q[AA_RESTRICT 4] );
1054 
1058 double aa_tf_qangle_rel( const double *q, const double *p );
1059 
1062 double aa_tf_quhypangle2
1063 ( const double q[AA_RESTRICT 4], const double p[AA_RESTRICT 4] );
1064 
1066 AA_API void aa_tf_qinv( const double q[AA_RESTRICT 4],
1067  double r[AA_RESTRICT 4] );
1068 
1070 AA_API void aa_tf_qadd( const double a[AA_RESTRICT 4],
1071  const double b[AA_RESTRICT 4],
1072  double c[AA_RESTRICT 4] );
1073 
1075 AA_API void aa_tf_qsub( const double a[AA_RESTRICT 4],
1076  const double b[AA_RESTRICT 4],
1077  double c[AA_RESTRICT 4] );
1078 
1080 AA_API void aa_tf_qmul( const double a[AA_RESTRICT 4],
1081  const double b[AA_RESTRICT 4],
1082  double c[AA_RESTRICT 4] );
1083 
1085 AA_API void aa_tf_qmulnorm( const double a[AA_RESTRICT 4],
1086  const double b[AA_RESTRICT 4],
1087  double c[AA_RESTRICT 4] );
1088 
1090 AA_API void aa_tf_qmul_qv( const double q[AA_RESTRICT 4],
1091  const double v[AA_RESTRICT 3],
1092  double c[AA_RESTRICT 4] );
1093 
1095 AA_API void aa_tf_qmul_vq( const double v[AA_RESTRICT 3],
1096  const double q[AA_RESTRICT 4],
1097  double c[AA_RESTRICT 4] );
1098 
1100 AA_API void aa_tf_qcmul( const double a[AA_RESTRICT 4],
1101  const double b[AA_RESTRICT 4],
1102  double c[AA_RESTRICT 4] );
1103 
1105 AA_API void aa_tf_qmulc( const double a[AA_RESTRICT 4],
1106  const double b[AA_RESTRICT 4],
1107  double c[AA_RESTRICT 4] );
1108 
1109 
1113 AA_API void
1114 aa_tf_qrot1( const double q[AA_RESTRICT 4], double v[AA_RESTRICT 3] );
1115 
1117 AA_API void aa_tf_qrot( const double q[AA_RESTRICT 4],
1118  const double v[AA_RESTRICT 3],
1119  double p[AA_RESTRICT 3] );
1120 
1124 AA_API void aa_tf_qrel(const double q1[AA_RESTRICT 4],
1125  const double q2[AA_RESTRICT 4],
1126  double q_rel[AA_RESTRICT 4]);
1127 
1129 AA_API void aa_tf_qslerp( double tau, const double a[AA_RESTRICT 4],
1130  const double b[AA_RESTRICT 4],
1131  double c[AA_RESTRICT 4] );
1132 
1133 
1135 AA_API void aa_tf_qslerpalg( double tau, const double a[AA_RESTRICT 4],
1136  const double b[AA_RESTRICT 4],
1137  double c[AA_RESTRICT 4] );
1138 
1144 AA_API void aa_tf_qslerpdiff( double tau, const double a[AA_RESTRICT 4],
1145  const double b[AA_RESTRICT 4],
1146  double c[AA_RESTRICT 4] );
1147 
1148 
1151 AA_API void aa_tf_qslerpdiffalg( double tau, const double a[AA_RESTRICT 4],
1152  const double b[AA_RESTRICT 4],
1153  double c[AA_RESTRICT 4] );
1154 
1156 AA_API void aa_tf_qslerpchaindiff( double u, double du,
1157  const double q1[AA_RESTRICT 4], const double dq1[AA_RESTRICT 4],
1158  const double q2[AA_RESTRICT 4], const double dq2[AA_RESTRICT 4],
1159  double q[AA_RESTRICT 4], double dq[AA_RESTRICT 4] );
1160 
1162 AA_API void aa_tf_qslerp3diff( double u12, double du12,
1163  const double q1[AA_RESTRICT 4], const double q2[AA_RESTRICT 4],
1164  double u34, double du34,
1165  const double q3[AA_RESTRICT 4], const double q4[AA_RESTRICT 4],
1166  double u, double du,
1167  double q[AA_RESTRICT 4], double dq[AA_RESTRICT 4] );
1168 
1170 AA_API void aa_tf_qdiff2vel( const double q[AA_RESTRICT 4],
1171  const double dq_dt[AA_RESTRICT 4],
1172  double v[AA_RESTRICT 3] );
1173 
1175 AA_API void aa_tf_qvel2diff( const double q[AA_RESTRICT 4],
1176  const double v[AA_RESTRICT 3],
1177  double dq_dt[AA_RESTRICT 4] );
1178 
1179 
1181 AA_API void aa_tf_rotvec_diff2vel( const double v[3], const double dv[3],
1182  double w[3] );
1183 
1191 AA_API void aa_tf_qrk1( const double q0[AA_RESTRICT 4],
1192  const double dq[AA_RESTRICT 4],
1193  double dt,
1194  double q1[AA_RESTRICT 4] );
1195 
1204 AA_API void aa_tf_qvelrk1( const double q0[AA_RESTRICT 4],
1205  const double v[AA_RESTRICT 3],
1206  double dt,
1207  double q1[AA_RESTRICT 4] );
1208 
1209 
1218 AA_API void aa_tf_qvelrk4( const double q0[AA_RESTRICT 4],
1219  const double v[AA_RESTRICT 3],
1220  double dt,
1221  double q1[AA_RESTRICT 4] );
1222 
1223 
1231 AA_API void aa_tf_qsvel( const double q0[AA_RESTRICT 4],
1232  const double v[AA_RESTRICT 3],
1233  double dt,
1234  double q1[AA_RESTRICT 4] );
1235 
1236 
1244 AA_API void aa_tf_qsdiff( const double q0[AA_RESTRICT 4],
1245  const double dq[AA_RESTRICT 4],
1246  double dt,
1247  double q1[AA_RESTRICT 4] );
1248 
1249 
1250 AA_API void aa_tf_qsacc_rk( const double q0[AA_RESTRICT 4],
1251  const double v[AA_RESTRICT 3],
1252  const double a[AA_RESTRICT 3],
1253  double dt,
1254  double q1[AA_RESTRICT 4] );
1255 
1256 AA_API void aa_tf_qsacc( const double q0[AA_RESTRICT 4],
1257  const double v[AA_RESTRICT 3],
1258  const double a[AA_RESTRICT 3],
1259  double dt,
1260  double q1[AA_RESTRICT 4] );
1261 
1262 
1264 AA_API void aa_tf_xangle2quat( double theta_x, double q[AA_RESTRICT 4] );
1266 AA_API void aa_tf_yangle2quat( double theta_y, double q[AA_RESTRICT 4] );
1268 AA_API void aa_tf_zangle2quat( double theta_z, double q[AA_RESTRICT 4] );
1269 
1274 ( size_t n, const double *w, const double *q, size_t ldqq, double *M );
1275 
1286 ( size_t n, const double *w, const double *Q, size_t ldq, double *y );
1287 
1288 
1289 
1293 AA_API void aa_tf_qmatrix_l( const double *q, double *M, size_t ldm );
1294 
1298 AA_API void aa_tf_qmatrix_r( const double *q, double *M, size_t ldm );
1299 
1300 
1302 void aa_tf_qurand( double q[4] );
1303 
1304 /*********/
1305 /* Axang */
1306 /*********/
1307 
1309 AA_API void aa_tf_axang_make( double x, double y, double z, double theta,
1310  double axang[AA_RESTRICT 4] );
1311 
1314 AA_API void aa_tf_axang_permute( const double rv[AA_RESTRICT 4], int k,
1315  double rv_p[AA_RESTRICT 4] );
1316 
1318 AA_API void aa_tf_rotvec_permute( const double rv[AA_RESTRICT 3], int k,
1319  double rv_p[AA_RESTRICT 3] );
1320 
1323 AA_API void aa_tf_rotvec_near( const double rv[AA_RESTRICT 3],
1324  const double rv_near[AA_RESTRICT 3],
1325  double rv_p[AA_RESTRICT 3] );
1326 
1327 /**************/
1328 /* Quat-Trans */
1329 /**************/
1330 
1332 void aa_tf_qv_conj( const double q[4], const double v[3],
1333  double qc[4], double vc[3] );
1334 
1335 
1339 #define AA_TF_QUTR_Q 0
1340 
1345 #define AA_TF_QUTR_V 4
1346 
1348 void aa_tf_qutr2duqu( const double e[7], double s[8] );
1349 
1351 void aa_tf_qutr2tfmat( const double e[7], double T[12] );
1352 
1354 void aa_tf_tfmat2qutr( const double T[12], double e[7] );
1355 
1357 void aa_tf_duqu2qutr( const double s[8], double e[7] );
1358 
1360 void aa_tf_qutr_mul( const double a[7], const double b[7], double c[7] ) ;
1361 
1365 AA_API void
1366 aa_tf_qutr_tf( const double E[AA_RESTRICT 7], const double p0[AA_RESTRICT 3],
1367  double p1[AA_RESTRICT 3] );
1368 
1370 void aa_tf_qutr_mulnorm( const double a[7], const double b[7], double c[7] ) ;
1371 
1373 void aa_tf_qutr_conj( const double a[7], double c[7] ) ;
1374 
1376 void aa_tf_qutr_mulc( const double a[7], const double b[7], double c[7] ) ;
1377 
1379 void aa_tf_qutr_cmul( const double a[7], const double b[7], double c[7] ) ;
1380 
1384 AA_API void aa_tf_qv_expv
1385 ( const double w[3], const double dv[3],
1386  double q[4], double v[3] );
1387 
1391 AA_API void aa_tf_qv_lnv
1392 ( const double q[4], const double v[3],
1393  double w[3], double dv[3] );
1394 
1399 ( const double w[6], double e[7] );
1400 
1405 ( const double e[7], double w[6] );
1406 
1407 void aa_tf_qutr_twist2vel
1408 ( const double e[7], const double w[6], double dx[6] );
1409 
1412 ( const double e[7], const double de[7], double dx[6] );
1413 
1416 ( const double e[7], const double dx[6], double de[7] );
1417 
1418 
1419 
1420 AA_API void
1421 aa_tf_qv_vel2twist( const double q[AA_RESTRICT 4], const double v[AA_RESTRICT 3],
1422  const double w[AA_RESTRICT 3], const double dv[AA_RESTRICT 3],
1423  double tw[AA_RESTRICT 3], double tv[AA_RESTRICT 3] );
1424 AA_API void
1425 aa_tf_qv_twist2vel( const double q[AA_RESTRICT 4], const double v[AA_RESTRICT 3],
1426  const double tw[AA_RESTRICT 3], const double tv[AA_RESTRICT 3],
1427  double w[AA_RESTRICT 3], double dv[AA_RESTRICT 3] );
1428 
1430 void aa_tf_qutr_svel
1431 ( const double e0[7], const double dx[6], double dt, double e1[7] );
1432 
1434 void aa_tf_qutr_sdiff
1435 ( const double e0[7], const double de[7], double dt, double e1[7] );
1436 
1438 void aa_tf_qutr_wavg
1439 ( size_t n, const double *w, const double *EE, size_t ldee, double *a );
1440 
1442 void aa_tf_qutr_rand( double E[7] );
1443 
1444 /***************/
1445 /* Conversions */
1446 /***************/
1447 
1449 AA_API void aa_tf_quat2axang( const double q[AA_RESTRICT 4],
1450  double axang[AA_RESTRICT 4] );
1451 
1452 
1454 AA_API void aa_tf_axang2quat( const double axang[AA_RESTRICT 4],
1455  double q[AA_RESTRICT 4] );
1456 
1458 AA_API void aa_tf_axang2quat2( const double axis[AA_RESTRICT 3],
1459  double angle,
1460  double q[AA_RESTRICT 4] );
1461 
1462 
1466 AA_API void
1467 aa_tf_vecs2quat( const double u[AA_RESTRICT 3],
1468  const double v[AA_RESTRICT 3],
1469  double q[AA_RESTRICT 4] );
1470 
1471 
1473 AA_API void aa_tf_axang2rotvec( const double axang[AA_RESTRICT 4],
1474  double rotvec[AA_RESTRICT 3] );
1475 
1477 AA_API void aa_tf_rotvec2axang( const double rotvec[AA_RESTRICT 3],
1478  double axang[AA_RESTRICT 4] );
1479 
1481 AA_API void aa_tf_rotvec2quat( const double rotvec[AA_RESTRICT 3],
1482  double q[AA_RESTRICT 4] );
1484 AA_API void aa_tf_quat2rotvec( const double q[AA_RESTRICT 4],
1485  double rotvec[AA_RESTRICT 3] );
1486 
1487 
1489 AA_API void aa_tf_quat2rotvec_near( const double q[AA_RESTRICT 4],
1490  const double rv_near[AA_RESTRICT 3],
1491  double rotvec[AA_RESTRICT 3] );
1492 
1494 AA_API void aa_tf_quat2rotmat( const double quat[AA_RESTRICT 4],
1495  double rotmat[AA_RESTRICT 9] );
1497 AA_API void aa_tf_rotmat2quat( const double rotmat[AA_RESTRICT 9],
1498  double quat[AA_RESTRICT 4] );
1499 
1501 AA_API void aa_tf_rotmat2axang( const double R[AA_RESTRICT 9],
1502  double ra[AA_RESTRICT 4] );
1504 AA_API void aa_tf_rotmat2rotvec( const double R[AA_RESTRICT 9],
1505  double rv[AA_RESTRICT 3] );
1506 
1508 AA_API void aa_tf_axang2rotmat( const double ra[AA_RESTRICT 4],
1509  double R[AA_RESTRICT 9] );
1510 
1512 AA_API void aa_tf_axang2rotmat2( const double axis[AA_RESTRICT 3],
1513  double angle,
1514  double R[AA_RESTRICT 9] );
1515 
1517 AA_API void aa_tf_rotvec2rotmat( const double rv[AA_RESTRICT 3],
1518  double R[AA_RESTRICT 9] );
1519 
1520 
1522 AA_API void aa_tf_qv2tfmat( const double q[AA_RESTRICT 4], const double v[AA_RESTRICT 3],
1523  double T[AA_RESTRICT 12] ) ;
1524 
1525 /* AA_API void aa_tf_tfv2tfq( const double vrv[AA_RESTRICT 6], */
1526 /* double x[AA_RESTRICT 3], double quat[AA_RESTRICT 4] ); */
1527 /* AA_API void aa_tf_tfq2tfv( const double x[AA_RESTRICT 3], const double quat[AA_RESTRICT 4], */
1528 /* double vrv[AA_RESTRICT 6] ); */
1529 
1531 AA_API void aa_tf_rotmat2eulerzyx( const double R[AA_RESTRICT 9],
1532  double e[AA_RESTRICT 3] );
1533 
1534 
1536 AA_API void aa_tf_quat2eulerzyx( const double q[AA_RESTRICT 4],
1537  double e[AA_RESTRICT 3] );
1538 
1542 #define AA_TF_DEF_EULER(letters) \
1543  AA_API void \
1544  aa_tf_euler ## letters ## 2rotmat( double e1, double e2, double e3, \
1545  double R[AA_RESTRICT 9] ); \
1546  AA_API void \
1547  aa_tf_euler ## letters ## 2quat( double e1, double e2, double e3, \
1548  double q[AA_RESTRICT 4] );
1549 
1550 AA_TF_DEF_EULER( xyz )
1551 AA_TF_DEF_EULER( xzy )
1552 
1553 AA_TF_DEF_EULER( yxz )
1554 AA_TF_DEF_EULER( yzx )
1555 
1556 AA_TF_DEF_EULER( zyx )
1557 AA_TF_DEF_EULER( zxy )
1558 
1559 AA_TF_DEF_EULER( xyx )
1560 AA_TF_DEF_EULER( xzx )
1561 
1562 AA_TF_DEF_EULER( yxy )
1563 AA_TF_DEF_EULER( yzy )
1564 
1565 AA_TF_DEF_EULER( zxz )
1566 AA_TF_DEF_EULER( zyz )
1567 
1568 
1569 AA_API void aa_tf_xangle2rotmat( double theta_x, double R[AA_RESTRICT 9] );
1571 AA_API void aa_tf_yangle2rotmat( double theta_y, double R[AA_RESTRICT 9] );
1573 AA_API void aa_tf_zangle2rotmat( double theta_z, double R[AA_RESTRICT 9] );
1574 
1580 AA_API void
1581 aa_tf_rotmat_mzlook( const double eye[AA_RESTRICT 3],
1582  const double target[AA_RESTRICT 3],
1583  const double up[AA_RESTRICT 3],
1584  double R[AA_RESTRICT 9] );
1585 
1586 
1590 AA_API void
1591 aa_tf_tfmat_mzlook( const double eye[AA_RESTRICT 3],
1592  const double target[AA_RESTRICT 3],
1593  const double up[AA_RESTRICT 3],
1594  double T[AA_RESTRICT 12] );
1595 
1599 AA_API void
1600 aa_tf_qmzlook( const double eye[AA_RESTRICT 3],
1601  const double target[AA_RESTRICT 3],
1602  const double up[AA_RESTRICT 3],
1603  double q[AA_RESTRICT 4] );
1604 
1608 AA_API void
1609 aa_tf_qv_mzlook( const double eye[AA_RESTRICT 3],
1610  const double target[AA_RESTRICT 3],
1611  const double up[AA_RESTRICT 3],
1612  double q[AA_RESTRICT 4],
1613  double v[AA_RESTRICT 3] );
1614 
1618 AA_API void
1619 aa_tf_qutr_mzlook( const double eye[AA_RESTRICT 3],
1620  const double target[AA_RESTRICT 3],
1621  const double up[AA_RESTRICT 3],
1622  double T[AA_RESTRICT 12] );
1623 
1624 
1625 /* Dual Quaternions */
1626 
1628 AA_API void aa_tf_duqu_add( const double d1[AA_RESTRICT 8], const double d2[AA_RESTRICT 8],
1629  double d3[AA_RESTRICT 8] );
1630 
1632 AA_API void aa_tf_duqu_sub( const double d1[AA_RESTRICT 8], const double d2[AA_RESTRICT 8],
1633  double d3[AA_RESTRICT 8] );
1634 
1636 AA_API void aa_tf_duqu_smul( const double d1[AA_RESTRICT 8], const double d2[AA_RESTRICT 8],
1637  double d3[AA_RESTRICT 8] );
1638 
1640 AA_API void aa_tf_duqu_mul( const double d1[AA_RESTRICT 8], const double d2[AA_RESTRICT 8],
1641  double d3[AA_RESTRICT 8] );
1642 
1646 AA_API void aa_tf_duqu_matrix_l( const double *q, double *M, size_t ldm );
1647 
1651 AA_API void aa_tf_duqu_matrix_r( const double *q, double *M, size_t ldm );
1652 
1654 AA_API void aa_tf_duqu_cmul( const double d1[AA_RESTRICT 8], const double d2[AA_RESTRICT 8],
1655  double d3[AA_RESTRICT 8] );
1656 
1658 AA_API void aa_tf_duqu_mulc( const double d1[AA_RESTRICT 8], const double d2[AA_RESTRICT 8],
1659  double d3[AA_RESTRICT 8] );
1660 
1662 AA_API void aa_tf_duqu_conj( const double d[AA_RESTRICT 8], double dconj[AA_RESTRICT 8] );
1663 
1665 AA_API void aa_tf_duqu_exp( const double d[AA_RESTRICT 8], double e[AA_RESTRICT 8] );
1666 
1668 AA_API void aa_tf_duqu_ln( const double d[AA_RESTRICT 8], double e[AA_RESTRICT 8] );
1669 
1671 AA_API void aa_tf_duqu_norm( const double d[AA_RESTRICT 8], double *nreal, double *ndual );
1672 
1674 AA_API void aa_tf_duqu_normalize( double d[AA_RESTRICT 8] );
1675 
1677 AA_API void aa_tf_duqu_minimize( double d[AA_RESTRICT 8] );
1678 
1680 AA_API void aa_tf_tf_duqu( const double d[AA_RESTRICT 8], const double p0[AA_RESTRICT 3],
1681  double p1[AA_RESTRICT 3] );
1682 
1684 AA_API void aa_tf_duqu_trans( const double d[AA_RESTRICT 8], double v[AA_RESTRICT 3] );
1685 
1687 AA_API void aa_tf_duqu2tfmat( const double d[AA_RESTRICT 8], double T[AA_RESTRICT 12] );
1688 
1690 AA_API void aa_tf_tfmat2duqu( const double T[AA_RESTRICT 12], double d[AA_RESTRICT 8] ) ;
1691 
1693 AA_API void aa_tf_qv2duqu( const double q[AA_RESTRICT 4], const double v[AA_RESTRICT 3],
1694  double d[AA_RESTRICT 8] ) ;
1695 
1697 AA_API void aa_tf_xyz2duqu( double x, double y, double z,
1698  double d[AA_RESTRICT 8] ) ;
1699 
1701 AA_API void aa_tf_xxyz2duqu( double theta, double x, double y, double z,
1702  double d[AA_RESTRICT 8] );
1703 
1705 AA_API void aa_tf_yxyz2duqu( double theta, double x, double y, double z,
1706  double d[AA_RESTRICT 8] );
1707 
1709 AA_API void aa_tf_zxyz2duqu( double theta, double x, double y, double z,
1710  double d[AA_RESTRICT 8] );
1711 
1713 AA_API void aa_tf_duqu2qv( const double d[AA_RESTRICT 8],
1714  double q[AA_RESTRICT 4], double v[AA_RESTRICT 3] ) ;
1715 
1717 void aa_tf_tfmat2av( const double T[12], double q[AA_RESTRICT 4], double v[AA_RESTRICT 3] ) ;
1718 
1720 AA_API void aa_tf_duqu_vel2twist( const double d[AA_RESTRICT 8], const double dx[AA_RESTRICT 6],
1721  double t[AA_RESTRICT 8] ) ;
1722 
1724 AA_API void aa_tf_duqu_twist2vel( const double d[AA_RESTRICT 8], const double t[AA_RESTRICT 8],
1725  double dx[AA_RESTRICT 6] ) ;
1726 
1728 AA_API void aa_tf_duqu_twist2diff( const double d[AA_RESTRICT 8], const double t[AA_RESTRICT 8],
1729  double dd[AA_RESTRICT 8] ) ;
1730 
1732 AA_API void aa_tf_duqu_vel2diff( const double d[AA_RESTRICT 8], const double dx[AA_RESTRICT 6],
1733  double dd[AA_RESTRICT 8] ) ;
1734 
1736 AA_API void aa_tf_duqu_diff2vel( const double d[AA_RESTRICT 8], const double dd[AA_RESTRICT 8],
1737  double dx[AA_RESTRICT 6] ) ;
1738 
1741 AA_API void aa_tf_duqu_diff2twist( const double d[AA_RESTRICT 8], const double dd[AA_RESTRICT 8],
1742  double twist[AA_RESTRICT 8] ) ;
1743 
1750 AA_API void aa_tf_duqu_stwist( const double d0[AA_RESTRICT 8], const double twist[AA_RESTRICT 8],
1751  double dt, double d1[AA_RESTRICT 6] ) ;
1752 
1759 AA_API void aa_tf_duqu_svel( const double d0[AA_RESTRICT 8], const double dx[AA_RESTRICT 6],
1760  double dt, double d1[AA_RESTRICT 6] ) ;
1761 
1768 AA_API void aa_tf_duqu_sdiff( const double d0[AA_RESTRICT 8], const double dd[AA_RESTRICT 8],
1769  double dt, double d1[AA_RESTRICT 6] ) ;
1770 
1771 
1774 AA_API void
1775 aa_tf_duqu2pure( const double S[AA_RESTRICT 8],
1776  double v[AA_RESTRICT 6] );
1777 
1780 AA_API void
1781 aa_tf_pure2duqu( const double v[AA_RESTRICT 6],
1782  double S[AA_RESTRICT 8]);
1783 
1784 
1785 
1786 /* Misc */
1787 
1788 AA_API void
1789 aa_tf_relx_mean( size_t n, const double *R,
1790  const double *X, size_t ldx,
1791  const double *Y, size_t ldy,
1792  double rel[3]);
1793 
1794 AA_API void
1795 aa_tf_relx_median( size_t n, const double *R,
1796  const double *X, size_t ldx,
1797  const double *Y, size_t ldy,
1798  double rel[3]);
1799 
1809 AA_API
1810 void aa_tf_proj( const double a[AA_RESTRICT 3],
1811  const double b[AA_RESTRICT 3],
1812  double c[AA_RESTRICT 3] );
1813 
1822 AA_API
1823 void aa_tf_proj_orth( const double a[AA_RESTRICT 3],
1824  const double b[AA_RESTRICT 3],
1825  double c[AA_RESTRICT 3] );
1826 
1827 
1828 #ifdef __cplusplus
1829 }
1830 #endif
1831 
1832 #endif //AMINO_TF_H
AA_API void aa_tf_qminimize(double q[AA_RESTRICT 4])
Minimize angle represented by the quaternion.
void aa_tf_qutr_sdiff(const double e0[7], const double de[7], double dt, double e1[7])
Integrate a quaternion-translation.
AA_API void aa_tf_tfmat2_muli(const double R0[AA_RESTRICT 9], const double v0[AA_RESTRICT 3], const double R1[AA_RESTRICT 9], const double v1[AA_RESTRICT 3], double R[AA_RESTRICT 9], double v[AA_RESTRICT 3])
Multiple (R0,v0) by inverse of transform transform (R1,v1).
AA_API void aa_tf_qdpexpj(const double e[AA_RESTRICT 3], const double de[AA_RESTRICT 3], double dq[AA_RESTRICT 4])
Derivative of the Pure Quaternion Exponential, computed via Jacobian.
AA_API void aa_tf_qv_mzlook(const double eye[AA_RESTRICT 3], const double target[AA_RESTRICT 3], const double up[AA_RESTRICT 3], double q[AA_RESTRICT 4], double v[AA_RESTRICT 3])
Find the camera frame, looking in negative z direction.
static double AA_TF_QDOT(const double a[AA_RESTRICT 4], const double b[AA_RESTRICT 4])
Inlined quaternion dot product.
Definition: tf.h:957
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.
AA_API void aa_tf_duqu_twist2vel(const double d[AA_RESTRICT 8], const double t[AA_RESTRICT 8], double dx[AA_RESTRICT 6])
Dual quaternion twist to velocity.
AA_API void aa_tf_tfmat_lnv(const double T[AA_RESTRICT 12], double v[AA_RESTRICT 6])
Transformation Matrix logarithm.
AA_API void aa_tf_qduln(const double q[AA_RESTRICT 4], const double dq[AA_RESTRICT 4], double dln[AA_RESTRICT 3])
Derivative of the Unit Quaternion Logarithm.
static const double aa_tf_rotvec_ident[3]
Identity rotation vector array.
Definition: tf.h:495
static void aa_tf_sinccos2(double theta2, double *sc, double *c)
Compute sinc(theta), cos(theta), given the square of theta.
Definition: tf.h:102
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.
AA_API void aa_tf_tfmat_inv1(double T[AA_RESTRICT 12])
Invert a transform in place.
AA_API void aa_tf_quat2rotvec(const double q[AA_RESTRICT 4], double rotvec[AA_RESTRICT 3])
covert quaternion to rotation vector
AA_API void aa_tf_rotmat_inv1(double R[AA_RESTRICT 9])
Invert a rotation in place.
static const double aa_tf_vec_ident[3]
Identity vector-3 array.
Definition: tf.h:500
AA_API void aa_tf_xyz2duqu(double x, double y, double z, double d[AA_RESTRICT 8])
Pure translation dual quaternion.
AA_API void aa_tf_duqu2tfmat(const double d[AA_RESTRICT 8], double T[AA_RESTRICT 12])
Convert dual quaternion to transformation matrix.
AA_API void aa_tf_9rot(const double R[AA_RESTRICT 9], const double p0[AA_RESTRICT 3], double p1[AA_RESTRICT 3])
rotate p0 by R
#define AA_TF_ROTVEC_IDENT_INITIALIZER
Static initializer for an identity rotation-vector.
Definition: tf.h:439
struct aa_tf_vec3 aa_tf_vec3_t
Memory layout for a vector of length 3.
aa_tf_vec3_t v
the origin vector part
Definition: tf.h:266
void aa_tf_qutr_svel(const double e0[7], const double dx[6], double dt, double e1[7])
Integrate a quaternion-translation.
AA_API void aa_tf_tfmat_mzlook(const double eye[AA_RESTRICT 3], const double target[AA_RESTRICT 3], const double up[AA_RESTRICT 3], double T[AA_RESTRICT 12])
Find the camera frame, looking in negative z direction.
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.
AA_API void aa_tf_vecs2quat(const double u[AA_RESTRICT 3], const double v[AA_RESTRICT 3], double q[AA_RESTRICT 4])
Convert rotation between two vectors to a quaternion.
AA_API float aa_tf_vdotf(const float a[AA_RESTRICT 3], const float b[AA_RESTRICT 3])
Vector dot product.
AA_API void aa_tf_duqu_matrix_l(const double *q, double *M, size_t ldm)
Construct matrix for left dual quaternion multiply q*p = M*p.
double p
pitch
Definition: tf.h:243
void aa_tf_qutr_mul(const double a[7], const double b[7], double c[7])
quaternion-translation multiply
AA_API void aa_tf_qmatrix_l(const double *q, double *M, size_t ldm)
Construct matrix for left quaternion multiply q*p = M*p.
AA_API void aa_tf_qadd(const double a[AA_RESTRICT 4], const double b[AA_RESTRICT 4], double c[AA_RESTRICT 4])
Quaternion addition.
AA_API void aa_tf_qcmul(const double a[AA_RESTRICT 4], const double b[AA_RESTRICT 4], double c[AA_RESTRICT 4])
Quaternion conjugate a and multiply by b.
AA_API void aa_tf_axang_make(double x, double y, double z, double theta, double axang[AA_RESTRICT 4])
copy x,y,z,theta into axang
AA_API void aa_tf_rotvec_diff2vel(const double v[3], const double dv[3], double w[3])
Convert rotation vector derivative to rotational velocity.
Memory layout for a vector of length 3.
Definition: tf.h:158
static const double aa_tf_rotmat_ident[9]
Identity rotation matrix array.
Definition: tf.h:470
AA_API void aa_tf_qminimize2(const double q[AA_RESTRICT 4], double qmin[AA_RESTRICT 4])
Minimize angle represented by the quaternion.
AA_API void aa_tf_93rel(const double R1[AA_RESTRICT 9], const double v1[AA_RESTRICT 3], const double R2[AA_RESTRICT 9], const double v2[AA_RESTRICT 3], double Rrel[AA_RESTRICT 9], double vrel[AA_RESTRICT 3])
relative transform
AA_API void aa_tf_vnormalizef(float v[AA_RESTRICT 3])
Normalize Vector.
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.
#define AA_TF_QUTR_IDENT_INITIALIZER
Static initializer for an identity quaternion-translation.
Definition: tf.h:429
static const double aa_tf_ident[12]
Identity transformation matrix array.
Definition: tf.h:460
AA_API void aa_tf_qmul_qv(const double q[AA_RESTRICT 4], const double v[AA_RESTRICT 3], double c[AA_RESTRICT 4])
Quaternion multiplication.
AA_API void aa_tf_tfmat2duqu(const double T[AA_RESTRICT 12], double d[AA_RESTRICT 8])
Convert transformation matrix to dual quaternion.
AA_API void aa_tf_quat2rotvec_near(const double q[AA_RESTRICT 4], const double rv_near[AA_RESTRICT 3], double rotvec[AA_RESTRICT 3])
covert quaternion to rotation vector minimizing distance from rv_near
AA_API void aa_tf_qv_lnv(const double q[4], const double v[3], double w[3], double dv[3])
Quaternion-vector logarithm.
Memory layout for a transformation matrix.
Definition: tf.h:262
double y
yaw
Definition: tf.h:242
void aa_tf_duqu2qutr(const double s[8], double e[7])
dual quaternion to quaternion-translation
#define AA_TF_IDENT_INITIALIZER
Static initializer for an identity transformation matrix.
Definition: tf.h:409
AA_API void aa_tf_qutr_vel2diff(const double e[7], const double dx[6], double de[7])
Quaternion-translation spatial velocity to derivative.
double x
x component
Definition: tf.h:161
struct aa_tf_duqu aa_tf_duqu_t
Memory layout for a dual quaternion .
AA_API void aa_tf_9rel(const double R1[AA_RESTRICT 9], const double R2[AA_RESTRICT 9], double Ri[AA_RESTRICT 9])
relative transform from R1 to R2
AA_API void aa_tf_rotmat_expv(const double rv[AA_RESTRICT 3], double R[AA_RESTRICT 9])
Rotation Matrix exponential from rotation vector.
AA_API void aa_tf_rotmat_xy(const double x_axis[AA_RESTRICT 3], const double y_axis[AA_RESTRICT 3], double R[AA_RESTRICT 9])
Construct rotation matrix from an x and y axis of the child frame.
AA_API void aa_tf_duqu_diff2vel(const double d[AA_RESTRICT 8], const double dd[AA_RESTRICT 8], double dx[AA_RESTRICT 6])
Dual quaternion derivative to spatial velocity.
AA_API void aa_tf_rotmat_mzlook(const double eye[AA_RESTRICT 3], const double target[AA_RESTRICT 3], const double up[AA_RESTRICT 3], double R[AA_RESTRICT 9])
Find the camera frame, looking in negative z direction.
AA_API void aa_tf_9(const double R[AA_RESTRICT 9], const double p0[AA_RESTRICT 3], double p1[AA_RESTRICT 4])
apply a euclidean transform
AA_API void aa_tf_duqu_matrix_r(const double *q, double *M, size_t ldm)
Construct matrix for right dual quaternion multiply p*q = M*p.
double r
roll
Definition: tf.h:244
AA_API void aa_tf_rotmat_exp_aa(const double aa[AA_RESTRICT 4], double R[AA_RESTRICT 9])
Rotation Matrix exponential from axis angle.
struct aa_tf_quat aa_tf_quat_t
Memory layout for a quaternion, x,y,z,w order.
AA_API void aa_tf_qutr_lnv(const double e[7], double w[6])
Quaternion-vector logarithm.
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.
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
static const double aa_tf_duqu_ident[8]
Identity dual quaternion array.
Definition: tf.h:480
AA_API void aa_tf_qslerp3diff(double u12, double du12, const double q1[AA_RESTRICT 4], const double q2[AA_RESTRICT 4], double u34, double du34, const double q3[AA_RESTRICT 4], const double q4[AA_RESTRICT 4], double u, double du, double q[AA_RESTRICT 4], double dq[AA_RESTRICT 4])
Triad sequence of slerp differentiation.
AA_API void aa_tf_rotmat_imul(const double R0[AA_RESTRICT 9], const double R1[AA_RESTRICT 9], double R[AA_RESTRICT 9])
Multiple inverse of R0 by R1.
AA_API void aa_tf_qnormalize2(const double q[AA_RESTRICT 4], double qnorm[AA_RESTRICT 4])
Normailize quaternion.
AA_API void aa_tf_proj(const double a[AA_RESTRICT 3], const double b[AA_RESTRICT 3], double c[AA_RESTRICT 3])
Vector projection of a onto b.
AA_API void aa_tf_qvelrk1(const double q0[AA_RESTRICT 4], const double v[AA_RESTRICT 3], double dt, double q1[AA_RESTRICT 4])
Integrate unit quaternion from angular velocity, Runge-Kutta-1 (euler) integration.
void aa_tf_qutr2duqu(const double e[7], double s[8])
quaternion-translation to dual quaternion
double x
x component
Definition: tf.h:220
void aa_tf_qurand(double q[4])
Generate random unit quaternion.
AA_API void aa_tf_qutr_diff2vel(const double e[7], const double de[7], double dx[6])
Quaternion-translation derivative to spatial velocity.
AA_API void aa_tf_proj_orth(const double a[AA_RESTRICT 3], const double b[AA_RESTRICT 3], double c[AA_RESTRICT 3])
Orthogonal projection of a onto b.
double aa_tf_quhypangle2(const double q[AA_RESTRICT 4], const double p[AA_RESTRICT 4])
Return the angle between unit quaterniosn in 4D space.
AA_API void aa_tf_qutr_tf(const double E[AA_RESTRICT 7], const double p0[AA_RESTRICT 3], double p1[AA_RESTRICT 3])
Transform a point,.
struct aa_tf_rotmat aa_tf_rotmat_t
Memory layout for a rotation matrix.
AA_API void aa_tf_rotmat2axang(const double R[AA_RESTRICT 9], double ra[AA_RESTRICT 4])
convert rotation matrix to axis angle
AA_API void aa_tf_qutr_mzlook(const double eye[AA_RESTRICT 3], const double target[AA_RESTRICT 3], const double up[AA_RESTRICT 3], double T[AA_RESTRICT 12])
Find the camera frame, looking in negative z direction.
AA_API void aa_tf_qmzlook(const double eye[AA_RESTRICT 3], const double target[AA_RESTRICT 3], const double up[AA_RESTRICT 3], double q[AA_RESTRICT 4])
Find the camera frame, looking in negative z direction.
AA_API void aa_tf_axang_permute(const double rv[AA_RESTRICT 4], int k, double rv_p[AA_RESTRICT 4])
Scales angle by k * 2 * pi.
AA_API void aa_tf_duqu_twist2diff(const double d[AA_RESTRICT 8], const double t[AA_RESTRICT 8], double dd[AA_RESTRICT 8])
Dual quaternion twist to derivative.
AA_API void aa_tf_qdiff2vel(const double q[AA_RESTRICT 4], const double dq_dt[AA_RESTRICT 4], double v[AA_RESTRICT 3])
Quaternaion time derivate to angular velocity.
AA_API void aa_tf_qsvel(const double q0[AA_RESTRICT 4], const double v[AA_RESTRICT 3], double dt, double q1[AA_RESTRICT 4])
Integrate unit quaternion from constant angular velocity.
AA_API void aa_tf_rotvec_permute(const double rv[AA_RESTRICT 3], int k, double rv_p[AA_RESTRICT 3])
find alternate equivalent representations of rv
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.
AA_API void aa_tf_qutr_expv(const double w[6], double e[7])
Quaternion-vector exponential.
double aa_tf_qangle_rel(const double *q, const double *p)
Relative quaternion angles.
AA_API void aa_tf_rotmat_yz(const double y_axis[AA_RESTRICT 3], const double z_axis[AA_RESTRICT 3], double R[AA_RESTRICT 9])
Construct rotation matrix from a y and z axis of the child frame.
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.
AA_API void aa_tf_qv_conj(const double q[AA_RESTRICT 4], const double v[AA_RESTRICT 3], double qc[AA_RESTRICT 4], double vc[AA_RESTRICT 3])
Invert transform.
AA_API void aa_tf_qnormalize(double q[AA_RESTRICT 4])
Normalize Quaternion.
AA_API void aa_tf_qdpexp(const double e[AA_RESTRICT 3], const double de[AA_RESTRICT 3], double dq[AA_RESTRICT 4])
Derivative of the Pure Quaternion Exponential.
void aa_tf_tfmat2qutr(const double T[12], double e[7])
transformation matrix to quaternion-translation
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
struct aa_tf_axang aa_tf_axang_t
Axis-Angle rotation.
AA_API void aa_tf_tfmat2_imul(const double R0[AA_RESTRICT 9], const double v0[AA_RESTRICT 3], const double R1[AA_RESTRICT 9], const double v1[AA_RESTRICT 3], double R[AA_RESTRICT 9], double v[AA_RESTRICT 3])
Multiple inverse of transform (R0,v0) by transform (R1,v1).
AA_API void aa_tf_qvelrk4(const double q0[AA_RESTRICT 4], const double v[AA_RESTRICT 3], double dt, double q1[AA_RESTRICT 4])
Integrate unit quaternion from angular velocity, Runge-Kutta-4 (euler) integration.
double aa_tf_qdot(const double a[AA_RESTRICT 4], const double b[AA_RESTRICT 4])
Quaternion dot product.
AA_API void aa_tf_axang2quat2(const double axis[AA_RESTRICT 3], double angle, double q[AA_RESTRICT 4])
axis-angle to quaternion.
AA_API void aa_tf_yangle2rotmat(double theta_y, double R[AA_RESTRICT 9])
Angle about y axis.
AA_API void aa_tf_tfmat2_tf(const double R[AA_RESTRICT 9], const double v[AA_RESTRICT 3], const double p0[AA_RESTRICT 3], double p1[AA_RESTRICT 4])
Transform a point using a split transformation matrix.
AA_API void aa_tf_qv_expv(const double w[3], const double dv[3], double q[4], double v[3])
Quaternion-vector exponential.
AA_API void aa_tf_axang2rotvec(const double axang[AA_RESTRICT 4], double rotvec[AA_RESTRICT 3])
convert axis-angle to rotation vector
struct aa_tf_eulerzyx aa_tf_eulerzyx_t
Memory layout for an Euler Angle in ZYX (yaw-pitch-roll) format.
void aa_tf_qutr_rand(double E[7])
Generate random transform.
struct aa_tf_tfmat aa_tf_tfmat_t
Memory layout for a transformation matrix.
double z
z component
Definition: tf.h:222
AA_API void aa_tf_qrot1(const double q[AA_RESTRICT 4], double v[AA_RESTRICT 3])
Quaternion point rotation, in place.
AA_API void aa_tf_qsub(const double a[AA_RESTRICT 4], const double b[AA_RESTRICT 4], double c[AA_RESTRICT 4])
Quaternion subtraction.
AA_API void aa_tf_12(const double T[AA_RESTRICT 12], const double p0[AA_RESTRICT 3], double p1[AA_RESTRICT 3])
apply a euclidean transform
AA_API void aa_tf_duqu_ln(const double d[AA_RESTRICT 8], double e[AA_RESTRICT 8])
Dual quaternion natural logarithm.
struct aa_tf_qv aa_tf_qv_t
Memory layout for a Transformation as rotation quaternion and translation vector. ...
AA_API void aa_tf_duqu2pure(const double S[AA_RESTRICT 8], double v[AA_RESTRICT 6])
Convert a pure dual quaternion to conventional dual quaternion.
double z
z component
Definition: tf.h:163
AA_API void aa_tf_skewsym_scal_c(const double u[AA_RESTRICT 3], const double a[AA_RESTRICT 3], const double b[AA_RESTRICT 3], double R[9])
Construct a skew-symmetric matrix.
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.
AA_API void aa_tf_tfmat_normalize(double T[AA_RESTRICT 12])
Orthonormalize the transformation matrix.
AA_API void aa_tf_tfmat_mul(const double T0[AA_RESTRICT 12], const double T1[AA_RESTRICT 12], double T[AA_RESTRICT 12])
Multiple two transformation matrices.
AA_API void aa_tf_duqu_mulc(const double d1[AA_RESTRICT 8], const double d2[AA_RESTRICT 8], double d3[AA_RESTRICT 8])
Dual quaternion multiply d1 by conjugate of d2.
AA_API void aa_tf_duqu_exp(const double d[AA_RESTRICT 8], double e[AA_RESTRICT 8])
Dual quaternion exponential.
static const double aa_tf_axang_ident[4]
Identity axis-angle array.
Definition: tf.h:490
static const double aa_tf_vec_y[3]
A Y axis.
Definition: tf.h:510
AA_API void aa_tf_qmulc(const double a[AA_RESTRICT 4], const double b[AA_RESTRICT 4], double c[AA_RESTRICT 4])
Quaternion multiply a by conjugate b.
AA_API void aa_tf_duqu_add(const double d1[AA_RESTRICT 8], const double d2[AA_RESTRICT 8], double d3[AA_RESTRICT 8])
Dual quaternion addition.
static void aa_tf_sinccos(double theta, double *sc, double *c)
Compute sinc(theta), cos(theta)
Definition: tf.h:119
AA_API void aa_tf_duqu_sdiff(const double d0[AA_RESTRICT 8], const double dd[AA_RESTRICT 8], double dt, double d1[AA_RESTRICT 6])
Dual quaternion derivative integration.
AA_API void aa_tf_duqu_svel(const double d0[AA_RESTRICT 8], const double dx[AA_RESTRICT 6], double dt, double d1[AA_RESTRICT 6])
Dual quaternion velocity integration.
static const double aa_tf_tfmat_ident[12]
Identity transformation matrix array.
Definition: tf.h:465
AA_API void aa_tf_qslerpdiffalg(double tau, const double a[AA_RESTRICT 4], const double b[AA_RESTRICT 4], double c[AA_RESTRICT 4])
Derivative of quaternation SLERP WRT tau, computed algebraicly.
AA_API void aa_tf_rotmat_zx(const double z_axis[AA_RESTRICT 3], const double x_axis[AA_RESTRICT 3], double R[AA_RESTRICT 9])
Construct rotation matrix from a z and x axis of the child frame.
AA_API void aa_tf_rotvec2quat(const double rotvec[AA_RESTRICT 3], double q[AA_RESTRICT 4])
covert rotation vector to quaternion
Memory layout for an SE(3) velocity.
Definition: tf.h:328
AA_API void aa_tf_qslerpalg(double tau, const double a[AA_RESTRICT 4], const double b[AA_RESTRICT 4], double c[AA_RESTRICT 4])
Quaternion SLERP, computed algebraicly.
Transform and spatial velocity.
Definition: tf.h:344
Memory layout for a quaternion, x,y,z,w order.
Definition: tf.h:217
#define AA_RESTRICT
Defined restrict keyword based on language flavor.
Definition: amino.h:99
static float AA_TF_VDOTF(const float a[AA_RESTRICT 3], const float b[AA_RESTRICT 3])
Inlined vector dot product.
Definition: tf.h:877
Memory layout for an Euler Angle in ZYX (yaw-pitch-roll) format.
Definition: tf.h:239
static void AA_TF_CROSS(const double a[AA_RESTRICT 3], const double b[AA_RESTRICT 3], double c[AA_RESTRICT 3])
Inlined Vector cross product.
Definition: tf.h:910
static const double aa_tf_vec_z[3]
A Z axis.
Definition: tf.h:515
AA_API void aa_tf_qmulnorm(const double a[AA_RESTRICT 4], const double b[AA_RESTRICT 4], double c[AA_RESTRICT 4])
Quaternion multiplication and normalize.
AA_API void aa_tf_quat2eulerzyx(const double q[AA_RESTRICT 4], double e[AA_RESTRICT 3])
Convert quaternion to ZYX Euler Angles.
AA_API void aa_tf_rotmat_rot(const double R[AA_RESTRICT 9], const double p0[AA_RESTRICT 3], double p1[AA_RESTRICT 3])
Rotate a point using a rotation matrix.
double dv[3]
translational velocity
Definition: tf.h:331
AA_API void aa_tf_tfmat_imul(const double T0[AA_RESTRICT 12], const double T1[AA_RESTRICT 12], double T[AA_RESTRICT 12])
Multiple inverse of T0 by T1.
AA_API void aa_tf_rotmat_lnv(const double R[AA_RESTRICT 9], double v[AA_RESTRICT 3])
Rotation Matrix logarithm.
AA_API void aa_tf_quat_davenport(size_t n, const double *w, const double *Q, size_t ldq, double *y)
Weighted average quaternion using Davenport&#39;s q-method.
AA_API void aa_tf_qmatrix_r(const double *q, double *M, size_t ldm)
Construct matrix for right quaternion multiply p*q = M*p.
AA_API void aa_tf_xangle2quat(double theta_x, double q[AA_RESTRICT 4])
Unit quaternion for angle about x axis.
static const double aa_tf_vec_x[3]
An X axis.
Definition: tf.h:505
AA_API void aa_tf_duqu_cmul(const double d1[AA_RESTRICT 8], const double d2[AA_RESTRICT 8], double d3[AA_RESTRICT 8])
Dual quaternion multiply conjugate of d1 by d2.
#define AA_TF_AXANG_IDENT_INITIALIZER
Static initializer for an identity axis-angle.
Definition: tf.h:434
AA_API void aa_tf_12rel(const double T1[AA_RESTRICT 12], const double T2[AA_RESTRICT 12], double Trel[AA_RESTRICT 12])
relative transform
AA_API void aa_tf_vnormalize(double v[AA_RESTRICT 3])
Normalize Vector.
Memory layout for a dual quaternion .
Definition: tf.h:315
AA_API void aa_tf_93(const double R[AA_RESTRICT 9], const double v[AA_RESTRICT 3], const double p0[AA_RESTRICT 3], double p1[AA_RESTRICT 4])
apply a euclidean transform
AA_API void aa_tf_93chain(const double R0[AA_RESTRICT 9], const double v0[AA_RESTRICT 3], const double R1[AA_RESTRICT 9], const double v1[AA_RESTRICT 3], double R[AA_RESTRICT 9], double v[AA_RESTRICT 3])
chain two transforms
#define AA_API
calling and name mangling convention for functions
Definition: amino.h:95
AA_API void aa_tf_xangle2rotmat(double theta_x, double R[AA_RESTRICT 9])
Angle about x axis.
AA_API void aa_tf_qvel2diff(const double q[AA_RESTRICT 4], const double v[AA_RESTRICT 3], double dq_dt[AA_RESTRICT 4])
Angular velocity to quaternion time derivative.
AA_API void aa_tf_duqu_diff2twist(const double d[AA_RESTRICT 8], const double dd[AA_RESTRICT 8], double twist[AA_RESTRICT 8])
Convert dual quaternion derivative to dual quaternion twist.
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.
#define AA_TF_DOTX(a, b)
Macro to compute a dot product of length 3 vectors.
Definition: tf.h:861
AA_API void aa_tf_rotmat_svel(const double R0[AA_RESTRICT 9], const double w[AA_RESTRICT 3], double dt, double R1[AA_RESTRICT 9])
Integrate rotational velocity.
AA_API void aa_tf_duqu_sub(const double d1[AA_RESTRICT 8], const double d2[AA_RESTRICT 8], double d3[AA_RESTRICT 8])
Dual quaternion subtraction.
AA_API void aa_tf_rotmat_inv2(const double R[AA_RESTRICT 9], double Ri[AA_RESTRICT 9])
Invert a rotation.
AA_API double aa_tf_vdot(const double a[AA_RESTRICT 3], const double b[AA_RESTRICT 3])
Vector dot product.
AA_API void aa_tf_rotmat_muli(const double R0[AA_RESTRICT 9], const double R1[AA_RESTRICT 9], double R[AA_RESTRICT 9])
Multiple R1 by inverse of R1.
static double aa_tf_sinc(double theta)
Compute sinc(theta)
Definition: tf.h:135
double aa_tf_qangle(const double q[AA_RESTRICT 4])
Return the angle of the quaternion.
#define AA_TF_ROTMAT_IDENT_INITIALIZER
Static initializer for an identity rotation matrix.
Definition: tf.h:414
AA_API void aa_tf_tfmat_svel(const double T0[AA_RESTRICT 12], const double w[AA_RESTRICT 3], double dt, double T1[AA_RESTRICT 12])
Integrate rotational velocity.
AA_API void aa_tf_qsdiff(const double q0[AA_RESTRICT 4], const double dq[AA_RESTRICT 4], double dt, double q1[AA_RESTRICT 4])
Integrate unit quaternion from quaternion derivative.
AA_API void aa_tf_qrel(const double q1[AA_RESTRICT 4], const double q2[AA_RESTRICT 4], double q_rel[AA_RESTRICT 4])
Relative orientation.
AA_API void aa_tf_duqu_conj(const double d[AA_RESTRICT 8], double dconj[AA_RESTRICT 8])
Dual quaternion conjugate.
static void AA_TF_CROSSF(const float a[AA_RESTRICT 3], const float b[AA_RESTRICT 3], float c[AA_RESTRICT 3])
Inlined Vector cross product.
Definition: tf.h:919
AA_API void aa_tf_tfmat_inv2(const double T[AA_RESTRICT 12], double Ti[AA_RESTRICT 12])
Invert a transform.
AA_API void aa_tf_axang2quat(const double axang[AA_RESTRICT 4], double q[AA_RESTRICT 4])
axis-angle to quaternion.
AA_API void aa_tf_qmul_vq(const double v[AA_RESTRICT 3], const double q[AA_RESTRICT 4], double c[AA_RESTRICT 4])
Quaternion multiplication.
AA_API void aa_tf_rotmat2rotvec(const double R[AA_RESTRICT 9], double rv[AA_RESTRICT 3])
convert axis rotation matrix to rotation vector
AA_API void aa_tf_qln(const double q[AA_RESTRICT 4], double r[AA_RESTRICT 4])
Quaternion natural log.
AA_API void aa_tf_tfmat2_mul(const double R0[AA_RESTRICT 9], const double v0[AA_RESTRICT 3], const double R1[AA_RESTRICT 9], const double v1[AA_RESTRICT 3], double R[AA_RESTRICT 9], double v[AA_RESTRICT 3])
Multiple two transformation matrices, in split representation.
AA_API void aa_tf_cross(const double a[AA_RESTRICT 3], const double b[AA_RESTRICT 3], double c[AA_RESTRICT 3])
Vector cross product.
AA_API void aa_tf_zangle2quat(double theta_z, double q[AA_RESTRICT 4])
Unit quaternion for angle about z axis.
AA_API double aa_tf_qnorm(const double q[AA_RESTRICT 4])
Return norm of the quaternion.
Axis-Angle rotation.
Definition: tf.h:188
AA_API void aa_tf_pure2duqu(const double v[AA_RESTRICT 6], double S[AA_RESTRICT 8])
Convert a conventional dual quaternion to pure dual quaternion.
void aa_tf_qutr_cmul(const double a[7], const double b[7], double c[7])
quaternion-translation conjugate multiply
double w
w component
Definition: tf.h:223
AA_API void aa_tf_duqu_normalize(double d[AA_RESTRICT 8])
Dual quaternion normalization.
AA_API void aa_tf_axang2rotmat2(const double axis[AA_RESTRICT 3], double angle, double R[AA_RESTRICT 9])
convert axis angle to rotation matrix
AA_API void aa_tf_rotmat_mul(const double R0[AA_RESTRICT 9], const double R1[AA_RESTRICT 9], double R[AA_RESTRICT 9])
Multiple two rotation matrices.
AA_API void aa_tf_tf_duqu(const double d[AA_RESTRICT 8], const double p0[AA_RESTRICT 3], double p1[AA_RESTRICT 3])
Dual quaternion transformation.
AA_API void aa_tf_quat2axang(const double q[AA_RESTRICT 4], double axang[AA_RESTRICT 4])
Quaternion to axis-angle.
static const double aa_tf_quat_ident[4]
Identity quaternion array.
Definition: tf.h:475
static const double aa_tf_qutr_ident[7]
Identity quaternion-translation array.
Definition: tf.h:485
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
void aa_tf_qutr_mulc(const double a[7], const double b[7], double c[7])
quaternion-translation conjugate multiply
AA_API void aa_tf_duqu_trans(const double d[AA_RESTRICT 8], double v[AA_RESTRICT 3])
Extract dual quaternion translation vector.
static double AA_TF_VDOT(const double a[AA_RESTRICT 3], const double b[AA_RESTRICT 3])
Inlined vector dot product.
Definition: tf.h:868
void aa_tf_qutr2tfmat(const double e[7], double T[12])
quaternion-translation to transformation matrix
AA_API void aa_tf_quat_davenport_matrix(size_t n, const double *w, const double *q, size_t ldqq, double *M)
Construct matrix for Davenport&#39;s q-method.
#define AA_TF_QUAT_IDENT_INITIALIZER
Static initializer for an identity quaternion.
Definition: tf.h:419
AA_API void aa_tf_crossf(const float a[AA_RESTRICT 3], const float b[AA_RESTRICT 3], float c[AA_RESTRICT 3])
Vector cross product.
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
AA_API void aa_tf_tfmat_muli(const double T0[AA_RESTRICT 12], const double T1[AA_RESTRICT 12], double T[AA_RESTRICT 12])
Multiple T1 by inverse of T1.
AA_API void aa_tf_qslerpdiff(double tau, const double a[AA_RESTRICT 4], const double b[AA_RESTRICT 4], double c[AA_RESTRICT 4])
Derivative of quaternation SLERP WRT tau.
AA_API void aa_tf_rotmat_vel2diff(const double R[AA_RESTRICT 9], const double w[AA_RESTRICT 3], double dR[AA_RESTRICT 9])
Velocity to rotation matrix derivative.
AA_API void aa_tf_duqu_vel2twist(const double d[AA_RESTRICT 8], const double dx[AA_RESTRICT 6], double t[AA_RESTRICT 8])
Dual quaternion twist from velocity.
AA_API void aa_tf_duqu_minimize(double d[AA_RESTRICT 8])
Dual quaternion angle minimization.
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
AA_API void aa_tf_v12chain(double T[AA_RESTRICT 12], const double T1[AA_RESTRICT 12], const double T2[AA_RESTRICT 12],...)
Varargs transform chain.
#define AA_TF_DEF_SERIES(name, a0, a1, a2)
Define an inline function for a series using Horner&#39;s rule.
Definition: tf.h:78
AA_API void aa_tf_tfmat_tf(const double T[AA_RESTRICT 12], const double p0[AA_RESTRICT 3], double p1[AA_RESTRICT 3])
Transform a point using a transformation matrix.
AA_API void aa_tf_tfmat_expv(const double v[AA_RESTRICT 6], double T[AA_RESTRICT 12])
Transformation Matrix exponential.
AA_API void aa_tf_qdulnj(const double q[AA_RESTRICT 4], const double dq[AA_RESTRICT 4], double dln[AA_RESTRICT 3])
Derivative of the Unit Quaternion Logarithm, computed via Jacobian.
AA_API void aa_tf_qrot(const double q[AA_RESTRICT 4], const double v[AA_RESTRICT 3], double p[AA_RESTRICT 3])
Quaternion point rotation.
AA_API void aa_tf_duqu_smul(const double d1[AA_RESTRICT 8], const double d2[AA_RESTRICT 8], double d3[AA_RESTRICT 8])
Dual quaternion scalar multiplication.
AA_API void aa_tf_93inv(const double R[AA_RESTRICT 9], const double v[AA_RESTRICT 3], double Ri[AA_RESTRICT 9], double vi[AA_RESTRICT 3])
invert transform
AA_API void aa_tf_duqu_stwist(const double d0[AA_RESTRICT 8], const double twist[AA_RESTRICT 8], double dt, double d1[AA_RESTRICT 6])
Dual quaternion twist integration.
AA_API void aa_tf_v9mul(double R[AA_RESTRICT 9], const double R1[AA_RESTRICT 9], const double R2[AA_RESTRICT 9],...)
Vararg multiply two rotation matrices.
AA_API void aa_tf_qslerpchaindiff(double u, double du, const double q1[AA_RESTRICT 4], const double dq1[AA_RESTRICT 4], const double q2[AA_RESTRICT 4], const double dq2[AA_RESTRICT 4], double q[AA_RESTRICT 4], double dq[AA_RESTRICT 4])
Chain-rule slerp differentiation.
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.
double scalar
scalar part
Definition: tf.h:230
AA_API void aa_tf_skewsym_scal2(double a, double b, const double u[3], double R[9])
Construct a skew-symmetric matrix.
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_rotvec_near(const double rv[AA_RESTRICT 3], const double rv_near[AA_RESTRICT 3], double rv_p[AA_RESTRICT 3])
Scales rv by multiple of 2pi to minimized SSD with rv_near.
void aa_tf_qutr_wavg(size_t n, const double *w, const double *EE, size_t ldee, double *a)
Weighted average transform.
AA_API void aa_tf_rotmat2quat(const double rotmat[AA_RESTRICT 9], double quat[AA_RESTRICT 4])
convert rotation matrix to quaternion
AA_API void aa_tf_tf_qv(const double quat[AA_RESTRICT 4], const double v[AA_RESTRICT 3], const double p0[AA_RESTRICT 3], double p1[AA_RESTRICT 4])
apply a euclidean transform
AA_API void aa_tf_rotmat_normalize(double R[AA_RESTRICT 9])
Orthonormalize the rotation matrix.
AA_API void aa_tf_qrk1(const double q0[AA_RESTRICT 4], const double dq[AA_RESTRICT 4], double dt, double q1[AA_RESTRICT 4])
Integrate unit quaternion, Runge-Kutta-1 (euler) integration.
AA_API void aa_tf_qexp(const double q[AA_RESTRICT 4], double r[AA_RESTRICT 4])
Quaternion exponential.
#define AA_TF_DEF_EULER(letters)
Create declarations for Euler angle conversion functions.
Definition: tf.h:1542
#define AA_TF_DUQU_IDENT_INITIALIZER
Static initializer for an identity dual quaternion.
Definition: tf.h:424
AA_API int aa_tf_isrotmat(const double R[AA_RESTRICT 9])
tests if R is a rotation matrix
AA_API void aa_tf_qslerp(double tau, const double a[AA_RESTRICT 4], const double b[AA_RESTRICT 4], double c[AA_RESTRICT 4])
Quaternion SLERP.
AA_API void aa_tf_rotmat_diff2vel(const double R[AA_RESTRICT 9], const double dR[AA_RESTRICT 9], double w[AA_RESTRICT 3])
Rotation matrix derivative to velocity.
AA_API void aa_tf_rotmat2eulerzyx(const double R[AA_RESTRICT 9], double e[AA_RESTRICT 3])
Convert Rotation Matrix to ZYX Euler Angles.
AA_API void aa_tf_qconj(const double q[AA_RESTRICT 4], double r[AA_RESTRICT 4])
Quaternion conjugate.
AA_API void aa_tf_qinv(const double q[AA_RESTRICT 4], double r[AA_RESTRICT 4])
Quaternion inverse.
AA_API void aa_tf_12inv(const double T[AA_RESTRICT 12], double Ti[AA_RESTRICT 12])
invert transform
void aa_tf_qutr_mulnorm(const double a[7], const double b[7], double c[7])
quaternion-translation multiply and normalize
AA_API void aa_tf_duqu_norm(const double d[AA_RESTRICT 8], double *nreal, double *ndual)
Dual quaternion norm.
#define AA_TF_CROSSX(a, b, c)
Macro to compute the cross product of length 3 vectors.
Definition: tf.h:900
void aa_tf_tfmat2av(const double T[12], double q[AA_RESTRICT 4], double v[AA_RESTRICT 3])
transformation matrix to quaternion-translation