Go to the documentation of this file.00001
00002
00003
00004
00005
00006
00007
00008
00009
00010
00011
00012
00013
00014
00015
00016
00017
00018
00019 #ifndef __CS_CSGEOM_DUALQUATERNION_H__
00020 #define __CS_CSGEOM_DUALQUATERNION_H__
00021
00029 #include "csextern.h"
00030 #include "csqsqrt.h"
00031
00032 #include "csgeom/vector3.h"
00033 #include "csgeom/quaternion.h"
00034 #include "csutil/tuple.h"
00035
00036 class csMatrix3;
00037
00039 typedef csTuple2<float, float> csDualNumber;
00040
00047 class csDualQuaternion
00048 {
00049 public:
00050
00051
00053 inline csDualQuaternion ()
00054 : real (0,0,0,1), dual (0,0,0,0)
00055 {}
00056
00058 inline csDualQuaternion (const csQuaternion& real, const csQuaternion& dual)
00059 : real (real), dual (dual)
00060 {}
00061
00063 inline csDualQuaternion (const csQuaternion& real)
00064 : real (real)
00065 {}
00066
00068 inline csDualQuaternion (const csQuaternion& real, const csVector3& t)
00069 : real (real), dual (0.5 * (csQuaternion (t, 0)*real))
00070 {}
00071
00073 inline csDualQuaternion (const csDualQuaternion& q)
00074 : real (q.real), dual (q.dual)
00075 {}
00076
00077
00079 inline void SetIdentity ()
00080 {
00081 real.SetIdentity ();
00082 dual.Set (0, 0, 0, 0);
00083 }
00084
00086 inline friend csDualQuaternion operator+ (const csDualQuaternion& q1,
00087 const csDualQuaternion& q2)
00088 {
00089 return csDualQuaternion (q1.real + q2.real, q1.dual + q2.dual);
00090 }
00091
00093 inline csDualQuaternion& operator+= (const csDualQuaternion& q)
00094 {
00095 real += q.real;
00096 dual += q.dual;
00097 return *this;
00098 }
00099
00101 inline friend csDualQuaternion operator- (const csDualQuaternion& q1,
00102 const csDualQuaternion& q2)
00103 {
00104 return csDualQuaternion (q1.real - q2.real, q1.dual - q2.dual);
00105 }
00106
00108 inline csDualQuaternion& operator-= (const csDualQuaternion& q)
00109 {
00110 real -= q.real;
00111 dual -= q.dual;
00112 return *this;
00113 }
00114
00116 inline friend csDualQuaternion operator- (const csDualQuaternion& q)
00117 {
00118 return csDualQuaternion (-q.real, -q.dual);
00119 }
00120
00122 inline friend csDualQuaternion operator* (const csDualQuaternion& q1,
00123 const csDualQuaternion& q2)
00124 {
00125 return csDualQuaternion (q1.real * q2.real,
00126 q1.real * q2.dual + q2.real*q1.dual);
00127 }
00128
00130 inline csDualQuaternion& operator*= (const csDualQuaternion& q)
00131 {
00132 const csQuaternion oldReal (real);
00133
00134 real *= q.real;
00135 dual = (oldReal*q.dual + q.real*dual);
00136
00137 return *this;
00138 }
00139
00141 inline friend csDualQuaternion operator* (const csDualQuaternion& q, float f)
00142 {
00143 return csDualQuaternion (q.real*f, q.dual*f);
00144 }
00145
00147 inline friend csDualQuaternion operator* (float f, const csDualQuaternion& q)
00148 {
00149 return csDualQuaternion (q.real*f, q.dual*f);
00150 }
00151
00153 inline csDualQuaternion& operator*= (float f)
00154 {
00155 real *= f;
00156 dual *= f;
00157 return *this;
00158 }
00159
00161 inline friend csDualQuaternion operator/ (const csDualQuaternion& q, float f)
00162 {
00163 float invF = 1.0f/f;
00164 return csDualQuaternion (q.real*invF, q.dual*invF);
00165 }
00166
00168 inline friend csDualQuaternion operator/ (float f, const csDualQuaternion& q)
00169 {
00170 float invF = 1.0f/f;
00171 return csDualQuaternion (q.real*invF, q.dual*invF);
00172 }
00173
00175 inline csDualQuaternion& operator/= (float f)
00176 {
00177 real /= f;
00178 dual /= f;
00179 return *this;
00180 }
00181
00183 inline csDualQuaternion GetConjugate () const
00184 {
00185 return csDualQuaternion (real.GetConjugate (), dual.GetConjugate ());
00186 }
00187
00189 inline void Conjugate ()
00190 {
00191 real.Conjugate ();
00192 dual.Conjugate ();
00193 }
00194
00196 inline csDualNumber Dot (const csDualQuaternion& q) const
00197 {
00198 return csDualNumber (real.Dot (q.real), dual.Dot (q.dual));
00199 }
00200
00202 inline csDualNumber Norm () const
00203 {
00204 return csDualNumber (real.Norm (), real.Dot (dual) / real.Norm ());
00205 }
00206
00213 inline csDualQuaternion Unit () const
00214 {
00215 const float lenReal = real.Norm ();
00216
00217 if (lenReal == 0)
00218 {
00219 return *this;
00220 }
00221
00222 const float lenRealInv = 1.0f / lenReal;
00223
00224 csQuaternion _real = real * lenRealInv;
00225 csQuaternion _dual = dual * lenRealInv;
00226
00227 csQuaternion r (_real);
00228 r *= _real.Dot (_dual);
00229 r *= -1.0f;
00230
00231 _dual += r;
00232
00233 return csDualQuaternion (_real, _dual);
00234 }
00235
00240 inline csDualQuaternion GetInverse () const
00241 {
00242 const float realNorm = real.Norm ();
00243 const float rdd = real.Dot (dual);
00244
00245 return csDualQuaternion (
00246 real.GetConjugate ()*realNorm,
00247 dual.GetConjugate ()*(realNorm - 2*rdd));
00248 }
00249
00253 inline csVector3 TransformPoint (const csVector3& v) const
00254 {
00255 csVector3 position = v + 2.0f * (real.v % ((real.v % v) + real.w * v));
00256 csVector3 trans = 2.0f * (real.w * dual.v - dual.w * real.v + (real.v % dual.v));
00257
00258 return position + trans;
00259 }
00260
00264 inline csVector3 Transform (const csVector3& v) const
00265 {
00266 csVector3 position = v + 2.0f * (real.v % ((real.v % v) + real.w * v));
00267
00268
00269 return position ;
00270 }
00271
00272
00274 csQuaternion real;
00275
00277 csQuaternion dual;
00278 };
00279
00282 #endif // __CS_QUATERNION_H__
00283