spatial.h File Reference

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

#include <float.h>

Go to the source code of this file.

Macros
#define	AA_TF_DEF_SERIES(name, a0, a1, a2)
	Define an inline function for a series using Horner's rule. More...
#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
	Offset of quaternion part in quaternion-translation representation.
#define	AA_TF_QUTR_V   4
	Offset of translation part in quaternion-translation representation.
#define	AA_TF_DEF_EULER(letters)
	Create declarations for Euler angle conversion functions. More...

Functions
static void	aa_tf_sinccos2 (double theta2, double *sc, double *c)
	Compute sinc(theta), cos(theta), given the square of theta. More...
static void	aa_tf_sinccos (double theta, double *sc, double *c)
	Compute sinc(theta), cos(theta)
static double	aa_tf_sinc (double theta)
	Compute sinc(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])
	Construct a skew-symmetric matrix. More...
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_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_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_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_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. More...
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. More...
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. More...
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. More...
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). More...
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). More...
AA_API void	aa_tf_rotmat_normalize (double R[AA_RESTRICT 9])
	Orthonormalize the rotation matrix.
AA_API void	aa_tf_rotmat_rot (const double R[AA_RESTRICT 9], const double p0[AA_RESTRICT 3], double p1[AA_RESTRICT 3])
	Rotate a point using a rotation matrix.
AA_API void	aa_tf_tfmat_tf (const double T[AA_RESTRICT 12], const double p0[AA_RESTRICT 3], double p1[AA_RESTRICT 3])
	Transform a point using a transformation matrix.
AA_API void	aa_tf_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_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 void	aa_tf_tfmat_normalize (double T[AA_RESTRICT 12])
	Orthonormalize the transformation matrix.
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_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_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_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_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.
AA_API double	aa_tf_vssd (const double a[AA_RESTRICT 3], const double b[AA_RESTRICT 3])
	Sum-square-differences of two vectors.
AA_API double	aa_tf_vnorm (const double a[AA_RESTRICT 3])
	Norm of vector.
static double	AA_TF_QDOT (const double a[AA_RESTRICT 4], const double b[AA_RESTRICT 4])
	Inlined quaternion dot product.
AA_API double	aa_tf_qssd (const double a[AA_RESTRICT 4], const double b[AA_RESTRICT 4])
	Sum-square-differences of two quaternions.
AA_API 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 void	aa_tf_qscal (double q[AA_RESTRICT 4], double alpha)
	Scale a 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])
	Minimize angle represented by the quaternion. More...
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_qconj1 (double q[AA_RESTRICT 4])
	Quaternion conjugate, in-place.
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])
	Derivative of the Unit Quaternion Logarithm, computed via Jacobian. More...
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])
	Derivative of the Pure Quaternion Exponential, computed via Jacobian. More...
AA_API double	aa_tf_qangle (const double q[AA_RESTRICT 4])
	Return the angle of the quaternion. More...
AA_API double	aa_tf_qangle_rel (const double *q, const double *p)
	Relative quaternion angles.
AA_API double	aa_tf_quhypangle2 (const double q[AA_RESTRICT 4], const double p[AA_RESTRICT 4])
	Return the angle between unit quaterniosn in 4D space.
AA_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.
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_qisub (double a[AA_RESTRICT 4], const double b[AA_RESTRICT 4])
	Quaternion subtraction, in-place.
AA_API void	aa_tf_qiadd (double a[AA_RESTRICT 4], const double b[AA_RESTRICT 4])
	Quaternion addition, in-place.
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_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_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_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_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_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.
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.
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. 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])
	Quaternaion time derivate to angular velocity.
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_rotvec_diff2vel (const double v[3], const double dv[3], double w[3])
	Convert rotation vector derivative to rotational velocity.
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_qsacc_rk (const double q0[AA_RESTRICT 4], const double v[AA_RESTRICT 3], const double a[AA_RESTRICT 3], double dt, double q1[AA_RESTRICT 4])
	Runge-kutta integration of unit quaternion from velocity and acceleration. More...
AA_API void	aa_tf_qsacc (const double q0[AA_RESTRICT 4], const double v[AA_RESTRICT 3], const double a[AA_RESTRICT 3], double dt, double q1[AA_RESTRICT 4])
	Integrate unit quaternion from velocity and acceleration. 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)
	Construct matrix for Davenport's q-method.
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. More...
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. More...
AA_API void	aa_tf_qmat_l (const double *q, struct aa_dmat *M)
	Construct matrix for left quaternion multiply q*p = M*p.
AA_API void	aa_tf_qmat_r (const double *q, struct aa_dmat *M)
	Construct matrix for right quaternion multiply. More...
AA_API 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.
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_qv_conj (const double q[4], const double v[3], double qc[4], double vc[3])
	quaternion-translation conjugate
AA_API void	aa_tf_qutr2duqu (const double e[7], double s[8])
	quaternion-translation to dual quaternion
AA_API void	aa_tf_qutr2tfmat (const double e[7], double T[12])
	quaternion-translation to transformation matrix
AA_API void	aa_tf_tfmat2qutr (const double T[12], double e[7])
	transformation matrix to quaternion-translation
AA_API void	aa_tf_duqu2qutr (const double s[8], double e[7])
	dual quaternion to quaternion-translation
AA_API 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,.
AA_API 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_qutr_conj (const double a[7], double c[7])
	quaternion-translation conjugate
AA_API 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_qutr_cmul (const double a[7], const double b[7], double c[7])
	quaternion-translation conjugate multiply
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_qv_lnv (const double q[4], const double v[3], double w[3], double dv[3])
	Quaternion-vector logarithm.
AA_API void	aa_tf_qutr_expv (const double w[6], double e[7])
	Quaternion-vector exponential.
AA_API void	aa_tf_qutr_lnv (const double e[7], double w[6])
	Quaternion-vector logarithm.
AA_API 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_qutr_diff2vel (const double e[7], const double de[7], double dx[6])
	Quaternion-translation derivative to spatial velocity.
AA_API void	aa_tf_qutr_vel2diff (const double e[7], const double dx[6], double de[7])
	Quaternion-translation spatial velocity to derivative.
AA_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_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_qutr_svel (const double e0[7], const double dx[6], double dt, double e1[7])
	Integrate a quaternion-translation.
AA_API void	aa_tf_qutr_sdiff (const double e0[7], const double de[7], double dt, double e1[7])
	Integrate a quaternion-translation.
AA_API 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_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.
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_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_rotmat_sincos (const double R[AA_RESTRICT 9], double *s, double *c)
	extract sin and cosine of a rotation matrix
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_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_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.
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_xangle2axang (double theta_x, double axang[AA_RESTRICT 4])
	Angle about x axis.
AA_API void	aa_tf_yangle2axang (double theta_y, double axang[AA_RESTRICT 4])
	Angle about y axis.
AA_API void	aa_tf_zangle2axang (double theta_z, double axang[AA_RESTRICT 4])
	Angle about z axis.
AA_API void	aa_tf_axang_normalize (double axang[AA_RESTRICT 4])
	Normalize the 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 (double alpha, const double x[AA_RESTRICT 8], double y[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_mat_l (const double *q, struct aa_dmat *M)
	Construct matrix for left dual quaternion multiply q*p = M*p.
AA_API void	aa_tf_duqu_mat_r (const double *q, struct aa_dmat *M)
	Construct matrix for right dual quaternion multiply p*q = M*p.
AA_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_conj1 (double s[AA_RESTRICT 8])
	Dual quaternion conjugate, in-place.
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_lnv (const double S[AA_RESTRICT 8], double w[AA_RESTRICT 6])
	Unit dual quaternion natural logarithm. More...
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_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_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.
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_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_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.
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_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...
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. More...
AA_API void	aa_tf_qln_jac (const double q[4], struct aa_dmat *J)
	Compute the Jacobian of the quaternion logarithm. More...
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_duqu_ln_jac (const double S[8], struct aa_dmat *J)
	Compute the Jacobian of the dual quaternion logarithm. More...
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_pure2duqu (const double v[AA_RESTRICT 6], double S[AA_RESTRICT 8])
	Convert a conventional dual quaternion to pure dual quaternion.
AA_API void	aa_tf_dhprox2tfmat (double alpha, double a, double d, double phi, double T[AA_RESTRICT 12])
	Convert proximal DH parameters to transformation matrix.
AA_API void	aa_tf_dhprox2duqu (double alpha, double a, double d, double phi, double S[AA_RESTRICT 8])
	Convert proximal DH parameters to dual quaternion.
AA_API void	aa_tf_dhprox2qutr (double alpha, double a, double d, double phi, double E[AA_RESTRICT 7])
	Convert proximal DH parameters to quaternion-translation.
AA_API void	aa_tf_dhprox2qv (double alpha, double a, double d, double phi, double q[AA_RESTRICT 4], double v[3])
	Convert proximal DH parameters to quaternion-translation.
AA_API void	aa_tf_dhdist2tfmat (double alpha, double a, double d, double phi, double T[AA_RESTRICT 12])
	Convert distal DH parameters to transformation matrix.
AA_API void	aa_tf_dhdist2duqu (double alpha, double a, double d, double phi, double S[AA_RESTRICT 8])
	Convert distal DH parameters to dual quaternion.
AA_API void	aa_tf_dhdist2qutr (double alpha, double a, double d, double phi, double E[AA_RESTRICT 7])
	Convert distal DH parameters to quaternion-translation.
AA_API void	aa_tf_dhdist2qv (double alpha, double a, double d, double phi, double q[AA_RESTRICT 4], double v[3])
	Convert distal DH parameters to quaternion-translation.
AA_API 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])
AA_API 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])
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. More...
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. More...

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.

Definition in file spatial.h.

Macro Definition Documentation

AA_TF_CROSSX

#define AA_TF_CROSSX	(		a,
			b,
			c
	)

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 643 of file spatial.h.

AA_TF_DEF_EULER

#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] );

Create declarations for Euler angle conversion functions.

Definition at line 1399 of file spatial.h.

AA_TF_DEF_SERIES

#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);     \
    }                                                       \

Define an inline function for a series using Horner's rule.

Parameters

name	pre-mangled name for the function
a0	zero-order coefficient
a1	first-order coefficient
a2	second-order coefficient

Definition at line 74 of file spatial.h.

AA_TF_DOTX

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

May evaluate its arguments multiple times.

Definition at line 604 of file spatial.h.

Function Documentation

aa_tf_duqu_jac_vel2diff()

AA_API void aa_tf_duqu_jac_vel2diff	(	const double	S[8],
		const struct aa_dmat *	Jvel,
		struct aa_dmat *	Js
	)

Convert a velocity Jacobian to a dual quaternion derivative Jacobian.

The velocity Jacobian Jvel relates velocities to configurations:

\[ \begin{bmatrix}\omega \\ \dot v\end{bmatrix} = \mathbf{J}_{\rm vel} \dot{\phi} \]

The derivative Jacobian Js relates the dual quaternion derivative to configurations:

\[ \dot{S} = \mathbf{J}_S \dot{\phi} \]

Parameters

S	The pose as a dual quaternion
Jvel	The velocity Jacobian
Js	The dual quaternion derivative Jacobian

aa_tf_duqu_ln_jac()

AA_API void aa_tf_duqu_ln_jac	(	const double	S[8],
		struct aa_dmat *	J
	)

Compute the Jacobian of the dual quaternion logarithm.

Parameters

[in]	S	The quaternion
[out]	J	Storage for the Jacobian, 8x8 matrix

aa_tf_duqu_lnv()

AA_API void aa_tf_duqu_lnv	(	const double	S[AA_RESTRICT 8],
		double	w[AA_RESTRICT 6]
	)

Unit dual quaternion natural logarithm.

Result is a pure dual quaterion.

aa_tf_duqu_sdiff()

AA_API void aa_tf_duqu_sdiff	(	const double	d0[AA_RESTRICT 8],
		const double	dd[AA_RESTRICT 8],
		double	dt,
		double	d1[AA_RESTRICT 6]
	)

Dual quaternion derivative integration.

Parameters

d0	initial position, dual quaternion
dd	dual quaternion derivative
dt	time step
d1	final position, dual quaternion

aa_tf_duqu_stwist()

AA_API void aa_tf_duqu_stwist	(	const double	d0[AA_RESTRICT 8],
		const double	twist[AA_RESTRICT 8],
		double	dt,
		double	d1[AA_RESTRICT 6]
	)

Dual quaternion twist integration.

Parameters

d0	initial position, dual quaternion
twist	dual quaternion twist
dt	time step
d1	final position, dual quaternion

aa_tf_duqu_svel()

AA_API void aa_tf_duqu_svel	(	const double	d0[AA_RESTRICT 8],
		const double	dx[AA_RESTRICT 6],
		double	dt,
		double	d1[AA_RESTRICT 6]
	)

Dual quaternion velocity integration.

Parameters

d0	initial position, dual quaternion
dx	spatial velocity
dt	time step
d1	final position, dual quaternion

aa_tf_proj()

AA_API void aa_tf_proj	(	const double	a[AA_RESTRICT 3],
		const double	b[AA_RESTRICT 3],
		double	c[AA_RESTRICT 3]
	)

Vector projection of a onto b.

\[ \vec{c} = \mathrm{proj}_\vec{b}(\vec{a}) = \frac{\vec{a} \cdot \vec{b}}{\vec{b} \cdot \vec{b}} \vec{b} \]

aa_tf_proj_orth()

AA_API void aa_tf_proj_orth	(	const double	a[AA_RESTRICT 3],
		const double	b[AA_RESTRICT 3],
		double	c[AA_RESTRICT 3]
	)

Orthogonal projection of a onto b.

\[ \vec{c} = \vec{a} - \mathrm{proj}_\vec{b}(\vec{a}) \]

aa_tf_qangle()

AA_API 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_tf_qdpexp()

AA_API void aa_tf_qdpexp	(	const double	e[AA_RESTRICT 3],
		const double	de[AA_RESTRICT 3],
		double	dq[AA_RESTRICT 4]
	)

Derivative of the Pure Quaternion Exponential.

Parameters

e	A pure quaternion
de	The derivative of e
dq	The derivative of exp(e)

aa_tf_qdpexpj()

AA_API void aa_tf_qdpexpj	(	const double	e[AA_RESTRICT 3],
		const double	de[AA_RESTRICT 3],
		double	dq[AA_RESTRICT 4]
	)

Derivative of the Pure Quaternion Exponential, computed via Jacobian.

Parameters

e	A pure quaternion
de	The derivative of e
dq	The derivative of exp(e)

aa_tf_qduln()

AA_API void aa_tf_qduln	(	const double	q[AA_RESTRICT 4],
		const double	dq[AA_RESTRICT 4],
		double	dln[AA_RESTRICT 3]
	)

Derivative of the Unit Quaternion Logarithm.

Parameters

q	A unit quaternion
dq	The derivative of q
dln	The derivative of ln(dq)

aa_tf_qdulnj()

AA_API void aa_tf_qdulnj	(	const double	q[AA_RESTRICT 4],
		const double	dq[AA_RESTRICT 4],
		double	dln[AA_RESTRICT 3]
	)

Derivative of the Unit Quaternion Logarithm, computed via Jacobian.

Parameters

q	A unit quaternion
dq	The derivative of q
dln	The derivative of ln(dq)

aa_tf_qln_jac()

AA_API void aa_tf_qln_jac	(	const double	q[4],
		struct aa_dmat *	J
	)

Compute the Jacobian of the quaternion logarithm.

Parameters

[in]	q	The quaternion
[out]	J	Storage for the Jacobian, 4x4 matrix

aa_tf_qmat_r()

AA_API void aa_tf_qmat_r	(	const double *	q,
		struct aa_dmat *	M
	)

Construct matrix for right quaternion multiply.

p*q = M*p

aa_tf_qmatrix_r()

AA_API void aa_tf_qmatrix_r	(	const double *	q,
		double *	M,
		size_t	ldm
	)

Construct matrix for right quaternion multiply.

p*q = M*p

aa_tf_qminimize()

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_tf_qminimize2()

AA_API void aa_tf_qminimize2	(	const double	q[AA_RESTRICT 4],
		double	qmin[AA_RESTRICT 4]
	)

Minimize angle represented by the quaternion.

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

aa_tf_qnormalize()

AA_API void aa_tf_qnormalize

(

double

q[AA_RESTRICT 4]

)

Normalize Quaternion.

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

aa_tf_qrel()

AA_API void aa_tf_qrel	(	const double	q1[AA_RESTRICT 4],
		const double	q2[AA_RESTRICT 4],
		double	q_rel[AA_RESTRICT 4]
	)

Relative orientation.

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

aa_tf_qrk1()

AA_API void aa_tf_qrk1	(	const double	q0[AA_RESTRICT 4],
		const double	dq[AA_RESTRICT 4],
		double	dt,
		double	q1[AA_RESTRICT 4]
	)

Integrate unit quaternion, Runge-Kutta-1 (euler) integration.

Parameters

q0	Initial rotation quaternion
dq	Quaternion derivative
dt	Time step
q1	Final rotation quaternion

aa_tf_qsacc()

AA_API void aa_tf_qsacc	(	const double	q0[AA_RESTRICT 4],
		const double	v[AA_RESTRICT 3],
		const double	a[AA_RESTRICT 3],
		double	dt,
		double	q1[AA_RESTRICT 4]
	)

Integrate unit quaternion from velocity and acceleration.

Parameters

q0	Initial rotation quaternion
v	angular velocity
a	angular acceleration
dt	Time step
q1	Final rotation quaternion

aa_tf_qsacc_rk()

AA_API void aa_tf_qsacc_rk	(	const double	q0[AA_RESTRICT 4],
		const double	v[AA_RESTRICT 3],
		const double	a[AA_RESTRICT 3],
		double	dt,
		double	q1[AA_RESTRICT 4]
	)

Runge-kutta integration of unit quaternion from velocity and acceleration.

This is a test function.

Parameters

q0	Initial rotation quaternion
v	angular velocity
a	angular acceleration
dt	Time step
q1	Final rotation quaternion

See also

aa_tf_qsacc

aa_tf_qscal()

AA_API void aa_tf_qscal	(	double	q[AA_RESTRICT 4],
		double	alpha
	)

Scale a quaternion.

\[ \bf{q} \leftarrow \alpha {\bf q} \]

aa_tf_qsdiff()

AA_API void aa_tf_qsdiff	(	const double	q0[AA_RESTRICT 4],
		const double	dq[AA_RESTRICT 4],
		double	dt,
		double	q1[AA_RESTRICT 4]
	)

Integrate unit quaternion from quaternion derivative.

Parameters

q0	Initial rotation quaternion
dq	Quaternion derivative
dt	Time step
q1	Final rotation quaternion

aa_tf_qslerpdiff()

AA_API void aa_tf_qslerpdiff	(	double	tau,
		const double	a[AA_RESTRICT 4],
		const double	b[AA_RESTRICT 4],
		double	c[AA_RESTRICT 4]
	)

Derivative of quaternation SLERP WRT tau.

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

aa_tf_qsvel()

AA_API void aa_tf_qsvel	(	const double	q0[AA_RESTRICT 4],
		const double	v[AA_RESTRICT 3],
		double	dt,
		double	q1[AA_RESTRICT 4]
	)

Integrate unit quaternion from constant angular velocity.

Parameters

q0	Initial rotation quaternion
v	Rotational velocity
dt	Time step
q1	Final rotation quaternion

aa_tf_quat_davenport()

AA_API void aa_tf_quat_davenport	(	size_t	n,
		const double *	w,
		const double *	Q,
		size_t	ldq,
		double *	y
	)

Weighted average quaternion using Davenport's q-method.

Parameters

n	number of quaternions
w	weights
Q	array of quaternions
ldq	leading dimension of Q
y	average quaternion

aa_tf_qv_conj()

AA_API void aa_tf_qv_conj	(	const double	q[AA_RESTRICT 4],
		const double	v[AA_RESTRICT 3],
		double	qc[AA_RESTRICT 4],
		double	vc[AA_RESTRICT 3]
	)

Invert transform.

Unit quaternion inverse is equal to the conjuage

aa_tf_qvelrk1()

AA_API void aa_tf_qvelrk1	(	const double	q0[AA_RESTRICT 4],
		const double	v[AA_RESTRICT 3],
		double	dt,
		double	q1[AA_RESTRICT 4]
	)

Integrate unit quaternion from angular velocity, Runge-Kutta-1 (euler) integration.

Parameters

q0	Initial rotation quaternion
v	Rotational velocity
dt	Time step
q1	Final rotation quaternion

aa_tf_qvelrk4()

AA_API void aa_tf_qvelrk4	(	const double	q0[AA_RESTRICT 4],
		const double	v[AA_RESTRICT 3],
		double	dt,
		double	q1[AA_RESTRICT 4]
	)

Integrate unit quaternion from angular velocity, Runge-Kutta-4 (euler) integration.

Parameters

q0	Initial rotation quaternion
v	Rotational velocity
dt	Time step
q1	Final rotation quaternion

aa_tf_rotmat_imul()

AA_API void aa_tf_rotmat_imul	(	const double	R0[AA_RESTRICT 9],
		const double	R1[AA_RESTRICT 9],
		double	R[AA_RESTRICT 9]
	)

Multiple inverse of R0 by R1.

\[ R = ({R_0}^{-1}) * (R_1) \]

aa_tf_rotmat_muli()

AA_API void aa_tf_rotmat_muli	(	const double	R0[AA_RESTRICT 9],
		const double	R1[AA_RESTRICT 9],
		double	R[AA_RESTRICT 9]
	)

Multiple R1 by inverse of R1.

\[ R = ({R_0}) * ({R_1}^{-1}) \]

aa_tf_sinccos2()

static void aa_tf_sinccos2 ( double  theta2, double *  sc, double *  c  )

inlinestatic

Compute sinc(theta), cos(theta), given the square of theta.

Parameters

theta2	theta*theta
sc	output for sinc(theta)
c	output for cos(theta)

Definition at line 98 of file spatial.h.

aa_tf_skewsym_scal2()

AA_API void aa_tf_skewsym_scal2	(	double	a,
		double	b,
		const double	u[3],
		double	R[9]
	)

Construct a skew-symmetric matrix.

\[ I + a*[u] + b*[u]^2 \]

where

\[ [u] = \begin{bmatrix} 0 & -u_2 & u_1 \\ u_2 & 0 & -u_0 \\ -u_1 & u_0 & 0 \end{bmatrix} \]

aa_tf_tfmat2_imul()

AA_API void aa_tf_tfmat2_imul	(	const double	R0[AA_RESTRICT 9],
		const double	v0[AA_RESTRICT 3],
		const double	R1[AA_RESTRICT 9],
		const double	v1[AA_RESTRICT 3],
		double	R[AA_RESTRICT 9],
		double	v[AA_RESTRICT 3]
	)

Multiple inverse of transform (R0,v0) by transform (R1,v1).

\[ (R,v) = \left(\left(R_0,v_0\right)^{-1}\right) * ({R_1},v_1) \]

aa_tf_tfmat2_muli()

AA_API void aa_tf_tfmat2_muli	(	const double	R0[AA_RESTRICT 9],
		const double	v0[AA_RESTRICT 3],
		const double	R1[AA_RESTRICT 9],
		const double	v1[AA_RESTRICT 3],
		double	R[AA_RESTRICT 9],
		double	v[AA_RESTRICT 3]
	)

Multiple (R0,v0) by inverse of transform transform (R1,v1).

\[ (R,v) = ({R_0},v_0) \left(\left(R_1,v_1\right)^{-1}\right) \]

aa_tf_tfmat_imul()

AA_API void aa_tf_tfmat_imul	(	const double	T0[AA_RESTRICT 12],
		const double	T1[AA_RESTRICT 12],
		double	T[AA_RESTRICT 12]
	)

Multiple inverse of T0 by T1.

\[ T = ({T_0}^{-1}) * (T_1) \]

aa_tf_tfmat_muli()

AA_API void aa_tf_tfmat_muli	(	const double	T0[AA_RESTRICT 12],
		const double	T1[AA_RESTRICT 12],
		double	T[AA_RESTRICT 12]
	)

Multiple T1 by inverse of T1.

\[ T = ({T_0}) * ({T_1}^{-1}) \]