ColladaUtil.h
Go to the documentation of this file.
1 // -*- coding: utf-8 -*-
2 // Copyright (C) 2011 University of Tokyo, General Robotix Inc.
3 //
4 // Licensed under the Apache License, Version 2.0 (the "License");
5 // you may not use this file except in compliance with the License.
6 // You may obtain a copy of the License at
7 // http://www.apache.org/licenses/LICENSE-2.0
8 //
9 // Unless required by applicable law or agreed to in writing, software
10 // distributed under the License is distributed on an "AS IS" BASIS,
11 // WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
12 // See the License for the specific language governing permissions and
13 // limitations under the License.
21 #ifndef OPENHRP_COLLADA_UTIL_H
22 #define OPENHRP_COLLADA_UTIL_H
23 
24 #include <dae.h>
25 #include <dae/daeErrorHandler.h>
26 #include <1.5/dom/domCOLLADA.h>
27 #include <dae/domAny.h>
28 #include <1.5/dom/domConstants.h>
29 #include <1.5/dom/domTriangles.h>
30 #include <dae/daeDocument.h>
31 #include <1.5/dom/domTypes.h>
32 #include <1.5/dom/domImage.h>
33 #include <1.5/dom/domElements.h>
34 #include <1.5/dom/domKinematics.h>
35 #include <dae/daeStandardURIResolver.h>
36 #include <locale>
37 #include <string>
38 #include <vector>
39 #include <list>
40 #include <map>
41 #include <list>
42 #include <vector>
43 #include <sstream>
44 
45 #if defined(WIN32) || defined(_WIN32) || defined(__WIN32__) || defined(__NT__)
46 # if (BOOST_VERSION > 104400)
47 # define BOOST_ENABLE_ASSERT_HANDLER
48 # endif
49 #else
50 # define BOOST_ENABLE_ASSERT_HANDLER
51 #endif
52 
53 #include <boost/date_time/posix_time/posix_time.hpp>
54 #include <boost/date_time/time_facet.hpp>
55 #include <boost/algorithm/string.hpp>
56 #include <boost/format.hpp>
57 #include <boost/shared_ptr.hpp>
58 #include <boost/array.hpp>
59 #include <boost/lexical_cast.hpp>
60 
61 #include <hrpCorba/ORBwrap.h>
62 #include <hrpCorba/ModelLoader.hh>
63 
64 #ifdef _MSC_VER
65 #ifndef __PRETTY_FUNCTION__
66 #define __PRETTY_FUNCTION__ __FUNCDNAME__
67 #endif
68 #endif
69 
70 #ifndef __PRETTY_FUNCTION__
71 #define __PRETTY_FUNCTION__ __func__
72 #endif
73 
74 #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()); } }
75 
76 namespace ColladaUtil
77 {
78 typedef double dReal;
79 
80 // logging functions
81 inline void COLLADALOG_VERBOSE(const std::string& s) {
82  //std::cout << "Collada Verbose: " << s << std::endl;
83 }
84 inline void COLLADALOG_DEBUG(const std::string& s) {
85  //std::cout << "Collada Debug: " << s << std::endl;
86 }
87 inline void COLLADALOG_INFO(const std::string& s) {
88  std::cout << "Collada Info: " << s << std::endl;
89 }
90 inline void COLLADALOG_WARN(const std::string& s) {
91  std::cout << "Collada Warning: " << s << std::endl;
92 }
93 inline void COLLADALOG_ERROR(const std::string& s) {
94  std::cout << "Collada Error: " << s << std::endl;
95 }
96 
97 //
98 // openrave math functions (from geometry.h)
99 //
100 
101 inline void PoseMultVector(OpenHRP::DblArray3& vnew, const OpenHRP::DblArray12& m, const OpenHRP::DblArray3& v)
102 {
103  dReal x = v[0], y = v[1], z = v[2];
104  vnew[0] = m[4*0+0] * x + m[4*0+1] * y + m[4*0+2] * z + m[4*0+3];
105  vnew[1] = m[4*1+0] * x + m[4*1+1] * y + m[4*1+2] * z + m[4*1+3];
106  vnew[2] = m[4*2+0] * x + m[4*2+1] * y + m[4*2+2] * z + m[4*2+3];
107 }
108 
109 inline void PoseRotateVector(OpenHRP::DblArray3& vnew, const OpenHRP::DblArray12& m, const OpenHRP::DblArray3& v)
110 {
111  dReal x = v[0], y = v[1], z = v[2];
112  vnew[0] = m[4*0+0] * x + m[4*0+1] * y + m[4*0+2] * z;
113  vnew[1] = m[4*1+0] * x + m[4*1+1] * y + m[4*1+2] * z;
114  vnew[2] = m[4*2+0] * x + m[4*2+1] * y + m[4*2+2] * z;
115 }
116 
117 inline void PoseIdentity(OpenHRP::DblArray12& pose)
118 {
119  pose[0] = 1; pose[1] = 0; pose[2] = 0; pose[3] = 0;
120  pose[4] = 0; pose[5] = 1; pose[6] = 0; pose[7] = 0;
121  pose[8] = 0; pose[9] = 0; pose[10] = 1; pose[11] = 0;
122 }
123 
124 inline void PoseInverse(OpenHRP::DblArray12& poseinv, const OpenHRP::DblArray12& pose)
125 {
126  COLLADA_ASSERT((void*)&poseinv != (void*)&pose);
127  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];
128  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];
129  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];
130 }
131 
132 template <typename T>
133 inline void PoseMult(OpenHRP::DblArray12& mres, const T& m0, const OpenHRP::DblArray12& m1)
134 {
135  COLLADA_ASSERT((void*)&mres != (void*)&m0 && (void*)&mres != (void*)&m1);
136  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];
137  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];
138  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];
139  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];
140  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];
141  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];
142  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];
143  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];
144  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];
145  mres[3] = m1[3] * m0[0] + m1[7] * m0[1] + m1[11] * m0[2] + m0[3];
146  mres[7] = m1[3] * m0[4] + m1[7] * m0[5] + m1[11] * m0[6] + m0[7];
147  mres[11] = m1[3] * m0[8] + m1[7] * m0[9] + m1[11] * m0[10] + m0[11];
148 }
149 
150 template <typename T>
151 inline void QuatFromMatrix(OpenHRP::DblArray4& quat, const T& mat)
152 {
153  dReal tr = mat[4*0+0] + mat[4*1+1] + mat[4*2+2];
154  if (tr >= 0) {
155  quat[0] = tr + 1;
156  quat[1] = (mat[4*2+1] - mat[4*1+2]);
157  quat[2] = (mat[4*0+2] - mat[4*2+0]);
158  quat[3] = (mat[4*1+0] - mat[4*0+1]);
159  }
160  else {
161  // find the largest diagonal element and jump to the appropriate case
162  if (mat[4*1+1] > mat[4*0+0]) {
163  if (mat[4*2+2] > mat[4*1+1]) {
164  quat[3] = (mat[4*2+2] - (mat[4*0+0] + mat[4*1+1])) + 1;
165  quat[1] = (mat[4*2+0] + mat[4*0+2]);
166  quat[2] = (mat[4*1+2] + mat[4*2+1]);
167  quat[0] = (mat[4*1+0] - mat[4*0+1]);
168  }
169  else {
170  quat[2] = (mat[4*1+1] - (mat[4*2+2] + mat[4*0+0])) + 1;
171  quat[3] = (mat[4*1+2] + mat[4*2+1]);
172  quat[1] = (mat[4*0+1] + mat[4*1+0]);
173  quat[0] = (mat[4*0+2] - mat[4*2+0]);
174  }
175  }
176  else if (mat[4*2+2] > mat[4*0+0]) {
177  quat[3] = (mat[4*2+2] - (mat[4*0+0] + mat[4*1+1])) + 1;
178  quat[1] = (mat[4*2+0] + mat[4*0+2]);
179  quat[2] = (mat[4*1+2] + mat[4*2+1]);
180  quat[0] = (mat[4*1+0] - mat[4*0+1]);
181  }
182  else {
183  quat[1] = (mat[4*0+0] - (mat[4*1+1] + mat[4*2+2])) + 1;
184  quat[2] = (mat[4*0+1] + mat[4*1+0]);
185  quat[3] = (mat[4*2+0] + mat[4*0+2]);
186  quat[0] = (mat[4*2+1] - mat[4*1+2]);
187  }
188  }
189  dReal fnorm = std::sqrt(quat[0]*quat[0]+quat[1]*quat[1]+quat[2]*quat[2]+quat[3]*quat[3]);
190  // don't touch the divides
191  quat[0] /= fnorm;
192  quat[1] /= fnorm;
193  quat[2] /= fnorm;
194  quat[3] /= fnorm;
195 }
196 
197 inline void AxisAngleFromQuat(OpenHRP::DblArray4& rotation, const OpenHRP::DblArray4& quat)
198 {
199  dReal sinang = quat[1]*quat[1]+quat[2]*quat[2]+quat[3]*quat[3];
200  if( sinang == 0 ) {
201  rotation[0] = 1;
202  rotation[1] = 0;
203  rotation[2] = 0;
204  rotation[3] = 0;
205  }
206  else {
207  dReal _quat[4];
208  if( quat[0] < 0 ) {
209  for(int i = 0; i < 4; ++i) {
210  _quat[i] = -quat[i];
211  }
212  }
213  else {
214  for(int i = 0; i < 4; ++i) {
215  _quat[i] = quat[i];
216  }
217  }
218  sinang = sqrt(sinang);
219  rotation[0] = _quat[1]/sinang;
220  rotation[1] = _quat[2]/sinang;
221  rotation[2] = _quat[3]/sinang;
222  rotation[3] = 2.0*atan2(sinang,_quat[0]);
223  }
224 }
225 
226 template <typename T>
227 inline void QuatFromAxisAngle(OpenHRP::DblArray4& quat, const T& rotation, dReal fanglemult=1)
228 {
229  dReal axislen = sqrt(rotation[0]*rotation[0]+rotation[1]*rotation[1]+rotation[2]*rotation[2]);
230  if( axislen == 0 ) {
231  quat[0] = 1;
232  quat[1] = 0;
233  quat[2] = 0;
234  quat[3] = 0;
235  }
236  else {
237  dReal angle = rotation[3] * 0.5*fanglemult;
238  dReal sang = sin(angle)/axislen;
239  quat[0] = cos(angle);
240  quat[1] = rotation[0]*sang;
241  quat[2] = rotation[1]*sang;
242  quat[3] = rotation[2]*sang;
243  }
244 }
245 
246 inline void PoseFromQuat(OpenHRP::DblArray12& pose, const OpenHRP::DblArray4& quat)
247 {
248  dReal qq1 = 2*quat[1]*quat[1];
249  dReal qq2 = 2*quat[2]*quat[2];
250  dReal qq3 = 2*quat[3]*quat[3];
251  pose[4*0+0] = 1 - qq2 - qq3;
252  pose[4*0+1] = 2*(quat[1]*quat[2] - quat[0]*quat[3]);
253  pose[4*0+2] = 2*(quat[1]*quat[3] + quat[0]*quat[2]);
254  pose[4*0+3] = 0;
255  pose[4*1+0] = 2*(quat[1]*quat[2] + quat[0]*quat[3]);
256  pose[4*1+1] = 1 - qq1 - qq3;
257  pose[4*1+2] = 2*(quat[2]*quat[3] - quat[0]*quat[1]);
258  pose[4*1+3] = 0;
259  pose[4*2+0] = 2*(quat[1]*quat[3] - quat[0]*quat[2]);
260  pose[4*2+1] = 2*(quat[2]*quat[3] + quat[0]*quat[1]);
261  pose[4*2+2] = 1 - qq1 - qq2;
262  pose[4*2+3] = 0;
263 }
264 
265 inline void PoseFromAxisAngleTranslation(OpenHRP::DblArray12& pose, const OpenHRP::DblArray4& rotation, const OpenHRP::DblArray3& translation)
266 {
267  OpenHRP::DblArray4 quat;
268  QuatFromAxisAngle(quat,rotation);
269  PoseFromQuat(pose,quat);
270  pose[3] = translation[0];
271  pose[7] = translation[1];
272  pose[11] = translation[2];
273 }
274 
275 inline void AxisAngleTranslationFromPose(OpenHRP::DblArray4& rotation, OpenHRP::DblArray3& translation, const OpenHRP::DblArray12& pose)
276 {
277  OpenHRP::DblArray4 quat;
278  QuatFromMatrix(quat,pose);
279  AxisAngleFromQuat(rotation,quat);
280  translation[0] = pose[3];
281  translation[1] = pose[7];
282  translation[2] = pose[11];
283 }
284 
285 inline std::vector<dReal> toVector(const OpenHRP::DblSequence& seq)
286 {
287  std::vector<dReal> v;
288  v.resize(seq.length());
289  for(size_t i = 0; i < v.size(); ++i) {
290  v[i] = seq[i];
291  }
292  return v;
293 }
294 
295 // returns a lower case version of the string
296 inline std::string tolowerstring(const std::string & s)
297 {
298  std::string d = s;
299  std::transform(d.begin(), d.end(), d.begin(), ::tolower);
300  return d;
301 }
302 
303 }
304 
306 namespace boost
307 {
308  inline void assertion_failed(char const * expr, char const * function, char const * file, long line) {
309  throw OpenHRP::ModelLoader::ModelLoaderException(boost::str(boost::format("[%s:%d] -> %s, expr: %s")%file%line%function%expr).c_str());
310  }
311 }
312 
313 #endif
void AxisAngleFromQuat(OpenHRP::DblArray4 &rotation, const OpenHRP::DblArray4 &quat)
Definition: ColladaUtil.h:197
void PoseInverse(OpenHRP::DblArray12 &poseinv, const OpenHRP::DblArray12 &pose)
Definition: ColladaUtil.h:124
std::string tolowerstring(const std::string &s)
Definition: ColladaUtil.h:296
Modifications controlling boost library behavior.
Definition: ColladaUtil.h:306
void COLLADALOG_DEBUG(const std::string &s)
Definition: ColladaUtil.h:84
void PoseMult(OpenHRP::DblArray12 &mres, const T &m0, const OpenHRP::DblArray12 &m1)
Definition: ColladaUtil.h:133
* x
Definition: IceUtils.h:98
void COLLADALOG_ERROR(const std::string &s)
Definition: ColladaUtil.h:93
std::vector< dReal > toVector(const OpenHRP::DblSequence &seq)
Definition: ColladaUtil.h:285
void PoseRotateVector(OpenHRP::DblArray3 &vnew, const OpenHRP::DblArray12 &m, const OpenHRP::DblArray3 &v)
Definition: ColladaUtil.h:109
png_uint_32 i
Definition: png.h:2735
#define COLLADA_ASSERT(b)
Definition: ColladaUtil.h:74
double dReal
Definition: ColladaUtil.h:78
void PoseMultVector(OpenHRP::DblArray3 &vnew, const OpenHRP::DblArray12 &m, const OpenHRP::DblArray3 &v)
Definition: ColladaUtil.h:101
void PoseFromAxisAngleTranslation(OpenHRP::DblArray12 &pose, const OpenHRP::DblArray4 &rotation, const OpenHRP::DblArray3 &translation)
Definition: ColladaUtil.h:265
void AxisAngleTranslationFromPose(OpenHRP::DblArray4 &rotation, OpenHRP::DblArray3 &translation, const OpenHRP::DblArray12 &pose)
Definition: ColladaUtil.h:275
void PoseIdentity(OpenHRP::DblArray12 &pose)
Definition: ColladaUtil.h:117
void COLLADALOG_WARN(const std::string &s)
Definition: ColladaUtil.h:90
void COLLADALOG_INFO(const std::string &s)
Definition: ColladaUtil.h:87
void QuatFromMatrix(OpenHRP::DblArray4 &quat, const T &mat)
Definition: ColladaUtil.h:151
void assertion_failed(char const *expr, char const *function, char const *file, long line)
Definition: ColladaUtil.h:308
void PoseFromQuat(OpenHRP::DblArray12 &pose, const OpenHRP::DblArray4 &quat)
Definition: ColladaUtil.h:246
void QuatFromAxisAngle(OpenHRP::DblArray4 &quat, const T &rotation, dReal fanglemult=1)
Definition: ColladaUtil.h:227
* y
Definition: IceUtils.h:97
void COLLADALOG_VERBOSE(const std::string &s)
Definition: ColladaUtil.h:81


openhrp3
Author(s): AIST, General Robotix Inc., Nakamura Lab of Dept. of Mechano Informatics at University of Tokyo
autogenerated on Sat Apr 13 2019 02:14:20