Quat.cpp

00001 #include "Quat.h"
00002 //#include "svl/Mat3.h"
00003 #include <cctype>
00004 #include <iomanip>
00005 
00006 //
00007 // Quat in SVL: [(q0,q1,q2),q3]
00008 // update: 
00009 // Modify to conform with most literature:
00010 // Quat in SVL [q0, (q1,q2,q3)]
00011 // (may need to modify other SVL files relating to quat!!!
00012 //
00013 // implement:
00014 //
00015 // Quaterion multiplication
00016 // 3x3 -> mQuat; 
00017 // mQuat->3x3 (NY) .... already available 
00018 // slerp
00019 //
00020 // find application that demonstrate the needs ...
00021 //
00022 
00023 Mat3 Rot3 (const Quat &q)
00024 {
00025     Mat3 result; 
00026     Vec4 v(q[0],q[1],q[2],q[3]);
00027     result.MakeRot(v); 
00028     return(result); 
00029 }
00030 
00031 Mat4 HRot4 (const Quat &q)
00032 {
00033     Mat4 result; 
00034     Vec4 v(q[0],q[1],q[2],q[3]);
00035     result.MakeHRot(v);
00036     return(result); 
00037 }
00038 
00039 Quat::Quat(const Mat3 & m)
00040 {   
00041     Real compare[4], max;
00042     compare[0] = 1 + m[0][0] + m[1][1] + m[2][2];
00043     compare[1] = 1 + m[0][0] - m[1][1] - m[2][2];
00044     compare[2] = 1 - m[0][0] + m[1][1] - m[2][2];
00045     compare[3] = 1 - m[0][0] - m[1][1] + m[2][2];
00046 
00047     int i, which;
00048     for (i = 0, which = -1, max = 0; i < 4; i++)
00049     if (fabs(compare[i]) > max)
00050         which = i, max = fabs(compare[i]);
00051 
00052     Real q0,q1,q2,q3;
00053     switch (which) {
00054       case 0:
00055         q0 = 0.5*sqrt (compare[which]);
00056         q1 = 0.25*(m[2][1] - m[1][2])/q0;
00057         q2 = 0.25*(m[0][2] - m[2][0])/q0;
00058         q3 = 0.25*(m[1][0] - m[0][1])/q0;
00059         break;
00060 
00061       case 1:
00062         q1 = 0.5*sqrt (compare[which]);
00063         q0 = 0.25*(m[2][1] - m[1][2])/q1;
00064         q2 = 0.25*(m[0][1] + m[1][0])/q1;
00065         q3 = 0.25*(m[0][2] + m[2][0])/q1;
00066         break;
00067 
00068       case 2:
00069         q2 = 0.5*sqrt (compare[which]);
00070         q0 = 0.25*(m[0][2] - m[2][0])/q2;
00071         q1 = 0.25*(m[0][1] + m[1][0])/q2;
00072         q3 = 0.25*(m[1][2] + m[2][1])/q2;
00073         break;
00074 
00075       case 3:
00076         q3 = 0.5*sqrt (compare[which]);
00077         q0 = 0.25*(m[1][0] - m[0][1])/q3;
00078         q1 = 0.25*(m[0][2] + m[2][0])/q3;
00079         q2 = 0.25*(m[1][2] + m[2][1])/q3;
00080         break;
00081     }
00082     elt[1] = q1, elt[2] = q2, elt[3] = q3, elt[0] = q0;
00083 }
00084 
00085 void printQuat(const Quat &v)
00086 {
00087     Int i;
00088     
00089     printf("[");
00090     for (i = 0; i < 3; i++){
00091         printf("%f", v[i]);
00092         printf(" ");
00093     }
00094     printf("%f ]/r/n", v[3]);
00095 }
00096 
00097 /*
00098 ostream &operator << (ostream &s, const Quat &v)
00099 {
00100     Int w = s.width();
00101 
00102     return(s << '[' << v[0] << ' ' << setw(w) << v[1] << ' '
00103         << setw(w) << v[2] << ' ' << setw(w) << v[3] << ']');
00104 }
00105 
00106 istream &operator >> (istream &s, Quat &v)
00107 {
00108     Quat    result;
00109     Char    c;
00110 
00111     // Expected format: [1 2 3 4]
00112 
00113     while (s >> c && isspace(c))
00114         ;
00115 
00116     if (c == '[')
00117     {
00118         s >> result[0] >> result[1] >> result[2] >> result[3];
00119 
00120         if (!s)
00121         {
00122             cerr << "Error: Expected number while reading vector\n";
00123             return(s);
00124         }
00125 
00126         while (s >> c && isspace(c))
00127             ;
00128 
00129         if (c != ']')
00130         {
00131             s.clear(ios::failbit);
00132             cerr << "Error: Expected ']' while reading vector\n";
00133             return(s);
00134         }
00135     }
00136     else
00137     {
00138         s.clear(ios::failbit);
00139         cerr << "Error: Expected '[' while reading vector\n";
00140         return(s);
00141     }
00142 
00143     v = result;
00144     return(s);
00145 }
00146 */