BodySymmetry::BodySymmetry(BodyPtr body)
: body(body)
{
const YamlSequence& sjoints = *body->info()->findSequence("symmetricJoints");
if(sjoints.isValid() && !sjoints.empty()){
for(int i=0; i < sjoints.size(); ++i){
const YamlSequence& jointPair = *sjoints[i].toSequence();
if(jointPair.size() == 1){
Link* link = body->link(jointPair[0].toString());
if(link){
JointSymmetry& symmetry = jointSymmetryMap[link->jointId];
symmetry.counterPartJointId = link->jointId;
symmetry.jointPositionSign = -1.0;
symmetry.offset = 0.0;
}
} else if(jointPair.size() >= 2){
Link* joint[2];
joint[0] = body->link(jointPair[0].toString());
joint[1] = body->link(jointPair[1].toString());
if(joint[0] && joint[1] && joint[0]->jointId >= 0 && joint[1]->jointId >= 0){
for(int j=0; j < 2; ++j){
int other = 1 - j;
JointSymmetry& symmetry = jointSymmetryMap[joint[j]->jointId];
symmetry.counterPartJointId = joint[other]->jointId;
symmetry.sign = (jointPair.size() >= 3) ? jointPair[2].toDouble() : 1.0;
symmetry.offset = (jointPair.size() >= 4) ? jointPair[3].toDouble() : 0.0;
}
}
}
}
}
const YamlSequence& slinks = *body->info()->findSequence("symmetricIkLinks");
if(slinks.isValid() && !slinks.empty()){
for(int i=0; i < slinks.size(); ++i){
const YamlSequence& linkPair = *slinks[i].toSequence();
if(linkPair.size() == 1){
Link* link = body->link(linkPair[0].toString());
if(link){
LinkSymmetry& symmetry = linkFlipMap[link->index];
symmetry.counterPartIndex = link->index;
}
} else if(linkPair.size() >= 2){
Link* link[2];
link[0] = body->link(linkPair[0].toString());
link[1] = body->link(linkPair[1].toString());
if(link[0] && link[1]){
for(int j=0; j < 2; ++j){
int other = 1 - j;
LinkSymmetry& symmetry = linkFlipMap[link[j]->index];
symmetry.counterPartIndex = link[other]->index;
}
}
}
}
}
}
bool BodyLoaderImpl::load(BodyPtr& body, const std::string& filename)
{
bool result = false;
filesystem::path orgpath(filename);
string ext = getExtension(orgpath);
string modelFilename;
MappingPtr info;
try {
if(ext != "yaml"){
modelFilename = filename;
} else {
YAMLReader parser;
info = parser.loadDocument(filename)->toMapping();
filesystem::path mpath(info->get("modelFile").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);
int dn = defaultDivisionNumber;
if(info){
Mapping& geometryInfo = *info->findMapping("geometry");
if(geometryInfo.isValid()){
geometryInfo.read("divisionNumber", dn);
}
}
if(dn > 0){
loader->setDefaultDivisionNumber(dn);
}
if(defaultCreaseAngle >= 0.0){
loader->setDefaultCreaseAngle(defaultCreaseAngle);
}
body->clearDevices();
body->clearExtraJoints();
if(info){
body->resetInfo(info.get());
} 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;
}