BodyInfoCollada_impl.cpp
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 #include "ColladaUtil.h"
00022 #include "BodyInfoCollada_impl.h"
00023 #include "hrpUtil/UrlUtil.h"
00024 #include "hrpCorba/ViewSimulator.hh"
00025 
00026 #include <sys/stat.h> // for checkInlineFileUpdateTime
00027 
00028 #include <boost/typeof/typeof.hpp>
00029 #define FOREACH(it, v) for(BOOST_TYPEOF((v).begin()) it = (v).begin(); it != (v).end(); (it)++)
00030 
00031 using namespace std;
00032 using namespace ColladaUtil;
00033 
00034 namespace {
00035     typedef map<string, string> SensorTypeMap;
00036     SensorTypeMap sensorTypeMap;
00037 }
00038 
00039 void operator<<(std::ostream& os, const OpenHRP::DblArray12& ttemp) {
00040     OpenHRP::DblArray4 quat;
00041     QuatFromMatrix(quat, ttemp);
00042     os << ttemp[3] << " " << ttemp[7] << " " << ttemp[11] << " / ";
00043     os << quat[0] << " " << quat[1] << " " << quat[2] << " " << quat[3];
00044 }
00045 
00046 #define printArrayWARN(name, ary) \
00047   COLLADALOG_WARN(str(boost::format(name": %f\t%f\t%f\t%f")%ary[0]%ary[1]%ary[2]%ary[3])); \
00048   COLLADALOG_WARN(str(boost::format(name": %f\t%f\t%f\t%f")%ary[4]%ary[5]%ary[6]%ary[7])); \
00049   COLLADALOG_WARN(str(boost::format(name": %f\t%f\t%f\t%f")%ary[8]%ary[9]%ary[10]%ary[11]));
00050 #define printArrayDEBUG(name, ary) \
00051   COLLADALOG_DEBUG(str(boost::format(name": %f\t%f\t%f\t%f")%ary[0]%ary[1]%ary[2]%ary[3])); \
00052   COLLADALOG_DEBUG(str(boost::format(name": %f\t%f\t%f\t%f")%ary[4]%ary[5]%ary[6]%ary[7])); \
00053   COLLADALOG_DEBUG(str(boost::format(name": %f\t%f\t%f\t%f")%ary[8]%ary[9]%ary[10]%ary[11]));
00054 #define printArray printArrayDEBUG
00055 
00056 
00057 
00058 class ColladaReader : public daeErrorHandler
00059 {
00060     class JointAxisBinding
00061     {
00062     public:
00063         JointAxisBinding(daeElementRef pvisualtrans, domAxis_constraintRef pkinematicaxis, domCommon_float_or_paramRef jointvalue, domKinematics_axis_infoRef kinematics_axis_info, domMotion_axis_infoRef motion_axis_info) : pvisualtrans(pvisualtrans), pkinematicaxis(pkinematicaxis), jointvalue(jointvalue), kinematics_axis_info(kinematics_axis_info), motion_axis_info(motion_axis_info) {
00064             COLLADA_ASSERT( !!pkinematicaxis );
00065             daeElement* pae = pvisualtrans->getParentElement();
00066             while (!!pae) {
00067                 visualnode = daeSafeCast<domNode> (pae);
00068                 if (!!visualnode) {
00069                     break;
00070                 }
00071                 pae = pae->getParentElement();
00072             }
00073         
00074             if (!visualnode) {
00075                 COLLADALOG_WARN(str(boost::format("couldn't find parent node of element id %s, sid %s")%pkinematicaxis->getID()%pkinematicaxis->getSid()));
00076             }
00077         }
00078         
00079         daeElementRef pvisualtrans;
00080         domAxis_constraintRef   pkinematicaxis;
00081         domCommon_float_or_paramRef jointvalue;
00082         domNodeRef visualnode;
00083         domKinematics_axis_infoRef kinematics_axis_info;
00084         domMotion_axis_infoRef motion_axis_info;
00085     };
00086 
00087     class LinkBinding
00088     {
00089     public:
00090         domNodeRef node;
00091         domLinkRef domlink;
00092         domInstance_rigid_bodyRef irigidbody;
00093         domRigid_bodyRef rigidbody;
00094         domNodeRef nodephysicsoffset;
00095     };
00096 
00097     class ConstraintBinding
00098     {
00099     public:
00100         domNodeRef node;
00101         domInstance_rigid_constraintRef irigidconstraint;
00102         domRigid_constraintRef rigidconstraint;
00103     };
00104 
00106     class KinematicsSceneBindings
00107     {
00108     public:
00109         std::list< std::pair<domNodeRef,domInstance_kinematics_modelRef> > listKinematicsVisualBindings;
00110         std::list<JointAxisBinding> listAxisBindings;
00111         std::list<LinkBinding> listLinkBindings;
00112         std::list<ConstraintBinding> listConstraintBindings;
00113 
00114         bool AddAxisInfo(const domInstance_kinematics_model_Array& arr, domKinematics_axis_infoRef kinematics_axis_info, domMotion_axis_infoRef motion_axis_info)
00115         {
00116             if( !kinematics_axis_info ) {
00117                 return false;
00118             }
00119             for(size_t ik = 0; ik < arr.getCount(); ++ik) {
00120                 daeElement* pelt = daeSidRef(kinematics_axis_info->getAxis(), arr[ik]->getUrl().getElement()).resolve().elt;
00121                 if( !!pelt ) {
00122                     // look for the correct placement
00123                     bool bfound = false;
00124                     for(std::list<JointAxisBinding>::iterator itbinding = listAxisBindings.begin(); itbinding != listAxisBindings.end(); ++itbinding) {
00125                         if( itbinding->pkinematicaxis.cast() == pelt ) {
00126                             itbinding->kinematics_axis_info = kinematics_axis_info;
00127                             if( !!motion_axis_info ) {
00128                                 itbinding->motion_axis_info = motion_axis_info;
00129                             }
00130                             bfound = true;
00131                             break;
00132                         }
00133                     }
00134                     if( !bfound ) {
00135                         COLLADALOG_WARN(str(boost::format("could not find binding for axis: %s, %s")%kinematics_axis_info->getAxis()%pelt->getAttribute("sid")));
00136                         return false;
00137                     }
00138                     return true;
00139                 }
00140             }
00141             COLLADALOG_WARN(str(boost::format("could not find kinematics axis target: %s")%kinematics_axis_info->getAxis()));
00142             return false;
00143         }
00144     };
00145 
00146  public:
00147     ColladaReader() : _dom(NULL), _nGlobalSensorId(0), _nGlobalActuatorId(0), _nGlobalManipulatorId(0), _nGlobalIndex(0) {
00148         daeErrorHandler::setErrorHandler(this);
00149         _bOpeningZAE = false;
00150     }
00151     virtual ~ColladaReader() {
00152         _dae.reset();
00153         //DAE::cleanup();
00154     }
00155 
00156     bool InitFromURL(const string& url)
00157     {
00158         string filename( deleteURLScheme( url ) );
00159 
00160         // URL文字列の' \' 区切り子を'/' に置き換え  Windows ファイルパス対応  
00161         string url2;
00162         url2 = filename;
00163         for(size_t i = 0; i < url2.size(); ++i) {
00164             if( url2[i] == '\\' ) {
00165                 url2[i] = '/';
00166             }
00167         }
00168         filename = url2;
00169 
00170         COLLADALOG_VERBOSE(str(boost::format("init COLLADA reader version: %s, namespace: %s, filename: %s")%COLLADA_VERSION%COLLADA_NAMESPACE%filename));
00171 
00172         _dae.reset(new DAE);
00173         _bOpeningZAE = filename.find(".zae") == filename.size()-4;
00174         _dom = daeSafeCast<domCOLLADA>(_dae->open(filename));
00175         _bOpeningZAE = false;
00176         if (!_dom) {
00177             return false;
00178         }
00179         _filename=filename;
00180         return _Init();
00181     }
00182 
00183     bool InitFromData(const string& pdata)
00184     {
00185         COLLADALOG_DEBUG(str(boost::format("init COLLADA reader version: %s, namespace: %s")%COLLADA_VERSION%COLLADA_NAMESPACE));
00186         _dae.reset(new DAE);
00187         _dom = daeSafeCast<domCOLLADA>(_dae->openFromMemory(".",pdata.c_str()));
00188         if (!_dom) {
00189             return false;
00190         }
00191         return _Init();
00192     }
00193 
00194     bool _Init()
00195     {
00196         _fGlobalScale = 1;
00197         if( !!_dom->getAsset() ) {
00198             if( !!_dom->getAsset()->getUnit() ) {
00199                 _fGlobalScale = _dom->getAsset()->getUnit()->getMeter();
00200             }
00201         }
00202         return true;
00203     }
00204 
00205     void _ResetRobotCache()
00206     {
00207         _mapJointUnits.clear();
00208         _mapJointIds.clear();
00209         _mapLinkNames.clear();
00210         _veclinks.clear();
00211         _veclinknames.clear();
00212     }
00213 
00215     bool Extract(BodyInfoCollada_impl* probot)
00216     {
00217         std::list< pair<domInstance_kinematics_modelRef, boost::shared_ptr<KinematicsSceneBindings> > > listPossibleBodies;
00218         domCOLLADA::domSceneRef allscene = _dom->getScene();
00219         if( !allscene ) {
00220             return false;
00221         }
00222 
00223         // fill asset info into robot->info_
00224         //"Step 2 of the Project S"
00225         //"Author  : Hajime Saito, General Robotix, Inc."
00226         //"Date    : 2005.11.01"
00227         //"Modified: 2007.09.29"
00228         COLLADALOG_WARN("fill asset info");
00229         _ResetRobotCache();
00230         
00231         //  parse each instance kinematics scene, prioritize robots
00232         for (size_t iscene = 0; iscene < allscene->getInstance_kinematics_scene_array().getCount(); iscene++) {
00233             domInstance_kinematics_sceneRef kiscene = allscene->getInstance_kinematics_scene_array()[iscene];
00234             domKinematics_sceneRef kscene = daeSafeCast<domKinematics_scene> (kiscene->getUrl().getElement().cast());
00235             if (!kscene) {
00236                 continue;
00237             }
00238             boost::shared_ptr<KinematicsSceneBindings> bindings(new KinematicsSceneBindings());
00239             _ExtractKinematicsVisualBindings(allscene->getInstance_visual_scene(),kiscene,*bindings);
00240             _ExtractPhysicsBindings(allscene,*bindings);
00241             for(size_t ias = 0; ias < kscene->getInstance_articulated_system_array().getCount(); ++ias) {
00242                 if( ExtractArticulatedSystem(probot, kscene->getInstance_articulated_system_array()[ias], *bindings) && !!probot ) {
00243                     PostProcess(probot);
00244                     return true;
00245                 }
00246             }
00247             for(size_t ikmodel = 0; ikmodel < kscene->getInstance_kinematics_model_array().getCount(); ++ikmodel) {
00248                 listPossibleBodies.push_back(make_pair(kscene->getInstance_kinematics_model_array()[ikmodel], bindings));
00249             }
00250         }
00251 
00252         for(std::list< pair<domInstance_kinematics_modelRef, boost::shared_ptr<KinematicsSceneBindings> > >::iterator it = listPossibleBodies.begin(); it != listPossibleBodies.end(); ++it) {
00253             if( ExtractKinematicsModel(probot, it->first, *it->second) ) {
00254                 PostProcess(probot);
00255                 return true;
00256             }
00257         }
00258 
00259 
00260         // add left-over visual objects
00261         if (!!allscene->getInstance_visual_scene()) {
00262             domVisual_sceneRef viscene = daeSafeCast<domVisual_scene>(allscene->getInstance_visual_scene()->getUrl().getElement().cast());
00263             for (size_t node = 0; node < viscene->getNode_array().getCount(); node++) {
00264                 boost::shared_ptr<KinematicsSceneBindings> bindings(new KinematicsSceneBindings());
00265                 if ( ExtractKinematicsModel(probot, viscene->getNode_array()[node],*bindings,std::vector<std::string>()) ) {
00266                     PostProcess(probot);
00267                     return true;
00268                 }
00269             }
00270         }
00271 
00272         return false;
00273     }
00274 
00275     void PostProcess(BodyInfoCollada_impl* probot)
00276     {
00277         probot->links_.length(_veclinks.size());
00278         probot->linkShapeIndices_.length(_veclinks.size()); 
00279         for(size_t i = 0; i < _veclinks.size(); ++i) {
00280             probot->links_[i] = *_veclinks[i];
00281             probot->linkShapeIndices_[i] = probot->links_[i].shapeIndices;
00282 
00283             //
00284             // segments
00285             int numSegment = 1;
00286             probot->links_[i].segments.length(numSegment);
00287             for ( int s = 0; s < numSegment; ++s ) {
00288               SegmentInfo_var segmentInfo(new SegmentInfo());
00289               Matrix44 T;
00290               hrp::calcRodrigues(T, Vector3(0,0,1), 0.0);
00291               int p = 0;
00292               for(int row=0; row < 3; ++row){
00293                 for(int col=0; col < 4; ++col){
00294                   segmentInfo->transformMatrix[p++] = T(row, col);
00295                 }
00296               }
00297               segmentInfo->name = _veclinknames[i].c_str();
00298               segmentInfo->shapeIndices.length(probot->links_[i].shapeIndices.length());
00299               for(unsigned int j = 0; j < probot->links_[i].shapeIndices.length(); j++ ) {
00300                 segmentInfo->shapeIndices[j] = j;
00301               }
00302               segmentInfo->mass = probot->links_[i].mass;
00303               segmentInfo->centerOfMass[0] = probot->links_[i].centerOfMass[0];
00304               segmentInfo->centerOfMass[1] = probot->links_[i].centerOfMass[1];
00305               segmentInfo->centerOfMass[2] = probot->links_[i].centerOfMass[2];
00306               for(int j = 0; j < 9 ; j++ ) {
00307                   segmentInfo->inertia[j] = probot->links_[i].inertia[j];
00308               }
00309               probot->links_[i].segments[s] = segmentInfo;
00310             }
00311         }
00312         //applyTriangleMeshShaper(modelNodeSet.humanoidNode());
00313     
00314         // build coldetModels 
00315         probot->linkColdetModels.resize(_veclinks.size());
00316         for(size_t linkIndex = 0; linkIndex < _veclinks.size(); ++linkIndex) {
00317             ColdetModelPtr coldetModel(new ColdetModel());
00318             coldetModel->setName(std::string(probot->links_[linkIndex].name));
00319             int vertexIndex = 0;
00320             int triangleIndex = 0;
00321         
00322             Matrix44 E(Matrix44::Identity());
00323             const TransformedShapeIndexSequence& shapeIndices = probot->linkShapeIndices_[linkIndex];
00324             probot->setColdetModel(coldetModel, shapeIndices, E, vertexIndex, triangleIndex);
00325 
00326             Matrix44 T(Matrix44::Identity());
00327             const SensorInfoSequence& sensors = probot->links_[linkIndex].sensors;
00328             for (unsigned int i=0; i<sensors.length(); i++){
00329                 const SensorInfo& sensor = sensors[i];
00330                 calcRodrigues(T, Vector3(sensor.rotation[0], sensor.rotation[1], 
00331                                          sensor.rotation[2]), sensor.rotation[3]);
00332                 T(0,3) = sensor.translation[0];
00333                 T(1,3) = sensor.translation[1];
00334                 T(2,3) = sensor.translation[2];
00335                 const TransformedShapeIndexSequence& sensorShapeIndices = sensor.shapeIndices;
00336                 probot->setColdetModel(coldetModel, sensorShapeIndices, T, vertexIndex, triangleIndex);
00337             }          
00338             if(triangleIndex>0) {
00339                 coldetModel->build();
00340             }
00341             probot->linkColdetModels[linkIndex] = coldetModel;
00342             probot->links_[linkIndex].AABBmaxDepth = coldetModel->getAABBTreeDepth();
00343             probot->links_[linkIndex].AABBmaxNum = coldetModel->getAABBmaxNum();
00344         }
00345     }
00346 
00349     bool ExtractArticulatedSystem(BodyInfoCollada_impl*& probot, domInstance_articulated_systemRef ias, KinematicsSceneBindings& bindings)
00350     {
00351         if( !ias ) {
00352             return false;
00353         }
00354         //COLLADALOG_DEBUG(str(boost::format("instance articulated system sid %s")%ias->getSid()));
00355         domArticulated_systemRef articulated_system = daeSafeCast<domArticulated_system> (ias->getUrl().getElement().cast());
00356         if( !articulated_system ) {
00357             return false;
00358         }
00359         if( !probot ) {
00360             boost::shared_ptr<std::string> pinterface_type = _ExtractInterfaceType(ias->getExtra_array());
00361             if( !pinterface_type ) {
00362                 pinterface_type = _ExtractInterfaceType(articulated_system->getExtra_array());
00363             }
00364             if( !!pinterface_type ) {
00365                 COLLADALOG_WARN(str(boost::format("need to create a robot with type %s")%*(pinterface_type)));
00366             }
00367             _ResetRobotCache();
00368             return false;
00369         }
00370         if( probot->url_.size() == 0 ) {
00371             probot->url_ = _filename;
00372         }
00373         if( probot->name_.size() == 0 && !!ias->getName() ) {
00374             probot->name_ = CORBA::string_dup(ias->getName());
00375         }
00376         if( probot->name_.size() == 0 && !!ias->getSid()) {
00377             probot->name_ = CORBA::string_dup(ias->getSid());
00378         }
00379         if( probot->name_.size() == 0 && !!articulated_system->getName() ) {
00380             probot->name_ = CORBA::string_dup(articulated_system->getName());
00381         }
00382         if( probot->name_.size() == 0 && !!articulated_system->getId()) {
00383             probot->name_ = CORBA::string_dup(articulated_system->getId());
00384         }
00385 
00386         if( !!articulated_system->getMotion() ) {
00387             domInstance_articulated_systemRef ias_new = articulated_system->getMotion()->getInstance_articulated_system();
00388             if( !!articulated_system->getMotion()->getTechnique_common() ) {
00389                 for(size_t i = 0; i < articulated_system->getMotion()->getTechnique_common()->getAxis_info_array().getCount(); ++i) {
00390                     domMotion_axis_infoRef motion_axis_info = articulated_system->getMotion()->getTechnique_common()->getAxis_info_array()[i];
00391                     // this should point to a kinematics axis_info
00392                     domKinematics_axis_infoRef kinematics_axis_info = daeSafeCast<domKinematics_axis_info>(daeSidRef(motion_axis_info->getAxis(), ias_new->getUrl().getElement()).resolve().elt);
00393                     if( !!kinematics_axis_info ) {
00394                         // find the parent kinematics and go through all its instance kinematics models
00395                         daeElement* pparent = kinematics_axis_info->getParent();
00396                         while(!!pparent && pparent->typeID() != domKinematics::ID()) {
00397                             pparent = pparent->getParent();
00398                         }
00399                         COLLADA_ASSERT(!!pparent);
00400                         bindings.AddAxisInfo(daeSafeCast<domKinematics>(pparent)->getInstance_kinematics_model_array(), kinematics_axis_info, motion_axis_info);
00401                     }
00402                     else {
00403                         COLLADALOG_WARN(str(boost::format("failed to find kinematics axis %s")%motion_axis_info->getAxis()));
00404                     }
00405                 }
00406             }
00407             if( !ExtractArticulatedSystem(probot,ias_new,bindings) ) {
00408                 return false;
00409             }
00410         }
00411         else {
00412             if( !articulated_system->getKinematics() ) {
00413                 COLLADALOG_WARN(str(boost::format("collada <kinematics> tag empty? instance_articulated_system=%s")%ias->getID()));
00414                 return true;
00415             }
00416 
00417             if( !!articulated_system->getKinematics()->getTechnique_common() ) {
00418                 for(size_t i = 0; i < articulated_system->getKinematics()->getTechnique_common()->getAxis_info_array().getCount(); ++i) {
00419                     bindings.AddAxisInfo(articulated_system->getKinematics()->getInstance_kinematics_model_array(), articulated_system->getKinematics()->getTechnique_common()->getAxis_info_array()[i], NULL);
00420                 }
00421             }
00422 
00423             // parse the kinematics information
00424             if (!probot) {
00425                 // create generic robot?
00426                 return false;
00427             }
00428 
00429             for(size_t ik = 0; ik < articulated_system->getKinematics()->getInstance_kinematics_model_array().getCount(); ++ik) {
00430                 ExtractKinematicsModel(probot,articulated_system->getKinematics()->getInstance_kinematics_model_array()[ik],bindings);
00431             }
00432         }
00433 
00434         ExtractRobotManipulators(probot, articulated_system);
00435         ExtractRobotAttachedSensors(probot, articulated_system);
00436         ExtractRobotAttachedActuators(probot, articulated_system);
00437         return true;
00438     }
00439 
00440     bool ExtractKinematicsModel(BodyInfoCollada_impl* pkinbody, domInstance_kinematics_modelRef ikm, KinematicsSceneBindings& bindings)
00441     {
00442         if( !ikm ) {
00443             return false;
00444         }
00445         COLLADALOG_DEBUG(str(boost::format("instance kinematics model sid %s")%ikm->getSid()));
00446         domKinematics_modelRef kmodel = daeSafeCast<domKinematics_model> (ikm->getUrl().getElement().cast());
00447         if (!kmodel) {
00448             COLLADALOG_WARN(str(boost::format("%s does not reference valid kinematics")%ikm->getSid()));
00449             return false;
00450         }
00451         domPhysics_modelRef pmodel;
00452         if( !pkinbody ) {
00453             boost::shared_ptr<std::string> pinterface_type = _ExtractInterfaceType(ikm->getExtra_array());
00454             if( !pinterface_type ) {
00455                 pinterface_type = _ExtractInterfaceType(kmodel->getExtra_array());
00456             }
00457             if( !!pinterface_type ) {
00458                 COLLADALOG_WARN(str(boost::format("need to create a robot with type %s")%*pinterface_type));
00459             }
00460             _ResetRobotCache();
00461             return false;
00462         }
00463         if( pkinbody->url_.size() == 0 ) {
00464             pkinbody->url_ = _filename;
00465         }
00466 
00467         // find matching visual node
00468         domNodeRef pvisualnode;
00469         for(std::list< std::pair<domNodeRef,domInstance_kinematics_modelRef> >::iterator it = bindings.listKinematicsVisualBindings.begin(); it != bindings.listKinematicsVisualBindings.end(); ++it) {
00470             if( it->second == ikm ) {
00471                 pvisualnode = it->first;
00472                 break;
00473             }
00474         }
00475         if( !pvisualnode ) {
00476             COLLADALOG_WARN(str(boost::format("failed to find visual node for instance kinematics model %s")%ikm->getSid()));
00477             return false;
00478         }
00479 
00480         if( pkinbody->name_.size() == 0 && !!ikm->getName() ) {
00481             pkinbody->name_ = CORBA::string_dup(ikm->getName());
00482         }
00483         if( pkinbody->name_.size() == 0 && !!ikm->getID() ) {
00484             pkinbody->name_ = CORBA::string_dup(ikm->getID());
00485         }
00486 
00487         if (!ExtractKinematicsModel(pkinbody, kmodel, pvisualnode, pmodel, bindings)) {
00488             COLLADALOG_WARN(str(boost::format("failed to load kinbody from kinematics model %s")%kmodel->getID()));
00489             return false;
00490         }
00491         return true;
00492     }
00493 
00495     bool ExtractKinematicsModel(BodyInfoCollada_impl* pkinbody, domNodeRef pdomnode, const KinematicsSceneBindings& bindings, const std::vector<std::string>& vprocessednodes)
00496     {
00497         if( !!pdomnode->getID() && find(vprocessednodes.begin(),vprocessednodes.end(),pdomnode->getID()) != vprocessednodes.end() ) {
00498             COLLADALOG_WARN(str(boost::format("could not create kinematics type pnode getid %s")%pdomnode->getID()));
00499             return false;
00500         }
00501         _ResetRobotCache();
00502         string name = !pdomnode->getName() ? "" : _ConvertToValidName(pdomnode->getName());
00503         if( name.size() == 0 ) {
00504             name = _ConvertToValidName(pdomnode->getID());
00505         }
00506         boost::shared_ptr<LinkInfo> plink(new LinkInfo());
00507         _veclinks.push_back(plink);
00508         plink->jointId = -1;
00509         plink->jointType = CORBA::string_dup("fixed");
00510         plink->parentIndex = -1;
00511         plink->name = CORBA::string_dup(name.c_str());
00512         DblArray12 tlink; PoseIdentity(tlink);
00513         
00514         //  Gets the geometry
00515         bool bhasgeometry = ExtractGeometry(pkinbody,plink,tlink,pdomnode,bindings.listAxisBindings,vprocessednodes);
00516         if( !bhasgeometry ) {
00517             COLLADALOG_WARN(str(boost::format("could not extract geometry %s")%name));
00518             return false;
00519         }
00520 
00521         COLLADALOG_INFO(str(boost::format("Loading non-kinematics node '%s'")%name));
00522         pkinbody->name_ = name;
00523         return pkinbody;
00524     }
00525 
00527     bool ExtractKinematicsModel(BodyInfoCollada_impl* pkinbody, domKinematics_modelRef kmodel, domNodeRef pnode, domPhysics_modelRef pmodel, const KinematicsSceneBindings bindings)
00528     {
00529         vector<domJointRef> vdomjoints;
00530         if (!pkinbody) {
00531             _ResetRobotCache();
00532         }
00533         if( pkinbody->name_.size() == 0 && !!kmodel->getName() ) {
00534             pkinbody->name_ = CORBA::string_dup(kmodel->getName());
00535         }
00536         if( pkinbody->name_.size() == 0 && !!kmodel->getID() ) {
00537             pkinbody->name_ = CORBA::string_dup(kmodel->getID());
00538         }
00539         COLLADALOG_DEBUG(str(boost::format("kinematics model: %s")%pkinbody->name_));
00540         if( !!pnode ) {
00541             COLLADALOG_DEBUG(str(boost::format("node name: %s")%pnode->getId()));
00542         }
00543         if( !kmodel->getID() ) {
00544             COLLADALOG_DEBUG(str(boost::format("kinematics model: %s has no id attribute!")%pkinbody->name_));
00545         }
00546 
00547         //  Process joint of the kinbody
00548         domKinematics_model_techniqueRef ktec = kmodel->getTechnique_common();
00549 
00550         //  Store joints
00551         for (size_t ijoint = 0; ijoint < ktec->getJoint_array().getCount(); ++ijoint) {
00552             vdomjoints.push_back(ktec->getJoint_array()[ijoint]);
00553         }
00554 
00555         //  Store instances of joints
00556         for (size_t ijoint = 0; ijoint < ktec->getInstance_joint_array().getCount(); ++ijoint) {
00557             domJointRef pelt = daeSafeCast<domJoint> (ktec->getInstance_joint_array()[ijoint]->getUrl().getElement());
00558             if (!pelt) {
00559                 COLLADALOG_WARN("failed to get joint from instance");
00560             }
00561             else {
00562                 vdomjoints.push_back(pelt);
00563             }
00564         }
00565 
00566         COLLADALOG_VERBOSE(str(boost::format("Number of root links in the kmodel %d")%ktec->getLink_array().getCount()));
00567         DblArray12 identity;
00568         PoseIdentity(identity);
00569         for (size_t ilink = 0; ilink < ktec->getLink_array().getCount(); ++ilink) {
00570             domLinkRef pdomlink = ktec->getLink_array()[ilink];
00571             //
00572             _ExtractFullTransform(rootOrigin, pdomlink);
00573             printArray("rootOrigin", rootOrigin);
00574             domNodeRef pvisualnode;
00575             FOREACH(it, bindings.listKinematicsVisualBindings) {
00576               if(strcmp(it->first->getName() ,pdomlink->getName()) == 0) {
00577                 pvisualnode = it->first;
00578                 break;
00579               }
00580             }
00581             if (!!pvisualnode) {
00582               getNodeParentTransform(visualRootOrigin, pvisualnode);
00583               printArray("visualRootOrigin", visualRootOrigin);
00584             }
00585             //
00586             DblArray12 identity;
00587             PoseIdentity(identity);
00588             int linkindex = ExtractLink(pkinbody, pdomlink, ilink == 0 ? pnode : domNodeRef(),
00589                                         identity, identity, vdomjoints, bindings);
00590 
00591             boost::shared_ptr<LinkInfo> plink = _veclinks.at(linkindex);
00592             AxisAngleTranslationFromPose(plink->rotation,plink->translation,rootOrigin);
00593         }
00594 
00595         for (size_t iform = 0; iform < ktec->getFormula_array().getCount(); ++iform) {
00596             domFormulaRef pf = ktec->getFormula_array()[iform];
00597             if (!pf->getTarget()) {
00598                 COLLADALOG_WARN("formula target not valid");
00599                 continue;
00600             }
00601 
00602             // find the target joint
00603             boost::shared_ptr<LinkInfo>  pjoint = _getJointFromRef(pf->getTarget()->getParam()->getValue(),pf,pkinbody).first;
00604             if (!pjoint) {
00605                 continue;
00606             }
00607 
00608             int iaxis = 0;
00609             dReal ftargetunit = 1;
00610             if(_mapJointUnits.find(pjoint) != _mapJointUnits.end() ) {
00611                 ftargetunit = _mapJointUnits[pjoint].at(iaxis);
00612             }
00613 
00614             daeTArray<daeElementRef> children;
00615             pf->getTechnique_common()->getChildren(children);
00616 
00617             domTechniqueRef popenravetec = _ExtractOpenRAVEProfile(pf->getTechnique_array());
00618             if( !!popenravetec ) {
00619                 for(size_t ic = 0; ic < popenravetec->getContents().getCount(); ++ic) {
00620                     daeElementRef pequation = popenravetec->getContents()[ic];
00621                     if( pequation->getElementName() == string("equation") ) {
00622                         if( !pequation->hasAttribute("type") ) {
00623                             COLLADALOG_WARN("equaiton needs 'type' attribute, ignoring");
00624                             continue;
00625                         }
00626                         if( children.getCount() != 1 ) {
00627                             COLLADALOG_WARN("equaiton needs exactly one child");
00628                             continue;
00629                         }
00630                         std::string equationtype = pequation->getAttribute("type");
00631                         boost::shared_ptr<LinkInfo>  pjointtarget;
00632                         if( pequation->hasAttribute("target") ) {
00633                             pjointtarget = _getJointFromRef(pequation->getAttribute("target").c_str(),pf,pkinbody).first;
00634                         }
00635                         try {
00636                             std::string eq = _ExtractMathML(pf,pkinbody,children[0]);
00637                             if( ftargetunit != 1 ) {
00638                                 eq = str(boost::format("%f*(%s)")%ftargetunit%eq);
00639                             }
00640                             if( equationtype == "position" ) {
00641                                 COLLADALOG_WARN(str(boost::format("cannot set joint %s position equation: %s!")%pjoint->name%eq));
00642                             }
00643                             else if( equationtype == "first_partial" ) {
00644                                 if( !pjointtarget ) {
00645                                     COLLADALOG_WARN(str(boost::format("first_partial equation '%s' needs a target attribute! ignoring...")%eq));
00646                                     continue;
00647                                 }
00648                                 COLLADALOG_WARN(str(boost::format("cannot set joint %s first partial equation: d %s=%s!")%pjoint->name%pjointtarget->name%eq));
00649                             }
00650                             else if( equationtype == "second_partial" ) {
00651                                 if( !pjointtarget ) {
00652                                     COLLADALOG_WARN(str(boost::format("second_partial equation '%s' needs a target attribute! ignoring...")%eq));
00653                                     continue;
00654                                 }
00655                                 COLLADALOG_WARN(str(boost::format("cannot set joint %s second partial equation: d^2 %s = %s!")%pjoint->name%pjointtarget->name%eq));
00656                             }
00657                             else {
00658                                 COLLADALOG_WARN(str(boost::format("unknown equation type %s")%equationtype));
00659                             }
00660                         }
00661                         catch(const ModelLoader::ModelLoaderException& ex) {
00662                             COLLADALOG_WARN(str(boost::format("failed to parse formula %s for target %s")%equationtype%pjoint->name));
00663                         }
00664                     }
00665                 }
00666             }
00667             else if (!!pf->getTechnique_common()) {
00668                 try {
00669                     for(size_t ic = 0; ic < children.getCount(); ++ic) {
00670                         string eq = _ExtractMathML(pf,pkinbody,children[ic]);
00671                         if( ftargetunit != 1 ) {
00672                             eq = str(boost::format("%f*(%s)")%ftargetunit%eq);
00673                         }
00674                         if( eq.size() > 0 ) {
00675                             COLLADALOG_WARN(str(boost::format("cannot set joint %s position equation: %s!")%pjoint->name%eq));
00676                             break;
00677                         }
00678                     }
00679                 }
00680                 catch(const ModelLoader::ModelLoaderException& ex) {
00681                     COLLADALOG_WARN(str(boost::format("failed to parse formula for target %s: %s")%pjoint->name%ex.description));
00682                 }
00683             }
00684         }
00685 
00686         // read the collision data
00687         for(size_t ie = 0; ie < kmodel->getExtra_array().getCount(); ++ie) {
00688             domExtraRef pextra = kmodel->getExtra_array()[ie];
00689             if( strcmp(pextra->getType(), "collision") == 0 ) {
00690                 domTechniqueRef tec = _ExtractOpenRAVEProfile(pextra->getTechnique_array());
00691                 if( !!tec ) {
00692                     for(size_t ic = 0; ic < tec->getContents().getCount(); ++ic) {
00693                         daeElementRef pelt = tec->getContents()[ic];
00694                         if( pelt->getElementName() == string("ignore_link_pair") ) {
00695                             domLinkRef pdomlink0 = daeSafeCast<domLink>(daeSidRef(pelt->getAttribute("link0"), kmodel).resolve().elt);
00696                             domLinkRef pdomlink1 = daeSafeCast<domLink>(daeSidRef(pelt->getAttribute("link1"), kmodel).resolve().elt);
00697                             if( !pdomlink0 || !pdomlink1 ) {
00698                                 COLLADALOG_WARN(str(boost::format("failed to reference <ignore_link_pair> links: %s %s")%pelt->getAttribute("link0")%pelt->getAttribute("link1")));
00699                                 continue;
00700                             }
00701                             COLLADALOG_INFO(str(boost::format("need to specifying ignore link pair %s:%s")%_ExtractLinkName(pdomlink0)%_ExtractLinkName(pdomlink1)));
00702                         }
00703                         else if( pelt->getElementName() == string("bind_instance_geometry") ) {
00704                             COLLADALOG_WARN("currently do not support bind_instance_geometry");
00705                         }
00706                     }
00707                 }
00708             }
00709         }
00710         return true;
00711     }
00712 
00713     boost::shared_ptr<LinkInfo> GetLink(const std::string& name)
00714     {
00715         for(std::map<std::string,boost::shared_ptr<LinkInfo> >::iterator it = _mapJointIds.begin(); it != _mapJointIds.end(); ++it) {
00716             string linkname(CORBA::String_var(it->second->name));
00717             if( linkname == name ) {
00718                 return it->second;
00719             }
00720         }
00721         return boost::shared_ptr<LinkInfo>();
00722     }
00723 
00725     int  ExtractLink(BodyInfoCollada_impl* pkinbody, const domLinkRef pdomlink,const domNodeRef pdomnode, const DblArray12& tParentWorldLink, const DblArray12& tParentLink, const std::vector<domJointRef>& vdomjoints, const KinematicsSceneBindings bindings) {
00726         const std::list<JointAxisBinding>& listAxisBindings = bindings.listAxisBindings;
00727 
00728         //  Set link name with the name of the COLLADA's Link
00729         std::string linkname;
00730         if( !!pdomlink ) {
00731             linkname = _ExtractLinkName(pdomlink);
00732             if( linkname.size() == 0 ) {
00733                 COLLADALOG_WARN("<link> has no name or id, falling back to <node>!");
00734             }
00735         }
00736         if( linkname.size() == 0 ) {
00737             if( !!pdomnode ) {
00738                 if (!!pdomnode->getName()) {
00739                     linkname = _ConvertToValidName(pdomnode->getName());
00740                 }
00741                 if( linkname.size() == 0 && !!pdomnode->getID()) {
00742                     linkname = _ConvertToValidName(pdomnode->getID());
00743                 }
00744             }
00745         }
00746 
00747         boost::shared_ptr<LinkInfo>  plink(new LinkInfo());
00748         plink->parentIndex = -1;
00749         plink->jointId = -1;
00750         plink->mass = 1;
00751         plink->centerOfMass[0] = plink->centerOfMass[1] = plink->centerOfMass[2] = 0;
00752         plink->inertia[0] = plink->inertia[4] = plink->inertia[8] = 1;
00753         plink->jointValue = 0;
00754         _mapLinkNames[linkname] = plink;
00755         plink->name = CORBA::string_dup(linkname.c_str());
00756         plink->jointType = CORBA::string_dup("free");
00757         int ilinkindex = (int)_veclinks.size();
00758         _veclinks.push_back(plink);
00759         _veclinknames.push_back(linkname);
00760 
00761         if( !!pdomnode ) {
00762             COLLADALOG_VERBOSE(str(boost::format("Node Id %s and Name %s")%pdomnode->getId()%pdomnode->getName()));
00763         }
00764 
00765         // physics
00766         domInstance_rigid_bodyRef irigidbody;
00767         domRigid_bodyRef rigidbody;
00768         domInstance_rigid_constraintRef irigidconstraint;
00769         FOREACH(itlinkbinding, bindings.listLinkBindings) {
00770             if( !!pdomnode->getID() && !!itlinkbinding->node->getID() && strcmp(pdomnode->getID(),itlinkbinding->node->getID()) == 0 ) {
00771                 irigidbody = itlinkbinding->irigidbody;
00772                 rigidbody = itlinkbinding->rigidbody;
00773             }
00774         }
00775         FOREACH(itconstraintbinding, bindings.listConstraintBindings) {
00776             if( !!pdomnode->getID() && !!itconstraintbinding->rigidconstraint->getName() && strcmp(linkname.c_str(),itconstraintbinding->rigidconstraint->getName()) == 0 ) {
00777                 plink->jointType = CORBA::string_dup("fixed");
00778             }
00779         }
00780 
00781         if (!pdomlink) {
00782             ExtractGeometry(pkinbody,plink,tParentLink,pdomnode,listAxisBindings,std::vector<std::string>());
00783         }
00784         else {
00785             COLLADALOG_DEBUG(str(boost::format("Attachment link elements: %d")%pdomlink->getAttachment_full_array().getCount()));
00786             // use the kinematics coordinate system for each link
00787             DblArray12 tlink;
00788             _ExtractFullTransform(tlink, pdomlink);
00789             {
00790               DblArray12 tgeomlink;
00791               PoseMult(tgeomlink, tParentWorldLink, tlink);
00792               COLLADALOG_DEBUG(str(boost::format("geom_link:%s")%linkname.c_str()));
00793               printArray("geom", tgeomlink);
00794               ExtractGeometry(pkinbody,plink,tgeomlink,pdomnode,listAxisBindings,std::vector<std::string>());
00795             }
00796             COLLADALOG_DEBUG(str(boost::format("After ExtractGeometry Attachment link elements: %d")%pdomlink->getAttachment_full_array().getCount()));
00797           
00798             if( !!rigidbody && !!rigidbody->getTechnique_common() ) {
00799                 domRigid_body::domTechnique_commonRef rigiddata = rigidbody->getTechnique_common();
00800                 if( !!rigiddata->getMass() ) {
00801                     plink->mass = rigiddata->getMass()->getValue();
00802                 }
00803                 if( !!rigiddata->getInertia() ) {
00804                     plink->inertia[0] = rigiddata->getInertia()->getValue()[0];
00805                     plink->inertia[4] = rigiddata->getInertia()->getValue()[1];
00806                     plink->inertia[8] = rigiddata->getInertia()->getValue()[2];
00807                 }
00808                 if( !!rigiddata->getMass_frame() ) {
00809                      DblArray12 atemp1,atemp2,atemp3,tlocalmass;
00810                      printArray("tlink", tlink);
00811                      printArray("plink", tParentWorldLink);
00812                      PoseMult(atemp1, tParentWorldLink, tlink);
00813                      PoseInverse(atemp2, rootOrigin);
00814                      PoseMult(atemp3, atemp2, atemp1);
00815                      PoseInverse(atemp1, atemp3);
00816                      _ExtractFullTransform(atemp2, rigiddata->getMass_frame());
00817                      PoseMult(tlocalmass, atemp1, atemp2);
00818                      printArray("i_org", tlocalmass);
00819 
00820                      plink->centerOfMass[0] = tlocalmass[3];
00821                      plink->centerOfMass[1] = tlocalmass[7];
00822                      plink->centerOfMass[2] = tlocalmass[11];
00823                      if( !!rigiddata->getInertia() ) {
00824                        DblArray12 temp_i, temp_a, m_inertia, m_result;
00825                        for (int i = 0; i < 12; i++) { m_inertia[i] = 0.0; }
00826                        m_inertia[4*0 + 0] = plink->inertia[0];
00827                        m_inertia[4*1 + 1] = plink->inertia[4];
00828                        m_inertia[4*2 + 2] = plink->inertia[8];
00829                        tlocalmass[3] = 0; tlocalmass[7] = 0; tlocalmass[11] = 0;
00830                        PoseInverse(temp_i, tlocalmass);
00831                        PoseMult(temp_a, tlocalmass, m_inertia);
00832                        PoseMult(m_result, temp_a, temp_i);
00833 
00834                        for(int i = 0; i < 3; i++) {
00835                          for(int j = 0; j < 3; j++) {
00836                            plink->inertia[3*i + j] = m_result[4*i + j];
00837                          }
00838                        }
00839                      }
00840                 }
00841             }
00842 
00843             //  Process all atached links
00844             for (size_t iatt = 0; iatt < pdomlink->getAttachment_full_array().getCount(); ++iatt) {
00845                 domLink::domAttachment_fullRef pattfull = pdomlink->getAttachment_full_array()[iatt];
00846 
00847                 // get link kinematics transformation
00848                 DblArray12 tatt;
00849                 _ExtractFullTransform(tatt,pattfull);
00850 
00851                 // Transform applied to the joint
00852                 // the joint anchor is actually tatt.trans! However, in openhrp3 the link and joint coordinate systems are the same!
00853                 // this means we need to change the coordinate system of the joint and all the attached geometry
00854                 //dReal anchor[3] = {tatt[3],tatt[7],tatt[11]};
00855                 //tatt[3] = 0; tatt[7] = 0; tatt[11] = 0;
00856                 //COLLADALOG_INFO(str(boost::format("tatt: %f %f %f")%anchor[0]%anchor[1]%anchor[2]));
00857 
00858                 // process attached links
00859                 daeElement* peltjoint = daeSidRef(pattfull->getJoint(), pattfull).resolve().elt;
00860                 if( !peltjoint ) {
00861                     COLLADALOG_WARN(str(boost::format("could not find attached joint %s!")%pattfull->getJoint()));
00862                     continue;
00863                 }
00864                 string jointid;
00865                 if( string(pattfull->getJoint()).find("./") == 0 ) {
00866                     jointid = str(boost::format("%s/%s")%_ExtractParentId(pattfull)%&pattfull->getJoint()[1]);
00867                 }
00868                 else {
00869                     jointid = pattfull->getJoint();
00870                 }
00871                     
00872                 domJointRef pdomjoint = daeSafeCast<domJoint> (peltjoint);
00873                 if (!pdomjoint) {
00874                     domInstance_jointRef pdomijoint = daeSafeCast<domInstance_joint> (peltjoint);
00875                     if (!!pdomijoint) {
00876                         pdomjoint = daeSafeCast<domJoint> (pdomijoint->getUrl().getElement().cast());
00877                     }
00878                     else {
00879                         COLLADALOG_WARN(str(boost::format("could not cast element <%s> to <joint>!")%peltjoint->getElementName()));
00880                         continue;
00881                     }
00882                 }
00883                 
00884                 // get direct child link
00885                 if (!pattfull->getLink()) {
00886                     COLLADALOG_WARN(str(boost::format("joint %s needs to be attached to a valid link")%jointid));
00887                     continue;
00888                 }
00889                 
00890                 // find the correct joint in the bindings
00891                 daeTArray<domAxis_constraintRef> vdomaxes = pdomjoint->getChildrenByType<domAxis_constraint>();
00892                 domNodeRef pchildnode;
00893                 
00894                 // see if joint has a binding to a visual node
00895                 for(std::list<JointAxisBinding>::const_iterator itaxisbinding = listAxisBindings.begin(); itaxisbinding != listAxisBindings.end(); ++itaxisbinding) {
00896                     for (size_t ic = 0; ic < vdomaxes.getCount(); ++ic) {
00897                         //  If the binding for the joint axis is found, retrieve the info
00898                         if (vdomaxes[ic] == itaxisbinding->pkinematicaxis) {
00899                             pchildnode = itaxisbinding->visualnode;
00900                             break;
00901                         }
00902                     }
00903                     if( !!pchildnode ) {
00904                         break;
00905                     }
00906                 }
00907                 if (!pchildnode) {
00908                     COLLADALOG_DEBUG(str(boost::format("joint %s has no visual binding")%jointid));
00909                 }
00910 
00911                 
00912                 DblArray12 tnewparent;
00913                 {
00914                   DblArray12 ttemp;
00915                   PoseMult(ttemp, tParentWorldLink, tlink);
00916                   PoseMult(tnewparent, ttemp, tatt);
00917                 }
00918                 int ijointindex = ExtractLink(pkinbody, pattfull->getLink(), pchildnode,
00919                                               tnewparent,
00920                                               tatt, vdomjoints, bindings);
00921                 boost::shared_ptr<LinkInfo> pjoint = _veclinks.at(ijointindex);
00922                 int cindex = plink->childIndices.length();
00923                 plink->childIndices.length(cindex+1);
00924                 plink->childIndices[cindex] = ijointindex;
00925                 pjoint->parentIndex = ilinkindex;
00926 
00927                 if ( ilinkindex == 0 ){ // due to  ExtractKinematicsModel calls ExtractLink with identity
00928                     memcpy(tnewparent, tatt, sizeof(tatt));
00929                 } else {
00930                     DblArray12 ttemp;
00931                     _ExtractFullTransform(ttemp, pattfull->getLink());
00932                     PoseMult(tnewparent, tatt, ttemp);
00933                 }
00934                 AxisAngleTranslationFromPose(pjoint->rotation,pjoint->translation,tnewparent);
00935 
00936                 bool bActive = true; // if not active, put into the passive list
00937 
00938                 if( vdomaxes.getCount() > 1 ) {
00939                     COLLADALOG_WARN(str(boost::format("joint %s has %d degrees of freedom, only 1 DOF is supported")%pjoint->name%vdomaxes.getCount()));
00940                 }
00941                 else if( vdomaxes.getCount() == 0 ) {
00942                     continue;
00943                 }
00944 
00945                 size_t ic = 0;
00946                 std::vector<dReal> vaxisunits(1,dReal(1));
00947                 for(std::list<JointAxisBinding>::const_iterator itaxisbinding = listAxisBindings.begin(); itaxisbinding != listAxisBindings.end(); ++itaxisbinding) {
00948                     if (vdomaxes[ic] == itaxisbinding->pkinematicaxis) {
00949                         if( !!itaxisbinding->kinematics_axis_info ) {
00950                             if( !!itaxisbinding->kinematics_axis_info->getActive() ) {
00951                                 // what if different axes have different active profiles?
00952                                 bActive = resolveBool(itaxisbinding->kinematics_axis_info->getActive(),itaxisbinding->kinematics_axis_info);
00953                             }
00954                         }
00955                         break;
00956                     }
00957                 }
00958                 domAxis_constraintRef pdomaxis = vdomaxes[ic];
00959                 bool bIsRevolute = false;
00960                 if( strcmp(pdomaxis->getElementName(), "revolute") == 0 ) {
00961                     pjoint->jointType = CORBA::string_dup("rotate");
00962                     bIsRevolute = true;
00963                 }
00964                 else if( strcmp(pdomaxis->getElementName(), "prismatic") == 0 ) {
00965                     pjoint->jointType = CORBA::string_dup("slide");
00966                     vaxisunits[ic] = _GetUnitScale(pdomaxis,_fGlobalScale);
00967                 }
00968                 else {
00969                     COLLADALOG_WARN(str(boost::format("unsupported joint type: %s")%pdomaxis->getElementName()));
00970                 }
00971 
00972                 _mapJointUnits[pjoint] = vaxisunits;
00973                 string jointname;
00974                 if( !!pdomjoint->getName() ) {
00975                     jointname = _ConvertToValidName(pdomjoint->getName());
00976                 }
00977                 else {
00978                     jointname = str(boost::format("dummy%d")%pjoint->jointId);
00979                 }
00980                 pjoint->name = CORBA::string_dup(jointname.c_str());
00981                 
00982                 if( _mapJointIds.find(jointid) != _mapJointIds.end() ) {
00983                     COLLADALOG_WARN(str(boost::format("jointid '%s' is duplicated!")%jointid));
00984                 }
00985                 
00986                 int jointsid;
00987                 if ( sscanf(jointid.c_str(), "kmodel1/jointsid%d", &jointsid) ) {
00988                     pjoint->jointId = jointsid;
00989                 } else {
00990                     pjoint->jointId = ijointindex - 1;
00991                 }
00992                 _mapJointIds[jointid] = pjoint;
00993                 COLLADALOG_DEBUG(str(boost::format("joint %s (%d)")%pjoint->name%pjoint->jointId));
00994 
00995                 domKinematics_axis_infoRef kinematics_axis_info;
00996                 domMotion_axis_infoRef motion_axis_info;
00997                 for(std::list<JointAxisBinding>::const_iterator itaxisbinding = listAxisBindings.begin(); itaxisbinding != listAxisBindings.end(); ++itaxisbinding) {
00998                     if (vdomaxes[ic] == itaxisbinding->pkinematicaxis) {
00999                         kinematics_axis_info = itaxisbinding->kinematics_axis_info;
01000                         motion_axis_info = itaxisbinding->motion_axis_info;
01001                         break;
01002                     }
01003                 }
01004 
01005                 //  Axes and Anchor assignment.
01006                 dReal len2 = 0;
01007                 for(int i = 0; i < 3; ++i) {
01008                     len2 += pdomaxis->getAxis()->getValue()[i] * pdomaxis->getAxis()->getValue()[i];
01009                 }
01010                 if( len2 > 0 ) {
01011                     len2 = 1/len2;
01012                     DblArray12  trans_joint_to_child;
01013                     _ExtractFullTransform(trans_joint_to_child, pattfull->getLink());
01014                     DblArray12 patt;
01015                     PoseInverse(patt, trans_joint_to_child);
01016                     len2 = 1/len2;
01017                     double ax = pdomaxis->getAxis()->getValue()[0]*len2;
01018                     double ay = pdomaxis->getAxis()->getValue()[1]*len2;
01019                     double az = pdomaxis->getAxis()->getValue()[2]*len2;
01020                     pjoint->jointAxis[0] = patt[0] * ax + patt[1] * ay + patt[2] * az;
01021                     pjoint->jointAxis[1] = patt[4] * ax + patt[5] * ay + patt[6] * az;
01022                     pjoint->jointAxis[2] = patt[8] * ax + patt[9] * ay + patt[10] * az;
01023                     COLLADALOG_DEBUG(str(boost::format("axis: %f %f %f -> %f %f %f")%ax%ay%az%pjoint->jointAxis[0]%pjoint->jointAxis[1]%pjoint->jointAxis[2]));
01024                 }
01025                 else {
01026                     pjoint->jointAxis[0] = 0;
01027                     pjoint->jointAxis[1] = 0;
01028                     pjoint->jointAxis[2] = 1;
01029                 }
01030                 //  Rotate axis from the parent offset
01031                 //PoseRotateVector(pjoint->jointAxis,tatt,pjoint->jointAxis);
01032                 COLLADALOG_WARN(str(boost::format("joint %s has axis: %f %f %f")%jointname%pjoint->jointAxis[0]%pjoint->jointAxis[1]%pjoint->jointAxis[2]));
01033 
01034                 if( !motion_axis_info ) {
01035                     COLLADALOG_WARN(str(boost::format("No motion axis info for joint %s")%pjoint->name));
01036                 }
01037 
01038                 //  Sets the Speed and the Acceleration of the joint
01039                 if (!!motion_axis_info) {
01040                     if (!!motion_axis_info->getSpeed()) {
01041                         pjoint->lvlimit.length(1);
01042                         pjoint->uvlimit.length(1);
01043                         pjoint->uvlimit[0] = resolveFloat(motion_axis_info->getSpeed(),motion_axis_info);
01044                         pjoint->lvlimit[0] = -pjoint->uvlimit[0];
01045                     }
01046                     if (!!motion_axis_info->getAcceleration()) {
01047                         COLLADALOG_DEBUG("robot has max acceleration info");
01048                     }
01049                 }
01050 
01051                 bool joint_locked = false; // if locked, joint angle is static
01052                 bool kinematics_limits = false; 
01053 
01054                 if (!!kinematics_axis_info) {
01055                     if (!!kinematics_axis_info->getLocked()) {
01056                         joint_locked = resolveBool(kinematics_axis_info->getLocked(),kinematics_axis_info);
01057                     }
01058                         
01059                     if (joint_locked) { // If joint is locked set limits to the static value.
01060                         COLLADALOG_WARN("lock joint!!");
01061                         pjoint->llimit.length(1);
01062                         pjoint->ulimit.length(1);
01063                         pjoint->llimit[ic] = 0;
01064                         pjoint->ulimit[ic] = 0;
01065                     }
01066                     else if (kinematics_axis_info->getLimits()) { // If there are articulated system kinematics limits
01067                         kinematics_limits   = true;
01068                         pjoint->llimit.length(1);
01069                         pjoint->ulimit.length(1);
01070                         dReal fscale = bIsRevolute?(M_PI/180.0f):_GetUnitScale(kinematics_axis_info,_fGlobalScale);
01071                         pjoint->llimit[ic] = fscale*(dReal)(resolveFloat(kinematics_axis_info->getLimits()->getMin(),kinematics_axis_info));
01072                         pjoint->ulimit[ic] = fscale*(dReal)(resolveFloat(kinematics_axis_info->getLimits()->getMax(),kinematics_axis_info));
01073                     }
01074                 }
01075                   
01076                 //  Search limits in the joints section
01077                 if (!kinematics_axis_info || (!joint_locked && !kinematics_limits)) {
01078                     //  If there are NO LIMITS
01079                     if( !!pdomaxis->getLimits() ) {
01080                         dReal fscale = bIsRevolute?(M_PI/180.0f):_GetUnitScale(pdomaxis,_fGlobalScale);
01081                         pjoint->llimit.length(1);
01082                         pjoint->ulimit.length(1);
01083                         pjoint->llimit[ic] = (dReal)pdomaxis->getLimits()->getMin()->getValue()*fscale;
01084                         pjoint->ulimit[ic] = (dReal)pdomaxis->getLimits()->getMax()->getValue()*fscale;
01085                     }
01086                     else {
01087                         COLLADALOG_VERBOSE(str(boost::format("There are NO LIMITS in joint %s:%d ...")%pjoint->name%kinematics_limits));
01088                     }
01089                 }
01090             }
01091             if( pdomlink->getAttachment_start_array().getCount() > 0 ) {
01092                 COLLADALOG_WARN("openrave collada reader does not support attachment_start");
01093             }
01094             if( pdomlink->getAttachment_end_array().getCount() > 0 ) {
01095                 COLLADALOG_WARN("openrave collada reader does not support attachment_end");
01096             }
01097         }
01098         return ilinkindex;
01099     }
01100 
01104     bool ExtractGeometry(BodyInfoCollada_impl* pkinbody, boost::shared_ptr<LinkInfo>  plink, const DblArray12& tlink, const domNodeRef pdomnode, const std::list<JointAxisBinding>& listAxisBindings,const std::vector<std::string>& vprocessednodes)
01105     {
01106         if( !pdomnode ) {
01107             COLLADALOG_WARN(str(boost::format("fail to ExtractGeometry(LinkInfo,plink,tlink,pdomnode) of %s")%plink->name));
01108             return false;
01109         }
01110         if( !!pdomnode->getID() && find(vprocessednodes.begin(),vprocessednodes.end(),pdomnode->getID()) != vprocessednodes.end() ) {
01111             COLLADALOG_WARN(str(boost::format("could not create geometry type pnode getid %s")%pdomnode->getID()));
01112             return false;
01113         }
01114 
01115         COLLADALOG_VERBOSE(str(boost::format("ExtractGeometry(node,link) of %s")%pdomnode->getName()));
01116 
01117         bool bhasgeometry = false;
01118         // For all child nodes of pdomnode
01119         for (size_t i = 0; i < pdomnode->getNode_array().getCount(); i++) {
01120             // check if contains a joint
01121             bool contains=false;
01122             for(std::list<JointAxisBinding>::const_iterator it = listAxisBindings.begin(); it != listAxisBindings.end(); ++it) {
01123                 // don't check ID's check if the reference is the same!
01124                 if ( (pdomnode->getNode_array()[i])  == (it->visualnode)){
01125                     contains=true;
01126                     break;
01127                 }
01128             }
01129             if (contains) {
01130                 continue;
01131             }
01132 
01133             bhasgeometry |= ExtractGeometry(pkinbody, plink, tlink, pdomnode->getNode_array()[i],listAxisBindings,vprocessednodes);
01134             // Plink stayes the same for all children
01135             // replace pdomnode by child = pdomnode->getNode_array()[i]
01136             // hope for the best!
01137             // put everything in a subroutine in order to process pdomnode too!
01138         }
01139 
01140         unsigned int nGeomBefore =  plink->shapeIndices.length(); // #of Meshes already associated to this link
01141 
01142         // get the geometry
01143         for (size_t igeom = 0; igeom < pdomnode->getInstance_geometry_array().getCount(); ++igeom) {
01144             domInstance_geometryRef domigeom = pdomnode->getInstance_geometry_array()[igeom];
01145             domGeometryRef domgeom = daeSafeCast<domGeometry> (domigeom->getUrl().getElement());
01146             if (!domgeom) {
01147                 COLLADALOG_WARN(str(boost::format("link %s does not have valid geometry")%plink->name));
01148                 continue;
01149             }
01150 
01151             //  Gets materials
01152             map<string, int> mapmaterials;
01153             map<string, int> maptextures;
01154             if (!!domigeom->getBind_material() && !!domigeom->getBind_material()->getTechnique_common()) {
01155                 const domInstance_material_Array& matarray = domigeom->getBind_material()->getTechnique_common()->getInstance_material_array();
01156                 for (size_t imat = 0; imat < matarray.getCount(); ++imat) {
01157                     domMaterialRef pdommat = daeSafeCast<domMaterial>(matarray[imat]->getTarget().getElement());
01158                     if (!!pdommat) {
01159                         int mindex = pkinbody->materials_.length();
01160                         pkinbody->materials_.length(mindex+1);
01161                         _FillMaterial(pkinbody->materials_[mindex],pdommat);
01162                         mapmaterials[matarray[imat]->getSymbol()] = mindex;
01163 
01164                         if( !!pdommat && !!pdommat->getInstance_effect() ) {
01165                             domEffectRef peffect = daeSafeCast<domEffect>(pdommat->getInstance_effect()->getUrl().getElement().cast());
01166                             if( !!peffect ) {
01167                                 domProfile_common::domTechnique::domPhongRef pphong = daeSafeCast<domProfile_common::domTechnique::domPhong>(peffect->getDescendant(daeElement::matchType(domProfile_common::domTechnique::domPhong::ID())));
01168                                 if( !!pphong && !!pphong->getDiffuse() && !!pphong->getDiffuse()->getTexture() ) {
01169                                     // search for newparam that matches sid
01170                                     daeElementRefArray psamplers = daeSidRef(pphong->getDiffuse()->getTexture()->getTexture(), peffect).resolve().elt->getChildren();
01171                                     daeElementRef psampler;
01172                                     for (size_t i = 0; i < psamplers.getCount(); ++i ) {
01173                                         if ( strcmp(psamplers[i]->getElementName(),"sampler2D") == 0 ) {
01174                                             psampler = psamplers[i];
01175                                         }
01176                                     }
01177                                     //
01178                                     domInstance_imageRef pinstanceimage;
01179                                     for (size_t i = 0; i < psampler->getChildren().getCount(); ++i ) {
01180                                         if ( strcmp(psampler->getChildren()[i]->getElementName(), "instance_image") == 0 ) {
01181                                             pinstanceimage = daeSafeCast<domInstance_image>(psampler->getChildren()[i]);
01182                                         }
01183                                     }
01184                                     domImageRef image = daeSafeCast<domImage>(pinstanceimage->getUrl().getElement());
01185                                     if ( image && image->getInit_from() && image->getInit_from()->getRef() ) {
01186                                         int tindex = pkinbody->textures_.length();
01187                                         pkinbody->textures_.length(tindex+1);
01188                                         TextureInfo_var texture(new TextureInfo());
01189                                         texture->repeatS = 1;
01190                                         texture->repeatT = 1;
01191                                         texture->url = string(image->getInit_from()->getRef()->getValue().path()).c_str();
01192                                         pkinbody->textures_[tindex] = texture;
01193                                         maptextures[matarray[imat]->getSymbol()] = tindex;
01194 
01195                                     }
01196                                 }
01197                             }
01198                         }
01199                     }
01200                 }
01201             }
01202 
01203             //  Gets the geometry
01204             bhasgeometry |= ExtractGeometry(pkinbody, plink, domgeom, mapmaterials, maptextures);
01205         }
01206 
01207         if( !bhasgeometry ) {
01208             return false;
01209         }
01210 
01211         DblArray12 tmnodegeom, ttemp1, ttemp2, ttemp3;
01212         PoseInverse(ttemp1, visualRootOrigin);
01213         getNodeParentTransform(ttemp2, pdomnode);
01214         PoseMult(ttemp3, ttemp1, ttemp2);
01215         _ExtractFullTransform(ttemp1, pdomnode);
01216         PoseMult(ttemp2, ttemp3, ttemp1);
01217         PoseInverse(ttemp1, tlink);
01218         PoseMult(tmnodegeom, ttemp1, ttemp2);
01219         printArray("tmnodegeom", tmnodegeom);
01220 
01221         // there is a weird bug with the viewer, but should try to normalize the rotation!
01222         DblArray4 quat;
01223         double x=tmnodegeom[3],y=tmnodegeom[7],z=tmnodegeom[11];
01224         QuatFromMatrix(quat, tmnodegeom);
01225         PoseFromQuat(tmnodegeom,quat);
01226         tmnodegeom[3] = x; tmnodegeom[7] = y; tmnodegeom[11] = z;
01227 
01228         // change only the transformations of the newly found geometries.
01229         for(unsigned int i = nGeomBefore; i < plink->shapeIndices.length(); ++i) {
01230             memcpy(plink->shapeIndices[i].transformMatrix,tmnodegeom,sizeof(tmnodegeom));
01231             plink->shapeIndices[i].inlinedShapeTransformMatrixIndex = -1;
01232         }
01233 
01234         return true;
01235     }
01236 
01242     bool _ExtractGeometry(BodyInfoCollada_impl* pkinbody, boost::shared_ptr<LinkInfo> plink, const domTrianglesRef triRef, const domVerticesRef vertsRef, const map<string,int>& mapmaterials, const map<string,int>& maptextures)
01243     {
01244         if( !triRef ) {
01245             COLLADALOG_WARN(str(boost::format("fail to _ExtractGeometry(LinkInfo,Triangles,Vertices) of %s")%plink->name));
01246             return false;
01247         }
01248         int shapeIndex = pkinbody->shapes_.length();
01249         pkinbody->shapes_.length(shapeIndex+1);
01250         ShapeInfo& shape = pkinbody->shapes_[shapeIndex];
01251         shape.primitiveType = SP_MESH;
01252         //shape.url = "";
01253         int lsindex = plink->shapeIndices.length();
01254         plink->shapeIndices.length(lsindex+1);
01255         plink->shapeIndices[lsindex].shapeIndex = shapeIndex;
01256 
01257         int aindex = pkinbody->appearances_.length();
01258         pkinbody->appearances_.length(aindex+1);
01259         AppearanceInfo& ainfo = pkinbody->appearances_[aindex];
01260         ainfo.materialIndex = -1;
01261         ainfo.normalPerVertex = 0;
01262         ainfo.solid = 0;
01263         ainfo.creaseAngle = 0;
01264         ainfo.colorPerVertex = 0;
01265         ainfo.textureIndex = -1;
01266         if( !!triRef->getMaterial() ) {
01267             map<string,int>::const_iterator itmat = mapmaterials.find(triRef->getMaterial());
01268             if( itmat != mapmaterials.end() ) {
01269                 ainfo.materialIndex = itmat->second;
01270             }
01271         }
01272         shape.appearanceIndex = aindex;
01273 
01274         size_t triangleIndexStride = 0;
01275         int vertexoffset = -1, normaloffset = -1, texcoordoffset = -1;
01276         domInput_local_offsetRef indexOffsetRef, normalOffsetRef, texcoordOffsetRef;
01277 
01278         for (unsigned int w=0;w<triRef->getInput_array().getCount();w++) {
01279             size_t offset = triRef->getInput_array()[w]->getOffset();
01280             daeString str = triRef->getInput_array()[w]->getSemantic();
01281             if (!strcmp(str,"VERTEX")) {
01282                 indexOffsetRef = triRef->getInput_array()[w];
01283                 vertexoffset = offset;
01284             }
01285             if (!strcmp(str,"NORMAL")) {
01286                 normalOffsetRef = triRef->getInput_array()[w];
01287                 normaloffset = offset;
01288             }
01289             if (!strcmp(str,"TEXCOORD")) {
01290                 texcoordOffsetRef = triRef->getInput_array()[w];
01291                 texcoordoffset = offset;
01292             }
01293             if (offset> triangleIndexStride) {
01294                 triangleIndexStride = offset;
01295             }
01296         }
01297         triangleIndexStride++;
01298 
01299         const domList_of_uints& indexArray =triRef->getP()->getValue();
01300         shape.triangles.length(triRef->getCount()*3);
01301         shape.vertices.length(triRef->getCount()*9);
01302 
01303         int itriangle = 0;
01304         for (size_t i=0;i<vertsRef->getInput_array().getCount();++i) {
01305             domInput_localRef localRef = vertsRef->getInput_array()[i];
01306             daeString str = localRef->getSemantic();
01307             if ( strcmp(str,"POSITION") == 0 ) {
01308                 const domSourceRef node = daeSafeCast<domSource>(localRef->getSource().getElement());
01309                 if( !node ) {
01310                     continue;
01311                 }
01312                 dReal fUnitScale = _GetUnitScale(node,_fGlobalScale);
01313                 const domFloat_arrayRef flArray = node->getFloat_array();
01314                 if (!!flArray) {
01315                     const domList_of_floats& listFloats = flArray->getValue();
01316                     int k=vertexoffset;
01317                     int vertexStride = 3;//instead of hardcoded stride, should use the 'accessor'
01318                     int ivertex = 0;
01319                     for(size_t itri = 0; itri < triRef->getCount(); ++itri) {
01320                         if(k+2*triangleIndexStride < indexArray.getCount() ) {
01321                             for (int j=0;j<3;j++) {
01322                                 int index0 = indexArray.get(k)*vertexStride;
01323                                 domFloat fl0 = listFloats.get(index0);
01324                                 domFloat fl1 = listFloats.get(index0+1);
01325                                 domFloat fl2 = listFloats.get(index0+2);
01326                                 k+=triangleIndexStride;
01327                                 shape.triangles[itriangle++] = ivertex/3;
01328                                 shape.vertices[ivertex++] = fl0*fUnitScale;
01329                                 shape.vertices[ivertex++] = fl1*fUnitScale;
01330                                 shape.vertices[ivertex++] = fl2*fUnitScale;
01331                             }
01332                         }
01333                     }
01334                 }
01335                 else {
01336                     COLLADALOG_WARN("float array not defined!");
01337                 }
01338 
01339                 if ( normaloffset >= 0 ) {
01340                     const domSourceRef normalNode = daeSafeCast<domSource>(normalOffsetRef->getSource().getElement());
01341                     const domFloat_arrayRef normalArray = normalNode->getFloat_array();
01342                     const domList_of_floats& normalFloats = normalArray->getValue();
01343                     AppearanceInfo& ainfo = pkinbody->appearances_[shape.appearanceIndex];
01344                     ainfo.normalPerVertex = 1;
01345                     ainfo.normals.length(triRef->getCount()*9);
01346                     int k = normaloffset;
01347                     int vertexStride = 3;//instead of hardcoded stride, should use the 'accessor'
01348                     int ivertex = 0;
01349                     for(size_t itri = 0; itri < triRef->getCount(); ++itri) {
01350                         if(k+2*triangleIndexStride < indexArray.getCount() ) {
01351                             for (int j=0;j<3;j++) {
01352                                 int index0 = indexArray.get(k)*vertexStride;
01353                                 domFloat fl0 = normalFloats.get(index0);
01354                                 domFloat fl1 = normalFloats.get(index0+1);
01355                                 domFloat fl2 = normalFloats.get(index0+2);
01356                                 k+=triangleIndexStride;
01357                                 ainfo.normals[ivertex++] = fl0;
01358                                 ainfo.normals[ivertex++] = fl1;
01359                                 ainfo.normals[ivertex++] = fl2;
01360                             }
01361                         }
01362                     }
01363                 }
01364 
01365                 if ( texcoordoffset >= 0 ) {
01366                     const domSourceRef texcoordNode = daeSafeCast<domSource>(texcoordOffsetRef->getSource().getElement());
01367                     const domFloat_arrayRef texcoordArray = texcoordNode->getFloat_array();
01368                     const domList_of_floats& texcoordFloats = texcoordArray->getValue();
01369                     AppearanceInfo& ainfo = pkinbody->appearances_[shape.appearanceIndex];
01370                     map<string,int>::const_iterator itmat = maptextures.find(triRef->getMaterial());
01371                     if( itmat != maptextures.end() ) {
01372                         ainfo.textureIndex = itmat->second;
01373 
01374                         ainfo.textureCoordinate.length(texcoordArray->getCount());
01375                         ainfo.textureCoordIndices.length(triRef->getCount()*3);
01376                         int k = texcoordoffset;
01377                         int itriangle = 0;
01378                         for(size_t itex = 0; itex < texcoordArray->getCount() ; ++itex) {
01379                             ainfo.textureCoordinate[itex] = texcoordFloats.get(itex);
01380                         }
01381                         for(size_t itri = 0; itri < triRef->getCount(); ++itri) {
01382                             if(k+2*triangleIndexStride < indexArray.getCount() ) {
01383                                 for (int j=0;j<3;j++) {
01384                                     int index0 = indexArray.get(k);
01385                                     k+=triangleIndexStride;
01386                                     ainfo.textureCoordIndices[itriangle++]  = index0;
01387                                 }
01388                             }
01389                         }
01390                         for(int i=0,k=0; i<3; i++) {
01391                             for(int j=0; j<3; j++) {
01392                                 if ( i==j )
01393                                     ainfo.textransformMatrix[k++] = 1;
01394                                 else
01395                                     ainfo.textransformMatrix[k++] = 0;
01396                             }
01397                         }
01398                     }
01399                 }
01400             } else if ( strcmp(str,"NORMAL") == 0 ) {
01401                 COLLADALOG_WARN("read normals from collada file");
01402                 const domSourceRef normalNode = daeSafeCast<domSource>(localRef->getSource().getElement());
01403                 if( !normalNode ) {
01404                     continue;
01405                 }
01406                 const domFloat_arrayRef normalArray = normalNode->getFloat_array();
01407                 if (!!normalArray) {
01408                     const domList_of_floats& normalFloats = normalArray->getValue();
01409                     int k = 0;
01410                     int vertexStride = 3;//instead of hardcoded stride, should use the 'accessor'
01411                     int ivertex = 0;
01412                     AppearanceInfo& ainfo = pkinbody->appearances_[shape.appearanceIndex];
01413                     ainfo.normalPerVertex = 1;
01414                     ainfo.normals.length(triRef->getCount()*9);
01415                     for(size_t itri = 0; itri < triRef->getCount(); ++itri) {
01416                         if(2*triangleIndexStride < indexArray.getCount() ) {
01417                             for (int j=0;j<3;j++) {
01418                                 int index0 = indexArray.get(k)*vertexStride;
01419                                 domFloat fl0 = normalFloats.get(index0);
01420                                 domFloat fl1 = normalFloats.get(index0+1);
01421                                 domFloat fl2 = normalFloats.get(index0+2);
01422                                 k+=triangleIndexStride;
01423                                 ainfo.normals[ivertex++] = fl0;
01424                                 ainfo.normals[ivertex++] = fl1;
01425                                 ainfo.normals[ivertex++] = fl2;
01426                             }
01427                         }
01428                     }
01429                 }
01430                 else {
01431                     COLLADALOG_WARN("float array not defined!");
01432                 }
01433             }
01434         }
01435 
01436         if ( ainfo.normals.length() == 0 ) {
01437             COLLADALOG_WARN("generate normals from vertices");
01438             // compute face normals
01439             std::vector<Vector3> faceNormals(itriangle/3);
01440             std::vector<std::vector<int> > vertexFaceMapping(itriangle);
01441             for(int i=0; i < itriangle/3; ++i) {
01442                 int vIndex1 = shape.triangles[i*3+0];
01443                 int vIndex2 = shape.triangles[i*3+1];
01444                 int vIndex3 = shape.triangles[i*3+2];
01445                 Vector3 a(shape.vertices[vIndex1*3+0]-
01446                           shape.vertices[vIndex3*3+0],
01447                           shape.vertices[vIndex1*3+1]-
01448                           shape.vertices[vIndex3*3+1],
01449                           shape.vertices[vIndex1*3+2]-
01450                           shape.vertices[vIndex3*3+2]);
01451                 Vector3 b(shape.vertices[vIndex1*3+0]-
01452                           shape.vertices[vIndex2*3+0],
01453                           shape.vertices[vIndex1*3+1]-
01454                           shape.vertices[vIndex2*3+1],
01455                           shape.vertices[vIndex1*3+2]-
01456                           shape.vertices[vIndex2*3+2]);
01457                 a.normalize();
01458                 b.normalize();
01459                 Vector3 c = a.cross(b);
01460                 faceNormals[i] = c.normalized();
01461                 vertexFaceMapping[vIndex1].push_back(i);
01462                 vertexFaceMapping[vIndex2].push_back(i);
01463                 vertexFaceMapping[vIndex3].push_back(i);
01464             }
01465             // compute vertex normals
01466             ainfo.normalPerVertex = 1;
01467             ainfo.normals.length(itriangle*3); // number_of_tris*3*3
01468             double creaseAngle = M_PI/4;
01469             for (int i=0; i<itriangle/3; i++){ // each triangle
01470                 const Vector3 &currentFaceNormal = faceNormals[i];
01471                 for (int vertex=0; vertex<3; vertex++){ // each vertex
01472                     bool normalIsFaceNormal = true;
01473                     int vIndex = shape.triangles[i*3+vertex];
01474                     Vector3 normal = currentFaceNormal;
01475                     const std::vector<int>& faces = vertexFaceMapping[vIndex];
01476                     for (size_t k=0; k<faces.size(); k++){
01477                         const Vector3& adjoingFaceNormal = faceNormals[k];
01478                         double angle = acos(currentFaceNormal.dot(adjoingFaceNormal) /
01479                                             (currentFaceNormal.norm() * adjoingFaceNormal.norm()));
01480                         if(angle > 1.0e-6 && angle < creaseAngle){
01481                             normal += adjoingFaceNormal;
01482                             normalIsFaceNormal = false;
01483                         }
01484                     }
01485                     if (!normalIsFaceNormal){
01486                         normal.normalize();
01487                     }
01488                     ainfo.normals[vIndex*3  ] = normal[0];
01489                     ainfo.normals[vIndex*3+1] = normal[1];
01490                     ainfo.normals[vIndex*3+2] = normal[2];
01491                 }
01492             }
01493         }
01494 
01495         if( itriangle != 3*(int)triRef->getCount() ) {
01496             COLLADALOG_WARN("triangles declares wrong count!");
01497         }
01498         return true;
01499     }
01500 
01506     bool _ExtractGeometry(BodyInfoCollada_impl* pkinbody, boost::shared_ptr<LinkInfo> plink, const domTrifansRef triRef, const domVerticesRef vertsRef, const map<string,int>& mapmaterials, const map<string,int>& maptextures)
01507     {
01508         if( !triRef ) {
01509             COLLADALOG_WARN(str(boost::format("fail to _ExtractGeometry(LinkInfo,Trifans,Vertices) of %s")%plink->name));
01510             return false;
01511         }
01512         int shapeIndex = pkinbody->shapes_.length();
01513         pkinbody->shapes_.length(shapeIndex+1);
01514         ShapeInfo& shape = pkinbody->shapes_[shapeIndex];
01515         shape.primitiveType = SP_MESH;
01516         int lsindex = plink->shapeIndices.length();
01517         plink->shapeIndices.length(lsindex+1);
01518         plink->shapeIndices[lsindex].shapeIndex = shapeIndex;
01519 
01520         // resolve the material and assign correct colors to the geometry
01521         shape.appearanceIndex = -1;
01522         if( !!triRef->getMaterial() ) {
01523             map<string,int>::const_iterator itmat = mapmaterials.find(triRef->getMaterial());
01524             if( itmat != mapmaterials.end() ) {
01525                 shape.appearanceIndex = itmat->second;
01526             }
01527         }
01528 
01529         size_t triangleIndexStride = 0, vertexoffset = -1;
01530         domInput_local_offsetRef indexOffsetRef;
01531 
01532         for (unsigned int w=0;w<triRef->getInput_array().getCount();w++) {
01533             size_t offset = triRef->getInput_array()[w]->getOffset();
01534             daeString str = triRef->getInput_array()[w]->getSemantic();
01535             if (!strcmp(str,"VERTEX")) {
01536                 indexOffsetRef = triRef->getInput_array()[w];
01537                 vertexoffset = offset;
01538             }
01539             if (offset> triangleIndexStride) {
01540                 triangleIndexStride = offset;
01541             }
01542         }
01543         triangleIndexStride++;
01544         size_t primitivecount = triRef->getCount();
01545         if( primitivecount > triRef->getP_array().getCount() ) {
01546             COLLADALOG_WARN("trifans has incorrect count");
01547             primitivecount = triRef->getP_array().getCount();
01548         }
01549         int itriangle = 0, ivertex = 0;
01550         for(size_t ip = 0; ip < primitivecount; ++ip) {
01551             domList_of_uints indexArray =triRef->getP_array()[ip]->getValue();
01552             for (size_t i=0;i<vertsRef->getInput_array().getCount();++i) {
01553                 domInput_localRef localRef = vertsRef->getInput_array()[i];
01554                 daeString str = localRef->getSemantic();
01555                 if ( strcmp(str,"POSITION") == 0 ) {
01556                     const domSourceRef node = daeSafeCast<domSource>(localRef->getSource().getElement());
01557                     if( !node ) {
01558                         continue;
01559                     }
01560                     dReal fUnitScale = _GetUnitScale(node,_fGlobalScale);
01561                     const domFloat_arrayRef flArray = node->getFloat_array();
01562                     if (!!flArray) {
01563                         const domList_of_floats& listFloats = flArray->getValue();
01564                         int k=vertexoffset;
01565                         int vertexStride = 3;//instead of hardcoded stride, should use the 'accessor'
01566                         size_t usedindices = 3*(indexArray.getCount()-2);
01567                         shape.triangles.length(shape.triangles.length()+usedindices);
01568                         shape.vertices.length(shape.vertices.length()+3*indexArray.getCount());
01569                         size_t startoffset = ivertex/3;
01570                         while(k < (int)indexArray.getCount() ) {
01571                             int index0 = indexArray.get(k)*vertexStride;
01572                             domFloat fl0 = listFloats.get(index0);
01573                             domFloat fl1 = listFloats.get(index0+1);
01574                             domFloat fl2 = listFloats.get(index0+2);
01575                             k+=triangleIndexStride;
01576                             shape.vertices[ivertex++] = fl0*fUnitScale;
01577                             shape.vertices[ivertex++] = fl1*fUnitScale;
01578                             shape.vertices[ivertex++] = fl2*fUnitScale;
01579                         }
01580                         for(size_t ivert = 2; ivert < indexArray.getCount(); ++ivert) {
01581                             shape.triangles[itriangle++] = startoffset;
01582                             shape.triangles[itriangle++] = startoffset+ivert-1;
01583                             shape.triangles[itriangle++] = startoffset+ivert;
01584                         }
01585                     }
01586                     else {
01587                         COLLADALOG_WARN("float array not defined!");
01588                     }
01589                     break;
01590                 }
01591             }
01592         }
01593         return true;
01594     }
01595 
01601     bool _ExtractGeometry(BodyInfoCollada_impl* pkinbody, boost::shared_ptr<LinkInfo> plink, const domTristripsRef triRef, const domVerticesRef vertsRef, const map<string,int>& mapmaterials, const map<string,int>& maptextures)
01602     {
01603         if( !triRef ) {
01604             COLLADALOG_WARN(str(boost::format("fail to _ExtractGeometry(LinkInfo,Tristrips,Vertices) of %s")%plink->name));
01605             return false;
01606         }
01607         int shapeIndex = pkinbody->shapes_.length();
01608         pkinbody->shapes_.length(shapeIndex+1);
01609         ShapeInfo& shape = pkinbody->shapes_[shapeIndex];
01610         shape.primitiveType = SP_MESH;
01611         int lsindex = plink->shapeIndices.length();
01612         plink->shapeIndices.length(lsindex+1);
01613         plink->shapeIndices[lsindex].shapeIndex = shapeIndex;
01614 
01615         // resolve the material and assign correct colors to the geometry
01616         shape.appearanceIndex = -1;
01617         if( !!triRef->getMaterial() ) {
01618             map<string,int>::const_iterator itmat = mapmaterials.find(triRef->getMaterial());
01619             if( itmat != mapmaterials.end() ) {
01620                 shape.appearanceIndex = itmat->second;
01621             }
01622         }
01623 
01624         size_t triangleIndexStride = 0, vertexoffset = -1;
01625         domInput_local_offsetRef indexOffsetRef;
01626 
01627         for (unsigned int w=0;w<triRef->getInput_array().getCount();w++) {
01628             size_t offset = triRef->getInput_array()[w]->getOffset();
01629             daeString str = triRef->getInput_array()[w]->getSemantic();
01630             if (!strcmp(str,"VERTEX")) {
01631                 indexOffsetRef = triRef->getInput_array()[w];
01632                 vertexoffset = offset;
01633             }
01634             if (offset> triangleIndexStride) {
01635                 triangleIndexStride = offset;
01636             }
01637         }
01638         triangleIndexStride++;
01639         size_t primitivecount = triRef->getCount();
01640         if( primitivecount > triRef->getP_array().getCount() ) {
01641             COLLADALOG_WARN("tristrips has incorrect count");
01642             primitivecount = triRef->getP_array().getCount();
01643         }
01644         int itriangle = 0, ivertex = 0;
01645         for(size_t ip = 0; ip < primitivecount; ++ip) {
01646             domList_of_uints indexArray = triRef->getP_array()[ip]->getValue();
01647             for (size_t i=0;i<vertsRef->getInput_array().getCount();++i) {
01648                 domInput_localRef localRef = vertsRef->getInput_array()[i];
01649                 daeString str = localRef->getSemantic();
01650                 if ( strcmp(str,"POSITION") == 0 ) {
01651                     const domSourceRef node = daeSafeCast<domSource>(localRef->getSource().getElement());
01652                     if( !node ) {
01653                         continue;
01654                     }
01655                     dReal fUnitScale = _GetUnitScale(node,_fGlobalScale);
01656                     const domFloat_arrayRef flArray = node->getFloat_array();
01657                     if (!!flArray) {
01658                         const domList_of_floats& listFloats = flArray->getValue();
01659                         int k=vertexoffset;
01660                         int vertexStride = 3;//instead of hardcoded stride, should use the 'accessor'
01661                         size_t usedindices = 3*(indexArray.getCount()-2);
01662                         shape.triangles.length(shape.triangles.length()+usedindices);
01663                         shape.vertices.length(shape.vertices.length()+3*indexArray.getCount());
01664                         size_t startoffset = ivertex/3;
01665                         while(k < (int)indexArray.getCount() ) {
01666                             int index0 = indexArray.get(k)*vertexStride;
01667                             domFloat fl0 = listFloats.get(index0);
01668                             domFloat fl1 = listFloats.get(index0+1);
01669                             domFloat fl2 = listFloats.get(index0+2);
01670                             k+=triangleIndexStride;
01671                             shape.vertices[ivertex++] = fl0*fUnitScale;
01672                             shape.vertices[ivertex++] = fl1*fUnitScale;
01673                             shape.vertices[ivertex++] = fl2*fUnitScale;
01674                         }
01675 
01676                         bool bFlip = false;
01677                         for(size_t ivert = 2; ivert < indexArray.getCount(); ++ivert) {
01678                             shape.triangles[itriangle++] = startoffset-2;
01679                             shape.triangles[itriangle++] = bFlip ? startoffset+ivert : startoffset+ivert-1;
01680                             shape.triangles[itriangle++] = bFlip ? startoffset+ivert-1 : startoffset+ivert;
01681                             bFlip = !bFlip;
01682                         }
01683                     }
01684                     else {
01685                         COLLADALOG_WARN("float array not defined!");
01686                     }
01687                     break;
01688                 }
01689             }
01690         }
01691         return true;
01692     }
01693 
01699     bool _ExtractGeometry(BodyInfoCollada_impl* pkinbody, boost::shared_ptr<LinkInfo> plink, const domPolylistRef triRef, const domVerticesRef vertsRef, const map<string,int>& mapmaterials, const map<string,int>& maptextures)
01700     {
01701         if( !triRef ) {
01702             COLLADALOG_WARN(str(boost::format("fail to _ExtractGeometry(LinkInfo,Polylist,VErtices) of %s")%plink->name));
01703             return false;
01704         }
01705         int shapeIndex = pkinbody->shapes_.length();
01706         pkinbody->shapes_.length(shapeIndex+1);
01707         ShapeInfo& shape = pkinbody->shapes_[shapeIndex];
01708         shape.primitiveType = SP_MESH;
01709         int lsindex = plink->shapeIndices.length();
01710         plink->shapeIndices.length(lsindex+1);
01711         plink->shapeIndices[lsindex].shapeIndex = shapeIndex;
01712 
01713         // resolve the material and assign correct colors to the geometry
01714         shape.appearanceIndex = -1;
01715         if( !!triRef->getMaterial() ) {
01716             map<string,int>::const_iterator itmat = mapmaterials.find(triRef->getMaterial());
01717             if( itmat != mapmaterials.end() ) {
01718                 shape.appearanceIndex = itmat->second;
01719             }
01720         }
01721 
01722         size_t triangleIndexStride = 0,vertexoffset = -1;
01723         domInput_local_offsetRef indexOffsetRef;
01724         for (unsigned int w=0;w<triRef->getInput_array().getCount();w++) {
01725             size_t offset = triRef->getInput_array()[w]->getOffset();
01726             daeString str = triRef->getInput_array()[w]->getSemantic();
01727             if (!strcmp(str,"VERTEX")) {
01728                 indexOffsetRef = triRef->getInput_array()[w];
01729                 vertexoffset = offset;
01730             }
01731             if (offset> triangleIndexStride) {
01732                 triangleIndexStride = offset;
01733             }
01734         }
01735         triangleIndexStride++;
01736         const domList_of_uints& indexArray =triRef->getP()->getValue();
01737         int ivertex = 0, itriangle=0;
01738         for (size_t i=0;i<vertsRef->getInput_array().getCount();++i) {
01739             domInput_localRef localRef = vertsRef->getInput_array()[i];
01740             daeString str = localRef->getSemantic();
01741             if ( strcmp(str,"POSITION") == 0 ) {
01742                 const domSourceRef node = daeSafeCast<domSource>(localRef->getSource().getElement());
01743                 if( !node ) {
01744                     continue;
01745                 }
01746                 dReal fUnitScale = _GetUnitScale(node,_fGlobalScale);
01747                 const domFloat_arrayRef flArray = node->getFloat_array();
01748                 if (!!flArray) {
01749                     const domList_of_floats& listFloats = flArray->getValue();
01750                     size_t k=vertexoffset;
01751                     int vertexStride = 3;//instead of hardcoded stride, should use the 'accessor'
01752                     for(size_t ipoly = 0; ipoly < triRef->getVcount()->getValue().getCount(); ++ipoly) {
01753                         size_t numverts = triRef->getVcount()->getValue()[ipoly];
01754                         if(numverts > 0 && k+(numverts-1)*triangleIndexStride < indexArray.getCount() ) {
01755                             size_t startoffset = ivertex/3;
01756                             shape.vertices.length(shape.vertices.length()+3*numverts);
01757                             shape.triangles.length(shape.triangles.length()+3*(numverts-2));
01758                             for (size_t j=0;j<numverts;j++) {
01759                                 int index0 = indexArray.get(k)*vertexStride;
01760                                 domFloat fl0 = listFloats.get(index0);
01761                                 domFloat fl1 = listFloats.get(index0+1);
01762                                 domFloat fl2 = listFloats.get(index0+2);
01763                                 k+=triangleIndexStride;
01764                                 shape.vertices[ivertex++] = fl0*fUnitScale;
01765                                 shape.vertices[ivertex++] = fl1*fUnitScale;
01766                                 shape.vertices[ivertex++] = fl2*fUnitScale;
01767                             }
01768                             for(size_t ivert = 2; ivert < numverts; ++ivert) {
01769                                 shape.triangles[itriangle++] = startoffset;
01770                                 shape.triangles[itriangle++] = startoffset+ivert-1;
01771                                 shape.triangles[itriangle++] = startoffset+ivert;
01772                             }   
01773                         }
01774                     }
01775                 }
01776                 else {
01777                     COLLADALOG_WARN("float array not defined!");
01778                 }
01779                 break;
01780             }
01781         }
01782         return true;
01783     }
01784 
01789     bool ExtractGeometry(BodyInfoCollada_impl* pkinbody, boost::shared_ptr<LinkInfo> plink, const domGeometryRef geom, const map<string,int>& mapmaterials, const map<string,int>& maptextures)
01790     {
01791         if( !geom ) {
01792             COLLADALOG_WARN(str(boost::format("fail to ExtractGeometry(plink,geom) of %s")%plink->name));
01793             return false;
01794         }
01795         std::vector<Vector3> vconvexhull;
01796         if (geom->getMesh()) {
01797             const domMeshRef meshRef = geom->getMesh();
01798             for (size_t tg = 0;tg<meshRef->getTriangles_array().getCount();tg++) {
01799                 _ExtractGeometry(pkinbody, plink, meshRef->getTriangles_array()[tg], meshRef->getVertices(), mapmaterials, maptextures);
01800             }
01801             for (size_t tg = 0;tg<meshRef->getTrifans_array().getCount();tg++) {
01802                 _ExtractGeometry(pkinbody, plink, meshRef->getTrifans_array()[tg], meshRef->getVertices(), mapmaterials, maptextures);
01803             }
01804             for (size_t tg = 0;tg<meshRef->getTristrips_array().getCount();tg++) {
01805                 _ExtractGeometry(pkinbody, plink, meshRef->getTristrips_array()[tg], meshRef->getVertices(), mapmaterials, maptextures);
01806             }
01807             for (size_t tg = 0;tg<meshRef->getPolylist_array().getCount();tg++) {
01808                 _ExtractGeometry(pkinbody, plink, meshRef->getPolylist_array()[tg], meshRef->getVertices(), mapmaterials, maptextures);
01809             }
01810             if( meshRef->getPolygons_array().getCount()> 0 ) {
01811                 COLLADALOG_WARN("openrave does not support collada polygons");
01812             }
01813 
01814             //            if( alltrimesh.vertices.size() == 0 ) {
01815             //                const domVerticesRef vertsRef = meshRef->getVertices();
01816             //                for (size_t i=0;i<vertsRef->getInput_array().getCount();i++) {
01817             //                    domInput_localRef localRef = vertsRef->getInput_array()[i];
01818             //                    daeString str = localRef->getSemantic();
01819             //                    if ( strcmp(str,"POSITION") == 0 ) {
01820             //                        const domSourceRef node = daeSafeCast<domSource>(localRef->getSource().getElement());
01821             //                        if( !node )
01822             //                            continue;
01823             //                        dReal fUnitScale = _GetUnitScale(node,_fGlobalScale);
01824             //                        const domFloat_arrayRef flArray = node->getFloat_array();
01825             //                        if (!!flArray) {
01826             //                            const domList_of_floats& listFloats = flArray->getValue();
01827             //                            int vertexStride = 3;//instead of hardcoded stride, should use the 'accessor'
01828             //                            vconvexhull.reserve(vconvexhull.size()+listFloats.getCount());
01829             //                            for (size_t vertIndex = 0;vertIndex < listFloats.getCount();vertIndex+=vertexStride) {
01830             //                                //btVector3 verts[3];
01831             //                                domFloat fl0 = listFloats.get(vertIndex);
01832             //                                domFloat fl1 = listFloats.get(vertIndex+1);
01833             //                                domFloat fl2 = listFloats.get(vertIndex+2);
01834             //                                vconvexhull.push_back(Vector(fl0*fUnitScale,fl1*fUnitScale,fl2*fUnitScale));
01835             //                            }
01836             //                        }
01837             //                    }
01838             //                }
01839             //
01840             //                _computeConvexHull(vconvexhull,alltrimesh);
01841             //            }
01842 
01843             return true;
01844         }
01845         else if (geom->getConvex_mesh()) {
01846             {
01847                 const domConvex_meshRef convexRef = geom->getConvex_mesh();
01848                 daeElementRef otherElemRef = convexRef->getConvex_hull_of().getElement();
01849                 if ( !!otherElemRef ) {
01850                     domGeometryRef linkedGeom = *(domGeometryRef*)&otherElemRef;
01851                     COLLADALOG_WARN( "otherLinked");
01852                 }
01853                 else {
01854                     COLLADALOG_WARN(str(boost::format("convexMesh polyCount = %d")%(int)convexRef->getPolygons_array().getCount()));
01855                     COLLADALOG_WARN(str(boost::format("convexMesh triCount = %d")%(int)convexRef->getTriangles_array().getCount()));
01856                 }
01857             }
01858 
01859             const domConvex_meshRef convexRef = geom->getConvex_mesh();
01860             //daeString urlref = convexRef->getConvex_hull_of().getURI();
01861             daeString urlref2 = convexRef->getConvex_hull_of().getOriginalURI();
01862             if (urlref2) {
01863                 daeElementRef otherElemRef = convexRef->getConvex_hull_of().getElement();
01864 
01865                 // Load all the geometry libraries
01866                 for ( size_t i = 0; i < _dom->getLibrary_geometries_array().getCount(); i++) {
01867                     domLibrary_geometriesRef libgeom = _dom->getLibrary_geometries_array()[i];
01868                     for (size_t i = 0; i < libgeom->getGeometry_array().getCount(); i++) {
01869                         domGeometryRef lib = libgeom->getGeometry_array()[i];
01870                         if (!strcmp(lib->getId(),urlref2+1)) { // skip the # at the front of urlref2
01871                             //found convex_hull geometry
01872                             domMesh *meshElement = lib->getMesh();//linkedGeom->getMesh();
01873                             if (meshElement) {
01874                                 const domVerticesRef vertsRef = meshElement->getVertices();
01875                                 for (size_t i=0;i<vertsRef->getInput_array().getCount();i++) {
01876                                     domInput_localRef localRef = vertsRef->getInput_array()[i];
01877                                     daeString str = localRef->getSemantic();
01878                                     if ( strcmp(str,"POSITION") == 0) {
01879                                         const domSourceRef node = daeSafeCast<domSource>(localRef->getSource().getElement());
01880                                         if( !node ) {
01881                                             continue;
01882                                         }
01883                                         dReal fUnitScale = _GetUnitScale(node,_fGlobalScale);
01884                                         const domFloat_arrayRef flArray = node->getFloat_array();
01885                                         if (!!flArray) {
01886                                             vconvexhull.reserve(vconvexhull.size()+flArray->getCount());
01887                                             const domList_of_floats& listFloats = flArray->getValue();
01888                                             for (size_t k=0;k+2<flArray->getCount();k+=3) {
01889                                                 domFloat fl0 = listFloats.get(k);
01890                                                 domFloat fl1 = listFloats.get(k+1);
01891                                                 domFloat fl2 = listFloats.get(k+2);
01892                                                 vconvexhull.push_back(Vector3(fl0*fUnitScale,fl1*fUnitScale,fl2*fUnitScale));
01893                                             }
01894                                         }
01895                                     }
01896                                 }
01897                             }
01898                         }
01899                     }
01900                 }
01901             }
01902             else {
01903                 //no getConvex_hull_of but direct vertices
01904                 const domVerticesRef vertsRef = convexRef->getVertices();
01905                 for (size_t i=0;i<vertsRef->getInput_array().getCount();i++) {
01906                     domInput_localRef localRef = vertsRef->getInput_array()[i];
01907                     daeString str = localRef->getSemantic();
01908                     if ( strcmp(str,"POSITION") == 0 ) {
01909                         const domSourceRef node = daeSafeCast<domSource>(localRef->getSource().getElement());
01910                         if( !node ) {
01911                             continue;
01912                         }
01913                         dReal fUnitScale = _GetUnitScale(node,_fGlobalScale);
01914                         const domFloat_arrayRef flArray = node->getFloat_array();
01915                         if (!!flArray) {
01916                             const domList_of_floats& listFloats = flArray->getValue();
01917                             vconvexhull.reserve(vconvexhull.size()+flArray->getCount());
01918                             for (size_t k=0;k+2<flArray->getCount();k+=3) {
01919                                 domFloat fl0 = listFloats.get(k);
01920                                 domFloat fl1 = listFloats.get(k+1);
01921                                 domFloat fl2 = listFloats.get(k+2);
01922                                 vconvexhull.push_back(Vector3(fl0*fUnitScale,fl1*fUnitScale,fl2*fUnitScale));
01923                             }
01924                         }
01925                     }
01926                 }
01927             }
01928 
01929             if( vconvexhull.size()> 0 ) {
01930                 COLLADALOG_ERROR("convex hulls not supported");
01931                 //plink->_listGeomProperties.push_back(KinBody::Link::GEOMPROPERTIES(plink));
01932                 //KinBody::Link::GEOMPROPERTIES& geom = plink->_listGeomProperties.back();
01933                 //KinBody::Link::TRIMESH& trimesh = geom.collisionmesh;
01934                 //geom._type = KinBody::Link::GEOMPROPERTIES::GeomTrimesh;
01935                 //geom.InitCollisionMesh();
01936             }
01937             return true;
01938         }
01939 
01940         return false;
01941     }
01942 
01946     void _FillMaterial(MaterialInfo& mat, const domMaterialRef pdommat)
01947     {
01948         mat.ambientIntensity = 0.1;
01949         mat.diffuseColor[0] = 0.8; mat.diffuseColor[1] = 0.8; mat.diffuseColor[2] = 0.8;
01950         mat.emissiveColor[0] = 0; mat.emissiveColor[1] = 0; mat.emissiveColor[2] = 0;
01951         mat.shininess = 0;
01952         mat.specularColor[0] = 0; mat.specularColor[1] = 0; mat.specularColor[2] = 0;
01953         mat.transparency = 0; // 0 is opaque
01954         if( !!pdommat && !!pdommat->getInstance_effect() ) {
01955             domEffectRef peffect = daeSafeCast<domEffect>(pdommat->getInstance_effect()->getUrl().getElement().cast());
01956             if( !!peffect ) {
01957                 domProfile_common::domTechnique::domPhongRef pphong = daeSafeCast<domProfile_common::domTechnique::domPhong>(peffect->getDescendant(daeElement::matchType(domProfile_common::domTechnique::domPhong::ID())));
01958                 if( !!pphong ) {
01959                     if( !!pphong->getAmbient() && !!pphong->getAmbient()->getColor() ) {
01960                         domFx_color c = pphong->getAmbient()->getColor()->getValue();
01961                         mat.ambientIntensity = (fabs(c[0])+fabs(c[1])+fabs(c[2]))/3;
01962                     }
01963                     if( !!pphong->getDiffuse() && !!pphong->getDiffuse()->getColor() ) {
01964                         domFx_color c = pphong->getDiffuse()->getColor()->getValue();
01965                         mat.diffuseColor[0] = c[0];
01966                         mat.diffuseColor[1] = c[1];
01967                         mat.diffuseColor[2] = c[2];
01968                     }
01969                 }
01970                 domProfile_common::domTechnique::domLambertRef plambert = daeSafeCast<domProfile_common::domTechnique::domLambert>(peffect->getDescendant(daeElement::matchType(domProfile_common::domTechnique::domLambert::ID())));
01971                 if( !!plambert ) {
01972                     if( !!plambert->getAmbient() && !!plambert->getAmbient()->getColor() ) {
01973                         domFx_color c = plambert->getAmbient()->getColor()->getValue();
01974                         mat.ambientIntensity = (fabs(c[0])+fabs(c[1])+fabs(c[2]))/3;
01975                     }
01976                     if( !!plambert->getDiffuse() && !!plambert->getDiffuse()->getColor() ) {
01977                         domFx_color c = plambert->getDiffuse()->getColor()->getValue();
01978                         mat.diffuseColor[0] = c[0];
01979                         mat.diffuseColor[1] = c[1];
01980                         mat.diffuseColor[2] = c[2];
01981                     }
01982                 }
01983             }
01984         }
01985     }
01986 
01988     void ExtractRobotManipulators(BodyInfoCollada_impl* probot, const domArticulated_systemRef as)
01989     {
01990         for(size_t ie = 0; ie < as->getExtra_array().getCount(); ++ie) {
01991             domExtraRef pextra = as->getExtra_array()[ie];
01992             if( strcmp(pextra->getType(), "manipulator") == 0 ) {
01993                 string name = pextra->getAttribute("name");
01994                 if( name.size() == 0 ) {
01995                     name = str(boost::format("manipulator%d")%_nGlobalManipulatorId++);
01996                 }
01997                 domTechniqueRef tec = _ExtractOpenRAVEProfile(pextra->getTechnique_array());
01998                 if( !!tec ) {
01999                     
02000                 }
02001                 else {
02002                     COLLADALOG_WARN(str(boost::format("cannot create robot manipulator %s")%name));
02003                 }
02004             }
02005         }
02006     }
02007 
02009     void ExtractRobotAttachedSensors(BodyInfoCollada_impl* probot, const domArticulated_systemRef as)
02010     {
02011         for (size_t ie = 0; ie < as->getExtra_array().getCount(); ie++) {
02012             domExtraRef pextra = as->getExtra_array()[ie];
02013             if( strcmp(pextra->getType(), "attach_sensor") == 0 ) {
02014                 string name = pextra->getAttribute("name");
02015                 if( name.size() == 0 ) {
02016                     name = str(boost::format("sensor%d")%_nGlobalSensorId++);
02017                 }
02018                 domTechniqueRef tec = _ExtractOpenRAVEProfile(pextra->getTechnique_array());
02019                 if( !!tec ) {
02020                     daeElement* pframe_origin = tec->getChild("frame_origin");
02021                     if( !!pframe_origin ) {
02022                         domLinkRef pdomlink = daeSafeCast<domLink>(daeSidRef(pframe_origin->getAttribute("link"), as).resolve().elt);
02023                         SensorInfo_var psensor(new SensorInfo());
02024                         psensor->name = CORBA::string_dup( name.c_str() );
02025                         boost::shared_ptr<LinkInfo> plink;
02026                         if( _mapLinkNames.find(_ExtractLinkName(pdomlink)) != _mapLinkNames.end() ) {
02027                             plink = _mapLinkNames[_ExtractLinkName(pdomlink)];
02028                         } else {
02029                             COLLADALOG_WARN(str(boost::format("unknown joint %s")%_ExtractLinkName(pdomlink)));
02030                         }
02031 
02032                         if( plink && _ExtractSensor(psensor,tec->getChild("instance_sensor")) ) {
02033 
02034                             DblArray12 ttemp;
02035                             _ExtractFullTransformFromChildren(ttemp, pframe_origin);
02036                             //std::cerr << psensor->type << std::endl;
02037                             if ( string(psensor->type) == "Vision" ) {
02038                                 DblArray4 quat;
02039                                 DblArray12 rotation, ttemp2;
02040                                 QuatFromAxisAngle(quat, Vector4(1,0,0,M_PI));
02041                                 PoseFromQuat(rotation,quat);
02042                                 PoseMult(ttemp2,ttemp,rotation);
02043                                 AxisAngleTranslationFromPose(psensor->rotation,psensor->translation,ttemp2);
02044                             } else {
02045                                 AxisAngleTranslationFromPose(psensor->rotation,psensor->translation,ttemp);
02046                             }
02047 
02048                             int numSensors = plink->sensors.length();
02049                             plink->sensors.length(numSensors + 1);
02050                             plink->sensors[numSensors] = psensor;
02051                         } else {
02052                             COLLADALOG_WARN(str(boost::format("cannot find instance_sensor for attached sensor %s:%s")%probot->name_%name));
02053                         }
02054                     }
02055                 }
02056                 else {
02057                     COLLADALOG_WARN(str(boost::format("cannot create robot attached sensor %s")%name));
02058                 }
02059             }
02060         }
02061     }
02062 
02064     void ExtractRobotAttachedActuators(BodyInfoCollada_impl* probot, const domArticulated_systemRef as)
02065     {
02066         for (size_t ie = 0; ie < as->getExtra_array().getCount(); ie++) {
02067             domExtraRef pextra = as->getExtra_array()[ie];
02068             if( strcmp(pextra->getType(), "attach_actuator") == 0 ) {
02069                 string name = pextra->getAttribute("name");
02070                 if( name.size() == 0 ) {
02071                     name = str(boost::format("actuator%d")%_nGlobalActuatorId++);
02072                 }
02073                 domTechniqueRef tec = _ExtractOpenRAVEProfile(pextra->getTechnique_array());
02074                 if( !!tec ) {
02075                     if ( GetLink(name) && _ExtractActuator(GetLink(name), tec->getChild("instance_actuator"))  ) {
02076                     } else {
02077                         COLLADALOG_WARN(str(boost::format("cannot find instance_actuator for attached sensor %s:%s")%probot->name_%name));
02078                     }
02079                 }
02080                 else {
02081                     COLLADALOG_WARN(str(boost::format("cannot create robot attached actuators %s")%name));
02082                 }
02083             }
02084         }
02085     }
02086 
02088     bool _ExtractSensor(SensorInfo &psensor, daeElementRef instance_sensor)
02089     {
02090         if( !instance_sensor ) {
02091             COLLADALOG_WARN("could not find instance_sensor");
02092             return false;
02093         }
02094         if( !instance_sensor->hasAttribute("url") ) {
02095             COLLADALOG_WARN("instance_sensor has no url");
02096             return false;
02097         }
02098 
02099         std::string instance_id = instance_sensor->getAttribute("id");
02100         std::string instance_url = instance_sensor->getAttribute("url");
02101         daeElementRef domsensor = _getElementFromUrl(daeURI(*instance_sensor,instance_url));
02102         if( !domsensor ) {
02103             COLLADALOG_WARN(str(boost::format("failed to find senor id %s url=%s")%instance_id%instance_url));
02104             return false;
02105         }
02106         if( !domsensor->hasAttribute("type") ) {
02107             COLLADALOG_WARN("collada <sensor> needs type attribute");
02108             return false;
02109         }
02110         if ( domsensor->hasAttribute("sid")) {
02111             psensor.id = boost::lexical_cast<int>(domsensor->getAttribute("sid"));
02112         } else {
02113             psensor.id = 0;
02114         }
02115         std::string sensortype = domsensor->getAttribute("type");
02116         if ( sensortype == "base_imu" ) {// AccelerationSensor  // GyroSeesor
02117             psensor.specValues.length( CORBA::ULong(3) );
02118             daeElement *max_angular_velocity = domsensor->getChild("max_angular_velocity");
02119             daeElement *max_acceleration = domsensor->getChild("max_acceleration");
02120             if ( !! max_angular_velocity ) {
02121                 istringstream ins(max_angular_velocity->getCharData());
02122                 ins >> psensor.specValues[0] >> psensor.specValues[1] >> psensor.specValues[2];
02123                 psensor.type = CORBA::string_dup( "RateGyro" );
02124             } else if ( !! max_acceleration ) {
02125                 istringstream ins(max_acceleration->getCharData());
02126                 ins >> psensor.specValues[0] >> psensor.specValues[1] >> psensor.specValues[2];
02127                 psensor.type = CORBA::string_dup( "Acceleration" );
02128             } else {
02129                 COLLADALOG_WARN(str(boost::format("couldn't find max_angular_velocity nor max_acceleration%s")%sensortype));
02130             }
02131             return true;
02132         }
02133         if ( sensortype == "base_pinhole_camera" ) { // VisionSensor
02134             psensor.type = CORBA::string_dup( "Vision" );
02135             psensor.specValues.length( CORBA::ULong(7) );
02136             //psensor.specValues[0] = frontClipDistance
02137             //psensor.specValues[1] = backClipDistance
02138             psensor.specValues[1] = 10;
02139             //psensor.specValues[2] = fieldOfView
02140             //psensor.specValues[3] = OpenHRP::Camera::COLOR; // type
02141             //psensor.specValues[4] = width
02142             //psensor.specValues[5] = height
02143             //psensor.specValues[6] = frameRate
02144             daeElement *measurement_time = domsensor->getChild("measurement_time");
02145             if ( !! measurement_time ) {
02146                 psensor.specValues[6] = 1.0/(boost::lexical_cast<double>(measurement_time->getCharData()));
02147             } else {
02148                 COLLADALOG_WARN(str(boost::format("couldn't find measurement_time %s")%sensortype));
02149             }
02150             daeElement *focal_length = domsensor->getChild("focal_length");
02151             if ( !! focal_length ) {
02152                 psensor.specValues[0] = boost::lexical_cast<double>(focal_length->getCharData());
02153             } else {
02154                 COLLADALOG_WARN(str(boost::format("couldn't find focal_length %s")%sensortype));
02155                 psensor.specValues[0] = 0.1;
02156             }
02157             daeElement *image_format = domsensor->getChild("format");
02158             std::string string_format = "uint8";
02159             if ( !! image_format ) {
02160                 string_format = image_format->getCharData();
02161             }
02162             daeElement *intrinsic = domsensor->getChild("intrinsic");
02163             if ( !! intrinsic ) {
02164                 istringstream ins(intrinsic->getCharData());
02165                 float f0,f1,f2,f3,f4,f5;
02166                 ins >> f0 >> f1 >> f2 >> f3 >> f4 >> f5;
02167                 psensor.specValues[2] = atan( f2 / f0) * 2.0;
02168             } else {
02169                 COLLADALOG_WARN(str(boost::format("couldn't find intrinsic %s")%sensortype));
02170                 psensor.specValues[2] = 0.785398;
02171             }
02172             daeElement *image_dimensions = domsensor->getChild("image_dimensions");
02173             if ( !! image_dimensions ) {
02174                 istringstream ins(image_dimensions->getCharData());
02175                 int ichannel;
02176                 ins >> psensor.specValues[4] >> psensor.specValues[5] >> ichannel;
02177                 switch (ichannel) {
02178                     case 1:
02179                         if ( string_format == "uint8") {
02180                             psensor.specValues[3] = OpenHRP::Camera::MONO;
02181                         } else if ( string_format == "float32") {
02182                             psensor.specValues[3] = OpenHRP::Camera::DEPTH;
02183                         } else {
02184                             COLLADALOG_WARN(str(boost::format("unknown image format %s %d")%string_format%ichannel));
02185                         }
02186                         break;
02187                     case 2:
02188                         if ( string_format == "float32") {
02189                             psensor.specValues[3] = OpenHRP::Camera::MONO_DEPTH;
02190                         } else {
02191                             COLLADALOG_WARN(str(boost::format("unknown image format %s %d")%string_format%ichannel));
02192                         }
02193                         break;
02194                     case 3:
02195                         if ( string_format == "uint8") {
02196                             psensor.specValues[3] = OpenHRP::Camera::COLOR;
02197                         } else {
02198                             COLLADALOG_WARN(str(boost::format("unknown image format %s %d")%string_format%ichannel));
02199                         }
02200                         break;
02201                     case 4:
02202                         if ( string_format == "float32") {
02203                             psensor.specValues[3] = OpenHRP::Camera::COLOR_DEPTH;
02204                         } else {
02205                             COLLADALOG_WARN(str(boost::format("unknown image format %s %d")%string_format%ichannel));
02206                         }
02207                         break;
02208                 default:
02209                     COLLADALOG_WARN(str(boost::format("unknown image format %s %d")%string_format%ichannel));
02210                 }
02211 
02212             } else {
02213                 COLLADALOG_WARN(str(boost::format("couldn't find image_dimensions %s")%sensortype));
02214                 psensor.specValues[4] = 320;
02215                 psensor.specValues[5] = 240;
02216             }
02217             return true;
02218         }
02219         if ( sensortype == "base_force6d" ) { // ForceSensor
02220             psensor.type = CORBA::string_dup( "Force" );
02221             psensor.specValues.length( CORBA::ULong(6) );
02222             daeElement *max_force = domsensor->getChild("load_range_force");
02223             if ( !! max_force ) {
02224                 istringstream ins(max_force->getCharData());
02225                 ins >> psensor.specValues[0] >> psensor.specValues[1] >> psensor.specValues[2];
02226             } else {
02227                 COLLADALOG_WARN(str(boost::format("couldn't find load_range_force %s")%sensortype));
02228             }
02229             daeElement *max_torque = domsensor->getChild("load_range_torque");
02230             if ( !! max_torque ) {
02231                 istringstream ins(max_torque->getCharData());
02232                 ins >> psensor.specValues[3] >> psensor.specValues[4] >> psensor.specValues[5];
02233             } else {
02234                 COLLADALOG_WARN(str(boost::format("couldn't findload_range_torque %s")%sensortype));
02235             }
02236             return true;
02237         }
02238         if ( sensortype == "base_laser1d" ) { // RangeSensor
02239             psensor.type = CORBA::string_dup( "Range" );
02240             psensor.specValues.length( CORBA::ULong(4) );
02241             daeElement *scan_angle = domsensor->getChild("angle_range");
02242             if ( !! scan_angle ) {
02243                 psensor.specValues[0] = boost::lexical_cast<double>(scan_angle->getCharData());
02244             } else {
02245                 COLLADALOG_WARN(str(boost::format("couldn't find angle_range %s")%sensortype));
02246             }
02247             daeElement *scan_step = domsensor->getChild("angle_increment");
02248             if ( !! scan_step ) {
02249                 psensor.specValues[1] = boost::lexical_cast<double>(scan_step->getCharData());
02250             } else {
02251                 COLLADALOG_WARN(str(boost::format("couldn't find angle_incremnet %s")%sensortype));
02252             }
02253             daeElement *scan_rate = domsensor->getChild("measurement_time");
02254             if ( !! scan_rate ) {
02255                 psensor.specValues[2] = 1.0/boost::lexical_cast<double>(scan_rate->getCharData());
02256             } else {
02257                 COLLADALOG_WARN(str(boost::format("couldn't find measurement_time %s")%sensortype));
02258             }
02259             daeElement *max_distance = domsensor->getChild("distance_range");
02260             if ( !! max_distance ) {
02261                 psensor.specValues[3] = boost::lexical_cast<double>(max_distance->getCharData());
02262             } else {
02263                 COLLADALOG_WARN(str(boost::format("couldn't find distance_range %s")%sensortype));
02264             }
02265             return true;
02266         }
02267         COLLADALOG_WARN(str(boost::format("need to create sensor type: %s")%sensortype));
02268         return false;
02269     }
02270 
02272     bool _ExtractActuator(boost::shared_ptr<LinkInfo> plink, daeElementRef instance_actuator)
02273     {
02274         if( !instance_actuator ) {
02275             return false;
02276         }
02277         if( !instance_actuator->hasAttribute("url") ) {
02278             COLLADALOG_WARN("instance_actuator has no url");
02279             return false;
02280         }
02281 
02282         std::string instance_id = instance_actuator->getAttribute("id");
02283         std::string instance_url = instance_actuator->getAttribute("url");
02284         daeElementRef domactuator = _getElementFromUrl(daeURI(*instance_actuator,instance_url));
02285         if( !domactuator ) {
02286             COLLADALOG_WARN(str(boost::format("failed to find actuator id %s url=%s")%instance_id%instance_url));
02287             return false;
02288         }
02289         if( !domactuator->hasAttribute("type") ) {
02290             COLLADALOG_WARN("collada <actuator> needs type attribute");
02291             return false;
02292         }
02293         std::string actuatortype = domactuator->getAttribute("type");
02294         daeElement *rotor_inertia = domactuator->getChild("rotor_inertia");
02295         if ( !! rotor_inertia ) {
02296             plink->rotorInertia =  boost::lexical_cast<double>(rotor_inertia->getCharData());
02297         }
02298         daeElement *rotor_resistor = domactuator->getChild("rotor_resistor");
02299         if ( !! rotor_resistor ) {
02300             plink->rotorResistor = boost::lexical_cast<double>(rotor_resistor->getCharData());
02301         }
02302         daeElement *gear_ratio = domactuator->getChild("gear_ratio");
02303         if ( !! gear_ratio ) {
02304             plink->gearRatio = boost::lexical_cast<double>(gear_ratio->getCharData());
02305         }
02306         daeElement *torque_const = domactuator->getChild("torque_constant");
02307         if ( !! torque_const ) {
02308             plink->torqueConst = boost::lexical_cast<double>(torque_const->getCharData());
02309         }
02310         daeElement *encoder_pulse = domactuator->getChild("encoder_pulse");
02311         if ( !! encoder_pulse ) {
02312             plink->encoderPulse = boost::lexical_cast<double>(encoder_pulse->getCharData());
02313         }
02314         daeElement *nom_torque = domactuator->getChild("nominal_torque");
02315         if ( !! nom_torque ) {
02316             if(plink->climit.length() < 1) plink->climit.length(1);
02317             plink->climit[0] = boost::lexical_cast<double>(nom_torque->getCharData()) / ( plink->gearRatio * plink->torqueConst );
02318         }
02319         return true;
02320     }
02321 
02322     inline daeElementRef _getElementFromUrl(const daeURI &uri)
02323     {
02324         return daeStandardURIResolver(*_dae).resolveElement(uri);
02325     }
02326 
02327     static daeElement* searchBinding(domCommon_sidref_or_paramRef paddr, daeElementRef parent)
02328     {
02329         if( !!paddr->getSIDREF() ) {
02330             return daeSidRef(paddr->getSIDREF()->getValue(),parent).resolve().elt;
02331         }
02332         if (!!paddr->getParam()) {
02333             return searchBinding(paddr->getParam()->getValue(),parent);
02334         }
02335         return NULL;
02336     }
02337 
02341     static daeElement* searchBinding(daeString ref, daeElementRef parent)
02342     {
02343         if( !parent ) {
02344             return NULL;
02345         }
02346         daeElement* pelt = NULL;
02347         domKinematics_sceneRef kscene = daeSafeCast<domKinematics_scene>(parent.cast());
02348         if( !!kscene ) {
02349             pelt = searchBindingArray(ref,kscene->getInstance_articulated_system_array());
02350             if( !!pelt ) {
02351                 return pelt;
02352             }
02353             return searchBindingArray(ref,kscene->getInstance_kinematics_model_array());
02354         }
02355         domArticulated_systemRef articulated_system = daeSafeCast<domArticulated_system>(parent.cast());
02356         if( !!articulated_system ) {
02357             if( !!articulated_system->getKinematics() ) {
02358                 pelt = searchBindingArray(ref,articulated_system->getKinematics()->getInstance_kinematics_model_array());
02359                 if( !!pelt ) {
02360                     return pelt;
02361                 }
02362             }
02363             if( !!articulated_system->getMotion() ) {
02364                 return searchBinding(ref,articulated_system->getMotion()->getInstance_articulated_system());
02365             }
02366             return NULL;
02367         }
02368         // try to get a bind array
02369         daeElementRef pbindelt;
02370         const domKinematics_bind_Array* pbindarray = NULL;
02371         const domKinematics_newparam_Array* pnewparamarray = NULL;
02372         domInstance_articulated_systemRef ias = daeSafeCast<domInstance_articulated_system>(parent.cast());
02373         if( !!ias ) {
02374             pbindarray = &ias->getBind_array();
02375             pbindelt = ias->getUrl().getElement();
02376             pnewparamarray = &ias->getNewparam_array();
02377         }
02378         if( !pbindarray || !pbindelt ) {
02379             domInstance_kinematics_modelRef ikm = daeSafeCast<domInstance_kinematics_model>(parent.cast());
02380             if( !!ikm ) {
02381                 pbindarray = &ikm->getBind_array();
02382                 pbindelt = ikm->getUrl().getElement();
02383                 pnewparamarray = &ikm->getNewparam_array();
02384             }
02385         }
02386         if( !!pbindarray && !!pbindelt ) {
02387             for (size_t ibind = 0; ibind < pbindarray->getCount(); ++ibind) {
02388                 domKinematics_bindRef pbind = (*pbindarray)[ibind];
02389                 if( !!pbind->getSymbol() && strcmp(pbind->getSymbol(), ref) == 0 ) { 
02390                     // found a match
02391                     if( !!pbind->getParam() ) {
02392                         //return searchBinding(pbind->getParam()->getRef(), pbindelt);
02393                         return daeSidRef(pbind->getParam()->getRef(), pbindelt).resolve().elt;
02394                     }
02395                     else if( !!pbind->getSIDREF() ) {
02396                         return daeSidRef(pbind->getSIDREF()->getValue(), pbindelt).resolve().elt;
02397                     }
02398                 }
02399             }
02400             for(size_t inewparam = 0; inewparam < pnewparamarray->getCount(); ++inewparam) {
02401                 domKinematics_newparamRef newparam = (*pnewparamarray)[inewparam];
02402                 if( !!newparam->getSid() && strcmp(newparam->getSid(), ref) == 0 ) {
02403                     if( !!newparam->getSIDREF() ) { // can only bind with SIDREF
02404                         return daeSidRef(newparam->getSIDREF()->getValue(),pbindelt).resolve().elt;
02405                     }
02406                     COLLADALOG_WARN(str(boost::format("newparam sid=%s does not have SIDREF")%newparam->getSid()));
02407                 }
02408             }
02409         }
02410         COLLADALOG_WARN(str(boost::format("failed to get binding '%s' for element: %s")%ref%parent->getElementName()));
02411         return NULL;
02412     }
02413 
02414     static daeElement* searchBindingArray(daeString ref, const domInstance_articulated_system_Array& paramArray)
02415     {
02416         for(size_t iikm = 0; iikm < paramArray.getCount(); ++iikm) {
02417             daeElement* pelt = searchBinding(ref,paramArray[iikm].cast());
02418             if( !!pelt ) {
02419                 return pelt;
02420             }
02421         }
02422         return NULL;
02423     }
02424 
02425     static daeElement* searchBindingArray(daeString ref, const domInstance_kinematics_model_Array& paramArray)
02426     {
02427         for(size_t iikm = 0; iikm < paramArray.getCount(); ++iikm) {
02428             daeElement* pelt = searchBinding(ref,paramArray[iikm].cast());
02429             if( !!pelt ) {
02430                 return pelt;
02431             }
02432         }
02433         return NULL;
02434     }
02435 
02436     template <typename U> static xsBoolean resolveBool(domCommon_bool_or_paramRef paddr, const U& parent) {
02437         if( !!paddr->getBool() ) {
02438             return paddr->getBool()->getValue();
02439         }
02440         if( !paddr->getParam() ) {
02441             COLLADALOG_WARN("param not specified, setting to 0");
02442             return false;
02443         }
02444         for(size_t iparam = 0; iparam < parent->getNewparam_array().getCount(); ++iparam) {
02445             domKinematics_newparamRef pnewparam = parent->getNewparam_array()[iparam];
02446             if( !!pnewparam->getSid() && strcmp(pnewparam->getSid(), paddr->getParam()->getValue()) == 0 ) {
02447                 if( !!pnewparam->getBool() ) {
02448                     return pnewparam->getBool()->getValue();
02449                 }
02450                 else if( !!pnewparam->getSIDREF() ) {
02451                     domKinematics_newparam::domBoolRef ptarget = daeSafeCast<domKinematics_newparam::domBool>(daeSidRef(pnewparam->getSIDREF()->getValue(), pnewparam).resolve().elt);
02452                     if( !ptarget ) {
02453                         COLLADALOG_WARN(str(boost::format("failed to resolve %s from %s")%pnewparam->getSIDREF()->getValue()%paddr->getID()));
02454                         continue;
02455                     }
02456                     return ptarget->getValue();
02457                 }
02458             }
02459         }
02460         COLLADALOG_WARN(str(boost::format("failed to resolve %s")%paddr->getParam()->getValue()));
02461         return false;
02462     }
02463     template <typename U> static domFloat resolveFloat(domCommon_float_or_paramRef paddr, const U& parent) {
02464         if( !!paddr->getFloat() ) {
02465             return paddr->getFloat()->getValue();
02466         }
02467         if( !paddr->getParam() ) {
02468             COLLADALOG_WARN("param not specified, setting to 0");
02469             return 0;
02470         }
02471         for(size_t iparam = 0; iparam < parent->getNewparam_array().getCount(); ++iparam) {
02472             domKinematics_newparamRef pnewparam = parent->getNewparam_array()[iparam];
02473             if( !!pnewparam->getSid() && strcmp(pnewparam->getSid(), paddr->getParam()->getValue()) == 0 ) {
02474                 if( !!pnewparam->getFloat() ) {
02475                     return pnewparam->getFloat()->getValue();
02476                 }
02477                 else if( !!pnewparam->getSIDREF() ) {
02478                     domKinematics_newparam::domFloatRef ptarget = daeSafeCast<domKinematics_newparam::domFloat>(daeSidRef(pnewparam->getSIDREF()->getValue(), pnewparam).resolve().elt);
02479                     if( !ptarget ) {
02480                         COLLADALOG_WARN(str(boost::format("failed to resolve %s from %s")%pnewparam->getSIDREF()->getValue()%paddr->getID()));
02481                         continue;
02482                     }
02483                     return ptarget->getValue();
02484                 }
02485             }
02486         }
02487         COLLADALOG_WARN(str(boost::format("failed to resolve %s")%paddr->getParam()->getValue()));
02488         return 0;
02489     }
02490 
02491     static bool resolveCommon_float_or_param(daeElementRef pcommon, daeElementRef parent, float& f)
02492     {
02493         daeElement* pfloat = pcommon->getChild("float");
02494         if( !!pfloat ) {
02495             stringstream sfloat(pfloat->getCharData());
02496             sfloat >> f;
02497             return !!sfloat;
02498         }
02499         daeElement* pparam = pcommon->getChild("param");
02500         if( !!pparam ) {
02501             if( pparam->hasAttribute("ref") ) {
02502                 COLLADALOG_WARN("cannot process param ref");
02503             }
02504             else {
02505                 daeElement* pelt = daeSidRef(pparam->getCharData(),parent).resolve().elt;
02506                 if( !!pelt ) {
02507                     COLLADALOG_WARN(str(boost::format("found param ref: %s from %s")%pelt->getCharData()%pparam->getCharData()));
02508                 }
02509             }
02510         }
02511         return false;
02512     }
02513 
02515     void getTransform(DblArray12& tres, daeElementRef pelt)
02516     {
02517         domRotateRef protate = daeSafeCast<domRotate>(pelt);
02518         if( !!protate ) {
02519             DblArray4 quat;
02520             QuatFromAxisAngle(quat, protate->getValue(),M_PI/180.0);
02521             PoseFromQuat(tres,quat);
02522             return;
02523         }
02524 
02525         dReal fscale = _GetUnitScale(pelt,_fGlobalScale);
02526         domTranslateRef ptrans = daeSafeCast<domTranslate>(pelt);
02527         if( !!ptrans ) {
02528             PoseIdentity(tres);
02529             tres[3] = ptrans->getValue()[0]*fscale;
02530             tres[7] = ptrans->getValue()[1]*fscale;
02531             tres[11] = ptrans->getValue()[2]*fscale;
02532             return;
02533         }
02534 
02535         domMatrixRef pmat = daeSafeCast<domMatrix>(pelt);
02536         if( !!pmat ) {
02537             for(int i = 0; i < 12; ++i) {
02538                 tres[i] = pmat->getValue()[i];
02539             }
02540             tres[3] *= fscale;
02541             tres[7] *= fscale;
02542             tres[11] *= fscale;
02543             return;
02544         }
02545 
02546         PoseIdentity(tres);
02547         domScaleRef pscale = daeSafeCast<domScale>(pelt);
02548         if( !!pscale ) {
02549             tres[0] = pscale->getValue()[0];
02550             tres[5] = pscale->getValue()[1];
02551             tres[10] = pscale->getValue()[2];
02552             return;
02553         }
02554 
02555         domLookatRef pcamera = daeSafeCast<domLookat>(pelt);
02556         if( pelt->typeID() == domLookat::ID() ) {
02557             COLLADALOG_ERROR("lookat not implemented");
02558             return;
02559         }
02560 
02561         domSkewRef pskew = daeSafeCast<domSkew>(pelt);
02562         if( !!pskew ) {
02563             COLLADALOG_ERROR("skew transform not implemented");
02564             return;
02565         }
02566     }
02567 
02570     template <typename T> void getNodeParentTransform(DblArray12& tres, const T pelt) {
02571         PoseIdentity(tres);
02572         domNodeRef pnode = daeSafeCast<domNode>(pelt->getParent());
02573         if( !!pnode ) {
02574             DblArray12 ttemp, ttemp2;
02575             _ExtractFullTransform(ttemp, pnode);
02576             getNodeParentTransform(tres, pnode);
02577             PoseMult(ttemp2,tres,ttemp);
02578             memcpy(&tres[0],&ttemp2[0],sizeof(DblArray12));
02579         }
02580     }
02581 
02583     template <typename T> void _ExtractFullTransform(DblArray12& tres, const T pelt) {
02584         PoseIdentity(tres);
02585         DblArray12 ttemp, ttemp2;
02586         for(size_t i = 0; i < pelt->getContents().getCount(); ++i) {
02587             getTransform(ttemp, pelt->getContents()[i]);
02588             PoseMult(ttemp2,tres,ttemp);
02589             memcpy(&tres[0],&ttemp2[0],sizeof(DblArray12));
02590         }
02591     }
02592 
02594     template <typename T> void _ExtractFullTransformFromChildren(DblArray12& tres, const T pelt) {
02595         PoseIdentity(tres);
02596         DblArray12 ttemp, ttemp2;
02597         daeTArray<daeElementRef> children;
02598         pelt->getChildren(children);
02599         for(size_t i = 0; i < children.getCount(); ++i) {
02600             getTransform(ttemp, children[i]);
02601             PoseMult(ttemp2,tres,ttemp);
02602             memcpy(&tres[0],&ttemp2[0],sizeof(DblArray12));
02603         }
02604     }
02605 
02606     // decompose a matrix into a scale and rigid transform (necessary for model scales)
02607 //    static void _decompose(const DblArray12& tm, DblArray12& tout, DblArray3& vscale)
02608 //    {
02609 //        DblArray4 quat;
02610 //        QuatFromMatrix(quat,tm);
02611 //        PoseFromQuat(tout,quat);
02612 //        tout[3] = tm[3];
02613 //        tout[7] = tm[7];
02614 //        tout[11] = tm[11];
02615 //        for(int i = 0; i < 3; ++i) {
02616 //            vscale[i] = (fabs(tm[0+i])+fabs(tm[4+i])+fabs(tm[8+i]))/(fabs(tout[0+i])+fabs(tout[4+i])+fabs(tout[8+i]));
02617 //        }
02618 //    }
02619 
02620     virtual void handleError( daeString msg )
02621     {
02622         if( _bOpeningZAE && (msg == string("Document is empty\n") || msg == string("Error parsing XML in daeLIBXMLPlugin::read\n")) ) {
02623             return; // collada-dom prints these messages even if no error
02624         }
02625         COLLADALOG_ERROR(str(boost::format("COLLADA error: %s")%msg));
02626     }
02627 
02628     virtual void handleWarning( daeString msg )
02629     {
02630         COLLADALOG_WARN(str(boost::format("COLLADA warning: %s")%msg));
02631     }
02632 
02633  private:
02634 
02639     static void _ExtractKinematicsVisualBindings(domInstance_with_extraRef viscene, domInstance_kinematics_sceneRef kiscene, KinematicsSceneBindings& bindings)
02640     {
02641         domKinematics_sceneRef kscene = daeSafeCast<domKinematics_scene> (kiscene->getUrl().getElement().cast());
02642         if (!kscene) {
02643             return;
02644         }
02645         for (size_t imodel = 0; imodel < kiscene->getBind_kinematics_model_array().getCount(); imodel++) {
02646             domArticulated_systemRef articulated_system; // if filled, contains robot-specific information, so create a robot
02647             domBind_kinematics_modelRef kbindmodel = kiscene->getBind_kinematics_model_array()[imodel];
02648             if (!kbindmodel->getNode()) {
02649                 COLLADALOG_WARN("do not support kinematics models without references to nodes");
02650                 continue;
02651             }
02652        
02653             // visual information
02654             domNodeRef node = daeSafeCast<domNode>(daeSidRef(kbindmodel->getNode(), viscene->getUrl().getElement()).resolve().elt);
02655             if (!node) {
02656                 COLLADALOG_WARN(str(boost::format("bind_kinematics_model does not reference valid node %sn")%kbindmodel->getNode()));
02657                 continue;
02658             }
02659 
02660             //  kinematics information
02661             daeElement* pelt = searchBinding(kbindmodel,kscene);
02662             domInstance_kinematics_modelRef kimodel = daeSafeCast<domInstance_kinematics_model>(pelt);
02663             if (!kimodel) {
02664                 if( !pelt ) {
02665                     COLLADALOG_WARN("bind_kinematics_model does not reference element");
02666                 }
02667                 else {
02668                     COLLADALOG_WARN(str(boost::format("bind_kinematics_model cannot find reference to %s:%s:n")%kbindmodel->getNode()%pelt->getElementName()));
02669                 }
02670                 continue;
02671             }
02672             bindings.listKinematicsVisualBindings.push_back(make_pair(node,kimodel));
02673         }
02674         // axis info
02675         for (size_t ijoint = 0; ijoint < kiscene->getBind_joint_axis_array().getCount(); ++ijoint) {
02676             domBind_joint_axisRef bindjoint = kiscene->getBind_joint_axis_array()[ijoint];
02677             daeElementRef pjtarget = daeSidRef(bindjoint->getTarget(), viscene->getUrl().getElement()).resolve().elt;
02678             if (!pjtarget) {
02679                 COLLADALOG_WARN(str(boost::format("Target Node '%s' not found")%bindjoint->getTarget()));
02680                 continue;
02681             }
02682             daeElement* pelt = searchBinding(bindjoint->getAxis(),kscene);
02683             domAxis_constraintRef pjointaxis = daeSafeCast<domAxis_constraint>(pelt);
02684             if (!pjointaxis) {
02685                 COLLADALOG_WARN(str(boost::format("joint axis for target %s")%bindjoint->getTarget()));
02686                 continue;
02687             }
02688             bindings.listAxisBindings.push_back(JointAxisBinding(pjtarget, pjointaxis, bindjoint->getValue(), NULL, NULL));
02689         }
02690     }
02691 
02692     static void _ExtractPhysicsBindings(domCOLLADA::domSceneRef allscene, KinematicsSceneBindings& bindings)
02693     {
02694         for(size_t iphysics = 0; iphysics < allscene->getInstance_physics_scene_array().getCount(); ++iphysics) {
02695             domPhysics_sceneRef pscene = daeSafeCast<domPhysics_scene>(allscene->getInstance_physics_scene_array()[iphysics]->getUrl().getElement().cast());
02696             for(size_t imodel = 0; imodel < pscene->getInstance_physics_model_array().getCount(); ++imodel) {
02697                 domInstance_physics_modelRef ipmodel = pscene->getInstance_physics_model_array()[imodel];
02698                 domPhysics_modelRef pmodel = daeSafeCast<domPhysics_model> (ipmodel->getUrl().getElement().cast());
02699                 domNodeRef nodephysicsoffset = daeSafeCast<domNode>(ipmodel->getParent().getElement().cast());
02700                 for(size_t ibody = 0; ibody < ipmodel->getInstance_rigid_body_array().getCount(); ++ibody) {
02701                     LinkBinding lb;
02702                     lb.irigidbody = ipmodel->getInstance_rigid_body_array()[ibody];
02703                     lb.node = daeSafeCast<domNode>(lb.irigidbody->getTarget().getElement().cast());
02704                     lb.rigidbody = daeSafeCast<domRigid_body>(daeSidRef(lb.irigidbody->getBody(),pmodel).resolve().elt);
02705                     lb.nodephysicsoffset = nodephysicsoffset;
02706                     if( !!lb.rigidbody && !!lb.node ) {
02707                         bindings.listLinkBindings.push_back(lb);
02708                     }
02709                 }
02710                 for(size_t iconst = 0; iconst < ipmodel->getInstance_rigid_constraint_array().getCount(); ++iconst) {
02711                     ConstraintBinding cb;
02712                     cb.irigidconstraint = ipmodel->getInstance_rigid_constraint_array()[iconst];
02713                     cb.rigidconstraint = daeSafeCast<domRigid_constraint>(daeSidRef(cb.irigidconstraint->getConstraint(),pmodel).resolve().elt);
02714                     cb.node = daeSafeCast<domNode>(cb.rigidconstraint->getAttachment()->getRigid_body().getElement());
02715                     if( !!cb.rigidconstraint ) {
02716                         bindings.listConstraintBindings.push_back(cb);
02717                     }
02718                 }
02719             }
02720         }
02721     }
02722 
02723     domTechniqueRef _ExtractOpenRAVEProfile(const domTechnique_Array& arr)
02724     {
02725         for(size_t i = 0; i < arr.getCount(); ++i) {
02726             if( strcmp(arr[i]->getProfile(), "OpenRAVE") == 0 ) {
02727                 return arr[i];
02728             }
02729         }
02730         return domTechniqueRef();
02731     }
02732 
02733     daeElementRef _ExtractOpenRAVEProfile(const daeElementRef pelt)
02734     {
02735         daeTArray<daeElementRef> children;
02736         pelt->getChildren(children);
02737         for(size_t i = 0; i < children.getCount(); ++i) {
02738             if( children[i]->getElementName() == string("technique") && children[i]->hasAttribute("profile") && children[i]->getAttribute("profile") == string("OpenRAVE") ) {
02739                 return children[i];
02740             }
02741         }
02742         return daeElementRef();
02743     }
02744 
02746     boost::shared_ptr<std::string> _ExtractInterfaceType(const daeElementRef pelt) {
02747         daeTArray<daeElementRef> children;
02748         pelt->getChildren(children);
02749         for(size_t i = 0; i < children.getCount(); ++i) {
02750             if( children[i]->getElementName() == string("interface_type") ) {
02751                 daeElementRef ptec = _ExtractOpenRAVEProfile(children[i]);
02752                 if( !!ptec ) {
02753                     daeElement* ptype = ptec->getChild("interface");
02754                     if( !!ptype ) {
02755                         return boost::shared_ptr<std::string>(new std::string(ptype->getCharData()));
02756                     }
02757                 }
02758             }
02759         }
02760         return boost::shared_ptr<std::string>();
02761     }
02762 
02764     boost::shared_ptr<std::string> _ExtractInterfaceType(const domExtra_Array& arr) {
02765         for(size_t i = 0; i < arr.getCount(); ++i) {
02766             if( strcmp(arr[i]->getType(),"interface_type") == 0 ) {
02767                 domTechniqueRef tec = _ExtractOpenRAVEProfile(arr[i]->getTechnique_array());
02768                 if( !!tec ) {
02769                     daeElement* ptype = tec->getChild("interface");
02770                     if( !!ptype ) {
02771                         return boost::shared_ptr<std::string>(new std::string(ptype->getCharData()));
02772                     }
02773                 }
02774             }
02775         }
02776         return boost::shared_ptr<std::string>();
02777     }
02778 
02779     std::string _ExtractLinkName(domLinkRef pdomlink) {
02780         std::string linkname;
02781         if( !!pdomlink ) {
02782             if( !!pdomlink->getName() ) {
02783                 linkname = pdomlink->getName();
02784             }
02785             if( linkname.size() == 0 && !!pdomlink->getID() ) {
02786                 linkname = pdomlink->getID();
02787             }
02788         }
02789         return _ConvertToValidName(linkname);
02790     }
02791 
02792     bool _checkMathML(daeElementRef pelt,const string& type)
02793     {
02794         if( pelt->getElementName()==type ) {
02795             return true;
02796         }
02797         // check the substring after ':', the substring before is the namespace set in some parent attribute
02798         string name = pelt->getElementName();
02799         size_t pos = name.find_last_of(':');
02800         if( pos == string::npos ) {
02801             return false;
02802         }
02803         return name.substr(pos+1)==type;
02804     }
02805 
02806     std::pair<boost::shared_ptr<LinkInfo> ,domJointRef> _getJointFromRef(xsToken targetref, daeElementRef peltref, BodyInfoCollada_impl* pkinbody) {
02807         daeElement* peltjoint = daeSidRef(targetref, peltref).resolve().elt;
02808         domJointRef pdomjoint = daeSafeCast<domJoint> (peltjoint);
02809         if (!pdomjoint) {
02810             domInstance_jointRef pdomijoint = daeSafeCast<domInstance_joint> (peltjoint);
02811             if (!!pdomijoint) {
02812                 pdomjoint = daeSafeCast<domJoint> (pdomijoint->getUrl().getElement().cast());
02813             }
02814         }
02815 
02816         if (!pdomjoint) {
02817             COLLADALOG_WARN(str(boost::format("could not find collada joint '%s'!")%targetref));
02818             return std::make_pair(boost::shared_ptr<LinkInfo>(),domJointRef());
02819         }
02820 
02821         if( string(targetref).find("./") != 0 ) {
02822             std::map<std::string,boost::shared_ptr<LinkInfo> >::iterator itjoint = _mapJointIds.find(targetref);
02823             if( itjoint != _mapJointIds.end() ) {
02824                 return std::make_pair(itjoint->second,pdomjoint);
02825             }
02826             COLLADALOG_WARN(str(boost::format("failed to find joint target '%s' in _mapJointIds")%targetref));
02827         }
02828 
02829         boost::shared_ptr<LinkInfo>  pjoint = GetLink(pdomjoint->getName());
02830         if(!pjoint) {
02831             COLLADALOG_WARN(str(boost::format("could not find openrave joint '%s'!")%pdomjoint->getName()));
02832         }
02833         return std::make_pair(pjoint,pdomjoint);
02834     }
02835 
02837     std::string _getElementName(daeElementRef pelt) {
02838         std::string name = pelt->getElementName();
02839         std::size_t pos = name.find_last_of(':');
02840         if( pos != string::npos ) {
02841             return name.substr(pos+1);
02842         }
02843         return name;
02844     }
02845 
02846     std::string _ExtractParentId(daeElementRef p) {
02847         while(!!p) {
02848             if( p->hasAttribute("id") ) {
02849                 return p->getAttribute("id");
02850             }
02851             p = p->getParent();
02852         }
02853         return "";
02854     }
02855 
02857     std::string _ExtractMathML(daeElementRef proot, BodyInfoCollada_impl* pkinbody, daeElementRef pelt)
02858     {
02859         std::string name = _getElementName(pelt);
02860         std::string eq;
02861         daeTArray<daeElementRef> children;
02862         pelt->getChildren(children);
02863         if( name == "math" ) {
02864             for(std::size_t ic = 0; ic < children.getCount(); ++ic) {
02865                 std::string childname = _getElementName(children[ic]);
02866                 if( childname == "apply" || childname == "csymbol" || childname == "cn" || childname == "ci" ) {
02867                     eq = _ExtractMathML(proot, pkinbody, children[ic]);
02868                 }
02869                 else {
02870                     throw ModelLoader::ModelLoaderException(str(boost::format("_ExtractMathML: do not support element %s in mathml")%childname).c_str());
02871                 }
02872             }
02873         }
02874         else if( name == "apply" ) {
02875             if( children.getCount() == 0 ) {
02876                 return eq;
02877             }
02878             string childname = _getElementName(children[0]);
02879             if( childname == "plus" ) {
02880                 eq += '(';
02881                 for(size_t ic = 1; ic < children.getCount(); ++ic) {
02882                     eq += _ExtractMathML(proot, pkinbody, children[ic]);
02883                     if( ic+1 < children.getCount() ) {
02884                         eq += '+';
02885                     }
02886                 }
02887                 eq += ')';
02888             }
02889             else if( childname == "quotient" ) {
02890                 COLLADA_ASSERT(children.getCount()==3);
02891                 eq += str(boost::format("floor(%s/%s)")%_ExtractMathML(proot,pkinbody,children[1])%_ExtractMathML(proot,pkinbody,children[2]));
02892             }
02893             else if( childname == "divide" ) {
02894                 COLLADA_ASSERT(children.getCount()==3);
02895                 eq += str(boost::format("(%s/%s)")%_ExtractMathML(proot,pkinbody,children[1])%_ExtractMathML(proot,pkinbody,children[2]));
02896             }
02897             else if( childname == "minus" ) {
02898                 COLLADA_ASSERT(children.getCount()>1 && children.getCount()<=3);
02899                 if( children.getCount() == 2 ) {
02900                     eq += str(boost::format("(-%s)")%_ExtractMathML(proot,pkinbody,children[1]));
02901                 }
02902                 else {
02903                     eq += str(boost::format("(%s-%s)")%_ExtractMathML(proot,pkinbody,children[1])%_ExtractMathML(proot,pkinbody,children[2]));
02904                 }
02905             }
02906             else if( childname == "power" ) {
02907                 COLLADA_ASSERT(children.getCount()==3);
02908                 std::string sbase = _ExtractMathML(proot,pkinbody,children[1]);
02909                 std::string sexp = _ExtractMathML(proot,pkinbody,children[2]);
02910 //                try {
02911 //                    int degree = boost::lexical_cast<int>(sexp);
02912 //                    if( degree == 1 ) {
02913 //                        eq += str(boost::format("(%s)")%sbase);
02914 //                    }
02915 //                    else if( degree == 2 ) {
02916 //                        eq += str(boost::format("sqr(%s)")%sbase);
02917 //                    }
02918 //                    else {
02919 //                        eq += str(boost::format("pow(%s,%s)")%sbase%sexp);
02920 //                    }
02921 //                }
02922 //                catch(const boost::bad_lexical_cast&) {
02923                     eq += str(boost::format("pow(%s,%s)")%sbase%sexp);
02924                     //}
02925             }
02926             else if( childname == "rem" ) {
02927                 COLLADA_ASSERT(children.getCount()==3);
02928                 eq += str(boost::format("(%s%%%s)")%_ExtractMathML(proot,pkinbody,children[1])%_ExtractMathML(proot,pkinbody,children[2]));
02929             }
02930             else if( childname == "times" ) {
02931                 eq += '(';
02932                 for(size_t ic = 1; ic < children.getCount(); ++ic) {
02933                     eq += _ExtractMathML(proot, pkinbody, children[ic]);
02934                     if( ic+1 < children.getCount() ) {
02935                         eq += '*';
02936                     }
02937                 }
02938                 eq += ')';
02939             }
02940             else if( childname == "root" ) {
02941                 COLLADA_ASSERT(children.getCount()==3);
02942                 string sdegree, snum;
02943                 for(size_t ic = 1; ic < children.getCount(); ++ic) {
02944                     if( _getElementName(children[ic]) == string("degree") ) {
02945                         sdegree = _ExtractMathML(proot,pkinbody,children[ic]->getChildren()[0]);
02946                     }
02947                     else {
02948                         snum = _ExtractMathML(proot,pkinbody,children[ic]);
02949                     }
02950                 }
02951                 try {
02952                     int degree = boost::lexical_cast<int>(sdegree);
02953                     if( degree == 1 ) {
02954                         eq += str(boost::format("(%s)")%snum);
02955                     }
02956                     else if( degree == 2 ) {
02957                         eq += str(boost::format("sqrt(%s)")%snum);
02958                     }
02959                     else if( degree == 3 ) {
02960                         eq += str(boost::format("cbrt(%s)")%snum);
02961                     }
02962                     else {
02963                         eq += str(boost::format("pow(%s,1.0/%s)")%snum%sdegree);
02964                     }
02965                 }
02966                 catch(const boost::bad_lexical_cast&) {
02967                     eq += str(boost::format("pow(%s,1.0/%s)")%snum%sdegree);
02968                 }
02969             }
02970             else if( childname == "and" ) {
02971                 eq += '(';
02972                 for(size_t ic = 1; ic < children.getCount(); ++ic) {
02973                     eq += _ExtractMathML(proot, pkinbody, children[ic]);
02974                     if( ic+1 < children.getCount() ) {
02975                         eq += '&';
02976                     }
02977                 }
02978                 eq += ')';
02979             }
02980             else if( childname == "or" ) {
02981                 eq += '(';
02982                 for(size_t ic = 1; ic < children.getCount(); ++ic) {
02983                     eq += _ExtractMathML(proot, pkinbody, children[ic]);
02984                     if( ic+1 < children.getCount() ) {
02985                         eq += '|';
02986                     }
02987                 }
02988                 eq += ')';
02989             }
02990             else if( childname == "not" ) {
02991                 COLLADA_ASSERT(children.getCount()==2);
02992                 eq += str(boost::format("(!%s)")%_ExtractMathML(proot,pkinbody,children[1]));
02993             }
02994             else if( childname == "floor" ) {
02995                 COLLADA_ASSERT(children.getCount()==2);
02996                 eq += str(boost::format("floor(%s)")%_ExtractMathML(proot,pkinbody,children[1]));
02997             }
02998             else if( childname == "ceiling" ) {
02999                 COLLADA_ASSERT(children.getCount()==2);
03000                 eq += str(boost::format("ceil(%s)")%_ExtractMathML(proot,pkinbody,children[1]));
03001             }
03002             else if( childname == "eq" ) {
03003                 COLLADA_ASSERT(children.getCount()==3);
03004                 eq += str(boost::format("(%s=%s)")%_ExtractMathML(proot,pkinbody,children[1])%_ExtractMathML(proot,pkinbody,children[2]));
03005             }
03006             else if( childname == "neq" ) {
03007                 COLLADA_ASSERT(children.getCount()==3);
03008                 eq += str(boost::format("(%s!=%s)")%_ExtractMathML(proot,pkinbody,children[1])%_ExtractMathML(proot,pkinbody,children[2]));
03009             }
03010             else if( childname == "gt" ) {
03011                 COLLADA_ASSERT(children.getCount()==3);
03012                 eq += str(boost::format("(%s>%s)")%_ExtractMathML(proot,pkinbody,children[1])%_ExtractMathML(proot,pkinbody,children[2]));
03013             }
03014             else if( childname == "lt" ) {
03015                 COLLADA_ASSERT(children.getCount()==3);
03016                 eq += str(boost::format("(%s<%s)")%_ExtractMathML(proot,pkinbody,children[1])%_ExtractMathML(proot,pkinbody,children[2]));
03017             }
03018             else if( childname == "geq" ) {
03019                 COLLADA_ASSERT(children.getCount()==3);
03020                 eq += str(boost::format("(%s>=%s)")%_ExtractMathML(proot,pkinbody,children[1])%_ExtractMathML(proot,pkinbody,children[2]));
03021             }
03022             else if( childname == "leq" ) {
03023                 COLLADA_ASSERT(children.getCount()==3);
03024                 eq += str(boost::format("(%s<=%s)")%_ExtractMathML(proot,pkinbody,children[1])%_ExtractMathML(proot,pkinbody,children[2]));
03025             }
03026             else if( childname == "ln" ) {
03027                 COLLADA_ASSERT(children.getCount()==2);
03028                 eq += str(boost::format("log(%s)")%_ExtractMathML(proot,pkinbody,children[1]));
03029             }
03030             else if( childname == "log" ) {
03031                 COLLADA_ASSERT(children.getCount()==2 || children.getCount()==3);
03032                 string sbase="10", snum;
03033                 for(size_t ic = 1; ic < children.getCount(); ++ic) {
03034                     if( _getElementName(children[ic]) == string("logbase") ) {
03035                         sbase = _ExtractMathML(proot,pkinbody,children[ic]->getChildren()[0]);
03036                     }
03037                     else {
03038                         snum = _ExtractMathML(proot,pkinbody,children[ic]);
03039                     }
03040                 }
03041                 try {
03042                     int base = boost::lexical_cast<int>(sbase);
03043                     if( base == 10 ) {
03044                         eq += str(boost::format("log10(%s)")%snum);
03045                     }
03046                     else if( base == 2 ) {
03047                         eq += str(boost::format("log2(%s)")%snum);
03048                     }
03049                     else {
03050                         eq += str(boost::format("(log(%s)/log(%s))")%snum%sbase);
03051                     }
03052                 }
03053                 catch(const boost::bad_lexical_cast&) {
03054                     eq += str(boost::format("(log(%s)/log(%s))")%snum%sbase);
03055                 }
03056             }
03057             else if( childname == "arcsin" ) {
03058                 COLLADA_ASSERT(children.getCount()==2);
03059                 eq += str(boost::format("asin(%s)")%_ExtractMathML(proot,pkinbody,children[1]));
03060             }
03061             else if( childname == "arccos" ) {
03062                 COLLADA_ASSERT(children.getCount()==2);
03063                 eq += str(boost::format("acos(%s)")%_ExtractMathML(proot,pkinbody,children[1]));
03064             }
03065             else if( childname == "arctan" ) {
03066                 COLLADA_ASSERT(children.getCount()==2);
03067                 eq += str(boost::format("atan(%s)")%_ExtractMathML(proot,pkinbody,children[1]));
03068             }
03069             else if( childname == "arccosh" ) {
03070                 COLLADA_ASSERT(children.getCount()==2);
03071                 eq += str(boost::format("acosh(%s)")%_ExtractMathML(proot,pkinbody,children[1]));
03072             }
03073             else if( childname == "arccot" ) {
03074                 COLLADA_ASSERT(children.getCount()==2);
03075                 eq += str(boost::format("acot(%s)")%_ExtractMathML(proot,pkinbody,children[1]));
03076             }
03077             else if( childname == "arccoth" ) {
03078                 COLLADA_ASSERT(children.getCount()==2);
03079                 eq += str(boost::format("acoth(%s)")%_ExtractMathML(proot,pkinbody,children[1]));
03080             }
03081             else if( childname == "arccsc" ) {
03082                 COLLADA_ASSERT(children.getCount()==2);
03083                 eq += str(boost::format("acsc(%s)")%_ExtractMathML(proot,pkinbody,children[1]));
03084             }
03085             else if( childname == "arccsch" ) {
03086                 COLLADA_ASSERT(children.getCount()==2);
03087                 eq += str(boost::format("acsch(%s)")%_ExtractMathML(proot,pkinbody,children[1]));
03088             }
03089             else if( childname == "arcsec" ) {
03090                 COLLADA_ASSERT(children.getCount()==2);
03091                 eq += str(boost::format("asec(%s)")%_ExtractMathML(proot,pkinbody,children[1]));
03092             }
03093             else if( childname == "arcsech" ) {
03094                 COLLADA_ASSERT(children.getCount()==2);
03095                 eq += str(boost::format("asech(%s)")%_ExtractMathML(proot,pkinbody,children[1]));
03096             }
03097             else if( childname == "arcsinh" ) {
03098                 COLLADA_ASSERT(children.getCount()==2);
03099                 eq += str(boost::format("asinh(%s)")%_ExtractMathML(proot,pkinbody,children[1]));
03100             }
03101             else if( childname == "arctanh" ) {
03102                 COLLADA_ASSERT(children.getCount()==2);
03103                 eq += str(boost::format("atanh(%s)")%_ExtractMathML(proot,pkinbody,children[1]));
03104             }
03105             else if( childname == "implies" || childname == "forall" || childname == "exists" || childname == "conjugate" || childname == "arg" || childname == "real" || childname == "imaginary" || childname == "lcm" || childname == "factorial" || childname == "xor") {
03106                 throw ModelLoader::ModelLoaderException(str(boost::format("_ExtractMathML: %s function in <apply> tag not supported")%childname).c_str());
03107             }
03108             else if( childname == "csymbol" ) {
03109                 if( children[0]->getAttribute("encoding")==string("text/xml") ) {
03110                     domFormulaRef pformula;
03111                     string functionname;
03112                     if( children[0]->hasAttribute("definitionURL") ) {
03113                         // search for the formula in library_formulas
03114                         string formulaurl = children[0]->getAttribute("definitionURL");
03115                         if( formulaurl.size() > 0 ) {
03116                             daeElementRef pelt = _getElementFromUrl(daeURI(*children[0],formulaurl));
03117                             pformula = daeSafeCast<domFormula>(pelt);
03118                             if( !pformula ) {
03119                                 COLLADALOG_WARN(str(boost::format("could not find csymbol %s formula")%children[0]->getAttribute("definitionURL")));
03120                             }
03121                             else {
03122                                 COLLADALOG_DEBUG(str(boost::format("csymbol formula %s found")%pformula->getId()));
03123                             }
03124                         }
03125                     }
03126                     if( !pformula ) {
03127                         if( children[0]->hasAttribute("type") ) {
03128                             if( children[0]->getAttribute("type") == "function" ) {
03129                                 functionname = children[0]->getCharData();
03130                             }
03131                         }
03132                     }
03133                     else {
03134                         if( !!pformula->getName() ) {
03135                             functionname = pformula->getName();
03136                         }
03137                         else {
03138                             functionname = children[0]->getCharData();
03139                         }
03140                     }
03141 
03142                     if( functionname == "INRANGE" ) {
03143                         COLLADA_ASSERT(children.getCount()==4);
03144                         string a = _ExtractMathML(proot,pkinbody,children[1]), b = _ExtractMathML(proot,pkinbody,children[2]), c = _ExtractMathML(proot,pkinbody,children[3]);
03145                         eq += str(boost::format("((%s>=%s)&(%s<=%s))")%a%b%a%c);
03146                     }
03147                     else if( functionname == "SSSA" || functionname == "SASA" || functionname == "SASS" ) {
03148                         COLLADA_ASSERT(children.getCount()==4);
03149                         eq += str(boost::format("%s(%s,%s,%s)")%functionname%_ExtractMathML(proot,pkinbody,children[1])%_ExtractMathML(proot,pkinbody,children[2])%_ExtractMathML(proot,pkinbody,children[3]));
03150                     }
03151                     else if( functionname == "atan2") {
03152                         COLLADA_ASSERT(children.getCount()==3);
03153                         eq += str(boost::format("atan2(%s,%s)")%_ExtractMathML(proot,pkinbody,children[1])%_ExtractMathML(proot,pkinbody,children[2]));
03154                     }
03155                     else {
03156                         COLLADALOG_WARN(str(boost::format("csymbol %s not implemented")%functionname));
03157                         eq += "1";
03158                     }
03159                 }
03160                 else if( children[0]->getAttribute("encoding")!=string("COLLADA") ) {
03161                     COLLADALOG_WARN(str(boost::format("_ExtractMathML: csymbol '%s' has unknown encoding '%s'")%children[0]->getCharData()%children[0]->getAttribute("encoding")));
03162                 }
03163                 else {
03164                     eq += _ExtractMathML(proot,pkinbody,children[0]);
03165                 }
03166             }
03167             else {
03168                 // make a function call
03169                 eq += childname;
03170                 eq += '(';
03171                 for(size_t ic = 1; ic < children.getCount(); ++ic) {
03172                     eq += _ExtractMathML(proot, pkinbody, children[ic]);
03173                     if( ic+1 < children.getCount() ) {
03174                         eq += ',';
03175                     }
03176                 }
03177                 eq += ')';
03178             }
03179         }
03180         else if( name == "csymbol" ) {
03181             if( !pelt->hasAttribute("encoding") ) {
03182                 COLLADALOG_WARN(str(boost::format("_ExtractMathML: csymbol '%s' does not have any encoding")%pelt->getCharData()));
03183             }
03184             else if( pelt->getAttribute("encoding")!=string("COLLADA") ) {
03185                 COLLADALOG_WARN(str(boost::format("_ExtractMathML: csymbol '%s' has unknown encoding '%s'")%pelt->getCharData()%pelt->getAttribute("encoding")));
03186             }
03187             boost::shared_ptr<LinkInfo> pjoint = _getJointFromRef(pelt->getCharData().c_str(),proot,pkinbody).first;
03188             if( !pjoint ) {
03189                 COLLADALOG_WARN(str(boost::format("_ExtractMathML: failed to find csymbol: %s")%pelt->getCharData()));
03190                 eq = pelt->getCharData();
03191             }
03192             string jointname(CORBA::String_var(pjoint->name));
03193             //if( pjoint->GetDOF() > 1 ) {
03194             //COLLADALOG_WARN(str(boost::format("formulas do not support using joints with > 1 DOF yet (%s)")%pjoint->GetName()));
03195             if( _mapJointUnits.find(pjoint) != _mapJointUnits.end() && _mapJointUnits[pjoint].at(0) != 1 ) {
03196                 eq = str(boost::format("(%f*%s)")%(1/_mapJointUnits[pjoint].at(0))%jointname);
03197             }
03198             else {
03199                 eq = jointname;
03200             }
03201         }
03202         else if( name == "cn" ) {
03203             eq = pelt->getCharData();
03204         }
03205         else if( name == "ci" ) {
03206             eq = pelt->getCharData();
03207         }
03208         else if( name == "pi" ) {
03209             eq = "3.14159265358979323846";
03210         }
03211         else {
03212             COLLADALOG_WARN(str(boost::format("mathml unprocessed tag: %s")));
03213         }
03214         return eq;
03215     }
03216 
03217     static inline bool _IsValidCharInName(char c) { return isalnum(c) || c == '_' || c == '.' || c == '-' || c == '/'; }
03218     static inline bool _IsValidName(const std::string& s) {
03219         if( s.size() == 0 ) {
03220             return false;
03221         }
03222         return std::count_if(s.begin(), s.end(), _IsValidCharInName) == (int)s.size();
03223     }
03224 
03225     inline std::string _ConvertToValidName(const std::string& name) {
03226         if( name.size() == 0 ) {
03227             return str(boost::format("__dummy%d")%_nGlobalIndex++);
03228         }
03229         if( _IsValidName(name) ) {
03230             return name;
03231         }
03232         std::string newname = name;
03233         for(size_t i = 0; i < newname.size(); ++i) {
03234             if( !_IsValidCharInName(newname[i]) ) {
03235                 newname[i] = '_';
03236             }
03237         }
03238         COLLADALOG_WARN(str(boost::format("name '%s' is not a valid name, converting to '%s'")%name%newname));
03239         return newname;
03240     }
03241 
03242     inline static dReal _GetUnitScale(daeElementRef pelt, dReal startscale)
03243     {
03244         // getChild could be optimized since asset tag is supposed to appear as the first element
03245         domExtraRef pextra = daeSafeCast<domExtra> (pelt->getChild("extra"));
03246         if( !!pextra && !!pextra->getAsset() && !!pextra->getAsset()->getUnit() ) {
03247             return pextra->getAsset()->getUnit()->getMeter();
03248         }
03249         if( !!pelt->getParent() ) {
03250             return _GetUnitScale(pelt->getParent(),startscale);
03251         }
03252         return startscale;
03253     }
03254         
03255     boost::shared_ptr<DAE> _dae;
03256     bool _bOpeningZAE;
03257     domCOLLADA* _dom;
03258     dReal _fGlobalScale;
03259     std::map<boost::shared_ptr<LinkInfo> , std::vector<dReal> > _mapJointUnits;
03260     std::map<std::string,boost::shared_ptr<LinkInfo> > _mapJointIds;
03261     std::map<std::string,boost::shared_ptr<LinkInfo> > _mapLinkNames;
03262     std::vector<boost::shared_ptr<LinkInfo> > _veclinks;
03263     std::vector<std::string> _veclinknames;
03264     int _nGlobalSensorId, _nGlobalActuatorId, _nGlobalManipulatorId, _nGlobalIndex;
03265     std::string _filename;
03266     DblArray12 rootOrigin;
03267     DblArray12 visualRootOrigin;
03268 };
03269 
03270 BodyInfoCollada_impl::BodyInfoCollada_impl(PortableServer::POA_ptr poa) :
03271     ShapeSetInfo_impl(poa)
03272 {
03273     lastUpdate_ = 0;
03274 }
03275 
03276 
03277 BodyInfoCollada_impl::~BodyInfoCollada_impl()
03278 {   
03279 }
03280 
03281 const std::string& BodyInfoCollada_impl::topUrl()
03282 {
03283     return url_;
03284 }
03285 
03286 
03287 char* BodyInfoCollada_impl::name()
03288 {
03289     return CORBA::string_dup(name_.c_str());
03290 }
03291 
03292 
03293 char* BodyInfoCollada_impl::url()
03294 {
03295     return CORBA::string_dup(url_.c_str());
03296 }
03297 
03298 
03299 StringSequence* BodyInfoCollada_impl::info()
03300 {
03301     return new StringSequence(info_);
03302 }
03303 
03304 
03305 LinkInfoSequence* BodyInfoCollada_impl::links()
03306 {
03307     return new LinkInfoSequence(links_);
03308 }
03309 
03310 
03311 AllLinkShapeIndexSequence* BodyInfoCollada_impl::linkShapeIndices()
03312 {
03313     return new AllLinkShapeIndexSequence(linkShapeIndices_);
03314 }
03315 
03316 ExtraJointInfoSequence* BodyInfoCollada_impl::extraJoints()
03317 {
03318         return new ExtraJointInfoSequence(extraJoints_);
03319 }
03320 
03321 boost::mutex BodyInfoCollada_impl::lock_;
03322 void BodyInfoCollada_impl::loadModelFile(const std::string& url)
03323 {
03324     boost::mutex::scoped_lock lock(lock_);
03325     ColladaReader reader;
03326     if( !reader.InitFromURL(url) ) {
03327         throw ModelLoader::ModelLoaderException("The model file cannot be found.");
03328     }
03329     if( !reader.Extract(this) ) {
03330         throw ModelLoader::ModelLoaderException("The model file cannot be loaded.");
03331     }
03332 }
03333 
03334 void BodyInfoCollada_impl::setParam(std::string param, bool value)
03335 {
03336     if(param == "readImage") {
03337         readImage_ = value;
03338     }
03339 }
03340 
03341 bool BodyInfoCollada_impl::getParam(std::string param)
03342 {
03343     if(param == "readImage") {
03344         return readImage_;
03345     }
03346     return false;
03347 }
03348 
03349 void BodyInfoCollada_impl::setParam(std::string param, int value)
03350 {
03351     if(param == "AABBType") {
03352         AABBdataType_ = (OpenHRP::ModelLoader::AABBdataType)value;
03353     }
03354 }
03355 
03356 void BodyInfoCollada_impl::setColdetModel(ColdetModelPtr& coldetModel, TransformedShapeIndexSequence shapeIndices, const Matrix44& Tparent, int& vertexIndex, int& triangleIndex){
03357     for(unsigned int i=0; i < shapeIndices.length(); i++){
03358         setColdetModelTriangles(coldetModel, shapeIndices[i], Tparent, vertexIndex, triangleIndex);
03359     }
03360 }
03361 
03362 void BodyInfoCollada_impl::setColdetModelTriangles
03363 (ColdetModelPtr& coldetModel, const TransformedShapeIndex& tsi, const Matrix44& Tparent, int& vertexIndex, int& triangleIndex)
03364 {
03365     short shapeIndex = tsi.shapeIndex;
03366     const DblArray12& M = tsi.transformMatrix;;
03367     Matrix44 T, Tlocal;
03368     Tlocal << M[0], M[1], M[2],  M[3],
03369              M[4], M[5], M[6],  M[7],
03370              M[8], M[9], M[10], M[11],
03371              0.0,  0.0,  0.0,   1.0;
03372     T = Tparent * Tlocal;
03373     
03374     const ShapeInfo& shapeInfo = shapes_[shapeIndex];
03375     
03376     const FloatSequence& vertices = shapeInfo.vertices;
03377     const LongSequence& triangles = shapeInfo.triangles;
03378     const int numTriangles = triangles.length() / 3;
03379     
03380     coldetModel->setNumTriangles(coldetModel->getNumTriangles()+numTriangles);
03381     int numVertices = numTriangles * 3;
03382     coldetModel->setNumVertices(coldetModel->getNumVertices()+numVertices);
03383     for(int j=0; j < numTriangles; ++j){
03384         int vertexIndexTop = vertexIndex;
03385         for(int k=0; k < 3; ++k){
03386             long orgVertexIndex = shapeInfo.triangles[j * 3 + k];
03387             int p = orgVertexIndex * 3;
03388             Vector4 v(T * Vector4(vertices[p+0], vertices[p+1], vertices[p+2], 1.0));
03389             coldetModel->setVertex(vertexIndex++, (float)v[0], (float)v[1], (float)v[2]);
03390         }
03391         coldetModel->setTriangle(triangleIndex++, vertexIndexTop, vertexIndexTop + 1, vertexIndexTop + 2);
03392     }
03393     
03394 }
03395 
03396 void BodyInfoCollada_impl::changetoBoundingBox(unsigned int* inputData){
03397     const double EPS = 1.0e-6;
03398     createAppearanceInfo();
03399     std::vector<Vector3> boxSizeMap;
03400     std::vector<Vector3> boundingBoxData;
03401     
03402     for(unsigned int i=0; i<links_.length(); i++){
03403         int _depth;
03404         if( AABBdataType_ == OpenHRP::ModelLoader::AABB_NUM )
03405             _depth = linkColdetModels[i]->numofBBtoDepth(inputData[i]);
03406         else
03407             _depth = inputData[i];
03408         if( _depth >= links_[i].AABBmaxDepth)
03409             _depth = links_[i].AABBmaxDepth-1;
03410         if(_depth >= 0 ){
03411             linkColdetModels[i]->getBoundingBoxData(_depth, boundingBoxData);
03412             std::vector<TransformedShapeIndex> tsiMap;
03413             links_[i].shapeIndices.length(0);
03414             SensorInfoSequence& sensors = links_[i].sensors;
03415             for (unsigned int j=0; j<sensors.length(); j++){
03416                 SensorInfo& sensor = sensors[j];
03417                 sensor.shapeIndices.length(0);
03418             }
03419 
03420             for(unsigned int j=0; j<boundingBoxData.size()/2; j++){
03421 
03422                 bool flg=false;
03423                 unsigned int k=0;
03424                 for( ; k<boxSizeMap.size(); k++)
03425                     if((boxSizeMap[k] - boundingBoxData[j*2+1]).norm() < EPS)
03426                         break;
03427                 if( k<boxSizeMap.size() )
03428                     flg=true;
03429                 else{
03430                     boxSizeMap.push_back(boundingBoxData[j*2+1]);
03431                     setBoundingBoxData(boundingBoxData[j*2+1],k);
03432                 }
03433 
03434                 if(flg){
03435                     unsigned int l=0;
03436                     for( ; l<tsiMap.size(); l++){
03437                         Vector3 p(tsiMap[l].transformMatrix[3],tsiMap[l].transformMatrix[7],tsiMap[l].transformMatrix[11]);
03438                         if((p - boundingBoxData[j*2]).norm() < EPS && tsiMap[l].shapeIndex == (int)k)
03439                             break;
03440                     }
03441                     if( l==tsiMap.size() )
03442                         flg=false;
03443                 }
03444 
03445                 if(!flg){
03446                     int num = links_[i].shapeIndices.length();
03447                     links_[i].shapeIndices.length(num+1);
03448                     TransformedShapeIndex& tsi = links_[i].shapeIndices[num];
03449                     tsi.inlinedShapeTransformMatrixIndex = -1;
03450                     tsi.shapeIndex = k;
03451                     Matrix44 T(Matrix44::Identity());
03452                     for(int p = 0,row=0; row < 3; ++row)
03453                        for(int col=0; col < 4; ++col)
03454                             if(col==3){
03455                                 switch(row){
03456                                     case 0:
03457                                         tsi.transformMatrix[p++] = boundingBoxData[j*2][0];
03458                                         break;
03459                                      case 1:
03460                                         tsi.transformMatrix[p++] = boundingBoxData[j*2][1];
03461                                         break;
03462                                      case 2:
03463                                         tsi.transformMatrix[p++] = boundingBoxData[j*2][2];
03464                                         break;
03465                                      default:
03466                                         ;
03467                                 }
03468                             }else
03469                                 tsi.transformMatrix[p++] = T(row, col);
03470 
03471                     tsiMap.push_back(tsi);
03472                 }
03473             }
03474         }   
03475         linkShapeIndices_[i] = links_[i].shapeIndices;
03476     }
03477 }
03478 
03479 bool BodyInfoCollada_impl::checkInlineFileUpdateTime()
03480 {
03481     bool ret=true;
03482     for( std::map<std::string, time_t>::iterator it=fileTimeMap.begin(); it != fileTimeMap.end(); ++it){
03483         struct stat statbuff;
03484         time_t mtime = 0;
03485         if( stat( it->first.c_str(), &statbuff ) == 0 ){
03486             mtime = statbuff.st_mtime;
03487         }  
03488         if(it->second!=mtime){
03489             ret=false;
03490             break;
03491         }
03492     }
03493     return ret;
03494 }


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:52