fEulerPara.cpp
Go to the documentation of this file.
00001 /*
00002  * Copyright (c) 2008, AIST, the University of Tokyo and General Robotix Inc.
00003  * All rights reserved. This program is made available under the terms of the
00004  * Eclipse Public License v1.0 which accompanies this distribution, and is
00005  * available at http://www.eclipse.org/legal/epl-v10.html
00006  * Contributors:
00007  * The University of Tokyo
00008  */
00009 /*
00010  * fEulerPara.cpp
00011  * Create: Katz Yamane, 01.07.09
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 //      fEulerPara ep = mat2ep(tmp);
00256 //      (*this) = ep;
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 


openhrp3
Author(s): AIST, General Robotix Inc., Nakamura Lab of Dept. of Mechano Informatics at University of Tokyo
autogenerated on Sun Apr 2 2017 03:43:53