00001
00002
00003
00004
00005
00006
00007
00008
00009
00010
00011
00012
00013
00014
00015
00016
00017
00018
00019 #ifndef __CS_QUATERNION_H__
00020 #define __CS_QUATERNION_H__
00021
00022 #include "csgeom/math3d.h"
00023 #include "qsqrt.h"
00024
00028 class csQuaternion
00029 {
00030 public:
00032 inline void Init (float theR, float theX, float theY, float theZ)
00033 { r = theR; x = theX; y = theY; z = theZ; }
00034
00036 csQuaternion () { Init(0, 0, 0, 0 ); }
00038 csQuaternion (float theR, float theX=0.0, float theY=0.0, float theZ=0.0)
00039 { Init (theR, theX, theY, theZ ); }
00041 csQuaternion (const csQuaternion& q) { Init (q.r, q.x, q.y, q.z); }
00043 csQuaternion (const csVector3& q) { Init (0, q.x, q.y, q.z); }
00044
00046 inline friend csQuaternion operator+ (const csQuaternion& q1, const csQuaternion& q2)
00047 { return csQuaternion (q1.r + q2.r, q1.x + q2.x, q1.y + q2.y, q1.z + q2.z ); }
00049 inline friend csQuaternion operator- (const csQuaternion& q1, const csQuaternion& q2)
00050 { return csQuaternion (q1.r - q2.r, q1.x - q2.x, q1.y - q2.y, q1.z - q2.z ); }
00052 inline friend csQuaternion operator* (const csQuaternion& q1, const csQuaternion& q2)
00053 { return csQuaternion (q1.r*q2.r - q1.x*q2.x - q1.y*q2.y - q1.z*q2.z,
00054 q1.y*q2.z - q1.z*q2.y + q1.r*q2.x + q1.x*q2.r,
00055 q1.z*q2.x - q1.x*q2.z + q1.r*q2.y + q1.y*q2.r,
00056 q1.x*q2.y - q1.y*q2.x + q1.r*q2.z + q1.z*q2.r); }
00057
00059 csQuaternion& operator*= (const csQuaternion& q2)
00060 {
00061 Init (r*q2.r - x*q2.x - y*q2.y - z*q2.z,
00062 y*q2.z - z*q2.y + r*q2.x + x*q2.r,
00063 z*q2.x - x*q2.z + r*q2.y + y*q2.r,
00064 x*q2.y - y*q2.x + r*q2.z + z*q2.r);
00065 return *this;
00066 }
00067
00069 void Conjugate () { Init (r, -x, -y, -z); }
00070
00072 void Negate () { Init(-r, -x, -y, -z); }
00073
00079 void PrepRotation (float angle, csVector3 vec)
00080 {
00081 double theSin = sin (angle / 2.0f);
00082 Init ((float) cos (angle / 2.0f), vec.x * theSin, vec.y * theSin, vec.z * theSin);
00083 }
00084
00086 csVector3 Rotate (csVector3 vec)
00087 {
00088 csQuaternion p (vec);
00089 csQuaternion qConj (r, -x, -y, -z);
00090
00091 p = *this * p;
00092 p *= qConj;
00093 return csVector3 (p.x, p.y, p.z);
00094 }
00095
00097 void Normalize ()
00098 {
00099 if(x*x + y*y + z*z > .999)
00100 {
00101
00102 float inverselen = 1.0f / (x*x + y*y + z*z);
00103 x *= inverselen;
00104 y *= inverselen;
00105 z *= inverselen;
00106 r = 0.0f;
00107 }
00108 else
00109 {
00110 r = qsqrt(1.0f - x*x - y*y - z*z);
00111 }
00112 }
00113
00119 void SetWithEuler(const csVector3 &rot);
00120
00124 csQuaternion ToAxisAngle() const;
00125
00131 csQuaternion Slerp (const csQuaternion &quat2, float slerp) const;
00132
00133
00134
00135 float r,x,y,z;
00136 };
00137
00138
00139 #endif // __CS_QUATERNION_H__