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";
}
}
