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;
}
