00001
00002
00003
00004
00005
00006
00007
00008
00009
00010
00011
00012
00013
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>
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
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
00154 }
00155
00156 bool InitFromURL(const string& url)
00157 {
00158 string filename( deleteURLScheme( url ) );
00159
00160
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
00224
00225
00226
00227
00228 COLLADALOG_WARN("fill asset info");
00229 _ResetRobotCache();
00230
00231
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
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
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
00313
00314
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
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
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
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
00424 if (!probot) {
00425
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
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
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
00548 domKinematics_model_techniqueRef ktec = kmodel->getTechnique_common();
00549
00550
00551 for (size_t ijoint = 0; ijoint < ktec->getJoint_array().getCount(); ++ijoint) {
00552 vdomjoints.push_back(ktec->getJoint_array()[ijoint]);
00553 }
00554
00555
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
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
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
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
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
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
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
00848 DblArray12 tatt;
00849 _ExtractFullTransform(tatt,pattfull);
00850
00851
00852
00853
00854
00855
00856
00857
00858
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
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
00891 daeTArray<domAxis_constraintRef> vdomaxes = pdomjoint->getChildrenByType<domAxis_constraint>();
00892 domNodeRef pchildnode;
00893
00894
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
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 ){
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;
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
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
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
01031
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
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;
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) {
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()) {
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
01077 if (!kinematics_axis_info || (!joint_locked && !kinematics_limits)) {
01078
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
01119 for (size_t i = 0; i < pdomnode->getNode_array().getCount(); i++) {
01120
01121 bool contains=false;
01122 for(std::list<JointAxisBinding>::const_iterator it = listAxisBindings.begin(); it != listAxisBindings.end(); ++it) {
01123
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
01135
01136
01137
01138 }
01139
01140 unsigned int nGeomBefore = plink->shapeIndices.length();
01141
01142
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
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
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
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
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
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
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;
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;
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;
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
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
01466 ainfo.normalPerVertex = 1;
01467 ainfo.normals.length(itriangle*3);
01468 double creaseAngle = M_PI/4;
01469 for (int i=0; i<itriangle/3; i++){
01470 const Vector3 ¤tFaceNormal = faceNormals[i];
01471 for (int vertex=0; vertex<3; 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
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;
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
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;
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
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;
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
01815
01816
01817
01818
01819
01820
01821
01822
01823
01824
01825
01826
01827
01828
01829
01830
01831
01832
01833
01834
01835
01836
01837
01838
01839
01840
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
01861 daeString urlref2 = convexRef->getConvex_hull_of().getOriginalURI();
01862 if (urlref2) {
01863 daeElementRef otherElemRef = convexRef->getConvex_hull_of().getElement();
01864
01865
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)) {
01871
01872 domMesh *meshElement = lib->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
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
01932
01933
01934
01935
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;
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
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" ) {
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" ) {
02134 psensor.type = CORBA::string_dup( "Vision" );
02135 psensor.specValues.length( CORBA::ULong(7) );
02136
02137
02138 psensor.specValues[1] = 10;
02139
02140
02141
02142
02143
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" ) {
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" ) {
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
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
02391 if( !!pbind->getParam() ) {
02392
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() ) {
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
02607
02608
02609
02610
02611
02612
02613
02614
02615
02616
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;
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;
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
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
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
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
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
02911
02912
02913
02914
02915
02916
02917
02918
02919
02920
02921
02922
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
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
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
03194
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
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 }