bool BodyLoaderImpl::load(Body* body, const std::string& filename) { bool result = false; filesystem::path orgpath(filename); string ext = getExtension(orgpath); string modelFilename; MappingPtr yamlDoc; try { if(ext != "yaml"){ modelFilename = filename; } else { YAMLReader parser; yamlDoc = parser.loadDocument(filename)->toMapping(); ValueNode* modelFileNode = yamlDoc->find("modelFile"); if(modelFileNode->isValid()){ filesystem::path mpath(modelFileNode->toString()); if(mpath.has_root_path()){ modelFilename = getNativePathString(mpath); } else { modelFilename = getNativePathString(orgpath.parent_path() / mpath); } ext = getExtension(mpath); } } LoaderMap::iterator p = loaderMap.find(ext); if(p != loaderMap.end()){ loader = p->second; } else { boost::lock_guard<boost::mutex> lock(loaderFactoryMapMutex); LoaderFactoryMap::iterator q = loaderFactoryMap.find(ext); if(q != loaderFactoryMap.end()){ LoaderFactory factory = q->second; loader = factory(); loaderMap[ext] = loader; } } if(!loader){ (*os) << str(boost::format(_("The file format of \"%1%\" is not supported by the body loader.\n")) % getFilename(filesystem::path(modelFilename))); } else { loader->setMessageSink(*os); loader->setVerbose(isVerbose); loader->setShapeLoadingEnabled(isShapeLoadingEnabled); YAMLBodyLoader* yamlBodyLoader = 0; if(yamlDoc){ YAMLBodyLoader* yamlBodyLoader = dynamic_cast<YAMLBodyLoader*>(loader.get()); } if(yamlBodyLoader){ result = yamlBodyLoader->read(body, yamlDoc); } else { int dn = defaultDivisionNumber; if(yamlDoc){ Mapping& geometryInfo = *yamlDoc->findMapping("geometry"); if(geometryInfo.isValid()){ geometryInfo.read("divisionNumber", dn); } } if(dn > 0){ loader->setDefaultDivisionNumber(dn); } if(defaultCreaseAngle >= 0.0){ loader->setDefaultCreaseAngle(defaultCreaseAngle); } if(yamlDoc){ body->resetInfo(yamlDoc); } else { body->info()->clear(); } result = loader->load(body, modelFilename); } } } catch(const ValueNode::Exception& ex){ (*os) << ex.message(); } catch(const nonexistent_key_error& error){ if(const std::string* message = boost::get_error_info<error_info_message>(error)){ (*os) << *message; } } catch(const std::exception& ex){ (*os) << ex.what(); } os->flush(); return result; }
bool PoseSeqItem::convertSub(BodyPtr orgBody, const Mapping& convInfo) { bool converted = false; const Listing& jointMap = *convInfo.findListing("jointMap"); const Mapping& linkMap = *convInfo.findMapping("linkMap"); BodyPtr body = ownerBodyItem->body(); for(PoseSeq::iterator p = seq->begin(); p != seq->end(); ++p){ PosePtr pose = p->get<Pose>(); if(pose){ bool modified = false; seq->beginPoseModification(p); PosePtr orgPose = dynamic_cast<Pose*>(pose->duplicate()); if(jointMap.isValid()){ modified = true; pose->setNumJoints(0); int n = orgPose->numJoints(); for(int i=0; i < n; ++i){ if(orgPose->isJointValid(i)){ if(i < jointMap.size()){ int newJointId = jointMap[i].toInt(); if(newJointId >= 0){ pose->setJointPosition(newJointId, orgPose->jointPosition(i)); pose->setJointStationaryPoint(newJointId, orgPose->isJointStationaryPoint(i)); } } } } } if(linkMap.isValid()){ modified = true; pose->clearIkLinks(); int baseLinkIndex = -1; for(Pose::LinkInfoMap::const_iterator q = orgPose->ikLinkBegin(); q != orgPose->ikLinkEnd(); ++q){ Link* orgLink = orgBody->link(q->first); string linkName; ValueNode* linkNameNode = linkMap.find(orgLink->name()); if(linkNameNode->isValid()){ linkName = linkNameNode->toString(); } else { linkName = orgLink->name(); } Link* link = body->link(linkName); if(link){ const Pose::LinkInfo& orgLinkInfo = q->second; Pose::LinkInfo* linkInfo = pose->addIkLink(link->index()); linkInfo->p = orgLinkInfo.p; linkInfo->R = orgLinkInfo.R; linkInfo->setStationaryPoint(orgLinkInfo.isStationaryPoint()); if(orgLinkInfo.isTouching()){ linkInfo->setTouching(orgLinkInfo.partingDirection()); } linkInfo->setSlave(orgLinkInfo.isSlave()); if(orgLinkInfo.isBaseLink()){ baseLinkIndex = link->index(); } } } if(baseLinkIndex >= 0){ pose->setBaseLink(baseLinkIndex); } } if(modified){ seq->endPoseModification(p); converted = true; } } } return converted; }