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
00028 #include "csgeom/math3d.h"
00029 #include "csgeom/matrix3.h"
00030 #include "qsqrt.h"
00031
00035 class csQuaternion
00036 {
00037 public:
00039 inline void Init (float theR, float theX, float theY, float theZ)
00040 { r = theR; x = theX; y = theY; z = theZ; }
00041
00043 csQuaternion () { Init(0, 0, 0, 0 ); }
00045 csQuaternion (float theR, float theX=0.0, float theY=0.0, float theZ=0.0)
00046 { Init (theR, theX, theY, theZ ); }
00048 csQuaternion (const csQuaternion& q) { Init (q.r, q.x, q.y, q.z); }
00050 csQuaternion (const csVector3& q) { Init (0, q.x, q.y, q.z); }
00051
00053 csQuaternion (const csMatrix3& smat);
00054
00056 inline friend csQuaternion operator+ (const csQuaternion& q1, const csQuaternion& q2)
00057 { return csQuaternion (q1.r + q2.r, q1.x + q2.x, q1.y + q2.y, q1.z + q2.z ); }
00059 inline friend csQuaternion operator- (const csQuaternion& q1, const csQuaternion& q2)
00060 { return csQuaternion (q1.r - q2.r, q1.x - q2.x, q1.y - q2.y, q1.z - q2.z ); }
00062 inline friend csQuaternion operator* (const csQuaternion& q1, const csQuaternion& q2)
00063 { return csQuaternion (q1.r*q2.r - q1.x*q2.x - q1.y*q2.y - q1.z*q2.z,
00064 q1.y*q2.z - q1.z*q2.y + q1.r*q2.x + q1.x*q2.r,
00065 q1.z*q2.x - q1.x*q2.z + q1.r*q2.y + q1.y*q2.r,
00066 q1.x*q2.y - q1.y*q2.x + q1.r*q2.z + q1.z*q2.r); }
00067
00069 csQuaternion& operator*= (const csQuaternion& q2)
00070 {
00071 Init (r*q2.r - x*q2.x - y*q2.y - z*q2.z,
00072 y*q2.z - z*q2.y + r*q2.x + x*q2.r,
00073 z*q2.x - x*q2.z + r*q2.y + y*q2.r,
00074 x*q2.y - y*q2.x + r*q2.z + z*q2.r);
00075 return *this;
00076 }
00077
00079 void Conjugate () { Init (r, -x, -y, -z); }
00080
00082 void Negate () { Init(-r, -x, -y, -z); }
00083
00085 void Invert();
00086
00091 void GetAxisAngle(csVector3& axis, float& phi) const;
00092
00097 void SetWithAxisAngle(csVector3 axis, float phi);
00098
00104 void PrepRotation (float angle, csVector3 vec)
00105 {
00106 double theSin = sin (angle / 2.0f);
00107 Init ((float) cos (angle / 2.0f), vec.x * theSin, vec.y * theSin, vec.z * theSin);
00108 }
00109
00111 csVector3 Rotate (csVector3 vec)
00112 {
00113 csQuaternion p (vec);
00114 csQuaternion qConj (r, -x, -y, -z);
00115
00116 p = *this * p;
00117 p *= qConj;
00118 return csVector3 (p.x, p.y, p.z);
00119 }
00120
00122 void Normalize ()
00123 {
00124 float dist, square;
00125
00126 square = x * x + y * y + z * z + r * r;
00127
00128 if (square > 0.0)
00129 dist = (float)(1.0 / sqrt(square));
00130 else dist = 1;
00131
00132 x *= dist;
00133 y *= dist;
00134 z *= dist;
00135 r *= dist;
00136
00137
00138
00139
00140
00141
00142
00143
00144
00145
00146
00147
00148
00149
00150
00151 }
00152
00158 void SetWithEuler(const csVector3 &rot);
00159
00163 csQuaternion ToAxisAngle() const;
00164
00170 csQuaternion Slerp (const csQuaternion &quat2, float slerp) const;
00171
00172
00173
00174 float r,x,y,z;
00175 };
00176
00179 #endif // __CS_QUATERNION_H__