ColladaUtil.h
Go to the documentation of this file.
00001 // -*- coding: utf-8 -*-
00002 // Copyright (C) 2011 University of Tokyo, General Robotix Inc.
00003 //
00004 // Licensed under the Apache License, Version 2.0 (the "License");
00005 // you may not use this file except in compliance with the License.
00006 // You may obtain a copy of the License at
00007 //     http://www.apache.org/licenses/LICENSE-2.0
00008 // 
00009 // Unless required by applicable law or agreed to in writing, software
00010 // distributed under the License is distributed on an "AS IS" BASIS,
00011 // WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
00012 // See the License for the specific language governing permissions and
00013 // limitations under the License.
00021 #ifndef OPENHRP_COLLADA_UTIL_H
00022 #define OPENHRP_COLLADA_UTIL_H
00023 
00024 #include <dae.h>
00025 #include <dae/daeErrorHandler.h>
00026 #include <1.5/dom/domCOLLADA.h>
00027 #include <dae/domAny.h>
00028 #include <1.5/dom/domConstants.h>
00029 #include <1.5/dom/domTriangles.h>
00030 #include <dae/daeDocument.h>
00031 #include <1.5/dom/domTypes.h>
00032 #include <1.5/dom/domImage.h>
00033 #include <1.5/dom/domElements.h>
00034 #include <1.5/dom/domKinematics.h>
00035 #include <dae/daeStandardURIResolver.h>
00036 #include <locale>
00037 #include <string>
00038 #include <vector>
00039 #include <list>
00040 #include <map>
00041 #include <list>
00042 #include <vector>
00043 #include <sstream>
00044 
00045 #if defined(WIN32) || defined(_WIN32) || defined(__WIN32__) || defined(__NT__)
00046 #  if (BOOST_VERSION > 104400) 
00047 #    define BOOST_ENABLE_ASSERT_HANDLER
00048 #  endif
00049 #else
00050 #  define BOOST_ENABLE_ASSERT_HANDLER
00051 #endif
00052 
00053 #include <boost/date_time/posix_time/posix_time.hpp>
00054 #include <boost/date_time/time_facet.hpp>
00055 #include <boost/algorithm/string.hpp>
00056 #include <boost/format.hpp>
00057 #include <boost/shared_ptr.hpp>
00058 #include <boost/array.hpp>
00059 #include <boost/lexical_cast.hpp>
00060 
00061 #include <hrpCorba/ORBwrap.h>
00062 #include <hrpCorba/ModelLoader.hh>
00063 
00064 #ifdef _MSC_VER
00065 #ifndef __PRETTY_FUNCTION__
00066 #define __PRETTY_FUNCTION__ __FUNCDNAME__
00067 #endif
00068 #endif
00069 
00070 #ifndef __PRETTY_FUNCTION__
00071 #define __PRETTY_FUNCTION__ __func__
00072 #endif
00073 
00074 #define COLLADA_ASSERT(b) { if( !(b) ) { std::stringstream ss; ss << "ikfast exception: " << __FILE__ << ":" << __LINE__ << ": " <<__PRETTY_FUNCTION__ << ": Assertion '" << #b << "' failed"; throw OpenHRP::ModelLoader::ModelLoaderException(ss.str().c_str()); } }
00075 
00076 namespace ColladaUtil
00077 {
00078 typedef double dReal;
00079 
00080 // logging functions
00081 inline void COLLADALOG_VERBOSE(const std::string& s) {
00082     //std::cout << "Collada Verbose: " << s << std::endl;
00083 }
00084 inline void COLLADALOG_DEBUG(const std::string& s) {
00085     //std::cout << "Collada Debug: " << s << std::endl;
00086 }
00087 inline void COLLADALOG_INFO(const std::string& s) {
00088     std::cout << "Collada Info: " << s << std::endl;
00089 }
00090 inline void COLLADALOG_WARN(const std::string& s) {
00091     std::cout << "Collada Warning: " << s << std::endl;
00092 }
00093 inline void COLLADALOG_ERROR(const std::string& s) {
00094     std::cout << "Collada Error: " << s << std::endl;
00095 }
00096 
00097 //
00098 // openrave math functions (from geometry.h)
00099 //
00100 
00101 inline void PoseMultVector(OpenHRP::DblArray3& vnew, const OpenHRP::DblArray12& m, const OpenHRP::DblArray3& v)
00102 {
00103     dReal x = v[0], y = v[1], z = v[2];
00104     vnew[0] = m[4*0+0] * x + m[4*0+1] * y + m[4*0+2] * z + m[4*0+3];
00105     vnew[1] = m[4*1+0] * x + m[4*1+1] * y + m[4*1+2] * z + m[4*1+3];
00106     vnew[2] = m[4*2+0] * x + m[4*2+1] * y + m[4*2+2] * z + m[4*2+3];
00107 }
00108 
00109 inline void PoseRotateVector(OpenHRP::DblArray3& vnew, const OpenHRP::DblArray12& m, const OpenHRP::DblArray3& v)
00110 {
00111     dReal x = v[0], y = v[1], z = v[2];
00112     vnew[0] = m[4*0+0] * x + m[4*0+1] * y + m[4*0+2] * z;
00113     vnew[1] = m[4*1+0] * x + m[4*1+1] * y + m[4*1+2] * z;
00114     vnew[2] = m[4*2+0] * x + m[4*2+1] * y + m[4*2+2] * z;
00115 }
00116 
00117 inline void PoseIdentity(OpenHRP::DblArray12& pose)
00118 {
00119     pose[0] = 1; pose[1] = 0; pose[2] = 0; pose[3] = 0;
00120     pose[4] = 0; pose[5] = 1; pose[6] = 0; pose[7] = 0;
00121     pose[8] = 0; pose[9] = 0; pose[10] = 1; pose[11] = 0;
00122 }
00123 
00124 inline void PoseInverse(OpenHRP::DblArray12& poseinv, const OpenHRP::DblArray12& pose)
00125 {
00126     COLLADA_ASSERT((void*)&poseinv != (void*)&pose);
00127     poseinv[0] = pose[0]; poseinv[1] = pose[4]; poseinv[2] = pose[8]; poseinv[3] = -pose[3]*pose[0]-pose[7]*pose[4]-pose[11]*pose[8];
00128     poseinv[4] = pose[1]; poseinv[5] = pose[5]; poseinv[6] = pose[9]; poseinv[7] = -pose[3]*pose[1]-pose[7]*pose[5]-pose[11]*pose[9];
00129     poseinv[8] = pose[2]; poseinv[9] = pose[6]; poseinv[10] = pose[10]; poseinv[11] = -pose[3]*pose[2]-pose[7]*pose[6]-pose[11]*pose[10];
00130 }
00131 
00132 template <typename T>
00133 inline void PoseMult(OpenHRP::DblArray12& mres, const T& m0, const OpenHRP::DblArray12& m1)
00134 {
00135     COLLADA_ASSERT((void*)&mres != (void*)&m0 && (void*)&mres != (void*)&m1);
00136     mres[0*4+0] = m0[0*4+0]*m1[0*4+0]+m0[0*4+1]*m1[1*4+0]+m0[0*4+2]*m1[2*4+0];
00137     mres[0*4+1] = m0[0*4+0]*m1[0*4+1]+m0[0*4+1]*m1[1*4+1]+m0[0*4+2]*m1[2*4+1];
00138     mres[0*4+2] = m0[0*4+0]*m1[0*4+2]+m0[0*4+1]*m1[1*4+2]+m0[0*4+2]*m1[2*4+2];
00139     mres[1*4+0] = m0[1*4+0]*m1[0*4+0]+m0[1*4+1]*m1[1*4+0]+m0[1*4+2]*m1[2*4+0];
00140     mres[1*4+1] = m0[1*4+0]*m1[0*4+1]+m0[1*4+1]*m1[1*4+1]+m0[1*4+2]*m1[2*4+1];
00141     mres[1*4+2] = m0[1*4+0]*m1[0*4+2]+m0[1*4+1]*m1[1*4+2]+m0[1*4+2]*m1[2*4+2];
00142     mres[2*4+0] = m0[2*4+0]*m1[0*4+0]+m0[2*4+1]*m1[1*4+0]+m0[2*4+2]*m1[2*4+0];
00143     mres[2*4+1] = m0[2*4+0]*m1[0*4+1]+m0[2*4+1]*m1[1*4+1]+m0[2*4+2]*m1[2*4+1];
00144     mres[2*4+2] = m0[2*4+0]*m1[0*4+2]+m0[2*4+1]*m1[1*4+2]+m0[2*4+2]*m1[2*4+2];
00145     mres[3] = m1[3] * m0[0] + m1[7] * m0[1] + m1[11] * m0[2] + m0[3];
00146     mres[7] = m1[3] * m0[4] + m1[7] * m0[5] + m1[11] * m0[6] + m0[7];
00147     mres[11] = m1[3] * m0[8] + m1[7] * m0[9] + m1[11] * m0[10] + m0[11];
00148 }
00149 
00150 template <typename T>
00151 inline void QuatFromMatrix(OpenHRP::DblArray4& quat, const T& mat)
00152 {
00153     dReal tr = mat[4*0+0] + mat[4*1+1] + mat[4*2+2];
00154     if (tr >= 0) {
00155         quat[0] = tr + 1;
00156         quat[1] = (mat[4*2+1] - mat[4*1+2]);
00157         quat[2] = (mat[4*0+2] - mat[4*2+0]);
00158         quat[3] = (mat[4*1+0] - mat[4*0+1]);
00159     }
00160     else {
00161         // find the largest diagonal element and jump to the appropriate case
00162         if (mat[4*1+1] > mat[4*0+0]) {
00163             if (mat[4*2+2] > mat[4*1+1]) {
00164                 quat[3] = (mat[4*2+2] - (mat[4*0+0] + mat[4*1+1])) + 1;
00165                 quat[1] = (mat[4*2+0] + mat[4*0+2]);
00166                 quat[2] = (mat[4*1+2] + mat[4*2+1]);
00167                 quat[0] = (mat[4*1+0] - mat[4*0+1]);
00168             }
00169             else {
00170                 quat[2] = (mat[4*1+1] - (mat[4*2+2] + mat[4*0+0])) + 1;
00171                 quat[3] = (mat[4*1+2] + mat[4*2+1]);
00172                 quat[1] = (mat[4*0+1] + mat[4*1+0]);
00173                 quat[0] = (mat[4*0+2] - mat[4*2+0]);
00174             }
00175         }
00176         else if (mat[4*2+2] > mat[4*0+0]) {
00177             quat[3] = (mat[4*2+2] - (mat[4*0+0] + mat[4*1+1])) + 1;
00178             quat[1] = (mat[4*2+0] + mat[4*0+2]);
00179             quat[2] = (mat[4*1+2] + mat[4*2+1]);
00180             quat[0] = (mat[4*1+0] - mat[4*0+1]);
00181         }
00182         else {
00183             quat[1] = (mat[4*0+0] - (mat[4*1+1] + mat[4*2+2])) + 1;
00184             quat[2] = (mat[4*0+1] + mat[4*1+0]);
00185             quat[3] = (mat[4*2+0] + mat[4*0+2]);
00186             quat[0] = (mat[4*2+1] - mat[4*1+2]);
00187         }
00188     }
00189     dReal fnorm = std::sqrt(quat[0]*quat[0]+quat[1]*quat[1]+quat[2]*quat[2]+quat[3]*quat[3]);
00190     // don't touch the divides
00191     quat[0] /= fnorm;
00192     quat[1] /= fnorm;
00193     quat[2] /= fnorm;
00194     quat[3] /= fnorm;
00195 }
00196 
00197 inline void AxisAngleFromQuat(OpenHRP::DblArray4& rotation, const OpenHRP::DblArray4& quat)
00198 {
00199     dReal sinang = quat[1]*quat[1]+quat[2]*quat[2]+quat[3]*quat[3];
00200     if( sinang == 0 ) {
00201         rotation[0] = 1;
00202         rotation[1] = 0;
00203         rotation[2] = 0;
00204         rotation[3] = 0;
00205     }
00206     else {
00207         dReal _quat[4];
00208         if( quat[0] < 0 ) {
00209             for(int i = 0; i < 4; ++i) {
00210                 _quat[i] = -quat[i];
00211             }
00212         }
00213         else {
00214             for(int i = 0; i < 4; ++i) {
00215                 _quat[i] = quat[i];
00216             }
00217         }
00218         sinang = sqrt(sinang);
00219         rotation[0] = _quat[1]/sinang;
00220         rotation[1] = _quat[2]/sinang;
00221         rotation[2] = _quat[3]/sinang;
00222         rotation[3] = 2.0*atan2(sinang,_quat[0]);
00223     }
00224 }
00225 
00226 template <typename T>
00227 inline void QuatFromAxisAngle(OpenHRP::DblArray4& quat, const T& rotation, dReal fanglemult=1)
00228 {
00229     dReal axislen = sqrt(rotation[0]*rotation[0]+rotation[1]*rotation[1]+rotation[2]*rotation[2]);
00230     if( axislen == 0 ) {
00231         quat[0] = 1;
00232         quat[1] = 0;
00233         quat[2] = 0;
00234         quat[3] = 0;
00235     }
00236     else {
00237         dReal angle = rotation[3] * 0.5*fanglemult;
00238         dReal sang = sin(angle)/axislen;
00239         quat[0] = cos(angle);
00240         quat[1] = rotation[0]*sang;
00241         quat[2] = rotation[1]*sang;
00242         quat[3] = rotation[2]*sang;
00243     }
00244 }
00245 
00246 inline void PoseFromQuat(OpenHRP::DblArray12& pose, const OpenHRP::DblArray4& quat)
00247 {
00248     dReal qq1 = 2*quat[1]*quat[1];
00249     dReal qq2 = 2*quat[2]*quat[2];
00250     dReal qq3 = 2*quat[3]*quat[3];
00251     pose[4*0+0] = 1 - qq2 - qq3;
00252     pose[4*0+1] = 2*(quat[1]*quat[2] - quat[0]*quat[3]);
00253     pose[4*0+2] = 2*(quat[1]*quat[3] + quat[0]*quat[2]);
00254     pose[4*0+3] = 0;
00255     pose[4*1+0] = 2*(quat[1]*quat[2] + quat[0]*quat[3]);
00256     pose[4*1+1] = 1 - qq1 - qq3;
00257     pose[4*1+2] = 2*(quat[2]*quat[3] - quat[0]*quat[1]);
00258     pose[4*1+3] = 0;
00259     pose[4*2+0] = 2*(quat[1]*quat[3] - quat[0]*quat[2]);
00260     pose[4*2+1] = 2*(quat[2]*quat[3] + quat[0]*quat[1]);
00261     pose[4*2+2] = 1 - qq1 - qq2;
00262     pose[4*2+3] = 0;
00263 }
00264 
00265 inline void PoseFromAxisAngleTranslation(OpenHRP::DblArray12& pose, const OpenHRP::DblArray4& rotation, const OpenHRP::DblArray3& translation)
00266 {
00267     OpenHRP::DblArray4 quat;
00268     QuatFromAxisAngle(quat,rotation);
00269     PoseFromQuat(pose,quat);
00270     pose[3] = translation[0];
00271     pose[7] = translation[1];
00272     pose[11] = translation[2];
00273 }
00274 
00275 inline void AxisAngleTranslationFromPose(OpenHRP::DblArray4& rotation, OpenHRP::DblArray3& translation, const OpenHRP::DblArray12& pose)
00276 {
00277     OpenHRP::DblArray4 quat;
00278     QuatFromMatrix(quat,pose);
00279     AxisAngleFromQuat(rotation,quat);
00280     translation[0] = pose[3];
00281     translation[1] = pose[7];
00282     translation[2] = pose[11];
00283 }
00284 
00285 inline std::vector<dReal> toVector(const OpenHRP::DblSequence& seq)
00286 {
00287     std::vector<dReal> v;
00288     v.resize(seq.length());
00289     for(size_t i = 0; i < v.size(); ++i) {
00290         v[i] = seq[i];
00291     }
00292     return v;
00293 }
00294 
00295 // returns a lower case version of the string
00296 inline std::string tolowerstring(const std::string & s)
00297 {
00298     std::string d = s;
00299     std::transform(d.begin(), d.end(), d.begin(), ::tolower);
00300     return d;
00301 }
00302 
00303 }
00304 
00306 namespace boost
00307 {
00308     inline void assertion_failed(char const * expr, char const * function, char const * file, long line) {
00309         throw OpenHRP::ModelLoader::ModelLoaderException(boost::str(boost::format("[%s:%d] -> %s, expr: %s")%file%line%function%expr).c_str());
00310     }
00311 }
00312 
00313 #endif


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