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