80 #define AA_TF_DEF_SERIES(name, a0, a1, a2) \ 81 static inline double \ 82 aa_tf_ ## name ##_series2(double theta2) \ 84 return aa_horner3( theta2, a0, a1, a2 ); \ 86 static inline double \ 87 aa_tf_ ## name ## _series(double theta) \ 89 return aa_tf_ ## name ## _series2(theta*theta); \ 106 if( theta2 < sqrt(DBL_EPSILON) ) {
107 *sc = aa_tf_sinc_series2(theta2);
108 *c = aa_tf_cos_series2(theta2);
110 double theta = sqrt(theta2);
111 double s = sin(theta);
123 if( theta*theta < sqrt(DBL_EPSILON) ) {
124 *sc = aa_tf_sinc_series(theta);
125 *c = aa_tf_cos_series(theta);
127 double s = sin(theta);
139 if( fabs(theta) < sqrt(sqrt(DBL_EPSILON)) ) {
140 return aa_tf_sinc_series(theta);
142 return sin(theta)/theta;
204 #define AA_TF_QUAT_V 0 205 #define AA_TF_QUAT_XYZ AA_TF_QUAT_V 207 #define AA_TF_QUAT_X (AA_TF_QUAT_V + AA_TF_X) 209 #define AA_TF_QUAT_Y (AA_TF_QUAT_V + AA_TF_Y) 211 #define AA_TF_QUAT_Z (AA_TF_QUAT_V + AA_TF_Z) 213 #define AA_TF_QUAT_W 3 277 #define AA_TF_TFMAT_R 0 282 #define AA_TF_TFMAT_V 9 287 #define AA_TF_TFMAT_X 9 292 #define AA_TF_TFMAT_Y 10 297 #define AA_TF_TFMAT_Z 11 357 #define AA_TF_QUTR_Q 0 358 #define AA_TF_QUTR_QX (AA_TF_QUTR_Q + AA_TF_QUAT_X) 360 #define AA_TF_QUTR_QY (AA_TF_QUTR_Q + AA_TF_QUAT_Y) 362 #define AA_TF_QUTR_QZ (AA_TF_QUTR_Q + AA_TF_QUAT_Z) 364 #define AA_TF_QUTR_QW (AA_TF_QUTR_Q + AA_TF_QUAT_W) 368 #define AA_TF_QUTR_T 4 369 #define AA_TF_QUTR_TX (AA_TF_QUTR_T + AA_TF_X) 371 #define AA_TF_QUTR_TY (AA_TF_QUTR_T + AA_TF_Y) 373 #define AA_TF_QUTR_TZ (AA_TF_QUTR_T + AA_TF_Z) 377 #define AA_TF_DUQU_REAL 0 378 #define AA_TF_DUQU_DUAL 4 382 #define AA_TF_DUQU_REAL_W (AA_TF_DUQU_REAL + AA_TF_QUAT_W) 383 #define AA_TF_DUQU_REAL_XYZ (AA_TF_DUQU_REAL + AA_TF_QUAT_XYZ) 385 #define AA_TF_DUQU_REAL_X (AA_TF_DUQU_REAL + AA_TF_QUAT_X) 387 #define AA_TF_DUQU_REAL_Y (AA_TF_DUQU_REAL + AA_TF_QUAT_Y) 389 #define AA_TF_DUQU_REAL_Z (AA_TF_DUQU_REAL + AA_TF_QUAT_Z) 393 #define AA_TF_DUQU_DUAL_W (AA_TF_DUQU_DUAL + AA_TF_QUAT_W) 394 #define AA_TF_DUQU_DUAL_XYZ (AA_TF_DUQU_DUAL + AA_TF_QUAT_XYZ) 396 #define AA_TF_DUQU_DUAL_X (AA_TF_DUQU_DUAL + AA_TF_QUAT_X) 398 #define AA_TF_DUQU_DUAL_Y (AA_TF_DUQU_DUAL + AA_TF_QUAT_Y) 400 #define AA_TF_DUQU_DUAL_Z (AA_TF_DUQU_DUAL + AA_TF_QUAT_Z) 404 #define AA_TF_EPSILON .0001 411 #define AA_TF_IDENT_INITIALIZER {1,0,0, 0,1,0, 0,0,1, 0,0,0} 416 #define AA_TF_ROTMAT_IDENT_INITIALIZER {1,0,0, 0,1,0, 0,0,1} 421 #define AA_TF_QUAT_IDENT_INITIALIZER {0,0,0,1} 426 #define AA_TF_DUQU_IDENT_INITIALIZER {0,0,0,1, 0,0,0,0} 431 #define AA_TF_QUTR_IDENT_INITIALIZER {0,0,0,1, 0,0,0} 436 #define AA_TF_AXANG_IDENT_INITIALIZER {1,0,0,0} 441 #define AA_TF_ROTVEC_IDENT_INITIALIZER {0,0,0} 446 #define AA_TF_VEC_IDENT_INITIALIZER {0,0,0} 449 #define AA_TF_IDENT ( (double[12]) AA_TF_IDENT_INITIALIZER ) 450 #define AA_TF_ROTMAT_IDENT ( (double[9] AA_TF_ROTMAT_IDENT_INITIALIZER ) 452 #define AA_TF_QUAT_IDENT ( (double[4]) AA_TF_QUAT_IDENT_INITIALIZER ) 454 #define AA_TF_AXANG_IDENT ( (double[4]) AA_TF_AXANG_IDENT_INITIALIZER ) 456 #define AA_TF_ROTVEC_IDENT ( (double[3]) AA_TF_ROTVEC_IDENT_INITIALIZER ) 525 const double p0[AA_RESTRICT 3],
526 double p1[AA_RESTRICT 3] );
529 const double v[AA_RESTRICT 3],
530 const double p0[AA_RESTRICT 3],
531 double p1[AA_RESTRICT 4] );
534 const double v[AA_RESTRICT 3],
535 const double p0[AA_RESTRICT 3],
536 double p1[AA_RESTRICT 4] );
540 const double p0[AA_RESTRICT 3],
541 double p1[AA_RESTRICT 4] );
545 double Ti[AA_RESTRICT 12] );
548 const double v[AA_RESTRICT 3],
549 double Ri[AA_RESTRICT 9],
double vi[AA_RESTRICT 3] );
556 double qc[AA_RESTRICT 4],
double vc[AA_RESTRICT 3] );
560 const double T2[AA_RESTRICT 12],
561 double T[AA_RESTRICT 12] );
565 const double T1[AA_RESTRICT 12],
566 const double T2[AA_RESTRICT 12],
571 const double v0[AA_RESTRICT 3],
572 const double R1[AA_RESTRICT 9],
573 const double v1[AA_RESTRICT 3],
574 double R[AA_RESTRICT 9],
double v[AA_RESTRICT 3] );
577 const double v0[AA_RESTRICT 3],
578 const double q1[AA_RESTRICT 4],
579 const double v1[AA_RESTRICT 3],
580 double q[AA_RESTRICT 4],
581 double v[AA_RESTRICT 3] );
585 const double v1[AA_RESTRICT 3],
586 const double R2[AA_RESTRICT 9],
587 const double v2[AA_RESTRICT 3],
588 double Rrel[AA_RESTRICT 9],
589 double vrel[AA_RESTRICT 3] );
593 const double T2[AA_RESTRICT 12],
594 double Trel[AA_RESTRICT 12] );
624 const double a[AA_RESTRICT 3],
const double b[AA_RESTRICT 3],
634 const double R1[AA_RESTRICT 9],
635 double R[AA_RESTRICT 9] );
641 const double T1[AA_RESTRICT 12],
642 double T[AA_RESTRICT 12] );
648 const double v0[AA_RESTRICT 3],
649 const double R1[AA_RESTRICT 9],
650 const double v1[AA_RESTRICT 3],
651 double R[AA_RESTRICT 9],
double v[AA_RESTRICT 3] );
661 const double R1[AA_RESTRICT 9],
662 double R[AA_RESTRICT 9] );
670 const double R1[AA_RESTRICT 9],
671 double R[AA_RESTRICT 9] );
679 const double T1[AA_RESTRICT 12],
680 double T[AA_RESTRICT 12] );
688 const double T1[AA_RESTRICT 12],
689 double T[AA_RESTRICT 12] );
697 const double v0[AA_RESTRICT 3],
698 const double R1[AA_RESTRICT 9],
699 const double v1[AA_RESTRICT 3],
700 double R[AA_RESTRICT 9],
double v[AA_RESTRICT 3] );
708 const double v0[AA_RESTRICT 3],
709 const double R1[AA_RESTRICT 9],
710 const double v1[AA_RESTRICT 3],
711 double R[AA_RESTRICT 9],
double v[AA_RESTRICT 3] );
726 const double p0[AA_RESTRICT 3],
727 double p1[AA_RESTRICT 3] );
733 const double p0[AA_RESTRICT 3],
734 double p1[AA_RESTRICT 3] );
740 const double v[AA_RESTRICT 3],
741 const double p0[AA_RESTRICT 3],
742 double p1[AA_RESTRICT 4] );
753 double Ri[AA_RESTRICT 9] );
764 double Ti[AA_RESTRICT 12] );
779 const double R1[AA_RESTRICT 9],
780 double R[AA_RESTRICT 9] );
783 const double p0[AA_RESTRICT 3],
784 double p1[AA_RESTRICT 3] );
787 AA_API void aa_tf_9rel(
const double R1[AA_RESTRICT 9],
const double R2[AA_RESTRICT 9],
788 double Ri[AA_RESTRICT 9] );
807 const double w[AA_RESTRICT 3],
double dR[AA_RESTRICT 9] );
811 const double dR[AA_RESTRICT 9],
double w[AA_RESTRICT 3] );
815 const double w[AA_RESTRICT 3],
double dt,
816 double R1[AA_RESTRICT 9] );
822 const double w[AA_RESTRICT 3],
double dt,
823 double T1[AA_RESTRICT 12] );
827 const double R1[AA_RESTRICT 9],
const double R2[AA_RESTRICT 9], ... );
835 const double y_axis[AA_RESTRICT 3],
836 double R[AA_RESTRICT 9] );
842 const double z_axis[AA_RESTRICT 3],
843 double R[AA_RESTRICT 9] );
850 const double x_axis[AA_RESTRICT 3],
851 double R[AA_RESTRICT 9] );
863 #define AA_TF_DOTX(a,b) \ 864 ((a)[0]*(b)[0] + (a)[1]*(b)[1] + (a)[2]*(b)[2]) 870 AA_TF_VDOT(
const double a[AA_RESTRICT 3],
const double b[AA_RESTRICT 3] )
879 AA_TF_VDOTF(
const float a[AA_RESTRICT 3],
const float b[AA_RESTRICT 3] )
889 aa_tf_vdot(
const double a[AA_RESTRICT 3],
const double b[AA_RESTRICT 3] );
895 aa_tf_vdotf(
const float a[AA_RESTRICT 3],
const float b[AA_RESTRICT 3] );
902 #define AA_TF_CROSSX( a, b, c ) \ 903 (c)[0] = (a)[1]*(b)[2] - (a)[2]*(b)[1]; \ 904 (c)[1] = (a)[2]*(b)[0] - (a)[0]*(b)[2]; \ 905 (c)[2] = (a)[0]*(b)[1] - (a)[1]*(b)[0]; \ 912 AA_TF_CROSS(
const double a[AA_RESTRICT 3],
const double b[AA_RESTRICT 3],
double c[AA_RESTRICT 3] )
921 AA_TF_CROSSF(
const float a[AA_RESTRICT 3],
const float b[AA_RESTRICT 3],
float c[AA_RESTRICT 3] )
929 AA_API void aa_tf_cross(
const double a[AA_RESTRICT 3],
const double b[AA_RESTRICT 3],
930 double c[AA_RESTRICT 3] ) ;
936 double c[AA_RESTRICT 3] ) ;
956 float c[AA_RESTRICT 3] ) ;
973 aa_tf_vssd(
const double a[AA_RESTRICT 3],
const double b[AA_RESTRICT 3] );
989 AA_TF_QDOT(
const double a[AA_RESTRICT 4],
const double b[AA_RESTRICT 4] )
991 return a[0]*b[0] + a[1]*b[1] + a[2]*b[2] + a[3]*b[3];
998 aa_tf_qssd(
const double a[AA_RESTRICT 4],
const double b[AA_RESTRICT 4] );
1004 aa_tf_qdot(
const double a[AA_RESTRICT 4],
const double b[AA_RESTRICT 4] );
1016 aa_tf_qscal(
double q[AA_RESTRICT 4],
double alpha );
1040 double r[AA_RESTRICT 4] );
1048 double r[AA_RESTRICT 4] );
1053 double r[AA_RESTRICT 4] );
1062 const double dq[AA_RESTRICT 4],
1063 double dln[AA_RESTRICT 3] );
1073 const double dq[AA_RESTRICT 4],
1074 double dln[AA_RESTRICT 3] );
1083 const double de[AA_RESTRICT 3],
1084 double dq[AA_RESTRICT 4] );
1094 const double de[AA_RESTRICT 3],
1095 double dq[AA_RESTRICT 4] );
1111 (
const double q[AA_RESTRICT 4],
const double p[AA_RESTRICT 4] );
1115 double r[AA_RESTRICT 4] );
1119 const double b[AA_RESTRICT 4],
1120 double c[AA_RESTRICT 4] );
1124 const double b[AA_RESTRICT 4],
1125 double c[AA_RESTRICT 4] );
1129 const double b[AA_RESTRICT 4] );
1133 const double b[AA_RESTRICT 4] );
1137 const double b[AA_RESTRICT 4],
1138 double c[AA_RESTRICT 4] );
1142 const double b[AA_RESTRICT 4],
1143 double c[AA_RESTRICT 4] );
1147 const double v[AA_RESTRICT 3],
1148 double c[AA_RESTRICT 4] );
1152 const double q[AA_RESTRICT 4],
1153 double c[AA_RESTRICT 4] );
1157 const double b[AA_RESTRICT 4],
1158 double c[AA_RESTRICT 4] );
1162 const double b[AA_RESTRICT 4],
1163 double c[AA_RESTRICT 4] );
1170 aa_tf_qrot1(
const double q[AA_RESTRICT 4],
double v[AA_RESTRICT 3] );
1174 const double v[AA_RESTRICT 3],
1175 double p[AA_RESTRICT 3] );
1181 const double q2[AA_RESTRICT 4],
1182 double q_rel[AA_RESTRICT 4]);
1186 const double b[AA_RESTRICT 4],
1187 double c[AA_RESTRICT 4] );
1192 const double b[AA_RESTRICT 4],
1193 double c[AA_RESTRICT 4] );
1201 const double b[AA_RESTRICT 4],
1202 double c[AA_RESTRICT 4] );
1208 const double b[AA_RESTRICT 4],
1209 double c[AA_RESTRICT 4] );
1213 const double q1[AA_RESTRICT 4],
const double dq1[AA_RESTRICT 4],
1214 const double q2[AA_RESTRICT 4],
const double dq2[AA_RESTRICT 4],
1215 double q[AA_RESTRICT 4],
double dq[AA_RESTRICT 4] );
1219 const double q1[AA_RESTRICT 4],
const double q2[AA_RESTRICT 4],
1220 double u34,
double du34,
1221 const double q3[AA_RESTRICT 4],
const double q4[AA_RESTRICT 4],
1222 double u,
double du,
1223 double q[AA_RESTRICT 4],
double dq[AA_RESTRICT 4] );
1227 const double dq_dt[AA_RESTRICT 4],
1228 double v[AA_RESTRICT 3] );
1232 const double v[AA_RESTRICT 3],
1233 double dq_dt[AA_RESTRICT 4] );
1248 const double dq[AA_RESTRICT 4],
1250 double q1[AA_RESTRICT 4] );
1261 const double v[AA_RESTRICT 3],
1263 double q1[AA_RESTRICT 4] );
1275 const double v[AA_RESTRICT 3],
1277 double q1[AA_RESTRICT 4] );
1288 const double v[AA_RESTRICT 3],
1290 double q1[AA_RESTRICT 4] );
1301 const double dq[AA_RESTRICT 4],
1303 double q1[AA_RESTRICT 4] );
1306 AA_API void aa_tf_qsacc_rk(
const double q0[AA_RESTRICT 4],
1307 const double v[AA_RESTRICT 3],
1308 const double a[AA_RESTRICT 3],
1310 double q1[AA_RESTRICT 4] );
1312 AA_API void aa_tf_qsacc(
const double q0[AA_RESTRICT 4],
1313 const double v[AA_RESTRICT 3],
1314 const double a[AA_RESTRICT 3],
1316 double q1[AA_RESTRICT 4] );
1330 (
size_t n,
const double *w,
const double *q,
size_t ldqq,
double *M );
1342 (
size_t n,
const double *w,
const double *Q,
size_t ldq,
double *
y );
1357 AA_API void aa_tf_qmat_l(
const double *q,
struct aa_dmat *M );
1358 AA_API void aa_tf_qmat_r(
const double *q,
struct aa_dmat *M );
1370 double axang[AA_RESTRICT 4] );
1375 double rv_p[AA_RESTRICT 4] );
1379 double rv_p[AA_RESTRICT 3] );
1384 const double rv_near[AA_RESTRICT 3],
1385 double rv_p[AA_RESTRICT 3] );
1390 const double p[AA_RESTRICT 3],
1391 double q[AA_RESTRICT 3] );
1399 double qc[4],
double vc[3] );
1405 #define AA_TF_QUTR_Q 0 1411 #define AA_TF_QUTR_V 4 1426 void aa_tf_qutr_mul(
const double a[7],
const double b[7],
double c[7] ) ;
1432 aa_tf_qutr_tf(
const double E[AA_RESTRICT 7],
const double p0[AA_RESTRICT 3],
1433 double p1[AA_RESTRICT 3] );
1442 void aa_tf_qutr_mulc(
const double a[7],
const double b[7],
double c[7] ) ;
1445 void aa_tf_qutr_cmul(
const double a[7],
const double b[7],
double c[7] ) ;
1451 (
const double w[3],
const double dv[3],
1452 double q[4],
double v[3] );
1458 (
const double q[4],
const double v[3],
1459 double w[3],
double dv[3] );
1465 (
const double w[6],
double e[7] );
1471 (
const double e[7],
double w[6] );
1475 (
const double e[7],
const double w[6],
double dx[6] );
1479 (
const double e[7],
const double de[7],
double dx[6] );
1483 (
const double e[7],
const double dx[6],
double de[7] );
1490 const double w[AA_RESTRICT 3],
const double dv[AA_RESTRICT 3],
1491 double tw[AA_RESTRICT 3],
double tv[AA_RESTRICT 3] );
1496 const double tw[AA_RESTRICT 3],
const double tv[AA_RESTRICT 3],
1497 double w[AA_RESTRICT 3],
double dv[AA_RESTRICT 3] );
1501 (
const double e0[7],
const double dx[6],
double dt,
double e1[7] );
1505 (
const double e0[7],
const double de[7],
double dt,
double e1[7] );
1509 (
size_t n,
const double *w,
const double *EE,
size_t ldee,
double *a );
1520 double axang[AA_RESTRICT 4] );
1525 double q[AA_RESTRICT 4] );
1530 double q[AA_RESTRICT 4] );
1538 const double v[AA_RESTRICT 3],
1539 double q[AA_RESTRICT 4] );
1544 double rotvec[AA_RESTRICT 3] );
1548 double axang[AA_RESTRICT 4] );
1552 double q[AA_RESTRICT 4] );
1555 double rotvec[AA_RESTRICT 3] );
1560 const double rv_near[AA_RESTRICT 3],
1561 double rotvec[AA_RESTRICT 3] );
1565 double rotmat[AA_RESTRICT 9] );
1568 double quat[AA_RESTRICT 4] );
1572 double ra[AA_RESTRICT 4] );
1575 double rv[AA_RESTRICT 3] );
1579 double R[AA_RESTRICT 9] );
1584 double R[AA_RESTRICT 9] );
1588 double R[AA_RESTRICT 9] );
1593 double T[AA_RESTRICT 12] ) ;
1602 double e[AA_RESTRICT 3] );
1607 double e[AA_RESTRICT 3] );
1612 #define AA_TF_DEF_EULER(letters) \ 1614 aa_tf_euler ## letters ## 2rotmat( double e1, double e2, double e3, \ 1615 double R[AA_RESTRICT 9] ); \ 1617 aa_tf_euler ## letters ## 2quat( double e1, double e2, double e3, \ 1618 double q[AA_RESTRICT 4] ); 1662 const double target[AA_RESTRICT 3],
1663 const double up[AA_RESTRICT 3],
1664 double R[AA_RESTRICT 9] );
1672 const double target[AA_RESTRICT 3],
1673 const double up[AA_RESTRICT 3],
1674 double T[AA_RESTRICT 12] );
1681 const double target[AA_RESTRICT 3],
1682 const double up[AA_RESTRICT 3],
1683 double q[AA_RESTRICT 4] );
1690 const double target[AA_RESTRICT 3],
1691 const double up[AA_RESTRICT 3],
1692 double q[AA_RESTRICT 4],
1693 double v[AA_RESTRICT 3] );
1700 const double target[AA_RESTRICT 3],
1701 const double up[AA_RESTRICT 3],
1702 double T[AA_RESTRICT 12] );
1709 double d3[AA_RESTRICT 8] );
1713 double d3[AA_RESTRICT 8] );
1717 double y[AA_RESTRICT 8] );
1721 double d3[AA_RESTRICT 8] );
1745 double d3[AA_RESTRICT 8] );
1749 double d3[AA_RESTRICT 8] );
1774 double p1[AA_RESTRICT 3] );
1778 double p1[AA_RESTRICT 3] );
1791 double d[AA_RESTRICT 8] ) ;
1795 double d[AA_RESTRICT 8] ) ;
1799 double d[AA_RESTRICT 8] );
1803 double d[AA_RESTRICT 8] );
1807 double d[AA_RESTRICT 8] );
1811 double q[AA_RESTRICT 4],
double v[AA_RESTRICT 3] ) ;
1814 void aa_tf_tfmat2av(
const double T[12],
double q[AA_RESTRICT 4],
double v[AA_RESTRICT 3] ) ;
1818 double t[AA_RESTRICT 8] ) ;
1822 double dx[AA_RESTRICT 6] ) ;
1826 double dd[AA_RESTRICT 8] ) ;
1830 double dd[AA_RESTRICT 8] ) ;
1834 double dx[AA_RESTRICT 6] ) ;
1839 double twist[AA_RESTRICT 8] ) ;
1848 double dt,
double d1[AA_RESTRICT 6] ) ;
1857 double dt,
double d1[AA_RESTRICT 6] ) ;
1866 double dt,
double d1[AA_RESTRICT 6] ) ;
1923 double v[AA_RESTRICT 6] );
1929 double S[AA_RESTRICT 8]);
1943 aa_tf_dhprox2tfmat(
double alpha,
double a,
double d,
double phi,
1944 double T[AA_RESTRICT 12]);
1946 aa_tf_dhprox2duqu(
double alpha,
double a,
double d,
double phi,
1947 double S[AA_RESTRICT 8]);
1949 aa_tf_dhprox2qutr(
double alpha,
double a,
double d,
double phi,
1950 double E[AA_RESTRICT 7]);
1952 aa_tf_dhprox2qv(
double alpha,
double a,
double d,
double phi,
1953 double q[AA_RESTRICT 4],
double v[3]);
1956 aa_tf_dhdist2tfmat(
double alpha,
double a,
double d,
double phi,
1957 double T[AA_RESTRICT 12]);
1959 aa_tf_dhdist2duqu(
double alpha,
double a,
double d,
double phi,
1960 double S[AA_RESTRICT 8]);
1962 aa_tf_dhdist2qutr(
double alpha,
double a,
double d,
double phi,
1963 double E[AA_RESTRICT 7]);
1965 aa_tf_dhdist2qv(
double alpha,
double a,
double d,
double phi,
1966 double q[AA_RESTRICT 4],
double v[3]);
1971 aa_tf_relx_mean(
size_t n,
const double *R,
1972 const double *X,
size_t ldx,
1973 const double *Y,
size_t ldy,
1977 aa_tf_relx_median(
size_t n,
const double *R,
1978 const double *X,
size_t ldx,
1979 const double *Y,
size_t ldy,
1992 void aa_tf_proj(
const double a[AA_RESTRICT 3],
1993 const double b[AA_RESTRICT 3],
1994 double c[AA_RESTRICT 3] );
2006 const double b[AA_RESTRICT 3],
2007 double c[AA_RESTRICT 3] );
AA_API void aa_tf_qln_jac(const double q[4], struct aa_dmat *J)
Compute the Jacobian of the quaternion logarithm.
AA_API void aa_tf_qminimize(double q[AA_RESTRICT 4])
Minimize angle represented by the quaternion.
AA_API void aa_tf_yangle2axang(double theta_y, double axang[AA_RESTRICT 4])
Angle about y axis.
void aa_tf_qutr_sdiff(const double e0[7], const double de[7], double dt, double e1[7])
Integrate a quaternion-translation.
AA_API void aa_tf_tfmat2_muli(const double R0[AA_RESTRICT 9], const double v0[AA_RESTRICT 3], const double R1[AA_RESTRICT 9], const double v1[AA_RESTRICT 3], double R[AA_RESTRICT 9], double v[AA_RESTRICT 3])
Multiple (R0,v0) by inverse of transform transform (R1,v1).
AA_API void aa_tf_qdpexpj(const double e[AA_RESTRICT 3], const double de[AA_RESTRICT 3], double dq[AA_RESTRICT 4])
Derivative of the Pure Quaternion Exponential, computed via Jacobian.
AA_API void aa_tf_qv_mzlook(const double eye[AA_RESTRICT 3], const double target[AA_RESTRICT 3], const double up[AA_RESTRICT 3], double q[AA_RESTRICT 4], double v[AA_RESTRICT 3])
Find the camera frame, looking in negative z direction.
static double AA_TF_QDOT(const double a[AA_RESTRICT 4], const double b[AA_RESTRICT 4])
Inlined quaternion dot product.
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.
static void aa_tf_sinccos2(double theta2, double *sc, double *c)
Compute sinc(theta), cos(theta), given the square of theta.
double aa_tf_qssd(const double a[AA_RESTRICT 4], const double b[AA_RESTRICT 4])
Sum-square-differences of two quaternions.
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.
AA_API void aa_tf_xyz2duqu(double x, double y, double z, double d[AA_RESTRICT 8])
Pure translation dual quaternion.
AA_API void aa_tf_qconj1(double q[AA_RESTRICT 4])
Quaternion conjugate, in-place.
AA_API void aa_tf_duqu2tfmat(const double d[AA_RESTRICT 8], double T[AA_RESTRICT 12])
Convert dual quaternion to transformation matrix.
AA_API void aa_tf_9rot(const double R[AA_RESTRICT 9], const double p0[AA_RESTRICT 3], double p1[AA_RESTRICT 3])
rotate p0 by R
#define AA_TF_ROTVEC_IDENT_INITIALIZER
Static initializer for an identity rotation-vector.
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
void aa_tf_qutr_svel(const double e0[7], const double dx[6], double dt, double e1[7])
Integrate a quaternion-translation.
AA_API void aa_tf_tfmat_mzlook(const double eye[AA_RESTRICT 3], const double target[AA_RESTRICT 3], const double up[AA_RESTRICT 3], double T[AA_RESTRICT 12])
Find the camera frame, looking in negative z direction.
AA_API void aa_tf_qiadd(double a[AA_RESTRICT 4], const double b[AA_RESTRICT 4])
Quaternion addition, in-place.
void aa_tf_qutr_conj(const double a[7], double c[7])
quaternion-translation conjugate
AA_API void aa_tf_9mul(const double R0[AA_RESTRICT 9], const double R1[AA_RESTRICT 9], double R[AA_RESTRICT 9])
Multiply two rotation matrices.
AA_API void aa_tf_vecs2quat(const double u[AA_RESTRICT 3], const double v[AA_RESTRICT 3], double q[AA_RESTRICT 4])
Convert rotation between two vectors to a quaternion.
AA_API float aa_tf_vdotf(const float a[AA_RESTRICT 3], const float b[AA_RESTRICT 3])
Vector dot product.
AA_API void aa_tf_duqu_matrix_l(const double *q, double *M, size_t ldm)
Construct matrix for left dual quaternion multiply q*p = M*p.
void aa_tf_qutr_mul(const double a[7], const double b[7], double c[7])
quaternion-translation multiply
AA_API void aa_tf_qmatrix_l(const double *q, double *M, size_t ldm)
Construct matrix for left quaternion multiply q*p = M*p.
AA_API void aa_tf_qadd(const double a[AA_RESTRICT 4], const double b[AA_RESTRICT 4], double c[AA_RESTRICT 4])
Quaternion addition.
AA_API void aa_tf_qcmul(const double a[AA_RESTRICT 4], const double b[AA_RESTRICT 4], double c[AA_RESTRICT 4])
Quaternion conjugate a and multiply by b.
AA_API void aa_tf_zangle2axang(double theta_z, double axang[AA_RESTRICT 4])
Angle about z axis.
AA_API void aa_tf_axang_make(double x, double y, double z, double theta, double axang[AA_RESTRICT 4])
copy x,y,z,theta into axang
AA_API void aa_tf_rotvec_diff2vel(const double v[3], const double dv[3], double w[3])
Convert rotation vector derivative to rotational velocity.
Memory layout for a vector of length 3.
static const double aa_tf_rotmat_ident[9]
Identity rotation matrix array.
AA_API void aa_tf_qminimize2(const double q[AA_RESTRICT 4], double qmin[AA_RESTRICT 4])
Minimize angle represented by the quaternion.
AA_API void aa_tf_93rel(const double R1[AA_RESTRICT 9], const double v1[AA_RESTRICT 3], const double R2[AA_RESTRICT 9], const double v2[AA_RESTRICT 3], double Rrel[AA_RESTRICT 9], double vrel[AA_RESTRICT 3])
relative transform
AA_API void aa_tf_vnormalizef(float v[AA_RESTRICT 3])
Normalize Vector.
AA_API void aa_tf_qmul(const double a[AA_RESTRICT 4], const double b[AA_RESTRICT 4], double c[AA_RESTRICT 4])
Quaternion multiplication.
#define AA_TF_QUTR_IDENT_INITIALIZER
Static initializer for an identity quaternion-translation.
static const double aa_tf_ident[12]
Identity transformation matrix array.
AA_API void aa_tf_qmul_qv(const double q[AA_RESTRICT 4], const double v[AA_RESTRICT 3], double c[AA_RESTRICT 4])
Quaternion multiplication.
AA_API void aa_tf_duqu_conj1(double s[AA_RESTRICT 8])
Dual quaternion conjugate, in-place.
AA_API void aa_tf_tfmat2duqu(const double T[AA_RESTRICT 12], double d[AA_RESTRICT 8])
Convert transformation matrix to dual quaternion.
AA_API void aa_tf_quat2rotvec_near(const double q[AA_RESTRICT 4], const double rv_near[AA_RESTRICT 3], double rotvec[AA_RESTRICT 3])
covert quaternion to rotation vector minimizing distance from rv_near
AA_API void aa_tf_qv_lnv(const double q[4], const double v[3], double w[3], double dv[3])
Quaternion-vector logarithm.
Memory layout for a transformation matrix.
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.
AA_API void aa_tf_qutr_vel2diff(const double e[7], const double dx[6], double de[7])
Quaternion-translation spatial velocity to derivative.
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.
AA_API void aa_tf_rotmat_exp_aa(const double aa[AA_RESTRICT 4], double R[AA_RESTRICT 9])
Rotation Matrix exponential from axis angle.
struct aa_tf_quat aa_tf_quat_t
Memory layout for a quaternion, x,y,z,w order.
AA_API void aa_tf_qutr_lnv(const double e[7], double w[6])
Quaternion-vector logarithm.
AA_API void aa_tf_duqu_vel2diff(const double d[AA_RESTRICT 8], const double dx[AA_RESTRICT 6], double dd[AA_RESTRICT 8])
Dual quaternion derivative from velocity.
AA_API void aa_tf_xangle2axang(double theta_x, double axang[AA_RESTRICT 4])
Angle about x axis.
double aa_tf_vssd(const double a[AA_RESTRICT 3], const double b[AA_RESTRICT 3])
Sum-square-differences of two vectors.
AA_API void aa_tf_axang2rotmat(const double ra[AA_RESTRICT 4], double R[AA_RESTRICT 9])
convert axis angle to rotation matrix
static const double aa_tf_duqu_ident[8]
Identity dual quaternion array.
AA_API void aa_tf_qslerp3diff(double u12, double du12, const double q1[AA_RESTRICT 4], const double q2[AA_RESTRICT 4], double u34, double du34, const double q3[AA_RESTRICT 4], const double q4[AA_RESTRICT 4], double u, double du, double q[AA_RESTRICT 4], double dq[AA_RESTRICT 4])
Triad sequence of slerp differentiation.
AA_API void aa_tf_rotmat_imul(const double R0[AA_RESTRICT 9], const double R1[AA_RESTRICT 9], double R[AA_RESTRICT 9])
Multiple inverse of R0 by R1.
AA_API void aa_tf_duqu_conj_jac(struct aa_dmat *J)
Fill J with Jacobian of dual quaternion conjugate.
AA_API void aa_tf_qnormalize2(const double q[AA_RESTRICT 4], double qnorm[AA_RESTRICT 4])
Normailize quaternion.
AA_API void aa_tf_proj(const double a[AA_RESTRICT 3], const double b[AA_RESTRICT 3], double c[AA_RESTRICT 3])
Vector projection of a onto b.
AA_API void aa_tf_qvelrk1(const double q0[AA_RESTRICT 4], const double v[AA_RESTRICT 3], double dt, double q1[AA_RESTRICT 4])
Integrate unit quaternion from angular velocity, Runge-Kutta-1 (euler) integration.
void aa_tf_qutr2duqu(const double e[7], double s[8])
quaternion-translation to dual quaternion
AA_API void aa_tf_duqu_mat_r(const double *q, struct aa_dmat *M)
Construct matrix for right dual quaternion multiply p*q = M*p.
void aa_tf_qurand(double q[4])
Generate random unit quaternion.
AA_API void aa_tf_qutr_diff2vel(const double e[7], const double de[7], double dx[6])
Quaternion-translation derivative to spatial velocity.
AA_API void aa_tf_proj_orth(const double a[AA_RESTRICT 3], const double b[AA_RESTRICT 3], double c[AA_RESTRICT 3])
Orthogonal projection of a onto b.
double aa_tf_quhypangle2(const double q[AA_RESTRICT 4], const double p[AA_RESTRICT 4])
Return the angle between unit quaterniosn in 4D space.
AA_API void aa_tf_qutr_tf(const double E[AA_RESTRICT 7], const double p0[AA_RESTRICT 3], double p1[AA_RESTRICT 3])
Transform a point,.
struct aa_tf_rotmat aa_tf_rotmat_t
Memory layout for a rotation matrix.
AA_API void aa_tf_duqu_jac_vel2diff(const double S[8], const struct aa_dmat *Jvel, struct aa_dmat *Js)
Convert a velocity Jacobian to a dual quaternion derivative Jacobian.
AA_API void aa_tf_rotmat2axang(const double R[AA_RESTRICT 9], double ra[AA_RESTRICT 4])
convert rotation matrix to axis angle
AA_API void aa_tf_qutr_mzlook(const double eye[AA_RESTRICT 3], const double target[AA_RESTRICT 3], const double up[AA_RESTRICT 3], double T[AA_RESTRICT 12])
Find the camera frame, looking in negative z direction.
AA_API void aa_tf_axang_normalize(double axang[AA_RESTRICT 4])
Normalize the axis.
AA_API void aa_tf_qmzlook(const double eye[AA_RESTRICT 3], const double target[AA_RESTRICT 3], const double up[AA_RESTRICT 3], double q[AA_RESTRICT 4])
Find the camera frame, looking in negative z direction.
AA_API void aa_tf_axang_permute(const double rv[AA_RESTRICT 4], int k, double rv_p[AA_RESTRICT 4])
Scales angle by k * 2 * pi.
AA_API void aa_tf_duqu_twist2diff(const double d[AA_RESTRICT 8], const double t[AA_RESTRICT 8], double dd[AA_RESTRICT 8])
Dual quaternion twist to derivative.
AA_API void aa_tf_qdiff2vel(const double q[AA_RESTRICT 4], const double dq_dt[AA_RESTRICT 4], double v[AA_RESTRICT 3])
Quaternaion time derivate to angular velocity.
AA_API void aa_tf_qsvel(const double q0[AA_RESTRICT 4], const double v[AA_RESTRICT 3], double dt, double q1[AA_RESTRICT 4])
Integrate unit quaternion from constant angular velocity.
AA_API void aa_tf_rotvec_permute(const double rv[AA_RESTRICT 3], int k, double rv_p[AA_RESTRICT 3])
find alternate equivalent representations of rv
AA_API void aa_tf_zangle2rotmat(double theta_z, double R[AA_RESTRICT 9])
Angle about z axis.
AA_API void aa_tf_yangle2quat(double theta_y, double q[AA_RESTRICT 4])
Unit quaternion for angle about y axis.
AA_API void aa_tf_qutr_expv(const double w[6], double e[7])
Quaternion-vector exponential.
double aa_tf_qangle_rel(const double *q, const double *p)
Relative quaternion angles.
AA_API void aa_tf_rotmat_yz(const double y_axis[AA_RESTRICT 3], const double z_axis[AA_RESTRICT 3], double R[AA_RESTRICT 9])
Construct rotation matrix from a y and z axis of the child frame.
AA_API void aa_tf_qv2duqu(const double q[AA_RESTRICT 4], const double v[AA_RESTRICT 3], double d[AA_RESTRICT 8])
Convert orientation unit quaternion and translation vector to dual quaternion.
AA_API void aa_tf_qv_conj(const double q[AA_RESTRICT 4], const double v[AA_RESTRICT 3], double qc[AA_RESTRICT 4], double vc[AA_RESTRICT 3])
Invert transform.
AA_API void aa_tf_qnormalize(double q[AA_RESTRICT 4])
Normalize Quaternion.
AA_API void aa_tf_qdpexp(const double e[AA_RESTRICT 3], const double de[AA_RESTRICT 3], double dq[AA_RESTRICT 4])
Derivative of the Pure Quaternion Exponential.
void aa_tf_tfmat2qutr(const double T[12], double e[7])
transformation matrix to quaternion-translation
Memory layout for a rotation matrix.
AA_API void aa_tf_rotvec2rotmat(const double rv[AA_RESTRICT 3], double R[AA_RESTRICT 9])
convert rotatoin vector to rotation matrix
struct aa_tf_axang aa_tf_axang_t
Axis-Angle rotation.
AA_API void aa_tf_tfmat2_imul(const double R0[AA_RESTRICT 9], const double v0[AA_RESTRICT 3], const double R1[AA_RESTRICT 9], const double v1[AA_RESTRICT 3], double R[AA_RESTRICT 9], double v[AA_RESTRICT 3])
Multiple inverse of transform (R0,v0) by transform (R1,v1).
AA_API void aa_tf_qvelrk4(const double q0[AA_RESTRICT 4], const double v[AA_RESTRICT 3], double dt, double q1[AA_RESTRICT 4])
Integrate unit quaternion from angular velocity, Runge-Kutta-4 (euler) integration.
double aa_tf_qdot(const double a[AA_RESTRICT 4], const double b[AA_RESTRICT 4])
Quaternion dot product.
AA_API void aa_tf_axang2quat2(const double axis[AA_RESTRICT 3], double angle, double q[AA_RESTRICT 4])
axis-angle to quaternion.
AA_API void aa_tf_yangle2rotmat(double theta_y, double R[AA_RESTRICT 9])
Angle about y axis.
AA_API void aa_tf_tfmat2_tf(const double R[AA_RESTRICT 9], const double v[AA_RESTRICT 3], const double p0[AA_RESTRICT 3], double p1[AA_RESTRICT 4])
Transform a point using a split transformation matrix.
AA_API void aa_tf_qv_expv(const double w[3], const double dv[3], double q[4], double v[3])
Quaternion-vector exponential.
AA_API void aa_tf_axang2rotvec(const double axang[AA_RESTRICT 4], double rotvec[AA_RESTRICT 3])
convert axis-angle to rotation vector
struct aa_tf_eulerzyx aa_tf_eulerzyx_t
Memory layout for an Euler Angle in ZYX (yaw-pitch-roll) format.
void aa_tf_qutr_rand(double E[7])
Generate random transform.
Block matrix descriptors and linear algebra operations.
struct aa_tf_tfmat aa_tf_tfmat_t
Memory layout for a transformation matrix.
AA_API void aa_tf_qscal(double q[AA_RESTRICT 4], double alpha)
Scale a quaternion.
AA_API void aa_tf_qrot1(const double q[AA_RESTRICT 4], double v[AA_RESTRICT 3])
Quaternion point rotation, in place.
AA_API void aa_tf_qsub(const double a[AA_RESTRICT 4], const double b[AA_RESTRICT 4], double c[AA_RESTRICT 4])
Quaternion subtraction.
AA_API void aa_tf_cross_mat_l(const double v[3], struct aa_dmat *M)
Construct matrix for left cross product v*a = M*a.
AA_API void aa_tf_12(const double T[AA_RESTRICT 12], const double p0[AA_RESTRICT 3], double p1[AA_RESTRICT 3])
apply a euclidean transform
AA_API void aa_tf_duqu_ln(const double d[AA_RESTRICT 8], double e[AA_RESTRICT 8])
Dual quaternion natural logarithm.
struct aa_tf_qv aa_tf_qv_t
Memory layout for a Transformation as rotation quaternion and translation vector. ...
AA_API void aa_tf_duqu2pure(const double S[AA_RESTRICT 8], double v[AA_RESTRICT 6])
Convert a pure dual quaternion to conventional dual quaternion.
AA_API void aa_tf_qv_twist2vel(const double q[AA_RESTRICT 4], const double v[AA_RESTRICT 3], const double tw[AA_RESTRICT 3], const double tv[AA_RESTRICT 3], double w[AA_RESTRICT 3], double dv[AA_RESTRICT 3])
Quaternion-vector twist to velocity.
AA_API void aa_tf_skewsym_scal_c(const double u[AA_RESTRICT 3], const double a[AA_RESTRICT 3], const double b[AA_RESTRICT 3], double R[9])
Construct a skew-symmetric matrix.
AA_API void aa_tf_zxyz2duqu(double theta, double x, double y, double z, double d[AA_RESTRICT 8])
Convert z angle and translation to dual quaternion.
void aa_tf_qutr_twist2vel(const double e[7], const double w[6], double dx[6])
Quaternion-translation twist to velocity.
AA_API void aa_tf_qisub(double a[AA_RESTRICT 4], const double b[AA_RESTRICT 4])
Quaternion subtraction, in-place.
AA_API void aa_tf_tfmat_normalize(double T[AA_RESTRICT 12])
Orthonormalize the transformation matrix.
AA_API void aa_tf_tfmat_mul(const double T0[AA_RESTRICT 12], const double T1[AA_RESTRICT 12], double T[AA_RESTRICT 12])
Multiple two transformation matrices.
AA_API void aa_tf_duqu_mulc(const double d1[AA_RESTRICT 8], const double d2[AA_RESTRICT 8], double d3[AA_RESTRICT 8])
Dual quaternion multiply d1 by conjugate of d2.
AA_API void aa_tf_duqu_exp(const double d[AA_RESTRICT 8], double e[AA_RESTRICT 8])
Dual quaternion exponential.
static const double aa_tf_axang_ident[4]
Identity axis-angle array.
static const double aa_tf_vec_y[3]
A Y axis.
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)
AA_API void aa_tf_duqu_sdiff(const double d0[AA_RESTRICT 8], const double dd[AA_RESTRICT 8], double dt, double d1[AA_RESTRICT 6])
Dual quaternion derivative integration.
AA_API void aa_tf_qv_vel2twist(const double q[AA_RESTRICT 4], const double v[AA_RESTRICT 3], const double w[AA_RESTRICT 3], const double dv[AA_RESTRICT 3], double tw[AA_RESTRICT 3], double tv[AA_RESTRICT 3])
Quaternion-vector velocity to twist.
AA_API void aa_tf_duqu_svel(const double d0[AA_RESTRICT 8], const double dx[AA_RESTRICT 6], double dt, double d1[AA_RESTRICT 6])
Dual quaternion velocity integration.
static const double aa_tf_tfmat_ident[12]
Identity transformation matrix array.
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.
AA_API void aa_tf_duqu_tf(const double d[AA_RESTRICT 8], const double p0[AA_RESTRICT 3], double p1[AA_RESTRICT 3])
Dual quaternion transformation.
AA_API void aa_tf_qslerpalg(double tau, const double a[AA_RESTRICT 4], const double b[AA_RESTRICT 4], double c[AA_RESTRICT 4])
Quaternion SLERP, computed algebraicly.
Transform and spatial velocity.
Memory layout for a quaternion, x,y,z,w order.
#define AA_RESTRICT
Defined restrict keyword based on language flavor.
static float AA_TF_VDOTF(const float a[AA_RESTRICT 3], const float b[AA_RESTRICT 3])
Inlined vector dot product.
Memory layout for an Euler Angle in ZYX (yaw-pitch-roll) format.
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.
AA_API void aa_tf_duqu_mat_l(const double *q, struct aa_dmat *M)
Construct matrix for left dual quaternion multiply q*p = M*p.
static const double aa_tf_vec_z[3]
A Z axis.
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
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.
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.
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.
AA_API void aa_tf_12rel(const double T1[AA_RESTRICT 12], const double T2[AA_RESTRICT 12], double Trel[AA_RESTRICT 12])
relative transform
AA_API void aa_tf_vnormalize(double v[AA_RESTRICT 3])
Normalize Vector.
Memory layout for a dual quaternion .
AA_API void aa_tf_93(const double R[AA_RESTRICT 9], const double v[AA_RESTRICT 3], const double p0[AA_RESTRICT 3], double p1[AA_RESTRICT 4])
apply a euclidean transform
AA_API void aa_tf_93chain(const double R0[AA_RESTRICT 9], const double v0[AA_RESTRICT 3], const double R1[AA_RESTRICT 9], const double v1[AA_RESTRICT 3], double R[AA_RESTRICT 9], double v[AA_RESTRICT 3])
chain two transforms
#define AA_API
calling and name mangling convention for functions
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.
AA_API void aa_tf_axang_rot(const double axang[AA_RESTRICT 4], const double p[AA_RESTRICT 3], double q[AA_RESTRICT 3])
Rotate a point.
AA_API void aa_tf_rotmat_svel(const double R0[AA_RESTRICT 9], const double w[AA_RESTRICT 3], double dt, double R1[AA_RESTRICT 9])
Integrate rotational velocity.
AA_API void aa_tf_duqu_sub(const double d1[AA_RESTRICT 8], const double d2[AA_RESTRICT 8], double d3[AA_RESTRICT 8])
Dual quaternion subtraction.
AA_API void aa_tf_rotmat_inv2(const double R[AA_RESTRICT 9], double Ri[AA_RESTRICT 9])
Invert a rotation.
AA_API double aa_tf_vdot(const double a[AA_RESTRICT 3], const double b[AA_RESTRICT 3])
Vector dot product.
AA_API void aa_tf_rotmat_muli(const double R0[AA_RESTRICT 9], const double R1[AA_RESTRICT 9], double R[AA_RESTRICT 9])
Multiple R1 by inverse of R1.
static double aa_tf_sinc(double theta)
Compute sinc(theta)
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.
AA_API void aa_tf_tfmat_svel(const double T0[AA_RESTRICT 12], const double w[AA_RESTRICT 3], double dt, double T1[AA_RESTRICT 12])
Integrate rotational velocity.
AA_API void aa_tf_qsdiff(const double q0[AA_RESTRICT 4], const double dq[AA_RESTRICT 4], double dt, double q1[AA_RESTRICT 4])
Integrate unit quaternion from quaternion derivative.
AA_API void aa_tf_qrel(const double q1[AA_RESTRICT 4], const double q2[AA_RESTRICT 4], double q_rel[AA_RESTRICT 4])
Relative orientation.
AA_API void aa_tf_duqu_conj(const double d[AA_RESTRICT 8], double dconj[AA_RESTRICT 8])
Dual quaternion conjugate.
static void AA_TF_CROSSF(const float a[AA_RESTRICT 3], const float b[AA_RESTRICT 3], float c[AA_RESTRICT 3])
Inlined Vector cross product.
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.
AA_API void aa_tf_pure2duqu(const double v[AA_RESTRICT 6], double S[AA_RESTRICT 8])
Convert a conventional dual quaternion to pure dual quaternion.
void aa_tf_qutr_cmul(const double a[7], const double b[7], double c[7])
quaternion-translation conjugate multiply
Descriptor for a block matrix.
AA_API void aa_tf_duqu_normalize(double d[AA_RESTRICT 8])
Dual quaternion normalization.
AA_API void aa_tf_cross_mat_r(const double v[3], struct aa_dmat *M)
Construct matrix for right cross product a*v = M*a.
AA_API void aa_tf_axang2rotmat2(const double axis[AA_RESTRICT 3], double angle, double R[AA_RESTRICT 9])
convert axis angle to rotation matrix
AA_API void aa_tf_rotmat_mul(const double R0[AA_RESTRICT 9], const double R1[AA_RESTRICT 9], double R[AA_RESTRICT 9])
Multiple two rotation matrices.
AA_API void aa_tf_tf_duqu(const double d[AA_RESTRICT 8], const double p0[AA_RESTRICT 3], double p1[AA_RESTRICT 3])
Dual quaternion transformation.
AA_API void aa_tf_quat2axang(const double q[AA_RESTRICT 4], double axang[AA_RESTRICT 4])
Quaternion to axis-angle.
static const double aa_tf_quat_ident[4]
Identity quaternion array.
static const double aa_tf_qutr_ident[7]
Identity quaternion-translation array.
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.
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.
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. ...
AA_API void aa_tf_tfmat_muli(const double T0[AA_RESTRICT 12], const double T1[AA_RESTRICT 12], double T[AA_RESTRICT 12])
Multiple T1 by inverse of T1.
AA_API void aa_tf_qslerpdiff(double tau, const double a[AA_RESTRICT 4], const double b[AA_RESTRICT 4], double c[AA_RESTRICT 4])
Derivative of quaternation SLERP WRT tau.
AA_API void aa_tf_rotmat_vel2diff(const double R[AA_RESTRICT 9], const double w[AA_RESTRICT 3], double dR[AA_RESTRICT 9])
Velocity to rotation matrix derivative.
AA_API void aa_tf_duqu_smul(double alpha, const double x[AA_RESTRICT 8], double y[AA_RESTRICT 8])
Dual quaternion scalar multiplication.
AA_API void aa_tf_duqu_vel2twist(const double d[AA_RESTRICT 8], const double dx[AA_RESTRICT 6], double t[AA_RESTRICT 8])
Dual quaternion twist from velocity.
AA_API void aa_tf_duqu_minimize(double d[AA_RESTRICT 8])
Dual quaternion angle minimization.
AA_API void aa_tf_qv_chain(const double q0[AA_RESTRICT 4], const double v0[AA_RESTRICT 3], const double q1[AA_RESTRICT 4], const double v1[AA_RESTRICT 3], double q[AA_RESTRICT 4], double v[AA_RESTRICT 3])
chain two transforms
AA_API void aa_tf_v12chain(double T[AA_RESTRICT 12], const double T1[AA_RESTRICT 12], const double T2[AA_RESTRICT 12],...)
Varargs transform chain.
#define AA_TF_DEF_SERIES(name, a0, a1, a2)
Define an inline function for a series using Horner's rule.
AA_API void aa_tf_tfmat_tf(const double T[AA_RESTRICT 12], const double p0[AA_RESTRICT 3], double p1[AA_RESTRICT 3])
Transform a point using a transformation matrix.
double aa_tf_vnorm(const double a[AA_RESTRICT 3])
Norm of vector.
AA_API void aa_tf_tfmat_expv(const double v[AA_RESTRICT 6], double T[AA_RESTRICT 12])
Transformation Matrix exponential.
AA_API void aa_tf_qdulnj(const double q[AA_RESTRICT 4], const double dq[AA_RESTRICT 4], double dln[AA_RESTRICT 3])
Derivative of the Unit Quaternion Logarithm, computed via Jacobian.
AA_API void aa_tf_qrot(const double q[AA_RESTRICT 4], const double v[AA_RESTRICT 3], double p[AA_RESTRICT 3])
Quaternion point rotation.
AA_API void aa_tf_duqu_ln_jac(const double S[8], struct aa_dmat *J)
Compute the Jacobian of the dual quaternion logarithm.
AA_API void aa_tf_93inv(const double R[AA_RESTRICT 9], const double v[AA_RESTRICT 3], double Ri[AA_RESTRICT 9], double vi[AA_RESTRICT 3])
invert transform
AA_API void aa_tf_duqu_stwist(const double d0[AA_RESTRICT 8], const double twist[AA_RESTRICT 8], double dt, double d1[AA_RESTRICT 6])
Dual quaternion twist integration.
AA_API void aa_tf_v9mul(double R[AA_RESTRICT 9], const double R1[AA_RESTRICT 9], const double R2[AA_RESTRICT 9],...)
Vararg multiply two rotation matrices.
AA_API void aa_tf_qslerpchaindiff(double u, double du, const double q1[AA_RESTRICT 4], const double dq1[AA_RESTRICT 4], const double q2[AA_RESTRICT 4], const double dq2[AA_RESTRICT 4], double q[AA_RESTRICT 4], double dq[AA_RESTRICT 4])
Chain-rule slerp differentiation.
AA_API void aa_tf_cross_a(const double a[AA_RESTRICT 3], const double b[AA_RESTRICT 3], double c[AA_RESTRICT 3])
Cross-product accumulate.
AA_API void aa_tf_duqu2qv(const double d[AA_RESTRICT 8], double q[AA_RESTRICT 4], double v[AA_RESTRICT 3])
Convert dual quaternion to orientation unit quaternion and translation vector.
AA_API void aa_tf_skewsym_scal2(double a, double b, const double u[3], double R[9])
Construct a skew-symmetric matrix.
AA_API void aa_tf_rotvec2axang(const double rotvec[AA_RESTRICT 3], double axang[AA_RESTRICT 4])
convert rotation vector to axis-angle
AA_API void aa_tf_rotvec_near(const double rv[AA_RESTRICT 3], const double rv_near[AA_RESTRICT 3], double rv_p[AA_RESTRICT 3])
Scales rv by multiple of 2pi to minimized SSD with rv_near.
void aa_tf_qutr_wavg(size_t n, const double *w, const double *EE, size_t ldee, double *a)
Weighted average transform.
AA_API void aa_tf_rotmat2quat(const double rotmat[AA_RESTRICT 9], double quat[AA_RESTRICT 4])
convert rotation matrix to quaternion
AA_API void aa_tf_tf_qv(const double quat[AA_RESTRICT 4], const double v[AA_RESTRICT 3], const double p0[AA_RESTRICT 3], double p1[AA_RESTRICT 4])
apply a euclidean transform
AA_API void aa_tf_rotmat_normalize(double R[AA_RESTRICT 9])
Orthonormalize the rotation matrix.
AA_API void aa_tf_qrk1(const double q0[AA_RESTRICT 4], const double dq[AA_RESTRICT 4], double dt, double q1[AA_RESTRICT 4])
Integrate unit quaternion, Runge-Kutta-1 (euler) integration.
AA_API void aa_tf_qexp(const double q[AA_RESTRICT 4], double r[AA_RESTRICT 4])
Quaternion exponential.
#define AA_TF_DEF_EULER(letters)
Create declarations for Euler angle conversion functions.
#define AA_TF_DUQU_IDENT_INITIALIZER
Static initializer for an identity dual quaternion.
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.
void aa_tf_tfmat2av(const double T[12], double q[AA_RESTRICT 4], double v[AA_RESTRICT 3])
transformation matrix to quaternion-translation