void ContactGeometry::TriangleMesh::Impl::splitObbAxis
(const Array_<int>& parentIndices, Array_<int>& child1Indices,
Array_<int>& child2Indices, int axis)
{ // For each face, find its minimum and maximum extent along the axis.
Vector minExtent(parentIndices.size());
Vector maxExtent(parentIndices.size());
for (int i = 0; i < (int) parentIndices.size(); i++) {
int* vertexIndices = faces[parentIndices[i]].vertices;
Real minVal = vertices[vertexIndices[0]].pos[axis];
Real maxVal = vertices[vertexIndices[0]].pos[axis];
minVal = std::min(minVal, vertices[vertexIndices[1]].pos[axis]);
maxVal = std::max(maxVal, vertices[vertexIndices[1]].pos[axis]);
minExtent[i] = std::min(minVal, vertices[vertexIndices[2]].pos[axis]);
maxExtent[i] = std::max(maxVal, vertices[vertexIndices[2]].pos[axis]);
}
// Select a split point that tries to put as many faces as possible
// entirely on one side or the other.
Real split = (median(minExtent)+median(maxExtent)) / 2;
// Choose a side for each face.
for (int i = 0; i < (int) parentIndices.size(); i++) {
if (maxExtent[i] <= split)
child1Indices.push_back(parentIndices[i]);
else if (minExtent[i] >= split)
child2Indices.push_back(parentIndices[i]);
else if (0.5*(minExtent[i]+maxExtent[i]) <= split)
child1Indices.push_back(parentIndices[i]);
else
child2Indices.push_back(parentIndices[i]);
}
}
Real CylinderImplicitFunction::
calcDerivative(const Array_<int>& derivComponents, const Vector& x) const {
if (derivComponents.size() == 1 && derivComponents[0] < 2)
return -2*x[derivComponents[0]]/square(ownerp->getRadius());
if (derivComponents.size() == 2 &&
derivComponents[0] == derivComponents[1] &&
derivComponents[0] < 2 )
return -2/square(ownerp->getRadius());
return 0;
}
Real ObservedPointFitter::findBestFit
(const MultibodySystem& system, State& state,
const Array_<MobilizedBodyIndex>& bodyIxs,
const Array_<Array_<Vec3> >& stations,
const Array_<Array_<Vec3> >& targetLocations,
Real tolerance)
{
Array_<Array_<Real> > weights(stations.size());
for (int i = 0; i < (int)stations.size(); ++i)
for (int j = 0; j < (int)stations[i].size(); ++j)
weights[i].push_back(1.0);
return findBestFit(system, state, bodyIxs, stations, targetLocations, weights, tolerance);
}
Real EllipsoidImplicitFunction::
calcDerivative(const Array_<int>& derivComponents, const Vector& x) const {
const Vec3& radii = ownerp->getRadii();
if (derivComponents.size() == 1) {
int c = derivComponents[0];
return -2*x[c]/(radii[c]*radii[c]);
}
if ( derivComponents.size() == 2
&& derivComponents[0] == derivComponents[1]) {
int c = derivComponents[0];
return -2/(radii[c]*radii[c]);
}
// A mixed second derivative, or any higher derivative is zero.
return 0;
}
int ObservedPointFitter::findBodiesForClonedSystem(MobilizedBodyIndex primaryBodyIx, const Array_<int> numStations, const SimbodyMatterSubsystem& matter, const Array_<Array_<MobilizedBodyIndex> > children, Array_<MobilizedBodyIndex>& bodyIxs) {
findUpstreamBodies(primaryBodyIx, numStations, matter, bodyIxs, 5);
int primaryBodyIndex = bodyIxs.size();
int requiredStations = 5;
findDownstreamBodies(primaryBodyIx, numStations, children, bodyIxs, requiredStations);
return primaryBodyIndex;
}
void PerSubsystemInfo::
popAllocationStackBackToStage(Array_<T>& stack, const Stage& g) {
unsigned newSize = stack.size();
while (newSize > 0 && stack[newSize-1].getAllocationStage() > g)
stack[--newSize].deepDestruct(*m_stateImpl);
stack.resize(newSize);
}
Real calcDerivative(const Array_<int>& derivComponents, const Vector& x) const
{
Real deriv = 0;
assert(3 == x.size());
int derivOrder = derivComponents.size();
assert(1 <= derivOrder);
if (derivOrder == 1)
{
// too clever
// df/dx
// = 2(x-y) + 2(x-z)
// = 2x - 2y + 2x - 2z
// = 4x - 2y - 2z
// = 6x - 2x - 2y - 2z
deriv = 2 * (3 * x[derivComponents[0]] -x[0] -x[1] -x[2]);
}
else if (derivOrder == 2)
{
if (derivComponents[0] == derivComponents[1])
deriv = 4.0; // df/dx^2
else
deriv = -2.0; // df/dxdy
}
else ; // all derivatives higher than two are zero
return deriv;
}
Real calcDerivative(const Array_<int>& derivComponents, const Vector& x) const{
Real deriv = 0;
assert(1 == x.size());
// Derivatives repeat after 4
int derivOrder = derivComponents.size() % 4;
// Derivatives 1, 5, 9, 13, ... are cos()
if ( 1 == derivOrder ) {
deriv = angle_t(amplitude*cos(x[0]*radians - phase));
}
// Derivatives 2, 6, 10, 14, ... are -sin()
else if ( 2 == derivOrder ) {
deriv = angle_t(-amplitude*sin(x[0]*radians - phase));
}
// Derivatives 3, 7, 11, 15, ... are -cos()
else if ( 3 == derivOrder ) {
deriv = angle_t(-amplitude*cos(x[0]*radians - phase));
}
// Derivatives 0, 4, 8, 12, ... are sin()
else if ( 0 == derivOrder ) {
deriv = angle_t(amplitude*sin(x[0]*radians - phase));
}
else assert(false);
return deriv;
}
Real TorusImplicitFunction::
calcDerivative(const Array_<int>& derivComponents, const Vector& x) const {
// first derivatives
if (derivComponents.size() == 1) {
if (derivComponents[0]<2) {
Real sqrt_xy = std::sqrt(x[0]*x[0] + x[1]*x[1]);
return 2*x[derivComponents[0]]*(ownerp->getTorusRadius() - sqrt_xy)/
(square(ownerp->getTubeRadius())*sqrt_xy);
}
else
return -2*x[2]/square(ownerp->getTubeRadius());
}
// second derivatives
if (derivComponents.size() == 2) {
if (derivComponents[0] < 2) { // fx_ fy_
if (derivComponents[1] < 2) {
Real tubeRadiusSq = square(ownerp->getTubeRadius());
Real xy = x[0]*x[0] + x[1]*x[1];
Real sqrt_xy = std::sqrt(xy);
Real den = tubeRadiusSq*xy*sqrt_xy;
if (derivComponents[0]==derivComponents[1]) { // fxx or fyy
int idx = derivComponents[1]==0; // if 0 then idx=1, if 1 then idx=0
Real num = 2*ownerp->getTorusRadius()*x[idx]*x[idx];
return num/den - 2/tubeRadiusSq;
}
else { // fxy or fyx
return - 2*ownerp->getTorusRadius()*x[0]*x[1]/den;
}
}
else // fxz = fyz = 0
return 0;
}
else { // fz_
if (derivComponents[1] == 2) // fzz
return -2/square(ownerp->getTubeRadius());
else // fzx = fzy = 0
return 0;
}
}
//TODO higher order derivatives
SimTK_ASSERT1_ALWAYS(!"derivative not implemented",
"Implicit Torus implements 1st&2nd derivs only but %d deriv requested.",
derivComponents.size());
return 0;
}
NumJacobianFunc(Assembler& assembler,
const Array_<AssemblyConditionIndex>& numCons,
const Array_<int>& nErrorEqns,
int totalNEqns)
: Differentiator::JacobianFunction
(totalNEqns, assembler.getNumFreeQs()),
assembler(assembler), numCons(numCons), nEqns(nErrorEqns),
totalNEqns(totalNEqns)
{ assert(numCons.size() == nEqns.size()); }
void calcVelocityDotErrors
(const State& s,
const Array_<SpatialVec,ConstrainedBodyIndex>& A_AB,
const Array_<Real, ConstrainedUIndex>& constrainedUDot,
Array_<Real>& vaerr) const override
{
assert(vaerr.size() == 1);
vaerr[0] = getOneUDot(s, constrainedUDot,
theMobilizer, whichMobility);
}
void PerSubsystemInfo::copyAllocationStackThroughStage
(Array_<T>& stack, const Array_<T>& src, const Stage& g)
{
unsigned nVarsToCopy = src.size(); // assume we'll copy all
while (nVarsToCopy && src[nVarsToCopy-1].getAllocationStage() > g)
--nVarsToCopy;
resizeAllocationStack(stack, nVarsToCopy);
for (unsigned i=0; i < nVarsToCopy; ++i)
stack[i].deepAssign(src[i]);
}
// One non-holonomic (well, velocity-level) constraint equation.
// verr = u - s
// aerr = udot
//
void calcVelocityErrors
(const State& s,
const Array_<SpatialVec,ConstrainedBodyIndex>& V_AB,
const Array_<Real, ConstrainedUIndex>& constrainedU,
Array_<Real>& verr) const override
{
assert(verr.size() == 1);
verr[0] = getOneU(s, constrainedU, theMobilizer, whichMobility)
- prescribedSpeed;
}
// apply generalized force lambda to the mobility
void addInVelocityConstraintForcesVirtual
(const State& s,
const Array_<Real>& multipliers,
Array_<SpatialVec,ConstrainedBodyIndex>& bodyForcesInA,
Array_<Real,ConstrainedUIndex>& mobilityForces) const
{
assert(multipliers.size() == 1);
const Real lambda = multipliers[0];
addInOneMobilityForce(s, theMobilizer, whichMobility,
lambda, mobilityForces);
}
int PolygonalMesh::addFace(const Array_<int>& vertices) {
initializeHandleIfEmpty();
for (int i = 0; i < (int) vertices.size(); i++)
updImpl().faceVertexIndex.push_back(vertices[i]);
// faceVertexStart is preloaded to have its first element 0 before any
// faces have been added. So the back() element of faceVertexStart is
// already the starting entry for the face we're adding.
// This is where the *next* face will begin.
updImpl().faceVertexStart.push_back(getImpl().faceVertexIndex.size());
// The current face start is now at end()-2 (back()-1).
return getImpl().faceVertexStart.size()-2;
}
// We also rummage through the model to find fixed geometry that should be part
// of every frame. The supplied State must be realized through Instance stage.
void ModelVisualizer::collectFixedGeometry(const State& state) const {
// Collect any fixed geometry from the ModelComponents.
Array_<DecorativeGeometry> fixedGeometry;
_model.generateDecorations
(true, _model.getDisplayHints(), state, fixedGeometry);
for (unsigned i=0; i < fixedGeometry.size(); ++i) {
const DecorativeGeometry& dgeo = fixedGeometry[i];
//cout << dgeo.getBodyId() << dgeo.getTransform() << endl;
_viz->addDecoration(MobilizedBodyIndex(dgeo.getBodyId()),
Transform(), dgeo);
}
}
void testMoveConstructionAndAssignment() {
Array_<double> ad1{1,2,3.5,4};
const double* p1 = ad1.data();
Array_<double> ad2{.01,.02};
const double* p2 = ad2.data();
Array_<double> ad3(ad1); // copy construction
const double* p3 = ad3.data();
SimTK_TEST(p3 != p2);
ad3 = std::move(ad1); // move assignment
SimTK_TEST(ad3.data() == p1 && ad1.data() == p3);
Array_<double> ad4(std::move(ad2)); // move construction
SimTK_TEST(ad4.data()==p2 && ad2.empty());
auto returned = returnByValue(3.25); // construction
SimTK_TEST(returned.first == std::vector<double>({1,2,3,4,5.5,3.25}));
SimTK_TEST(returned.first.data() == returned.second);
returned = returnByValue(-1); // assignment
SimTK_TEST(returned.first == std::vector<double>({1,2,3,4,5.5,-1}));
SimTK_TEST(returned.first.data() == returned.second);
// std::unique_ptr has only move construction so this won't compile if
// Array_ requires copy construction
Array_<std::unique_ptr<double>> aud;
aud.push_back(std::unique_ptr<double>(new double(5.125)));
aud.push_back(std::unique_ptr<double>(new double(3.5)));
aud.push_back(std::unique_ptr<double>(new double(-2.25)));
SimTK_TEST(aud.size()==3);
SimTK_TEST(*aud[0]==5.125 && *aud[1]==3.5 && *aud[2]==-2.25);
aud.emplace_back(new double(123.));
SimTK_TEST(aud.size()==4 && *aud[3]==123.);
aud.emplace(&aud[2], new double(100));
SimTK_TEST(aud.size()==5 && *aud[2]==100. && *aud[3]==-2.25);
}
/** Same as above but for a given time series */
void InverseDynamicsSolver::solve(State &s, const FunctionSet &Qs, const Array_<double> ×, Array_<Vector> &genForceTrajectory)
{
int nq = getModel().getNumCoordinates();
int nt = times.size();
//Preallocate if not done already
genForceTrajectory.resize(nt, Vector(nq));
AnalysisSet& analysisSet = const_cast<AnalysisSet&>(getModel().getAnalysisSet());
//fill in results for each time
for(int i=0; i<nt; i++){
genForceTrajectory[i] = solve(s, Qs, times[i]);
analysisSet.step(s, i);
}
}
void Assembler::initialize() const {
if (alreadyInitialized)
return;
++nInitializations;
Array_<QIndex> toBeLocked;
reinitializeWithExtraQsLocked(toBeLocked);
alreadyInitialized = true;
return;
/*NOTREACHED*/
// TODO: This currently unused code would allow the Assembler to lock out
// variables that it thinks aren't worth bothering with. Needs real-world
// testing and probably some override options. And should there be a
// desperation mode where all variables are tried if we can't assemble
// with some of them removed?
Vector grad = abs(asmSys->calcCurrentGradient());
Real maxGrad = 0;
for (FreeQIndex fx(0); fx < grad.size(); ++fx)
maxGrad = std::max(maxGrad, grad[fx]);
if (maxGrad == 0) // no q does anything; probably no objective
maxGrad = Infinity; // no q will be kept for gradient purposes
Matrix jac = asmSys->calcCurrentJacobian();
Vector colNorm(getNumFreeQs());
Real maxJac = 0;
for (FreeQIndex fx(0); fx < grad.size(); ++fx) {
colNorm[fx] = jac(fx).norm();
maxJac = std::max(maxJac, colNorm[fx]);
}
if (maxJac == 0) // no q does anything; probably no constraints
maxJac = Infinity; // no q will be kept for Jacobian purposes
const Real QTol = SqrtEps;
const Real minGradAllowed = maxGrad*QTol;
const Real minJacAllowed = maxJac*QTol;
for (FreeQIndex fx(0); fx < grad.size(); ++fx)
if (grad[fx] < minGradAllowed && colNorm[fx] < minJacAllowed)
toBeLocked.push_back(getQIndexOfFreeQ(fx));
if (toBeLocked.size()) {
cout << "Reinitializing with these q's locked:\n";
cout << toBeLocked << endl;
reinitializeWithExtraQsLocked(toBeLocked);
alreadyInitialized = true;
}
}
void VisualizerProtocol::
addMenu(const String& title, int id, const Array_<pair<String, int> >& items) {
pthread_mutex_lock(&sceneLock);
WRITE(outPipe, &DefineMenu, 1);
short titleLength = (short)title.size();
WRITE(outPipe, &titleLength, sizeof(short));
WRITE(outPipe, title.c_str(), titleLength);
WRITE(outPipe, &id, sizeof(int));
short numItems = (short)items.size();
WRITE(outPipe, &numItems, sizeof(short));
for (int i = 0; i < numItems; i++) {
int buffer[] = {items[i].second, items[i].first.size()};
WRITE(outPipe, buffer, 2*sizeof(int));
WRITE(outPipe, items[i].first.c_str(), items[i].first.size());
}
pthread_mutex_unlock(&sceneLock);
}
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;
}
// Generate geometry for the given state and send it to the visualizer using
// the VisualizerProtocol object. In buffered mode this is called from the
// rendering thread; otherwise, this is just the main simulation thread.
void Visualizer::Impl::drawFrameNow(const State& state) {
m_system.realize(state, Stage::Position);
// Collect up the geometry that constitutes this scene.
Array_<DecorativeGeometry> geometry;
for (Stage stage = Stage::Topology; stage <= state.getSystemStage(); ++stage)
m_system.calcDecorativeGeometryAndAppend(state, stage, geometry);
for (unsigned i = 0; i < m_generators.size(); i++)
m_generators[i]->generateDecorations(state, geometry);
// Execute frame controls (e.g. camera positioning).
for (unsigned i = 0; i < m_controllers.size(); ++i)
m_controllers[i]->generateControls(Visualizer(this), state, geometry);
// Calculate the spatial pose of all the geometry and send it to the
// renderer.
m_protocol.beginScene(state.getTime());
VisualizerGeometry geometryCreator
(m_protocol, m_system.getMatterSubsystem(), state);
for (unsigned i = 0; i < geometry.size(); ++i)
geometry[i].implementGeometry(geometryCreator);
for (unsigned i = 0; i < m_addedGeometry.size(); ++i)
m_addedGeometry[i].implementGeometry(geometryCreator);
const SimbodyMatterSubsystem& matter = m_system.getMatterSubsystem();
for (unsigned i = 0; i < m_lines.size(); ++i) {
const RubberBandLine& line = m_lines[i];
const MobilizedBody& B1 = matter.getMobilizedBody(line.b1);
const MobilizedBody& B2 = matter.getMobilizedBody(line.b2);
const Transform& X_GB1 = B1.getBodyTransform(state);
const Transform& X_GB2 = B2.getBodyTransform(state);
const Vec3 end1 = X_GB1*line.station1;
const Vec3 end2 = X_GB2*line.station2;
const Real thickness = line.line.getLineThickness() == -1
? 1 : line.line.getLineThickness();
m_protocol.drawLine(end1, end2,
VisualizerGeometry::getColor(line.line), thickness);
}
m_protocol.finishScene();
++numFramesSentToRenderer;
}
void ObservedPointFitter::
createClonedSystem(const MultibodySystem& original, MultibodySystem& copy,
const Array_<MobilizedBodyIndex>& originalBodyIxs,
Array_<MobilizedBodyIndex>& copyBodyIxs,
bool& hasArtificialBaseBody)
{
const SimbodyMatterSubsystem& originalMatter = original.getMatterSubsystem();
SimbodyMatterSubsystem copyMatter(copy);
Body::Rigid body = Body::Rigid(MassProperties(1, Vec3(0), Inertia(1)));
body.addDecoration(Transform(), DecorativeSphere(Real(.1)));
std::map<MobilizedBodyIndex, MobilizedBodyIndex> idMap;
hasArtificialBaseBody = false;
for (int i = 0; i < (int)originalBodyIxs.size(); ++i) {
const MobilizedBody& originalBody = originalMatter.getMobilizedBody(originalBodyIxs[i]);
MobilizedBody* copyBody;
if (i == 0) {
if (originalBody.isGround())
copyBody = ©Matter.Ground();
else {
hasArtificialBaseBody = true; // not using the original joint here
MobilizedBody::Free free(copyMatter.Ground(), body);
copyBody = ©Matter.updMobilizedBody(free.getMobilizedBodyIndex());
}
}
else {
MobilizedBody& parent = copyMatter.updMobilizedBody(idMap[originalBody.getParentMobilizedBody().getMobilizedBodyIndex()]);
copyBody = &originalBody.cloneForNewParent(parent);
}
copyBodyIxs.push_back(copyBody->getMobilizedBodyIndex());
idMap[originalBodyIxs[i]] = copyBody->getMobilizedBodyIndex();
}
copy.realizeTopology();
State& s = copy.updDefaultState();
copyMatter.setUseEulerAngles(s, true);
copy.realizeModel(s);
}
int main() {
try
{ // Create the system.
MultibodySystem system;
SimbodyMatterSubsystem matter(system);
GeneralForceSubsystem forces(system);
/// uncoment gravity to get some sort of collision interaction
/// for cylinder mesh
// Force::UniformGravity gravity(forces, matter,Vec3(0,0.001,0), 2);
ContactTrackerSubsystem tracker(system);
//GeneralContactSubsystem contactsys(system);
CompliantContactSubsystem contactForces(system, tracker);
contactForces.setTrackDissipatedEnergy(true);
for(SubsystemIndex i(0); i<system.getNumSubsystems(); ++i)
{
fprintf(stderr,"subsytem name %d %s\n", (int)i,
system.getSubsystem((SubsystemIndex)i).getName().c_str());
}
const Real rad = .4;
PolygonalMesh pyramidMesh1,pyramidMesh2;
/// load cylinder forces drawn, but interaction depends on gravity???
const Real fFac =1; // to turn off friction
const Real fDis = .5*0.2; // to turn off dissipation
const Real fVis = .1*.1; // to turn off viscous friction
const Real fK = 100*1e6; // pascals
Body::Rigid pendulumBody3(MassProperties(100.0, Vec3(0), 100*Inertia(1)));
PolygonalMesh body3contact = PolygonalMesh::createSphereMesh(rad, 2);
ContactGeometry::TriangleMesh geo3(body3contact);
const DecorativeMesh mesh3(geo3.createPolygonalMesh());
pendulumBody3.addDecoration(Transform(),
DecorativeMesh(mesh3).setOpacity(.2));
pendulumBody3.addDecoration(Transform(),
DecorativeMesh(mesh3).setColor(Gray)
.setRepresentation(DecorativeGeometry::DrawWireframe)
.setOpacity(.1));
ContactSurface s1(geo3,
ContactMaterial(fK*.1,fDis*.9,fFac*.8,fFac*.7,fVis*10));
s1.setThickness(1);
s1.setShape(geo3);
//ContactGeometry::Sphere geo3(rad);
pendulumBody3.addContactSurface(Transform(),s1);
/*
std::ifstream meshFile1,meshFile2;
meshFile1.open("cyl3.obj");
pyramidMesh1.loadObjFile(meshFile1); meshFile1.close();
*/
pyramidMesh1 = PolygonalMesh::createSphereMesh(rad, 2);
ContactGeometry::TriangleMesh pyramid1(pyramidMesh1);
DecorativeMesh showPyramid1(pyramid1.createPolygonalMesh());
const Real ballMass = 200;
Body::Rigid ballBody(MassProperties(ballMass, Vec3(0),
ballMass*UnitInertia::sphere(1)));
ballBody.addDecoration(Transform(),
showPyramid1.setColor(Cyan).setOpacity(.2));
ballBody.addDecoration(Transform(),
showPyramid1.setColor(Gray)
.setRepresentation(DecorativeGeometry::DrawWireframe));
ContactSurface s2(pyramid1,
ContactMaterial(fK*.1,fDis*.9,
.1*fFac*.8,.1*fFac*.7,fVis*1));
s2.setThickness(1);
s2.setShape(pyramid1);
ballBody.addContactSurface(Transform(),/*ContactSurface(ContactGeometry::Sphere(rad),ContactMaterial(fK*.1,fDis*.9,
.1*fFac*.8,.1*fFac*.7,fVis*1))*/ s2/*.joinClique(clique1)*/);
/* Body::Rigid d(MassProperties(1.0, Vec3(0),Inertia(1)));
MobilizedBody::Pin dud(matter.Ground(),Transform(),d,Transform());
*/
MobilizedBody::Free ball(matter.Ground(), Transform(Vec3(-2,-2,0)),
ballBody, Transform(Vec3(0)));
MobilizedBody::Free ball1(matter.Ground(), Transform(Vec3(0,0,0)),
ballBody, Transform(Vec3(0)));
/*
MobilizedBody::Free ball2(matter.Ground(), Transform(Vec3(-4,0,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);
// 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.
//ExplicitEulerIntegrator integ(system);
CPodesIntegrator integ(system,CPodes::BDF,CPodes::Newton);
//RungeKuttaFeldbergIntegrator integ(system);
//RungeKuttaMersonIntegrator integ(system);
//RungeKutta3Integrator integ(system);
//VerletIntegrator integ(system);
//integ.setMaximumStepSize(1e-1);
//integ.setAllowInterpolation(false);
integ.setAccuracy(1e-3); // minimum for CPodes
//integ.setAccuracy(.1);
TimeStepper ts(system, integ);
ts.initialize(state);
double cpuStart = cpuTime();
double realStart = realTime();
ts.stepTo(2000.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;
}
void testInsert() {
const int data1[3] = {7, -2, 3};
const int data2[4] = {101, 121, -111, 321};
int wdata[3] = {99, 9999, 999}; // writable
Array_<int> a(data2,data2+4); // copy the data
SimTK_TEST(&a[0] != data2);
ArrayViewConst_<int> avc(data2, data2+4); // share the data
SimTK_TEST(&avc[1] == data2+1);
Array_<int> aw(wdata, wdata+3, DontCopy()); // shared
SimTK_TEST(&aw[0] == wdata);
// Can't insert into non-owner.
SimTK_TEST_MUST_THROW(aw.insert(aw.begin(), avc.begin(), avc.end()));
// Unless we're inserting zero elements; that's allowed.
aw.insert(&aw[1], avc.begin(), avc.begin());
Array_<int> ac(data1, data1+3);
std::vector<int> vc(data1, data1+3);
ac.insert(&ac[1], &a[1], &a[1]+2);
vc.insert(vc.begin()+1, &a[1], &a[1]+2);
SimTK_TEST(ac.size()==5);
SimTK_TEST(ac == vc); // 7, 121, -111, -2, 3
// insert vc onto beginning and end of ac
ac.insert(ac.begin(), vc.begin(), vc.end());
ac.insert(ac.end(), vc.begin(), vc.end());
SimTK_TEST(ac.size()==15);
SimTK_TEST(ac(0,5)==vc && ac(5,5)==vc && ac(10,5)==vc);
// Shrink ac back down to 5 again.
ac.erase(ac.begin()+2, ac.begin()+12);
SimTK_TEST(ac == vc);
SimTK_TEST(ac.allocated() >= 15);
ac.shrink_to_fit();
SimTK_TEST(ac.allocated() < 15);
SimTK_TEST(ac == vc); // make sure we didn't lose the data
// Now try some null insertions
Array_<int> null;
ac.insert(ac.begin(), null.begin(), null.end());
ac.insert(ac.begin()+2, null.begin(), null.end());
ac.insert(ac.end(), null.begin(), null.end());
ac.insert(ac.begin(), 0, 929);
ac.insert(ac.begin()+2, 0, 929);
ac.insert(ac.end(), 0, 929);
SimTK_TEST(ac == vc);
ArrayView_<int> null2;
null.insert(null.begin(), null2.begin(), null2.end());
SimTK_TEST(null.empty() && null2.empty());
// How about inserting into a null array?
null.insert(null.begin(), 3, 987);
SimTK_TEST(null == std::vector<int>(3,987));
null.deallocate(); // back to null
SimTK_TEST(null.data()==0 && null.size()==0 && null.allocated()==0);
null.insert(null.begin(), ac.begin(), ac.end());
SimTK_TEST(null == vc);
null.deallocate();
// Fill in a bunch of 1000's in the middle, erase the beginning and
// end, and make sure we see just the 1000's.
ac.insert(ac.begin()+2, 99, 1000);
ac.erase(ac.begin(), ac.begin()+2);
ac.erase(ac.end()-3, ac.end());
SimTK_TEST(ac == Array_<int>(99,1000));
}
// We assume a path name structure like this:
// (1) Everything up to and including the final directory separator
// character is the directory; the rest is the file name. On return
// we fix the slashes in the directory name to suit the current
// system ('\' for Windows, '/' otherwise).
// (2) If the file name contains a ".", characters after the last
// "." are the extension and the last "." is removed.
// (3) What's left is the fileName.
// We accept both "/" and "\" as separator characters. We leave the
// case as it was supplied.
// Leading and trailing white space is removed; embedded white space
// remains.
// Leading "X:" for some drive letter X is recognized on Windows as
// a drive specification, otherwise the drive is the current drive.
// That is then removed for further processing.
// Absolute paths are designated like this:
// Leading "/" means root relative (on the drive).
// Leading "./" means current working directory relative (on the drive).
// Leading "../" is interpreted as "./..".
// Leading "@/" means relative to executable location.
// Above leading characters are removed and replaced with the
// full path name they represent, then the entire path is divided
// into components. If a component is "." or "" (empty) it is
// removed. If a component is ".." it and the previous component
// if any are removed. (".." as a first component will report as
// ill formed.)
// If there is something ill-formed about the file name we'll return
// false.
void Pathname::deconstructPathname( const string& name,
bool& dontApplySearchPath,
string& directory,
string& fileName,
string& extension)
{
dontApplySearchPath = false;
directory.erase(); fileName.erase(); extension.erase();
// Remove all the white space and make all the slashes be forward ones.
// (For Windows they'll be changed to backslashes later.)
String processed = String::trimWhiteSpace(name).replaceAllChar('\\', '/');
if (processed.empty())
return; // name consisted only of white space
string drive;
removeDriveInPlace(processed, drive);
// Now the drive if any has been removed and we're looking at
// the beginning of the path name.
// If the pathname in its entirety is just one of these, append
// a slash to avoid special cases below.
if (processed == "." || processed == ".." || processed == "@")
processed += "/";
// If the path begins with "../" we'll make it ./../ to simplify handling.
if (processed.substr(0, 3) == "../")
processed.insert(0, "./");
if (processed.substr(0, 1) == "/") {
dontApplySearchPath = true;
processed.erase(0, 1);
if (drive.empty()) drive = getCurrentDriveLetter();
}
else if (processed.substr(0, 2) == "./") {
dontApplySearchPath = true;
processed.replace(0, 2, getCurrentWorkingDirectory(drive));
removeDriveInPlace(processed, drive);
}
else if (processed.substr(0, 2) == "@/") {
dontApplySearchPath = true;
processed.replace(0, 2, getThisExecutableDirectory());
removeDriveInPlace(processed, drive);
}
else if (!drive.empty()) {
// Looks like a relative path name. But if it had an initial
// drive specification, e.g. X:something.txt, that is supposed
// to be interpreted relative to the current working directory
// on drive X, just as though it were X:./something.txt.
dontApplySearchPath = true;
processed.insert(0, getCurrentWorkingDirectory(drive));
removeDriveInPlace(processed, drive);
}
// We may have picked up a new batch of backslashes above.
processed.replaceAllChar('\\', '/');
// Now we have the full path name if this is absolute, otherwise
// we're looking at a relative path name. In any case the last
// component is the file name if it isn't empty, ".", or "..".
// Process the ".." segments and eliminate meaningless ones
// as we go through.
Array_<string> segmentsInReverse;
bool isFinalSegment = true; // first time around might be the fileName
int numDotDotsSeen = 0;
while (!processed.empty()) {
string component;
removeLastPathComponentInPlace(processed, component);
if (component == "..")
++numDotDotsSeen;
else if (!component.empty() && component != ".") {
if (numDotDotsSeen)
--numDotDotsSeen; // skip component
else if (isFinalSegment) fileName = component;
else segmentsInReverse.push_back(component);
}
isFinalSegment = false;
}
// Now we can put together the canonicalized directory.
if (dontApplySearchPath) {
if (!drive.empty())
directory = drive + ":";
directory += "/";
}
for (int i = (int)segmentsInReverse.size() - 1; i >= 0; --i)
directory += segmentsInReverse[i] + "/";
// Fix the slashes.
makeNativeSlashesInPlace(directory);
// If there is a .extension, strip it off.
string::size_type lastDot = fileName.rfind('.');
if (lastDot != string::npos) {
extension = fileName.substr(lastDot);
fileName.erase(lastDot);
}
}
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 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);
if (menuId != RunMenuId || item != GoItem)
cout << "\aDude ... follow instructions!\n";
} while (menuId != RunMenuId || item != GoItem);
// Simulate it.
//ExplicitEulerIntegrator integ(system);
//CPodesIntegrator integ(system,CPodes::BDF,CPodes::Newton);
//RungeKuttaFeldbergIntegrator integ(system);
RungeKuttaMersonIntegrator integ(system);
//RungeKutta3Integrator integ(system);
//VerletIntegrator integ(system);
//integ.setMaximumStepSize(1e-0001);
integ.setAccuracy(1e-4); // minimum for CPodes
//integ.setAccuracy(.01);
TimeStepper ts(system, integ);
ts.initialize(state);
double cpuStart = cpuTime();
double realStart = realTime();
ts.stepTo(10.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;
}
void PerSubsystemInfo::resizeAllocationStack(Array_<T>& stack, int newSize) {
assert(newSize >= 0);
for (int i = stack.size()-1; i >= newSize; --i)
stack[i].deepDestruct(*m_stateImpl);
stack.resize(newSize);
}
void PerSubsystemInfo::clearAllocationStack(Array_<T>& stack) {
for (int i=stack.size()-1; i >= 0; --i)
stack[i].deepDestruct(*m_stateImpl);
stack.clear();
}
