amino  1.0-beta2
Lightweight Robot Utility Library
 All Classes Namespaces Files Functions Variables Typedefs Enumerations Enumerator Macros Pages
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 
601 AA_API void aa_tf_skewsym_scal2( double a, double b, const double u[3], double R[9] );
602 
606 AA_API void
607 aa_tf_skewsym_scal_c( const double u[AA_RESTRICT 3],
608  const double a[AA_RESTRICT 3], const double b[AA_RESTRICT 3],
609  double R[9] );
610 
611 
612 /* Multiply */
613 
617 AA_API void aa_tf_rotmat_mul( const double R0[AA_RESTRICT 9],
618  const double R1[AA_RESTRICT 9],
619  double R[AA_RESTRICT 9] );
620 
624 AA_API void aa_tf_tfmat_mul( const double T0[AA_RESTRICT 12],
625  const double T1[AA_RESTRICT 12],
626  double T[AA_RESTRICT 12] );
627 
631 AA_API void aa_tf_tfmat2_mul( const double R0[AA_RESTRICT 9],
632  const double v0[AA_RESTRICT 3],
633  const double R1[AA_RESTRICT 9],
634  const double v1[AA_RESTRICT 3],
635  double R[AA_RESTRICT 9], double v[AA_RESTRICT 3] );
636 
637 
638 /* Inverting Multiply */
644 AA_API void aa_tf_rotmat_imul( const double R0[AA_RESTRICT 9],
645  const double R1[AA_RESTRICT 9],
646  double R[AA_RESTRICT 9] );
647 
653 AA_API void aa_tf_rotmat_muli( const double R0[AA_RESTRICT 9],
654  const double R1[AA_RESTRICT 9],
655  double R[AA_RESTRICT 9] );
656 
662 AA_API void aa_tf_tfmat_imul( const double T0[AA_RESTRICT 12],
663  const double T1[AA_RESTRICT 12],
664  double T[AA_RESTRICT 12] );
665 
671 AA_API void aa_tf_tfmat_muli( const double T0[AA_RESTRICT 12],
672  const double T1[AA_RESTRICT 12],
673  double T[AA_RESTRICT 12] );
674 
680 AA_API void aa_tf_tfmat2_imul( const double R0[AA_RESTRICT 9],
681  const double v0[AA_RESTRICT 3],
682  const double R1[AA_RESTRICT 9],
683  const double v1[AA_RESTRICT 3],
684  double R[AA_RESTRICT 9], double v[AA_RESTRICT 3] );
685 
691 AA_API void aa_tf_tfmat2_muli( const double R0[AA_RESTRICT 9],
692  const double v0[AA_RESTRICT 3],
693  const double R1[AA_RESTRICT 9],
694  const double v1[AA_RESTRICT 3],
695  double R[AA_RESTRICT 9], double v[AA_RESTRICT 3] );
696 
697 /* Transform */
698 
702 AA_API void aa_tf_rotmat_rot( const double R[AA_RESTRICT 9],
703  const double p0[AA_RESTRICT 3],
704  double p1[AA_RESTRICT 3] );
705 
709 AA_API void aa_tf_tfmat_tf( const double T[AA_RESTRICT 12],
710  const double p0[AA_RESTRICT 3],
711  double p1[AA_RESTRICT 3] );
712 
716 AA_API void aa_tf_tfmat2_tf( const double R[AA_RESTRICT 9],
717  const double v[AA_RESTRICT 3],
718  const double p0[AA_RESTRICT 3],
719  double p1[AA_RESTRICT 4] );
720 
724 AA_API void aa_tf_rotmat_inv1( double R[AA_RESTRICT 9] );
725 
729 AA_API void aa_tf_rotmat_inv2( const double R[AA_RESTRICT 9],
730  double Ri[AA_RESTRICT 9] );
731 
735 AA_API void aa_tf_tfmat_inv1( double T[AA_RESTRICT 12] );
736 
740 AA_API void aa_tf_tfmat_inv2( const double T[AA_RESTRICT 12],
741  double Ti[AA_RESTRICT 12] );
742 
744 AA_API int aa_tf_isrotmat( const double R[AA_RESTRICT 9] );
745 
746 
748 AA_API void aa_tf_9mul( const double R0[AA_RESTRICT 9],
749  const double R1[AA_RESTRICT 9],
750  double R[AA_RESTRICT 9] );
752 AA_API void aa_tf_9rot( const double R[AA_RESTRICT 9],
753  const double p0[AA_RESTRICT 3],
754  double p1[AA_RESTRICT 3] );
755 
757 AA_API void aa_tf_9rel( const double R1[AA_RESTRICT 9], const double R2[AA_RESTRICT 9],
758  double Ri[AA_RESTRICT 9] );
759 
761 AA_API void aa_tf_rotmat_exp_aa( const double aa[AA_RESTRICT 4], double R[AA_RESTRICT 9] );
762 
764 AA_API void aa_tf_rotmat_expv( const double rv[AA_RESTRICT 3], double R[AA_RESTRICT 9] );
765 
767 AA_API void aa_tf_rotmat_lnv( const double R[AA_RESTRICT 9], double v[AA_RESTRICT 3] );
768 
770 AA_API void aa_tf_tfmat_expv( const double v[AA_RESTRICT 6], double T[AA_RESTRICT 12] );
771 
773 AA_API void aa_tf_tfmat_lnv( const double T[AA_RESTRICT 12], double v[AA_RESTRICT 6] );
774 
776 AA_API void aa_tf_rotmat_vel2diff( const double R[AA_RESTRICT 9],
777  const double w[AA_RESTRICT 3], double dR[AA_RESTRICT 9] );
778 
780 AA_API void aa_tf_rotmat_diff2vel( const double R[AA_RESTRICT 9],
781  const double dR[AA_RESTRICT 9], double w[AA_RESTRICT 3] );
782 
784 AA_API void aa_tf_rotmat_svel( const double R0[AA_RESTRICT 9],
785  const double w[AA_RESTRICT 3], double dt,
786  double R1[AA_RESTRICT 9] );
787 
788 
789 
791 AA_API void aa_tf_tfmat_svel( const double T0[AA_RESTRICT 12],
792  const double w[AA_RESTRICT 3], double dt,
793  double T1[AA_RESTRICT 12] );
794 
796 AA_API void aa_tf_v9mul( double R[AA_RESTRICT 9],
797  const double R1[AA_RESTRICT 9], const double R2[AA_RESTRICT 9], ... );
798 
799 
803 AA_API void
804 aa_tf_rotmat_xy( const double x_axis[AA_RESTRICT 3],
805  const double y_axis[AA_RESTRICT 3],
806  double R[AA_RESTRICT 9] );
810 AA_API void
811 aa_tf_rotmat_yz( const double y_axis[AA_RESTRICT 3],
812  const double z_axis[AA_RESTRICT 3],
813  double R[AA_RESTRICT 9] );
814 
818 AA_API void
819 aa_tf_rotmat_zx( const double z_axis[AA_RESTRICT 3],
820  const double x_axis[AA_RESTRICT 3],
821  double R[AA_RESTRICT 9] );
822 
823 
824 /***********/
825 /* Vectors */
826 /***********/
827 
833 #define AA_TF_DOTX(a,b) \
834  ((a)[0]*(b)[0] + (a)[1]*(b)[1] + (a)[2]*(b)[2])
835 
839 static inline double
840 AA_TF_VDOT( const double a[AA_RESTRICT 3], const double b[AA_RESTRICT 3] )
841 {
842  return AA_TF_DOTX(a,b);
843 }
844 
848 static inline float
849 AA_TF_VDOTF( const float a[AA_RESTRICT 3], const float b[AA_RESTRICT 3] )
850 {
851  return AA_TF_DOTX(a,b);
852 }
853 
854 
858 AA_API double
859 aa_tf_vdot( const double a[AA_RESTRICT 3], const double b[AA_RESTRICT 3] );
860 
864 AA_API float
865 aa_tf_vdotf( const float a[AA_RESTRICT 3], const float b[AA_RESTRICT 3] );
866 
872 #define AA_TF_CROSSX( a, b, c ) \
873  (c)[0] = (a)[1]*(b)[2] - (a)[2]*(b)[1]; \
874  (c)[1] = (a)[2]*(b)[0] - (a)[0]*(b)[2]; \
875  (c)[2] = (a)[0]*(b)[1] - (a)[1]*(b)[0]; \
876 
877 
881 static inline void
882 AA_TF_CROSS( const double a[AA_RESTRICT 3], const double b[AA_RESTRICT 3], double c[AA_RESTRICT 3] )
883 {
884  AA_TF_CROSSX(a,b,c);
885 }
886 
890 static inline void
891 AA_TF_CROSSF( const float a[AA_RESTRICT 3], const float b[AA_RESTRICT 3], float c[AA_RESTRICT 3] )
892 {
893  AA_TF_CROSSX(a,b,c);
894 }
895 
899 AA_API void aa_tf_cross( const double a[AA_RESTRICT 3], const double b[AA_RESTRICT 3],
900  double c[AA_RESTRICT 3] ) ;
901 
902 
906 AA_API void aa_tf_crossf( const float a[AA_RESTRICT 3], const float b[AA_RESTRICT 3],
907  float c[AA_RESTRICT 3] ) ;
908 
912 AA_API void aa_tf_vnormalize( double v[AA_RESTRICT 3] );
913 
914 
918 AA_API void aa_tf_vnormalizef( float v[AA_RESTRICT 3] );
919 
920 
921 /***************/
922 /* Quaternions */
923 /***************/
924 
928 static inline double
929 AA_TF_QDOT( const double a[AA_RESTRICT 4], const double b[AA_RESTRICT 4] )
930 {
931  return a[0]*b[0] + a[1]*b[1] + a[2]*b[2] + a[3]*b[3];
932 }
933 
937 double
938 aa_tf_qdot( const double a[AA_RESTRICT 4], const double b[AA_RESTRICT 4] );
939 
943 AA_API void aa_tf_qnormalize( double q[AA_RESTRICT 4] );
944 
945 
947 AA_API double aa_tf_qnorm( const double q[AA_RESTRICT 4] );
948 
953 AA_API void aa_tf_qminimize( double q[AA_RESTRICT 4] );
954 
959 AA_API void aa_tf_qminimize2( const double q[AA_RESTRICT 4], double qmin[AA_RESTRICT 4] );
960 
963 AA_API void aa_tf_qnormalize2( const double q[AA_RESTRICT 4], double qnorm[AA_RESTRICT 4] );
964 
966 AA_API void aa_tf_qconj( const double q[AA_RESTRICT 4],
967  double r[AA_RESTRICT 4] );
968 
969 
971 AA_API void aa_tf_qexp( const double q[AA_RESTRICT 4],
972  double r[AA_RESTRICT 4] );
973 
974 
976 AA_API void aa_tf_qln( const double q[AA_RESTRICT 4],
977  double r[AA_RESTRICT 4] );
978 
985 AA_API void aa_tf_qduln( const double q[AA_RESTRICT 4],
986  const double dq[AA_RESTRICT 4],
987  double dln[AA_RESTRICT 3] );
988 
996 AA_API void aa_tf_qdulnj( const double q[AA_RESTRICT 4],
997  const double dq[AA_RESTRICT 4],
998  double dln[AA_RESTRICT 3] );
999 
1006 AA_API void aa_tf_qdpexp( const double e[AA_RESTRICT 3],
1007  const double de[AA_RESTRICT 3],
1008  double dq[AA_RESTRICT 4] );
1009 
1017 AA_API void aa_tf_qdpexpj( const double e[AA_RESTRICT 3],
1018  const double de[AA_RESTRICT 3],
1019  double dq[AA_RESTRICT 4] );
1020 
1025 double aa_tf_qangle( const double q[AA_RESTRICT 4] );
1026 
1030 double aa_tf_qangle_rel( const double *q, const double *p );
1031 
1034 double aa_tf_quhypangle2
1035 ( const double q[AA_RESTRICT 4], const double p[AA_RESTRICT 4] );
1036 
1038 AA_API void aa_tf_qinv( const double q[AA_RESTRICT 4],
1039  double r[AA_RESTRICT 4] );
1040 
1042 AA_API void aa_tf_qadd( const double a[AA_RESTRICT 4],
1043  const double b[AA_RESTRICT 4],
1044  double c[AA_RESTRICT 4] );
1045 
1047 AA_API void aa_tf_qsub( const double a[AA_RESTRICT 4],
1048  const double b[AA_RESTRICT 4],
1049  double c[AA_RESTRICT 4] );
1050 
1052 AA_API void aa_tf_qmul( const double a[AA_RESTRICT 4],
1053  const double b[AA_RESTRICT 4],
1054  double c[AA_RESTRICT 4] );
1055 
1057 AA_API void aa_tf_qmulnorm( const double a[AA_RESTRICT 4],
1058  const double b[AA_RESTRICT 4],
1059  double c[AA_RESTRICT 4] );
1060 
1062 AA_API void aa_tf_qmul_qv( const double q[AA_RESTRICT 4],
1063  const double v[AA_RESTRICT 3],
1064  double c[AA_RESTRICT 4] );
1065 
1067 AA_API void aa_tf_qmul_vq( const double v[AA_RESTRICT 3],
1068  const double q[AA_RESTRICT 4],
1069  double c[AA_RESTRICT 4] );
1070 
1072 AA_API void aa_tf_qcmul( const double a[AA_RESTRICT 4],
1073  const double b[AA_RESTRICT 4],
1074  double c[AA_RESTRICT 4] );
1075 
1077 AA_API void aa_tf_qmulc( const double a[AA_RESTRICT 4],
1078  const double b[AA_RESTRICT 4],
1079  double c[AA_RESTRICT 4] );
1080 
1081 
1085 AA_API void
1086 aa_tf_qrot1( const double q[AA_RESTRICT 4], double v[AA_RESTRICT 3] );
1087 
1089 AA_API void aa_tf_qrot( const double q[AA_RESTRICT 4],
1090  const double v[AA_RESTRICT 3],
1091  double p[AA_RESTRICT 3] );
1092 
1096 AA_API void aa_tf_qrel(const double q1[AA_RESTRICT 4],
1097  const double q2[AA_RESTRICT 4],
1098  double q_rel[AA_RESTRICT 4]);
1099 
1101 AA_API void aa_tf_qslerp( double tau, const double a[AA_RESTRICT 4],
1102  const double b[AA_RESTRICT 4],
1103  double c[AA_RESTRICT 4] );
1104 
1105 
1107 AA_API void aa_tf_qslerpalg( double tau, const double a[AA_RESTRICT 4],
1108  const double b[AA_RESTRICT 4],
1109  double c[AA_RESTRICT 4] );
1110 
1116 AA_API void aa_tf_qslerpdiff( double tau, const double a[AA_RESTRICT 4],
1117  const double b[AA_RESTRICT 4],
1118  double c[AA_RESTRICT 4] );
1119 
1120 
1123 AA_API void aa_tf_qslerpdiffalg( double tau, 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_qslerpchaindiff( double u, double du,
1129  const double q1[AA_RESTRICT 4], const double dq1[AA_RESTRICT 4],
1130  const double q2[AA_RESTRICT 4], const double dq2[AA_RESTRICT 4],
1131  double q[AA_RESTRICT 4], double dq[AA_RESTRICT 4] );
1132 
1134 AA_API void aa_tf_qslerp3diff( double u12, double du12,
1135  const double q1[AA_RESTRICT 4], const double q2[AA_RESTRICT 4],
1136  double u34, double du34,
1137  const double q3[AA_RESTRICT 4], const double q4[AA_RESTRICT 4],
1138  double u, double du,
1139  double q[AA_RESTRICT 4], double dq[AA_RESTRICT 4] );
1140 
1142 AA_API void aa_tf_qdiff2vel( const double q[AA_RESTRICT 4],
1143  const double dq_dt[AA_RESTRICT 4],
1144  double v[AA_RESTRICT 3] );
1145 
1147 AA_API void aa_tf_qvel2diff( const double q[AA_RESTRICT 4],
1148  const double v[AA_RESTRICT 3],
1149  double dq_dt[AA_RESTRICT 4] );
1150 
1151 
1153 AA_API void aa_tf_rotvec_diff2vel( const double v[3], const double dv[3],
1154  double w[3] );
1155 
1163 AA_API void aa_tf_qrk1( const double q0[AA_RESTRICT 4],
1164  const double dq[AA_RESTRICT 4],
1165  double dt,
1166  double q1[AA_RESTRICT 4] );
1167 
1176 AA_API void aa_tf_qvelrk1( const double q0[AA_RESTRICT 4],
1177  const double v[AA_RESTRICT 3],
1178  double dt,
1179  double q1[AA_RESTRICT 4] );
1180 
1181 
1190 AA_API void aa_tf_qvelrk4( const double q0[AA_RESTRICT 4],
1191  const double v[AA_RESTRICT 3],
1192  double dt,
1193  double q1[AA_RESTRICT 4] );
1194 
1195 
1203 AA_API void aa_tf_qsvel( const double q0[AA_RESTRICT 4],
1204  const double v[AA_RESTRICT 3],
1205  double dt,
1206  double q1[AA_RESTRICT 4] );
1207 
1215 AA_API void aa_tf_qsdiff( const double q0[AA_RESTRICT 4],
1216  const double dq[AA_RESTRICT 4],
1217  double dt,
1218  double q1[AA_RESTRICT 4] );
1219 
1220 
1222 AA_API void aa_tf_xangle2quat( double theta_x, double q[AA_RESTRICT 4] );
1224 AA_API void aa_tf_yangle2quat( double theta_y, double q[AA_RESTRICT 4] );
1226 AA_API void aa_tf_zangle2quat( double theta_z, double q[AA_RESTRICT 4] );
1227 
1232 ( size_t n, const double *w, const double *q, size_t ldqq, double *M );
1233 
1244 ( size_t n, const double *w, const double *Q, size_t ldq, double *y );
1245 
1246 
1247 
1251 AA_API void aa_tf_qmatrix_l( const double *q, double *M, size_t ldm );
1252 
1256 AA_API void aa_tf_qmatrix_r( const double *q, double *M, size_t ldm );
1257 
1258 
1260 void aa_tf_qurand( double q[4] );
1261 
1262 /*********/
1263 /* Axang */
1264 /*********/
1265 
1267 AA_API void aa_tf_axang_make( double x, double y, double z, double theta,
1268  double axang[AA_RESTRICT 4] );
1269 
1272 AA_API void aa_tf_axang_permute( const double rv[AA_RESTRICT 4], int k,
1273  double rv_p[AA_RESTRICT 4] );
1274 
1276 AA_API void aa_tf_rotvec_permute( const double rv[AA_RESTRICT 3], int k,
1277  double rv_p[AA_RESTRICT 3] );
1278 
1281 AA_API void aa_tf_rotvec_near( const double rv[AA_RESTRICT 3],
1282  const double rv_near[AA_RESTRICT 3],
1283  double rv_p[AA_RESTRICT 3] );
1284 
1285 /**************/
1286 /* Quat-Trans */
1287 /**************/
1288 
1290 void aa_tf_qv_conj( const double q[4], const double v[3],
1291  double qc[4], double vc[3] );
1292 
1293 
1297 #define AA_TF_QUTR_Q 0
1298 
1303 #define AA_TF_QUTR_V 4
1304 
1306 void aa_tf_qutr2duqu( const double e[7], double s[8] );
1307 
1309 void aa_tf_qutr2tfmat( const double e[7], double T[12] );
1310 
1312 void aa_tf_tfmat2qutr( const double T[12], double e[7] );
1313 
1315 void aa_tf_duqu2qutr( const double s[8], double e[7] );
1316 
1318 void aa_tf_qutr_mul( const double a[7], const double b[7], double c[7] ) ;
1319 
1323 AA_API void
1324 aa_tf_qutr_tf( const double E[AA_RESTRICT 7], const double p0[AA_RESTRICT 3],
1325  double p1[AA_RESTRICT 3] );
1326 
1328 void aa_tf_qutr_mulnorm( const double a[7], const double b[7], double c[7] ) ;
1329 
1331 void aa_tf_qutr_conj( const double a[7], double c[7] ) ;
1332 
1334 void aa_tf_qutr_mulc( const double a[7], const double b[7], double c[7] ) ;
1335 
1337 void aa_tf_qutr_cmul( const double a[7], const double b[7], double c[7] ) ;
1338 
1341 ( const double e[7], const double de[7], double dx[6] );
1342 
1345 ( const double e[7], const double dx[6], double de[7] );
1346 
1348 void aa_tf_qutr_svel
1349 ( const double e0[7], const double dx[6], double dt, double e1[7] );
1350 
1352 void aa_tf_qutr_sdiff
1353 ( const double e0[7], const double de[7], double dt, double e1[7] );
1354 
1356 void aa_tf_qutr_wavg
1357 ( size_t n, const double *w, const double *EE, size_t ldee, double *a );
1358 
1360 void aa_tf_qutr_rand( double E[7] );
1361 
1362 /***************/
1363 /* Conversions */
1364 /***************/
1365 
1367 AA_API void aa_tf_quat2axang( const double q[AA_RESTRICT 4],
1368  double axang[AA_RESTRICT 4] );
1369 
1370 
1372 AA_API void aa_tf_axang2quat( const double axang[AA_RESTRICT 4],
1373  double q[AA_RESTRICT 4] );
1374 
1376 AA_API void aa_tf_axang2quat2( const double axis[AA_RESTRICT 3],
1377  double angle,
1378  double q[AA_RESTRICT 4] );
1379 
1380 
1384 AA_API void
1385 aa_tf_vecs2quat( const double u[AA_RESTRICT 3],
1386  const double v[AA_RESTRICT 3],
1387  double q[AA_RESTRICT 4] );
1388 
1389 
1391 AA_API void aa_tf_axang2rotvec( const double axang[AA_RESTRICT 4],
1392  double rotvec[AA_RESTRICT 3] );
1393 
1395 AA_API void aa_tf_rotvec2axang( const double rotvec[AA_RESTRICT 3],
1396  double axang[AA_RESTRICT 4] );
1397 
1399 AA_API void aa_tf_rotvec2quat( const double rotvec[AA_RESTRICT 3],
1400  double q[AA_RESTRICT 4] );
1402 AA_API void aa_tf_quat2rotvec( const double q[AA_RESTRICT 4],
1403  double rotvec[AA_RESTRICT 3] );
1404 
1405 
1407 AA_API void aa_tf_quat2rotvec_near( const double q[AA_RESTRICT 4],
1408  const double rv_near[AA_RESTRICT 3],
1409  double rotvec[AA_RESTRICT 3] );
1410 
1412 AA_API void aa_tf_quat2rotmat( const double quat[AA_RESTRICT 4],
1413  double rotmat[AA_RESTRICT 9] );
1415 AA_API void aa_tf_rotmat2quat( const double rotmat[AA_RESTRICT 9],
1416  double quat[AA_RESTRICT 4] );
1417 
1419 AA_API void aa_tf_rotmat2axang( const double R[AA_RESTRICT 9],
1420  double ra[AA_RESTRICT 4] );
1422 AA_API void aa_tf_rotmat2rotvec( const double R[AA_RESTRICT 9],
1423  double rv[AA_RESTRICT 3] );
1424 
1426 AA_API void aa_tf_axang2rotmat( const double ra[AA_RESTRICT 4],
1427  double R[AA_RESTRICT 9] );
1429 AA_API void aa_tf_rotvec2rotmat( const double rv[AA_RESTRICT 3],
1430  double R[AA_RESTRICT 9] );
1431 
1432 
1434 AA_API void aa_tf_qv2tfmat( const double q[AA_RESTRICT 4], const double v[AA_RESTRICT 3],
1435  double T[AA_RESTRICT 12] ) ;
1436 
1437 /* AA_API void aa_tf_tfv2tfq( const double vrv[AA_RESTRICT 6], */
1438 /* double x[AA_RESTRICT 3], double quat[AA_RESTRICT 4] ); */
1439 /* AA_API void aa_tf_tfq2tfv( const double x[AA_RESTRICT 3], const double quat[AA_RESTRICT 4], */
1440 /* double vrv[AA_RESTRICT 6] ); */
1441 
1443 AA_API void aa_tf_rotmat2eulerzyx( const double R[AA_RESTRICT 9],
1444  double e[AA_RESTRICT 3] );
1445 
1446 
1448 AA_API void aa_tf_quat2eulerzyx( const double q[AA_RESTRICT 4],
1449  double e[AA_RESTRICT 3] );
1450 
1454 #define AA_TF_DEF_EULER(letters) \
1455  AA_API void \
1456  aa_tf_euler ## letters ## 2rotmat( double e1, double e2, double e3, \
1457  double R[AA_RESTRICT 9] ); \
1458  AA_API void \
1459  aa_tf_euler ## letters ## 2quat( double e1, double e2, double e3, \
1460  double q[AA_RESTRICT 4] );
1461 
1462 AA_TF_DEF_EULER( xyz )
1463 AA_TF_DEF_EULER( xzy )
1464 
1465 AA_TF_DEF_EULER( yxz )
1466 AA_TF_DEF_EULER( yzx )
1467 
1468 AA_TF_DEF_EULER( zyx )
1469 AA_TF_DEF_EULER( zxy )
1470 
1471 AA_TF_DEF_EULER( xyx )
1472 AA_TF_DEF_EULER( xzx )
1473 
1474 AA_TF_DEF_EULER( yxy )
1475 AA_TF_DEF_EULER( yzy )
1476 
1477 AA_TF_DEF_EULER( zxz )
1478 AA_TF_DEF_EULER( zyz )
1479 
1481 AA_API void aa_tf_xangle2rotmat( double theta_x, double R[AA_RESTRICT 9] );
1483 AA_API void aa_tf_yangle2rotmat( double theta_y, double R[AA_RESTRICT 9] );
1485 AA_API void aa_tf_zangle2rotmat( double theta_z, double R[AA_RESTRICT 9] );
1486 
1492 AA_API void
1493 aa_tf_rotmat_mzlook( const double eye[AA_RESTRICT 3],
1494  const double target[AA_RESTRICT 3],
1495  const double up[AA_RESTRICT 3],
1496  double R[AA_RESTRICT 9] );
1497 
1498 
1502 AA_API void
1503 aa_tf_tfmat_mzlook( const double eye[AA_RESTRICT 3],
1504  const double target[AA_RESTRICT 3],
1505  const double up[AA_RESTRICT 3],
1506  double T[AA_RESTRICT 12] );
1507 
1511 AA_API void
1512 aa_tf_qmzlook( const double eye[AA_RESTRICT 3],
1513  const double target[AA_RESTRICT 3],
1514  const double up[AA_RESTRICT 3],
1515  double q[AA_RESTRICT 4] );
1516 
1520 AA_API void
1521 aa_tf_qv_mzlook( const double eye[AA_RESTRICT 3],
1522  const double target[AA_RESTRICT 3],
1523  const double up[AA_RESTRICT 3],
1524  double q[AA_RESTRICT 4],
1525  double v[AA_RESTRICT 3] );
1526 
1530 AA_API void
1531 aa_tf_qutr_mzlook( const double eye[AA_RESTRICT 3],
1532  const double target[AA_RESTRICT 3],
1533  const double up[AA_RESTRICT 3],
1534  double T[AA_RESTRICT 12] );
1535 
1536 
1537 /* Dual Quaternions */
1538 
1540 AA_API void aa_tf_duqu_add( const double d1[AA_RESTRICT 8], const double d2[AA_RESTRICT 8],
1541  double d3[AA_RESTRICT 8] );
1542 
1544 AA_API void aa_tf_duqu_sub( const double d1[AA_RESTRICT 8], const double d2[AA_RESTRICT 8],
1545  double d3[AA_RESTRICT 8] );
1546 
1548 AA_API void aa_tf_duqu_smul( const double d1[AA_RESTRICT 8], const double d2[AA_RESTRICT 8],
1549  double d3[AA_RESTRICT 8] );
1550 
1552 AA_API void aa_tf_duqu_mul( const double d1[AA_RESTRICT 8], const double d2[AA_RESTRICT 8],
1553  double d3[AA_RESTRICT 8] );
1554 
1558 AA_API void aa_tf_duqu_matrix_l( const double *q, double *M, size_t ldm );
1559 
1563 AA_API void aa_tf_duqu_matrix_r( const double *q, double *M, size_t ldm );
1564 
1566 AA_API void aa_tf_duqu_cmul( const double d1[AA_RESTRICT 8], const double d2[AA_RESTRICT 8],
1567  double d3[AA_RESTRICT 8] );
1568 
1570 AA_API void aa_tf_duqu_mulc( const double d1[AA_RESTRICT 8], const double d2[AA_RESTRICT 8],
1571  double d3[AA_RESTRICT 8] );
1572 
1574 AA_API void aa_tf_duqu_conj( const double d[AA_RESTRICT 8], double dconj[AA_RESTRICT 8] );
1575 
1577 AA_API void aa_tf_duqu_exp( const double d[AA_RESTRICT 8], double e[AA_RESTRICT 8] );
1578 
1580 AA_API void aa_tf_duqu_ln( const double d[AA_RESTRICT 8], double e[AA_RESTRICT 8] );
1581 
1583 AA_API void aa_tf_duqu_norm( const double d[AA_RESTRICT 8], double *nreal, double *ndual );
1584 
1586 AA_API void aa_tf_duqu_normalize( double d[AA_RESTRICT 8] );
1587 
1589 AA_API void aa_tf_duqu_minimize( double d[AA_RESTRICT 8] );
1590 
1592 AA_API void aa_tf_tf_duqu( const double d[AA_RESTRICT 8], const double p0[AA_RESTRICT 3],
1593  double p1[AA_RESTRICT 3] );
1594 
1596 AA_API void aa_tf_duqu_trans( const double d[AA_RESTRICT 8], double v[AA_RESTRICT 3] );
1597 
1599 AA_API void aa_tf_duqu2tfmat( const double d[AA_RESTRICT 8], double T[AA_RESTRICT 12] );
1600 
1602 AA_API void aa_tf_tfmat2duqu( const double T[AA_RESTRICT 12], double d[AA_RESTRICT 8] ) ;
1603 
1605 AA_API void aa_tf_qv2duqu( const double q[AA_RESTRICT 4], const double v[AA_RESTRICT 3],
1606  double d[AA_RESTRICT 8] ) ;
1607 
1609 AA_API void aa_tf_xyz2duqu( double x, double y, double z,
1610  double d[AA_RESTRICT 8] ) ;
1611 
1613 AA_API void aa_tf_xxyz2duqu( double theta, double x, double y, double z,
1614  double d[AA_RESTRICT 8] );
1615 
1617 AA_API void aa_tf_yxyz2duqu( double theta, double x, double y, double z,
1618  double d[AA_RESTRICT 8] );
1619 
1621 AA_API void aa_tf_zxyz2duqu( double theta, double x, double y, double z,
1622  double d[AA_RESTRICT 8] );
1623 
1625 AA_API void aa_tf_duqu2qv( const double d[AA_RESTRICT 8],
1626  double q[AA_RESTRICT 4], double v[AA_RESTRICT 3] ) ;
1627 
1629 void aa_tf_tfmat2av( const double T[12], double q[AA_RESTRICT 4], double v[AA_RESTRICT 3] ) ;
1630 
1632 AA_API void aa_tf_duqu_vel2twist( const double d[AA_RESTRICT 8], const double dx[AA_RESTRICT 6],
1633  double t[AA_RESTRICT 8] ) ;
1634 
1636 AA_API void aa_tf_duqu_twist2vel( const double d[AA_RESTRICT 8], const double t[AA_RESTRICT 8],
1637  double dx[AA_RESTRICT 6] ) ;
1638 
1640 AA_API void aa_tf_duqu_twist2diff( const double d[AA_RESTRICT 8], const double t[AA_RESTRICT 8],
1641  double dd[AA_RESTRICT 8] ) ;
1642 
1644 AA_API void aa_tf_duqu_vel2diff( const double d[AA_RESTRICT 8], const double dx[AA_RESTRICT 6],
1645  double dd[AA_RESTRICT 8] ) ;
1646 
1648 AA_API void aa_tf_duqu_diff2vel( const double d[AA_RESTRICT 8], const double dd[AA_RESTRICT 8],
1649  double dx[AA_RESTRICT 6] ) ;
1650 
1653 AA_API void aa_tf_duqu_diff2twist( const double d[AA_RESTRICT 8], const double dd[AA_RESTRICT 8],
1654  double twist[AA_RESTRICT 8] ) ;
1655 
1662 AA_API void aa_tf_duqu_stwist( const double d0[AA_RESTRICT 8], const double twist[AA_RESTRICT 8],
1663  double dt, double d1[AA_RESTRICT 6] ) ;
1664 
1671 AA_API void aa_tf_duqu_svel( const double d0[AA_RESTRICT 8], const double dx[AA_RESTRICT 6],
1672  double dt, double d1[AA_RESTRICT 6] ) ;
1673 
1680 AA_API void aa_tf_duqu_sdiff( const double d0[AA_RESTRICT 8], const double dd[AA_RESTRICT 8],
1681  double dt, double d1[AA_RESTRICT 6] ) ;
1682 
1683 
1684 
1685 
1686 /* Misc */
1687 
1688 void aa_tf_relx_mean( size_t n, const double *R,
1689  const double *X, size_t ldx,
1690  const double *Y, size_t ldy,
1691  double rel[3]);
1692 
1693 void aa_tf_relx_median( size_t n, const double *R,
1694  const double *X, size_t ldx,
1695  const double *Y, size_t ldy,
1696  double rel[3]);
1697 
1698 #ifdef __cplusplus
1699 }
1700 #endif
1701 
1702 #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:929
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
struct aa_tf_vec3 col0
column 0
Definition: tf.h:177
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
struct aa_tf_dx dx
velocity
Definition: tf.h:348
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
double data[8]
data array
Definition: tf.h:321
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
struct aa_tf_vec3 vec
vector part
Definition: tf.h:227
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.
void aa_tf_qutr_vel2diff(const double e[7], const double dx[6], double de[7])
Quaternion-translation spatial velocity to derivative.
#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
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
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_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.
double data[7]
data array
Definition: tf.h:306
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
double data[4]
data array
Definition: tf.h:232
void aa_tf_qurand(double q[4])
Generate random unit quaternion.
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.
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_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. ...
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_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:849
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:882
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'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.
double data[12]
data array
Definition: tf.h:268
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
double data[6]
data array
Definition: tf.h:334
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
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_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
double omega[3]
rotational velocity
Definition: tf.h:332
#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:833
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.
struct aa_tf_vec3 col2
column 2
Definition: tf.h:179
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.
struct aa_tf_qv tf
transform
Definition: tf.h:347
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:891
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
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_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:840
double R[9]
the rotation matrix part
Definition: tf.h:265
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'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.
double v[3]
vector part
Definition: tf.h:228
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'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 data[3]
data array
Definition: tf.h:246
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
struct aa_tf_vec3 col1
column 1
Definition: tf.h:178
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
double data[9]
data array
Definition: tf.h:181
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:1454
#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:872
void aa_tf_tfmat2av(const double T[12], double q[AA_RESTRICT 4], double v[AA_RESTRICT 3])
transformation matrix to quaternion-translation