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