/* Visualizing Quaternions: Appendix E: Tables E.1 through E.7: pages 443-450 C and Mathematica code. This Appendix summarizes the essential quaternion utilities (Tables E.1 through E.7) needed to implement many of the concepts presented in the book. Selected programs are duplicated in the text as applicable. Copyright 2006, Trustees of Indiana University These files may be freely copied for personal and educational uses provided these lines are included. Full License available at: visualizingquaternions_license.txt */ /***************************************************************/ double MIN_NORM = 1.0e-7; void QuaternionProduct (double p0, double p1, double p2, double p3, double q0, double q1, double q2, double q3, double *Q0, double *Q1, double *Q2, double *Q3) { *Q0 = p0*q0 - p1*q1 - p2*q2 - p3*q3; *Q1 = p1*q0 + p0*q1 + p2*q3 - p3*q2; *Q2 = p2*q0 + p0*q2 + p3*q1 - p1*q3; *Q3 = p3*q0 + p0*q3 + p1*q2 - p2*q1; } double QuaternionDot (double p0, double p1, double p2, double p3, double q0, double q1, double q2, double q3) {return (p0*q0 + p1*q1 + p2*q2 + p3*q3); } void QuaternionConjugate (double q0, double q1, double q2, double q3, double *Q0, double *Q1, double *Q2, double *Q3) { *Q0 = q0; *Q1 = -q1; *Q2 = -q2; *Q3 = -q3; } void NormalizeQuaternion (double *q0, double *q1, double *q2, double *q3) {double denom; denom = sqrt((*q0)*(*q0) + (*q1)*(*q1) + (*q2)*(*q2) + (*q3)*(*q3)); if(denom > MIN_NORM) { *q0 = (*q0)/denom; *q1 = (*q1)/denom; *q2 = (*q2)/denom; *q3 = (*q3)/denom; } } /* Table E.1 Elementary C code implementing the quaternion operations of Equations 4.1 through 4.3, and forcing unit magnitude as required by Equation 4.4 In this straight C-coding method, we return multiple values as results only through pointers such as double *Q0. */ /***************************************************************/ typedef struct tag_Quat { double w, x, y, z;} Quat; typedef struct tag_Point { double x, y, z;} Point; void QuatMult(Quat *q1, Quat *q2, Quat *res) {res->w = q1->w * q2->w - q1->x * q2->x - q1->y * q2->y - q1->z * q2->z; res->x = q1->w * q2->x + q1->x * q2->w + q1->y * q2->z - q1->z * q2->y; res->y = q1->w * q2->y + q1->y * q2->w + q1->z * q2->x - q1->x * q2->z; res->z = q1->w * q2->z + q1->z * q2->w + q1->x * q2->y - q1->y * q2->x; } double QuatDot(Quat *q1, Quat *q2) {return( q1->w * q2->w + q1->x * q2->x + q1->y * q2->y + q1->z * q2->z); } void ConjQuat(Quat *q, Quat *qc) { qc->w = q->w; qc->x = - q->x; qc->y = - q->y; qc->z = - q->z; } double MIN_NORM = 1.0e-7; void UnitQuat(Quat *v) {double denom = sqrt( v->w*v->w + v->x*v->x + v->y*v->y + v->z*v->z ); if(denom > MIN_NORM){ v->x /= denom; v->y /= denom; v->z /= denom; v->w /= denom; } } double ScalarQuat(Quat *q) { return (q->w); } void VectorQuat(Quat *q, Point *v) { v->x = q->x; v->y = q->y; v->z = q->z; } /* Table E.2 An alternative quaternion code kit corresponding to Equations 4.1 through 4.4. The use of structures and pointers reduces the overhead for transmission of argument values compared to the version in Table E.1. */ /*********************************************************/ MatToQuat(double m[4][4], Quat * quat) { double tr, s, q[4]; int i, j, k; int nxt[3] = {1, 2, 0}; tr = m[0][0] + m[1][1] + m[2][2]; /* check the diagonal */ if (tr > 0.0) { s = sqrt (tr + 1.0); quat->w = s / 2.0; s = 0.5 / s; quat->x = (m[2][1] - m[1][2]) * s; quat->y = (m[0][2] - m[2][0]) * s; quat->z = (m[1][0] - m[0][1]) * s; } else { /* diagonal is negative */ i = 0; if (m[1][1] > m[0][0]) i = 1; if (m[2][2] > m[i][i]) i = 2; j = nxt[i]; k = nxt[j]; s = sqrt ((m[i][i] - (m[j][j] + m[k][k])) + 1.0); q[i] = s * 0.5; if (s != 0.0) s = 0.5 / s; q[3] = (m[k][j] - m[j][k]) * s; q[j] = (m[i][j] + m[j][i]) * s; q[k] = (m[i][k] + m[k][i]) * s; quat->x = q[0]; quat->y = q[1]; quat->z = q[2]; quat->w = q[3]; } } /* Table E.3 Basic C programs for manipulating quaternions, part I. */ /*************************************************************/ QuatToMatrix(Quat * quat, double m[4][4]) { double wx, wy, wz, xx, yy, yz, xy, xz, zz, x2, y2, z2; /* calculate coefficients */ x2 = quat->x + quat->x; y2 = quat->y + quat->y; z2 = quat->z + quat->z; xx = quat->x * x2; xy = quat->x * y2; xz = quat->x * z2; yy = quat->y * y2; yz = quat->y * z2; zz = quat->z * z2; wx = quat->w * x2; wy = quat->w * y2; wz = quat->w * z2; m[0][0] = 1.0 - (yy + zz); m[0][1] = xy - wz; m[0][2] = xz + wy; m[0][3] = 0.0; m[1][0] = xy + wz; m[1][1] = 1.0 - (xx + zz); m[1][2] = yz - wx; m[1][3] = 0.0; m[2][0] = xz - wy; m[2][1] = yz + wx; m[2][2] = 1.0 - (xx + yy); m[2][3] = 0.0; m[3][0] = 0; m[3][1] = 0; m[3][2] = 0; m[3][3] = 1; } /* Table E.4 Basic C programs for manipulating quaternions, part II. */ /************************************************************/ /* Adapted from Graphics Gems Module: GLquat.c */ void slerp(Quat *q1, Quat *q2, double alpha, int spin, Quat *qOut) { double beta; // complementary interp parameter double theta; // angle between A and B double sin_t, cos_t; // sine, cosine of theta double phi; // theta plus spins int bflip; // use negation of B? // cosine theta = dot product of A and B cos_t = QuatDot(q1,q2); // if B is on opposite hemisphere from A, use -B instead if (cos_t < 0.0) { cos_t = -cos_t; bflip = 1; } else bflip = 0; /* if B is (within precision limits) the same as A, * just linear interpolate between A and B. * Can't do spins, since we don't know what direction to spin. */ if (1.0 - cos_t < 1e-7) { beta = 1.0 - alpha; } else { /* normal case */ theta = acos(cos_t); phi = theta + spin * M_PI; sin_t = sin(theta); beta = sin(theta - alpha*phi) / sin_t; alpha = sin(alpha*phi) / sin_t; } if (bflip) alpha = -alpha; /* interpolate */ qOut->x = beta*q1->x + alpha*q2->x; qOut->y = beta*q1->y + alpha*q2->y; qOut->z = beta*q1->z + alpha*q2->z; qOut->w = beta*q1->w + alpha*q2->w; UnitQuat(qOut); } /* Table E.5 Basic C programs for quaternion SLERPs, part I. */ /************************************************************/ void CatmullQuat(Quat *q00, Quat *q01, Quat *q02, Quat *q03, double t, Quat *qOut) { Quat q10,q11,q12,q20,q21; slerp( q00, q01, t+1, 0, &q10 ); slerp( q01, q02, t, 0, &q11 ); slerp( q02, q03, t-1, 0, &q12 ); slerp( &q10, &q11,(t+1)/2, 0, &q20 ); slerp( &q11, &q12, t/2, 0, &q21 ); slerp( &q20, &q21, t, 0, qOut ); } void BezierQuat(Quat *q00, Quat *q01, Quat *q02, Quat *q03, double t, Quat *qOut) { Quat q10,q11,q12,q20,q21; slerp( q00, q01, t, 0, &q10 ); slerp( q01, q02, t, 0, &q11 ); slerp( q02, q03, t, 0, &q12 ); slerp( &q10, &q11, t, 0, &q20 ); slerp( &q11, &q12, t, 0, &q21 ); slerp( &q20, &q21, t, 0, qOut ); } void UniformBSplineQuat(Quat *q00, Quat *q01, Quat *q02, Quat *q03, double t, Quat *qOut) { Quat q10,q11,q12,q20,q21; slerp( q00, q01, (t+2.0)/3.0, 0, &q10 ); slerp( q01, q02, (t+1.0)/3.0, 0, &q11 ); slerp( q02, q03, t/3.0, 0, &q12 ); slerp( &q10, &q11, (t+1.0)/2.0, 0, &q20 ); slerp( &q11, &q12, t/2.0, 0, &q21 ); slerp( &q20, &q21, t, 0,qOut); } /* Table E.6 Basic C programs for quaternion SLERPs, part II. */ /******************************************************************/ (* Mathematica Code *) qprod[q_List,p_List] := {q[[1]]*p[[1]] - q[[2]]*p[[2]] - q[[3]]*p[[3]] - q[[4]]*p[[4]], q[[1]]*p[[2]] + q[[2]]*p[[1]] + q[[3]]*p[[4]] - q[[4]]*p[[3]], q[[1]]*p[[3]] + q[[3]]*p[[1]] + q[[4]]*p[[2]] - q[[2]]*p[[4]], q[[1]]*p[[4]] + q[[4]]*p[[1]] + q[[2]]*p[[3]] - q[[3]]*p[[2]]} /; Length[q] == Length[p] == 4 QuatToRot[q_List] := Module[{q0= q[[1]], q1=q[[2]], q2 = q[[3]], q3 = q[[4]]}, Module[{d23 = 2 q2 q3, d1 = 2 q0 q1, d31 = 2 q3 q1, d2 = 2 q0 q2, d12 = 2 q1 q2, d3 = 2 q0 q3, q0sq = q0^2, q1sq = q1^2, q2sq = q2^2, q3sq = q3^2}, {{q0sq + q1sq - q2sq - q3sq, d12 - d3, d31 + d2}, {d12 + d3, q0sq - q1sq + q2sq - q3sq, d23 - d1}, {d31 - d2, d23 + d1, q0sq - q1sq - q2sq + q3sq}} ]] makeQRot[angle_:0, n_List:{0,0,1}] := Module[{c = Cos[angle/2], s = Sin[angle/2]}, {c, n[[1]]*s, n[[2]]*s, n[[3]]*s}//N] RotToQuat[mat_List] := Module[{q0,q1,q2,q3,trace,s,t1,t2,t3}, trace = Sum[mat[[i,i]],{i,1,3}]; If[trace > 0, s = Sqrt[trace + 1.0]; q0 = s/2; s = 1/(2 s); q1 = (mat[[3,2]] - mat[[2,3]])*s; q2 = (mat[[1,3]] - mat[[3,1]])*s; q3 = (mat[[2,1]] - mat[[1,2]])*s, If[(mat[[1,1]] >= mat[[2,2]]) && (mat[[1,1]] >= mat[[3,3]]), (* i=0, j = 1, k = 2 *) s = Sqrt[mat[[1,1]] - mat[[2,2]] - mat[[3,3]] + 1.0]; q1 = s/2; s = 1/(2 s); q0 = (mat[[3,2]] - mat[[2,3]])*s; q2= (mat[[2,1]] + mat[[1,2]])*s; q3 = (mat[[1,3]] + mat[[3,1]])*s, If[(mat[[1,1]] < mat[[2,2]]) && (mat[[1,1]] >= mat[[3,3]]), (* i=1, j = 2, k = 0 *) s = Sqrt[mat[[2,2]] - mat[[3,3]] - mat[[1,1]] + 1.0]; q2 = s/2; s = 1/(2 s); q0 = (mat[[1,3]] - mat[[3,1]])*s; q3= (mat[[3,2]] + mat[[2,3]])*s; q1 = (mat[[2,1]] + mat[[1,2]])*s, (* Else: i=2, j = 0, k = 1 (mat[[1,1]] < mat[[2,2]]) && (mat[[1,1]] < mat[[3,3]]) *) s = Sqrt[mat[[3,3]] - mat[[1,1]] - mat[[2,2]] + 1.0]; q3 = s/2; s = 1/(2 s); q0 = (mat[[2,1]] - mat[[1,2]])*s; q1= (mat[[1,3]] + mat[[3,1]])*s; q2 = (mat[[3,2]] + mat[[2,3]])*s]]]; normalize[N[{q0,q1,q2,q3}]]] (* Table E.7 Basic Mathematica programs for manipulating quaternions. *)