Quat.h

00001 #ifndef __Quat__
00002 #define __Quat__
00003 #include "Vec3.h"
00004 #include "Mat3.h"
00005 #include "Vec4.h"
00006 #include "Mat4.h"
00007 
00008 class Quat 
00009 {
00010 public:
00011     // constructors
00012     Quat();
00013     Quat(Real q0, Real q1, Real q2, Real q3); // [q0,(q1,q2,q3)]
00014     Quat (const Vec3 &axis, Real angle);
00015     Quat (const Mat3 &m);
00016     
00017     Int Elts() const { return (4); };
00018     
00019     Real        &operator [] (Int i);
00020     const Real  &operator [] (Int i) const;
00021     
00022     // Assignment operators
00023 
00024     Quat        &operator =  (const Quat &a);
00025     Quat        &operator += (const Quat &a);
00026     Quat        &operator -= (const Quat &a);
00027     Quat        &operator *= (const Quat &a);
00028     Quat        &operator *= (Real s);
00029     Quat        &operator /= (Real s);
00030 
00031     // Arithmetic operators
00032 
00033     Quat        operator + (const Quat &a) const;   // v + a
00034     Quat        operator - (const Quat &a) const;   // v - a
00035     Quat        operator - () const;                // -v
00036     Quat        operator * (const Quat &a) const;   // v * a (vx * ax, ...)
00037     Quat        operator * (Real s) const;          // v * s
00038     Quat        operator / (Real s) const;          // v / s
00039 
00040 
00041     Quat        &Normalise();                       // normalise vector
00042 
00043 protected:
00044     Real elt[4];
00045 };
00046 
00047 inline Quat     operator * (Real s, const Quat &v); // Left mult. by s
00048 inline Real     dot(const Quat &a, const Quat &b);  // v . a
00049 inline Real     len(const Quat &v);                 // || v ||
00050 inline Real     sqrlen(const Quat &v);              // v . v
00051 inline Quat     norm(const Quat &v);                // v / || v ||
00052 inline Void     normalise(Quat &v);                 // v = norm(v)
00053 inline Quat     slerp(const Quat &q1, const Quat &q2, Real t);
00054 inline Quat     conjugate(const Quat &q);
00055 
00056 Mat3     Rot3(const Quat &q);
00057 Mat4     HRot4(const Quat &q);
00058 
00059             
00060 //std::ostream &operator << (std::ostream &s, const Quat &v);
00061 //std::istream &operator >> (std::istream &s, Quat &v);
00062 
00063 void printQuat(const Quat &v);
00064 
00065 inline Real &Quat::operator [] (Int i)
00066 {
00067     CheckRange(i, 0, 4, "(Quat::[i]) index out of range");
00068     return(elt[i]);
00069 }
00070 
00071 inline const Real &Quat::operator [] (Int i) const
00072 {
00073     CheckRange(i, 0, 4, "(Quat::[i]) index out of range");
00074     return(elt[i]);
00075 }
00076 
00077 inline Quat::Quat()
00078 {
00079 }
00080 
00081 inline Quat::Quat(Real q0, Real q1, Real q2, Real q3)
00082 {
00083     elt[0] = q0;
00084     elt[1] = q1;
00085     elt[2] = q2;
00086     elt[3] = q3;
00087 }
00088 
00089 inline Quat::Quat(const Vec3 &axis, Real angle)
00090 {
00091     Vec3 n = norm(axis);
00092     Real sinhalf = sin(angle/2);
00093     elt[1] = sinhalf*n[0];
00094     elt[2] = sinhalf*n[1];
00095     elt[3] = sinhalf*n[2];
00096 
00097     elt[0] = cos(angle/2);
00098 }
00099 
00100 
00101 inline Quat &Quat::operator = (const Quat &v)
00102 {
00103     elt[0] = v[0];
00104     elt[1] = v[1];
00105     elt[2] = v[2];
00106     elt[3] = v[3];
00107 
00108     return(SELF);
00109 }
00110 
00111 inline Quat &Quat::operator += (const Quat &v)
00112 {
00113     elt[0] += v[0];
00114     elt[1] += v[1];
00115     elt[2] += v[2];
00116     elt[3] += v[3];
00117 
00118     return(SELF);
00119 }
00120 
00121 inline Quat &Quat::operator -= (const Quat &v)
00122 {
00123     elt[0] -= v[0];
00124     elt[1] -= v[1];
00125     elt[2] -= v[2];
00126     elt[3] -= v[3];
00127 
00128     return(SELF);
00129 }
00130 
00131 inline Quat &Quat::operator *= (const Quat &v)
00132 {
00133     Quat tmp(elt[0],elt[1],elt[2],elt[3]);
00134     tmp = tmp * v;
00135 
00136     elt[0] = tmp[0];
00137     elt[1] = tmp[1];
00138     elt[2] = tmp[2];
00139     elt[3] = tmp[3];
00140     
00141     return(SELF);
00142 }
00143 
00144 inline Quat &Quat::operator *= (Real s)
00145 {
00146     elt[0] *= s;
00147     elt[1] *= s;
00148     elt[2] *= s;
00149     elt[3] *= s;
00150 
00151     return(SELF);
00152 }
00153 
00154 inline Quat &Quat::operator /= (Real s)
00155 {
00156     elt[0] /= s;
00157     elt[1] /= s;
00158     elt[2] /= s;
00159     elt[3] /= s;
00160 
00161     return(SELF);
00162 }
00163 
00164 
00165 inline Quat Quat::operator + (const Quat &a) const
00166 {
00167     Quat result;
00168 
00169     result[0] = elt[0] + a[0];
00170     result[1] = elt[1] + a[1];
00171     result[2] = elt[2] + a[2];
00172     result[3] = elt[3] + a[3];
00173 
00174     return(result);
00175 }
00176 
00177 inline Quat Quat::operator - (const Quat &a) const
00178 {
00179     Quat result;
00180 
00181     result[0] = elt[0] - a[0];
00182     result[1] = elt[1] - a[1];
00183     result[2] = elt[2] - a[2];
00184     result[3] = elt[3] - a[3];
00185 
00186     return(result);
00187 }
00188 
00189 inline Quat Quat::operator - () const
00190 {
00191     Quat result;
00192 
00193     result[0] = -elt[0];
00194     result[1] = -elt[1];
00195     result[2] = -elt[2];
00196     result[3] = -elt[3];
00197 
00198     return(result);
00199 }
00200 
00201 inline Quat Quat::operator * (const Quat &a) const
00202 {
00203     Quat result;
00204 
00205     Vec3 qv(elt[1],elt[2],elt[3]); Real qs = elt[0];
00206     Vec3 av(a[1],a[2],a[3]); Real as = a[0];
00207 
00208     Vec3 rv = qs*av + as*qv + cross(qv,av);
00209     Real rs = qs*as - dot(qv,av);
00210 
00211     result[1] = rv[0];
00212     result[2] = rv[1];
00213     result[3] = rv[2];
00214     result[0] = rs;
00215 
00216     return(result);
00217 }
00218 
00219 inline Quat Quat::operator * (Real s) const
00220 {
00221     Quat result;
00222 
00223     result[0] = elt[0] * s;
00224     result[1] = elt[1] * s;
00225     result[2] = elt[2] * s;
00226     result[3] = elt[3] * s;
00227 
00228     return(result);
00229 }
00230 
00231 inline Quat Quat::operator / (Real s) const
00232 {
00233     Quat result;
00234 
00235     result[0] = elt[0] / s;
00236     result[1] = elt[1] / s;
00237     result[2] = elt[2] / s;
00238     result[3] = elt[3] / s;
00239 
00240     return(result);
00241 }
00242 
00243 inline Quat operator * (Real s, const Quat &v)
00244 {
00245     return(v * s);
00246 }
00247 
00248 // for convenience. Quat has no dot operation.
00249 inline Real dot(const Quat &a, const Quat &b)
00250 {
00251     return(a[0] * b[0] + a[1] * b[1] + a[2] * b[2] + a[3] * b[3]);
00252 }
00253 
00254 inline Real len(const Quat &v)
00255 {
00256     return(sqrt(dot(v, v)));
00257 }
00258 
00259 inline Real sqrlen(const Quat &v)
00260 {
00261     return(dot(v, v));
00262 }
00263 
00264 inline Quat norm(const Quat &v)
00265 {
00266     Assert(sqrlen(v) > 0.0, "normalising length-zero vector");
00267     return(v / len(v));
00268 }
00269 
00270 inline Void normalise(Quat &v)
00271 {
00272     v /= len(v);
00273 }
00274 
00275 inline Quat slerp (const Quat& q1, const Quat& q2, Real t)
00276 {
00277     Quat result;
00278     Quat qq = q1;
00279 
00280     if (dot(qq,q2) < 0)
00281         qq = -q1;
00282 
00283     Real phi = acos(dot (qq, q2));
00284     Real denom = sin(phi);
00285     
00286     result = sin(phi*(1-t))/denom * qq + sin(phi*t)/denom * q2;
00287 
00288     return result;
00289 }
00290 
00291 inline Quat conjugate(const Quat &q)
00292 {
00293     return Quat (q[0], -q[1], -q[2], -q[3]);
00294 }
00295 
00296 #endif