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 
51 #include "mat.h"
52 
62 #include <float.h>
63 
64 #ifdef __cplusplus
65 extern "C" {
66 #endif
67 
68 /**********/
69 /* Series */
70 /**********/
71 
80 #define AA_TF_DEF_SERIES(name, a0, a1, a2) \
81  static inline double \
82  aa_tf_ ## name ##_series2(double theta2) \
83  { \
84  return aa_horner3( theta2, a0, a1, a2 ); \
85  } \
86  static inline double \
87  aa_tf_ ## name ## _series(double theta) \
88  { \
89  return aa_tf_ ## name ## _series2(theta*theta); \
90  } \
91 
92 AA_TF_DEF_SERIES( sinc, 1., -1./6, 1./120 )
93 AA_TF_DEF_SERIES( cos, 1., -1./2, 1./24 )
94 AA_TF_DEF_SERIES( invsinc, 1., 1./6, 7./360 )
95 
103 static inline void
104 aa_tf_sinccos2( double theta2, double *sc, double *c )
105 {
106  if( theta2 < sqrt(DBL_EPSILON) ) {
107  *sc = aa_tf_sinc_series2(theta2);
108  *c = aa_tf_cos_series2(theta2);
109  } else {
110  double theta = sqrt(theta2);
111  double s = sin(theta);
112  *c = cos(theta);
113  *sc = s / theta;
114  }
115 }
116 
120 static inline void
121 aa_tf_sinccos( double theta, double *sc, double *c )
122 {
123  if( theta*theta < sqrt(DBL_EPSILON) ) {
124  *sc = aa_tf_sinc_series(theta);
125  *c = aa_tf_cos_series(theta);
126  } else {
127  double s = sin(theta);
128  *c = cos(theta);
129  *sc = s / theta;
130  }
131 }
132 
136 static inline double
137 aa_tf_sinc( double theta )
138 {
139  if( fabs(theta) < sqrt(sqrt(DBL_EPSILON)) ) {
140  return aa_tf_sinc_series(theta);
141  } else {
142  return sin(theta)/theta;
143  }
144 }
145 
146 /*********/
147 /* Types */
148 /*********/
149 
151 #define AA_TF_X 0
152 #define AA_TF_Y 1
154 #define AA_TF_Z 2
156 
160 typedef struct aa_tf_vec3 {
161  union {
162  struct {
163  double x;
164  double y;
165  double z;
166  };
167  double data[3];
168  };
169 } aa_tf_vec3_t;
170 
176 typedef struct aa_tf_rotmat {
177  union {
178  struct {
179  struct aa_tf_vec3 col0;
180  struct aa_tf_vec3 col1;
181  struct aa_tf_vec3 col2;
182  };
183  double data[9];
184  };
186 
190 typedef struct aa_tf_axang {
191  union {
192  struct {
193  union {
194  struct aa_tf_vec3 axis;
195  double v[3];
196  };
197  double angle;
198  };
199  double data[4];
200  };
201 } aa_tf_axang_t;
202 
204 #define AA_TF_QUAT_V 0
205 #define AA_TF_QUAT_XYZ AA_TF_QUAT_V
207 #define AA_TF_QUAT_X (AA_TF_QUAT_V + AA_TF_X)
209 #define AA_TF_QUAT_Y (AA_TF_QUAT_V + AA_TF_Y)
211 #define AA_TF_QUAT_Z (AA_TF_QUAT_V + AA_TF_Z)
213 #define AA_TF_QUAT_W 3
215 
219 typedef struct aa_tf_quat {
220  union {
221  struct {
222  double x;
223  double y;
224  double z;
225  double w;
226  };
227  struct {
228  union {
229  struct aa_tf_vec3 vec;
230  double v[3];
231  };
232  double scalar;
233  };
234  double data[4];
235  };
236 } aa_tf_quat_t;
237 
241 typedef struct aa_tf_eulerzyx {
242  union {
243  struct {
244  double y;
245  double p;
246  double r;
247  };
248  double data[3];
249  };
251 
252 
264 typedef struct aa_tf_tfmat {
265  union {
266  struct {
267  double R[9];
269  };
270  double data[12];
271  };
272 } aa_tf_tfmat_t;
273 
277 #define AA_TF_TFMAT_R 0
278 
282 #define AA_TF_TFMAT_V 9
283 
287 #define AA_TF_TFMAT_X 9
288 
292 #define AA_TF_TFMAT_Y 10
293 
297 #define AA_TF_TFMAT_Z 11
298 
302 typedef struct aa_tf_qv {
303  union {
304  struct {
305  aa_tf_quat_t r;
306  aa_tf_vec3_t v;
307  };
308  double data[7];
309  };
310 } aa_tf_qv_t;
311 
312 
313 
317 typedef struct aa_tf_duqu {
318  union {
319  struct {
320  aa_tf_quat_t real;
321  aa_tf_quat_t dual;
322  };
323  double data[8];
324  };
325 } aa_tf_duqu_t;
326 
330 struct aa_tf_dx {
331  union {
332  struct {
333  double dv[3];
334  double omega[3];
335  };
336  double data[6];
337  };
338 };
339 
341 #define AA_TF_DX_V 0
342 #define AA_TF_DX_W 3
344 
346 struct aa_tf_qv_dx {
347  union {
348  struct {
349  struct aa_tf_qv tf;
350  struct aa_tf_dx dx;
351  };
352  double data[13];
353  };
354 };
355 
357 #define AA_TF_QUTR_Q 0
358 #define AA_TF_QUTR_QX (AA_TF_QUTR_Q + AA_TF_QUAT_X)
360 #define AA_TF_QUTR_QY (AA_TF_QUTR_Q + AA_TF_QUAT_Y)
362 #define AA_TF_QUTR_QZ (AA_TF_QUTR_Q + AA_TF_QUAT_Z)
364 #define AA_TF_QUTR_QW (AA_TF_QUTR_Q + AA_TF_QUAT_W)
366 
368 #define AA_TF_QUTR_T 4
369 #define AA_TF_QUTR_TX (AA_TF_QUTR_T + AA_TF_X)
371 #define AA_TF_QUTR_TY (AA_TF_QUTR_T + AA_TF_Y)
373 #define AA_TF_QUTR_TZ (AA_TF_QUTR_T + AA_TF_Z)
375 
377 #define AA_TF_DUQU_REAL 0
378 #define AA_TF_DUQU_DUAL 4
380 
382 #define AA_TF_DUQU_REAL_W (AA_TF_DUQU_REAL + AA_TF_QUAT_W)
383 #define AA_TF_DUQU_REAL_XYZ (AA_TF_DUQU_REAL + AA_TF_QUAT_XYZ)
385 #define AA_TF_DUQU_REAL_X (AA_TF_DUQU_REAL + AA_TF_QUAT_X)
387 #define AA_TF_DUQU_REAL_Y (AA_TF_DUQU_REAL + AA_TF_QUAT_Y)
389 #define AA_TF_DUQU_REAL_Z (AA_TF_DUQU_REAL + AA_TF_QUAT_Z)
391 
393 #define AA_TF_DUQU_DUAL_W (AA_TF_DUQU_DUAL + AA_TF_QUAT_W)
394 #define AA_TF_DUQU_DUAL_XYZ (AA_TF_DUQU_DUAL + AA_TF_QUAT_XYZ)
396 #define AA_TF_DUQU_DUAL_X (AA_TF_DUQU_DUAL + AA_TF_QUAT_X)
398 #define AA_TF_DUQU_DUAL_Y (AA_TF_DUQU_DUAL + AA_TF_QUAT_Y)
400 #define AA_TF_DUQU_DUAL_Z (AA_TF_DUQU_DUAL + AA_TF_QUAT_Z)
402 
404 #define AA_TF_EPSILON .0001
405 
406 
407 
411 #define AA_TF_IDENT_INITIALIZER {1,0,0, 0,1,0, 0,0,1, 0,0,0}
412 
416 #define AA_TF_ROTMAT_IDENT_INITIALIZER {1,0,0, 0,1,0, 0,0,1}
417 
421 #define AA_TF_QUAT_IDENT_INITIALIZER {0,0,0,1}
422 
426 #define AA_TF_DUQU_IDENT_INITIALIZER {0,0,0,1, 0,0,0,0}
427 
431 #define AA_TF_QUTR_IDENT_INITIALIZER {0,0,0,1, 0,0,0}
432 
436 #define AA_TF_AXANG_IDENT_INITIALIZER {1,0,0,0}
437 
441 #define AA_TF_ROTVEC_IDENT_INITIALIZER {0,0,0}
442 
446 #define AA_TF_VEC_IDENT_INITIALIZER {0,0,0}
447 
449 #define AA_TF_IDENT ( (double[12]) AA_TF_IDENT_INITIALIZER )
450 #define AA_TF_ROTMAT_IDENT ( (double[9] AA_TF_ROTMAT_IDENT_INITIALIZER )
452 #define AA_TF_QUAT_IDENT ( (double[4]) AA_TF_QUAT_IDENT_INITIALIZER )
454 #define AA_TF_AXANG_IDENT ( (double[4]) AA_TF_AXANG_IDENT_INITIALIZER )
456 #define AA_TF_ROTVEC_IDENT ( (double[3]) AA_TF_ROTVEC_IDENT_INITIALIZER )
458 
462 static const double aa_tf_ident[12] = AA_TF_IDENT_INITIALIZER;
463 
467 static const double aa_tf_tfmat_ident[12] = AA_TF_IDENT_INITIALIZER;
468 
473 
478 
483 
488 
493 
498 
503 
507 static const double aa_tf_vec_x[3] = {1, 0, 0};
508 
512 static const double aa_tf_vec_y[3] = {0, 1, 0};
513 
517 static const double aa_tf_vec_z[3] = {0, 0, 1};
518 
519 /**************/
520 /* Transforms */
521 /**************/
522 
524 AA_API void aa_tf_12( const double T[AA_RESTRICT 12],
525  const double p0[AA_RESTRICT 3],
526  double p1[AA_RESTRICT 3] );
528 AA_API void aa_tf_93( const double R[AA_RESTRICT 9],
529  const double v[AA_RESTRICT 3],
530  const double p0[AA_RESTRICT 3],
531  double p1[AA_RESTRICT 4] );
533 AA_API void aa_tf_tf_qv( const double quat[AA_RESTRICT 4],
534  const double v[AA_RESTRICT 3],
535  const double p0[AA_RESTRICT 3],
536  double p1[AA_RESTRICT 4] );
537 
539 AA_API void aa_tf_9( const double R[AA_RESTRICT 9],
540  const double p0[AA_RESTRICT 3],
541  double p1[AA_RESTRICT 4] );
542 
544 AA_API void aa_tf_12inv( const double T[AA_RESTRICT 12],
545  double Ti[AA_RESTRICT 12] );
547 AA_API void aa_tf_93inv( const double R[AA_RESTRICT 9],
548  const double v[AA_RESTRICT 3],
549  double Ri[AA_RESTRICT 9], double vi[AA_RESTRICT 3] );
555 AA_API void aa_tf_qv_conj( const double q[AA_RESTRICT 4], const double v[AA_RESTRICT 3],
556  double qc[AA_RESTRICT 4], double vc[AA_RESTRICT 3] );
557 
559 AA_API void aa_tf_12chain( const double T1[AA_RESTRICT 12],
560  const double T2[AA_RESTRICT 12],
561  double T[AA_RESTRICT 12] );
562 
564 AA_API void aa_tf_v12chain( double T[AA_RESTRICT 12] ,
565  const double T1[AA_RESTRICT 12],
566  const double T2[AA_RESTRICT 12],
567  ... );
568 
570 AA_API void aa_tf_93chain( const double R0[AA_RESTRICT 9],
571  const double v0[AA_RESTRICT 3],
572  const double R1[AA_RESTRICT 9],
573  const double v1[AA_RESTRICT 3],
574  double R[AA_RESTRICT 9], double v[AA_RESTRICT 3] );
576 AA_API void aa_tf_qv_chain( const double q0[AA_RESTRICT 4],
577  const double v0[AA_RESTRICT 3],
578  const double q1[AA_RESTRICT 4],
579  const double v1[AA_RESTRICT 3],
580  double q[AA_RESTRICT 4],
581  double v[AA_RESTRICT 3] );
582 
584 AA_API void aa_tf_93rel( const double R1[AA_RESTRICT 9],
585  const double v1[AA_RESTRICT 3],
586  const double R2[AA_RESTRICT 9],
587  const double v2[AA_RESTRICT 3],
588  double Rrel[AA_RESTRICT 9],
589  double vrel[AA_RESTRICT 3] );
590 
592 AA_API void aa_tf_12rel( const double T1[AA_RESTRICT 12],
593  const double T2[AA_RESTRICT 12],
594  double Trel[AA_RESTRICT 12] );
595 
596 /************/
597 /* Matrices */
598 /************/
599 
617 AA_API void aa_tf_skewsym_scal2( double a, double b, const double u[3], double R[9] );
618 
622 AA_API void
623 aa_tf_skewsym_scal_c( const double u[AA_RESTRICT 3],
624  const double a[AA_RESTRICT 3], const double b[AA_RESTRICT 3],
625  double R[9] );
626 
627 
628 /* Multiply */
629 
633 AA_API void aa_tf_rotmat_mul( const double R0[AA_RESTRICT 9],
634  const double R1[AA_RESTRICT 9],
635  double R[AA_RESTRICT 9] );
636 
640 AA_API void aa_tf_tfmat_mul( const double T0[AA_RESTRICT 12],
641  const double T1[AA_RESTRICT 12],
642  double T[AA_RESTRICT 12] );
643 
647 AA_API void aa_tf_tfmat2_mul( const double R0[AA_RESTRICT 9],
648  const double v0[AA_RESTRICT 3],
649  const double R1[AA_RESTRICT 9],
650  const double v1[AA_RESTRICT 3],
651  double R[AA_RESTRICT 9], double v[AA_RESTRICT 3] );
652 
653 
654 /* Inverting Multiply */
660 AA_API void aa_tf_rotmat_imul( const double R0[AA_RESTRICT 9],
661  const double R1[AA_RESTRICT 9],
662  double R[AA_RESTRICT 9] );
663 
669 AA_API void aa_tf_rotmat_muli( const double R0[AA_RESTRICT 9],
670  const double R1[AA_RESTRICT 9],
671  double R[AA_RESTRICT 9] );
672 
678 AA_API void aa_tf_tfmat_imul( const double T0[AA_RESTRICT 12],
679  const double T1[AA_RESTRICT 12],
680  double T[AA_RESTRICT 12] );
681 
687 AA_API void aa_tf_tfmat_muli( const double T0[AA_RESTRICT 12],
688  const double T1[AA_RESTRICT 12],
689  double T[AA_RESTRICT 12] );
690 
696 AA_API void aa_tf_tfmat2_imul( const double R0[AA_RESTRICT 9],
697  const double v0[AA_RESTRICT 3],
698  const double R1[AA_RESTRICT 9],
699  const double v1[AA_RESTRICT 3],
700  double R[AA_RESTRICT 9], double v[AA_RESTRICT 3] );
701 
707 AA_API void aa_tf_tfmat2_muli( const double R0[AA_RESTRICT 9],
708  const double v0[AA_RESTRICT 3],
709  const double R1[AA_RESTRICT 9],
710  const double v1[AA_RESTRICT 3],
711  double R[AA_RESTRICT 9], double v[AA_RESTRICT 3] );
712 
713 
717 AA_API void
718 aa_tf_rotmat_normalize( double R[AA_RESTRICT 9] );
719 
720 /* Transform */
721 
725 AA_API void aa_tf_rotmat_rot( const double R[AA_RESTRICT 9],
726  const double p0[AA_RESTRICT 3],
727  double p1[AA_RESTRICT 3] );
728 
732 AA_API void aa_tf_tfmat_tf( const double T[AA_RESTRICT 12],
733  const double p0[AA_RESTRICT 3],
734  double p1[AA_RESTRICT 3] );
735 
739 AA_API void aa_tf_tfmat2_tf( const double R[AA_RESTRICT 9],
740  const double v[AA_RESTRICT 3],
741  const double p0[AA_RESTRICT 3],
742  double p1[AA_RESTRICT 4] );
743 
747 AA_API void aa_tf_rotmat_inv1( double R[AA_RESTRICT 9] );
748 
752 AA_API void aa_tf_rotmat_inv2( const double R[AA_RESTRICT 9],
753  double Ri[AA_RESTRICT 9] );
754 
758 AA_API void aa_tf_tfmat_inv1( double T[AA_RESTRICT 12] );
759 
763 AA_API void aa_tf_tfmat_inv2( const double T[AA_RESTRICT 12],
764  double Ti[AA_RESTRICT 12] );
765 
766 
770 AA_API void
771 aa_tf_tfmat_normalize( double T[AA_RESTRICT 12] );
772 
774 AA_API int aa_tf_isrotmat( const double R[AA_RESTRICT 9] );
775 
776 
778 AA_API void aa_tf_9mul( const double R0[AA_RESTRICT 9],
779  const double R1[AA_RESTRICT 9],
780  double R[AA_RESTRICT 9] );
782 AA_API void aa_tf_9rot( const double R[AA_RESTRICT 9],
783  const double p0[AA_RESTRICT 3],
784  double p1[AA_RESTRICT 3] );
785 
787 AA_API void aa_tf_9rel( const double R1[AA_RESTRICT 9], const double R2[AA_RESTRICT 9],
788  double Ri[AA_RESTRICT 9] );
789 
791 AA_API void aa_tf_rotmat_exp_aa( const double aa[AA_RESTRICT 4], double R[AA_RESTRICT 9] );
792 
794 AA_API void aa_tf_rotmat_expv( const double rv[AA_RESTRICT 3], double R[AA_RESTRICT 9] );
795 
797 AA_API void aa_tf_rotmat_lnv( const double R[AA_RESTRICT 9], double v[AA_RESTRICT 3] );
798 
800 AA_API void aa_tf_tfmat_expv( const double v[AA_RESTRICT 6], double T[AA_RESTRICT 12] );
801 
803 AA_API void aa_tf_tfmat_lnv( const double T[AA_RESTRICT 12], double v[AA_RESTRICT 6] );
804 
806 AA_API void aa_tf_rotmat_vel2diff( const double R[AA_RESTRICT 9],
807  const double w[AA_RESTRICT 3], double dR[AA_RESTRICT 9] );
808 
810 AA_API void aa_tf_rotmat_diff2vel( const double R[AA_RESTRICT 9],
811  const double dR[AA_RESTRICT 9], double w[AA_RESTRICT 3] );
812 
814 AA_API void aa_tf_rotmat_svel( const double R0[AA_RESTRICT 9],
815  const double w[AA_RESTRICT 3], double dt,
816  double R1[AA_RESTRICT 9] );
817 
818 
819 
821 AA_API void aa_tf_tfmat_svel( const double T0[AA_RESTRICT 12],
822  const double w[AA_RESTRICT 3], double dt,
823  double T1[AA_RESTRICT 12] );
824 
826 AA_API void aa_tf_v9mul( double R[AA_RESTRICT 9],
827  const double R1[AA_RESTRICT 9], const double R2[AA_RESTRICT 9], ... );
828 
829 
833 AA_API void
834 aa_tf_rotmat_xy( const double x_axis[AA_RESTRICT 3],
835  const double y_axis[AA_RESTRICT 3],
836  double R[AA_RESTRICT 9] );
840 AA_API void
841 aa_tf_rotmat_yz( const double y_axis[AA_RESTRICT 3],
842  const double z_axis[AA_RESTRICT 3],
843  double R[AA_RESTRICT 9] );
844 
848 AA_API void
849 aa_tf_rotmat_zx( const double z_axis[AA_RESTRICT 3],
850  const double x_axis[AA_RESTRICT 3],
851  double R[AA_RESTRICT 9] );
852 
853 
854 /***********/
855 /* Vectors */
856 /***********/
857 
863 #define AA_TF_DOTX(a,b) \
864  ((a)[0]*(b)[0] + (a)[1]*(b)[1] + (a)[2]*(b)[2])
865 
869 static inline double
870 AA_TF_VDOT( const double a[AA_RESTRICT 3], const double b[AA_RESTRICT 3] )
871 {
872  return AA_TF_DOTX(a,b);
873 }
874 
878 static inline float
879 AA_TF_VDOTF( const float a[AA_RESTRICT 3], const float b[AA_RESTRICT 3] )
880 {
881  return AA_TF_DOTX(a,b);
882 }
883 
884 
888 AA_API double
889 aa_tf_vdot( const double a[AA_RESTRICT 3], const double b[AA_RESTRICT 3] );
890 
894 AA_API float
895 aa_tf_vdotf( const float a[AA_RESTRICT 3], const float b[AA_RESTRICT 3] );
896 
902 #define AA_TF_CROSSX( a, b, c ) \
903  (c)[0] = (a)[1]*(b)[2] - (a)[2]*(b)[1]; \
904  (c)[1] = (a)[2]*(b)[0] - (a)[0]*(b)[2]; \
905  (c)[2] = (a)[0]*(b)[1] - (a)[1]*(b)[0]; \
906 
907 
911 static inline void
912 AA_TF_CROSS( const double a[AA_RESTRICT 3], const double b[AA_RESTRICT 3], double c[AA_RESTRICT 3] )
913 {
914  AA_TF_CROSSX(a,b,c);
915 }
916 
920 static inline void
921 AA_TF_CROSSF( const float a[AA_RESTRICT 3], const float b[AA_RESTRICT 3], float c[AA_RESTRICT 3] )
922 {
923  AA_TF_CROSSX(a,b,c);
924 }
925 
929 AA_API void aa_tf_cross( const double a[AA_RESTRICT 3], const double b[AA_RESTRICT 3],
930  double c[AA_RESTRICT 3] ) ;
931 
935 AA_API void aa_tf_cross_a( const double a[AA_RESTRICT 3], const double b[AA_RESTRICT 3],
936  double c[AA_RESTRICT 3] ) ;
937 
942 AA_API void aa_tf_cross_mat_l( const double v[3], struct aa_dmat *M );
943 
948 AA_API void aa_tf_cross_mat_r( const double v[3], struct aa_dmat *M );
949 
950 
951 
955 AA_API void aa_tf_crossf( const float a[AA_RESTRICT 3], const float b[AA_RESTRICT 3],
956  float c[AA_RESTRICT 3] ) ;
957 
961 AA_API void aa_tf_vnormalize( double v[AA_RESTRICT 3] );
962 
963 
967 AA_API void aa_tf_vnormalizef( float v[AA_RESTRICT 3] );
968 
972 double
973 aa_tf_vssd( const double a[AA_RESTRICT 3], const double b[AA_RESTRICT 3] );
974 
978 double
979 aa_tf_vnorm( const double a[AA_RESTRICT 3] );
980 
981 /***************/
982 /* Quaternions */
983 /***************/
984 
988 static inline double
989 AA_TF_QDOT( const double a[AA_RESTRICT 4], const double b[AA_RESTRICT 4] )
990 {
991  return a[0]*b[0] + a[1]*b[1] + a[2]*b[2] + a[3]*b[3];
992 }
993 
997 double
998 aa_tf_qssd( const double a[AA_RESTRICT 4], const double b[AA_RESTRICT 4] );
999 
1003 double
1004 aa_tf_qdot( const double a[AA_RESTRICT 4], const double b[AA_RESTRICT 4] );
1005 
1009 AA_API void aa_tf_qnormalize( double q[AA_RESTRICT 4] );
1010 
1015 AA_API void
1016 aa_tf_qscal( double q[AA_RESTRICT 4], double alpha );
1017 
1018 
1020 AA_API double aa_tf_qnorm( const double q[AA_RESTRICT 4] );
1021 
1026 AA_API void aa_tf_qminimize( double q[AA_RESTRICT 4] );
1027 
1032 AA_API void aa_tf_qminimize2( const double q[AA_RESTRICT 4], double qmin[AA_RESTRICT 4] );
1033 
1036 AA_API void aa_tf_qnormalize2( const double q[AA_RESTRICT 4], double qnorm[AA_RESTRICT 4] );
1037 
1039 AA_API void aa_tf_qconj( const double q[AA_RESTRICT 4],
1040  double r[AA_RESTRICT 4] );
1041 
1043 AA_API void aa_tf_qconj1( double q[AA_RESTRICT 4] );
1044 
1045 
1047 AA_API void aa_tf_qexp( const double q[AA_RESTRICT 4],
1048  double r[AA_RESTRICT 4] );
1049 
1050 
1052 AA_API void aa_tf_qln( const double q[AA_RESTRICT 4],
1053  double r[AA_RESTRICT 4] );
1054 
1061 AA_API void aa_tf_qduln( const double q[AA_RESTRICT 4],
1062  const double dq[AA_RESTRICT 4],
1063  double dln[AA_RESTRICT 3] );
1064 
1072 AA_API void aa_tf_qdulnj( const double q[AA_RESTRICT 4],
1073  const double dq[AA_RESTRICT 4],
1074  double dln[AA_RESTRICT 3] );
1075 
1082 AA_API void aa_tf_qdpexp( const double e[AA_RESTRICT 3],
1083  const double de[AA_RESTRICT 3],
1084  double dq[AA_RESTRICT 4] );
1085 
1093 AA_API void aa_tf_qdpexpj( const double e[AA_RESTRICT 3],
1094  const double de[AA_RESTRICT 3],
1095  double dq[AA_RESTRICT 4] );
1096 
1101 double aa_tf_qangle( const double q[AA_RESTRICT 4] );
1102 
1106 double aa_tf_qangle_rel( const double *q, const double *p );
1107 
1110 double aa_tf_quhypangle2
1111 ( const double q[AA_RESTRICT 4], const double p[AA_RESTRICT 4] );
1112 
1114 AA_API void aa_tf_qinv( const double q[AA_RESTRICT 4],
1115  double r[AA_RESTRICT 4] );
1116 
1118 AA_API void aa_tf_qadd( const double a[AA_RESTRICT 4],
1119  const double b[AA_RESTRICT 4],
1120  double c[AA_RESTRICT 4] );
1121 
1123 AA_API void aa_tf_qsub( const double a[AA_RESTRICT 4],
1124  const double b[AA_RESTRICT 4],
1125  double c[AA_RESTRICT 4] );
1126 
1128 AA_API void aa_tf_qisub( double a[AA_RESTRICT 4],
1129  const double b[AA_RESTRICT 4] );
1130 
1132 AA_API void aa_tf_qiadd( double a[AA_RESTRICT 4],
1133  const double b[AA_RESTRICT 4] );
1134 
1136 AA_API void aa_tf_qmul( const double a[AA_RESTRICT 4],
1137  const double b[AA_RESTRICT 4],
1138  double c[AA_RESTRICT 4] );
1139 
1141 AA_API void aa_tf_qmulnorm( const double a[AA_RESTRICT 4],
1142  const double b[AA_RESTRICT 4],
1143  double c[AA_RESTRICT 4] );
1144 
1146 AA_API void aa_tf_qmul_qv( const double q[AA_RESTRICT 4],
1147  const double v[AA_RESTRICT 3],
1148  double c[AA_RESTRICT 4] );
1149 
1151 AA_API void aa_tf_qmul_vq( const double v[AA_RESTRICT 3],
1152  const double q[AA_RESTRICT 4],
1153  double c[AA_RESTRICT 4] );
1154 
1156 AA_API void aa_tf_qcmul( const double a[AA_RESTRICT 4],
1157  const double b[AA_RESTRICT 4],
1158  double c[AA_RESTRICT 4] );
1159 
1161 AA_API void aa_tf_qmulc( const double a[AA_RESTRICT 4],
1162  const double b[AA_RESTRICT 4],
1163  double c[AA_RESTRICT 4] );
1164 
1165 
1169 AA_API void
1170 aa_tf_qrot1( const double q[AA_RESTRICT 4], double v[AA_RESTRICT 3] );
1171 
1173 AA_API void aa_tf_qrot( const double q[AA_RESTRICT 4],
1174  const double v[AA_RESTRICT 3],
1175  double p[AA_RESTRICT 3] );
1176 
1180 AA_API void aa_tf_qrel(const double q1[AA_RESTRICT 4],
1181  const double q2[AA_RESTRICT 4],
1182  double q_rel[AA_RESTRICT 4]);
1183 
1185 AA_API void aa_tf_qslerp( double tau, const double a[AA_RESTRICT 4],
1186  const double b[AA_RESTRICT 4],
1187  double c[AA_RESTRICT 4] );
1188 
1189 
1191 AA_API void aa_tf_qslerpalg( double tau, const double a[AA_RESTRICT 4],
1192  const double b[AA_RESTRICT 4],
1193  double c[AA_RESTRICT 4] );
1194 
1200 AA_API void aa_tf_qslerpdiff( double tau, const double a[AA_RESTRICT 4],
1201  const double b[AA_RESTRICT 4],
1202  double c[AA_RESTRICT 4] );
1203 
1204 
1207 AA_API void aa_tf_qslerpdiffalg( double tau, const double a[AA_RESTRICT 4],
1208  const double b[AA_RESTRICT 4],
1209  double c[AA_RESTRICT 4] );
1210 
1212 AA_API void aa_tf_qslerpchaindiff( double u, double du,
1213  const double q1[AA_RESTRICT 4], const double dq1[AA_RESTRICT 4],
1214  const double q2[AA_RESTRICT 4], const double dq2[AA_RESTRICT 4],
1215  double q[AA_RESTRICT 4], double dq[AA_RESTRICT 4] );
1216 
1218 AA_API void aa_tf_qslerp3diff( double u12, double du12,
1219  const double q1[AA_RESTRICT 4], const double q2[AA_RESTRICT 4],
1220  double u34, double du34,
1221  const double q3[AA_RESTRICT 4], const double q4[AA_RESTRICT 4],
1222  double u, double du,
1223  double q[AA_RESTRICT 4], double dq[AA_RESTRICT 4] );
1224 
1226 AA_API void aa_tf_qdiff2vel( const double q[AA_RESTRICT 4],
1227  const double dq_dt[AA_RESTRICT 4],
1228  double v[AA_RESTRICT 3] );
1229 
1231 AA_API void aa_tf_qvel2diff( const double q[AA_RESTRICT 4],
1232  const double v[AA_RESTRICT 3],
1233  double dq_dt[AA_RESTRICT 4] );
1234 
1235 
1237 AA_API void aa_tf_rotvec_diff2vel( const double v[3], const double dv[3],
1238  double w[3] );
1239 
1247 AA_API void aa_tf_qrk1( const double q0[AA_RESTRICT 4],
1248  const double dq[AA_RESTRICT 4],
1249  double dt,
1250  double q1[AA_RESTRICT 4] );
1251 
1260 AA_API void aa_tf_qvelrk1( const double q0[AA_RESTRICT 4],
1261  const double v[AA_RESTRICT 3],
1262  double dt,
1263  double q1[AA_RESTRICT 4] );
1264 
1265 
1274 AA_API void aa_tf_qvelrk4( const double q0[AA_RESTRICT 4],
1275  const double v[AA_RESTRICT 3],
1276  double dt,
1277  double q1[AA_RESTRICT 4] );
1278 
1279 
1287 AA_API void aa_tf_qsvel( const double q0[AA_RESTRICT 4],
1288  const double v[AA_RESTRICT 3],
1289  double dt,
1290  double q1[AA_RESTRICT 4] );
1291 
1292 
1300 AA_API void aa_tf_qsdiff( const double q0[AA_RESTRICT 4],
1301  const double dq[AA_RESTRICT 4],
1302  double dt,
1303  double q1[AA_RESTRICT 4] );
1304 
1305 
1306 AA_API void aa_tf_qsacc_rk( const double q0[AA_RESTRICT 4],
1307  const double v[AA_RESTRICT 3],
1308  const double a[AA_RESTRICT 3],
1309  double dt,
1310  double q1[AA_RESTRICT 4] );
1311 
1312 AA_API void aa_tf_qsacc( const double q0[AA_RESTRICT 4],
1313  const double v[AA_RESTRICT 3],
1314  const double a[AA_RESTRICT 3],
1315  double dt,
1316  double q1[AA_RESTRICT 4] );
1317 
1318 
1320 AA_API void aa_tf_xangle2quat( double theta_x, double q[AA_RESTRICT 4] );
1322 AA_API void aa_tf_yangle2quat( double theta_y, double q[AA_RESTRICT 4] );
1324 AA_API void aa_tf_zangle2quat( double theta_z, double q[AA_RESTRICT 4] );
1325 
1330 ( size_t n, const double *w, const double *q, size_t ldqq, double *M );
1331 
1342 ( size_t n, const double *w, const double *Q, size_t ldq, double *y );
1343 
1344 
1345 
1349 AA_API void aa_tf_qmatrix_l( const double *q, double *M, size_t ldm );
1350 
1354 AA_API void aa_tf_qmatrix_r( const double *q, double *M, size_t ldm );
1355 
1356 
1357 AA_API void aa_tf_qmat_l( const double *q, struct aa_dmat *M );
1358 AA_API void aa_tf_qmat_r( const double *q, struct aa_dmat *M );
1359 
1360 
1362 void aa_tf_qurand( double q[4] );
1363 
1364 /*********/
1365 /* Axang */
1366 /*********/
1367 
1369 AA_API void aa_tf_axang_make( double x, double y, double z, double theta,
1370  double axang[AA_RESTRICT 4] );
1371 
1374 AA_API void aa_tf_axang_permute( const double rv[AA_RESTRICT 4], int k,
1375  double rv_p[AA_RESTRICT 4] );
1376 
1378 AA_API void aa_tf_rotvec_permute( const double rv[AA_RESTRICT 3], int k,
1379  double rv_p[AA_RESTRICT 3] );
1380 
1383 AA_API void aa_tf_rotvec_near( const double rv[AA_RESTRICT 3],
1384  const double rv_near[AA_RESTRICT 3],
1385  double rv_p[AA_RESTRICT 3] );
1389 AA_API void aa_tf_axang_rot( const double axang[AA_RESTRICT 4],
1390  const double p[AA_RESTRICT 3],
1391  double q[AA_RESTRICT 3] );
1392 
1393 /**************/
1394 /* Quat-Trans */
1395 /**************/
1396 
1398 void aa_tf_qv_conj( const double q[4], const double v[3],
1399  double qc[4], double vc[3] );
1400 
1401 
1405 #define AA_TF_QUTR_Q 0
1406 
1411 #define AA_TF_QUTR_V 4
1412 
1414 void aa_tf_qutr2duqu( const double e[7], double s[8] );
1415 
1417 void aa_tf_qutr2tfmat( const double e[7], double T[12] );
1418 
1420 void aa_tf_tfmat2qutr( const double T[12], double e[7] );
1421 
1423 void aa_tf_duqu2qutr( const double s[8], double e[7] );
1424 
1426 void aa_tf_qutr_mul( const double a[7], const double b[7], double c[7] ) ;
1427 
1431 AA_API void
1432 aa_tf_qutr_tf( const double E[AA_RESTRICT 7], const double p0[AA_RESTRICT 3],
1433  double p1[AA_RESTRICT 3] );
1434 
1436 void aa_tf_qutr_mulnorm( const double a[7], const double b[7], double c[7] ) ;
1437 
1439 void aa_tf_qutr_conj( const double a[7], double c[7] ) ;
1440 
1442 void aa_tf_qutr_mulc( const double a[7], const double b[7], double c[7] ) ;
1443 
1445 void aa_tf_qutr_cmul( const double a[7], const double b[7], double c[7] ) ;
1446 
1450 AA_API void aa_tf_qv_expv
1451 ( const double w[3], const double dv[3],
1452  double q[4], double v[3] );
1453 
1457 AA_API void aa_tf_qv_lnv
1458 ( const double q[4], const double v[3],
1459  double w[3], double dv[3] );
1460 
1465 ( const double w[6], double e[7] );
1466 
1471 ( const double e[7], double w[6] );
1472 
1475 ( const double e[7], const double w[6], double dx[6] );
1476 
1479 ( const double e[7], const double de[7], double dx[6] );
1480 
1483 ( const double e[7], const double dx[6], double de[7] );
1484 
1485 
1486 
1488 AA_API void
1489 aa_tf_qv_vel2twist( const double q[AA_RESTRICT 4], const double v[AA_RESTRICT 3],
1490  const double w[AA_RESTRICT 3], const double dv[AA_RESTRICT 3],
1491  double tw[AA_RESTRICT 3], double tv[AA_RESTRICT 3] );
1492 
1494 AA_API void
1495 aa_tf_qv_twist2vel( const double q[AA_RESTRICT 4], const double v[AA_RESTRICT 3],
1496  const double tw[AA_RESTRICT 3], const double tv[AA_RESTRICT 3],
1497  double w[AA_RESTRICT 3], double dv[AA_RESTRICT 3] );
1498 
1500 void aa_tf_qutr_svel
1501 ( const double e0[7], const double dx[6], double dt, double e1[7] );
1502 
1504 void aa_tf_qutr_sdiff
1505 ( const double e0[7], const double de[7], double dt, double e1[7] );
1506 
1508 void aa_tf_qutr_wavg
1509 ( size_t n, const double *w, const double *EE, size_t ldee, double *a );
1510 
1512 void aa_tf_qutr_rand( double E[7] );
1513 
1514 /***************/
1515 /* Conversions */
1516 /***************/
1517 
1519 AA_API void aa_tf_quat2axang( const double q[AA_RESTRICT 4],
1520  double axang[AA_RESTRICT 4] );
1521 
1522 
1524 AA_API void aa_tf_axang2quat( const double axang[AA_RESTRICT 4],
1525  double q[AA_RESTRICT 4] );
1526 
1528 AA_API void aa_tf_axang2quat2( const double axis[AA_RESTRICT 3],
1529  double angle,
1530  double q[AA_RESTRICT 4] );
1531 
1532 
1536 AA_API void
1537 aa_tf_vecs2quat( const double u[AA_RESTRICT 3],
1538  const double v[AA_RESTRICT 3],
1539  double q[AA_RESTRICT 4] );
1540 
1541 
1543 AA_API void aa_tf_axang2rotvec( const double axang[AA_RESTRICT 4],
1544  double rotvec[AA_RESTRICT 3] );
1545 
1547 AA_API void aa_tf_rotvec2axang( const double rotvec[AA_RESTRICT 3],
1548  double axang[AA_RESTRICT 4] );
1549 
1551 AA_API void aa_tf_rotvec2quat( const double rotvec[AA_RESTRICT 3],
1552  double q[AA_RESTRICT 4] );
1554 AA_API void aa_tf_quat2rotvec( const double q[AA_RESTRICT 4],
1555  double rotvec[AA_RESTRICT 3] );
1556 
1557 
1559 AA_API void aa_tf_quat2rotvec_near( const double q[AA_RESTRICT 4],
1560  const double rv_near[AA_RESTRICT 3],
1561  double rotvec[AA_RESTRICT 3] );
1562 
1564 AA_API void aa_tf_quat2rotmat( const double quat[AA_RESTRICT 4],
1565  double rotmat[AA_RESTRICT 9] );
1567 AA_API void aa_tf_rotmat2quat( const double rotmat[AA_RESTRICT 9],
1568  double quat[AA_RESTRICT 4] );
1569 
1571 AA_API void aa_tf_rotmat2axang( const double R[AA_RESTRICT 9],
1572  double ra[AA_RESTRICT 4] );
1574 AA_API void aa_tf_rotmat2rotvec( const double R[AA_RESTRICT 9],
1575  double rv[AA_RESTRICT 3] );
1576 
1578 AA_API void aa_tf_axang2rotmat( const double ra[AA_RESTRICT 4],
1579  double R[AA_RESTRICT 9] );
1580 
1582 AA_API void aa_tf_axang2rotmat2( const double axis[AA_RESTRICT 3],
1583  double angle,
1584  double R[AA_RESTRICT 9] );
1585 
1587 AA_API void aa_tf_rotvec2rotmat( const double rv[AA_RESTRICT 3],
1588  double R[AA_RESTRICT 9] );
1589 
1590 
1592 AA_API void aa_tf_qv2tfmat( const double q[AA_RESTRICT 4], const double v[AA_RESTRICT 3],
1593  double T[AA_RESTRICT 12] ) ;
1594 
1595 /* AA_API void aa_tf_tfv2tfq( const double vrv[AA_RESTRICT 6], */
1596 /* double x[AA_RESTRICT 3], double quat[AA_RESTRICT 4] ); */
1597 /* AA_API void aa_tf_tfq2tfv( const double x[AA_RESTRICT 3], const double quat[AA_RESTRICT 4], */
1598 /* double vrv[AA_RESTRICT 6] ); */
1599 
1601 AA_API void aa_tf_rotmat2eulerzyx( const double R[AA_RESTRICT 9],
1602  double e[AA_RESTRICT 3] );
1603 
1604 
1606 AA_API void aa_tf_quat2eulerzyx( const double q[AA_RESTRICT 4],
1607  double e[AA_RESTRICT 3] );
1608 
1612 #define AA_TF_DEF_EULER(letters) \
1613  AA_API void \
1614  aa_tf_euler ## letters ## 2rotmat( double e1, double e2, double e3, \
1615  double R[AA_RESTRICT 9] ); \
1616  AA_API void \
1617  aa_tf_euler ## letters ## 2quat( double e1, double e2, double e3, \
1618  double q[AA_RESTRICT 4] );
1619 
1620 AA_TF_DEF_EULER( xyz )
1621 AA_TF_DEF_EULER( xzy )
1622 
1623 AA_TF_DEF_EULER( yxz )
1624 AA_TF_DEF_EULER( yzx )
1625 
1626 AA_TF_DEF_EULER( zyx )
1627 AA_TF_DEF_EULER( zxy )
1628 
1629 AA_TF_DEF_EULER( xyx )
1630 AA_TF_DEF_EULER( xzx )
1631 
1632 AA_TF_DEF_EULER( yxy )
1633 AA_TF_DEF_EULER( yzy )
1634 
1635 AA_TF_DEF_EULER( zxz )
1636 AA_TF_DEF_EULER( zyz )
1637 
1638 
1639 AA_API void aa_tf_xangle2rotmat( double theta_x, double R[AA_RESTRICT 9] );
1641 AA_API void aa_tf_yangle2rotmat( double theta_y, double R[AA_RESTRICT 9] );
1643 AA_API void aa_tf_zangle2rotmat( double theta_z, double R[AA_RESTRICT 9] );
1644 
1646 AA_API void aa_tf_xangle2axang( double theta_x, double axang[AA_RESTRICT 4] );
1648 AA_API void aa_tf_yangle2axang( double theta_y, double axang[AA_RESTRICT 4] );
1650 AA_API void aa_tf_zangle2axang( double theta_z, double axang[AA_RESTRICT 4] );
1651 
1653 AA_API void aa_tf_axang_normalize( double axang[AA_RESTRICT 4] );
1654 
1660 AA_API void
1661 aa_tf_rotmat_mzlook( const double eye[AA_RESTRICT 3],
1662  const double target[AA_RESTRICT 3],
1663  const double up[AA_RESTRICT 3],
1664  double R[AA_RESTRICT 9] );
1665 
1666 
1670 AA_API void
1671 aa_tf_tfmat_mzlook( const double eye[AA_RESTRICT 3],
1672  const double target[AA_RESTRICT 3],
1673  const double up[AA_RESTRICT 3],
1674  double T[AA_RESTRICT 12] );
1675 
1679 AA_API void
1680 aa_tf_qmzlook( const double eye[AA_RESTRICT 3],
1681  const double target[AA_RESTRICT 3],
1682  const double up[AA_RESTRICT 3],
1683  double q[AA_RESTRICT 4] );
1684 
1688 AA_API void
1689 aa_tf_qv_mzlook( const double eye[AA_RESTRICT 3],
1690  const double target[AA_RESTRICT 3],
1691  const double up[AA_RESTRICT 3],
1692  double q[AA_RESTRICT 4],
1693  double v[AA_RESTRICT 3] );
1694 
1698 AA_API void
1699 aa_tf_qutr_mzlook( const double eye[AA_RESTRICT 3],
1700  const double target[AA_RESTRICT 3],
1701  const double up[AA_RESTRICT 3],
1702  double T[AA_RESTRICT 12] );
1703 
1704 
1705 /* Dual Quaternions */
1706 
1708 AA_API void aa_tf_duqu_add( const double d1[AA_RESTRICT 8], const double d2[AA_RESTRICT 8],
1709  double d3[AA_RESTRICT 8] );
1710 
1712 AA_API void aa_tf_duqu_sub( const double d1[AA_RESTRICT 8], const double d2[AA_RESTRICT 8],
1713  double d3[AA_RESTRICT 8] );
1714 
1716 AA_API void aa_tf_duqu_smul( double alpha, const double x[AA_RESTRICT 8],
1717  double y[AA_RESTRICT 8] );
1718 
1720 AA_API void aa_tf_duqu_mul( const double d1[AA_RESTRICT 8], const double d2[AA_RESTRICT 8],
1721  double d3[AA_RESTRICT 8] );
1722 
1726 AA_API void aa_tf_duqu_matrix_l( const double *q, double *M, size_t ldm );
1727 
1731 AA_API void aa_tf_duqu_matrix_r( const double *q, double *M, size_t ldm );
1732 
1736 AA_API void aa_tf_duqu_mat_l( const double *q, struct aa_dmat *M );
1737 
1741 AA_API void aa_tf_duqu_mat_r( const double *q, struct aa_dmat *M );
1742 
1744 AA_API void aa_tf_duqu_cmul( const double d1[AA_RESTRICT 8], const double d2[AA_RESTRICT 8],
1745  double d3[AA_RESTRICT 8] );
1746 
1748 AA_API void aa_tf_duqu_mulc( const double d1[AA_RESTRICT 8], const double d2[AA_RESTRICT 8],
1749  double d3[AA_RESTRICT 8] );
1750 
1752 AA_API void aa_tf_duqu_conj( const double d[AA_RESTRICT 8], double dconj[AA_RESTRICT 8] );
1753 
1755 AA_API void aa_tf_duqu_conj1( double s[AA_RESTRICT 8] );
1756 
1758 AA_API void aa_tf_duqu_exp( const double d[AA_RESTRICT 8], double e[AA_RESTRICT 8] );
1759 
1761 AA_API void aa_tf_duqu_ln( const double d[AA_RESTRICT 8], double e[AA_RESTRICT 8] );
1762 
1764 AA_API void aa_tf_duqu_norm( const double d[AA_RESTRICT 8], double *nreal, double *ndual );
1765 
1767 AA_API void aa_tf_duqu_normalize( double d[AA_RESTRICT 8] );
1768 
1770 AA_API void aa_tf_duqu_minimize( double d[AA_RESTRICT 8] );
1771 
1773 AA_API void aa_tf_tf_duqu( const double d[AA_RESTRICT 8], const double p0[AA_RESTRICT 3],
1774  double p1[AA_RESTRICT 3] );
1775 
1777 AA_API void aa_tf_duqu_tf( const double d[AA_RESTRICT 8], const double p0[AA_RESTRICT 3],
1778  double p1[AA_RESTRICT 3] );
1779 
1781 AA_API void aa_tf_duqu_trans( const double d[AA_RESTRICT 8], double v[AA_RESTRICT 3] );
1782 
1784 AA_API void aa_tf_duqu2tfmat( const double d[AA_RESTRICT 8], double T[AA_RESTRICT 12] );
1785 
1787 AA_API void aa_tf_tfmat2duqu( const double T[AA_RESTRICT 12], double d[AA_RESTRICT 8] ) ;
1788 
1790 AA_API void aa_tf_qv2duqu( const double q[AA_RESTRICT 4], const double v[AA_RESTRICT 3],
1791  double d[AA_RESTRICT 8] ) ;
1792 
1794 AA_API void aa_tf_xyz2duqu( double x, double y, double z,
1795  double d[AA_RESTRICT 8] ) ;
1796 
1798 AA_API void aa_tf_xxyz2duqu( double theta, double x, double y, double z,
1799  double d[AA_RESTRICT 8] );
1800 
1802 AA_API void aa_tf_yxyz2duqu( double theta, double x, double y, double z,
1803  double d[AA_RESTRICT 8] );
1804 
1806 AA_API void aa_tf_zxyz2duqu( double theta, double x, double y, double z,
1807  double d[AA_RESTRICT 8] );
1808 
1810 AA_API void aa_tf_duqu2qv( const double d[AA_RESTRICT 8],
1811  double q[AA_RESTRICT 4], double v[AA_RESTRICT 3] ) ;
1812 
1814 void aa_tf_tfmat2av( const double T[12], double q[AA_RESTRICT 4], double v[AA_RESTRICT 3] ) ;
1815 
1817 AA_API void aa_tf_duqu_vel2twist( const double d[AA_RESTRICT 8], const double dx[AA_RESTRICT 6],
1818  double t[AA_RESTRICT 8] ) ;
1819 
1821 AA_API void aa_tf_duqu_twist2vel( const double d[AA_RESTRICT 8], const double t[AA_RESTRICT 8],
1822  double dx[AA_RESTRICT 6] ) ;
1823 
1825 AA_API void aa_tf_duqu_twist2diff( const double d[AA_RESTRICT 8], const double t[AA_RESTRICT 8],
1826  double dd[AA_RESTRICT 8] ) ;
1827 
1829 AA_API void aa_tf_duqu_vel2diff( const double d[AA_RESTRICT 8], const double dx[AA_RESTRICT 6],
1830  double dd[AA_RESTRICT 8] ) ;
1831 
1833 AA_API void aa_tf_duqu_diff2vel( const double d[AA_RESTRICT 8], const double dd[AA_RESTRICT 8],
1834  double dx[AA_RESTRICT 6] ) ;
1835 
1838 AA_API void aa_tf_duqu_diff2twist( const double d[AA_RESTRICT 8], const double dd[AA_RESTRICT 8],
1839  double twist[AA_RESTRICT 8] ) ;
1840 
1847 AA_API void aa_tf_duqu_stwist( const double d0[AA_RESTRICT 8], const double twist[AA_RESTRICT 8],
1848  double dt, double d1[AA_RESTRICT 6] ) ;
1849 
1856 AA_API void aa_tf_duqu_svel( const double d0[AA_RESTRICT 8], const double dx[AA_RESTRICT 6],
1857  double dt, double d1[AA_RESTRICT 6] ) ;
1858 
1865 AA_API void aa_tf_duqu_sdiff( const double d0[AA_RESTRICT 8], const double dd[AA_RESTRICT 8],
1866  double dt, double d1[AA_RESTRICT 6] ) ;
1867 
1868 
1890 AA_API void
1891 aa_tf_duqu_jac_vel2diff(const double S[8], const struct aa_dmat *Jvel,
1892  struct aa_dmat *Js );
1893 
1894 
1901 AA_API void
1902 aa_tf_qln_jac(const double q[4], struct aa_dmat *J );
1903 
1907 AA_API void
1908 aa_tf_duqu_conj_jac( struct aa_dmat *J );
1909 
1916 AA_API void
1917 aa_tf_duqu_ln_jac(const double S[8], struct aa_dmat *J );
1918 
1921 AA_API void
1922 aa_tf_duqu2pure( const double S[AA_RESTRICT 8],
1923  double v[AA_RESTRICT 6] );
1924 
1927 AA_API void
1928 aa_tf_pure2duqu( const double v[AA_RESTRICT 6],
1929  double S[AA_RESTRICT 8]);
1930 
1931 
1932 
1933 
1934 /* DH Parameter Conversions
1935  *
1936  * Conventions:
1937  * - Proximal: Craig and Lynch & Park
1938  * - Distal: Murray, Li, & Sastry
1939  */
1940 
1941 
1942 AA_API void
1943 aa_tf_dhprox2tfmat( double alpha, double a, double d, double phi,
1944  double T[AA_RESTRICT 12]);
1945 AA_API void
1946 aa_tf_dhprox2duqu( double alpha, double a, double d, double phi,
1947  double S[AA_RESTRICT 8]);
1948 AA_API void
1949 aa_tf_dhprox2qutr( double alpha, double a, double d, double phi,
1950  double E[AA_RESTRICT 7]);
1951 AA_API void
1952 aa_tf_dhprox2qv( double alpha, double a, double d, double phi,
1953  double q[AA_RESTRICT 4], double v[3]);
1954 
1955 AA_API void
1956 aa_tf_dhdist2tfmat( double alpha, double a, double d, double phi,
1957  double T[AA_RESTRICT 12]);
1958 AA_API void
1959 aa_tf_dhdist2duqu( double alpha, double a, double d, double phi,
1960  double S[AA_RESTRICT 8]);
1961 AA_API void
1962 aa_tf_dhdist2qutr( double alpha, double a, double d, double phi,
1963  double E[AA_RESTRICT 7]);
1964 AA_API void
1965 aa_tf_dhdist2qv( double alpha, double a, double d, double phi,
1966  double q[AA_RESTRICT 4], double v[3]);
1967 
1968 /* Misc */
1969 
1970 AA_API void
1971 aa_tf_relx_mean( size_t n, const double *R,
1972  const double *X, size_t ldx,
1973  const double *Y, size_t ldy,
1974  double rel[3]);
1975 
1976 AA_API void
1977 aa_tf_relx_median( size_t n, const double *R,
1978  const double *X, size_t ldx,
1979  const double *Y, size_t ldy,
1980  double rel[3]);
1981 
1991 AA_API
1992 void aa_tf_proj( const double a[AA_RESTRICT 3],
1993  const double b[AA_RESTRICT 3],
1994  double c[AA_RESTRICT 3] );
1995 
2004 AA_API
2005 void aa_tf_proj_orth( const double a[AA_RESTRICT 3],
2006  const double b[AA_RESTRICT 3],
2007  double c[AA_RESTRICT 3] );
2008 
2009 
2010 #ifdef __cplusplus
2011 }
2012 #endif
2013 
2014 #endif //AMINO_TF_H
AA_API void aa_tf_qln_jac(const double q[4], struct aa_dmat *J)
Compute the Jacobian of the quaternion logarithm.
AA_API void aa_tf_qminimize(double q[AA_RESTRICT 4])
Minimize angle represented by the quaternion.
AA_API void aa_tf_yangle2axang(double theta_y, double axang[AA_RESTRICT 4])
Angle about y axis.
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:989
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:497
static void aa_tf_sinccos2(double theta2, double *sc, double *c)
Compute sinc(theta), cos(theta), given the square of theta.
Definition: tf.h:104
double aa_tf_qssd(const double a[AA_RESTRICT 4], const double b[AA_RESTRICT 4])
Sum-square-differences of two quaternions.
double y
y component
Definition: tf.h:223
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:502
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_qconj1(double q[AA_RESTRICT 4])
Quaternion conjugate, in-place.
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:441
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:268
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.
AA_API void aa_tf_qiadd(double a[AA_RESTRICT 4], const double b[AA_RESTRICT 4])
Quaternion addition, in-place.
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:245
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_zangle2axang(double theta_z, double axang[AA_RESTRICT 4])
Angle about z axis.
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:160
static const double aa_tf_rotmat_ident[9]
Identity rotation matrix array.
Definition: tf.h:472
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:431
static const double aa_tf_ident[12]
Identity transformation matrix array.
Definition: tf.h:462
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_duqu_conj1(double s[AA_RESTRICT 8])
Dual quaternion conjugate, in-place.
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:264
double y
yaw
Definition: tf.h:244
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:411
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:163
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:246
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_xangle2axang(double theta_x, double axang[AA_RESTRICT 4])
Angle about x axis.
double aa_tf_vssd(const double a[AA_RESTRICT 3], const double b[AA_RESTRICT 3])
Sum-square-differences of two vectors.
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:167
static const double aa_tf_duqu_ident[8]
Identity dual quaternion array.
Definition: tf.h:482
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_duqu_conj_jac(struct aa_dmat *J)
Fill J with Jacobian of dual quaternion conjugate.
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
AA_API void aa_tf_duqu_mat_r(const double *q, struct aa_dmat *M)
Construct matrix for right dual quaternion multiply p*q = M*p.
double x
x component
Definition: tf.h:222
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_duqu_jac_vel2diff(const double S[8], const struct aa_dmat *Jvel, struct aa_dmat *Js)
Convert a velocity Jacobian to a dual quaternion derivative Jacobian.
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_axang_normalize(double axang[AA_RESTRICT 4])
Normalize the axis.
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:164
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:176
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.
Block matrix descriptors and linear algebra operations.
struct aa_tf_tfmat aa_tf_tfmat_t
Memory layout for a transformation matrix.
AA_API void aa_tf_qscal(double q[AA_RESTRICT 4], double alpha)
Scale a quaternion.
double z
z component
Definition: tf.h:224
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_cross_mat_l(const double v[3], struct aa_dmat *M)
Construct matrix for left cross product v*a = M*a.
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:165
AA_API void aa_tf_qv_twist2vel(const double q[AA_RESTRICT 4], const double v[AA_RESTRICT 3], const double tw[AA_RESTRICT 3], const double tv[AA_RESTRICT 3], double w[AA_RESTRICT 3], double dv[AA_RESTRICT 3])
Quaternion-vector twist to velocity.
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.
void aa_tf_qutr_twist2vel(const double e[7], const double w[6], double dx[6])
Quaternion-translation twist to velocity.
AA_API void aa_tf_qisub(double a[AA_RESTRICT 4], const double b[AA_RESTRICT 4])
Quaternion subtraction, in-place.
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:492
static const double aa_tf_vec_y[3]
A Y axis.
Definition: tf.h:512
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:121
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_qv_vel2twist(const double q[AA_RESTRICT 4], const double v[AA_RESTRICT 3], const double w[AA_RESTRICT 3], const double dv[AA_RESTRICT 3], double tw[AA_RESTRICT 3], double tv[AA_RESTRICT 3])
Quaternion-vector velocity to twist.
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:467
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:330
AA_API void aa_tf_duqu_tf(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_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:346
Memory layout for a quaternion, x,y,z,w order.
Definition: tf.h:219
#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:879
Memory layout for an Euler Angle in ZYX (yaw-pitch-roll) format.
Definition: tf.h:241
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:912
AA_API void aa_tf_duqu_mat_l(const double *q, struct aa_dmat *M)
Construct matrix for left dual quaternion multiply q*p = M*p.
static const double aa_tf_vec_z[3]
A Z axis.
Definition: tf.h:517
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:333
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:507
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:436
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:317
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:863
AA_API void aa_tf_axang_rot(const double axang[AA_RESTRICT 4], const double p[AA_RESTRICT 3], double q[AA_RESTRICT 3])
Rotate a point.
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:137
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:416
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:921
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:190
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
Descriptor for a block matrix.
Definition: mat.h:69
double w
w component
Definition: tf.h:225
AA_API void aa_tf_duqu_normalize(double d[AA_RESTRICT 8])
Dual quaternion normalization.
AA_API void aa_tf_cross_mat_r(const double v[3], struct aa_dmat *M)
Construct matrix for right cross product a*v = M*a.
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:477
static const double aa_tf_qutr_ident[7]
Identity quaternion-translation array.
Definition: tf.h:487
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:870
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:421
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:302
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_smul(double alpha, const double x[AA_RESTRICT 8], double y[AA_RESTRICT 8])
Dual quaternion scalar multiplication.
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:80
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.
double aa_tf_vnorm(const double a[AA_RESTRICT 3])
Norm of vector.
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_ln_jac(const double S[8], struct aa_dmat *J)
Compute the Jacobian of the dual quaternion logarithm.
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_cross_a(const double a[AA_RESTRICT 3], const double b[AA_RESTRICT 3], double c[AA_RESTRICT 3])
Cross-product accumulate.
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:232
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:1612
#define AA_TF_DUQU_IDENT_INITIALIZER
Static initializer for an identity dual quaternion.
Definition: tf.h:426
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:902
void aa_tf_tfmat2av(const double T[12], double q[AA_RESTRICT 4], double v[AA_RESTRICT 3])
transformation matrix to quaternion-translation