Main Page   Class Hierarchy   Compound List   File List   Compound Members   File Members  

Quaternion.cc

Go to the documentation of this file.
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 }

Generated at Sat Aug 5 00:17:02 2000 for Graphics Class Library by doxygen 1.1.0 written by Dimitri van Heesch, © 1997-2000