amino  1.0-beta2
Lightweight Robot Utility Library
spatial.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  * Copyright (c) 2022, Colorado School of Mines
7  * All rights reserved.
8  *
9  * Author(s): Neil T. Dantam <ntd@gatech.edu>
10  * Georgia Tech Humanoid Robotics Lab
11  * Under Direction of Prof. Mike Stilman <mstilman@cc.gatech.edu>
12  *
13  *
14  * This file is provided under the following "BSD-style" License:
15  *
16  *
17  * Redistribution and use in source and binary forms, with or
18  * without modification, are permitted provided that the following
19  * conditions are met:
20  *
21  * * Redistributions of source code must retain the above copyright
22  * notice, this list of conditions and the following disclaimer.
23  *
24  * * Redistributions in binary form must reproduce the above
25  * copyright notice, this list of conditions and the following
26  * disclaimer in the documentation and/or other materials provided
27  * with the distribution.
28  *
29  * * Neither the name of copyright holder the names of its
30  * contributors may be used to endorse or promote products derived
31  * from this software without specific prior written permission.
32  *
33  * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND
34  * CONTRIBUTORS "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES,
35  * INCLUDING, BUT NOT LIMITED TO, THE IMPLIED WARRANTIES OF
36  * MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
37  * DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR
38  * CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL,
39  * SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT
40  * LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF
41  * USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
42  * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
43  * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
44  * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
45  * POSSIBILITY OF SUCH DAMAGE.
46  *
47  */
48 
49 #ifndef AMINO_TF_SPATIAL_H
50 #define AMINO_TF_SPATIAL_H
51 
58 #include <float.h>
59 
60 
61 
62 /**********/
63 /* Series */
64 /**********/
65 
74 #define AA_TF_DEF_SERIES(name, a0, a1, a2) \
75  static inline double \
76  aa_tf_ ## name ##_series2(double theta2) \
77  { \
78  return aa_horner3( theta2, a0, a1, a2 ); \
79  } \
80  static inline double \
81  aa_tf_ ## name ## _series(double theta) \
82  { \
83  return aa_tf_ ## name ## _series2(theta*theta); \
84  } \
85 
86 AA_TF_DEF_SERIES( sinc, 1., -1./6, 1./120 )
87 AA_TF_DEF_SERIES( cos, 1., -1./2, 1./24 )
88 AA_TF_DEF_SERIES( invsinc, 1., 1./6, 7./360 )
89 
97 static inline void
98 aa_tf_sinccos2( double theta2, double *sc, double *c )
99 {
100  if( theta2 < sqrt(DBL_EPSILON) ) {
101  *sc = aa_tf_sinc_series2(theta2);
102  *c = aa_tf_cos_series2(theta2);
103  } else {
104  double theta = sqrt(theta2);
105  double s = sin(theta);
106  *c = cos(theta);
107  *sc = s / theta;
108  }
109 }
110 
114 static inline void
115 aa_tf_sinccos( double theta, double *sc, double *c )
116 {
117  if( theta*theta < sqrt(DBL_EPSILON) ) {
118  *sc = aa_tf_sinc_series(theta);
119  *c = aa_tf_cos_series(theta);
120  } else {
121  double s = sin(theta);
122  *c = cos(theta);
123  *sc = s / theta;
124  }
125 }
126 
130 static inline double
131 aa_tf_sinc( double theta )
132 {
133  if( fabs(theta) < sqrt(sqrt(DBL_EPSILON)) ) {
134  return aa_tf_sinc_series(theta);
135  } else {
136  return sin(theta)/theta;
137  }
138 }
139 
140 /*********/
141 /* Types */
142 /*********/
143 
145 #define AA_TF_EPSILON .0001
146 
147 
148 
152 #define AA_TF_IDENT_INITIALIZER {1,0,0, 0,1,0, 0,0,1, 0,0,0}
153 
157 #define AA_TF_ROTMAT_IDENT_INITIALIZER {1,0,0, 0,1,0, 0,0,1}
158 
162 #define AA_TF_QUAT_IDENT_INITIALIZER {0,0,0,1}
163 
167 #define AA_TF_DUQU_IDENT_INITIALIZER {0,0,0,1, 0,0,0,0}
168 
172 #define AA_TF_QUTR_IDENT_INITIALIZER {0,0,0,1, 0,0,0}
173 
177 #define AA_TF_AXANG_IDENT_INITIALIZER {1,0,0,0}
178 
182 #define AA_TF_ROTVEC_IDENT_INITIALIZER {0,0,0}
183 
187 #define AA_TF_VEC_IDENT_INITIALIZER {0,0,0}
188 
190 #define AA_TF_IDENT ( (double[12]) AA_TF_IDENT_INITIALIZER )
192 #define AA_TF_ROTMAT_IDENT ( (double[9] AA_TF_ROTMAT_IDENT_INITIALIZER )
194 #define AA_TF_QUAT_IDENT ( (double[4]) AA_TF_QUAT_IDENT_INITIALIZER )
196 #define AA_TF_AXANG_IDENT ( (double[4]) AA_TF_AXANG_IDENT_INITIALIZER )
198 #define AA_TF_ROTVEC_IDENT ( (double[3]) AA_TF_ROTVEC_IDENT_INITIALIZER )
199 
203 static const double aa_tf_ident[12] = AA_TF_IDENT_INITIALIZER;
204 
208 static const double aa_tf_tfmat_ident[12] = AA_TF_IDENT_INITIALIZER;
209 
213 static const double aa_tf_rotmat_ident[9] = AA_TF_ROTMAT_IDENT_INITIALIZER;
214 
218 static const double aa_tf_quat_ident[4] = AA_TF_QUAT_IDENT_INITIALIZER;
219 
223 static const double aa_tf_duqu_ident[8] = AA_TF_DUQU_IDENT_INITIALIZER;
224 
228 static const double aa_tf_qutr_ident[7] = AA_TF_QUTR_IDENT_INITIALIZER;
229 
233 static const double aa_tf_axang_ident[4] = AA_TF_AXANG_IDENT_INITIALIZER;
234 
238 static const double aa_tf_rotvec_ident[3] = AA_TF_ROTVEC_IDENT_INITIALIZER;
239 
243 static const double aa_tf_vec_ident[3] = AA_TF_ROTVEC_IDENT_INITIALIZER;
244 
248 static const double aa_tf_vec_x[3] = {1, 0, 0};
249 
253 static const double aa_tf_vec_y[3] = {0, 1, 0};
254 
258 static const double aa_tf_vec_z[3] = {0, 0, 1};
259 
260 /**************/
261 /* Transforms */
262 /**************/
263 
265 AA_API void aa_tf_12( const double T[AA_RESTRICT 12],
266  const double p0[AA_RESTRICT 3],
267  double p1[AA_RESTRICT 3] );
269 AA_API void aa_tf_93( const double R[AA_RESTRICT 9],
270  const double v[AA_RESTRICT 3],
271  const double p0[AA_RESTRICT 3],
272  double p1[AA_RESTRICT 4] );
274 AA_API void aa_tf_tf_qv( const double quat[AA_RESTRICT 4],
275  const double v[AA_RESTRICT 3],
276  const double p0[AA_RESTRICT 3],
277  double p1[AA_RESTRICT 4] );
278 
280 AA_API void aa_tf_9( const double R[AA_RESTRICT 9],
281  const double p0[AA_RESTRICT 3],
282  double p1[AA_RESTRICT 4] );
283 
285 AA_API void aa_tf_12inv( const double T[AA_RESTRICT 12],
286  double Ti[AA_RESTRICT 12] );
288 AA_API void aa_tf_93inv( const double R[AA_RESTRICT 9],
289  const double v[AA_RESTRICT 3],
290  double Ri[AA_RESTRICT 9], double vi[AA_RESTRICT 3] );
296 AA_API void aa_tf_qv_conj( const double q[AA_RESTRICT 4], const double v[AA_RESTRICT 3],
297  double qc[AA_RESTRICT 4], double vc[AA_RESTRICT 3] );
298 
300 AA_API void aa_tf_12chain( const double T1[AA_RESTRICT 12],
301  const double T2[AA_RESTRICT 12],
302  double T[AA_RESTRICT 12] );
303 
305 AA_API void aa_tf_v12chain( double T[AA_RESTRICT 12] ,
306  const double T1[AA_RESTRICT 12],
307  const double T2[AA_RESTRICT 12],
308  ... );
309 
311 AA_API void aa_tf_93chain( const double R0[AA_RESTRICT 9],
312  const double v0[AA_RESTRICT 3],
313  const double R1[AA_RESTRICT 9],
314  const double v1[AA_RESTRICT 3],
315  double R[AA_RESTRICT 9], double v[AA_RESTRICT 3] );
317 AA_API void aa_tf_qv_chain( const double q0[AA_RESTRICT 4],
318  const double v0[AA_RESTRICT 3],
319  const double q1[AA_RESTRICT 4],
320  const double v1[AA_RESTRICT 3],
321  double q[AA_RESTRICT 4],
322  double v[AA_RESTRICT 3] );
323 
325 AA_API void aa_tf_93rel( const double R1[AA_RESTRICT 9],
326  const double v1[AA_RESTRICT 3],
327  const double R2[AA_RESTRICT 9],
328  const double v2[AA_RESTRICT 3],
329  double Rrel[AA_RESTRICT 9],
330  double vrel[AA_RESTRICT 3] );
331 
333 AA_API void aa_tf_12rel( const double T1[AA_RESTRICT 12],
334  const double T2[AA_RESTRICT 12],
335  double Trel[AA_RESTRICT 12] );
336 
337 /************/
338 /* Matrices */
339 /************/
340 
358 AA_API void aa_tf_skewsym_scal2( double a, double b, const double u[3], double R[9] );
359 
363 AA_API void
364 aa_tf_skewsym_scal_c( const double u[AA_RESTRICT 3],
365  const double a[AA_RESTRICT 3], const double b[AA_RESTRICT 3],
366  double R[9] );
367 
368 
369 /* Multiply */
370 
374 AA_API void aa_tf_rotmat_mul( const double R0[AA_RESTRICT 9],
375  const double R1[AA_RESTRICT 9],
376  double R[AA_RESTRICT 9] );
377 
381 AA_API void aa_tf_tfmat_mul( const double T0[AA_RESTRICT 12],
382  const double T1[AA_RESTRICT 12],
383  double T[AA_RESTRICT 12] );
384 
388 AA_API void aa_tf_tfmat2_mul( const double R0[AA_RESTRICT 9],
389  const double v0[AA_RESTRICT 3],
390  const double R1[AA_RESTRICT 9],
391  const double v1[AA_RESTRICT 3],
392  double R[AA_RESTRICT 9], double v[AA_RESTRICT 3] );
393 
394 
395 /* Inverting Multiply */
401 AA_API void aa_tf_rotmat_imul( const double R0[AA_RESTRICT 9],
402  const double R1[AA_RESTRICT 9],
403  double R[AA_RESTRICT 9] );
404 
410 AA_API void aa_tf_rotmat_muli( const double R0[AA_RESTRICT 9],
411  const double R1[AA_RESTRICT 9],
412  double R[AA_RESTRICT 9] );
413 
419 AA_API void aa_tf_tfmat_imul( const double T0[AA_RESTRICT 12],
420  const double T1[AA_RESTRICT 12],
421  double T[AA_RESTRICT 12] );
422 
428 AA_API void aa_tf_tfmat_muli( const double T0[AA_RESTRICT 12],
429  const double T1[AA_RESTRICT 12],
430  double T[AA_RESTRICT 12] );
431 
437 AA_API void aa_tf_tfmat2_imul( const double R0[AA_RESTRICT 9],
438  const double v0[AA_RESTRICT 3],
439  const double R1[AA_RESTRICT 9],
440  const double v1[AA_RESTRICT 3],
441  double R[AA_RESTRICT 9], double v[AA_RESTRICT 3] );
442 
448 AA_API void aa_tf_tfmat2_muli( const double R0[AA_RESTRICT 9],
449  const double v0[AA_RESTRICT 3],
450  const double R1[AA_RESTRICT 9],
451  const double v1[AA_RESTRICT 3],
452  double R[AA_RESTRICT 9], double v[AA_RESTRICT 3] );
453 
454 
458 AA_API void
459 aa_tf_rotmat_normalize( double R[AA_RESTRICT 9] );
460 
461 /* Transform */
462 
466 AA_API void aa_tf_rotmat_rot( const double R[AA_RESTRICT 9],
467  const double p0[AA_RESTRICT 3],
468  double p1[AA_RESTRICT 3] );
469 
473 AA_API void aa_tf_tfmat_tf( const double T[AA_RESTRICT 12],
474  const double p0[AA_RESTRICT 3],
475  double p1[AA_RESTRICT 3] );
476 
480 AA_API void aa_tf_tfmat2_tf( const double R[AA_RESTRICT 9],
481  const double v[AA_RESTRICT 3],
482  const double p0[AA_RESTRICT 3],
483  double p1[AA_RESTRICT 4] );
484 
488 AA_API void aa_tf_rotmat_inv1( double R[AA_RESTRICT 9] );
489 
493 AA_API void aa_tf_rotmat_inv2( const double R[AA_RESTRICT 9],
494  double Ri[AA_RESTRICT 9] );
495 
499 AA_API void aa_tf_tfmat_inv1( double T[AA_RESTRICT 12] );
500 
504 AA_API void aa_tf_tfmat_inv2( const double T[AA_RESTRICT 12],
505  double Ti[AA_RESTRICT 12] );
506 
507 
511 AA_API void
512 aa_tf_tfmat_normalize( double T[AA_RESTRICT 12] );
513 
515 AA_API int aa_tf_isrotmat( const double R[AA_RESTRICT 9] );
516 
517 
519 AA_API void aa_tf_9mul( const double R0[AA_RESTRICT 9],
520  const double R1[AA_RESTRICT 9],
521  double R[AA_RESTRICT 9] );
523 AA_API void aa_tf_9rot( const double R[AA_RESTRICT 9],
524  const double p0[AA_RESTRICT 3],
525  double p1[AA_RESTRICT 3] );
526 
528 AA_API void aa_tf_9rel( const double R1[AA_RESTRICT 9], const double R2[AA_RESTRICT 9],
529  double Ri[AA_RESTRICT 9] );
530 
532 AA_API void aa_tf_rotmat_exp_aa( const double aa[AA_RESTRICT 4], double R[AA_RESTRICT 9] );
533 
535 AA_API void aa_tf_rotmat_expv( const double rv[AA_RESTRICT 3], double R[AA_RESTRICT 9] );
536 
538 AA_API void aa_tf_rotmat_lnv( const double R[AA_RESTRICT 9], double v[AA_RESTRICT 3] );
539 
541 AA_API void aa_tf_tfmat_expv( const double v[AA_RESTRICT 6], double T[AA_RESTRICT 12] );
542 
544 AA_API void aa_tf_tfmat_lnv( const double T[AA_RESTRICT 12], double v[AA_RESTRICT 6] );
545 
547 AA_API void aa_tf_rotmat_vel2diff( const double R[AA_RESTRICT 9],
548  const double w[AA_RESTRICT 3], double dR[AA_RESTRICT 9] );
549 
551 AA_API void aa_tf_rotmat_diff2vel( const double R[AA_RESTRICT 9],
552  const double dR[AA_RESTRICT 9], double w[AA_RESTRICT 3] );
553 
555 AA_API void aa_tf_rotmat_svel( const double R0[AA_RESTRICT 9],
556  const double w[AA_RESTRICT 3], double dt,
557  double R1[AA_RESTRICT 9] );
558 
559 
560 
562 AA_API void aa_tf_tfmat_svel( const double T0[AA_RESTRICT 12],
563  const double w[AA_RESTRICT 3], double dt,
564  double T1[AA_RESTRICT 12] );
565 
567 AA_API void aa_tf_v9mul( double R[AA_RESTRICT 9],
568  const double R1[AA_RESTRICT 9], const double R2[AA_RESTRICT 9], ... );
569 
570 
574 AA_API void
575 aa_tf_rotmat_xy( const double x_axis[AA_RESTRICT 3],
576  const double y_axis[AA_RESTRICT 3],
577  double R[AA_RESTRICT 9] );
581 AA_API void
582 aa_tf_rotmat_yz( const double y_axis[AA_RESTRICT 3],
583  const double z_axis[AA_RESTRICT 3],
584  double R[AA_RESTRICT 9] );
585 
589 AA_API void
590 aa_tf_rotmat_zx( const double z_axis[AA_RESTRICT 3],
591  const double x_axis[AA_RESTRICT 3],
592  double R[AA_RESTRICT 9] );
593 
594 
595 /***********/
596 /* Vectors */
597 /***********/
598 
604 #define AA_TF_DOTX(a,b) \
605  ((a)[0]*(b)[0] + (a)[1]*(b)[1] + (a)[2]*(b)[2])
606 
610 static inline double
611 AA_TF_VDOT( const double a[AA_RESTRICT 3], const double b[AA_RESTRICT 3] )
612 {
613  return AA_TF_DOTX(a,b);
614 }
615 
619 static inline float
620 AA_TF_VDOTF( const float a[AA_RESTRICT 3], const float b[AA_RESTRICT 3] )
621 {
622  return AA_TF_DOTX(a,b);
623 }
624 
625 
629 AA_API double
630 aa_tf_vdot( const double a[AA_RESTRICT 3], const double b[AA_RESTRICT 3] );
631 
635 AA_API float
636 aa_tf_vdotf( const float a[AA_RESTRICT 3], const float b[AA_RESTRICT 3] );
637 
643 #define AA_TF_CROSSX( a, b, c ) \
644  (c)[0] = (a)[1]*(b)[2] - (a)[2]*(b)[1]; \
645  (c)[1] = (a)[2]*(b)[0] - (a)[0]*(b)[2]; \
646  (c)[2] = (a)[0]*(b)[1] - (a)[1]*(b)[0]; \
647 
648 
652 static inline void
653 AA_TF_CROSS( const double a[AA_RESTRICT 3], const double b[AA_RESTRICT 3], double c[AA_RESTRICT 3] )
654 {
655  AA_TF_CROSSX(a,b,c);
656 }
657 
661 static inline void
662 AA_TF_CROSSF( const float a[AA_RESTRICT 3], const float b[AA_RESTRICT 3], float c[AA_RESTRICT 3] )
663 {
664  AA_TF_CROSSX(a,b,c);
665 }
666 
670 AA_API void aa_tf_cross( const double a[AA_RESTRICT 3], const double b[AA_RESTRICT 3],
671  double c[AA_RESTRICT 3] ) ;
672 
676 AA_API void aa_tf_cross_a( const double a[AA_RESTRICT 3], const double b[AA_RESTRICT 3],
677  double c[AA_RESTRICT 3] ) ;
678 
683 AA_API void aa_tf_cross_mat_l( const double v[3], struct aa_dmat *M );
684 
689 AA_API void aa_tf_cross_mat_r( const double v[3], struct aa_dmat *M );
690 
691 
692 
696 AA_API void aa_tf_crossf( const float a[AA_RESTRICT 3], const float b[AA_RESTRICT 3],
697  float c[AA_RESTRICT 3] ) ;
698 
702 AA_API void aa_tf_vnormalize( double v[AA_RESTRICT 3] );
703 
704 
708 AA_API void aa_tf_vnormalizef( float v[AA_RESTRICT 3] );
709 
713 AA_API double
714 aa_tf_vssd( const double a[AA_RESTRICT 3], const double b[AA_RESTRICT 3] );
715 
719 AA_API double
720 aa_tf_vnorm( const double a[AA_RESTRICT 3] );
721 
722 /***************/
723 /* Quaternions */
724 /***************/
725 
729 static inline double
730 AA_TF_QDOT( const double a[AA_RESTRICT 4], const double b[AA_RESTRICT 4] )
731 {
732  return a[0]*b[0] + a[1]*b[1] + a[2]*b[2] + a[3]*b[3];
733 }
734 
738 AA_API double
739 aa_tf_qssd( const double a[AA_RESTRICT 4], const double b[AA_RESTRICT 4] );
740 
744 AA_API double
745 aa_tf_qdot( const double a[AA_RESTRICT 4], const double b[AA_RESTRICT 4] );
746 
750 AA_API void aa_tf_qnormalize( double q[AA_RESTRICT 4] );
751 
756 AA_API void
757 aa_tf_qscal( double q[AA_RESTRICT 4], double alpha );
758 
759 
761 AA_API double aa_tf_qnorm( const double q[AA_RESTRICT 4] );
762 
767 AA_API void aa_tf_qminimize( double q[AA_RESTRICT 4] );
768 
773 AA_API void aa_tf_qminimize2( const double q[AA_RESTRICT 4], double qmin[AA_RESTRICT 4] );
774 
777 AA_API void aa_tf_qnormalize2( const double q[AA_RESTRICT 4], double qnorm[AA_RESTRICT 4] );
778 
780 AA_API void aa_tf_qconj( const double q[AA_RESTRICT 4],
781  double r[AA_RESTRICT 4] );
782 
784 AA_API void aa_tf_qconj1( double q[AA_RESTRICT 4] );
785 
786 
788 AA_API void aa_tf_qexp( const double q[AA_RESTRICT 4],
789  double r[AA_RESTRICT 4] );
790 
791 
793 AA_API void aa_tf_qln( const double q[AA_RESTRICT 4],
794  double r[AA_RESTRICT 4] );
795 
802 AA_API void aa_tf_qduln( const double q[AA_RESTRICT 4],
803  const double dq[AA_RESTRICT 4],
804  double dln[AA_RESTRICT 3] );
805 
813 AA_API void aa_tf_qdulnj( const double q[AA_RESTRICT 4],
814  const double dq[AA_RESTRICT 4],
815  double dln[AA_RESTRICT 3] );
816 
823 AA_API void aa_tf_qdpexp( const double e[AA_RESTRICT 3],
824  const double de[AA_RESTRICT 3],
825  double dq[AA_RESTRICT 4] );
826 
834 AA_API void aa_tf_qdpexpj( const double e[AA_RESTRICT 3],
835  const double de[AA_RESTRICT 3],
836  double dq[AA_RESTRICT 4] );
837 
842 AA_API double
843 aa_tf_qangle(const double q[AA_RESTRICT 4]);
844 
848 AA_API double
849 aa_tf_qangle_rel(const double *q, const double *p);
850 
853 AA_API double
854 aa_tf_quhypangle2(const double q[AA_RESTRICT 4], const double p[AA_RESTRICT 4]);
855 
857 AA_API void aa_tf_qinv( const double q[AA_RESTRICT 4],
858  double r[AA_RESTRICT 4] );
859 
861 AA_API void aa_tf_qadd( const double a[AA_RESTRICT 4],
862  const double b[AA_RESTRICT 4],
863  double c[AA_RESTRICT 4] );
864 
866 AA_API void aa_tf_qsub( const double a[AA_RESTRICT 4],
867  const double b[AA_RESTRICT 4],
868  double c[AA_RESTRICT 4] );
869 
871 AA_API void aa_tf_qisub( double a[AA_RESTRICT 4],
872  const double b[AA_RESTRICT 4] );
873 
875 AA_API void aa_tf_qiadd( double a[AA_RESTRICT 4],
876  const double b[AA_RESTRICT 4] );
877 
879 AA_API void aa_tf_qmul( const double a[AA_RESTRICT 4],
880  const double b[AA_RESTRICT 4],
881  double c[AA_RESTRICT 4] );
882 
884 AA_API void aa_tf_qmulnorm( const double a[AA_RESTRICT 4],
885  const double b[AA_RESTRICT 4],
886  double c[AA_RESTRICT 4] );
887 
889 AA_API void aa_tf_qmul_qv( const double q[AA_RESTRICT 4],
890  const double v[AA_RESTRICT 3],
891  double c[AA_RESTRICT 4] );
892 
894 AA_API void aa_tf_qmul_vq( const double v[AA_RESTRICT 3],
895  const double q[AA_RESTRICT 4],
896  double c[AA_RESTRICT 4] );
897 
899 AA_API void aa_tf_qcmul( const double a[AA_RESTRICT 4],
900  const double b[AA_RESTRICT 4],
901  double c[AA_RESTRICT 4] );
902 
904 AA_API void aa_tf_qmulc( const double a[AA_RESTRICT 4],
905  const double b[AA_RESTRICT 4],
906  double c[AA_RESTRICT 4] );
907 
908 
912 AA_API void
913 aa_tf_qrot1( const double q[AA_RESTRICT 4], double v[AA_RESTRICT 3] );
914 
916 AA_API void aa_tf_qrot( const double q[AA_RESTRICT 4],
917  const double v[AA_RESTRICT 3],
918  double p[AA_RESTRICT 3] );
919 
923 AA_API void aa_tf_qrel(const double q1[AA_RESTRICT 4],
924  const double q2[AA_RESTRICT 4],
925  double q_rel[AA_RESTRICT 4]);
926 
928 AA_API void aa_tf_qslerp( double tau, const double a[AA_RESTRICT 4],
929  const double b[AA_RESTRICT 4],
930  double c[AA_RESTRICT 4] );
931 
932 
934 AA_API void aa_tf_qslerpalg( double tau, const double a[AA_RESTRICT 4],
935  const double b[AA_RESTRICT 4],
936  double c[AA_RESTRICT 4] );
937 
943 AA_API void aa_tf_qslerpdiff( double tau, const double a[AA_RESTRICT 4],
944  const double b[AA_RESTRICT 4],
945  double c[AA_RESTRICT 4] );
946 
947 
950 AA_API void aa_tf_qslerpdiffalg( double tau, const double a[AA_RESTRICT 4],
951  const double b[AA_RESTRICT 4],
952  double c[AA_RESTRICT 4] );
953 
955 AA_API void aa_tf_qslerpchaindiff( double u, double du,
956  const double q1[AA_RESTRICT 4], const double dq1[AA_RESTRICT 4],
957  const double q2[AA_RESTRICT 4], const double dq2[AA_RESTRICT 4],
958  double q[AA_RESTRICT 4], double dq[AA_RESTRICT 4] );
959 
961 AA_API void aa_tf_qslerp3diff( double u12, double du12,
962  const double q1[AA_RESTRICT 4], const double q2[AA_RESTRICT 4],
963  double u34, double du34,
964  const double q3[AA_RESTRICT 4], const double q4[AA_RESTRICT 4],
965  double u, double du,
966  double q[AA_RESTRICT 4], double dq[AA_RESTRICT 4] );
967 
969 AA_API void aa_tf_qdiff2vel( const double q[AA_RESTRICT 4],
970  const double dq_dt[AA_RESTRICT 4],
971  double v[AA_RESTRICT 3] );
972 
974 AA_API void aa_tf_qvel2diff( const double q[AA_RESTRICT 4],
975  const double v[AA_RESTRICT 3],
976  double dq_dt[AA_RESTRICT 4] );
977 
978 
980 AA_API void aa_tf_rotvec_diff2vel( const double v[3], const double dv[3],
981  double w[3] );
982 
990 AA_API void aa_tf_qrk1( const double q0[AA_RESTRICT 4],
991  const double dq[AA_RESTRICT 4],
992  double dt,
993  double q1[AA_RESTRICT 4] );
994 
1003 AA_API void aa_tf_qvelrk1( const double q0[AA_RESTRICT 4],
1004  const double v[AA_RESTRICT 3],
1005  double dt,
1006  double q1[AA_RESTRICT 4] );
1007 
1008 
1017 AA_API void aa_tf_qvelrk4( const double q0[AA_RESTRICT 4],
1018  const double v[AA_RESTRICT 3],
1019  double dt,
1020  double q1[AA_RESTRICT 4] );
1021 
1022 
1030 AA_API void aa_tf_qsvel( const double q0[AA_RESTRICT 4],
1031  const double v[AA_RESTRICT 3],
1032  double dt,
1033  double q1[AA_RESTRICT 4] );
1034 
1035 
1043 AA_API void aa_tf_qsdiff( const double q0[AA_RESTRICT 4],
1044  const double dq[AA_RESTRICT 4],
1045  double dt,
1046  double q1[AA_RESTRICT 4] );
1047 
1060 AA_API void aa_tf_qsacc_rk( const double q0[AA_RESTRICT 4],
1061  const double v[AA_RESTRICT 3],
1062  const double a[AA_RESTRICT 3],
1063  double dt,
1064  double q1[AA_RESTRICT 4] );
1065 
1074 AA_API void aa_tf_qsacc( const double q0[AA_RESTRICT 4],
1075  const double v[AA_RESTRICT 3],
1076  const double a[AA_RESTRICT 3],
1077  double dt,
1078  double q1[AA_RESTRICT 4] );
1079 
1080 
1082 AA_API void aa_tf_xangle2quat( double theta_x, double q[AA_RESTRICT 4] );
1084 AA_API void aa_tf_yangle2quat( double theta_y, double q[AA_RESTRICT 4] );
1086 AA_API void aa_tf_zangle2quat( double theta_z, double q[AA_RESTRICT 4] );
1087 
1091 AA_API void aa_tf_quat_davenport_matrix
1092 ( size_t n, const double *w, const double *q, size_t ldqq, double *M );
1093 
1103 AA_API void aa_tf_quat_davenport
1104 ( size_t n, const double *w, const double *Q, size_t ldq, double *y );
1105 
1106 
1107 
1111 AA_API void aa_tf_qmatrix_l( const double *q, double *M, size_t ldm );
1112 
1117 AA_API void aa_tf_qmatrix_r( const double *q, double *M, size_t ldm );
1118 
1119 
1123 AA_API void aa_tf_qmat_l( const double *q, struct aa_dmat *M );
1124 
1129 AA_API void aa_tf_qmat_r( const double *q, struct aa_dmat *M );
1130 
1131 
1133 AA_API void
1134 aa_tf_qurand( double q[4] );
1135 
1136 /*********/
1137 /* Axang */
1138 /*********/
1139 
1141 AA_API void aa_tf_axang_make( double x, double y, double z, double theta,
1142  double axang[AA_RESTRICT 4] );
1143 
1146 AA_API void aa_tf_axang_permute( const double rv[AA_RESTRICT 4], int k,
1147  double rv_p[AA_RESTRICT 4] );
1148 
1150 AA_API void aa_tf_rotvec_permute( const double rv[AA_RESTRICT 3], int k,
1151  double rv_p[AA_RESTRICT 3] );
1152 
1155 AA_API void aa_tf_rotvec_near( const double rv[AA_RESTRICT 3],
1156  const double rv_near[AA_RESTRICT 3],
1157  double rv_p[AA_RESTRICT 3] );
1161 AA_API void aa_tf_axang_rot( const double axang[AA_RESTRICT 4],
1162  const double p[AA_RESTRICT 3],
1163  double q[AA_RESTRICT 3] );
1164 
1165 /**************/
1166 /* Quat-Trans */
1167 /**************/
1168 
1170 AA_API void
1171 aa_tf_qv_conj(const double q[4], const double v[3],
1172  double qc[4], double vc[3]);
1173 
1174 
1178 #define AA_TF_QUTR_Q 0
1179 
1184 #define AA_TF_QUTR_V 4
1185 
1187 AA_API void
1188 aa_tf_qutr2duqu( const double e[7], double s[8] );
1189 
1191 AA_API void
1192 aa_tf_qutr2tfmat( const double e[7], double T[12] );
1193 
1195 AA_API void
1196 aa_tf_tfmat2qutr( const double T[12], double e[7] );
1197 
1199 AA_API void
1200 aa_tf_duqu2qutr( const double s[8], double e[7] );
1201 
1203 AA_API void
1204 aa_tf_qutr_mul( const double a[7], const double b[7], double c[7] ) ;
1205 
1209 AA_API void
1210 aa_tf_qutr_tf( const double E[AA_RESTRICT 7], const double p0[AA_RESTRICT 3],
1211  double p1[AA_RESTRICT 3] );
1212 
1214 AA_API void
1215 aa_tf_qutr_mulnorm( const double a[7], const double b[7], double c[7] ) ;
1216 
1218 AA_API void
1219 aa_tf_qutr_conj( const double a[7], double c[7] ) ;
1220 
1222 AA_API void
1223 aa_tf_qutr_mulc( const double a[7], const double b[7], double c[7] ) ;
1224 
1226 AA_API void
1227 aa_tf_qutr_cmul( const double a[7], const double b[7], double c[7] ) ;
1228 
1232 AA_API void aa_tf_qv_expv
1233 ( const double w[3], const double dv[3],
1234  double q[4], double v[3] );
1235 
1239 AA_API void aa_tf_qv_lnv
1240 ( const double q[4], const double v[3],
1241  double w[3], double dv[3] );
1242 
1246 AA_API void aa_tf_qutr_expv
1247 ( const double w[6], double e[7] );
1248 
1252 AA_API void aa_tf_qutr_lnv
1253 ( const double e[7], double w[6] );
1254 
1256 AA_API void aa_tf_qutr_twist2vel
1257 ( const double e[7], const double w[6], double dx[6] );
1258 
1260 AA_API void aa_tf_qutr_diff2vel
1261 ( const double e[7], const double de[7], double dx[6] );
1262 
1264 AA_API void aa_tf_qutr_vel2diff
1265 ( const double e[7], const double dx[6], double de[7] );
1266 
1267 
1268 
1270 AA_API void
1271 aa_tf_qv_vel2twist( const double q[AA_RESTRICT 4], const double v[AA_RESTRICT 3],
1272  const double w[AA_RESTRICT 3], const double dv[AA_RESTRICT 3],
1273  double tw[AA_RESTRICT 3], double tv[AA_RESTRICT 3] );
1274 
1276 AA_API void
1277 aa_tf_qv_twist2vel( const double q[AA_RESTRICT 4], const double v[AA_RESTRICT 3],
1278  const double tw[AA_RESTRICT 3], const double tv[AA_RESTRICT 3],
1279  double w[AA_RESTRICT 3], double dv[AA_RESTRICT 3] );
1280 
1282 AA_API void
1283 aa_tf_qutr_svel(const double e0[7], const double dx[6], double dt,
1284  double e1[7]);
1285 
1287 AA_API void aa_tf_qutr_sdiff
1288 ( const double e0[7], const double de[7], double dt, double e1[7] );
1289 
1291 AA_API void aa_tf_qutr_wavg
1292 ( size_t n, const double *w, const double *EE, size_t ldee, double *a );
1293 
1295 AA_API void aa_tf_qutr_rand( double E[7] );
1296 
1297 /***************/
1298 /* Conversions */
1299 /***************/
1300 
1302 AA_API void aa_tf_quat2axang( const double q[AA_RESTRICT 4],
1303  double axang[AA_RESTRICT 4] );
1304 
1305 
1307 AA_API void aa_tf_axang2quat( const double axang[AA_RESTRICT 4],
1308  double q[AA_RESTRICT 4] );
1309 
1311 AA_API void aa_tf_axang2quat2( const double axis[AA_RESTRICT 3],
1312  double angle,
1313  double q[AA_RESTRICT 4] );
1314 
1315 
1319 AA_API void
1320 aa_tf_vecs2quat( const double u[AA_RESTRICT 3],
1321  const double v[AA_RESTRICT 3],
1322  double q[AA_RESTRICT 4] );
1323 
1324 
1326 AA_API void aa_tf_axang2rotvec( const double axang[AA_RESTRICT 4],
1327  double rotvec[AA_RESTRICT 3] );
1328 
1330 AA_API void aa_tf_rotvec2axang( const double rotvec[AA_RESTRICT 3],
1331  double axang[AA_RESTRICT 4] );
1332 
1334 AA_API void aa_tf_rotvec2quat( const double rotvec[AA_RESTRICT 3],
1335  double q[AA_RESTRICT 4] );
1337 AA_API void aa_tf_quat2rotvec( const double q[AA_RESTRICT 4],
1338  double rotvec[AA_RESTRICT 3] );
1339 
1340 
1342 AA_API void aa_tf_quat2rotvec_near( const double q[AA_RESTRICT 4],
1343  const double rv_near[AA_RESTRICT 3],
1344  double rotvec[AA_RESTRICT 3] );
1345 
1347 AA_API void aa_tf_quat2rotmat( const double quat[AA_RESTRICT 4],
1348  double rotmat[AA_RESTRICT 9] );
1350 AA_API void aa_tf_rotmat2quat( const double rotmat[AA_RESTRICT 9],
1351  double quat[AA_RESTRICT 4] );
1352 
1354 AA_API void aa_tf_rotmat2axang( const double R[AA_RESTRICT 9],
1355  double ra[AA_RESTRICT 4] );
1357 AA_API void aa_tf_rotmat2rotvec( const double R[AA_RESTRICT 9],
1358  double rv[AA_RESTRICT 3] );
1359 
1361 AA_API void aa_tf_rotmat_sincos(const double R[AA_RESTRICT 9],
1362  double *s, double *c);
1363 
1365 AA_API void aa_tf_axang2rotmat( const double ra[AA_RESTRICT 4],
1366  double R[AA_RESTRICT 9] );
1367 
1369 AA_API void aa_tf_axang2rotmat2( const double axis[AA_RESTRICT 3],
1370  double angle,
1371  double R[AA_RESTRICT 9] );
1372 
1374 AA_API void aa_tf_rotvec2rotmat( const double rv[AA_RESTRICT 3],
1375  double R[AA_RESTRICT 9] );
1376 
1377 
1379 AA_API void aa_tf_qv2tfmat( const double q[AA_RESTRICT 4], const double v[AA_RESTRICT 3],
1380  double T[AA_RESTRICT 12] ) ;
1381 
1382 /* AA_API void aa_tf_tfv2tfq( const double vrv[AA_RESTRICT 6], */
1383 /* double x[AA_RESTRICT 3], double quat[AA_RESTRICT 4] ); */
1384 /* AA_API void aa_tf_tfq2tfv( const double x[AA_RESTRICT 3], const double quat[AA_RESTRICT 4], */
1385 /* double vrv[AA_RESTRICT 6] ); */
1386 
1388 AA_API void aa_tf_rotmat2eulerzyx( const double R[AA_RESTRICT 9],
1389  double e[AA_RESTRICT 3] );
1390 
1391 
1393 AA_API void aa_tf_quat2eulerzyx( const double q[AA_RESTRICT 4],
1394  double e[AA_RESTRICT 3] );
1395 
1399 #define AA_TF_DEF_EULER(letters) \
1400  AA_API void \
1401  aa_tf_euler ## letters ## 2rotmat( double e1, double e2, double e3, \
1402  double R[AA_RESTRICT 9] ); \
1403  AA_API void \
1404  aa_tf_euler ## letters ## 2quat( double e1, double e2, double e3, \
1405  double q[AA_RESTRICT 4] );
1406 
1407 AA_TF_DEF_EULER( xyz )
1408 AA_TF_DEF_EULER( xzy )
1409 
1410 AA_TF_DEF_EULER( yxz )
1411 AA_TF_DEF_EULER( yzx )
1412 
1413 AA_TF_DEF_EULER( zyx )
1414 AA_TF_DEF_EULER( zxy )
1415 
1416 AA_TF_DEF_EULER( xyx )
1417 AA_TF_DEF_EULER( xzx )
1418 
1419 AA_TF_DEF_EULER( yxy )
1420 AA_TF_DEF_EULER( yzy )
1421 
1422 AA_TF_DEF_EULER( zxz )
1423 AA_TF_DEF_EULER( zyz )
1424 
1425 
1426 AA_API void aa_tf_xangle2rotmat( double theta_x, double R[AA_RESTRICT 9] );
1428 AA_API void aa_tf_yangle2rotmat( double theta_y, double R[AA_RESTRICT 9] );
1430 AA_API void aa_tf_zangle2rotmat( double theta_z, double R[AA_RESTRICT 9] );
1431 
1433 AA_API void aa_tf_xangle2axang( double theta_x, double axang[AA_RESTRICT 4] );
1435 AA_API void aa_tf_yangle2axang( double theta_y, double axang[AA_RESTRICT 4] );
1437 AA_API void aa_tf_zangle2axang( double theta_z, double axang[AA_RESTRICT 4] );
1438 
1440 AA_API void aa_tf_axang_normalize( double axang[AA_RESTRICT 4] );
1441 
1447 AA_API void
1448 aa_tf_rotmat_mzlook( const double eye[AA_RESTRICT 3],
1449  const double target[AA_RESTRICT 3],
1450  const double up[AA_RESTRICT 3],
1451  double R[AA_RESTRICT 9] );
1452 
1453 
1457 AA_API void
1458 aa_tf_tfmat_mzlook( const double eye[AA_RESTRICT 3],
1459  const double target[AA_RESTRICT 3],
1460  const double up[AA_RESTRICT 3],
1461  double T[AA_RESTRICT 12] );
1462 
1466 AA_API void
1467 aa_tf_qmzlook( const double eye[AA_RESTRICT 3],
1468  const double target[AA_RESTRICT 3],
1469  const double up[AA_RESTRICT 3],
1470  double q[AA_RESTRICT 4] );
1471 
1475 AA_API void
1476 aa_tf_qv_mzlook( const double eye[AA_RESTRICT 3],
1477  const double target[AA_RESTRICT 3],
1478  const double up[AA_RESTRICT 3],
1479  double q[AA_RESTRICT 4],
1480  double v[AA_RESTRICT 3] );
1481 
1485 AA_API void
1486 aa_tf_qutr_mzlook( const double eye[AA_RESTRICT 3],
1487  const double target[AA_RESTRICT 3],
1488  const double up[AA_RESTRICT 3],
1489  double T[AA_RESTRICT 12] );
1490 
1491 
1492 /* Dual Quaternions */
1493 
1495 AA_API void aa_tf_duqu_add( const double d1[AA_RESTRICT 8], const double d2[AA_RESTRICT 8],
1496  double d3[AA_RESTRICT 8] );
1497 
1499 AA_API void aa_tf_duqu_sub( const double d1[AA_RESTRICT 8], const double d2[AA_RESTRICT 8],
1500  double d3[AA_RESTRICT 8] );
1501 
1503 AA_API void aa_tf_duqu_smul( double alpha, const double x[AA_RESTRICT 8],
1504  double y[AA_RESTRICT 8] );
1505 
1507 AA_API void aa_tf_duqu_mul( const double d1[AA_RESTRICT 8], const double d2[AA_RESTRICT 8],
1508  double d3[AA_RESTRICT 8] );
1509 
1513 AA_API void aa_tf_duqu_matrix_l( const double *q, double *M, size_t ldm );
1514 
1518 AA_API void aa_tf_duqu_matrix_r( const double *q, double *M, size_t ldm );
1519 
1523 AA_API void aa_tf_duqu_mat_l( const double *q, struct aa_dmat *M );
1524 
1528 AA_API void aa_tf_duqu_mat_r( const double *q, struct aa_dmat *M );
1529 
1531 AA_API void aa_tf_duqu_cmul( const double d1[AA_RESTRICT 8], const double d2[AA_RESTRICT 8],
1532  double d3[AA_RESTRICT 8] );
1533 
1535 AA_API void aa_tf_duqu_mulc( const double d1[AA_RESTRICT 8], const double d2[AA_RESTRICT 8],
1536  double d3[AA_RESTRICT 8] );
1537 
1539 AA_API void aa_tf_duqu_conj( const double d[AA_RESTRICT 8], double dconj[AA_RESTRICT 8] );
1540 
1542 AA_API void aa_tf_duqu_conj1( double s[AA_RESTRICT 8] );
1543 
1545 AA_API void aa_tf_duqu_exp( const double d[AA_RESTRICT 8], double e[AA_RESTRICT 8] );
1546 
1548 AA_API void aa_tf_duqu_ln( const double d[AA_RESTRICT 8], double e[AA_RESTRICT 8] );
1549 
1553 AA_API void aa_tf_duqu_lnv(const double S[AA_RESTRICT 8], double w[AA_RESTRICT 6]);
1554 
1556 AA_API void aa_tf_duqu_norm( const double d[AA_RESTRICT 8], double *nreal, double *ndual );
1557 
1559 AA_API void aa_tf_duqu_normalize( double d[AA_RESTRICT 8] );
1560 
1562 AA_API void aa_tf_duqu_minimize( double d[AA_RESTRICT 8] );
1563 
1565 AA_API void aa_tf_tf_duqu( const double d[AA_RESTRICT 8], const double p0[AA_RESTRICT 3],
1566  double p1[AA_RESTRICT 3] );
1567 
1569 AA_API void aa_tf_duqu_tf( const double d[AA_RESTRICT 8], const double p0[AA_RESTRICT 3],
1570  double p1[AA_RESTRICT 3] );
1571 
1573 AA_API void aa_tf_duqu_trans( const double d[AA_RESTRICT 8], double v[AA_RESTRICT 3] );
1574 
1576 AA_API void aa_tf_duqu2tfmat( const double d[AA_RESTRICT 8], double T[AA_RESTRICT 12] );
1577 
1579 AA_API void aa_tf_tfmat2duqu( const double T[AA_RESTRICT 12], double d[AA_RESTRICT 8] ) ;
1580 
1582 AA_API void aa_tf_qv2duqu( const double q[AA_RESTRICT 4], const double v[AA_RESTRICT 3],
1583  double d[AA_RESTRICT 8] ) ;
1584 
1586 AA_API void aa_tf_xyz2duqu( double x, double y, double z,
1587  double d[AA_RESTRICT 8] ) ;
1588 
1590 AA_API void aa_tf_xxyz2duqu( double theta, double x, double y, double z,
1591  double d[AA_RESTRICT 8] );
1592 
1594 AA_API void aa_tf_yxyz2duqu( double theta, double x, double y, double z,
1595  double d[AA_RESTRICT 8] );
1596 
1598 AA_API void aa_tf_zxyz2duqu( double theta, double x, double y, double z,
1599  double d[AA_RESTRICT 8] );
1600 
1602 AA_API void aa_tf_duqu2qv( const double d[AA_RESTRICT 8],
1603  double q[AA_RESTRICT 4], double v[AA_RESTRICT 3] ) ;
1604 
1606 AA_API void
1607 aa_tf_tfmat2av( const double T[12], double q[AA_RESTRICT 4], double v[AA_RESTRICT 3] ) ;
1608 
1610 AA_API void aa_tf_duqu_vel2twist( const double d[AA_RESTRICT 8], const double dx[AA_RESTRICT 6],
1611  double t[AA_RESTRICT 8] ) ;
1612 
1614 AA_API void aa_tf_duqu_twist2vel( const double d[AA_RESTRICT 8], const double t[AA_RESTRICT 8],
1615  double dx[AA_RESTRICT 6] ) ;
1616 
1618 AA_API void aa_tf_duqu_twist2diff( const double d[AA_RESTRICT 8], const double t[AA_RESTRICT 8],
1619  double dd[AA_RESTRICT 8] ) ;
1620 
1622 AA_API void aa_tf_duqu_vel2diff( const double d[AA_RESTRICT 8], const double dx[AA_RESTRICT 6],
1623  double dd[AA_RESTRICT 8] ) ;
1624 
1626 AA_API void aa_tf_duqu_diff2vel( const double d[AA_RESTRICT 8], const double dd[AA_RESTRICT 8],
1627  double dx[AA_RESTRICT 6] ) ;
1628 
1631 AA_API void aa_tf_duqu_diff2twist( const double d[AA_RESTRICT 8], const double dd[AA_RESTRICT 8],
1632  double twist[AA_RESTRICT 8] ) ;
1633 
1640 AA_API void aa_tf_duqu_stwist( const double d0[AA_RESTRICT 8], const double twist[AA_RESTRICT 8],
1641  double dt, double d1[AA_RESTRICT 6] ) ;
1642 
1649 AA_API void aa_tf_duqu_svel( const double d0[AA_RESTRICT 8], const double dx[AA_RESTRICT 6],
1650  double dt, double d1[AA_RESTRICT 6] ) ;
1651 
1658 AA_API void aa_tf_duqu_sdiff( const double d0[AA_RESTRICT 8], const double dd[AA_RESTRICT 8],
1659  double dt, double d1[AA_RESTRICT 6] ) ;
1660 
1661 
1683 AA_API void
1684 aa_tf_duqu_jac_vel2diff(const double S[8], const struct aa_dmat *Jvel,
1685  struct aa_dmat *Js );
1686 
1687 
1694 AA_API void
1695 aa_tf_qln_jac(const double q[4], struct aa_dmat *J );
1696 
1700 AA_API void
1701 aa_tf_duqu_conj_jac( struct aa_dmat *J );
1702 
1709 AA_API void
1710 aa_tf_duqu_ln_jac(const double S[8], struct aa_dmat *J );
1711 
1714 AA_API void
1715 aa_tf_duqu2pure( const double S[AA_RESTRICT 8],
1716  double v[AA_RESTRICT 6] );
1717 
1720 AA_API void
1721 aa_tf_pure2duqu( const double v[AA_RESTRICT 6],
1722  double S[AA_RESTRICT 8]);
1723 
1724 
1725 
1726 
1727 /* DH Parameter Conversions
1728  *
1729  * Conventions:
1730  * - Proximal: Craig and Lynch & Park
1731  * - Distal: Murray, Li, & Sastry
1732  */
1733 
1735 AA_API void
1736 aa_tf_dhprox2tfmat( double alpha, double a, double d, double phi,
1737  double T[AA_RESTRICT 12]);
1739 AA_API void
1740 aa_tf_dhprox2duqu( double alpha, double a, double d, double phi,
1741  double S[AA_RESTRICT 8]);
1743 AA_API void
1744 aa_tf_dhprox2qutr( double alpha, double a, double d, double phi,
1745  double E[AA_RESTRICT 7]);
1747 AA_API void
1748 aa_tf_dhprox2qv( double alpha, double a, double d, double phi,
1749  double q[AA_RESTRICT 4], double v[3]);
1750 
1752 AA_API void
1753 aa_tf_dhdist2tfmat( double alpha, double a, double d, double phi,
1754  double T[AA_RESTRICT 12]);
1756 AA_API void
1757 aa_tf_dhdist2duqu( double alpha, double a, double d, double phi,
1758  double S[AA_RESTRICT 8]);
1760 AA_API void
1761 aa_tf_dhdist2qutr( double alpha, double a, double d, double phi,
1762  double E[AA_RESTRICT 7]);
1764 AA_API void
1765 aa_tf_dhdist2qv( double alpha, double a, double d, double phi,
1766  double q[AA_RESTRICT 4], double v[3]);
1767 
1768 /* Misc */
1769 
1770 AA_API void
1771 aa_tf_relx_mean( size_t n, const double *R,
1772  const double *X, size_t ldx,
1773  const double *Y, size_t ldy,
1774  double rel[3]);
1775 
1776 AA_API void
1777 aa_tf_relx_median( size_t n, const double *R,
1778  const double *X, size_t ldx,
1779  const double *Y, size_t ldy,
1780  double rel[3]);
1781 
1791 AA_API void
1792 aa_tf_proj(const double a[AA_RESTRICT 3],
1793  const double b[AA_RESTRICT 3], double c[AA_RESTRICT 3]);
1794 
1803 AA_API void
1804 aa_tf_proj_orth(const double a[AA_RESTRICT 3],
1805  const double b[AA_RESTRICT 3],
1806  double c[AA_RESTRICT 3]);
1807 
1808 #endif
AA_API
#define AA_API
calling and name mangling convention for functions
Definition: amino.h:95
AA_RESTRICT
#define AA_RESTRICT
Defined restrict keyword based on language flavor.
Definition: amino.h:99
aa_tf_qrel
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_tf_duqu_conj
AA_API void aa_tf_duqu_conj(const double d[AA_RESTRICT 8], double dconj[AA_RESTRICT 8])
Dual quaternion conjugate.
aa_tf_tfmat_inv1
AA_API void aa_tf_tfmat_inv1(double T[AA_RESTRICT 12])
Invert a transform in place.
aa_tf_duqu_exp
AA_API void aa_tf_duqu_exp(const double d[AA_RESTRICT 8], double e[AA_RESTRICT 8])
Dual quaternion exponential.
aa_tf_qutr_wavg
AA_API void aa_tf_qutr_wavg(size_t n, const double *w, const double *EE, size_t ldee, double *a)
Weighted average transform.
aa_tf_qutr2tfmat
AA_API void aa_tf_qutr2tfmat(const double e[7], double T[12])
quaternion-translation to transformation matrix
aa_tf_qv_mzlook
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.
AA_TF_DUQU_IDENT_INITIALIZER
#define AA_TF_DUQU_IDENT_INITIALIZER
Static initializer for an identity dual quaternion.
Definition: spatial.h:167
aa_tf_qcmul
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_tf_dhprox2qv
AA_API void aa_tf_dhprox2qv(double alpha, double a, double d, double phi, double q[AA_RESTRICT 4], double v[3])
Convert proximal DH parameters to quaternion-translation.
aa_tf_rotmat_inv1
AA_API void aa_tf_rotmat_inv1(double R[AA_RESTRICT 9])
Invert a rotation in place.
aa_tf_dhprox2duqu
AA_API void aa_tf_dhprox2duqu(double alpha, double a, double d, double phi, double S[AA_RESTRICT 8])
Convert proximal DH parameters to dual quaternion.
aa_tf_qmulnorm
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_tf_ident
static const double aa_tf_ident[12]
Identity transformation matrix array.
Definition: spatial.h:203
aa_tf_qmatrix_r
AA_API void aa_tf_qmatrix_r(const double *q, double *M, size_t ldm)
Construct matrix for right quaternion multiply.
aa_tf_qdiff2vel
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_tf_qutr_rand
AA_API void aa_tf_qutr_rand(double E[7])
Generate random transform.
aa_tf_tf_qv
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_tf_v9mul
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_tf_axang_ident
static const double aa_tf_axang_ident[4]
Identity axis-angle array.
Definition: spatial.h:233
aa_tf_tfmat_normalize
AA_API void aa_tf_tfmat_normalize(double T[AA_RESTRICT 12])
Orthonormalize the transformation matrix.
AA_TF_CROSS
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: spatial.h:653
aa_tf_qrot1
AA_API void aa_tf_qrot1(const double q[AA_RESTRICT 4], double v[AA_RESTRICT 3])
Quaternion point rotation, in place.
aa_tf_duqu_norm
AA_API void aa_tf_duqu_norm(const double d[AA_RESTRICT 8], double *nreal, double *ndual)
Dual quaternion norm.
aa_tf_duqu_diff2vel
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_tf_quat2axang
AA_API void aa_tf_quat2axang(const double q[AA_RESTRICT 4], double axang[AA_RESTRICT 4])
Quaternion to axis-angle.
aa_tf_qutr_vel2diff
AA_API void aa_tf_qutr_vel2diff(const double e[7], const double dx[6], double de[7])
Quaternion-translation spatial velocity to derivative.
AA_TF_AXANG_IDENT_INITIALIZER
#define AA_TF_AXANG_IDENT_INITIALIZER
Static initializer for an identity axis-angle.
Definition: spatial.h:177
aa_tf_duqu_ln_jac
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_tf_duqu_jac_vel2diff
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_tf_skewsym_scal2
AA_API void aa_tf_skewsym_scal2(double a, double b, const double u[3], double R[9])
Construct a skew-symmetric matrix.
aa_tf_zangle2axang
AA_API void aa_tf_zangle2axang(double theta_z, double axang[AA_RESTRICT 4])
Angle about z axis.
aa_tf_rotmat2eulerzyx
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_tf_rotmat_inv2
AA_API void aa_tf_rotmat_inv2(const double R[AA_RESTRICT 9], double Ri[AA_RESTRICT 9])
Invert a rotation.
aa_tf_qsub
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_tf_duqu_ident
static const double aa_tf_duqu_ident[8]
Identity dual quaternion array.
Definition: spatial.h:223
aa_tf_xangle2rotmat
AA_API void aa_tf_xangle2rotmat(double theta_x, double R[AA_RESTRICT 9])
Angle about x axis.
aa_tf_93
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_tf_axang2quat2
AA_API void aa_tf_axang2quat2(const double axis[AA_RESTRICT 3], double angle, double q[AA_RESTRICT 4])
axis-angle to quaternion.
aa_tf_9
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_tf_rotmat_muli
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.
aa_tf_proj
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_tf_qslerpdiffalg
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_tf_qv_vel2twist
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_TF_QUTR_IDENT_INITIALIZER
#define AA_TF_QUTR_IDENT_INITIALIZER
Static initializer for an identity quaternion-translation.
Definition: spatial.h:172
AA_TF_VDOT
static double AA_TF_VDOT(const double a[AA_RESTRICT 3], const double b[AA_RESTRICT 3])
Inlined vector dot product.
Definition: spatial.h:611
aa_tf_qslerp3diff
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_TF_DEF_SERIES
#define AA_TF_DEF_SERIES(name, a0, a1, a2)
Define an inline function for a series using Horner's rule.
Definition: spatial.h:74
aa_tf_axang_permute
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_tf_qmul_vq
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_tf_qexp
AA_API void aa_tf_qexp(const double q[AA_RESTRICT 4], double r[AA_RESTRICT 4])
Quaternion exponential.
aa_tf_qadd
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_TF_CROSSF
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: spatial.h:662
aa_tf_axang_rot
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_tf_qnorm
AA_API double aa_tf_qnorm(const double q[AA_RESTRICT 4])
Return norm of the quaternion.
aa_tf_rotmat_rot
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.
aa_tf_rotmat_exp_aa
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.
aa_tf_duqu_matrix_r
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.
aa_tf_cross_mat_l
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_tf_duqu_add
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.
aa_tf_quat2rotmat
AA_API void aa_tf_quat2rotmat(const double quat[AA_RESTRICT 4], double rotmat[AA_RESTRICT 9])
convert quaternion to rotation matrix
aa_tf_dhdist2tfmat
AA_API void aa_tf_dhdist2tfmat(double alpha, double a, double d, double phi, double T[AA_RESTRICT 12])
Convert distal DH parameters to transformation matrix.
aa_tf_rotmat_diff2vel
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_tf_duqu_tf
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_tf_vnormalize
AA_API void aa_tf_vnormalize(double v[AA_RESTRICT 3])
Normalize Vector.
aa_tf_duqu_minimize
AA_API void aa_tf_duqu_minimize(double d[AA_RESTRICT 8])
Dual quaternion angle minimization.
aa_tf_qnormalize2
AA_API void aa_tf_qnormalize2(const double q[AA_RESTRICT 4], double qnorm[AA_RESTRICT 4])
Normailize quaternion.
aa_tf_qutr_mzlook
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_tf_qurand
AA_API void aa_tf_qurand(double q[4])
Generate random unit quaternion.
aa_tf_qdpexp
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.
aa_tf_duqu2pure
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.
aa_tf_qutr_lnv
AA_API void aa_tf_qutr_lnv(const double e[7], double w[6])
Quaternion-vector logarithm.
aa_tf_qinv
AA_API void aa_tf_qinv(const double q[AA_RESTRICT 4], double r[AA_RESTRICT 4])
Quaternion inverse.
AA_TF_ROTVEC_IDENT_INITIALIZER
#define AA_TF_ROTVEC_IDENT_INITIALIZER
Static initializer for an identity rotation-vector.
Definition: spatial.h:182
aa_tf_zxyz2duqu
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_tf_qduln
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.
aa_tf_qrot
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_tf_vecs2quat
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_tf_duqu2qv
AA_API void aa_tf_duqu2qv(const double d[AA_RESTRICT 8], double q[AA_RESTRICT 4], double v[AA_RESTRICT 3])
Convert dual quaternion to orientation unit quaternion and translation vector.
aa_tf_yangle2rotmat
AA_API void aa_tf_yangle2rotmat(double theta_y, double R[AA_RESTRICT 9])
Angle about y axis.
aa_tf_dhprox2qutr
AA_API void aa_tf_dhprox2qutr(double alpha, double a, double d, double phi, double E[AA_RESTRICT 7])
Convert proximal DH parameters to quaternion-translation.
AA_TF_DOTX
#define AA_TF_DOTX(a, b)
Macro to compute a dot product of length 3 vectors.
Definition: spatial.h:604
aa_tf_qv_lnv
AA_API void aa_tf_qv_lnv(const double q[4], const double v[3], double w[3], double dv[3])
Quaternion-vector logarithm.
aa_tf_qconj
AA_API void aa_tf_qconj(const double q[AA_RESTRICT 4], double r[AA_RESTRICT 4])
Quaternion conjugate.
aa_tf_qsvel
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_tf_qconj1
AA_API void aa_tf_qconj1(double q[AA_RESTRICT 4])
Quaternion conjugate, in-place.
aa_tf_qutr_ident
static const double aa_tf_qutr_ident[7]
Identity quaternion-translation array.
Definition: spatial.h:228
aa_tf_duqu_trans
AA_API void aa_tf_duqu_trans(const double d[AA_RESTRICT 8], double v[AA_RESTRICT 3])
Extract dual quaternion translation vector.
aa_tf_rotmat_ident
static const double aa_tf_rotmat_ident[9]
Identity rotation matrix array.
Definition: spatial.h:213
AA_TF_IDENT_INITIALIZER
#define AA_TF_IDENT_INITIALIZER
Static initializer for an identity transformation matrix.
Definition: spatial.h:152
aa_tf_qutr_twist2vel
AA_API void aa_tf_qutr_twist2vel(const double e[7], const double w[6], double dx[6])
Quaternion-translation twist to velocity.
aa_tf_v12chain
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.
aa_tf_qdulnj
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_tf_qutr_tf
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,.
aa_tf_qsacc
AA_API void aa_tf_qsacc(const double q0[AA_RESTRICT 4], const double v[AA_RESTRICT 3], const double a[AA_RESTRICT 3], double dt, double q1[AA_RESTRICT 4])
Integrate unit quaternion from velocity and acceleration.
aa_tf_yangle2axang
AA_API void aa_tf_yangle2axang(double theta_y, double axang[AA_RESTRICT 4])
Angle about y axis.
aa_tf_duqu2tfmat
AA_API void aa_tf_duqu2tfmat(const double d[AA_RESTRICT 8], double T[AA_RESTRICT 12])
Convert dual quaternion to transformation matrix.
aa_tf_cross_mat_r
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_tf_qmzlook
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_tf_qscal
AA_API void aa_tf_qscal(double q[AA_RESTRICT 4], double alpha)
Scale a quaternion.
aa_tf_quat2rotvec_near
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_tf_qmul_qv
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_tf_qutr_diff2vel
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_tf_rotmat_vel2diff
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_tf_qsdiff
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_tf_quhypangle2
AA_API 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_tf_vec_z
static const double aa_tf_vec_z[3]
A Z axis.
Definition: spatial.h:258
aa_tf_qutr_conj
AA_API void aa_tf_qutr_conj(const double a[7], double c[7])
quaternion-translation conjugate
aa_tf_dhdist2qv
AA_API void aa_tf_dhdist2qv(double alpha, double a, double d, double phi, double q[AA_RESTRICT 4], double v[3])
Convert distal DH parameters to quaternion-translation.
aa_tf_12
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_tf_rotmat_sincos
AA_API void aa_tf_rotmat_sincos(const double R[AA_RESTRICT 9], double *s, double *c)
extract sin and cosine of a rotation matrix
aa_tf_qslerp
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_tf_qutr_mulnorm
AA_API void aa_tf_qutr_mulnorm(const double a[7], const double b[7], double c[7])
quaternion-translation multiply and normalize
aa_tf_cross_a
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_tf_crossf
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_tf_rotmat_normalize
AA_API void aa_tf_rotmat_normalize(double R[AA_RESTRICT 9])
Orthonormalize the rotation matrix.
aa_tf_axang2rotmat2
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_tf_vssd
AA_API double aa_tf_vssd(const double a[AA_RESTRICT 3], const double b[AA_RESTRICT 3])
Sum-square-differences of two vectors.
aa_tf_duqu_twist2diff
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_tf_qutr2duqu
AA_API void aa_tf_qutr2duqu(const double e[7], double s[8])
quaternion-translation to dual quaternion
aa_tf_qvelrk4
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.
aa_tf_duqu_mat_l
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.
aa_tf_sinc
static double aa_tf_sinc(double theta)
Compute sinc(theta)
Definition: spatial.h:131
aa_tf_duqu_mat_r
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.
aa_tf_qv_chain
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_tf_qutr_cmul
AA_API void aa_tf_qutr_cmul(const double a[7], const double b[7], double c[7])
quaternion-translation conjugate multiply
aa_tf_qminimize2
AA_API void aa_tf_qminimize2(const double q[AA_RESTRICT 4], double qmin[AA_RESTRICT 4])
Minimize angle represented by the quaternion.
aa_tf_tfmat_mzlook
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_tf_duqu_normalize
AA_API void aa_tf_duqu_normalize(double d[AA_RESTRICT 8])
Dual quaternion normalization.
aa_tf_qv_expv
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_tf_vdot
AA_API double aa_tf_vdot(const double a[AA_RESTRICT 3], const double b[AA_RESTRICT 3])
Vector dot product.
aa_tf_vdotf
AA_API float aa_tf_vdotf(const float a[AA_RESTRICT 3], const float b[AA_RESTRICT 3])
Vector dot product.
aa_tf_qutr_expv
AA_API void aa_tf_qutr_expv(const double w[6], double e[7])
Quaternion-vector exponential.
aa_tf_quat2eulerzyx
AA_API void aa_tf_quat2eulerzyx(const double q[AA_RESTRICT 4], double e[AA_RESTRICT 3])
Convert quaternion to ZYX Euler Angles.
aa_tf_quat_davenport
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_tf_rotmat_mul
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_tf_qiadd
AA_API void aa_tf_qiadd(double a[AA_RESTRICT 4], const double b[AA_RESTRICT 4])
Quaternion addition, in-place.
aa_tf_isrotmat
AA_API int aa_tf_isrotmat(const double R[AA_RESTRICT 9])
tests if R is a rotation matrix
aa_tf_vec_x
static const double aa_tf_vec_x[3]
An X axis.
Definition: spatial.h:248
aa_tf_qvel2diff
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_tf_tfmat_inv2
AA_API void aa_tf_tfmat_inv2(const double T[AA_RESTRICT 12], double Ti[AA_RESTRICT 12])
Invert a transform.
aa_tf_rotmat_mzlook
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_tf_93chain
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
aa_tf_rotmat_zx
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_tf_duqu_lnv
AA_API void aa_tf_duqu_lnv(const double S[AA_RESTRICT 8], double w[AA_RESTRICT 6])
Unit dual quaternion natural logarithm.
aa_tf_duqu_ln
AA_API void aa_tf_duqu_ln(const double d[AA_RESTRICT 8], double e[AA_RESTRICT 8])
Dual quaternion natural logarithm.
aa_tf_duqu_conj_jac
AA_API void aa_tf_duqu_conj_jac(struct aa_dmat *J)
Fill J with Jacobian of dual quaternion conjugate.
aa_tf_qmul
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.
aa_tf_duqu2qutr
AA_API void aa_tf_duqu2qutr(const double s[8], double e[7])
dual quaternion to quaternion-translation
aa_tf_rotvec_permute
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_tf_vec_ident
static const double aa_tf_vec_ident[3]
Identity vector-3 array.
Definition: spatial.h:243
aa_tf_qv_twist2vel
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_tf_rotmat2axang
AA_API void aa_tf_rotmat2axang(const double R[AA_RESTRICT 9], double ra[AA_RESTRICT 4])
convert rotation matrix to axis angle
AA_TF_VDOTF
static float AA_TF_VDOTF(const float a[AA_RESTRICT 3], const float b[AA_RESTRICT 3])
Inlined vector dot product.
Definition: spatial.h:620
aa_tf_duqu_smul
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_tf_rotvec_ident
static const double aa_tf_rotvec_ident[3]
Identity rotation vector array.
Definition: spatial.h:238
aa_tf_qutr_svel
AA_API void aa_tf_qutr_svel(const double e0[7], const double dx[6], double dt, double e1[7])
Integrate a quaternion-translation.
aa_tf_axang2quat
AA_API void aa_tf_axang2quat(const double axang[AA_RESTRICT 4], double q[AA_RESTRICT 4])
axis-angle to quaternion.
aa_tf_tfmat2_muli
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_tf_qslerpdiff
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_tf_qnormalize
AA_API void aa_tf_qnormalize(double q[AA_RESTRICT 4])
Normalize Quaternion.
aa_tf_rotmat_expv
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_tf_tf_duqu
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_tf_rotvec_diff2vel
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.
aa_tf_qv2duqu
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_tf_duqu_vel2twist
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_tf_qln
AA_API void aa_tf_qln(const double q[AA_RESTRICT 4], double r[AA_RESTRICT 4])
Quaternion natural log.
aa_tf_duqu_mul
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_tf_pure2duqu
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.
aa_tf_qmatrix_l
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_tf_tfmat2av
AA_API void aa_tf_tfmat2av(const double T[12], double q[AA_RESTRICT 4], double v[AA_RESTRICT 3])
transformation matrix to quaternion-translation
aa_tf_vnormalizef
AA_API void aa_tf_vnormalizef(float v[AA_RESTRICT 3])
Normalize Vector.
aa_tf_duqu_diff2twist
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_tf_duqu_vel2diff
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_tf_quat_davenport_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.
aa_tf_qminimize
AA_API void aa_tf_qminimize(double q[AA_RESTRICT 4])
Minimize angle represented by the quaternion.
aa_tf_duqu_cmul
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.
aa_tf_zangle2rotmat
AA_API void aa_tf_zangle2rotmat(double theta_z, double R[AA_RESTRICT 9])
Angle about z axis.
aa_tf_duqu_sdiff
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_tf_proj_orth
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.
aa_tf_xangle2quat
AA_API void aa_tf_xangle2quat(double theta_x, double q[AA_RESTRICT 4])
Unit quaternion for angle about x axis.
aa_tf_quat_ident
static const double aa_tf_quat_ident[4]
Identity quaternion array.
Definition: spatial.h:218
aa_tf_rotmat_imul
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_tf_tfmat2_mul
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_tf_9mul
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_tf_sinccos
static void aa_tf_sinccos(double theta, double *sc, double *c)
Compute sinc(theta), cos(theta)
Definition: spatial.h:115
aa_tf_rotmat2rotvec
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_tf_12chain
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
aa_tf_9rot
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
aa_tf_qmat_r
AA_API void aa_tf_qmat_r(const double *q, struct aa_dmat *M)
Construct matrix for right quaternion multiply.
aa_tf_qangle
AA_API double aa_tf_qangle(const double q[AA_RESTRICT 4])
Return the angle of the quaternion.
aa_tf_12inv
AA_API void aa_tf_12inv(const double T[AA_RESTRICT 12], double Ti[AA_RESTRICT 12])
invert transform
aa_tf_tfmat_muli
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_tf_tfmat_ident
static const double aa_tf_tfmat_ident[12]
Identity transformation matrix array.
Definition: spatial.h:208
aa_tf_rotmat2quat
AA_API void aa_tf_rotmat2quat(const double rotmat[AA_RESTRICT 9], double quat[AA_RESTRICT 4])
convert rotation matrix to quaternion
aa_tf_qutr_mul
AA_API void aa_tf_qutr_mul(const double a[7], const double b[7], double c[7])
quaternion-translation multiply
aa_tf_qsacc_rk
AA_API void aa_tf_qsacc_rk(const double q0[AA_RESTRICT 4], const double v[AA_RESTRICT 3], const double a[AA_RESTRICT 3], double dt, double q1[AA_RESTRICT 4])
Runge-kutta integration of unit quaternion from velocity and acceleration.
aa_tf_axang2rotmat
AA_API void aa_tf_axang2rotmat(const double ra[AA_RESTRICT 4], double R[AA_RESTRICT 9])
convert axis angle to rotation matrix
aa_tf_rotvec2rotmat
AA_API void aa_tf_rotvec2rotmat(const double rv[AA_RESTRICT 3], double R[AA_RESTRICT 9])
convert rotatoin vector to rotation matrix
aa_tf_9rel
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_tf_qv_conj
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_tf_tfmat_svel
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_TF_DEF_EULER
#define AA_TF_DEF_EULER(letters)
Create declarations for Euler angle conversion functions.
Definition: spatial.h:1399
aa_tf_dhdist2qutr
AA_API void aa_tf_dhdist2qutr(double alpha, double a, double d, double phi, double E[AA_RESTRICT 7])
Convert distal DH parameters to quaternion-translation.
aa_tf_qslerpalg
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.
aa_tf_qln_jac
AA_API void aa_tf_qln_jac(const double q[4], struct aa_dmat *J)
Compute the Jacobian of the quaternion logarithm.
aa_tf_axang_normalize
AA_API void aa_tf_axang_normalize(double axang[AA_RESTRICT 4])
Normalize the axis.
aa_tf_yxyz2duqu
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_tf_sinccos2
static void aa_tf_sinccos2(double theta2, double *sc, double *c)
Compute sinc(theta), cos(theta), given the square of theta.
Definition: spatial.h:98
aa_tf_qslerpchaindiff
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_tf_duqu_svel
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.
aa_tf_rotmat_svel
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_tf_vec_y
static const double aa_tf_vec_y[3]
A Y axis.
Definition: spatial.h:253
aa_tf_qutr_sdiff
AA_API void aa_tf_qutr_sdiff(const double e0[7], const double de[7], double dt, double e1[7])
Integrate a quaternion-translation.
aa_tf_rotmat_xy
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_tf_dhdist2duqu
AA_API void aa_tf_dhdist2duqu(double alpha, double a, double d, double phi, double S[AA_RESTRICT 8])
Convert distal DH parameters to dual quaternion.
AA_TF_QUAT_IDENT_INITIALIZER
#define AA_TF_QUAT_IDENT_INITIALIZER
Static initializer for an identity quaternion.
Definition: spatial.h:162
aa_tf_qmulc
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_tf_rotmat_lnv
AA_API void aa_tf_rotmat_lnv(const double R[AA_RESTRICT 9], double v[AA_RESTRICT 3])
Rotation Matrix logarithm.
aa_tf_duqu_stwist
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_tf_12rel
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_tf_duqu_conj1
AA_API void aa_tf_duqu_conj1(double s[AA_RESTRICT 8])
Dual quaternion conjugate, in-place.
aa_tf_xyz2duqu
AA_API void aa_tf_xyz2duqu(double x, double y, double z, double d[AA_RESTRICT 8])
Pure translation dual quaternion.
aa_tf_duqu_matrix_l
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.
aa_tf_cross
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_tf_rotmat_yz
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_tf_rotvec2quat
AA_API void aa_tf_rotvec2quat(const double rotvec[AA_RESTRICT 3], double q[AA_RESTRICT 4])
covert rotation vector to quaternion
aa_tf_vnorm
AA_API double aa_tf_vnorm(const double a[AA_RESTRICT 3])
Norm of vector.
aa_tf_quat2rotvec
AA_API void aa_tf_quat2rotvec(const double q[AA_RESTRICT 4], double rotvec[AA_RESTRICT 3])
covert quaternion to rotation vector
aa_tf_qssd
AA_API double aa_tf_qssd(const double a[AA_RESTRICT 4], const double b[AA_RESTRICT 4])
Sum-square-differences of two quaternions.
aa_tf_axang2rotvec
AA_API void aa_tf_axang2rotvec(const double axang[AA_RESTRICT 4], double rotvec[AA_RESTRICT 3])
convert axis-angle to rotation vector
aa_tf_zangle2quat
AA_API void aa_tf_zangle2quat(double theta_z, double q[AA_RESTRICT 4])
Unit quaternion for angle about z axis.
aa_tf_qangle_rel
AA_API double aa_tf_qangle_rel(const double *q, const double *p)
Relative quaternion angles.
aa_tf_yangle2quat
AA_API void aa_tf_yangle2quat(double theta_y, double q[AA_RESTRICT 4])
Unit quaternion for angle about y axis.
aa_tf_qisub
AA_API void aa_tf_qisub(double a[AA_RESTRICT 4], const double b[AA_RESTRICT 4])
Quaternion subtraction, in-place.
aa_tf_duqu_twist2vel
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_tf_tfmat2_tf
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_tf_rotvec_near
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.
aa_tf_skewsym_scal_c
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_tf_tfmat2duqu
AA_API void aa_tf_tfmat2duqu(const double T[AA_RESTRICT 12], double d[AA_RESTRICT 8])
Convert transformation matrix to dual quaternion.
aa_tf_qdot
AA_API double aa_tf_qdot(const double a[AA_RESTRICT 4], const double b[AA_RESTRICT 4])
Quaternion dot product.
aa_tf_xangle2axang
AA_API void aa_tf_xangle2axang(double theta_x, double axang[AA_RESTRICT 4])
Angle about x axis.
aa_tf_xxyz2duqu
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.
aa_tf_duqu_sub
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_tf_qvelrk1
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.
aa_tf_rotvec2axang
AA_API void aa_tf_rotvec2axang(const double rotvec[AA_RESTRICT 3], double axang[AA_RESTRICT 4])
convert rotation vector to axis-angle
aa_tf_tfmat2_imul
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_TF_QDOT
static double AA_TF_QDOT(const double a[AA_RESTRICT 4], const double b[AA_RESTRICT 4])
Inlined quaternion dot product.
Definition: spatial.h:730
aa_tf_93inv
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_tf_tfmat_tf
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_tf_tfmat_imul
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_tf_qutr_mulc
AA_API void aa_tf_qutr_mulc(const double a[7], const double b[7], double c[7])
quaternion-translation conjugate multiply
AA_TF_ROTMAT_IDENT_INITIALIZER
#define AA_TF_ROTMAT_IDENT_INITIALIZER
Static initializer for an identity rotation matrix.
Definition: spatial.h:157
aa_tf_qrk1
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_TF_CROSSX
#define AA_TF_CROSSX(a, b, c)
Macro to compute the cross product of length 3 vectors.
Definition: spatial.h:643
aa_tf_tfmat_lnv
AA_API void aa_tf_tfmat_lnv(const double T[AA_RESTRICT 12], double v[AA_RESTRICT 6])
Transformation Matrix logarithm.
aa_tf_tfmat_mul
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_tf_qmat_l
AA_API void aa_tf_qmat_l(const double *q, struct aa_dmat *M)
Construct matrix for left quaternion multiply q*p = M*p.
aa_tf_qv2tfmat
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.
aa_tf_dhprox2tfmat
AA_API void aa_tf_dhprox2tfmat(double alpha, double a, double d, double phi, double T[AA_RESTRICT 12])
Convert proximal DH parameters to transformation matrix.
aa_tf_93rel
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_tf_tfmat2qutr
AA_API void aa_tf_tfmat2qutr(const double T[12], double e[7])
transformation matrix to quaternion-translation
aa_tf_tfmat_expv
AA_API void aa_tf_tfmat_expv(const double v[AA_RESTRICT 6], double T[AA_RESTRICT 12])
Transformation Matrix exponential.
aa_tf_qdpexpj
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_tf_duqu_mulc
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_tf_axang_make
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_dmat
Descriptor for a block matrix.
Definition: mat.h:72