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