void VRMLBodyLoaderImpl::readJointSubNodes(LinkInfo& iLink, MFNode& childNodes, const ProtoIdSet& acceptableProtoIds, const Affine3& T)
{
for(size_t i = 0; i < childNodes.size(); ++i){
bool doTraverse = false;
VRMLNode* childNode = childNodes[i].get();
if(!childNode->isCategoryOf(PROTO_INSTANCE_NODE)){
doTraverse = true;
} else {
VRMLProtoInstance* protoInstance = static_cast<VRMLProtoInstance*>(childNode);
int id = PROTO_UNDEFINED;
const string& protoName = protoInstance->proto->protoName;
ProtoInfoMap::iterator p = protoInfoMap.find(protoName);
if(p == protoInfoMap.end()){
doTraverse = true;
childNode = protoInstance->actualNode.get();
} else {
id = p->second.id;
if(!acceptableProtoIds.test(id)){
throw invalid_argument(str(format(_("%1% node is not in a correct place.")) % protoName));
}
if(isVerbose){
messageIndent += 2;
}
switch(id){
case PROTO_JOINT:
if(!T.matrix().isApprox(Affine3::MatrixType::Identity())){
throw invalid_argument(
str(format(_("Joint node \"%1%\" is not in a correct place.")) % protoInstance->defName));
}
iLink.link->appendChild(readJointNode(protoInstance, iLink.link->Rs()));
break;
case PROTO_SEGMENT:
readSegmentNode(iLink, protoInstance, T);
linkOriginalMap[iLink.link] = childNodes[i];
break;
case PROTO_SURFACE:
readSurfaceNode(iLink, protoInstance, T);
break;
case PROTO_DEVICE:
readDeviceNode(iLink, protoInstance, T);
break;
default:
doTraverse = true;
break;
}
if(isVerbose){
messageIndent -= 2;
}
}
}
if(doTraverse && childNode->isCategoryOf(GROUPING_NODE)){
VRMLGroup* group = static_cast<VRMLGroup*>(childNode);
if(VRMLTransform* transform = dynamic_cast<VRMLTransform*>(group)){
readJointSubNodes(iLink, group->getChildren(), acceptableProtoIds, T * transform->toAffine3d());
} else {
readJointSubNodes(iLink, group->getChildren(), acceptableProtoIds, T);
}
}
}
}
void ModelNodeSetImpl::extractChildNodes
(JointNodeSetPtr jointNodeSet, MFNode& childNodes, const ProtoIdSet acceptableProtoIds, const Matrix44& T)
{
for(size_t i = 0; i < childNodes.size(); i++){
VrmlNode* childNode = childNodes[i].get();
const Matrix44* pT;
if(childNode->isCategoryOf(GROUPING_NODE)){
VrmlGroup* groupNode = static_cast<VrmlGroup*>(childNode);
VrmlTransform* transformNode = dynamic_cast<VrmlTransform*>(groupNode);
Matrix44 T2;
if( transformNode ){
Matrix44 Tlocal;
calcTransformMatrix(transformNode, Tlocal);
T2 = T * Tlocal;
pT = &T2;
} else {
pT = &T;
}
extractChildNodes(jointNodeSet, groupNode->getChildren(), acceptableProtoIds, *pT);
} else if(childNode->isCategoryOf(LIGHT_NODE)){
jointNodeSet->lightNodes.push_back(std::make_pair(T,childNode));
} else if(childNode->isCategoryOf(PROTO_INSTANCE_NODE)){
VrmlProtoInstance* protoInstance = static_cast<VrmlProtoInstance*>(childNode);
int id = PROTO_UNDEFINED;
bool doTraverseChildren = false;
ProtoIdSet acceptableChildProtoIds(acceptableProtoIds);
const string& protoName = protoInstance->proto->protoName;
ProtoNameToInfoMap::iterator p = protoNameToInfoMap.find(protoName);
if(p == protoNameToInfoMap.end()){
doTraverseChildren = true;
} else {
id = p->second.id;
if(!acceptableProtoIds.test(id)){
throw ModelNodeSet::Exception(protoName + " node is not in a correct place.");
}
}
messageIndent += 2;
switch(id){
case PROTO_JOINT:
if(T != Matrix44::Identity())
throw ModelNodeSet::Exception(protoName + " node is not in a correct place.");
jointNodeSet->childJointNodeSets.push_back(addJointNodeSet(protoInstance));
break;
case PROTO_SENSOR:
if(T != Matrix44::Identity())
throw ModelNodeSet::Exception(protoName + " node is not in a correct place.");
jointNodeSet->sensorNodes.push_back(protoInstance);
putMessage(protoName + protoInstance->defName);
break;
case PROTO_HARDWARECOMPONENT:
if(T != Matrix44::Identity())
throw ModelNodeSet::Exception(protoName + " node is not in a correct place.");
jointNodeSet->hwcNodes.push_back(protoInstance);
putMessage(protoName + protoInstance->defName);
break;
case PROTO_SEGMENT:
{
jointNodeSet->segmentNodes.push_back(protoInstance);
jointNodeSet->transforms.push_back(T);
putMessage(string("Segment node ") + protoInstance->defName);
doTraverseChildren = true;
acceptableChildProtoIds.reset();
acceptableChildProtoIds.set(PROTO_SENSOR);
}
break;
default:
break;
}
if(doTraverseChildren){
if (protoInstance->fields.count("children")){
MFNode& childNodes = protoInstance->fields["children"].mfNode();
extractChildNodes(jointNodeSet, childNodes, acceptableChildProtoIds, T);
}
}
messageIndent -= 2;
}
}
}