csgeom/dualquaternion.h
Go to the documentation of this file.00001 /* 00002 Copyright (C) 2008 by Marten Svanfeldt 00003 00004 This library is free software; you can redistribute it and/or 00005 modify it under the terms of the GNU Library General Public 00006 License as published by the Free Software Foundation; either 00007 version 2 of the License, or (at your option) any later version. 00008 00009 This library is distributed in the hope that it will be useful, 00010 but WITHOUT ANY WARRANTY; without even the implied warranty of 00011 MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU 00012 Library General Public License for more details. 00013 00014 You should have received a copy of the GNU Library General Public 00015 License along with this library; if not, write to the Free 00016 Software Foundation, Inc., 675 Mass Ave, Cambridge, MA 02139, USA. 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 00046 class csDualQuaternion 00047 { 00048 public: 00049 // Constructors 00050 00052 inline csDualQuaternion () 00053 : real (0,0,0,1), dual (0,0,0,0) 00054 {} 00055 00057 inline csDualQuaternion (const csQuaternion& real, const csQuaternion& dual) 00058 : real (real), dual (dual) 00059 {} 00060 00062 inline csDualQuaternion (const csQuaternion& real) 00063 : real (real) 00064 {} 00065 00067 inline csDualQuaternion (const csQuaternion& real, const csVector3& t) 00068 : real (real), dual (0.5 * (csQuaternion (t, 0)*real)) 00069 {} 00070 00072 inline csDualQuaternion (const csDualQuaternion& q) 00073 : real (q.real), dual (q.dual) 00074 {} 00075 00076 00078 inline void SetIdentity () 00079 { 00080 real.SetIdentity (); 00081 dual.Set (0, 0, 0, 0); 00082 } 00083 00085 inline friend csDualQuaternion operator+ (const csDualQuaternion& q1, 00086 const csDualQuaternion& q2) 00087 { 00088 return csDualQuaternion (q1.real + q2.real, q1.dual + q2.dual); 00089 } 00090 00092 inline csDualQuaternion& operator+= (const csDualQuaternion& q) 00093 { 00094 real += q.real; 00095 dual += q.dual; 00096 return *this; 00097 } 00098 00100 inline friend csDualQuaternion operator- (const csDualQuaternion& q1, 00101 const csDualQuaternion& q2) 00102 { 00103 return csDualQuaternion (q1.real - q2.real, q1.dual - q2.dual); 00104 } 00105 00107 inline csDualQuaternion& operator-= (const csDualQuaternion& q) 00108 { 00109 real -= q.real; 00110 dual -= q.dual; 00111 return *this; 00112 } 00113 00115 inline friend csDualQuaternion operator- (const csDualQuaternion& q) 00116 { 00117 return csDualQuaternion (-q.real, -q.dual); 00118 } 00119 00121 inline friend csDualQuaternion operator* (const csDualQuaternion& q1, 00122 const csDualQuaternion& q2) 00123 { 00124 return csDualQuaternion (q1.real * q2.real, 00125 q1.real * q2.dual + q2.real*q1.dual); 00126 } 00127 00129 inline csDualQuaternion& operator*= (const csDualQuaternion& q) 00130 { 00131 const csQuaternion oldReal (real); 00132 00133 real *= q.real; 00134 dual = (oldReal*q.dual + q.real*dual); 00135 00136 return *this; 00137 } 00138 00140 inline friend csDualQuaternion operator* (const csDualQuaternion& q, float f) 00141 { 00142 return csDualQuaternion (q.real*f, q.dual*f); 00143 } 00144 00146 inline friend csDualQuaternion operator* (float f, const csDualQuaternion& q) 00147 { 00148 return csDualQuaternion (q.real*f, q.dual*f); 00149 } 00150 00152 inline csDualQuaternion& operator*= (float f) 00153 { 00154 real *= f; 00155 dual *= f; 00156 return *this; 00157 } 00158 00160 inline friend csDualQuaternion operator/ (const csDualQuaternion& q, float f) 00161 { 00162 float invF = 1.0f/f; 00163 return csDualQuaternion (q.real*invF, q.dual*invF); 00164 } 00165 00167 inline friend csDualQuaternion operator/ (float f, const csDualQuaternion& q) 00168 { 00169 float invF = 1.0f/f; 00170 return csDualQuaternion (q.real*invF, q.dual*invF); 00171 } 00172 00174 inline csDualQuaternion& operator/= (float f) 00175 { 00176 real /= f; 00177 dual /= f; 00178 return *this; 00179 } 00180 00182 inline csDualQuaternion GetConjugate () const 00183 { 00184 return csDualQuaternion (real.GetConjugate (), dual.GetConjugate ()); 00185 } 00186 00188 inline void Conjugate () 00189 { 00190 real.Conjugate (); 00191 dual.Conjugate (); 00192 } 00193 00195 inline csDualNumber Dot (const csDualQuaternion& q) const 00196 { 00197 return csDualNumber (real.Dot (q.real), dual.Dot (q.dual)); 00198 } 00199 00201 inline csDualNumber Norm () const 00202 { 00203 return csDualNumber (real.Norm (), real.Dot (dual) / real.Norm ()); 00204 } 00205 00212 inline csDualQuaternion Unit () const 00213 { 00214 const float lenReal = real.Norm (); 00215 00216 if (lenReal == 0) 00217 { 00218 return *this; 00219 } 00220 00221 const float lenRealInv = 1.0f / lenReal; 00222 00223 csQuaternion _real = real * lenRealInv; 00224 csQuaternion _dual = dual * lenRealInv; 00225 00226 csQuaternion r (_real); 00227 r *= _real.Dot (_dual); 00228 r *= -1.0f; 00229 00230 _dual += r; 00231 00232 return csDualQuaternion (_real, _dual); 00233 } 00234 00239 inline csDualQuaternion GetInverse () const 00240 { 00241 const float realNorm = real.Norm (); 00242 const float rdd = real.Dot (dual); 00243 00244 return csDualQuaternion ( 00245 real.GetConjugate ()*realNorm, 00246 dual.GetConjugate ()*(realNorm - 2*rdd)); 00247 } 00248 00252 inline csVector3 TransformPoint (const csVector3& v) const 00253 { 00254 csVector3 position = v + 2.0f * (real.v % ((real.v % v) + real.w * v)); 00255 csVector3 trans = 2.0f * (real.w * dual.v - dual.w * real.v + (real.v % dual.v)); 00256 00257 return position + trans; 00258 } 00259 00263 inline csVector3 Transform (const csVector3& v) const 00264 { 00265 csVector3 position = v + 2.0f * (real.v % ((real.v % v) + real.w * v)); 00266 //csVector3 trans = 2.0f * (real.w * dual.v - dual.w * real.v + (real.v % dual.v)); 00267 00268 return position /*+ trans*/; 00269 } 00270 00271 // Data 00273 csQuaternion real; 00274 00276 csQuaternion dual; 00277 }; 00278 00281 #endif // __CS_QUATERNION_H__ 00282
Generated for Crystal Space 1.4.0 by doxygen 1.5.8