C++ Array_::reserve方法代码示例

void testSpeedSimTKArray() {
    Array_<int> v; 
    v.reserve(Inner);

    for (int i=0; i < Outer; ++i) {
        v.clear();
        for (int i=0; i < Inner; ++i)
            v.push_back(i);
    }

    int sum;
    for (int i=0; i < Outer; ++i) {
        sum = i;
        for (unsigned i=0; i < v.size(); ++i)
            sum += v[i];
    }
    cout << "Array sum=" << sum << endl;
}

//.........这里部分代码省略.........

    Array_<std::pair<String,int> > helpMenuItems;
    helpMenuItems.push_back(std::make_pair("TBD - Sorry!", 1));
    viz.addMenu("Help", HelpMenuId, helpMenuItems);

    // Check for a Run->Quit menu pick every 1/4 second.
    //system.addEventHandler(new UserInputHandler(*silo, .25));

    // Initialize the system and state.
    
    system.realizeTopology();
    State state = system.getDefaultState();

    // SET INITIAL CONDITIONS
    #ifdef USE_SHERM_PARAMETERS
        brick.setQToFitTranslation(state, Vec3(0,2,.8));
        brick.setQToFitRotation(state, Rotation(BodyRotationSequence, 
                                                Pi/4, XAxis, Pi/6, YAxis));
        brick.setUToFitLinearVelocity(state, Vec3(-5,0,0));
    #endif
    #ifdef USE_TOM_PARAMETERS
        Vector initQ = Vector(Vec7(1,0,0,0, 0,1,0.8));
        initQ(0,4) = Vector(Quaternion(Rotation(SimTK::Pi/4, XAxis) *
                                       Rotation(SimTK::Pi/6, YAxis))
                            .asVec4());
        Vector initU = Vector(Vec6(0,0,0, 0,0,6));
        initQ[6] = 1.5;
        initU[5] = -3.96;  //First impact at 0.181 s.
        initU[3] = -5.0;
        state.setQ(initQ);
        state.setU(initU);
    #endif

    saveEm.reserve(10000);

    viz.report(state);
    printf("Default state\n");
    cout << "t=" << state.getTime() 
         << " q=" << brick.getQAsVector(state)  
         << " u=" << brick.getUAsVector(state)  
         << endl;

    cout << "\nChoose 'Go' from Run menu to simulate:\n";
    int menuId, item;
    do { silo->waitForMenuPick(menuId, item);
         if (menuId != RunMenuId || item != GoItem) 
             cout << "\aDude ... follow instructions!\n";
    } while (menuId != RunMenuId || item != GoItem);

   
    // Simulate it.

    // The system as parameterized is very stiff (mostly due to friction)
    // and thus runs best with CPodes which is extremely stable for
    // stiff problems. To get reasonable performance out of the explicit
    // integrators (like the RKs) you'll have to run at a very loose
    // accuracy like 0.1, or reduce the friction coefficients and
    // maybe the stiffnesses.

    //SemiExplicitEuler2Integrator integ(system);
    //CPodesIntegrator integ(system,CPodes::BDF,CPodes::Newton);
    RungeKuttaMersonIntegrator integ(system);
    integ.setReturnEveryInternalStep(true);
    integ.setAllowInterpolation(false);
    //RungeKutta3Integrator integ(system);
    //VerletIntegrator integ(system);

    int realizeSubsystemTopologyImpl(State& s) const  override {
        forceEnabledIndex.invalidate();
        enabledParallelForcesIndex.invalidate();
        enabledNonParallelForcesIndex.invalidate();
        cachedForcesAreValidCacheIndex.invalidate();
        rigidBodyForceCacheIndex.invalidate();
        mobilityForceCacheIndex.invalidate();
        particleForceCacheIndex.invalidate();

        // Some forces are disabled by default; initialize the enabled flags
        // accordingly. Also, see if we're going to need to do any caching
        // on behalf of any forces that don't depend on velocities.
        Array_<bool> forceEnabled(getNumForces());
        bool someForceElementNeedsCaching = false;
        for (int i = 0; i < (int)forces.size(); ++i) {
            forceEnabled[i] = !(forces[i]->isDisabledByDefault());
            if (!someForceElementNeedsCaching)
                someForceElementNeedsCaching =
                    forces[i]->getImpl().dependsOnlyOnPositions();
        }

        forceEnabledIndex = allocateDiscreteVariable(s, Stage::Instance,
            new Value<Array_<bool> >(forceEnabled));
        
        
        // Track the enabled forces and whether they should be parallelized.
        Array_<Force*> enabledNonParallelForces;
        Array_<Force*> enabledParallelForces;

        // Avoid repeatedly allocating memory.
        enabledNonParallelForces.reserve(forces.size());
        enabledParallelForces.reserve(forces.size());

        for (int i = 0; i < (int) forces.size(); ++i) {
            if(forceEnabled[i])
            {
                if (forces[i]->getImpl().shouldBeParallelIfPossible())
                    enabledParallelForces.push_back(forces[i]);
                else
                    enabledNonParallelForces.push_back(forces[i]);
            }
        }

        enabledNonParallelForcesIndex = allocateCacheEntry(s, Stage::Instance,
                          new Value<Array_<Force*> >(enabledNonParallelForces));
        enabledParallelForcesIndex = allocateCacheEntry(s, Stage::Instance,
                             new Value<Array_<Force*> >(enabledParallelForces));

        //Determine whether the subsystem has parallel forces - if so, use the
        //parallel implementation of CalcForcesTask (even if those parallel
        //forces are not currently enabled - they could be enabled in the
        //future)
        //
        // *Consider the following example:*
        // Parallel Forces: A B C
        // NonParallel Forces: D E
        //
        // Enabled Forces at Time 0: D E
        // Enabled Forces at Time 1: A B C D E
        
        bool hasParallelForces = false;
        for(int x = 0; x < (int)forces.size(); ++x)
        {
            if (forces[x]->getImpl().shouldBeParallelIfPossible())
            {
                hasParallelForces = true;
                break;
            }
        }
        if(hasParallelForces)
            calcForcesTask = new CalcForcesParallelTask();
        else
            calcForcesTask = new CalcForcesNonParallelTask();
        
        // Note that we'll allocate these even if all the needs-caching
        // elements are presently disabled. That way they'll be around when
        // the force gets enabled.
        if (someForceElementNeedsCaching) {
            cachedForcesAreValidCacheIndex =
                allocateCacheEntry(s, Stage::Position, new Value<bool>());
            rigidBodyForceCacheIndex = allocateCacheEntry(s, Stage::Dynamics,
                new Value<Vector_<SpatialVec> >());
            mobilityForceCacheIndex = allocateCacheEntry(s, Stage::Dynamics,
                new Value<Vector>());
            particleForceCacheIndex = allocateCacheEntry(s, Stage::Dynamics,
                new Value<Vector_<Vec3> >());
        }

        // We must realizeTopology() even if the force is disabled by default.
        for (int i = 0; i < (int) forces.size(); ++i)
            forces[i]->getImpl().realizeTopology(s);
        return 0;
    }

//.........这里部分代码省略.........
    viz.setShowSimTime(true);
    viz.setDesiredFrameRate(FrameRate);
    viz.setShowFrameRate(true);
    viz.setBackgroundType(Visualizer::SolidColor);
    viz.setBackgroundColor(White*.9);

    Visualizer::InputSilo* silo = new Visualizer::InputSilo();
    viz.addInputListener(silo);
    Array_<std::pair<String,int> > runMenuItems;
    runMenuItems.push_back(std::make_pair("Go", GoItem));
    runMenuItems.push_back(std::make_pair("Replay", ReplayItem));
    runMenuItems.push_back(std::make_pair("Quit", QuitItem));
    viz.addMenu("Run", RunMenuId, runMenuItems);

    Array_<std::pair<String,int> > helpMenuItems;
    helpMenuItems.push_back(std::make_pair("TBD - Sorry!", 1));
    viz.addMenu("Help", HelpMenuId, helpMenuItems);

    system.addEventReporter(new MyReporter(system,contactForces,ReportInterval));
    system.addEventReporter(new Visualizer::Reporter(viz, ReportInterval));

    // Check for a Run->Quit menu pick every 1/4 second.
    system.addEventHandler(new UserInputHandler(*silo, .25));

    // Initialize the system and state.
    
    system.realizeTopology();
    State state = system.getDefaultState();

    brick.setQToFitRotation(state, Rotation(SpaceRotationSequence, 
                                            .1, ZAxis, .05, XAxis));
    brick.setUToFitLinearVelocity(state, Vec3(2,0,0));

    saveEm.reserve(10000);

    viz.report(state);
    printf("Default state\n");
    cout << "t=" << state.getTime() 
         << " q=" << brick.getQAsVector(state)  
         << " u=" << brick.getUAsVector(state)  
         << endl;

    cout << "\nChoose 'Go' from Run menu to simulate:\n";
    int menuId, item;
    do { silo->waitForMenuPick(menuId, item);
         if (menuId != RunMenuId || item != GoItem) 
             cout << "\aDude ... follow instructions!\n";
    } while (menuId != RunMenuId || item != GoItem);

   
    // Simulate it.

    // The system as parameterized is very stiff (mostly due to friction)
    // and thus runs best with CPodes which is extremely stable for
    // stiff problems. To get reasonable performance out of the explicit
    // integrators (like the RKs) you'll have to run at a very loose
    // accuracy like 0.1, or reduce the friction coefficients and
    // maybe the stiffnesses.

    //SemiExplicitEuler2Integrator integ(system);
    //CPodesIntegrator integ(system,CPodes::BDF,CPodes::Newton);
    RungeKuttaMersonIntegrator integ(system);
    //RungeKutta3Integrator integ(system);
    //VerletIntegrator integ(system);
    //integ.setMaximumStepSize(1e-0001);
    //integ.setAccuracy(1e-3); // minimum for CPodes

//.........这里部分代码省略.........
    Vector y = patchScaleY*y_;
    Matrix f = patchScaleF*f_; 

    BicubicSurface patch(x, y, f, 0);

    Real highRes = 30;
    Real lowRes  = 1;

    PolygonalMesh highResPatchMesh = patch.createPolygonalMesh(highRes);
    PolygonalMesh lowResPatchMesh = patch.createPolygonalMesh(lowRes);

   
    pendulumFemur.addBodyDecoration(patchTransform,
        DecorativeMesh(highResPatchMesh).setColor(Cyan).setOpacity(.75));

    pendulumFemur.addBodyDecoration(patchTransform,
         DecorativeMesh(lowResPatchMesh).setRepresentation(DecorativeGeometry::DrawWireframe));

    Vec3 patchP(-0.5,-1,2), patchQ(-0.5,1,2);

    pendulumFemur.addBodyDecoration(patchTransform,
        DecorativePoint(patchP).setColor(Green).setScale(2));

    pendulumFemur.addBodyDecoration(patchTransform,
        DecorativePoint(patchQ).setColor(Red).setScale(2));

     CableObstacle::Surface patchObstacle(path2, pendulumFemur, patchTransform,
         ContactGeometry::SmoothHeightMap(patch));
        
      patchObstacle.setContactPointHints(patchP, patchQ);
    
      patchObstacle.setDisabledByDefault(true);

    // Sphere
    Real      sphRadius = 1.5;

    Vec3      sphOffset(0, -0.5, 0);
    Rotation  sphRotation(0*Pi, YAxis);
    Transform sphTransform(sphRotation, sphOffset);

    CableObstacle::Surface tibiaSphere(path2, pendulumTibia, sphTransform,
        ContactGeometry::Sphere(sphRadius));

    Vec3 sphP(1.5,-0.5,0), sphQ(1.5,0.5,0);
    tibiaSphere.setContactPointHints(sphP, sphQ);

    pendulumTibia.addBodyDecoration(sphTransform,
        DecorativeSphere(sphRadius).setColor(Red).setOpacity(0.5));

    // Make cable a spring
    CableSpring cable2(forces, path2, 50., 18., 0.1); 

    Visualizer viz(system);
    viz.setShowFrameNumber(true);
    system.addEventReporter(new Visualizer::Reporter(viz, 1./30));
    system.addEventReporter(new ShowStuff(system, cable2, 0.02));    
    // Initialize the system and state.
    
    system.realizeTopology();
    State state = system.getDefaultState();

    system.realize(state, Stage::Position);
    viz.report(state);
    cout << "path2 init length=" << path2.getCableLength(state) << endl;
    cout << "Hit ENTER ...";
    getchar();

    // path1.setIntegratedCableLengthDot(state, path1.getCableLength(state));

    // Simulate it.
    saveStates.clear(); saveStates.reserve(2000);

    // RungeKutta3Integrator integ(system);
    RungeKuttaMersonIntegrator integ(system);
    // CPodesIntegrator integ(system);
    // integ.setAllowInterpolation(false);
    integ.setAccuracy(1e-5);
    TimeStepper ts(system, integ);
    ts.initialize(state);
    ShowStuff::showHeading(cout);

    const Real finalTime = 10;
    const double startTime = realTime();
    ts.stepTo(finalTime);
    cout << "DONE with " << finalTime 
         << "s simulated in " << realTime()-startTime
         << "s elapsed.\n";

    while (true) {
        cout << "Hit ENTER FOR REPLAY, Q to quit ...";
        const char ch = getchar();
        if (ch=='q' || ch=='Q') break;
        for (unsigned i=0; i < saveStates.size(); ++i)
            viz.report(saveStates[i]);
    }

  } catch (const std::exception& e) {
    cout << "EXCEPTION: " << e.what() << "\n";
  }
}