amino
Lightweight Robot Utility Library
 All Classes Namespaces Files Functions Variables Typedefs Enumerations Enumerator Macros Pages
tf.h File Reference

Low-level operations for SE(3) orientations and transformations. More...

#include <float.h>

Go to the source code of this file.

Classes

struct  aa_tf_vec3
 Memory layout for a vector of length 3. More...
 
struct  aa_tf_rotmat
 Memory layout for a rotation matrix. More...
 
struct  aa_tf_axang
 Axis-Angle rotation. More...
 
struct  aa_tf_quat
 Memory layout for a quaternion, x,y,z,w order. More...
 
struct  aa_tf_eulerzyx
 Memory layout for an Euler Angle in ZYX (yaw-pitch-roll) format. More...
 
struct  aa_tf_tfmat
 Memory layout for a transformation matrix. More...
 
struct  aa_tf_qv
 Memory layout for a Transformation as rotation quaternion and translation vector. More...
 
struct  aa_tf_duqu
 Memory layout for a dual quaternion . More...
 
struct  aa_tf_dx
 Memory layout for an SE(3) velocity. More...
 
struct  aa_tf_qv_dx
 Transform and spatial velocity. More...
 

Macros

#define AA_TF_DEF_SERIES(name, a0, a1, a2)
 
#define AA_TF_X   0
 Index of vector x element.
 
#define AA_TF_Y   1
 Index of vector y element.
 
#define AA_TF_Z   2
 Index of vector z element.
 
#define AA_TF_QUAT_V   0
 Index of quaternion vector part.
 
#define AA_TF_QUAT_XYZ   AA_TF_QUAT_V
 Index of quaternion vector part.
 
#define AA_TF_QUAT_X   (AA_TF_QUAT_V + AA_TF_X)
 Index of quaternion vector x.
 
#define AA_TF_QUAT_Y   (AA_TF_QUAT_V + AA_TF_Y)
 Index of quaternion vector y.
 
#define AA_TF_QUAT_Z   (AA_TF_QUAT_V + AA_TF_Z)
 Index of quaternion vector z.
 
#define AA_TF_QUAT_W   3
 Index of quaternion scalar part.
 
#define AA_TF_TFMAT_R   0
 Index of rotation matrix part of transformation matrix.
 
#define AA_TF_TFMAT_V   9
 Index of origin part of transformation matrix.
 
#define AA_TF_TFMAT_X   9
 Index of origin x component of transformation matrix.
 
#define AA_TF_TFMAT_Y   10
 Index of origin y component of transformation matrix.
 
#define AA_TF_TFMAT_Z   11
 Index of origin z component of transformation matrix.
 
#define AA_TF_DX_V   0
 Index of spatial velocity translational part.
 
#define AA_TF_DX_W   3
 Index of spatial velocity translational part.
 
#define AA_TF_QUTR_Q   0
 Index of quaternion-translation quaternion part.
 
#define AA_TF_QUTR_QX   (AA_TF_QUTR_Q + AA_TF_QUAT_X)
 Index of quaternion-translation quaternion x.
 
#define AA_TF_QUTR_QY   (AA_TF_QUTR_Q + AA_TF_QUAT_Y)
 Index of quaternion-translation quaternion y.
 
#define AA_TF_QUTR_QZ   (AA_TF_QUTR_Q + AA_TF_QUAT_Z)
 Index of quaternion-translation quaternion z.
 
#define AA_TF_QUTR_QW   (AA_TF_QUTR_Q + AA_TF_QUAT_W)
 Index of quaternion-translation quaternion w.
 
#define AA_TF_QUTR_T   4
 Index of quaternion-translation translation part.
 
#define AA_TF_QUTR_TX   (AA_TF_QUTR_T + AA_TF_X)
 Index of quaternion-translation translation x.
 
#define AA_TF_QUTR_TY   (AA_TF_QUTR_T + AA_TF_Y)
 Index of quaternion-translation translation y.
 
#define AA_TF_QUTR_TZ   (AA_TF_QUTR_T + AA_TF_Z)
 Index of quaternion-translation translation z.
 
#define AA_TF_DUQU_REAL   0
 index of dual quaternion real part
 
#define AA_TF_DUQU_DUAL   4
 index of dual quaternion dual part
 
#define AA_TF_DUQU_REAL_W   (AA_TF_DUQU_REAL + AA_TF_QUAT_W)
 index of dual quaternion real w
 
#define AA_TF_DUQU_REAL_XYZ   (AA_TF_DUQU_REAL + AA_TF_QUAT_XYZ)
 index of dual quaternion real xyz
 
#define AA_TF_DUQU_REAL_X   (AA_TF_DUQU_REAL + AA_TF_QUAT_X)
 index of dual quaternion real x
 
#define AA_TF_DUQU_REAL_Y   (AA_TF_DUQU_REAL + AA_TF_QUAT_Y)
 index of dual quaternion real y
 
#define AA_TF_DUQU_REAL_Z   (AA_TF_DUQU_REAL + AA_TF_QUAT_Z)
 index of dual quaternion real z
 
#define AA_TF_DUQU_DUAL_W   (AA_TF_DUQU_DUAL + AA_TF_QUAT_W)
 index of dual quaternion dual w
 
#define AA_TF_DUQU_DUAL_XYZ   (AA_TF_DUQU_DUAL + AA_TF_QUAT_XYZ)
 index of dual quaternion dual xyz
 
#define AA_TF_DUQU_DUAL_X   (AA_TF_DUQU_DUAL + AA_TF_QUAT_X)
 index of dual quaternion dual x
 
#define AA_TF_DUQU_DUAL_Y   (AA_TF_DUQU_DUAL + AA_TF_QUAT_Y)
 index of dual quaternion dual y
 
#define AA_TF_DUQU_DUAL_Z   (AA_TF_DUQU_DUAL + AA_TF_QUAT_Z)
 index of dual quaternion dual z
 
#define AA_TF_EPSILON   .0001
 a small number
 
#define AA_TF_IDENT_INITIALIZER   {1,0,0, 0,1,0, 0,0,1, 0,0,0}
 Static initializer for an identity transformation matrix.
 
#define AA_TF_ROTMAT_IDENT_INITIALIZER   {1,0,0, 0,1,0, 0,0,1}
 Static initializer for an identity rotation matrix.
 
#define AA_TF_QUAT_IDENT_INITIALIZER   {0,0,0,1}
 Static initializer for an identity quaternion.
 
#define AA_TF_DUQU_IDENT_INITIALIZER   {0,0,0,1, 0,0,0,0}
 Static initializer for an identity dual quaternion.
 
#define AA_TF_QUTR_IDENT_INITIALIZER   {0,0,0,1, 0,0,0}
 Static initializer for an identity quaternion-translation.
 
#define AA_TF_AXANG_IDENT_INITIALIZER   {1,0,0,0}
 Static initializer for an identity axis-angle.
 
#define AA_TF_ROTVEC_IDENT_INITIALIZER   {0,0,0}
 Static initializer for an identity rotation-vector.
 
#define AA_TF_VEC_IDENT_INITIALIZER   {0,0,0}
 Static initializer for an identity vector-3.
 
#define AA_TF_IDENT   ( (double[12]) AA_TF_IDENT_INITIALIZER )
 Identity transform.
 
#define AA_TF_ROTMAT_IDENT   ( (double[9] AA_TF_ROTMAT_IDENT_INITIALIZER )
 Identity rotation matrix.
 
#define AA_TF_QUAT_IDENT   ( (double[4]) AA_TF_QUAT_IDENT_INITIALIZER )
 Identity quaternion.
 
#define AA_TF_AXANG_IDENT   ( (double[4]) AA_TF_AXANG_IDENT_INITIALIZER )
 Identity axis-angle.
 
#define AA_TF_ROTVEC_IDENT   ( (double[3]) AA_TF_ROTVEC_IDENT_INITIALIZER )
 Identity Rotation Vector.
 
#define AA_TF_DOTX(a, b)   ((a)[0]*(b)[0] + (a)[1]*(b)[1] + (a)[2]*(b)[2])
 Macro to compute a dot product of length 3 vectors. More...
 
#define AA_TF_CROSSX(a, b, c)
 Macro to compute the cross product of length 3 vectors. More...
 
#define AA_TF_QUTR_Q   0
 Index of quaternion-translation quaternion part.
 
#define AA_TF_QUTR_V   4
 
#define AA_TF_DEF_EULER(letters)
 

Typedefs

typedef struct aa_tf_vec3 aa_tf_vec3_t
 Memory layout for a vector of length 3.
 
typedef struct aa_tf_rotmat aa_tf_rotmat_t
 Memory layout for a rotation matrix. More...
 
typedef struct aa_tf_axang aa_tf_axang_t
 Axis-Angle rotation.
 
typedef struct aa_tf_quat aa_tf_quat_t
 Memory layout for a quaternion, x,y,z,w order.
 
typedef struct aa_tf_eulerzyx aa_tf_eulerzyx_t
 Memory layout for an Euler Angle in ZYX (yaw-pitch-roll) format.
 
typedef struct aa_tf_tfmat aa_tf_tfmat_t
 Memory layout for a transformation matrix. More...
 
typedef struct aa_tf_qv aa_tf_qv_t
 Memory layout for a Transformation as rotation quaternion and translation vector.
 
typedef struct aa_tf_duqu aa_tf_duqu_t
 Memory layout for a dual quaternion .
 

Functions

static void aa_tf_sinccos2 (double theta2, double *sc, double *c)
 
static void aa_tf_sinccos (double theta, double *sc, double *c)
 
static double aa_tf_sinc (double theta)
 
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_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_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_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_12inv (const double T[AA_RESTRICT 12], double Ti[AA_RESTRICT 12])
 invert transform
 
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_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. More...
 
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_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_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_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_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_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_skewsym_scal2 (double a, double b, const double u[3], double R[9])
 
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])
 
AA_API void aa_tf_rotmat_mul (const double R0[AA_RESTRICT 9], const double R1[AA_RESTRICT 9], double R[AA_RESTRICT 9])
 
AA_API void aa_tf_tfmat_mul (const double T0[AA_RESTRICT 12], const double T1[AA_RESTRICT 12], double T[AA_RESTRICT 12])
 
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])
 
AA_API void aa_tf_rotmat_imul (const double R0[AA_RESTRICT 9], const double R1[AA_RESTRICT 9], double R[AA_RESTRICT 9])
 
AA_API void aa_tf_rotmat_muli (const double R0[AA_RESTRICT 9], const double R1[AA_RESTRICT 9], double R[AA_RESTRICT 9])
 
AA_API void aa_tf_tfmat_imul (const double T0[AA_RESTRICT 12], const double T1[AA_RESTRICT 12], double T[AA_RESTRICT 12])
 
AA_API void aa_tf_tfmat_muli (const double T0[AA_RESTRICT 12], const double T1[AA_RESTRICT 12], double T[AA_RESTRICT 12])
 
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])
 
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])
 
AA_API void aa_tf_rotmat_rot (const double R[AA_RESTRICT 9], const double p0[AA_RESTRICT 3], double p1[AA_RESTRICT 3])
 
AA_API void aa_tf_tfmat_tf (const double T[AA_RESTRICT 12], const double p0[AA_RESTRICT 3], double p1[AA_RESTRICT 3])
 
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])
 
AA_API void aa_tf_rotmat_inv1 (double R[AA_RESTRICT 9])
 Invert a rotation in place.
 
AA_API void aa_tf_rotmat_inv2 (const double R[AA_RESTRICT 9], double Ri[AA_RESTRICT 9])
 Invert a rotation.
 
AA_API void aa_tf_tfmat_inv1 (double T[AA_RESTRICT 12])
 Invert a transform in place.
 
AA_API void aa_tf_tfmat_inv2 (const double T[AA_RESTRICT 12], double Ti[AA_RESTRICT 12])
 Invert a transform.
 
AA_API int aa_tf_isrotmat (const double R[AA_RESTRICT 9])
 tests if R is a rotation matrix
 
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_9rot (const double R[AA_RESTRICT 9], const double p0[AA_RESTRICT 3], double p1[AA_RESTRICT 3])
 rotate p0 by R
 
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_exp_aa (const double aa[AA_RESTRICT 4], double R[AA_RESTRICT 9])
 Rotation Matrix exponential from axis angle.
 
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_lnv (const double R[AA_RESTRICT 9], double v[AA_RESTRICT 3])
 Rotation Matrix logarithm.
 
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_tfmat_lnv (const double T[AA_RESTRICT 12], double v[AA_RESTRICT 6])
 Transformation Matrix logarithm.
 
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_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_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_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_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_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_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_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.
 
static double AA_TF_VDOT (const double a[AA_RESTRICT 3], const double b[AA_RESTRICT 3])
 Inlined vector dot product.
 
static float AA_TF_VDOTF (const float a[AA_RESTRICT 3], const float b[AA_RESTRICT 3])
 Inlined vector dot product.
 
AA_API double aa_tf_vdot (const double a[AA_RESTRICT 3], const double b[AA_RESTRICT 3])
 Vector dot product.
 
AA_API float aa_tf_vdotf (const float a[AA_RESTRICT 3], const float b[AA_RESTRICT 3])
 Vector dot product.
 
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.
 
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_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_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_vnormalize (double v[AA_RESTRICT 3])
 Normalize Vector.
 
AA_API void aa_tf_vnormalizef (float v[AA_RESTRICT 3])
 Normalize Vector.
 
static double AA_TF_QDOT (const double a[AA_RESTRICT 4], const double b[AA_RESTRICT 4])
 Inlined quaternion dot product.
 
double aa_tf_qdot (const double a[AA_RESTRICT 4], const double b[AA_RESTRICT 4])
 Quaternion dot product.
 
AA_API void aa_tf_qnormalize (double q[AA_RESTRICT 4])
 Normalize Quaternion. More...
 
AA_API double aa_tf_qnorm (const double q[AA_RESTRICT 4])
 Return norm of the quaternion.
 
AA_API void aa_tf_qminimize (double q[AA_RESTRICT 4])
 Minimize angle represented by the quaternion. More...
 
AA_API void aa_tf_qminimize2 (const double q[AA_RESTRICT 4], double qmin[AA_RESTRICT 4])
 
AA_API void aa_tf_qnormalize2 (const double q[AA_RESTRICT 4], double qnorm[AA_RESTRICT 4])
 Normailize quaternion.
 
AA_API void aa_tf_qconj (const double q[AA_RESTRICT 4], double r[AA_RESTRICT 4])
 Quaternion conjugate.
 
AA_API void aa_tf_qexp (const double q[AA_RESTRICT 4], double r[AA_RESTRICT 4])
 Quaternion exponential.
 
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_qduln (const double q[AA_RESTRICT 4], const double dq[AA_RESTRICT 4], double dln[AA_RESTRICT 3])
 Derivative of the Unit Quaternion Logarithm. More...
 
AA_API void aa_tf_qdulnj (const double q[AA_RESTRICT 4], const double dq[AA_RESTRICT 4], double dln[AA_RESTRICT 3])
 
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. More...
 
AA_API void aa_tf_qdpexpj (const double e[AA_RESTRICT 3], const double de[AA_RESTRICT 3], double dq[AA_RESTRICT 4])
 
double aa_tf_qangle (const double q[AA_RESTRICT 4])
 Return the angle of the quaternion. More...
 
double aa_tf_qangle_rel (const double *q, const double *p)
 
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_qinv (const double q[AA_RESTRICT 4], double r[AA_RESTRICT 4])
 Quaternion inverse.
 
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. More...
 
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. More...
 
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. More...
 
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. More...
 
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. More...
 
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. More...
 
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. More...
 
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. More...
 
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_qrot (const double q[AA_RESTRICT 4], const double v[AA_RESTRICT 3], double p[AA_RESTRICT 3])
 Quaternion point rotation. More...
 
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. More...
 
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. More...
 
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. More...
 
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. More...
 
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_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_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_qdiff2vel (const double q[AA_RESTRICT 4], const double dq_dt[AA_RESTRICT 4], double v[AA_RESTRICT 3])
 
AA_API void aa_tf_qvel2diff (const double q[AA_RESTRICT 4], const double v[AA_RESTRICT 3], double dq_dt[AA_RESTRICT 4])
 
AA_API void aa_tf_rotvec_diff2vel (const double v[3], const double dv[3], double w[3])
 
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. More...
 
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. More...
 
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. More...
 
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. More...
 
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. More...
 
AA_API void aa_tf_xangle2quat (double theta_x, double q[AA_RESTRICT 4])
 Unit quaternion for angle about x 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_zangle2quat (double theta_z, double q[AA_RESTRICT 4])
 Unit quaternion for angle about z axis.
 
AA_API void aa_tf_quat_davenport_matrix (size_t n, const double *w, const double *q, size_t ldqq, double *M)
 
AA_API void aa_tf_quat_davenport (size_t n, const double *w, const double *Q, size_t ldq, double *y)
 
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_qmatrix_r (const double *q, double *M, size_t ldm)
 Construct matrix for right quaternion multiply p*q = M*p.
 
void aa_tf_qurand (double q[4])
 Generate random unit quaternion.
 
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_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_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_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_qv_conj (const double q[4], const double v[3], double qc[4], double vc[3])
 quaternion-translation conjugate
 
void aa_tf_qutr2duqu (const double e[7], double s[8])
 quaternion-translation to dual quaternion
 
void aa_tf_qutr2tfmat (const double e[7], double T[12])
 quaternion-translation to transformation matrix
 
void aa_tf_tfmat2qutr (const double T[12], double e[7])
 transformation matrix to quaternion-translation
 
void aa_tf_duqu2qutr (const double s[8], double e[7])
 dual quaternion to quaternion-translation
 
void aa_tf_qutr_mul (const double a[7], const double b[7], double c[7])
 quaternion-translation multiply
 
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,.
 
void aa_tf_qutr_mulnorm (const double a[7], const double b[7], double c[7])
 quaternion-translation multiply and normalize
 
void aa_tf_qutr_conj (const double a[7], double c[7])
 quaternion-translation conjugate
 
void aa_tf_qutr_mulc (const double a[7], const double b[7], double c[7])
 quaternion-translation conjugate multiply
 
void aa_tf_qutr_cmul (const double a[7], const double b[7], double c[7])
 quaternion-translation conjugate multiply
 
void aa_tf_qutr_diff2vel (const double e[7], const double de[7], double dx[6])
 Quaternion-translation derivative to spatial velocity.
 
void aa_tf_qutr_vel2diff (const double e[7], const double dx[6], double de[7])
 Quaternion-translation spatial velocity to derivative.
 
void aa_tf_qutr_svel (const double e0[7], const double dx[6], double dt, double e1[7])
 Integrate a quaternion-translation.
 
void aa_tf_qutr_sdiff (const double e0[7], const double de[7], double dt, double e1[7])
 Integrate a quaternion-translation.
 
void aa_tf_qutr_wavg (size_t n, const double *w, const double *EE, size_t ldee, double *a)
 Weighted average transform.
 
void aa_tf_qutr_rand (double E[7])
 Generate random transform.
 
AA_API void aa_tf_quat2axang (const double q[AA_RESTRICT 4], double axang[AA_RESTRICT 4])
 Quaternion to axis-angle. More...
 
AA_API void aa_tf_axang2quat (const double axang[AA_RESTRICT 4], double q[AA_RESTRICT 4])
 axis-angle to quaternion. More...
 
AA_API void aa_tf_axang2quat2 (const double axis[AA_RESTRICT 3], double angle, double q[AA_RESTRICT 4])
 axis-angle to quaternion. More...
 
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 void aa_tf_axang2rotvec (const double axang[AA_RESTRICT 4], double rotvec[AA_RESTRICT 3])
 convert axis-angle to rotation vector
 
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_rotvec2quat (const double rotvec[AA_RESTRICT 3], double q[AA_RESTRICT 4])
 covert rotation vector to quaternion
 
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_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_quat2rotmat (const double quat[AA_RESTRICT 4], double rotmat[AA_RESTRICT 9])
 convert quaternion to rotation matrix
 
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_rotmat2axang (const double R[AA_RESTRICT 9], double ra[AA_RESTRICT 4])
 convert rotation matrix to axis angle
 
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_axang2rotmat (const double ra[AA_RESTRICT 4], double R[AA_RESTRICT 9])
 convert axis angle to 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
 
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. More...
 
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_quat2eulerzyx (const double q[AA_RESTRICT 4], double e[AA_RESTRICT 3])
 Convert quaternion to ZYX Euler Angles.
 
AA_API void aa_tf_xangle2rotmat (double theta_x, double R[AA_RESTRICT 9])
 Angle about x axis.
 
AA_API void aa_tf_yangle2rotmat (double theta_y, double R[AA_RESTRICT 9])
 Angle about y axis.
 
AA_API void aa_tf_zangle2rotmat (double theta_z, double R[AA_RESTRICT 9])
 Angle about z axis.
 
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_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_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_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_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_duqu_add (const double d1[AA_RESTRICT 8], const double d2[AA_RESTRICT 8], double d3[AA_RESTRICT 8])
 Dual quaternion addition.
 
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_duqu_smul (const double d1[AA_RESTRICT 8], const double d2[AA_RESTRICT 8], double d3[AA_RESTRICT 8])
 Dual quaternion scalar multiplication.
 
AA_API void aa_tf_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_matrix_l (const double *q, double *M, size_t ldm)
 Construct matrix for left dual quaternion multiply q*p = M*p.
 
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_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_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_conj (const double d[AA_RESTRICT 8], double dconj[AA_RESTRICT 8])
 Dual quaternion conjugate.
 
AA_API void aa_tf_duqu_exp (const double d[AA_RESTRICT 8], double e[AA_RESTRICT 8])
 Dual quaternion exponential.
 
AA_API void aa_tf_duqu_ln (const double d[AA_RESTRICT 8], double e[AA_RESTRICT 8])
 Dual quaternion natural logarithm.
 
AA_API void aa_tf_duqu_norm (const double d[AA_RESTRICT 8], double *nreal, double *ndual)
 Dual quaternion norm.
 
AA_API void aa_tf_duqu_normalize (double d[AA_RESTRICT 8])
 Dual quaternion normalization.
 
AA_API void aa_tf_duqu_minimize (double d[AA_RESTRICT 8])
 Dual quaternion angle minimization.
 
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_duqu_trans (const double d[AA_RESTRICT 8], double v[AA_RESTRICT 3])
 Extract dual quaternion translation vector.
 
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_tfmat2duqu (const double T[AA_RESTRICT 12], double d[AA_RESTRICT 8])
 Convert transformation matrix to dual quaternion.
 
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. More...
 
AA_API void aa_tf_xyz2duqu (double x, double y, double z, double d[AA_RESTRICT 8])
 Pure translation dual quaternion. More...
 
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. More...
 
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. More...
 
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. More...
 
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. More...
 
void aa_tf_tfmat2av (const double T[12], double q[AA_RESTRICT 4], double v[AA_RESTRICT 3])
 transformation matrix to quaternion-translation
 
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_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_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_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_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_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_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. More...
 
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. More...
 
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. More...
 
void aa_tf_relx_mean (size_t n, const double *R, const double *X, size_t ldx, const double *Y, size_t ldy, double rel[3])
 
void aa_tf_relx_median (size_t n, const double *R, const double *X, size_t ldx, const double *Y, size_t ldy, double rel[3])
 

Variables

static const double aa_tf_ident [12] = AA_TF_IDENT_INITIALIZER
 Identity transformation matrix array.
 
static const double aa_tf_tfmat_ident [12] = AA_TF_IDENT_INITIALIZER
 Identity transformation matrix array.
 
static const double aa_tf_rotmat_ident [9] = AA_TF_ROTMAT_IDENT_INITIALIZER
 Identity rotation matrix array.
 
static const double aa_tf_quat_ident [4] = AA_TF_QUAT_IDENT_INITIALIZER
 Identity quaternion array.
 
static const double aa_tf_duqu_ident [8] = AA_TF_DUQU_IDENT_INITIALIZER
 Identity dual quaternion array.
 
static const double aa_tf_qutr_ident [7] = AA_TF_QUTR_IDENT_INITIALIZER
 Identity quaternion-translation array.
 
static const double aa_tf_axang_ident [4] = AA_TF_AXANG_IDENT_INITIALIZER
 Identity axis-angle array.
 
static const double aa_tf_rotvec_ident [3] = AA_TF_ROTVEC_IDENT_INITIALIZER
 Identity rotation vector array.
 
static const double aa_tf_vec_ident [3] = AA_TF_ROTVEC_IDENT_INITIALIZER
 Identity vector-3 array.
 
static const double aa_tf_vec_x [3] = {1, 0, 0}
 An X axis.
 
static const double aa_tf_vec_y [3] = {0, 1, 0}
 A Y axis.
 
static const double aa_tf_vec_z [3] = {0, 0, 1}
 A Z axis.
 

Detailed Description

Low-level operations for SE(3) orientations and transformations.

See also
tf.hpp provides a higher-level object-oriented interface which may ease conversion between different representations.

Definition in file tf.h.

Macro Definition Documentation

#define AA_TF_CROSSX (   a,
  b,
 
)
Value:
(c)[0] = (a)[1]*(b)[2] - (a)[2]*(b)[1]; \
(c)[1] = (a)[2]*(b)[0] - (a)[0]*(b)[2]; \
(c)[2] = (a)[0]*(b)[1] - (a)[1]*(b)[0]; \

Macro to compute the cross product of length 3 vectors.

May evaluate its arguments multiple times.

Definition at line 796 of file tf.h.

#define AA_TF_DEF_EULER (   letters)
Value:
AA_API void \
aa_tf_euler ## letters ## 2rotmat( double e1, double e2, double e3, \
double R[AA_RESTRICT 9] ); \
AA_API void \
aa_tf_euler ## letters ## 2quat( double e1, double e2, double e3, \
double q[AA_RESTRICT 4] );
#define AA_RESTRICT
Defined restrict keyword based on language flavor.
Definition: amino.h:99
#define AA_API
calling and name mangling convention for functions
Definition: amino.h:95

Definition at line 1346 of file tf.h.

#define AA_TF_DEF_SERIES (   name,
  a0,
  a1,
  a2 
)
Value:
static inline double \
aa_tf_ ## name ##_series2(double theta2) \
{ \
return aa_horner3( theta2, a0, a1, a2 ); \
} \
static inline double \
aa_tf_ ## name ## _series(double theta) \
{ \
return aa_tf_ ## name ## _series2(theta*theta); \
} \
static double aa_horner3(double x, double a0, double a1, double a2)
Evaluate three-term polynomial using horner's rule.
Definition: math.h:178

Definition at line 69 of file tf.h.

#define AA_TF_DOTX (   a,
 
)    ((a)[0]*(b)[0] + (a)[1]*(b)[1] + (a)[2]*(b)[2])

Macro to compute a dot product of length 3 vectors.

May evaluate its arguments multiple times.

Definition at line 757 of file tf.h.

Typedef Documentation

typedef struct aa_tf_rotmat aa_tf_rotmat_t

Memory layout for a rotation matrix.

column major

The matrix is in column-major order.

typedef struct aa_tf_tfmat aa_tf_tfmat_t

Memory layout for a transformation matrix.

It is a column-major matrix, but the bottom row is omitted because this row is the same for all SE(3) transformation matrices.

The first 9 elements are a column major rotation matrix. The last 3 elements are the origin vector.

Function Documentation

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_axang2quat2 ( const double  axis[AA_RESTRICT 3],
double  angle,
double  q[AA_RESTRICT 4] 
)

axis-angle to quaternion.

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_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.

Parameters
d0initial position, dual quaternion
dddual quaternion derivative
dttime step
d1final position, dual quaternion
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.

Parameters
d0initial position, dual quaternion
twistdual quaternion twist
dttime step
d1final position, dual quaternion
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.

Parameters
d0initial position, dual quaternion
dxspatial velocity
dttime step
d1final position, dual quaternion
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.

double aa_tf_qangle ( const double  q[AA_RESTRICT 4])

Return the angle of the quaternion.

Note that this result is half the 3D angle.

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_qdpexp ( const double  e[AA_RESTRICT 3],
const double  de[AA_RESTRICT 3],
double  dq[AA_RESTRICT 4] 
)

Derivative of the Pure Quaternion Exponential.

Parameters
eA pure quaternion
deThe derivative of e
dqThe derivative of exp(e)
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.

Parameters
qA unit quaternion
dqThe derivative of q
dlnThe derivative of ln(dq)
AA_API void aa_tf_qminimize ( double  q[AA_RESTRICT 4])

Minimize angle represented by the quaternion.

This puts the quaternion in the right-hand side of the complex plane. Its w value will be positive.

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_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_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_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_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_qnormalize ( double  q[AA_RESTRICT 4])

Normalize Quaternion.

\[ \bf{q} \leftarrow \frac{\bf q}{\Arrowvert {\bf q} \Arrowvert} \]

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.

\[ q_{\rm rel} = q_1 q_2^{-1} \]

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.

Parameters
q0Initial rotation quaternion
dqQuaternion derivative
dtTime step
q1Final rotation quaternion
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_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.

Parameters
q0Initial rotation quaternion
dqQuaternion derivative
dtTime step
q1Final rotation quaternion
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_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_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.

This is NOT a time derivative. Use the chain rule: (dq/dt = * (dq/dtau)*(dt/dtau) )

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_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.

Parameters
q0Initial rotation quaternion
vRotational velocity
dtTime step
q1Final rotation quaternion
AA_API void aa_tf_quat2axang ( const double  q[AA_RESTRICT 4],
double  axang[AA_RESTRICT 4] 
)

Quaternion to axis-angle.

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_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_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.

Unit quaternion inverse is equal to the conjuage

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.

Parameters
q0Initial rotation quaternion
vRotational velocity
dtTime step
q1Final rotation quaternion
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.

Parameters
q0Initial rotation quaternion
vRotational velocity
dtTime step
q1Final rotation quaternion
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_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_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_zxyz2duqu ( double  theta,
double  x,
double  y,
double  z,
double  d[AA_RESTRICT 8] 
)

Convert z angle and translation to dual quaternion.