Link* BodyLoaderImpl::createLink(JointNodeSetPtr jointNodeSet, const Matrix3& parentRs)
{
Link* link = new Link();
setJointProperties(link, jointNodeSet->jointNode, parentRs);
vector<VrmlProtoInstancePtr> segmentNodes = jointNodeSet->segmentNodes;
const JointNodeSet::Affine3Array& transforms = jointNodeSet->transforms;
setSegmentProperties(link, segmentNodes, transforms);
if(createColdetModel && !segmentNodes.empty()){
setColdetModel(link, segmentNodes, transforms);
}
// The following code adds child links from the back of the child array
// in order to keep the original order of the children.
// ( addChild() function of the Link class prepends a child to the child list )
int numChildren = jointNodeSet->childJointNodeSets.size();
for(int i = numChildren - 1; i >= 0; --i){
JointNodeSetPtr childJointNodeSet = jointNodeSet->childJointNodeSets[i];
Link* childLink = createLink(childJointNodeSet, link->Rs);
link->addChild(childLink);
}
createSensors(link, jointNodeSet->sensorNodes);
return link;
}
Link* createLink(int index, const Matrix33& parentRs)
{
const LinkInfo& linkInfo = linkInfoSeq[index];
int jointId = linkInfo.jointId;
Link* link = new Link(); //(*createLinkFunc)();
CORBA::String_var name0 = linkInfo.name;
link->name = string( name0 );
link->jointId = jointId;
Vector3 relPos(linkInfo.translation[0], linkInfo.translation[1], linkInfo.translation[2]);
link->b = parentRs * relPos;
Vector3 rotAxis(linkInfo.rotation[0], linkInfo.rotation[1], linkInfo.rotation[2]);
Matrix33 R = rodrigues(rotAxis, linkInfo.rotation[3]);
link->Rs = (parentRs * R);
const Matrix33& Rs = link->Rs;
CORBA::String_var jointType = linkInfo.jointType;
const std::string jt( jointType );
if(jt == "fixed" ){
link->jointType = Link::FIXED_JOINT;
} else if(jt == "free" ){
link->jointType = Link::FREE_JOINT;
} else if(jt == "rotate" ){
link->jointType = Link::ROTATIONAL_JOINT;
} else if(jt == "slide" ){
link->jointType = Link::SLIDE_JOINT;
} else if(jt == "crawler"){
link->jointType == Link::FIXED_JOINT;
link->isCrawler = true;
} else {
link->jointType = Link::FREE_JOINT;
}
if(jointId < 0){
if(link->jointType == Link::ROTATIONAL_JOINT || link->jointType == Link::SLIDE_JOINT){
std::cerr << "Warning: Joint ID is not given to joint " << link->name
<< " of model " << body->modelName() << "." << std::endl;
}
}
link->a.setZero();
link->d.setZero();
Vector3 axis( Rs * Vector3(linkInfo.jointAxis[0], linkInfo.jointAxis[1], linkInfo.jointAxis[2]));
if(link->jointType == Link::ROTATIONAL_JOINT || jt == "crawler"){
link->a = axis;
} else if(link->jointType == Link::SLIDE_JOINT){
link->d = axis;
}
link->m = linkInfo.mass;
link->Ir = linkInfo.rotorInertia;
link->gearRatio = linkInfo.gearRatio;
link->rotorResistor = linkInfo.rotorResistor;
link->torqueConst = linkInfo.torqueConst;
link->encoderPulse = linkInfo.encoderPulse;
link->Jm2 = link->Ir * link->gearRatio * link->gearRatio;
DblSequence ulimit = linkInfo.ulimit;
DblSequence llimit = linkInfo.llimit;
DblSequence uvlimit = linkInfo.uvlimit;
DblSequence lvlimit = linkInfo.lvlimit;
DblSequence climit = linkInfo.climit;
double maxlimit = (numeric_limits<double>::max)();
link->ulimit = getLimitValue(ulimit, +maxlimit);
link->llimit = getLimitValue(llimit, -maxlimit);
link->uvlimit = getLimitValue(uvlimit, +maxlimit);
link->lvlimit = getLimitValue(lvlimit, -maxlimit);
link->climit = getLimitValue(climit, +maxlimit);
link->c = Rs * Vector3(linkInfo.centerOfMass[0], linkInfo.centerOfMass[1], linkInfo.centerOfMass[2]);
Matrix33 Io;
getMatrix33FromRowMajorArray(Io, linkInfo.inertia);
link->I = Rs * Io * Rs.transpose();
// a stack is used for keeping the same order of children
std::stack<Link*> children;
//##### [Changed] Link Structure (convert NaryTree to BinaryTree).
int childNum = linkInfo.childIndices.length();
for(int i = 0 ; i < childNum ; i++) {
int childIndex = linkInfo.childIndices[i];
Link* childLink = createLink(childIndex, Rs);
if(childLink) {
children.push(childLink);
}
}
while(!children.empty()){
link->addChild(children.top());
children.pop();
}
createSensors(link, linkInfo.sensors, Rs);
//createLights(link, linkInfo.lights, Rs);
#if 0
if(collisionDetectionModelLoading){
createColdetModel(link, linkInfo);
}
#endif
return link;
}
