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;
                    }
                }
            }
        }
    }
}
示例#2
0
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;
}