Quaternion.cc

00001 /*
00002 File: Quaternion.cc
00003 
00004 Function: 
00005 
00006 Author: Andrew Willmott
00007 
00008 Notes: 
00009 */
00010 
00011 #include "gcl/Quaternion.h"
00012 
00039  Quaternion MakeQuat(const Vector &axis, GCLReal theta)
00040 {
00041 GCLReal s;
00042 Quaternion q;
00043 
00044 theta /= 2.0;
00045 s = sin(theta);
00046 
00047 q[0] = s * axis[0];
00048 q[1] = s * axis[1];
00049 q[2] = s * axis[2];
00050 q[3] = cos(theta);
00051 
00052 return(q);
00053 }
00054 
00055  Quaternion MakeQuat(const Vector &point)
00056 {
00057 return(Quaternion(point, 0.0));
00058 }
00059 
00060  Quaternion QuatMult(const Quaternion &a, const Quaternion &b)
00061 {
00062 Quaternion result;
00063 Vector &va = (Vector&) a;
00064 Vector &vb = (Vector&) b;
00065 
00066 result = Quaternion(a[3] * vb + b[3] * va + cross(va, vb), a[3] * b[3] - dot(va, vb));
00067 
00068 return(result);
00069 }
00070 
00071  Quaternion MakeQuat(const VecTrans &R)
00077 {
00078 const Double epsilon = 1e-10;
00079 
00080 GCLReal x2, y2, w2;
00081 GCLReal s;
00082 Quaternion q;
00083 
00084 /* Use the Matrix -> Quaternion algorithm from Shoemake's paper */
00085 
00086 /*
00087 This algorithm avoids near-zero divides by looking for a large
00088 component; first w, then x, y, or z. When the trace is greater
00089 than zero, |w| is greater than 1/2, which is as small as a
00090 largest component can be. Otherwise the largest diagonal entry
00091 corresponds to the largest of |x|, |w| or |z|, one of which must
00092 be larger than |w|, and at least 1/2.
00093 */
00094 
00095 w2 = 0.25 * (1.0 + trace(R));
00096 
00097 if (w2 > epsilon)
00098 {
00099 s = sqrt(w2);
00100 q[3] = s;
00101 s = 0.25 / s;
00102 q[0] = (R[2][1] - R[1][2]) * s;
00103 q[1] = (R[0][2] - R[2][0]) * s;
00104 q[2] = (R[1][0] - R[0][1]) * s;
00105 }
00106 else
00107 {
00108 q[3] = 0.0;
00109 x2 = -0.5 * (R[1][1] + R[2][2]);
00110 
00111 if (x2 > epsilon)
00112 {
00113 s = sqrt(x2);
00114 q[0] = s;
00115 s = 0.5 / s;
00116 q[1] = R[1][0] * s;
00117 q[2] = R[2][0] * s;
00118 }
00119 else
00120 {
00121 q[0] = 0.0;
00122 y2 = 0.5 * (1.0 - R[2][2]);
00123 
00124 if (y2 > epsilon)
00125 {
00126 s = sqrt(y2);
00127 q[1] = s;
00128 s = 0.5 / s;
00129 q[2] = R[2][1] * s;
00130 }
00131 else
00132 {
00133 q[1] = 0.0;
00134 q[2] = 1.0;
00135 }
00136 }
00137 }
00138 
00139 q.Normalise();
00140 
00141 return(q);
00142 }