void SceneInfo_impl::load(const std::string& url)
{
    string filename(deleteURLScheme(url));

    // URL文字列の' \' 区切り子を'/' に置き換え  Windows ファイルパス対応  
    string url2;
    url2 = filename;
    replace( url2, string("\\"), string("/") );
    filename = url2;
    url_ = CORBA::string_dup(url2.c_str());

    bool result = false;

    try {
        VrmlParser parser;
        parser.load(filename);

        Matrix44 E(Matrix44::Identity());

        while(VrmlNodePtr node = parser.readNode()){
            if(!node->isCategoryOf(PROTO_DEF_NODE)){
                applyTriangleMeshShaper(node);
                traverseShapeNodes(node.get(), E, shapeIndices_, inlinedShapeTransformMatrices_, &topUrl());
            }
        }
    } catch(EasyScanner::Exception& ex){
        cout << ex.getFullMessage() << endl;
	throw ModelLoader::ModelLoaderException(ex.getFullMessage().c_str());
    }
}
Example #2
0
void ModelNodeSetImpl::extractHumanoidNode(VrmlParser& parser)
{
    while(VrmlNodePtr node = parser.readNode()){
		
        if(node->isCategoryOf(PROTO_DEF_NODE)){
			
            protoToCheck = static_pointer_cast<VrmlProto>(node);
			
            ProtoNameToInfoMap::iterator p = protoNameToInfoMap.find(protoToCheck->protoName);
            if(p != protoNameToInfoMap.end()){
                ProtoInfo& info = p->second;
                (this->*info.protoCheckFunc)();
            }
			
        } else if(node->isCategoryOf(PROTO_INSTANCE_NODE)){
			
            VrmlProtoInstancePtr instance = static_pointer_cast<VrmlProtoInstance>(node);
            if(instance->proto->protoName == "Humanoid") {
                humanoidNode = instance;
            } else if(instance->proto->protoName == "ExtraJoint") {
                extraJointNodes.push_back(instance);
            }
        }
    }
	
    if(humanoidNode){
        putMessage("Humanoid node");
        extractJointNodes();
    } else {
        throw ModelNodeSet::Exception("Humanoid node is not found");
	}
}
Example #3
0
/*!
  @if jp
  @brief モデルファイルをロードし、BodyInfoを構築する。
  @else
  @brief This function loads a model file and creates a BodyInfo object.
  @param url The url to a model file
  @endif
*/
void BodyInfo_impl::loadModelFile(const std::string& url)
{
    string filename( deleteURLScheme( url ) );

    // URL文字列の' \' 区切り子を'/' に置き換え  Windows ファイルパス対応  
    string url2;
    url2 = filename;
    replace( url2, string("\\"), string("/") );
    filename = url2;
    url_ = CORBA::string_dup(url2.c_str());
    

    ModelNodeSet modelNodeSet;
    modelNodeSet.sigMessage.connect(boost::bind(&putMessage, _1));

    bool result = false;

    try	{
        result = modelNodeSet.loadModelFile( filename );

        if(result){
            applyTriangleMeshShaper(modelNodeSet.humanoidNode());
        }
        cout.flush();
    }
    catch(const ModelNodeSet::Exception& ex) {
        cout << ex.what() << endl;
        cout << "Retrying to load the file as a standard VRML file" << endl;
        try {
            VrmlParser parser;
            parser.load(filename);

            links_.length(1);
            LinkInfo &linfo = links_[0];
            linfo.name = CORBA::string_dup("root");
            linfo.parentIndex = -1;
            linfo.jointId = -1;
            linfo.jointType = CORBA::string_dup("fixed");
            linfo.jointValue = 0;
            for (int i=0; i<3; i++){
                linfo.jointAxis[i] = 0;
                linfo.translation[i] = 0;
                linfo.rotation[i] = 0;
            }
            linfo.jointAxis[2] = 1; 
            linfo.rotation[2] = 1; linfo.rotation[3] = 0;
            
            Matrix44 E(Matrix44::Identity());
            
            while(VrmlNodePtr node = parser.readNode()){
                if(!node->isCategoryOf(PROTO_DEF_NODE)){
                    applyTriangleMeshShaper(node);
                    traverseShapeNodes(node.get(), E, linfo.shapeIndices, linfo.inlinedShapeTransformMatrices, &topUrl());
                }
            }
            return;
        } catch(EasyScanner::Exception& ex){
            cout << ex.getFullMessage() << endl;
            throw ModelLoader::ModelLoaderException(ex.getFullMessage().c_str());
        }
    }

    if(!result){
        throw ModelLoader::ModelLoaderException("The model file cannot be loaded.");
    }

    const string& humanoidName = modelNodeSet.humanoidNode()->defName;
    name_ = CORBA::string_dup(humanoidName.c_str());
    const MFString& info = modelNodeSet.humanoidNode()->fields["info"].mfString();
    info_.length(info.size());
    for (unsigned int i=0; i<info_.length(); i++){
        info_[i] = CORBA::string_dup(info[i].c_str());
    }

    int numJointNodes = modelNodeSet.numJointNodes();

    links_.length(numJointNodes);
    if( 0 < numJointNodes ) {
        int currentIndex = 0;
        JointNodeSetPtr rootJointNodeSet = modelNodeSet.rootJointNodeSet();
        readJointNodeSet(rootJointNodeSet, currentIndex, -1);
    }

    linkShapeIndices_.length(numJointNodes); 
    for(size_t i = 0 ; i < numJointNodes ; ++i) {
        linkShapeIndices_[i] = links_[i].shapeIndices;
    }
    
    // build coldetModels 
    linkColdetModels.resize(numJointNodes);    
    for(int linkIndex = 0; linkIndex < numJointNodes ; ++linkIndex){
        ColdetModelPtr coldetModel(new ColdetModel());
        coldetModel->setName(std::string(links_[linkIndex].name));
        int vertexIndex = 0;
        int triangleIndex = 0;
        
        Matrix44 E(Matrix44::Identity());
        const TransformedShapeIndexSequence& shapeIndices = linkShapeIndices_[linkIndex];
        setColdetModel(coldetModel, shapeIndices, E, vertexIndex, triangleIndex);

        Matrix44 T(Matrix44::Identity());
        const SensorInfoSequence& sensors = links_[linkIndex].sensors;
        for (unsigned int i=0; i<sensors.length(); i++){
            const SensorInfo& sensor = sensors[i];
            calcRodrigues(T, Vector3(sensor.rotation[0], sensor.rotation[1], 
                                 sensor.rotation[2]), sensor.rotation[3]);
            T(0,3) = sensor.translation[0];
            T(1,3) = sensor.translation[1];
            T(2,3) = sensor.translation[2];
            const TransformedShapeIndexSequence& sensorShapeIndices = sensor.shapeIndices;
            setColdetModel(coldetModel, sensorShapeIndices, T, vertexIndex, triangleIndex);
        }
                       
        if(triangleIndex>0)    
            coldetModel->build();

        linkColdetModels[linkIndex] = coldetModel;
        links_[linkIndex].AABBmaxDepth = coldetModel->getAABBTreeDepth();
        links_[linkIndex].AABBmaxNum = coldetModel->getAABBmaxNum();
    }
    //saveOriginalData();
    //originlinkShapeIndices_ = linkShapeIndices_;

	int n = modelNodeSet.numExtraJointNodes();
	extraJoints_.length(n);
    for(int i=0; i < n; ++i){
		
        TProtoFieldMap& f = modelNodeSet.extraJointNode(i)->fields;
        ExtraJointInfo_var extraJointInfo(new ExtraJointInfo());
		extraJointInfo->name =  CORBA::string_dup( modelNodeSet.extraJointNode(i)->defName.c_str() );

        string link1Name, link2Name;
		copyVrmlField( f, "link1Name", link1Name );
		copyVrmlField( f, "link2Name", link2Name );
		extraJointInfo->link[0] = CORBA::string_dup(link1Name.c_str());
		extraJointInfo->link[1] = CORBA::string_dup(link2Name.c_str());
 
		string jointType;
		copyVrmlField( f, "jointType", jointType);
        if(jointType == "xy"){
		   extraJointInfo->jointType = EJ_XY;
        } else if(jointType == "xyz"){
           extraJointInfo->jointType = EJ_XYZ;
        } else if(jointType == "z"){
           extraJointInfo->jointType = EJ_Z;
        }else {
            throw ModelNodeSet::Exception(str(format("JointType \"%1%\" is not supported.") % jointType));
        }
        copyVrmlField( f, "jointAxis", extraJointInfo->axis );    
		copyVrmlField( f, "link1LocalPos", extraJointInfo->point[0] );
		copyVrmlField( f, "link2LocalPos", extraJointInfo->point[1] );
	
		extraJoints_[i] = extraJointInfo;
    }
}