C++ Array_类代码示例

    int gradientFunc(const Vector&     parameters,
                     bool              new_parameters,
                     Vector&           gradient) const override
    {   SimTK_ASSERT2_ALWAYS(gradient.size() == getNumFreeQs(),
            "AssemblySystem::gradientFunc(): expected gradient vector of"
            " size %d but got %d; this is likely a problem with the optimizer"
            " which is required to allocate the right amount of space.",
            getNumFreeQs(), gradient.size());

        ++nEvalGradient;

        if (new_parameters)
            setInternalStateFromFreeQs(parameters);

        for (unsigned i=0; i < assembler.reporters.size(); ++i)
            assembler.reporters[i]->handleEvent(getInternalState());

        // This will record the indices of any goals we encounter that can't
        // provide their own gradients; we'll handle them all together at
        // the end.
        Array_<AssemblyConditionIndex> needNumericalGradient;

        gradient = 0;
        Vector tmpGradient(gradient.size());
        for (unsigned i=0; i < assembler.goals.size(); ++i) {
            AssemblyConditionIndex   goalIx = assembler.goals[i];
            const AssemblyCondition& cond   = *assembler.conditions[goalIx];
            const int stat = (assembler.forceNumericalGradient
                              ? -1
                              : cond.calcGoalGradient(getInternalState(),
                                                      tmpGradient));
            if (stat == -1) {
                needNumericalGradient.push_back(goalIx);
                continue;
            }
            if (stat != 0)
                return stat;

            gradient += assembler.weights[goalIx] * tmpGradient;
        }

        if (!needNumericalGradient.empty()) {
            //cout << "Need numerical gradient for "
            //     << needNumericalGradient.size() << " goals." << endl;
            NumGradientFunc numGoals(assembler, needNumericalGradient);
            // Essential to use central difference here so that the
            // approximate gradient is actually zero at the optimum
            // solution, otherwise IpOpt won't converge.
            Differentiator gradNumGoals
               (numGoals,Differentiator::CentralDifference);
            // weights are already included here
            gradient += gradNumGoals.calcGradient(getFreeQsFromInternalState());

            nEvalObjective += gradNumGoals.getNumCallsToUserFunction();
        }

        //cout << "Grad=" << gradient << endl;

        return 0;
    }

//------------------------------------------------------------------------------
//       REACHING AND GRAVITY COMPENSATION :: CALC DECORATIVE GEOMETRY
//------------------------------------------------------------------------------
void ReachingAndGravityCompensation::
calcDecorativeGeometryAndAppend(const State & state, Stage stage,
                                Array_<DecorativeGeometry>& geometry) const
{
    if (stage != Stage::Position) return;

    const Vec3 targetPos = m_modelTasks.getTarget();
    geometry.push_back(DecorativeSphere(0.02)
            .setTransform(targetPos)
            .setColor(m_targetColor));
    geometry.push_back(DecorativeText("Target: " +
        String(targetPos[0])+","+String(targetPos[1])+","+String(targetPos[2]))
        .setIsScreenText(true));

    const MobilizedBody& ee = m_realRobot.getEndEffectorBody();
    Vec3 taskPosInGround = ee.findStationLocationInGround(state,
                                        m_realRobot.getEndEffectorStation());
    geometry.push_back(DecorativePoint(taskPosInGround)
                       .setColor(Green).setLineThickness(3));

    geometry.push_back(DecorativeText(String("TOGGLES: [t]Task point ")
        + (m_modelTasks.isTaskPointFollowingOn() ? "ON" : "OFF")
        + "...[g]Gravity comp "
        + (m_modelTasks.isGravityCompensationOn() ? "ON" : "OFF")
        + "...[p]Posture control "
        + (m_modelTasks.isPoseControlOn() ? "ON" : "OFF")
        + "...[e]End effector sensor "
        + (m_modelTasks.isEndEffectorSensingOn() ? "ON" : "OFF")
        )
        .setIsScreenText(true));
}

//----------------------------- SET UP VISUALIZER ------------------------------
void MyMechanism::setUpVisualizer() {
    m_viz.setShutdownWhenDestructed(true) // make sure display window dies
         .setBackgroundType(Visualizer::SolidColor); // turn off Ground & Sky

    // Add sliders.
    m_viz.addSlider("Motor speed", SliderIdMotorSpeed,
                    -MaxMotorSpeed, MaxMotorSpeed, InitialMotorSpeed)
         .addSlider("Torque limit", SliderIdTorqueLimit,
                    0, MaxTorqueLimit, InitialTorqueLimit)
         .addSlider("Tach",   SliderIdTach,
                    -MaxMotorSpeed,  MaxMotorSpeed,  0)
         .addSlider("Torque", SliderIdTorque,
                    -InitialTorqueLimit, InitialTorqueLimit, 0);

    // Add Run menu.
    Array_<std::pair<String,int> > runMenuItems;
    runMenuItems.push_back(std::make_pair("Reset", ResetItem));
    runMenuItems.push_back(std::make_pair("Quit", QuitItem));
    m_viz.addMenu("Run", MenuIdRun, runMenuItems);

    // Add per-frame text and geometry.
    m_viz.addDecorationGenerator(new ShowStuff(*this));

    // Add an input listener so the user can talk to us.
    m_userInput = new Visualizer::InputSilo();
    m_viz.addInputListener(m_userInput);
}

void PerSubsystemInfo::
popAllocationStackBackToStage(Array_<T>& stack, const Stage& g) {
    unsigned newSize = stack.size();
    while (newSize > 0 && stack[newSize-1].getAllocationStage() > g)
        stack[--newSize].deepDestruct(*m_stateImpl);
    stack.resize(newSize); 
}

// Create a polygonal mesh for this torus using parameterization as follows:
// u = [0, 2*Pi] traces a circle in the x-y plane with radius torusRadius,
// which is the centroid of the torus. A point P on this circle is
// given by P = torusRadius*~[cos(u) sin(u) 0].
// v = [0, 2*Pi] traces a circle arond the cross-section (or tube) of the
// torus with radius tubeRadius, at a given u. A point Q on this circle
// is given by Q = (torusRadius + tubeRadius*cos(v))*e1 + tubeRadius*(~[0 0 1]*sin(v))
// where e1 = ~[sin(u) cos(u) 0]. The tube circle is in a plane spanned
// by e1 and the z-axis.
void ContactGeometry::Torus::Impl::createPolygonalMesh(PolygonalMesh& mesh) const {
    // TODO add resolution argument
    const int numSides = 12; //*resolution;
    const int numSlices = 36; //*resolution;   

    // add vertices 
    for (int i = 0; i < numSlices; ++i) {
      Real u = Real((i*2*SimTK_PI)/numSlices);
      UnitVec3 e1(std::sin(u), std::cos(u), 0); // torus circle aligned with z-axis (z-axis through hole)
      for (int j = 0; j < numSides; ++j) {
        Real v = Real((j*2*SimTK_PI)/numSides);
        Vec3 vtx = (torusRadius + tubeRadius*std::cos(v))*e1 + tubeRadius*std::sin(v)*Vec3(0,0,1); // use ZAXIS? 
        mesh.addVertex(vtx);  
      }
    }

    // add faces, be careful to wrap indices for the last slice
    int numVertices = mesh.getNumVertices();
//    cout << "num verts = " << numVertices << endl;
    for (int i = 0; i < numVertices; ++i) {
//      cout << "v" << i << ": " << mesh.getVertexPosition(i) << endl;
      // define counter-clockwise quad faces
      Array_<int> faceIndices;
      faceIndices.push_back(i); // u_i,v_i
      faceIndices.push_back((i+1)%numVertices); // u_i, v_i+1
      faceIndices.push_back((i+1+numSides)%numVertices); // u_i+1, v_i+1
      faceIndices.push_back((i+numSides)%numVertices); // u_i+1, v_i
      mesh.addFace(faceIndices);	
    }

}

    virtual void generateDecorations(const State& state, Array_<DecorativeGeometry>& geometry) override {
        const Vec3 frcColors[] = {Red,Orange,Cyan};
        const Vec3 momColors[] = {Blue,Green,Purple};
        m_system.realize(state, Stage::Velocity);

        const int ncont = m_compliant.getNumContactForces(state);
        for (int i=0; i < ncont; ++i) {
            const ContactForce& force = m_compliant.getContactForce(state,i);
            const ContactId     id    = force.getContactId();
            const Vec3& frc = force.getForceOnSurface2()[1];

            const Vec3& mom = force.getForceOnSurface2()[0];
            Real  frcMag = frc.norm(), momMag=mom.norm();
            int frcThickness = 1, momThickness = 1;
            Real frcScale = ForceScale, momScale = ForceScale;
            while (frcMag > 10)
                frcThickness++, frcScale /= 10, frcMag /= 10;
            while (momMag > 10)
                momThickness++, momScale /= 10, momMag /= 10;
            DecorativeLine frcLine(force.getContactPoint(),
                                   force.getContactPoint() + frcScale*frc);
            DecorativeLine momLine(force.getContactPoint(),
                                   force.getContactPoint() + momScale*mom);
            frcLine.setColor(frcColors[id%3]);
            momLine.setColor(momColors[id%3]);
            frcLine.setLineThickness(2*frcThickness);
            momLine.setLineThickness(2*momThickness);
            geometry.push_back(frcLine);
            geometry.push_back(momLine);

            ContactPatch patch;
            const bool found = m_compliant.calcContactPatchDetailsById(state,id,patch);
            //cout << "patch for id" << id << " found=" << found << endl;
            //cout << "resultant=" << patch.getContactForce() << endl;
            //cout << "num details=" << patch.getNumDetails() << endl;
            for (int i=0; i < patch.getNumDetails(); ++i) {
                const ContactDetail& detail = patch.getContactDetail(i);
                const Real peakPressure = detail.getPeakPressure();
                // Make a black line from the element's contact point in the normal
                // direction, with length proportional to log(peak pressure)
                // on that element.

                DecorativeLine normal(detail.getContactPoint(),
                                      detail.getContactPoint()+ std::log10(peakPressure)
                                      * detail.getContactNormal());
                normal.setColor(Black);
                geometry.push_back(normal);

                // Make a red line that extends from the contact
                // point in the direction of the slip velocity, of length 3*slipvel.
                DecorativeLine slip(detail.getContactPoint(),
                                    detail.getContactPoint()+3*detail.getSlipVelocity());
                slip.setColor(Red);
                geometry.push_back(slip);
            }
        }
    }

Real CylinderImplicitFunction::
calcDerivative(const Array_<int>& derivComponents, const Vector& x) const {
    if (derivComponents.size() == 1 && derivComponents[0] < 2)
        return -2*x[derivComponents[0]]/square(ownerp->getRadius());
    if (derivComponents.size() == 2 &&
        derivComponents[0] == derivComponents[1] &&
        derivComponents[0] < 2 )
        return -2/square(ownerp->getRadius());
    return 0;
}

Real ObservedPointFitter::findBestFit
   (const MultibodySystem& system, State& state, 
    const Array_<MobilizedBodyIndex>&  bodyIxs, 
    const Array_<Array_<Vec3> >&       stations, 
    const Array_<Array_<Vec3> >&       targetLocations, 
    Real                                        tolerance) 
{
    Array_<Array_<Real> > weights(stations.size());
    for (int i = 0; i < (int)stations.size(); ++i)
        for (int j = 0; j < (int)stations[i].size(); ++j)
            weights[i].push_back(1.0);
    return findBestFit(system, state, bodyIxs, stations, targetLocations, weights, tolerance);
}

// We also rummage through the model to find fixed geometry that should be part
// of every frame. The supplied State must be realized through Instance stage.
void ModelVisualizer::collectFixedGeometry(const State& state) const {
    // Collect any fixed geometry from the ModelComponents.
    Array_<DecorativeGeometry> fixedGeometry;
    _model.generateDecorations
       (true, _model.getDisplayHints(), state, fixedGeometry);

    for (unsigned i=0; i < fixedGeometry.size(); ++i) {
        const DecorativeGeometry& dgeo = fixedGeometry[i];
        //cout << dgeo.getBodyId() << dgeo.getTransform() << endl;
        _viz->addDecoration(MobilizedBodyIndex(dgeo.getBodyId()), 
                            Transform(), dgeo);
    }
}

/** Same as above but for a given time series */
void InverseDynamicsSolver::solve(State &s, const FunctionSet &Qs, const Array_<double> &times, Array_<Vector> &genForceTrajectory)
{
	int nq = getModel().getNumCoordinates();
	int nt = times.size();

	//Preallocate if not done already
	genForceTrajectory.resize(nt, Vector(nq));
	
	AnalysisSet& analysisSet = const_cast<AnalysisSet&>(getModel().getAnalysisSet());
	//fill in results for each time
	for(int i=0; i<nt; i++){ 
		genForceTrajectory[i] = solve(s, Qs, times[i]);
		analysisSet.step(s, i);
	}
}

Real EllipsoidImplicitFunction::
calcDerivative(const Array_<int>& derivComponents, const Vector& x) const {
    const Vec3& radii = ownerp->getRadii();
    if (derivComponents.size() == 1) {
        int c = derivComponents[0];
        return -2*x[c]/(radii[c]*radii[c]);
    }
    if (   derivComponents.size() == 2 
        && derivComponents[0] == derivComponents[1]) {
        int c = derivComponents[0];
        return -2/(radii[c]*radii[c]);
    }
    // A mixed second derivative, or any higher derivative is zero.
    return 0;
}

void Constraint::SphereOnPlaneContactImpl::
calcDecorativeGeometryAndAppendVirtual
   (const State& s, Stage stage, Array_<DecorativeGeometry>& geom) const
{
    // We can't generate the artwork until we know the plane frame and the
    // sphere center and radius, which might not be until Position stage.
    if (   stage == Stage::Position
        && getMyMatterSubsystemRep().getShowDefaultGeometry())
    {
        const SimbodyMatterSubsystemRep& matterRep = getMyMatterSubsystemRep();
        const Parameters& params = getParameters(s);
        const Transform& X_FP = params.m_X_FP;
        const Vec3&      p_BO = params.m_p_BO;
        const Real       r    = params.m_radius;

        // TODO: should be instance-stage data from State rather than
        // topological data.
        // This makes z axis point along plane normal

        const MobilizedBodyIndex planeMBIx =
            getMobilizedBodyIndexOfConstrainedBody(m_planeBody_F);
        const MobilizedBodyIndex ballMBIx =
            getMobilizedBodyIndexOfConstrainedBody(m_ballBody_B);

        if (m_planeHalfWidth > 0) {
            // On the inboard body, draw a gray transparent rectangle,
            // outlined in black lines.
            geom.push_back(DecorativeBrick
               (Vec3(m_planeHalfWidth,m_planeHalfWidth,r/10))
                .setColor(Gray)
                .setRepresentation(DecorativeGeometry::DrawSurface)
                .setOpacity(Real(0.3))
                .setBodyId(planeMBIx)
                .setTransform(X_FP));
            geom.push_back(DecorativeFrame(m_planeHalfWidth/5)
                           .setColor(Gray)
                           .setBodyId(planeMBIx)
                           .setTransform(X_FP));
        }

        // On the ball body draw an orange mesh sphere.
        geom.push_back(DecorativeSphere(r)
            .setColor(Orange)
            .setRepresentation(DecorativeGeometry::DrawWireframe)
            .setBodyId(ballMBIx)
            .setTransform(p_BO));
    }
}

void Assembler::initialize() const {
    if (alreadyInitialized)
        return;

    ++nInitializations;

    Array_<QIndex> toBeLocked;
    reinitializeWithExtraQsLocked(toBeLocked);
    alreadyInitialized = true;
    return;

    /*NOTREACHED*/
    // TODO: This currently unused code would allow the Assembler to lock out
    // variables that it thinks aren't worth bothering with. Needs real-world
    // testing and probably some override options. And should there be a
    // desperation mode where all variables are tried if we can't assemble
    // with some of them removed?
    Vector grad = abs(asmSys->calcCurrentGradient());
    Real maxGrad = 0;
    for (FreeQIndex fx(0); fx < grad.size(); ++fx)
        maxGrad = std::max(maxGrad, grad[fx]);
    if (maxGrad == 0) // no q does anything; probably no objective
        maxGrad = Infinity; // no q will be kept for gradient purposes

    Matrix jac = asmSys->calcCurrentJacobian();
    Vector colNorm(getNumFreeQs());
    Real maxJac = 0;
    for (FreeQIndex fx(0); fx < grad.size(); ++fx) {
        colNorm[fx] = jac(fx).norm();
        maxJac = std::max(maxJac, colNorm[fx]);
    }
    if (maxJac == 0) // no q does anything; probably no constraints
        maxJac = Infinity; // no q will be kept for Jacobian purposes

    const Real QTol = SqrtEps;
    const Real minGradAllowed = maxGrad*QTol;
    const Real minJacAllowed = maxJac*QTol;
    for (FreeQIndex fx(0); fx < grad.size(); ++fx)
        if (grad[fx] < minGradAllowed && colNorm[fx] < minJacAllowed)
            toBeLocked.push_back(getQIndexOfFreeQ(fx));

    if (toBeLocked.size()) {
        cout << "Reinitializing with these q's locked:\n";
        cout << toBeLocked << endl;
        reinitializeWithExtraQsLocked(toBeLocked);
        alreadyInitialized = true;
    }
}

    virtual void generateDecorations(const State& state, Array_<DecorativeGeometry>& geometry) {
        const Vec3 frcColors[] = {Red,Orange,Cyan};
        const Vec3 momColors[] = {Blue,Green,Purple};
        m_system.realize(state, Stage::Velocity);

        const int ncont = m_compliant.getNumContactForces(state);
        for (int i=0; i < ncont; ++i) {
            const ContactForce& force = m_compliant.getContactForce(state,i);
            const ContactId     id    = force.getContactId();
            printf("viz contact %d: id=%d\n", i, (int)id);
            const Vec3& frc = force.getForceOnSurface2()[1];
            const Vec3& mom = force.getForceOnSurface2()[0];
            Real  frcMag = frc.norm(), momMag=mom.norm();
            int frcThickness = 1, momThickness = 1;
            Real frcScale = ForceScale, momScale = ForceScale;
            while (frcMag > 10)
                frcThickness++, frcScale /= 10, frcMag /= 10;
            while (momMag > 10)
                momThickness++, momScale /= 10, momMag /= 10;
            DecorativeLine frcLine(force.getContactPoint(),
                force.getContactPoint() + frcScale*frc);
            DecorativeLine momLine(force.getContactPoint(),
                force.getContactPoint() + momScale*mom);
            frcLine.setColor(frcColors[id%3]);
            momLine.setColor(momColors[id%3]);
            frcLine.setLineThickness(2*frcThickness);
            momLine.setLineThickness(2*momThickness);
            geometry.push_back(frcLine);
            geometry.push_back(momLine);
        }
    }

void ContactGeometry::TriangleMesh::
findVertexEdges(int vertex, Array_<int>& edges) const {
    // Begin at an arbitrary edge which intersects the vertex.

    int firstEdge = getImpl().vertices[vertex].firstEdge;
    int previousEdge = firstEdge;
    int previousFace = getImpl().edges[firstEdge].faces[0];

    // Walk around the vertex, using each edge to find the next face and each
    // face to find the next edge.

    do {
        edges.push_back(previousEdge);
        const ContactGeometry::TriangleMesh::Impl::Edge&
            edge = getImpl().edges[previousEdge];
        int nextFace = (edge.faces[0] == previousFace ? edge.faces[1]
                                                      : edge.faces[0]);
        const ContactGeometry::TriangleMesh::Impl::Face&
            face = getImpl().faces[nextFace];
        int nextEdge;
        if (    face.edges[0] != previousEdge
            && (face.vertices[0] == vertex || face.vertices[1] == vertex))
            nextEdge = face.edges[0];
        else if (   face.edges[1] != previousEdge
                 && (face.vertices[1] == vertex || face.vertices[2] == vertex))
            nextEdge = face.edges[1];
        else
            nextEdge = face.edges[2];
        previousEdge = nextEdge;
        previousFace = nextFace;
    } while (previousEdge != firstEdge);
}