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