C++ MultibodySystem::addEventReporter方法代码示例

int main() {
    
    // Create the system.
    
    MultibodySystem system; system.setUseUniformBackground(true);
    SimbodyMatterSubsystem matter(system);
    GeneralForceSubsystem forces(system);
    Force::UniformGravity gravity(forces, matter, Vec3(0, -9.8, 0));
    Body::Rigid pendulumBody(MassProperties(1.0, Vec3(0), Inertia(1)));
    pendulumBody.addDecoration(Transform(), DecorativeSphere(0.1));

    MobilizedBody lastBody = matter.Ground();
    for (int i = 0; i < 10; ++i) {
        MobilizedBody::Ball pendulum(lastBody,     Transform(Vec3(0)), 
                                     pendulumBody, Transform(Vec3(0, 1, 0)));
        lastBody = pendulum;
    }

    system.addEventReporter(new Visualizer::Reporter(system, 1./30));
    
    // Initialize the system and state.
    
    system.realizeTopology();
    State state = system.getDefaultState();
    Random::Gaussian random;
    for (int i = 0; i < state.getNQ(); ++i)
        state.updQ()[i] = random.getValue();
    
    // Simulate it.

    RungeKuttaMersonIntegrator integ(system);
    TimeStepper ts(system, integ);
    ts.initialize(state);
    ts.stepTo(10.0);
}

int main() {
    try { // catch errors if any

    // Create the system, with subsystems for the bodies and some forces.
    MultibodySystem system; 
    SimbodyMatterSubsystem matter(system);
    GeneralForceSubsystem forces(system);
    Force::Gravity gravity(forces, matter, -YAxis, 9.8);

    // Describe a body with a point mass at (0, -3, 0) and draw a sphere there.
    Real mass = 3; Vec3 pos(0,-3,0);
    Body::Rigid bodyInfo(MassProperties(mass, pos, UnitInertia::pointMassAt(pos)));
    bodyInfo.addDecoration(pos, DecorativeSphere(.2).setOpacity(.5));

    // Create the tree of mobilized bodies, reusing the above body description.
    MobilizedBody::Pin bodyT  (matter.Ground(), Vec3(0), bodyInfo, Vec3(0));
    MobilizedBody::Pin leftArm(bodyT, Vec3(-2, 0, 0),    bodyInfo, Vec3(0));
    MobilizedBody::Pin rtArm  (bodyT, Vec3(2, 0, 0),     bodyInfo, Vec3(0));

    // Add a joint stop to the left arm restricting it to q in [0,Pi/5].
    Force::MobilityLinearStop stop(forces, leftArm, MobilizerQIndex(0), 
        10000,  // stiffness
        0.5,    // dissipation coefficient
        0*Pi,   // lower stop
        Pi/5);  // upper stop

    // Ask for visualization every 1/30 second.
    system.setUseUniformBackground(true); // turn off floor    
    system.addEventReporter(new Visualizer::Reporter(system, 1./30));
    
    // Initialize the system and state.    
    State state = system.realizeTopology();
    leftArm.setAngle(state, Pi/5);

    // Simulate for 10 seconds.
    RungeKuttaMersonIntegrator integ(system);
    TimeStepper ts(system, integ);
    ts.initialize(state);
    ts.stepTo(10);

    } catch (const std::exception& e) {
        std::cout << "ERROR: " << e.what() << std::endl;
        return 1;
    }
    return 0;
}

int main() {
  try
  { // Create the system.
    
    MultibodySystem         system; system.setUseUniformBackground(true);
    SimbodyMatterSubsystem  matter(system);
    GeneralForceSubsystem   forces(system);
    Force::UniformGravity   gravity(forces, matter, Vec3(0, -9.8, 0));

    Body::Rigid pendulumBody(MassProperties(1.0, Vec3(0), Inertia(1)));
    pendulumBody.addDecoration(Transform(), DecorativeSphere(0.1));
    MobilizedBody::Pin pendulum(matter.Ground(), Transform(Vec3(0)), 
                                pendulumBody,    Transform(Vec3(0, 1, 0)));
    //Motion::Steady(pendulum, 1);

    Visualizer viz(system);
    system.addEventReporter(new Visualizer::Reporter(viz, 1./30));
   
    // Initialize the system and state.
    
    system.realizeTopology();
    State state = system.getDefaultState();
    pendulum.setOneU(state, 0, 1.0);

    
    // Simulate it.

    RungeKuttaMersonIntegrator integ(system);
    TimeStepper ts(system, integ);
    ts.initialize(state);
    ts.stepTo(100.0);

  } catch (const std::exception& e) {
    std::printf("EXCEPTION THROWN: %s\n", e.what());
    exit(1);

  } catch (...) {
    std::printf("UNKNOWN EXCEPTION THROWN\n");
    exit(1);
  }

    return 0;
}

//.........这里部分代码省略.........
                                 ballBody, Transform(Vec3(0)));
*/

        MobilizedBody::Free ball3(matter.Ground(), Transform(Vec3(-1,-2,0)),
                                  ballBody, Transform(Vec3(0)));


        MobilizedBody::Pin pendulum3(matter.Ground(), Transform(Vec3(-2,0,0)),
                                     pendulumBody3, Transform(Vec3(0, 2, 0)));

        ball.updBody();
        ball1.updBody();

        Visualizer viz(system);
        viz.addDecorationGenerator(new ForceArrowGenerator(system,contactForces));
        viz.setMode(Visualizer::RealTime);
        viz.setDesiredBufferLengthInSec(1);
        viz.setDesiredFrameRate(FrameRate);
        viz.setGroundHeight(-3);
        viz.setShowShadows(true);
        viz.setBackgroundType(Visualizer::SolidColor);
        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));
        //  system.addEventHandler(new TriggeredEventHandler(Stage::Model));
        // Initialize the system and state.

        system.realizeTopology();

        State state = system.getDefaultState();
        /*
        ball.setQToFitTransform(state, Transform(Rotation(Pi/2,XAxis),
                                                 Vec3(0,-1.8,0)));
*/
        //pendulum.setOneQ(state, 0, -Pi/12);
        pendulum3.setOneQ(state, 0, -Pi/2);
        pendulum3.setOneU(state, 0, Pi/4);
        // ball.setOneU(state, 1, 0.1);
        viz.report(state);
        matter.updAllParticleVelocities(state);
        printf("Default state\n");
        /* cout << "t=" << state.getTime()
         << " q=" << pendulum.getQAsVector(state) << pendulum2.getQAsVector(state)
         << " u=" << pendulum.getUAsVector(state) << pendulum2.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);

int main() {
  try
  { // Create the system.

    MultibodySystem         system;
    SimbodyMatterSubsystem  matter(system);
    GeneralForceSubsystem   forces(system);
    Force::Gravity   gravity(forces, matter, UnitVec3(-1,0,0), 9.81);

    ContactTrackerSubsystem  tracker(system);
    CompliantContactSubsystem contactForces(system, tracker);
    contactForces.setTrackDissipatedEnergy(true);
    contactForces.setTransitionVelocity(1e-2); // m/s

    // Ground's normal is +x for this model
    system.setUpDirection(+XAxis);

    // Uncomment this if you want a more elegant movie.
    //matter.setShowDefaultGeometry(false);

    const Real ud = .3; // dynamic
    const Real us = .6; // static
    const Real uv = 0;  // viscous (force/velocity)
    const Real k = 1e8; // pascals
    const Real c = 0.01; // dissipation (1/v)


    // Halfspace default is +x, this one occupies -x instead, so flip.
    const Rotation R_xdown(Pi,ZAxis);

    matter.Ground().updBody().addContactSurface(
        Transform(R_xdown, Vec3(0,0,0)),
        ContactSurface(ContactGeometry::HalfSpace(),
                       ContactMaterial(k,c,us,ud,uv)));


    const Real ellipsoidMass = 1; // kg
    const Vec3 halfDims(2*Cm2m, 20*Cm2m, 3*Cm2m); // m (read in cm)
    const Vec3 comLoc(-1*Cm2m, 0, 0);
    const Inertia centralInertia(Vec3(17,2,16)*CmSq2mSq, Vec3(0,0,.2)*CmSq2mSq); // now kg-m^2
    const Inertia inertia(centralInertia.shiftFromMassCenter(-comLoc, ellipsoidMass)); // in S
    Body::Rigid ellipsoidBody(MassProperties(ellipsoidMass, comLoc, inertia));

    ellipsoidBody.addDecoration(Transform(),
        DecorativeEllipsoid(halfDims).setColor(Cyan)
         //.setOpacity(.5)
         .setResolution(3));
    ellipsoidBody.addContactSurface(Transform(),
        ContactSurface(ContactGeometry::Ellipsoid(halfDims),
                       ContactMaterial(k,c,us,ud,uv))
                       );
    MobilizedBody::Free ellipsoid(matter.Ground(), Transform(Vec3(0,0,0)),
        ellipsoidBody, Transform(Vec3(0)));


    Visualizer viz(system);
    viz.addDecorationGenerator(new ForceArrowGenerator(system,contactForces));
    viz.setMode(Visualizer::RealTime);
    viz.setDesiredFrameRate(FrameRate);
    viz.setCameraClippingPlanes(0.1, 10);

    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();
    matter.setUseEulerAngles(state, true);
    system.realizeModel(state);

    ellipsoid.setQToFitTransform(state, Transform(
        Rotation(BodyRotationSequence,  0  *Deg2Rad, XAxis,
                                        0.5*Deg2Rad, YAxis,
                                       -0.5*Deg2Rad, ZAxis),
        Vec3(2.1*Cm2m, 0, 0)));

    ellipsoid.setUToFitAngularVelocity(state, 2*Vec3(5,0,0)); // rad/s

    viz.report(state);
    printf("Default state\n");

    cout << "\nChoose 'Go' from Run menu to simulate:\n";
    int menuId, item;
    do { silo->waitForMenuPick(menuId, item);
//.........这里部分代码省略.........

int main() {
  try {

    // setup test problem
    double r = .5;
    double uP = -Pi/2;
    double vP = Pi/3;
    double uQ = 0;
    double vQ = 2;
    Vec3 O(-r, -r, 0.2);
    Vec3 I(r, r, -r);
    Vec3 P(r*cos(uP)*sin(vP), r*sin(uP)*sin(vP), r*cos(vP));
    Vec3 Q(r*cos(uQ)*sin(vQ), r*sin(uQ)*sin(vQ), r*cos(vQ));

    Vec3 r_OP = P-O;
    Vec3 r_IQ = Q-I;
    Vec3 tP = r_OP.normalize();
    Vec3 tQ = r_IQ.normalize();

    int n = 6; // problem size
    Vector x(n), dx(n), Fx(n), xold(n);
    Matrix J(n,n);

    ContactGeometry::Sphere geom(r);
//    r = 2;
//    Vec3 radii(1,2,3);
//    ContactGeometry::Ellipsoid geom(radii);
    Geodesic geod;

    // Create a dummy MultibodySystem for visualization purposes
    MultibodySystem dummySystem;
    SimbodyMatterSubsystem matter(dummySystem);
    matter.updGround().addBodyDecoration(Transform(), geom.createDecorativeGeometry()
            .setColor(Gray)
            .setOpacity(0.5)
            .setResolution(5));

    // Visualize with default options; ask for a report every 1/30 of a second
    // to match the Visualizer's default 30 frames per second rate.
    Visualizer viz(dummySystem);
    viz.setBackgroundType(Visualizer::SolidColor);
    dummySystem.addEventReporter(new Visualizer::Reporter(viz, 1./30));

    // add vizualization callbacks for geodesics, contact points, etc.
    viz.addDecorationGenerator(new GeodesicDecorator(geom.getGeodP(), Red));
    viz.addDecorationGenerator(new GeodesicDecorator(geom.getGeodQ(), Blue));
    viz.addDecorationGenerator(new GeodesicDecorator(geod, Orange));
    viz.addDecorationGenerator(new PlaneDecorator(geom.getPlane(), Gray));
    viz.addDecorationGenerator(new PathDecorator(x, O, I, Green));
    dummySystem.realizeTopology();
    State dummyState = dummySystem.getDefaultState();


    // calculate the geodesic
    geom.addVizReporter(new VizPeriodicReporter(viz, dummyState, vizInterval));
    viz.report(dummyState);

    // creat path error function
    //PathError pathErrorFnc(n, n, geom, geod, O, I);
    PathErrorSplit pathErrorFnc(n, n, geom, geod, O, I);
    pathErrorFnc.setEstimatedAccuracy(estimatedPathErrorAccuracy);
    Differentiator diff(pathErrorFnc);

    // set initial conditions
    x[0]=P[0]; x[1]=P[1]; x[2]=P[2];
    x[3]=Q[0]; x[4]=Q[1]; x[5]=Q[2];

    Real f, fold, lam;

    pathErrorFnc.f(x, Fx);
    viz.report(dummyState);
    sleepInSec(pauseBetweenPathIterations);

    f = std::sqrt(~Fx*Fx);
    for (int i = 0; i < maxNewtonIterations; ++i) {
        if (f < ftol) {
            std::cout << "path converged in " << i << " iterations" << std::endl;
//            cout << "obstacle err = " << Fx << ", x = " << x << endl;
            break;
        }

        diff.calcJacobian(x, Fx, J, Differentiator::ForwardDifference);
        dx = J.invert()*Fx;

        fold = f;
        xold = x;

        // backtracking
        lam = 1;
        while (true) {
            x = xold - lam*dx;
            cout << "TRY stepsz=" << lam << " sz*dx=" << lam*dx << endl;
            pathErrorFnc.f(x, Fx);
            f = std::sqrt(~Fx*Fx);
            if (f > fold && lam > minlam) {
                lam = lam / 2;
            } else {
                break;
            }
        }
//.........这里部分代码省略.........

int main() {
  try {   
    // Create the system, with subsystems for the bodies and some forces.
    MultibodySystem system;
    SimbodyMatterSubsystem matter(system);
    GeneralForceSubsystem forces(system);

    // Hint to Visualizer: don't show ground plane.
    system.setUseUniformBackground(true);

    // Add gravity as a force element.
    Force::UniformGravity gravity(forces, matter, Vec3(0, -9.81, 0));

    // Create the body and some artwork for it.
    const Vec3 halfLengths(.5, .1, .25); // half-size of brick (m)
    const Real mass = 2; // total mass of brick (kg)
    Body::Rigid pendulumBody(MassProperties(mass, Vec3(0), 
                                mass*UnitInertia::brick(halfLengths)));
    pendulumBody.addDecoration(Transform(), 
        DecorativeBrick(halfLengths).setColor(Red));

    // Add an instance of the body to the multibody system by connecting
    // it to Ground via a Ball mobilizer.
    MobilizedBody::Ball pendulum1(matter.updGround(), Transform(Vec3(-1,-1, 0)), 
                                  pendulumBody,       Transform(Vec3( 0, 1, 0)));

    // Add a second instance of the pendulum nearby.
    MobilizedBody::Ball pendulum2(matter.updGround(), Transform(Vec3(1,-1, 0)), 
                                  pendulumBody,       Transform(Vec3(0, 1, 0)));

    // Connect the origins of the two pendulum bodies together with our
    // rod-like custom constraint.
    const Real d = 1.5; // desired separation distance
    Constraint::Custom rod(new ExampleConstraint(pendulum1, pendulum2, d));

    // Visualize with default options.
    Visualizer viz(system);

    // Add a rubber band line connecting the origins of the two bodies to
    // represent the rod constraint.
    viz.addRubberBandLine(pendulum1, Vec3(0), pendulum2, Vec3(0),
        DecorativeLine().setColor(Blue).setLineThickness(3));

    // Ask for a report every 1/30 of a second to match the Visualizer's 
    // default rate of 30 frames per second.
    system.addEventReporter(new Visualizer::Reporter(viz, 1./30));
    // Output total energy to the console once per second.
    system.addEventReporter(new TextDataEventReporter
                                   (system, new MyEvaluateEnergy(), 1.0));
    
    // Initialize the system and state.   
    State state = system.realizeTopology();

    // Orient the two pendulums asymmetrically so they'll do something more 
    // interesting than just hang there.
    pendulum1.setQToFitRotation(state, Rotation(Pi/4, ZAxis));
    pendulum2.setQToFitRotation(state, Rotation(BodyRotationSequence,
                                                Pi/4, ZAxis, Pi/4, YAxis));

    // Evaluate the system at the new state and draw one frame manually.
    system.realize(state);
    viz.report(state);

    // Simulate it.
    cout << "Hit ENTER to run a short simulation.\n";
    cout << "(Energy should be conserved to about four decimal places.)\n";
    getchar();

    RungeKuttaMersonIntegrator integ(system);
    integ.setAccuracy(1e-4); // ask for about 4 decimal places (default is 3)
    TimeStepper ts(system, integ);
    ts.initialize(state);
    ts.stepTo(10.0);

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

    return 0;
}

void testConservationOfEnergy() {
    // Create the system.
    MultibodySystem         system;
    SimbodyMatterSubsystem  matter(system);
    GeneralForceSubsystem   forces(system);
    Force::UniformGravity   gravity(forces, matter, Vec3(0, -9.8, 0));

    const Real Mass = 1;
    const Vec3 HalfShape = Vec3(1,.5,.25)/2;
    const Transform BodyAttach(Rotation(), Vec3(HalfShape[0],0,0));
    Body::Rigid brickBody(MassProperties(Mass, Vec3(.1,.2,.3),
                                Mass*Inertia(1,1.1,1.2,0.01,0.02,0.03)));
    //Body::Rigid brickBody(MassProperties(Mass, Vec3(0),
    //                        Mass*UnitInertia::ellipsoid(HalfShape)));
    brickBody.addDecoration(Transform(), DecorativeEllipsoid(HalfShape)
                                            .setOpacity(0.25)
                                            .setColor(Blue));
    brickBody.addDecoration(BodyAttach,
                DecorativeFrame(0.5).setColor(Red));

    const int NBod=50;
    MobilizedBody::Free brick1(matter.Ground(), Transform(),
                               brickBody,       BodyAttach);
    MobilizedBody::Free brick2(brick1, Transform(),
                               brickBody,       BodyAttach);
    MobilizedBody prev=brick2;
    MobilizedBody body25;
    for (int i=0; i<NBod; ++i) {
        MobilizedBody::Ball next(prev, -1*BodyAttach.p(),
                                 brickBody, BodyAttach);
        if (i==25) body25=next;
        //Force::TwoPointLinearSpring(forces,
        //    prev, Vec3(0), next, Vec3(0), 1000, 1);
        prev=next;
    }

    Constraint::Ball(matter.Ground(), Vec3(0,1,0)-2*NBod/3*BodyAttach.p(),
        prev, BodyAttach.p());
    Constraint::Ball(matter.Ground(), Vec3(0,1,0)-1*NBod/3*BodyAttach.p(),
        body25, BodyAttach.p());


    Vec6 k1(1,100,1,100,100,100), c1(0);
    Force::LinearBushing(forces, matter.Ground(), -2*NBod/3*BodyAttach.p(),
                 prev, BodyAttach.p(), k1, c1);
    matter.Ground().addBodyDecoration(-2*NBod/3*BodyAttach.p(),
        DecorativeFrame().setColor(Green));

    Force::Thermostat thermo(forces, matter,
        SimTK_BOLTZMANN_CONSTANT_MD,
        5000,
        .1);

    Vec6 k(1,100,1,100,100,100), c(0);
    Force::LinearBushing bushing1(forces, matter.Ground(), -1*NBod/3*BodyAttach.p(),
        brick1, BodyAttach, k, c);
    Force::LinearBushing bushing2(forces, brick1, Transform(),
        brick2, BodyAttach, k, c);
    matter.Ground().addBodyDecoration(-1*NBod/3*BodyAttach.p(),
        DecorativeFrame().setColor(Green));


    Visualizer viz(system);
    Visualizer::Reporter* reporter = new Visualizer::Reporter(viz, 1./30);
    viz.setBackgroundType(Visualizer::SolidColor);
    system.addEventReporter(reporter);

    ThermoReporter* thermoReport = new ThermoReporter
        (system, thermo, bushing1, bushing2, 1./10);
    system.addEventReporter(thermoReport);

    // Initialize the system and state.

    system.realizeTopology();
    State state = system.getDefaultState();

    viz.report(state);
    printf("Default state -- hit ENTER\n");
    cout << "t=" << state.getTime()
         << " q=" << brick1.getQAsVector(state) << brick2.getQAsVector(state)
         << " u=" << brick1.getUAsVector(state) << brick2.getUAsVector(state)
         << "\nnChains=" << thermo.getNumChains(state)
         << " T="        << thermo.getBathTemperature(state)
         << "\nt_relax=" << thermo.getRelaxationTime(state)
         << " kB="       << thermo.getBoltzmannsConstant()
         << endl;
    getchar();

    state.setTime(0);
    system.realize(state, Stage::Acceleration);
    Vector initU(state.getNU());
    initU = Test::randVector(state.getNU());
    state.updU()=initU;


    RungeKuttaMersonIntegrator integ(system);
    //integ.setMinimumStepSize(1e-1);
    integ.setAccuracy(1e-2);
    TimeStepper ts(system, integ);
    ts.initialize(state);
//.........这里部分代码省略.........

//==============================================================================
//                                   MAIN
//==============================================================================
int main() {
    try { // catch errors if any
    // Create the system, with subsystems for the bodies and some forces.
    MultibodySystem system;
    SimbodyMatterSubsystem matter(system);
    GeneralForceSubsystem forces(system);
    ContactTrackerSubsystem   tracker(system);
    CompliantContactSubsystem contact(system, tracker);
    contact.setTransitionVelocity(transitionVelocity);
    Force::Gravity(forces, matter, -YAxis, 9.81);

    // Set up visualization and ask for a frame every 1/30 second.
    Visualizer viz(system);
    viz.setShowSimTime(true); viz.setShowFrameRate(true);
    viz.addSlider("Speed", SpeedControlSlider, -10, 10, 0);
    Visualizer::InputSilo* silo = new Visualizer::InputSilo();
    viz.addInputListener(silo);
    #ifdef ANIMATE
    system.addEventReporter(new Visualizer::Reporter(viz, 1./30));
    #endif
    DecorativeText help("Any input to start; ESC to quit");
    help.setIsScreenText(true);
    viz.addDecoration(MobilizedBodyIndex(0),Vec3(0),help);
    matter.setShowDefaultGeometry(false);

    // Add the Ground contact geometry. Contact half space has -XAxis normal
    // (right hand wall) so we have to rotate.
    MobilizedBody& Ground = matter.updGround(); // Nicer name for Ground.
    Ground.updBody().addContactSurface(Transform(YtoX,Vec3(0)),
        ContactSurface(ContactGeometry::HalfSpace(),concrete));

    // Add some speed bumps.
    const int NBumps = 2; const Vec3 BumpShape(.8,0.2,2);
    for (int i=0; i < NBumps; ++i) {
        const Real x = -2*(i+1);
        Ground.updBody().addContactSurface(Vec3(x,0,0),
            ContactSurface(ContactGeometry::Ellipsoid(BumpShape), rubber));
        Ground.updBody().addDecoration(Vec3(x,0,0),
            DecorativeEllipsoid(BumpShape).setColor(Gray).setResolution(3));
    }

    // TORSO
    const Real TorsoHeight = 1.1;
    const Vec3 torsoHDims(1,.08,.8);
    const Real torsoVolume = 8*torsoHDims[0]*torsoHDims[1]*torsoHDims[2];
    const Real torsoMass = torsoVolume*rubber_density/10;
    const Vec3 torsoCOM(0,-.75,0); // put it low for stability
    Body::Rigid torsoInfo(MassProperties(torsoMass,torsoCOM,
        UnitInertia::brick(torsoHDims).shiftFromCentroidInPlace(-torsoCOM)));
    torsoInfo.addDecoration(Vec3(0),
        DecorativeBrick(torsoHDims).setColor(Cyan));

    // CRANK
    const Vec3 crankCenter(0,0,0); // in torso frame
    const Vec3 crankOffset(0,0,torsoHDims[2]+LinkDepth); // left/right offset
    const Real MLen=15/100.; // crank radius
    Body::Rigid crankInfo(MassProperties(.1,Vec3(0),
                            UnitInertia::cylinderAlongZ(MLen, LinkDepth)));
    crankInfo.addDecoration(Vec3(0),
        DecorativeBrick(Vec3(LinkWidth,LinkWidth,torsoHDims[2]))
        .setColor(Black));
    const Vec3 CrankConnect(MLen,0,0); // in crank frame

    // Add the torso and crank mobilized bodies.
    MobilizedBody::Free torso(Ground,Vec3(0,TorsoHeight,0), torsoInfo,Vec3(0));
    MobilizedBody::Pin crank(torso,crankCenter, crankInfo, Vec3(0));

    // Add the legs.
    for (int i=-1; i<=1; ++i) {
        const Vec3 offset = crankCenter + i*crankOffset;
        const Vec3 linkSpace(0,0,2*LinkDepth);
        const Rotation R_CP(i*2*Pi/3,ZAxis);
        // Add crank bars for looks.
        crank.addBodyDecoration(
            Transform(R_CP, offset+1.5*MLen/2*R_CP.x()+(i==0?linkSpace:Vec3(0))),
            DecorativeBrick(Vec3(1.5*MLen/2,LinkWidth,LinkDepth))
                        .setColor(Yellow));

        addOneLeg(viz, torso, offset + i*linkSpace,
                  crank, R_CP*CrankConnect);
        addOneLeg(viz, torso, Transform(Rotation(Pi,YAxis), offset + i*linkSpace),
                  crank, R_CP*CrankConnect);
    }

    // Add speed control.
    Motion::Steady motor(crank, 0); // User controls speed.
    system.addEventHandler
       (new UserInputHandler(*silo, motor, Real(0.1))); //check input every 100ms

    // Initialize the system and state.
    State state = system.realizeTopology();
    system.realize(state);
    printf("Theo Jansen Strandbeest in 3D:\n");
    printf("%d bodies, %d mobilities, -%d constraint equations -%d motions\n",
        matter.getNumBodies(), state.getNU(), state.getNMultipliers(),
        matter.getMotionMultipliers(state).size());

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

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

    Body::Rigid patellaBody(MassProperties(1.0, Vec3(0), Inertia(1)));
    patellaBody.addDecoration(Transform(), 
        showPatella.setColor(Red).setOpacity(.2));
    patellaBody.addContactSurface(Transform(),
        ContactSurface(patellaTri,
            ContactMaterial(fK*.001,fDis*.9,fFac*.8,fFac*.7,fVis*10),
            .01 /*thickness*/));

    MobilizedBody::Free patella(matter.Ground(), Transform(Vec3(0)), 
                                patellaBody,    Transform(Vec3(0)));


    //// The old way ...
    //OLDcontact.addBody(OLDcontactSet, ball,
    //    pyramid, Transform());

    //OLDcontact.addBody(OLDcontactSet, matter.updGround(),
    //    ContactGeometry::HalfSpace(), Transform(R_xdown, Vec3(0,-3,0)));
    //ElasticFoundationForce ef(forces, OLDcontact, OLDcontactSet);
    //Real stiffness = 1e6, dissipation = 0.01, us = 0.1, 
    //    ud = 0.05, uv = 0.01, vt = 0.01;
    ////Real stiffness = 1e6, dissipation = 0.1, us = 0.8, 
    ////    ud = 0.7, uv = 0.01, vt = 0.01;

    //ef.setBodyParameters(ContactSurfaceIndex(0), 
    //    stiffness, dissipation, us, ud, uv);
    //ef.setTransitionVelocity(vt);
    //// end of old way.

    Visualizer viz(system);
    Visualizer::Reporter& reporter = *new Visualizer::Reporter(viz, ReportInterval);
    viz.addDecorationGenerator(new ForceArrowGenerator(system,contactForces));
    MyReporter& myRep = *new MyReporter(system,contactForces,ReportInterval);

    system.addEventReporter(&myRep);
    system.addEventReporter(&reporter);

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

    viz.report(state);
    printf("Reference state -- hit ENTER\n");
    cout << "t=" << state.getTime() 
         << " q=" << patella.getQAsVector(state)  
         << " u=" << patella.getUAsVector(state)  
         << endl;
    char c=getchar();

    patella.setQToFitTransform(state, ~X_FP);
    viz.report(state);
    printf("Initial state -- hit ENTER\n");
    cout << "t=" << state.getTime() 
         << " q=" << patella.getQAsVector(state)  
         << " u=" << patella.getUAsVector(state)  
         << endl;
    c=getchar();
    
    // Simulate it.
    const clock_t start = clock();

    RungeKutta3Integrator integ(system);
    TimeStepper ts(system, integ);
    ts.initialize(state);
    ts.stepTo(2.0);

    const double timeInSec = (double)(clock()-start)/CLOCKS_PER_SEC;
    const int evals = integ.getNumRealizations();
    cout << "Done -- took " << integ.getNumStepsTaken() << " steps in " <<
        timeInSec << "s for " << ts.getTime() << "s sim (avg step=" 
        << (1000*ts.getTime())/integ.getNumStepsTaken() << "ms) " 
        << (1000*ts.getTime())/evals << "ms/eval\n";

    printf("Using Integrator %s at accuracy %g:\n", 
        integ.getMethodName(), integ.getAccuracyInUse());
    printf("# STEPS/ATTEMPTS = %d/%d\n", integ.getNumStepsTaken(), integ.getNumStepsAttempted());
    printf("# ERR TEST FAILS = %d\n", integ.getNumErrorTestFailures());
    printf("# REALIZE/PROJECT = %d/%d\n", integ.getNumRealizations(), integ.getNumProjections());


    while(true) {
        for (int i=0; i < (int)saveEm.size(); ++i) {
            viz.report(saveEm[i]);
        }
        getchar();
    }

  } catch (const std::exception& e) {
    std::printf("EXCEPTION THROWN: %s\n", e.what());
    exit(1);

  } catch (...) {
    std::printf("UNKNOWN EXCEPTION THROWN\n");
    exit(1);
  }

    return 0;
}

//.........这里部分代码省略.........
           (DecorativePoint(ctr).setColor(Magenta).setScale(3));
        brickBody.addDecoration
           (Transform(Rotation(n,ZAxis,Vec3(0,1,0),YAxis),ctr),
            DecorativeText(String(i)).setColor(Magenta)
             .setScale(.75).setFaceCamera(false));
        brickBody.addDecoration
           (DecorativeLine(ctr, ctr + 1.*n).setColor(Magenta));
    }

    MobilizedBody::Free brick(matter.Ground(), Transform(Vec3(0,3,0)),
        brickBody, Transform(Vec3(0)));

    Visualizer viz(system);
    viz.addDecorationGenerator(new ForceArrowGenerator(system,contactForces));
    viz.setShowShadows(true);
    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.

//.........这里部分代码省略.........
    //                                       platform, -phdim/2, 0.5, false);
    //ball2.setPlaneDisplayHalfWidth(5);
    //balls.push_back(ball2);

    //Constraint::SphereOnPlaneContact ball3(brick, 
    //                                       Transform(Vec3(0,0,-hdim[2])),
    //                                       platform, Vec3(-2,3,-.5), .7, true);
    //ball3.setPlaneDisplayHalfWidth(5);
    //balls.push_back(ball3);


    //MobilizedBody::Free ball(matter.Ground(), Vec3(0),
    //                         MassProperties(1,Vec3(0),UnitInertia(1,1,1)),
    //                         Vec3(0));
    //Constraint::SphereOnSphereContact ss(platform, Vec3(-2,1,-.5), .7,
    //                                     ball, Vec3(0), 1.2, true);                                         
    //Constraint::SphereOnSphereContact bb(brick, hdim, 0.5,
    //                                     ball, Vec3(0), 1.2, true);
    //sphsph.push_back(bb);

    Constraint::SphereOnSphereContact ss(brick, hdim, 0.5,
                                         platform, Vec3(-3,1,-.5), 1.2, 
                                         false);
    sphsph.push_back(ss);

    //Constraint::SphereOnSphereContact ss(platform, Vec3(-2,3,-.5), .7, 
    //                                     brick, hdim, 0.5, false);
    //Constraint::SphereOnSphereContact ss(platform, Vec3(-2,3,-.5), .7, 
    //                                     brick, hdim, 0.5, false);
    //Constraint::SphereOnSphereContact ss(brick, hdim, 0.5, 
    //                                     matter.Ground(), Vec3(-2,3,-.5), .7,true);
    Constraint::Rod rod1(brick, Vec3(0,hdim[1],hdim[2]), 
                         platform, Vec3(0,3,-.5), 1.5*1.2);
    

    // Spring to keep the brick near 000.
    //Force::TwoPointLinearSpring(forces, platform, Vec3(0),
    //                           brick, Vec3(0), 4, 1);

    // Rod to keep the brick near 000.
    //Constraint::Rod rod1(platform, Vec3(0,0,2),
    //                     brick, -hdim, 3);
    //rods.push_back(rod1);

    // Try edge/edge contact.
    Constraint::LineOnLineContact ll(platform, 
          Transform(Rotation(UnitVec3(1,1,1), XAxis, UnitVec3(-XAxis), ZAxis), 
                    Vec3(1,1,1)),
          2, // hlen
                                     brick, 
          Transform(Rotation(UnitVec3(ZAxis), XAxis, Vec3(-1,-1,0), ZAxis),
                    Vec3(-hdim[0],-hdim[1],0)),
          2, // hlen
          true);

    // Set up visualization at 30 fps.
    Visualizer viz(system);
    viz.setBackgroundType(Visualizer::SolidColor);
    viz.setShowFrameRate(true);
    system.addEventReporter(new Visualizer::Reporter(viz, 1./30));

    // Initialize the system and acquire default state.
    State state = system.realizeTopology();
    brick.setQToFitTransform(state, Vec3(0,5,0));
    brick.setUToFitAngularVelocity(state, Vec3(10,10,10));

    //rod1.setRodLength(state, 5);

    viz.report(state); 

    printf("Initial config. Ready to assemble.\n"); getchar();
    Assembler asmb(system);
    asmb.assemble(state);

    viz.report(state);
    printf("Assembled. Ready to initialize.\n"); getchar();

    //printf("Changed ball3 from rad=%g to rad=%g\n",
    //       ball3.getSphereRadius(state), 1.5);
    //ball3.setSphereRadius(state, 1.5);
    //viz.report(state); getchar();
    //asmb.assemble(state);

    //viz.report(state);
    //printf("Re-assembled. Ready to simulate.\n"); getchar();

    // Choose integrator and simulate for 10 seconds.
    RungeKuttaMersonIntegrator integ(system);
    //RungeKutta3Integrator integ(system);
    integ.setAccuracy(1e-8);
    //integ.setConstraintTolerance(1e-3);
    TimeStepper ts(system, integ);
    ts.initialize(state);
    viz.report(ts.getState());
    printf("Initialized. Ready to simulate.\n"); getchar();
    viz.addDecorationGenerator(new ShowEnergy(system,brick,balls,sphsph,rods));
    ts.stepTo(100.0);
    printf("# steps=%d/%d\n", 
           integ.getNumStepsTaken(), integ.getNumStepsAttempted());
}

//==============================================================================
//                                  MAIN
//==============================================================================
int main() {
    MultibodySystem system;
    SimbodyMatterSubsystem matter(system);
	matter.setShowDefaultGeometry(false);
    GeneralForceSubsystem forces(system);
    Force::Gravity gravity(forces, matter, -YAxis, 9.80665);
    Visualizer viz(system);

    MobilizedBody& Ground = matter.updGround(); // short name for Ground
	
    // Calculate the mass properties for a half-ellipsoid. a1,b1,c1 are the
    // radii (semi-axis lengths) of the full ellipsoid in x,y,z resp.
    // Body origin is still center of full ellipsoid (0,0,0) but the COM moves 
    // lower in y which affects the xx and zz inertias. Note that Simbody
    // requires the inertias to be given about the body origin, *not* COM.
    // Unit inertia about the body origin is the same as for a full ellipsoid,
    // but will be weighted by half as much mass.
    // TODO: is this right?
	Real m1 = 1.0, a1 = 0.25, b1 = 0.083333333333333, c1 = 0.083333333333333;
    Real comShiftY = (3./8.)*b1; // Because it's a half ellipsoid.
    Body::Rigid halfEllipsoid(MassProperties(m1, Vec3(0, -comShiftY, 0), 
                                      UnitInertia::ellipsoid(Vec3(a1,b1,c1))));
    // Add some artwork -- don't have a half-ellipsoid unfortunately.
	halfEllipsoid.addDecoration(Transform(), 
        DecorativeEllipsoid(Vec3(a1,b1,c1)).setColor(Red).setResolution(10));

    // Now define a rectangular solid that we'll weld to the rattleback to
    // give it asymmetrical mass properties.
	Real m2 = 2.0, a2 = 2.0*a1, b2 = 0.02, c2 = 0.05;
    const Vec3 barHalfDims = Vec3(a2,b2,c2)/2;
    Body::Rigid barBody(MassProperties(m2, Vec3(0), 
                                       UnitInertia::brick(barHalfDims)));
    barBody.addDecoration(Transform(), DecorativeBrick(barHalfDims)
                                                .setColor(Blue).setOpacity(1.));

    
    // Create a massless x-z base to provide the two slipping dofs.
    MobilizedBody::Slider xdir(Ground, Transform(),
                               Body::Massless(), Transform());
    const Rotation x2z(-Pi/2, YAxis); // rotate so +x moves to +z
    MobilizedBody::Slider base(xdir, x2z, Body::Massless(), x2z);
	base.addBodyDecoration(Transform(), DecorativeBrick(Vec3(0.25, 0.001, 0.25))
                                            .setColor(Orange).setOpacity(0.50));


	// Use a reverse mobilizer so that the contact point remains in a fixed
    // location of the base body.
	MobilizedBody::Ellipsoid rattle(base,          Rotation(Pi/2, XAxis),
                                    halfEllipsoid, Rotation(Pi/2, XAxis),
                                    Vec3(a1,b1,c1), // ellipsoid half-radii
                                    MobilizedBody::Reverse);

	// Weld the bar to the ellipsoid at a 45 degree angle to produce lopsided
    // inertia properties.
    MobilizedBody::Weld bar(rattle, Transform(Rotation(45*Pi/180, YAxis), 
                                              Vec3(0,-b2/2.1,0)), 
                            barBody, Transform());
	
	// Finally, the rattle cannot just slide on the surface of the ground, it 
    // must roll.
    #ifdef USE_BAD_CONSTRAINTS
    // TODO: (sherm 20130620) these are the wrong constraints because they
    // ignore the acceleration term caused by the contact point moving on the
    // ellipsoid's surface. The correct constraint has to cognizant of the
    // ellipsoid geometry at the contact point. Use of these constraints fails
    // to conserve energy.
    
    viz.addDecoration(Ground, Vec3(0), 
        DecorativeText("TODO: BROKEN -- USING INVALID NOSLIP CONSTRAINTS")
        .setIsScreenText(true));
	Constraint::NoSlip1D contactPointXdir(base, Vec3(0), UnitVec3(1,0,0), 
                                          matter.updGround(), rattle);
	Constraint::NoSlip1D contactPointZdir(base, Vec3(0), UnitVec3(0,0,1), 
                                          matter.updGround(), rattle);
    #endif

    // Draw a cute green box to rattle around in.
    Ground.addBodyDecoration(Vec3(0,  0*1e-5, 0), // floor
        DecorativeBrick(Vec3(.5, .00001, .5)).setColor(Green).setOpacity(.1));
    Ground.addBodyDecoration(Vec3(0.5, 0.25, 0),  // right wall
        DecorativeBrick(Vec3(1e-5, .25, .5)).setColor(Green).setOpacity(.25));
    Ground.addBodyDecoration(Vec3(-.5, .25, 0),   // left
        DecorativeBrick(Vec3(1e-5, .25, .5)).setColor(Green).setOpacity(.25));
    Ground.addBodyDecoration(Vec3(0, .25, -.5),   // back
        DecorativeBrick(Vec3(.5, .25, 1e-5)).setColor(Green).setOpacity(.25));
    Ground.addBodyDecoration(Vec3(0, .25, .5),    // front
        DecorativeBrick(Vec3(.5, .25, 1e-5)).setColor(Green).setOpacity(.1));
	
	// Output a visualization frame every 1/30 of a second, and output 
    // energy information every 1/4 second.
	system.addEventReporter(new Visualizer::Reporter(viz, 1./30));
	system.addEventReporter(new EnergyReporter(system, rattle, 1./4));
    
	// We're done building the system. Create it and obtain a copy of the
    // default state.
	State state = system.realizeTopology();

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

//.........这里部分代码省略.........
        Vec3(0),
        DecorativeLine().setColor(Vec3(0,0,0)).setLineThickness(6));

    PseudorotationMobilizer c1Body( 
        c2Body, 
        Rotation( angle_t(-80*degrees), YAxis ),
        rigidBody,
        Transform(location_t(Vec3(0,0,1.5)*angstroms)),
        angle_t(37.6*degrees), // amplitude
        angle_t(52.8*degrees) // phase
        );
    // sphere for C1 atom
    decorations.addBodyFixedDecoration(
        c1Body.getMobilizedBodyIndex(), 
        Transform(),
        DecorativeSphere( length_t(0.5*angstroms) )
    );
    // sphere for N1 atom
    decorations.addBodyFixedDecoration(
        c1Body.getMobilizedBodyIndex(), 
        location_t(Vec3(-1.0,-1.0,-0.5)*angstroms),
        DecorativeSphere( length_t(0.5*angstroms) ).setColor(Vec3(0,0,1))
    );
    // sphere for O4 atom
    decorations.addBodyFixedDecoration(
        c1Body.getMobilizedBodyIndex(), 
        location_t(Vec3(1.0,0,-0.5)*angstroms),
        DecorativeSphere( length_t(0.5*angstroms) ).setColor(Vec3(1,0,0))
    );

    decorations.addRubberBandLine(
        c2Body.getMobilizedBodyIndex(),
        Vec3(0),
        c1Body.getMobilizedBodyIndex(),
        Vec3(0),
        DecorativeLine().setColor(Vec3(0,0,0)).setLineThickness(6));
    decorations.addRubberBandLine(
        c1Body.getMobilizedBodyIndex(),
        Vec3(0),
        c1Body.getMobilizedBodyIndex(),
        location_t(Vec3(1.0,0,-0.5)*angstroms),
        DecorativeLine().setColor(Vec3(0,0,0)).setLineThickness(6));
    decorations.addRubberBandLine(
        c1Body.getMobilizedBodyIndex(),
        Vec3(0),
        c1Body.getMobilizedBodyIndex(),
        location_t(Vec3(-1.0,-1.0,-0.5)*angstroms),
        DecorativeLine().setColor(Vec3(0,0,0)).setLineThickness(6));
    decorations.addRubberBandLine(
        c4Body.getMobilizedBodyIndex(),
        Vec3(0),
        c1Body.getMobilizedBodyIndex(),
        location_t(Vec3(1.0,0,-0.5)*angstroms),
        DecorativeLine().setColor(Vec3(0,0,0)).setLineThickness(6));

    // Prescribed motion
    Constraint::ConstantSpeed(c3Body, 0.5);

    // Two constraint way works; one constraint way does not
    bool useTwoConstraints = true;

    if (useTwoConstraints) {
        // Constraints to make three generalized coordinates identical
        std::vector<MobilizedBodyIndex> c32bodies(2);
        c32bodies[0] = c3Body.getMobilizedBodyIndex();
        c32bodies[1] = c2Body.getMobilizedBodyIndex();
        std::vector<MobilizerQIndex> coordinates(2, MobilizerQIndex(0));
        Constraint::CoordinateCoupler(matter, new DifferenceFunction, c32bodies, coordinates);

        std::vector<MobilizedBodyIndex> c21bodies(2);
        c21bodies[0] = c2Body.getMobilizedBodyIndex();
        c21bodies[1] = c1Body.getMobilizedBodyIndex();
        Constraint::CoordinateCoupler(matter, new DifferenceFunction, c21bodies, coordinates);
    }
    else { // trying to get single constraint way to work
        // Try one constraint for all three mobilizers
        std::vector<MobilizedBodyIndex> c123Bodies(3);
        c123Bodies[0] = c1Body.getMobilizedBodyIndex();
        c123Bodies[1] = c2Body.getMobilizedBodyIndex();
        c123Bodies[2] = c3Body.getMobilizedBodyIndex();
        std::vector<MobilizerQIndex> coords3(3, MobilizerQIndex(0));
        Constraint::CoordinateCoupler(matter, new ThreeDifferencesFunction, c123Bodies, coords3);
    }


    Visualizer viz(system);
    viz.setBackgroundType(Visualizer::SolidColor);

    system.addEventReporter(new Visualizer::Reporter(viz, 0.10));

    system.realizeTopology();
    State& state = system.updDefaultState();
    
    // Simulate it.
    VerletIntegrator integ(system);
    //RungeKuttaMersonIntegrator integ(system);
    TimeStepper ts(system, integ);
    ts.initialize(state);
    ts.stepTo(50.0);
}

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