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()); } }
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"); } }
bool ModelNodeSetImpl::loadModelFile(const std::string& filename) { numJointNodes = 0; humanoidNode = 0; rootJointNodeSet.reset(); messageIndent = 0; extraJointNodes.clear(); try { VrmlParser parser; parser.load(filename); extractHumanoidNode(parser); } catch(EasyScanner::Exception& ex){ throw ModelNodeSet::Exception(ex.getFullMessage()); } return (humanoidNode && rootJointNodeSet); }
/*! @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; } }