/* * MODULE: urotate.c * * FUNCTION: * This module contains three different routines that compute rotation * matricies and return these to user. * Detailed description is provided below. * * HISTORY: * Developed & written, Linas Vepstas, Septmeber 1991 * double precision port, March 1993 * * DETAILED DESCRIPTION: * This module contains three routines: * -------------------------------------------------------------------- * * void urot_about_axis (float m[4][4], --- returned * float angle, --- input * float axis[3]) --- input * Computes a rotation matrix. * The rotation is around the the direction specified by the argument * argument axis[3]. User may specify vector which is not of unit * length. The angle of rotation is specified in degrees, and is in the * right-handed direction. * * void rot_about_axis (float angle, --- input * float axis[3]) --- input * Same as above routine, except that the matrix is multiplied into the * GL matrix stack. * * -------------------------------------------------------------------- * * void urot_axis (float m[4][4], --- returned * float omega, --- input * float axis[3]) --- input * Same as urot_about_axis(), but angle specified in radians. * It is assumed that the argument axis[3] is a vector of unit length. * If it is not of unit length, the returned matrix will not be correct. * * void rot_axis (float omega, --- input * float axis[3]) --- input * Same as above routine, except that the matrix is multiplied into the * GL matrix stack. * * -------------------------------------------------------------------- * * void urot_omega (float m[4][4], --- returned * float omega[3]) --- input * same as urot_axis(), but the angle is taken as the length of the * vector omega[3] * * void rot_omega (float omega[3]) --- input * Same as above routine, except that the matrix is multiplied into the * GL matrix stack. * * -------------------------------------------------------------------- */ #include #include "gutil.h" /* Some files do not define M_PI... */ #ifndef M_PI #define M_PI 3.14159265358979323846 #endif /* ========================================================== */ #ifdef __GUTIL_DOUBLE void urot_axis_d (double m[4][4], /* returned */ double omega, /* input */ double axis[3]) /* input */ #else void urot_axis_f (float m[4][4], /* returned */ float omega, /* input */ float axis[3]) /* input */ #endif { double s, c, ssq, csq, cts; double tmp; /* * The formula coded up below can be derived by using the * homomorphism between SU(2) and O(3), namely, that the * 3x3 rotation matrix R is given by * t.R.v = S(-1) t.v S * where * t are the Pauli matrices (similar to Quaternions, easier to use) * v is an arbitrary 3-vector * and S is a 2x2 hermitian matrix: * S = exp ( i omega t.axis / 2 ) * * (Also, remember that computer graphics uses the transpose of R). * * The Pauli matrices are: * * tx = (0 1) ty = (0 -i) tz = (1 0) * (1 0) (i 0) (0 -1) * * Note that no error checking is done -- if the axis vector is not * of unit length, you'll get strange results. */ tmp = (double) omega / 2.0; s = sin (tmp); c = cos (tmp); ssq = s*s; csq = c*c; m[0][0] = m[1][1] = m[2][2] = csq - ssq; ssq *= 2.0; /* on-diagonal entries */ m[0][0] += ssq * axis[0]*axis[0]; m[1][1] += ssq * axis[1]*axis[1]; m[2][2] += ssq * axis[2]*axis[2]; /* off-diagonal entries */ m[0][1] = m[1][0] = axis[0] * axis[1] * ssq; m[1][2] = m[2][1] = axis[1] * axis[2] * ssq; m[2][0] = m[0][2] = axis[2] * axis[0] * ssq; cts = 2.0 * c * s; tmp = cts * axis[2]; m[0][1] += tmp; m[1][0] -= tmp; tmp = cts * axis[0]; m[1][2] += tmp; m[2][1] -= tmp; tmp = cts * axis[1]; m[2][0] += tmp; m[0][2] -= tmp; /* homogeneous entries */ m[0][3] = m[1][3] = m[2][3] = m[3][2] = m[3][1] = m[3][0] = 0.0; m[3][3] = 1.0; } /* ========================================================== */ #ifdef __GUTIL_DOUBLE void urot_about_axis_d (double m[4][4], /* returned */ double angle, /* input */ double axis[3]) /* input */ #else void urot_about_axis_f (float m[4][4], /* returned */ float angle, /* input */ float axis[3]) /* input */ #endif { gutDouble len, ax[3]; angle *= M_PI/180.0; /* convert to radians */ /* renormalize axis vector, if needed */ len = axis[0]*axis[0] + axis[1]*axis[1] + axis[2]*axis[2]; /* we can save some machine instructions by normalizing only * if needed. The compiler should be able to schedule in the * if test "for free". */ if (len != 1.0) { len = (gutDouble) (1.0 / sqrt ((double) len)); ax[0] = axis[0] * len; ax[1] = axis[1] * len; ax[2] = axis[2] * len; #ifdef __GUTIL_DOUBLE urot_axis_d (m, angle, ax); #else urot_axis_f (m, angle, ax); #endif } else { #ifdef __GUTIL_DOUBLE urot_axis_d (m, angle, axis); #else urot_axis_f (m, angle, axis); #endif } } /* ========================================================== */ #ifdef __GUTIL_DOUBLE void urot_omega_d (double m[4][4], /* returned */ double axis[3]) /* input */ #else void urot_omega_f (float m[4][4], /* returned */ float axis[3]) /* input */ #endif { gutDouble len, ax[3]; /* normalize axis vector */ len = axis[0]*axis[0] + axis[1]*axis[1] + axis[2]*axis[2]; len = (gutDouble) (1.0 / sqrt ((double) len)); ax[0] = axis[0] * len; ax[1] = axis[1] * len; ax[2] = axis[2] * len; /* the amount of rotation is equal to the length, in radians */ #ifdef __GUTIL_DOUBLE urot_axis_d (m, len, ax); #else urot_axis_f (m, len, ax); #endif } /* ======================= END OF FILE ========================== */