//------------------------------------------------------------------------------ // CALC TIME OF NEXT SCHEDULED REPORT //------------------------------------------------------------------------------ void Subsystem::Guts:: calcTimeOfNextScheduledReport(const State& state, Real& tNextReport, Array_<EventId>& eventIds, bool includeCurrentTime) const { tNextReport = Infinity; eventIds.clear(); calcTimeOfNextScheduledReportImpl(state, tNextReport, eventIds, includeCurrentTime); }
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; }
void PerSubsystemInfo::clearAllocationStack(Array_<T>& stack) { for (int i=stack.size()-1; i >= 0; --i) stack[i].deepDestruct(*m_stateImpl); stack.clear(); }
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"; } }