void VRMLBodyLoaderImpl::readSurfaceNode(LinkInfo& iLink, VRMLProtoInstance* segmentShapeNode, const Affine3& T)
{
const string& typeName = segmentShapeNode->proto->protoName;
if(isVerbose) putMessage(string("Surface node ") + segmentShapeNode->defName);
iLink.isSurfaceNodeUsed = true;
// check if another Surface node does not appear in the subtree
MFNode& visualNodes = get<MFNode>(segmentShapeNode->fields["visual"]);
ProtoIdSet acceptableProtoIds;
readJointSubNodes(iLink, visualNodes, acceptableProtoIds, T);
MFNode& collisionNodes = get<MFNode>(segmentShapeNode->fields["collision"]);
readJointSubNodes(iLink, collisionNodes, acceptableProtoIds, T);
SgGroup* group;
SgPosTransform* transform = 0;
if(T.isApprox(Affine3::Identity())){
group = iLink.collisionShape;
} else {
transform = new SgPosTransform(T);
group = transform;
}
for(size_t i=0; i < collisionNodes.size(); ++i){
SgNodePtr node = sgConverter.convert(collisionNodes[i]);
if(node){
group->addChild(node);
}
}
if(transform && !transform->empty()){
iLink.collisionShape->addChild(transform);
}
}
void VRMLBodyLoaderImpl::readSegmentNode(LinkInfo& iLink, VRMLProtoInstance* segmentNode, const Affine3& T)
{
if(isVerbose) putMessage(string("Segment node ") + segmentNode->defName);
/*
Mass = Sigma mass
C = (Sigma mass * T * c) / Mass
I = Sigma(R * I * Rt + G)
R = Rotation matrix part of T
G = y*y+z*z, -x*y, -x*z, -y*x, z*z+x*x, -y*z, -z*x, -z*y, x*x+y*y
(x, y, z ) = T * c - C
*/
VRMLProtoFieldMap& sf = segmentNode->fields;
SegmentInfo iSegment;
readVRMLfield(sf["mass"], iSegment.m);
Vector3 c;
readVRMLfield(sf["centerOfMass"], c);
iSegment.c = T.linear() * c + T.translation();
iLink.c = (iSegment.c * iSegment.m + iLink.c * iLink.m) / (iLink.m + iSegment.m);
iLink.m += iSegment.m;
Matrix3 I;
readVRMLfield(sf["momentsOfInertia"], I);
iLink.I.noalias() += T.linear() * I * T.linear().transpose();
MFNode& childNodes = get<MFNode>(segmentNode->fields["children"]);
ProtoIdSet acceptableProtoIds;
acceptableProtoIds.set(PROTO_SURFACE);
acceptableProtoIds.set(PROTO_DEVICE);
readJointSubNodes(iLink, childNodes, acceptableProtoIds, T);
SgNodePtr node = sgConverter.convert(segmentNode);
if(node){
if(T.isApprox(Affine3::Identity())){
iLink.visualShape->addChild(node);
} else {
SgPosTransform* transform = new SgPosTransform(T);
transform->addChild(node);
iLink.visualShape->addChild(transform);
}
}
}