Link* VRMLBodyLoaderImpl::readJointNode(VRMLProtoInstance* jointNode, const Matrix3& parentRs)
{
putVerboseMessage(string("Joint node") + jointNode->defName);
Link* link = createLink(jointNode, parentRs);
LinkInfo iLink;
iLink.link = link;
SgInvariantGroupPtr shapeTop = new SgInvariantGroup();
if(link->Rs().isApprox(Matrix3::Identity())){
iLink.shape = shapeTop.get();
} else {
SgPosTransform* transformRs = new SgPosTransform;
transformRs->setRotation(link->Rs());
shapeTop->addChild(transformRs);
iLink.shape = transformRs;
}
iLink.m = 0.0;
iLink.c = Vector3::Zero();
iLink.I = Matrix3::Zero();
MFNode& childNodes = get<MFNode>(jointNode->fields["children"]);
Affine3 T(Affine3::Identity());
ProtoIdSet acceptableProtoIds;
acceptableProtoIds.set(PROTO_JOINT);
acceptableProtoIds.set(PROTO_SEGMENT);
acceptableProtoIds.set(PROTO_DEVICE);
readJointSubNodes(iLink, childNodes, acceptableProtoIds, T);
Matrix3& I = iLink.I;
for(size_t i=0; i < iLink.segments.size(); ++i){
const SegmentInfo& segment = iLink.segments[i];
const Vector3 o = segment.c - iLink.c;
const double& x = o.x();
const double& y = o.y();
const double& z = o.z();
const double& m = segment.m;
I(0,0) += m * (y * y + z * z);
I(0,1) += -m * (x * y);
I(0,2) += -m * (x * z);
I(1,0) += -m * (y * x);
I(1,1) += m * (z * z + x * x);
I(1,2) += -m * (y * z);
I(2,0) += -m * (z * x);
I(2,1) += -m * (z * y);
I(2,2) += m * (x * x + y * y);
}
link->setMass(iLink.m);
link->setCenterOfMass(link->Rs() * iLink.c);
link->setInertia(link->Rs() * iLink.I * link->Rs().transpose());
if(iLink.shape->empty()){
link->setShape(new SgNode()); // set empty node
} else {
link->setShape(shapeTop);
}
return link;
}
Link* VRMLBodyLoaderImpl::readJointNode(VRMLProtoInstance* jointNode, const Matrix3& parentRs)
{
if(isVerbose) putMessage(string("Joint node") + jointNode->defName);
Link* link = createLink(jointNode, parentRs);
LinkInfo iLink;
iLink.link = link;
iLink.m = 0.0;
iLink.c = Vector3::Zero();
iLink.I = Matrix3::Zero();
iLink.visualShape = new SgGroup;
iLink.collisionShape = new SgGroup;
iLink.isSurfaceNodeUsed = false;
MFNode& childNodes = get<MFNode>(jointNode->fields["children"]);
Affine3 T(Affine3::Identity());
ProtoIdSet acceptableProtoIds;
acceptableProtoIds.set(PROTO_JOINT);
acceptableProtoIds.set(PROTO_SEGMENT);
acceptableProtoIds.set(PROTO_DEVICE);
readJointSubNodes(iLink, childNodes, acceptableProtoIds, T);
Matrix3& I = iLink.I;
for(size_t i=0; i < iLink.segments.size(); ++i){
const SegmentInfo& segment = iLink.segments[i];
const Vector3 o = segment.c - iLink.c;
const double& x = o.x();
const double& y = o.y();
const double& z = o.z();
const double& m = segment.m;
I(0,0) += m * (y * y + z * z);
I(0,1) += -m * (x * y);
I(0,2) += -m * (x * z);
I(1,0) += -m * (y * x);
I(1,1) += m * (z * z + x * x);
I(1,2) += -m * (y * z);
I(2,0) += -m * (z * x);
I(2,1) += -m * (z * y);
I(2,2) += m * (x * x + y * y);
}
link->setMass(iLink.m);
link->setCenterOfMass(link->Rs() * iLink.c);
link->setInertia(link->Rs() * iLink.I * link->Rs().transpose());
setShape(link, iLink.visualShape, true);
if(iLink.isSurfaceNodeUsed){
setShape(link, iLink.collisionShape, false);
} else {
link->setCollisionShape(link->visualShape());
}
return link;
}
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 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);
}
}
}
JointNodeSetPtr ModelNodeSetImpl::addJointNodeSet(VrmlProtoInstancePtr jointNode)
{
numJointNodes++;
putMessage(string("Joint node ") + jointNode->defName);
JointNodeSetPtr jointNodeSet(new JointNodeSet());
jointNodeSet->jointNode = jointNode;
MFNode& childNodes = jointNode->fields["children"].mfNode();
ProtoIdSet acceptableProtoIds;
acceptableProtoIds.set(PROTO_JOINT);
acceptableProtoIds.set(PROTO_SEGMENT);
acceptableProtoIds.set(PROTO_SENSOR);
acceptableProtoIds.set(PROTO_HARDWARECOMPONENT);
Matrix44 T(Matrix44::Identity());
extractChildNodes(jointNodeSet, childNodes, acceptableProtoIds, T);
return jointNodeSet;
}
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;
}
}
}
