C++ Link::addChild方法代码示例

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();
//.........这里部分代码省略.........