/* structure representing 3D rotation */
typedef struct Gan_Rot3D
{
Gan_Rot3D_Type type; /* representation used */
union
{
Gan_Quaternion q; /* quaternion form */
Gan_Vector3 r; /* exponential form */
struct { Gan_Vector3 axis; double angle; } aa; /* angle/axis form */
Gan_Matrix33 R; /* matrix form */
} d;
} Gan_Rot3D;
This structure can then be used to implement unary and binary operations
involving rotations. Firstly there is a set of functions to build a rotation
structure. For quaternions the function is
Gan_Rot3D Rot; /* declare rotation structure */
double q0, q1, q2, q3; /* declare quaternion elements */
/* ... set q0, q1, q2 and q3 to quaternion coordinates, and scale to
unit length if desired ... */
/* build rotation structure from quaternion */
gan_rot3D_build_quaternion ( &Rot, q0, q1, q2, q3 );
For the exponential representation use
Gan_Rot3D Rot; /* declare rotation structure */
double rx, ry, rz; /* declare exponential rotation vector elements */
/* ... set rx, ry & rz ... */
/* build rotation structure from exponential rotation vector */
gan_rot3D_build_exponential ( &Rot, rx, ry, rz );
For the angle/axis representation use
Gan_Rot3D Rot; /* declare rotation structure */
double angle, ax, ay, az; /* declare angle and axis elements */
/* ... set angle, ax, ay & az ... */
/* build rotation structure from rotation angle and axis */
gan_rot3D_build_angle_axis ( &Rot, angle, ax, ay, az );
Finally for the matrix representation we have
Gan_Rot3D Rot; /* declare rotation structure */
double Rxx, Rxy, Rxz, Ryx, Ryy, Ryz, Rzx, Rzy, Rzz; /* declare matrix elements */
/* ... set matrix elements Rxx, Rxy etc. ... */
/* build rotation structure from rotation angle and axis */
gan_rot3D_build_matrix ( &Rot, Rxx, Rxy, Rxz,
Ryx, Ryy, Ryz,
Rzx, Rzy, Rzz );
Another way to build a rotation structure is from a fixed-size Gandalf vector or matrix. These routines give the added flexibility of allowing conversion to another rotation representation. So for instance
Gan_Rot3D Rot; /* declare rotation structure */
Gan_Quaternion q; /* declare quaternion */
/* ... fill quaternion q using e.g. gan_quat_fill_q() ... */
/* fill rotation structure with rotation matrix equivalent to
quaternion q */
gan_rot3D_from_quaternion_q ( &Rot, &q, GAN_ROT3D_MATRIX ); /* OR */
Rot = gan_rot3D_from_quaternion_s ( &q, GAN_ROT3D_MATRIX );
builds the rotation structure Rot containing the rotation matrix
equivalent to the quaternion q. This routine and others in the same
family do rescale and adjust as necessary to an exact rotation.
The other routines are:
Gan_Rot3D Rot; /* declare rotation structure */
Gan_Vector3 r; /* declare exponential rotation vector */
double angle; Gan_Vector3 axis; /* declare angle an axis */
Gan_Matrix33 R; /* declare rotation matrix */
/* ... fill vector r using e.g. gan_vec3_fill_q() ... */
/* fill rotation structure with quaternion equivalent to
exponential rotation vector r */
gan_rot3D_from_exponential_q ( &Rot, &r, GAN_ROT3D_QUATERNION ); /* OR */
Rot = gan_rot3D_from_exponential_s ( &r, GAN_ROT3D_QUATERNION );
/* ... fill angle and axis ... */
/* fill rotation structure with rotation matrix equivalent to
angle and axis */
gan_rot3D_from_angle_axis_q ( &Rot, angle, &axis, GAN_ROT3D_QUATERNION ); /* OR */
Rot = gan_rot3D_from_angle_axis_s ( angle, &axis, GAN_ROT3D_QUATERNION );
/* ... fill rotation matrix R ... */
/* fill rotation structure with angle and axis equivalent to
rotation matrix */
gan_rot3D_from_matrix_q ( &Rot, &R, GAN_ROT3D_QUATERNION ); /* OR */
Rot = gan_rot3D_from_matrix_s ( &R, GAN_ROT3D_QUATERNION );
Next are a pair of routines to set a rotation to zero:
Gan_Rot3D Rot; /* declare rotation structure */
/* set a rotation structure to be a quaternion representation and
set it to a zero rotation */
gan_rot3D_ident_q ( &Rot, GAN_ROT3D_QUATERNION ); /* OR */
Rot = gan_rot3D_ident_s ( GAN_ROT3D_QUATERNION );
To apply a rotation to a 3D point use one of the routines
Gan_Vector3 v3X, V3Xp; /* declare 3D points X & Xp */
/* ... fill 3D point X with values ... */
/* apply rotation such as Xp = R*X */
gan_rot3D_apply_v3_q ( &Rot, &v3X, &v3Xp ); /* OR */
v3Xp = gan_rot3D_apply_v3_s ( &Rot, &v3X );
To combine two rotations use
Gan_Rot3D Rot1, Rot2, Rot3; /* declare rotations R1, R2 & R3 */
/* ... fill R1 and R2 with rotation parameters of the same type ... */
/* combine two rotations into a third: for matrices R3 = R1*R2 */
gan_rot3D_mult_q ( &Rot1, &Rot2, &Rot3 ); /* OR */
Rot3 = gan_rot3D_mult_s ( &Rot1, &Rot2 );
The second rotation structure may also be implicitly inverted, yielding
/* combine two rotations into a third: for matrices R3 = R1*R2^-1 */
gan_rot3D_multI_q ( &Rot1, &Rot2, &Rot3 ); /* OR */
Rot3 = gan_rot3D_multI_s ( &Rot1, &Rot2 );
There is also a set of arithmetical routines. For binary arithmetical operations, both structures must have the same representation, and the operation is a pure parameter addition/subtraction etc., without rescaling or otherwise adjusting the rotation parameters to conform to an actual rotation. This is often required when implementing optimisation involving rotation parameters, for instance computing derivatives numerically. Firstly there are routines for multiplying or dividing rotation parameters by a scalar:
Gan_Rot3D Rot1, Rot2; /* declare rotations R1 & R2 */
/* ... fill R1 with rotation parameters ... */
/* multiply the rotation parameters R1 by 3, writing them into R2 */
gan_rot3D_scale_q ( &Rot1, 3.0, &Rot2 ); /* R2 = 3*R1, OR */
R2 = gan_rot3D_scale_s ( &Rot1, 3.0 ); /* R2 = 3*R1, OR */
gan_rot3D_scale_i ( &Rot1, 3.0 ); /* replace R1 = 3*R1 */
/* divide the rotation parameters R1 by 3, writing them into R2 */
gan_rot3D_divide_q ( &Rot1, 3.0, &Rot2 ); /* R2 = R1/3, OR */
R2 = gan_rot3D_divide_s ( &Rot1, 3.0 ); /* R2 = R1/3, OR */
gan_rot3D_divide_i ( &Rot1, 3.0 ); /* replace R1 = R1/3 */
Next a set of routines each for adding and subtracting rotation parameters:
Gan_Rot3D Rot1, Rot2, Rot3; /* declare rotations R1, R2 & R3 */
/* ... fill R1 and R2 with rotation parameters of the same type ... */
/* add the rotation parameters R1 and R2 */
gan_rot3D_add_q ( &Rot1, &Rot2, &Rot3 ); /* R3 = R1 + R2, OR */
Rot3 = gan_rot3D_add_s ( &Rot1, &Rot2 ); /* R3 = R1 + R2, OR */
gan_rot3D_increment ( &Rot1, &Rot2 ); /* replace R1 = R1 + R2 in-place, OR */
gan_rot3D_add_i1 ( &Rot1, &Rot2 ); /* replace R1 = R1 + R2 in-place, OR */
gan_rot3D_add_i2 ( &Rot1, &Rot2 ); /* replace R2 = R1 + R2 in-place */
/* subtract the rotation parameters R1 and R2 */
gan_rot3D_sub_q ( &Rot1, &Rot2, &Rot3 ); /* R3 = R1 - R2, OR */
Rot3 = gan_rot3D_sub_s ( &Rot1, &Rot2 ); /* R3 = R1 - R2, OR */
gan_rot3D_decrement ( &Rot1, &Rot2 ); /* replace R1 = R1 - R2 in-place, OR */
gan_rot3D_sub_i1 ( &Rot1, &Rot2 ); /* replace R1 = R1 - R2 in-place, OR */
gan_rot3D_sub_i2 ( &Rot1, &Rot2 ); /* replace R2 = R1 - R2 in-place */
There are a couple of routines to convert a rotation structure from one representation to another:
Gan_Rot3D Rot1, Rot2; /* declare rotations R1 & R2 */
/* ... fill R1 with rotation parameters ... */
/* convert rotation R1 to matrix representation in R2 */
gan_rot3D_convert_q ( &Rot1, GAN_ROT3D_MATRIX, &Rot2 ); /* OR */
Rot2 = gan_rot3D_convert_s ( &Rot1, GAN_ROT3D_MATRIX );
Finally a utility routine to correct a 
matrix to the ``nearest''
orthogonal matrix, using SVD:
Gan_Matrix33 m33R; /* declare matrix R */
/* ... set up R as "nearly" a rotation matrix */
/* adjust matrix R to be exactly a rotation matrix */
gan_rot3D_matrix_adjust ( &m33R );
For statistical optimisation purposes there is a structure designed to hold covariance information for rotation parameters, currently supporting only quaternion and exponential representations, they being the most likely representations to use for optimising rotation parameters:
/* structure representing covariance of 3D rotation */
typedef struct Gan_Rot3D_Cov
{
Gan_Rot3D_Type type;
union
{
Gan_SquMatrix44 q; /* covariance of quaternion */
Gan_SquMatrix33 r; /* covariance of exponential rotation vector */
} data;
} Gan_Rot3D_Cov;
Error detection: The gan_rot3D_build_...() and all the
gan_rot3D_..._[qi](), gan_rot3D_..._i1(),
gan_rot3D_..._i2(), gan_rot3D_..._increment() and
gan_rot3D_..._decrement() routines return a boolean value,
and return GAN_FALSE on error, invoking the Gandalf error handler.
The main error modes are difference of the representations between two
rotation structures for the arithmetic and combination routines,
and illegal parameter values.