void btBulletPhysicsEffectsWorld::solveConstraints(btContactSolverInfo& solverInfo) { BT_PROFILE("solveConstraints"); btCollisionDispatcher* disp = (btCollisionDispatcher*) getDispatcher(); int numBodies = getNumCollisionObjects(); btPersistentManifold** manifolds = disp->getInternalManifoldPointer(); int numManifolds = disp->getNumManifolds(); if ((getNumCollisionObjects()>0) && (numManifolds + m_constraints.size()>0)) { btCollisionObject** bodies = numBodies ? &getCollisionObjectArray()[0] : 0; btTypedConstraint** constraints = m_constraints.size() ? &m_constraints[0] : 0; getConstraintSolver()->solveGroup( bodies,numBodies, disp->getInternalManifoldPointer(),numManifolds, constraints, m_constraints.size() ,m_solverInfo,m_debugDrawer,m_stackAlloc,disp); } }
void ForkLiftDemo::stepSimulation(float deltaTime) { //glClear(GL_COLOR_BUFFER_BIT | GL_DEPTH_BUFFER_BIT); { int wheelIndex = 2; m_vehicle->applyEngineForce(gEngineForce,wheelIndex); m_vehicle->setBrake(gBreakingForce,wheelIndex); wheelIndex = 3; m_vehicle->applyEngineForce(gEngineForce,wheelIndex); m_vehicle->setBrake(gBreakingForce,wheelIndex); wheelIndex = 0; m_vehicle->setSteeringValue(gVehicleSteering,wheelIndex); wheelIndex = 1; m_vehicle->setSteeringValue(gVehicleSteering,wheelIndex); } float dt = deltaTime; if (m_dynamicsWorld) { //during idle mode, just run 1 simulation step maximum int maxSimSubSteps = 2; int numSimSteps; numSimSteps = m_dynamicsWorld->stepSimulation(dt,maxSimSubSteps); if (m_dynamicsWorld->getConstraintSolver()->getSolverType()==BT_MLCP_SOLVER) { btMLCPSolver* sol = (btMLCPSolver*) m_dynamicsWorld->getConstraintSolver(); int numFallbacks = sol->getNumFallbacks(); if (numFallbacks) { static int totalFailures = 0; totalFailures+=numFallbacks; printf("MLCP solver failed %d times, falling back to btSequentialImpulseSolver (SI)\n",totalFailures); } sol->setNumFallbacks(0); } //#define VERBOSE_FEEDBACK #ifdef VERBOSE_FEEDBACK if (!numSimSteps) printf("Interpolated transforms\n"); else { if (numSimSteps > maxSimSubSteps) { //detect dropping frames printf("Dropped (%i) simulation steps out of %i\n",numSimSteps - maxSimSubSteps,numSimSteps); } else { printf("Simulated (%i) steps\n",numSimSteps); } } #endif //VERBOSE_FEEDBACK } }
//to be implemented by the demo void ForkLiftDemo::renderScene() { m_guiHelper->syncPhysicsToGraphics(m_dynamicsWorld); for (int i=0;i<m_vehicle->getNumWheels();i++) { //synchronize the wheels with the (interpolated) chassis worldtransform m_vehicle->updateWheelTransform(i,true); CommonRenderInterface* renderer = m_guiHelper->getRenderInterface(); if (renderer) { btTransform tr = m_vehicle->getWheelInfo(i).m_worldTransform; btVector3 pos=tr.getOrigin(); btQuaternion orn = tr.getRotation(); renderer->writeSingleInstanceTransformToCPU(pos,orn,m_wheelInstances[i]); } } m_guiHelper->render(m_dynamicsWorld); ATTRIBUTE_ALIGNED16(btScalar) m[16]; int i; btVector3 wheelColor(1,0,0); btVector3 worldBoundsMin,worldBoundsMax; getDynamicsWorld()->getBroadphase()->getBroadphaseAabb(worldBoundsMin,worldBoundsMax); for (i=0;i<m_vehicle->getNumWheels();i++) { //synchronize the wheels with the (interpolated) chassis worldtransform m_vehicle->updateWheelTransform(i,true); //draw wheels (cylinders) m_vehicle->getWheelInfo(i).m_worldTransform.getOpenGLMatrix(m); // m_shapeDrawer->drawOpenGL(m,m_wheelShape,wheelColor,getDebugMode(),worldBoundsMin,worldBoundsMax); } #if 0 int lineWidth=400; int xStart = m_glutScreenWidth - lineWidth; int yStart = 20; if((getDebugMode() & btIDebugDraw::DBG_NoHelpText)==0) { setOrthographicProjection(); glDisable(GL_LIGHTING); glColor3f(0, 0, 0); char buf[124]; sprintf(buf,"SHIFT+Cursor Left/Right - rotate lift"); GLDebugDrawString(xStart,20,buf); yStart+=20; sprintf(buf,"SHIFT+Cursor UP/Down - fork up/down"); yStart+=20; GLDebugDrawString(xStart,yStart,buf); if (m_useDefaultCamera) { sprintf(buf,"F5 - camera mode (free)"); } else { sprintf(buf,"F5 - camera mode (follow)"); } yStart+=20; GLDebugDrawString(xStart,yStart,buf); yStart+=20; if (m_dynamicsWorld->getConstraintSolver()->getSolverType()==BT_MLCP_SOLVER) { sprintf(buf,"F6 - solver (direct MLCP)"); } else { sprintf(buf,"F6 - solver (sequential impulse)"); } GLDebugDrawString(xStart,yStart,buf); btDiscreteDynamicsWorld* world = (btDiscreteDynamicsWorld*) m_dynamicsWorld; if (world->getLatencyMotionStateInterpolation()) { sprintf(buf,"F7 - motionstate interpolation (on)"); } else { sprintf(buf,"F7 - motionstate interpolation (off)"); } yStart+=20; GLDebugDrawString(xStart,yStart,buf); sprintf(buf,"Click window for keyboard focus"); yStart+=20; GLDebugDrawString(xStart,yStart,buf); resetPerspectiveProjection(); glEnable(GL_LIGHTING); } #endif }
//============================================================================== TEST(Issue1184, Accuracy) { struct ShapeInfo { dart::dynamics::ShapePtr shape; double offset; }; std::function<ShapeInfo()> makePlaneGround = []() { return ShapeInfo{ std::make_shared<dart::dynamics::PlaneShape>( Eigen::Vector3d::UnitZ(), 0.0), 0.0}; }; std::function<ShapeInfo()> makeBoxGround = []() { const double thickness = 0.1; return ShapeInfo{ std::make_shared<dart::dynamics::BoxShape>( Eigen::Vector3d(100.0, 100.0, thickness)), -thickness/2.0}; }; std::function<dart::dynamics::ShapePtr(double)> makeBoxObject = [](const double s) -> dart::dynamics::ShapePtr { return std::make_shared<dart::dynamics::BoxShape>( Eigen::Vector3d::Constant(2*s)); }; std::function<dart::dynamics::ShapePtr(double)> makeSphereObject = [](const double s) -> dart::dynamics::ShapePtr { return std::make_shared<dart::dynamics::SphereShape>(s); }; #ifndef NDEBUG const auto groundInfoFunctions = {makePlaneGround}; const auto objectShapeFunctions = {makeSphereObject}; const auto halfsizes = {10.0}; const auto fallingModes = {true}; const double dropHeight = 0.1; const double tolerance = 1e-3; #else const auto groundInfoFunctions = {makePlaneGround, makeBoxGround}; const auto objectShapeFunctions = {makeBoxObject, makeSphereObject}; const auto halfsizes = {0.25, 1.0, 5.0, 10.0, 20.0}; const auto fallingModes = {true, false}; const double dropHeight = 1.0; const double tolerance = 1e-3; #endif for(const auto& groundInfoFunction : groundInfoFunctions) { for(const auto& objectShapeFunction : objectShapeFunctions) { for(const double halfsize : halfsizes) { for(const bool falling : fallingModes) { auto world = dart::simulation::World::create("test"); world->getConstraintSolver()->setCollisionDetector( dart::collision::BulletCollisionDetector::create()); Eigen::Isometry3d tf_object = Eigen::Isometry3d::Identity(); const double initialHeight = falling? dropHeight+halfsize : halfsize; tf_object.translate(initialHeight*Eigen::Vector3d::UnitZ()); auto object = dart::dynamics::Skeleton::create("ball"); object->createJointAndBodyNodePair<dart::dynamics::FreeJoint>() .first->setTransform(tf_object); const auto objectShape = objectShapeFunction(halfsize); object->getBodyNode(0)->createShapeNodeWith< dart::dynamics::VisualAspect, dart::dynamics::CollisionAspect>(objectShape); world->addSkeleton(object); const ShapeInfo groundInfo = groundInfoFunction(); auto ground = dart::dynamics::Skeleton::create("ground"); ground->createJointAndBodyNodePair<dart::dynamics::WeldJoint>() .second->createShapeNodeWith< dart::dynamics::VisualAspect, dart::dynamics::CollisionAspect>(groundInfo.shape); Eigen::Isometry3d tf_ground = Eigen::Isometry3d::Identity(); tf_ground.translate(groundInfo.offset*Eigen::Vector3d::UnitZ()); ground->getJoint(0)->setTransformFromParentBodyNode(tf_ground); world->addSkeleton(ground); // time until the object will strike const double t_strike = falling? sqrt(-2.0*dropHeight/world->getGravity()[2]) : 0.0; // give the object some time to settle const double min_time = 0.5; const double t_limit = 30.0*t_strike + min_time; double lowestHeight = std::numeric_limits<double>::infinity(); double time = 0.0; while(time < t_limit) { world->step(); const double currentHeight = object->getBodyNode(0)->getTransform().translation()[2]; if(currentHeight < lowestHeight) lowestHeight = currentHeight; time = world->getTime(); } // The simulation should have run for at least two seconds ASSERT_LE(min_time, time); EXPECT_GE(halfsize+tolerance, lowestHeight) << "object type: " << objectShape->getType() << "\nground type: " << groundInfo.shape->getType() << "\nfalling: " << falling << "\n"; const double finalHeight = object->getBodyNode(0)->getTransform().translation()[2]; EXPECT_NEAR(halfsize, finalHeight, tolerance) << "object type: " << objectShape->getType() << "\nground type: " << groundInfo.shape->getType() << "\nfalling: " << falling << "\n"; } } } } }