00001
00002
00003
00004
00005
00006
00007
00008
00009
00010
00011
00012
00013
00014 #include "fEulerPara.h"
00015
00016 #define tiny 0.05
00017 #define eps 1e-8
00018
00019 void fEulerPara::interpolate(const fEulerPara& ep1, const fEulerPara& _ep2, double t)
00020 {
00021 fEulerPara ep2;
00022 ep2.set(_ep2);
00023 double cth = ep1 * ep2;
00024 if(cth < 0)
00025 {
00026 ep2 *= -1.0;
00027 cth *= -1.0;
00028 }
00029 if(cth > 1.0) cth = 1.0;
00030 double th = acos(cth);
00031 double sth = sin(th);
00032 double th2 = th * t;
00033 fEulerPara ep_temp1, ep_temp2;
00034 if(fabs(sth) < eps)
00035 {
00036 ep_temp1 = (1-t)*ep1;
00037 ep_temp2 = t*ep2;
00038 add(ep_temp1, ep_temp2);
00039 }
00040 else
00041 {
00042 ep_temp1 = sin(th-th2) * ep1;
00043 ep_temp2 = sin(th2) * ep2;
00044 add(ep_temp1, ep_temp2);
00045 (*this) /= sth;
00046 }
00047 unit();
00048 }
00049
00050 void fEulerPara::set(const fEulerPara& _ep)
00051 {
00052 m_scalar = _ep.m_scalar;
00053 m_vec.set(_ep.m_vec);
00054 }
00055
00056 void fEulerPara::set(const fMat33& _mat)
00057 {
00058 static fMat33 mat;
00059 mat.set(_mat);
00060 double tr, s;
00061 tr = mat(0,0) + mat(1,1) + mat(2,2);
00062 if(tr >= 0)
00063 {
00064 s = sqrt(tr + 1);
00065 m_scalar = 0.5 * s;
00066 s = 0.5 / s;
00067 m_vec(0) = (mat(2,1) - mat(1,2)) * s;
00068 m_vec(1) = (mat(0,2) - mat(2,0)) * s;
00069 m_vec(2) = (mat(1,0) - mat(0,1)) * s;
00070 }
00071 else
00072 {
00073 int i = 0;
00074 if(mat(1,1) > mat(0,0)) i = 1;
00075 if(mat(2,2) > mat(i,i)) i = 2;
00076 switch(i)
00077 {
00078 case 0:
00079 s = sqrt((mat(0,0) - (mat(1,1) + mat(2,2))) + 1);
00080 m_vec(0) = 0.5 * s;
00081 s = 0.5 / s;
00082 m_vec(1) = (mat(0,1) + mat(1,0)) * s;
00083 m_vec(2) = (mat(2,0) + mat(0,2)) * s;
00084 m_scalar = (mat(2,1) - mat(1,2)) * s;
00085 break;
00086 case 1:
00087 s = sqrt((mat(1,1) - (mat(2,2) + mat(0,0))) + 1);
00088 m_vec(1) = 0.5 * s;
00089 s = 0.5 / s;
00090 m_vec(2) = (mat(1,2) + mat(2,1)) * s;
00091 m_vec(0) = (mat(0,1) + mat(1,0)) * s;
00092 m_scalar = (mat(0,2) - mat(2,0)) * s;
00093 break;
00094 case 2:
00095 s = sqrt((mat(2,2) - (mat(0,0) + mat(1,1))) + 1);
00096 m_vec(2) = 0.5 * s;
00097 s = 0.5 / s;
00098 m_vec(0) = (mat(2,0) + mat(0,2)) * s;
00099 m_vec(1) = (mat(1,2) + mat(2,1)) * s;
00100 m_scalar = (mat(1,0) - mat(0,1)) * s;
00101 break;
00102 }
00103 }
00104 }
00105
00106 void fEulerPara::angvel2epdot(const fEulerPara& _ep, const fVec3& _omega)
00107 {
00108 fEulerPara ep(_ep);
00109 static fVec3 vel;
00110 vel.set(_omega);
00111 double e0 = ep(3), e1 = ep(0), e2 = ep(1), e3 = ep(2);
00112 double x = vel(0), y = vel(1), z = vel(2);
00113 m_scalar = - 0.5 * (e1*x + e2*y + e3*z);
00114 m_vec(0) = 0.5 * (e0*x - e3*y + e2*z);
00115 m_vec(1) = 0.5 * (e3*x + e0*y - e1*z);
00116 m_vec(2) = 0.5 * (- e2*x + e1*y + e0*z);
00117 }
00118
00119 fEulerPara mat2ep(const fMat33& _mat)
00120 {
00121 fEulerPara ret;
00122 static fMat33 mat;
00123 mat.set(_mat);
00124 #if 1 // based on Baraff's code
00125 double tr, s;
00126 tr = mat(0,0) + mat(1,1) + mat(2,2);
00127 if(tr >= 0)
00128 {
00129 s = sqrt(tr + 1);
00130 ret(3) = 0.5 * s;
00131 s = 0.5 / s;
00132 ret(0) = (mat(2,1) - mat(1,2)) * s;
00133 ret(1) = (mat(0,2) - mat(2,0)) * s;
00134 ret(2) = (mat(1,0) - mat(0,1)) * s;
00135 }
00136 else
00137 {
00138 int i = 0;
00139 if(mat(1,1) > mat(0,0)) i = 1;
00140 if(mat(2,2) > mat(i,i)) i = 2;
00141 switch(i)
00142 {
00143 case 0:
00144 s = sqrt((mat(0,0) - (mat(1,1) + mat(2,2))) + 1);
00145 ret(0) = 0.5 * s;
00146 s = 0.5 / s;
00147 ret(1) = (mat(0,1) + mat(1,0)) * s;
00148 ret(2) = (mat(2,0) + mat(0,2)) * s;
00149 ret(3) = (mat(2,1) - mat(1,2)) * s;
00150 break;
00151 case 1:
00152 s = sqrt((mat(1,1) - (mat(2,2) + mat(0,0))) + 1);
00153 ret(1) = 0.5 * s;
00154 s = 0.5 / s;
00155 ret(2) = (mat(1,2) + mat(2,1)) * s;
00156 ret(0) = (mat(0,1) + mat(1,0)) * s;
00157 ret(3) = (mat(0,2) - mat(2,0)) * s;
00158 break;
00159 case 2:
00160 s = sqrt((mat(2,2) - (mat(0,0) + mat(1,1))) + 1);
00161 ret(2) = 0.5 * s;
00162 s = 0.5 / s;
00163 ret(0) = (mat(2,0) + mat(0,2)) * s;
00164 ret(1) = (mat(1,2) + mat(2,1)) * s;
00165 ret(3) = (mat(1,0) - mat(0,1)) * s;
00166 break;
00167 }
00168 }
00169 #else
00170 double e0, e1, e2, e3, temp, ee, e0e1, e0e2, e0e3;
00171 ee = (mat(0,0) + mat(1,1) + mat(2,2) + 1) / 4;
00172 if(ee < 0) ee = 0;
00173 e0 = sqrt(ee);
00174 ret.Ang() = e0;
00175 if(e0 < tiny)
00176 {
00177 temp = ee - ((mat(1,1) + mat(2,2)) / 2);
00178 if(temp < 0) temp = 0;
00179 e1 = sqrt(temp);
00180 e0e1 = mat(2,1) - mat(1,2);
00181 if(e0e1 < 0) e1 *= -1.0;
00182 ret(0) = e1;
00183 if(e1 < tiny)
00184 {
00185 temp = ee - ((mat(2,2) + mat(0,0)) / 2);
00186 if(temp < 0) temp = 0;
00187 e2 = sqrt(temp);
00188 e0e2 = mat(0,2) - mat(2,0);
00189 if(e0e2 < 0) e2 *= -1.0;
00190 ret(1) = e2;
00191 if(e2 < tiny)
00192 {
00193 temp = ee - ((mat(0,0) + mat(1,1)) / 2);
00194 if(temp < 0) temp = 0;
00195 e3 = sqrt(temp);
00196 e0e3 = mat(1,0) - mat(0,1);
00197 if(e0e3 < 0) e3 *= -1.0;
00198 ret(2) = e3;
00199 }
00200 else
00201 {
00202 temp = 4 * e2;
00203 ret(2) = (mat(1,2) + mat(2,1)) / temp;
00204 }
00205 }
00206 else
00207 {
00208 temp = 4 * e1;
00209 ret(1) = (mat(0,1) + mat(1,0)) / temp;
00210 ret(2) = (mat(0,2) + mat(2,0)) / temp;
00211 }
00212 }
00213 else
00214 {
00215 temp = 4 * e0;
00216 ret(0) = (mat(2,1) - mat(1,2)) / temp;
00217 ret(1) = (mat(0,2) - mat(2,0)) / temp;
00218 ret(2) = (mat(1,0) - mat(0,1)) / temp;
00219 }
00220 #endif
00221 ret.unit();
00222 return ret;
00223 }
00224
00225 fMat33 ep2mat(const fEulerPara& _epara)
00226 {
00227 fMat33 ret;
00228 fEulerPara epara(_epara);
00229 double ex = epara(0), ey = epara(1), ez = epara(2), e = epara(3);
00230 double ee = e * e, exx = ex * ex, eyy = ey * ey, ezz = ez * ez;
00231 double exy = ex * ey, eyz = ey * ez, ezx = ez * ex;
00232 double eex = e * ex, eey = e * ey, eez = e * ez;
00233 ret(0,0) = exx - eyy - ezz + ee;
00234 ret(1,1) = - exx + eyy - ezz + ee;
00235 ret(2,2) = - exx - eyy + ezz + ee;
00236 ret(0,1) = 2.0 * (exy - eez);
00237 ret(1,0) = 2.0 * (exy + eez);
00238 ret(0,2) = 2.0 * (ezx + eey);
00239 ret(2,0) = 2.0 * (ezx - eey);
00240 ret(1,2) = 2.0 * (eyz - eex);
00241 ret(2,1) = 2.0 * (eyz + eex);
00242 return ret;
00243 }
00244
00245 fEulerPara fEulerPara::operator= (const fEulerPara& ep)
00246 {
00247 m_vec = ep.m_vec;
00248 m_scalar = ep.m_scalar;
00249 return (*this);
00250 }
00251
00252 fEulerPara fEulerPara::operator= (const fMat33& mat)
00253 {
00254 fMat33 tmp(mat);
00255
00256
00257 (*this) = mat2ep(tmp);
00258 return *this;
00259 }
00260
00261 fEulerPara operator * (double d, const fEulerPara& ep)
00262 {
00263 fEulerPara ret;
00264 ret.m_vec = d * ep.m_vec;
00265 ret.m_scalar = d * ep.m_scalar;
00266 return ret;
00267 }
00268
00269 fEulerPara angvel2epdot(const fEulerPara& _epara, const fVec3& vel)
00270 {
00271 fEulerPara ret, epara(_epara);
00272 fMat33 mat(epara(3), -epara(2), epara(1), epara(2), epara(3), -epara(0), -epara(1), epara(0), epara(3));
00273 ret(3) = - epara.Vec() * vel * 0.5;
00274 ret.Axis() = mat * vel * 0.5;
00275 return ret;
00276 }
00277
00278 fVec3 epdot2angvel(const fEulerPara& _epara, const fEulerPara& _edot)
00279 {
00280 fVec3 ret;
00281 fEulerPara epara(_epara), edot(_edot);
00282 fMat33 mat(epara(3), epara(2), -epara(1), -epara(2), epara(3), epara(0), epara(1), -epara(0), epara(3));
00283 ret = (- edot(3)*epara.Vec() + mat*edot.Vec()) * 2;
00284 return ret;
00285 }
00286
00287 fVec3 epddot2angacc(const fEulerPara& _e, const fEulerPara& _de, const fEulerPara& _dde)
00288 {
00289 fVec3 ret;
00290 fEulerPara e(_e), de(_de), dde(_dde);
00291 fMat33 mat1(e(3), e(2), -e(1), -e(2), e(3), e(0), e(1), -e(0), e(3));
00292 fMat33 mat2(de(3), de(2), -de(1), -de(2), de(3), de(0), de(1), -de(0), de(3));
00293 ret = 2 * (-de(3)*de.Vec() + mat2*de.Vec() - dde(3)*e.Vec() + mat1*dde.Vec());
00294 return ret;
00295 }
00296
00297 void fEulerPara::unit()
00298 {
00299 double len = sqrt((*this) * (*this));
00300 if(len > eps) (*this) /= len;
00301 }
00302
00303 fEulerPara unit(const fEulerPara& ep)
00304 {
00305 fEulerPara ret = ep;
00306 ret.unit();
00307 return ret;
00308 }
00309
00310 fEulerPara operator - (const fEulerPara& _ep)
00311 {
00312 fEulerPara ret, ep(_ep);
00313 ret.Ang() = -ep.Ang();
00314 ret.Axis() = -ep.Axis();
00315 return ret;
00316 }
00317
00318 double fEulerPara::rotation(const fEulerPara& ep0, const fVec3& s)
00319 {
00320 double y = ep0.m_vec * s;
00321 double x = ep0.m_scalar;
00322 return atan2(y, x);
00323 }
00324