00001 #include "../math/Trig.hpp" 00002 #include "../math/QuatT.hpp" 00003 #include "Quat4.hpp" 00004 #include "Euler3.hpp" 00005 #include "AxisAngle4.hpp" 00006 #include <cstdio> 00007 00008 namespace se_core { 00009 const Quat4 Quat4::IDENTITY(0, 0, 0, 1); 00010 00011 void Quat4 00012 ::mul(const Quat4& q1, const Quat4& q2) { 00013 // store on stack for aliasing-safty 00014 set( 00015 QuatT::fmul(q1.x_, q2.w_) + QuatT::fmul(q1.w_, q2.x_) + QuatT::fmul(q1.y_, q2.z_) - QuatT::fmul(q1.z_, q2.y_), 00016 QuatT::fmul(q1.y_, q2.w_) + QuatT::fmul(q1.w_, q2.y_) + QuatT::fmul(q1.z_, q2.x_) - QuatT::fmul(q1.x_, q2.z_), 00017 QuatT::fmul(q1.z_, q2.w_) + QuatT::fmul(q1.w_, q2.z_) + QuatT::fmul(q1.x_, q2.y_) - QuatT::fmul(q1.y_, q2.x_), 00018 QuatT::fmul(q1.w_, q2.w_) - QuatT::fmul(q1.x_, q2.x_) - QuatT::fmul(q1.y_, q2.y_) - QuatT::fmul(q1.z_, q2.z_) 00019 ); 00020 } 00021 00022 00023 void Quat4 00024 ::mul(const Quat4& q1) { 00025 // store on stack for aliasing-safty 00026 set( 00027 QuatT::fmul(x_, q1.w_) + QuatT::fmul(w_, q1.x_) + QuatT::fmul(y_, q1.z_) - QuatT::fmul(z_, q1.y_), 00028 QuatT::fmul(y_, q1.w_) + QuatT::fmul(w_, q1.y_) + QuatT::fmul(z_, q1.x_) - QuatT::fmul(x_, q1.z_), 00029 QuatT::fmul(z_, q1.w_) + QuatT::fmul(w_, q1.z_) + QuatT::fmul(x_, q1.y_) - QuatT::fmul(y_, q1.x_), 00030 QuatT::fmul(w_, q1.w_) - QuatT::fmul(x_, q1.x_) - QuatT::fmul(y_, q1.y_) - QuatT::fmul(z_, q1.z_) 00031 ); 00032 } 00033 00034 00035 void Quat4 00036 ::mulInverse(const Quat4& q1, const Quat4& q2) { 00037 // zero-div may occur. 00038 scale_t n = QuatT::oneOver( norm() ); 00039 00040 // store on stack once for aliasing-safty 00041 set( 00042 QuatT::scale(n, 00043 (QuatT::fmul(q1.x_, q2.w_) 00044 - QuatT::fmul(q1.w_, q2.x_) 00045 - QuatT::fmul(q1.y_, q2.z_) 00046 + QuatT::fmul(q1.z_, q2.y_)) 00047 ), 00048 QuatT::scale(n, 00049 (QuatT::fmul(q1.y_, q2.w_) 00050 - QuatT::fmul(q1.w_, q2.y_) 00051 - QuatT::fmul(q1.z_, q2.x_) 00052 + QuatT::fmul(q1.x_, q2.z_))), 00053 QuatT::scale(n, 00054 (QuatT::fmul(q1.z_, q2.w_) 00055 - QuatT::fmul(q1.w_, q2.z_) 00056 - QuatT::fmul(q1.x_, q2.y_) 00057 + QuatT::fmul(q1.y_, q2.x_))), 00058 QuatT::scale(n, 00059 (QuatT::fmul(q1.w_, q2.w_) 00060 + QuatT::fmul(q1.x_, q2.x_) 00061 + QuatT::fmul(q1.y_, q2.y_) 00062 + QuatT::fmul(q1.z_, q2.z_))) 00063 ); 00064 } 00065 00066 00067 void Quat4 00068 ::mulInverse(const Quat4& q1) { 00069 // zero-div may occur. 00070 scale_t n = QuatT::oneOver( norm() ); 00071 00072 // store on stack once for aliasing-safty 00073 set( 00074 QuatT::scale(n, 00075 (QuatT::fmul(x_, q1.w_) 00076 - QuatT::fmul(w_, q1.x_) 00077 - QuatT::fmul(y_, q1.z_) 00078 + QuatT::fmul(z_, q1.y_)) 00079 ), 00080 QuatT::scale(n, 00081 (QuatT::fmul(y_, q1.w_) 00082 - QuatT::fmul(w_, q1.y_) 00083 - QuatT::fmul(z_, q1.x_) 00084 + QuatT::fmul(x_, q1.z_))), 00085 QuatT::scale(n, 00086 (QuatT::fmul(z_, q1.w_) 00087 - QuatT::fmul(w_, q1.z_) 00088 - QuatT::fmul(x_, q1.y_) 00089 + QuatT::fmul(y_, q1.x_))), 00090 QuatT::scale(n, 00091 (QuatT::fmul(w_, q1.w_) 00092 + QuatT::fmul(x_, q1.x_) 00093 + QuatT::fmul(y_, q1.y_) 00094 + QuatT::fmul(z_, q1.z_))) 00095 ); 00096 00097 } 00098 00099 00100 void Quat4 00101 ::set(const AxisAngle4& a1) { 00102 x_ = a1.x_; 00103 y_ = a1.y_; 00104 z_ = a1.z_; 00105 00106 coor_t n = QuatT::oneOver( CoorT::sqrt( CoorT::pow2(x_) + CoorT::pow2(y_) + CoorT::pow2(z_)) ); 00107 //LogDetail(toLog() << ":n " << n); 00108 00109 #ifdef SE_FIXED_POINT 00110 #error "Not implemented" 00111 #endif 00112 00113 // zero-div may occur. 00114 //coor_t s = ::sin(0.5*a1.angle_)/n; 00115 coor_t s = Trig::sinScale(n, a1.angle_ >> 1); 00116 //LogDetail(toLog() << ":s " << s); 00117 00118 x_ *= s; 00119 y_ *= s; 00120 z_ *= s; 00121 w_ = Trig::cosQuat(a1.angle_ >> 1); 00122 //LogDetail(toLog()); 00123 } 00124 00125 00126 void Quat4 00127 ::set(const Euler3& a1) { 00128 // Assuming the angles are in braybrookians (bray_t). 00129 quat_t c1 = -Trig::cosQuat(a1.yaw_ >> 1); 00130 quat_t c2 = -Trig::cosQuat(a1.roll_ >> 1); 00131 quat_t c3 = -Trig::cosQuat(a1.pitch_ >> 1); 00132 00133 quat_t s1 = Trig::sinQuat(a1.yaw_ >> 1); 00134 quat_t s2 = Trig::sinQuat(a1.roll_ >> 1); 00135 quat_t s3 = Trig::sinQuat(a1.pitch_ >> 1); 00136 00137 // Yaw and pitch 00138 quat_t x = QuatT::fmul(c1, s3); 00139 quat_t y = QuatT::fmul(s1, c3); 00140 quat_t z = -QuatT::fmul(s1, s3); 00141 quat_t w = QuatT::fmul(c1, c3); 00142 00143 // Then roll 00144 x_ = QuatT::fmul(x, c2) + QuatT::fmul(y, s2); 00145 y_ = QuatT::fmul(y, c2) - QuatT::fmul(x, s2); 00146 z_ = QuatT::fmul(z, c2) + QuatT::fmul(w, s2); 00147 w_ = QuatT::fmul(w, c2) - QuatT::fmul(z, s2); 00148 } 00149 00150 void Quat4 00151 ::setEuler(const bray_t yaw, const bray_t pitch, const bray_t roll) { 00152 set(Euler3(yaw, pitch, roll)); 00153 } 00154 00155 00156 void Quat4 00157 ::setYawAndPitch(const bray_t yaw, const bray_t pitch) { 00158 Assert(yaw == (yaw & BRAY_MASK)); 00159 Assert(pitch == (pitch & BRAY_MASK)); 00160 00161 00162 trig_t c1 = -Trig::cosQuat(yaw >> 1); 00163 trig_t c3 = -Trig::cosQuat(pitch >> 1); 00164 00165 trig_t s1 = Trig::sinQuat(yaw >> 1); 00166 trig_t s3 = Trig::sinQuat(pitch >> 1); 00167 00168 x_ = QuatT::fmul(c1, s3); 00169 y_ = QuatT::fmul(s1, c3); 00170 z_ = -QuatT::fmul(s1, s3); 00171 w_ = QuatT::fmul(c1, c3); 00172 } 00173 00174 00175 void Quat4 00176 ::setYaw(const bray_t yaw) { 00177 // Assuming the angles are in braybrookians (bray_t). 00178 Assert(yaw == (yaw & BRAY_MASK)); 00179 00180 x_ = 0; 00181 y_ = Trig::sinQuat(yaw >> 1); 00182 z_ = 0; 00183 w_ = -Trig::cosQuat(yaw >> 1); 00184 } 00185 00186 00187 void Quat4 00188 ::setPitch(const bray_t pitch) { 00189 // Assuming the angles are in braybrookians (bray_t). 00190 Assert(pitch == (pitch & BRAY_MASK)); 00191 00192 x_ = Trig::sinQuat(pitch >> 1); 00193 y_ = 0; 00194 z_ = 0; 00195 w_ = -Trig::cosQuat(pitch >> 1); 00196 00197 normalize(); 00198 } 00199 00200 00201 void Quat4 00202 ::setRoll(const bray_t roll) { 00203 // Assuming the angles are in braybrookians (bray_t). 00204 Assert(roll == (roll & BRAY_MASK)); 00205 00206 x_ = 0; 00207 y_ = 0; 00208 z_ = Trig::sinQuat(roll >> 1); 00209 w_ = -Trig::cosQuat(roll >> 1); 00210 } 00211 00212 void Quat4 00213 ::slerp(const Quat4& q1, scale_t alpha, bool findShortestPath) { 00214 #ifdef SE_FIXED_POINT 00215 LogFatal("Not yet implemented"); 00216 #else 00217 00218 // Both parameters must be pre-normalized 00219 Assert(isNormalized()); 00220 Assert(q1.isNormalized()); 00221 /* 00222 // From Hoggar. 00223 normalize(); 00224 00225 // zero-div may occur. 00226 scale_t n1 = QuatT::oneOver( CoorT::sqrt( q1.norm() ) ); 00227 coor_t x1 = QuatT::scale(n1, q1.x_); 00228 coor_t y1 = QuatT::scale(n1, q1.y_); 00229 coor_t z1 = QuatT::scale(n1, q1.z_); 00230 coor_t w1 = QuatT::scale(n1, q1.w_); 00231 */ 00232 00233 const coor_t x1 = q1.x_; 00234 const coor_t y1 = q1.y_; 00235 const coor_t z1 = q1.z_; 00236 const coor_t w1 = q1.w_; 00237 float cos_t = x_*x1 + y_*y1 + z_*z1 + w_*w1; 00238 00239 // t is cosine (dot product) 00240 // same quaternion (avoid domain error) 00241 if (1.0 <= CoorT::abs(cos_t)) 00242 return; 00243 00244 // t is now theta 00245 float theta = ::acos(cos_t); 00246 float sin_t = ::sin(theta); 00247 00248 // same quaternion (avoid zero-div) 00249 if (sin_t == 0) 00250 return; 00251 00252 scale_t s = static_cast<scale_t>(::sin((1.0-alpha)* cos_t)/sin_t); 00253 scale_t t = static_cast<scale_t>(::sin(alpha* cos_t)/sin_t); 00254 00255 if(findShortestPath && t < 0) { 00256 s = -s; 00257 00258 x_ = s*x_ + t*x1; 00259 y_ = s*y_ + t*y1; 00260 z_ = s*z_ + t*z1; 00261 w_ = s*w_ + t*w1; 00262 //normalize(); 00263 } 00264 else { 00265 // set values 00266 x_ = s*x_ + t*x1; 00267 y_ = s*y_ + t*y1; 00268 z_ = s*z_ + t*z1; 00269 w_ = s*w_ + t*w1; 00270 } 00271 00272 // Stay stable 00273 normalize(); 00274 00275 #endif 00276 } 00277 00278 00279 void Quat4 00280 ::slerp(const Quat4& q1, const Quat4& q2, scale_t alpha, bool findShortestPath) { 00281 set(q1); 00282 slerp(q2, alpha, findShortestPath); 00283 } 00284 00285 00286 }
Home Page | SagaEngine trunk (updated nightly) reference generated Sun Dec 2 20:06:13 2007 by Doxygen version 1.3.9.1.