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