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,
00057 const csQuaternion& q2)
00058 {
00059 return csQuaternion (q1.r + q2.r, q1.x + q2.x, q1.y + q2.y, q1.z + q2.z );
00060 }
00061
00063 inline friend csQuaternion operator- (const csQuaternion& q1,
00064 const csQuaternion& q2)
00065 {
00066 return csQuaternion (q1.r - q2.r, q1.x - q2.x, q1.y - q2.y, q1.z - q2.z );
00067 }
00068
00070 inline friend csQuaternion operator* (const csQuaternion& q1,
00071 const csQuaternion& q2)
00072 {
00073 return csQuaternion (q1.r*q2.r - q1.x*q2.x - q1.y*q2.y - q1.z*q2.z,
00074 q1.y*q2.z - q1.z*q2.y + q1.r*q2.x + q1.x*q2.r,
00075 q1.z*q2.x - q1.x*q2.z + q1.r*q2.y + q1.y*q2.r,
00076 q1.x*q2.y - q1.y*q2.x + q1.r*q2.z + q1.z*q2.r);
00077 }
00078
00080 csQuaternion& operator*= (const csQuaternion& q2)
00081 {
00082 Init (r*q2.r - x*q2.x - y*q2.y - z*q2.z,
00083 y*q2.z - z*q2.y + r*q2.x + x*q2.r,
00084 z*q2.x - x*q2.z + r*q2.y + y*q2.r,
00085 x*q2.y - y*q2.x + r*q2.z + z*q2.r);
00086 return *this;
00087 }
00088
00090 void Conjugate () { Init (r, -x, -y, -z); }
00091
00093 void Negate () { Init(-r, -x, -y, -z); }
00094
00096 void Invert();
00097
00103 void GetAxisAngle(csVector3& axis, float& phi) const;
00104
00110 void SetWithAxisAngle(csVector3 axis, float phi);
00111
00117 void PrepRotation (float angle, csVector3 vec)
00118 {
00119 double theSin = sin (angle / 2.0f);
00120 Init ((float) cos (angle / 2.0f), vec.x * theSin, vec.y * theSin,
00121 vec.z * theSin);
00122 }
00123
00125 csVector3 Rotate (csVector3 vec)
00126 {
00127 csQuaternion p (vec);
00128 csQuaternion qConj (r, -x, -y, -z);
00129
00130 p = *this * p;
00131 p *= qConj;
00132 return csVector3 (p.x, p.y, p.z);
00133 }
00134
00136 void Normalize ()
00137 {
00138 float dist, square;
00139 square = x * x + y * y + z * z + r * r;
00140
00141 if (square > 0.0) dist = (float)qisqrt(square);
00142 else dist = 1;
00143
00144 x *= dist;
00145 y *= dist;
00146 z *= dist;
00147 r *= dist;
00148
00149
00150
00151
00152
00153
00154
00155
00156
00157
00158
00159
00160
00161
00162
00163 }
00164
00170 void SetWithEuler (const csVector3 &rot);
00171
00176 void GetEulerAngles (csVector3& angles);
00177
00181 csQuaternion ToAxisAngle () const;
00182
00188 csQuaternion Slerp (const csQuaternion &quat2, float slerp) const;
00189
00190
00191
00192 float r,x,y,z;
00193 };
00194
00197 #endif // __CS_QUATERNION_H__
00198