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; }
int main() { try { // Create the system. MultibodySystem system; system.setUpDirection(ZAxis); SimbodyMatterSubsystem matter(system); GeneralForceSubsystem forces(system); Force::Gravity gravity(forces, matter, -ZAxis, 9.81); ContactTrackerSubsystem tracker(system); CompliantContactSubsystem contactForces(system, tracker); contactForces.setTrackDissipatedEnergy(true); contactForces.setTransitionVelocity(1e-3); const Vec3 hdim(.2,.3,.4); // Brick half dimensions const Real rad = .1; // Contact sphere radius const Real brickMass = 2; #ifdef USE_SHERM_PARAMETERS const Real mu_d =.3; // dynamic friction const Real mu_s =.3; // static friction const Real mu_v = 0; // viscous friction (1/v) const Real dissipation = .1; const Real fK = 1e6; // stiffness in pascals const Real simDuration = 5.; #endif #ifdef USE_TOM_PARAMETERS const Real mu_d =.3; // dynamic friction const Real mu_s =.3; // static friction const Real mu_v = 0; // viscous friction (1/v) const Real dissipation = .1756; //Second impact at 0.685 s. const Real fK = 1e6; // stiffness in pascals const Real simDuration = 0.5; //3.0; //0.8; #endif const ContactMaterial material(fK,dissipation,mu_s,mu_d,mu_v); // Halfspace floor const Rotation R_xdown(Pi/2,YAxis); matter.Ground().updBody().addContactSurface( Transform(R_xdown, Vec3(0,0,0)), ContactSurface(ContactGeometry::HalfSpace(), material)); Body::Rigid brickBody(MassProperties(brickMass, Vec3(0), UnitInertia::brick(hdim))); brickBody.addDecoration(Transform(), DecorativeBrick(hdim).setColor(BrickColor).setOpacity(.7)); for (int i=-1; i<=1; i+=2) for (int j=-1; j<=1; j+=2) for (int k=-1; k<=1; k+=2) { const Vec3 pt = Vec3(i,j,k).elementwiseMultiply(hdim); brickBody.addContactSurface(pt, ContactSurface(ContactGeometry::Sphere(rad), material)); brickBody.addDecoration(pt, DecorativeSphere(rad).setColor(SphereColor)); } MobilizedBody::Free brick(matter.Ground(), Transform(), brickBody, Transform()); Visualizer viz(system); viz.addDecorationGenerator(new ForceArrowGenerator(system,contactForces, brick)); //viz.addFrameController(new BodyWatcher(brick)); viz.addFrameController(new BodyWatcher(matter.Ground())); //viz.setShowSimTime(true); //viz.setShowFrameNumber(true); viz.setDesiredFrameRate(FrameRate); //viz.setShowFrameRate(true); 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); // 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); //integ.setMaximumStepSize(1e-0001); //integ.setAccuracy(1e-3); // minimum for CPodes integ.setAccuracy(1e-5); //integ.setAccuracy(.01); integ.initialize(state); double cpuStart = cpuTime(); double realStart = realTime(); Real lastReport = -Infinity; while (integ.getTime() < simDuration) { // Advance time by no more than ReportInterval. Might require multiple // internal steps. integ.stepBy(ReportInterval); if (integ.getTime() >= lastReport + VizReportInterval) { // The state being used by the integrator. const State& s = integ.getState(); viz.report(s); saveEm.push_back(s); // save state for playback lastReport = s.getTime(); } } const double timeInSec = realTime() - realStart; const int evals = integ.getNumRealizations(); cout << "Done -- took " << integ.getNumStepsTaken() << " steps in " << timeInSec << "s elapsed for " << integ.getTime() << "s sim (avg step=" << (1000*integ.getTime())/integ.getNumStepsTaken() << "ms) " << (1000*integ.getTime())/evals << "ms/eval\n"; cout << " CPU time was " << cpuTime() - cpuStart << "s\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()); viz.dumpStats(std::cout); // Add as slider to control playback speed. viz.addSlider("Speed", 1, 0, 2, 1); viz.setMode(Visualizer::PassThrough); silo->clear(); // forget earlier input double speed = 1; // will change if slider moves while(true) { cout << "Choose Run/Replay to see that again ...\n"; int menuId, item; silo->waitForMenuPick(menuId, item); if (menuId != RunMenuId) { cout << "\aUse the Run menu!\n"; continue; } if (item == QuitItem) break; if (item != ReplayItem) { cout << "\aHuh? Try again.\n"; continue; } for (double i=0; i < (int)saveEm.size(); i += speed ) { int slider; Real newValue; if (silo->takeSliderMove(slider,newValue)) { speed = newValue; } viz.report(saveEm[(int)i]); } } } 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; }
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; }
int main() { try { // Create the system. MultibodySystem system; SimbodyMatterSubsystem matter(system); GeneralForceSubsystem forces(system); Force::Gravity gravity(forces, matter, UnitVec3(.1,-1,0), 9.81); ContactTrackerSubsystem tracker(system); CompliantContactSubsystem contactForces(system, tracker); contactForces.setTrackDissipatedEnergy(true); contactForces.setTransitionVelocity(1e-3); const Vec3 hdim(1,2,3); const Real fFac =.15; // to turn off friction const Real fDis = .5; // to turn off dissipation const Real fVis = .1; // to turn off viscous friction const Real fK = .1*1e6; // pascals // Halfspace floor const Rotation R_xdown(-Pi/2,ZAxis); matter.Ground().updBody().addDecoration( Transform(Vec3(0,-.5,0)), DecorativeBrick(Vec3(10,.5,20)).setColor(Green).setOpacity(.1)); matter.Ground().updBody().addContactSurface( Transform(R_xdown, Vec3(0,0,0)), ContactSurface(ContactGeometry::HalfSpace(), ContactMaterial(fK*.1,fDis*.9, fFac*.8,fFac*.7,fVis*10))); const Real brickMass = 10; Body::Rigid brickBody(MassProperties(brickMass, Vec3(0), UnitInertia::brick(hdim))); brickBody.addDecoration(Transform(), DecorativeBrick(hdim).setColor(Cyan).setOpacity(.3)); const int surfx = brickBody.addContactSurface(Transform(), ContactSurface(ContactGeometry::Brick(hdim), ContactMaterial(fK,fDis, fFac*.8,fFac*.7,fVis)) ); //brickBody.addContactSurface(Transform(), // ContactSurface(ContactGeometry::Ellipsoid(hdim), // ContactMaterial(fK*.1,fDis*.9, // .1*fFac*.8,.1*fFac*.7,fVis*1)) // ); const ContactSurface& surf = brickBody.getContactSurface(surfx); const ContactGeometry& cg = surf.getShape(); const ContactGeometry::Brick& cgbrick = ContactGeometry::Brick::getAs(cg); cout << "cgbrick.hdim=" << cgbrick.getHalfLengths() << endl; const Geo::Box& box = cgbrick.getGeoBox(); cout << "box.hdim=" << box.getHalfLengths() << endl; // Vertices for (int i=0; i<8; ++i) { const Vec3 vpos = box.getVertexPos(i); const UnitVec3 vn = box.getVertexNormal(i); brickBody.addDecoration (DecorativePoint(vpos).setColor(Orange)); brickBody.addDecoration (DecorativeText(String(i)).setTransform(vpos).setColor(White) .setScale(.5)); brickBody.addDecoration (DecorativeLine(vpos, vpos + 0.5*vn).setColor(Orange)); printf("vertex %d:\n", i); int e[3],ew[3],f[3],fw[3]; box.getVertexEdges(i,e,ew); box.getVertexFaces(i,f,fw); for (int ex=0; ex<3; ++ex) { int ev[2]; box.getEdgeVertices(e[ex], ev); printf(" e%2d(%d) ev=%d\n", e[ex], ew[ex], ev[ew[ex]]); } for (int fx=0; fx<3; ++fx) { int fv[4]; box.getFaceVertices(f[fx], fv); printf(" f%2d(%d) fv=%d\n", f[fx], fw[fx], fv[fw[fx]]); } } // Edges for (int i=0; i<12; ++i) { const UnitVec3 n = box.getEdgeNormal(i); const UnitVec3 d = box.getEdgeDirection(i); const Vec3 ctr = box.getEdgeCenter(i); const Real len = .75; brickBody.addDecoration (DecorativePoint(ctr).setColor(Green).setScale(2)); brickBody.addDecoration (DecorativeText(String(i)).setTransform(ctr+len*n) .setColor(Green).setScale(.3)); brickBody.addDecoration (DecorativeLine(ctr, ctr + len*n).setColor(Green)); brickBody.addDecoration (DecorativeLine(ctr, ctr + len*d).setColor(Green)); printf("edge %d:\n", i); int f[2],fw[2]; box.getEdgeFaces(i,f,fw); for (int fx=0; fx<2; ++fx) { int fe[4]; box.getFaceEdges(f[fx], fe); printf(" f%2d(%d) fe=%d\n", f[fx], fw[fx], fe[fw[fx]]); } } // Faces for (int i=0; i<6; ++i) { int vertices[4]; box.getFaceVertices(i,vertices); const UnitVec3 n = box.getFaceNormal(i); const Vec3 ctr = box.getFaceCenter(i); brickBody.addDecoration (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. // 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 integ.setAccuracy(1e-5); //integ.setAccuracy(.01); TimeStepper ts(system, integ); ts.initialize(state); double cpuStart = cpuTime(); double realStart = realTime(); ts.stepTo(20.0); const double timeInSec = realTime() - realStart; const int evals = integ.getNumRealizations(); cout << "Done -- took " << integ.getNumStepsTaken() << " steps in " << timeInSec << "s elapsed for " << ts.getTime() << "s sim (avg step=" << (1000*ts.getTime())/integ.getNumStepsTaken() << "ms) " << (1000*ts.getTime())/evals << "ms/eval\n"; cout << " CPU time was " << cpuTime() - cpuStart << "s\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()); viz.dumpStats(std::cout); // Add as slider to control playback speed. viz.addSlider("Speed", 1, 0, 4, 1); viz.setMode(Visualizer::PassThrough); silo->clear(); // forget earlier input double speed = 1; // will change if slider moves while(true) { cout << "Choose Run/Replay to see that again ...\n"; int menuId, item; silo->waitForMenuPick(menuId, item); if (menuId != RunMenuId) { cout << "\aUse the Run menu!\n"; continue; } if (item == QuitItem) break; if (item != ReplayItem) { cout << "\aHuh? Try again.\n"; continue; } for (double i=0; i < (int)saveEm.size(); i += speed ) { int slider; Real newValue; if (silo->takeSliderMove(slider,newValue)) { speed = newValue; } viz.report(saveEm[(int)i]); } } } 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; }
int main() { try { // Create the system. MultibodySystem system; SimbodyMatterSubsystem matter(system); matter.setShowDefaultGeometry(false); CableTrackerSubsystem cables(system); GeneralForceSubsystem forces(system); Force::Gravity gravity(forces, matter, -YAxis, 9.81); // Force::GlobalDamper(forces, matter, 5); system.setUseUniformBackground(true); // no ground plane in display MobilizedBody Ground = matter.Ground(); // convenient abbreviation // Read in some bones. PolygonalMesh femur; PolygonalMesh tibia; femur.loadVtpFile("CableOverBicubicSurfaces-femur.vtp"); tibia.loadVtpFile("CableOverBicubicSurfaces-tibia.vtp"); femur.scaleMesh(30); tibia.scaleMesh(30); // Build a pendulum Body::Rigid pendulumBodyFemur( MassProperties(1.0, Vec3(0, -5, 0), UnitInertia(1).shiftFromCentroid(Vec3(0, 5, 0)))); pendulumBodyFemur.addDecoration(Transform(), DecorativeMesh(femur).setColor(Vec3(0.8, 0.8, 0.8))); Body::Rigid pendulumBodyTibia( MassProperties(1.0, Vec3(0, -5, 0), UnitInertia(1).shiftFromCentroid(Vec3(0, 5, 0)))); pendulumBodyTibia.addDecoration(Transform(), DecorativeMesh(tibia).setColor(Vec3(0.8, 0.8, 0.8))); Rotation z180(Pi, YAxis); MobilizedBody::Pin pendulumFemur( matter.updGround(), Transform(Vec3(0, 0, 0)), pendulumBodyFemur, Transform(Vec3(0, 0, 0)) ); Rotation rotZ45(-Pi/4, ZAxis); MobilizedBody::Pin pendulumTibia( pendulumFemur, Transform(rotZ45, Vec3(0, -12, 0)), pendulumBodyTibia, Transform(Vec3(0, 0, 0)) ); Real initialPendulumOffset = -0.25*Pi; Constraint::PrescribedMotion pres(matter, new Function::Sinusoid(0.25*Pi, 0.2*Pi, 0*initialPendulumOffset), pendulumTibia, MobilizerQIndex(0)); // Build a wrapping cable path CablePath path2(cables, Ground, Vec3(1, 3, 1), // origin pendulumTibia, Vec3(1, -4, 0)); // termination // Create a bicubic surface Vec3 patchOffset(0, -5, -1); Rotation rotZ90(0.5*Pi, ZAxis); Rotation rotX90(0.2*Pi, XAxis); Rotation patchRotation = rotZ90 * rotX90 * rotZ90; Transform patchTransform(patchRotation, patchOffset); Real patchScaleX = 2.0; Real patchScaleY = 2.0; Real patchScaleF = 0.75; const int Nx = 4, Ny = 4; const Real xData[Nx] = { -2, -1, 1, 2 }; const Real yData[Ny] = { -2, -1, 1, 2 }; const Real fData[Nx*Ny] = { 2, 3, 3, 1, 0, 1.5, 1.5, 0, 0, 1.5, 1.5, 0, 2, 3, 3, 1 }; const Vector x_(Nx, xData); const Vector y_(Ny, yData); const Matrix f_(Nx, Ny, fData); Vector x = patchScaleX*x_; 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"; } }