/// 基礎的な剛体群を生成し、世界への放り込みます
void initialize_bodies()
{
// 静的な剛体の生成
{
// 衝突形状（btCollisionShape）としてgroundShapeを定義します
// なお、できるだけ、剛体群は衝突形状（シェイプ）を使い回せる様にしましょう！
std::unique_ptr<btCollisionShape> groundShape
( new btBoxShape
( btVector3( btScalar(50.), btScalar(50.), btScalar(50.) )
)
);
// 静的な剛体を生成して、btDefaultMotionStateとbtRigidBodyのスマートポインターを含むタプルをスコープに維持します
// 質量（mass）を0に設定すると静的な物体を生成できます
create_rigidbody(0., btVector3(0, -56, 0), groundShape);
// ローカルスコープのcolShapdeオブジェクトの管理をcollision_shapesへstd::moveしクラススコープに移管します
collision_shapes.emplace_front(std::move(groundShape));
}
// 動的な剛体の生成
{
// これから生成する動的物体用に衝突形状を生成します
// もし、衝突形状を変更すると動力学の世界の中での挙動ももちろん変化する事でしょう
//std::unique_ptr<btCollisionShape> colShape( new btBoxShape( btVector3(1, 1, 1) ) );
std::unique_ptr<btCollisionShape> colShape( new btSphereShape( btScalar(1.) ) );
// 動的な剛体を生成して、btDefaultMotionStateとbtRigidBodyのスマートポインターを含むタプルをスコープに維持します
// 質量（mass）を0以外に設定すると動的な物体を生成できます
create_rigidbody(1.f, btVector3(2, 10, 0), colShape);
// ローカルスコープのcolShapdeオブジェクトの管理をcollision_shapesへstd::moveしクラススコープに移管します
collision_shapes.emplace_front(std::move(colShape));
}
}
int main(int argc, char** argv) {
sf::RenderWindow* RenderWin = new sf::RenderWindow(sf::VideoMode(WIDTH, HEIGHT, 32), "lol test");
RenderWin->UseVerticalSync(true);
// Collision configuration contains default setup for memory, collision setup. Advanced users can create their own configuration.
boost::shared_ptr<btDefaultCollisionConfiguration> collisionConfiguration(new btDefaultCollisionConfiguration());
// Use the default collision dispatcher. For parallel processing you can use a diffent dispatcher (see Extras/BulletMultiThreaded).
boost::shared_ptr<btCollisionDispatcher> dispatcher(new btCollisionDispatcher(collisionConfiguration.get()));
// btDbvtBroadphase is a good general purpose broadphase. You can also try out btAxis3Sweep.
boost::shared_ptr<btBroadphaseInterface> broadphase(new btDbvtBroadphase());
// The default constraint solver. For parallel processing you can use a different solver (see Extras/BulletMultiThreaded).
boost::shared_ptr<btVoronoiSimplexSolver> simplex(new btVoronoiSimplexSolver());
boost::shared_ptr<btMinkowskiPenetrationDepthSolver> pd_solver(new btMinkowskiPenetrationDepthSolver());
boost::shared_ptr<btSequentialImpulseConstraintSolver> solver(new btSequentialImpulseConstraintSolver());
boost::shared_ptr<btDiscreteDynamicsWorld> dynamicsWorld(new btDiscreteDynamicsWorld(dispatcher.get(), broadphase.get(), solver.get(), collisionConfiguration.get()));
boost::shared_ptr<btConvex2dConvex2dAlgorithm::CreateFunc> convex_algo_2d(new btConvex2dConvex2dAlgorithm::CreateFunc(simplex.get(),pd_solver.get()));
dispatcher->registerCollisionCreateFunc(CONVEX_2D_SHAPE_PROXYTYPE,CONVEX_2D_SHAPE_PROXYTYPE, convex_algo_2d.get());
dispatcher->registerCollisionCreateFunc(BOX_2D_SHAPE_PROXYTYPE,CONVEX_2D_SHAPE_PROXYTYPE, convex_algo_2d.get());
dispatcher->registerCollisionCreateFunc(CONVEX_2D_SHAPE_PROXYTYPE,BOX_2D_SHAPE_PROXYTYPE, convex_algo_2d.get());
dispatcher->registerCollisionCreateFunc(BOX_2D_SHAPE_PROXYTYPE,BOX_2D_SHAPE_PROXYTYPE, new btBox2dBox2dCollisionAlgorithm::CreateFunc());
// Set gravity to 9.8m/s² along y-axis.
dynamicsWorld->setGravity(btVector3(0, 1, 0));
// Get us some debug output. Without this, we'd see nothing at all.
boost::shared_ptr<DebugDraw> debugDraw(new DebugDraw(RenderWin));
debugDraw->setDebugMode(btIDebugDraw::DBG_DrawWireframe);
dynamicsWorld->setDebugDrawer(debugDraw.get());
// Keep track of the shapes, we release memory at exit.
// Make sure to re-use collision shapes among rigid bodies whenever possible!
btAlignedObjectArray<btCollisionShape*> collisionShapes;
// Create a ground body.
btScalar thickness(0.2);
boost::shared_ptr<btCollisionShape> groundShape(new btBoxShape(btVector3(btScalar(WIDTH / 2 * METERS_PER_PIXEL), thickness, btScalar(10))));
collisionShapes.push_back(groundShape.get());
btTransform groundTransform(btQuaternion(0, 0, 0, 1), btVector3(WIDTH / 2 * METERS_PER_PIXEL, HEIGHT * METERS_PER_PIXEL, 0));
// Using motionstate is recommended, it provides interpolation capabilities, and only synchronizes 'active' objects.
boost::shared_ptr<btDefaultMotionState> groundMotionState(new btDefaultMotionState(groundTransform));
btRigidBody::btRigidBodyConstructionInfo ground_rbInfo(0, groundMotionState.get(), groundShape.get(), btVector3(0, 0, 0));
boost::shared_ptr<btRigidBody> ground_body(new btRigidBody(ground_rbInfo));
ground_body->setLinearFactor(btVector3(1,1,0));
ground_body->setAngularFactor(btVector3(0,0,1));
// Add the body to the dynamics world.
dynamicsWorld->addRigidBody(ground_body.get());
// Create left wall.
btTransform leftWallTransform(btQuaternion(0, 0, 1, 1), btVector3(0, HEIGHT / 2 * METERS_PER_PIXEL, 0));
boost::shared_ptr<btDefaultMotionState> leftWallMotionState(new btDefaultMotionState(leftWallTransform));
btRigidBody::btRigidBodyConstructionInfo leftWall_rbInfo(0, leftWallMotionState.get(), groundShape.get(), btVector3(0, 0, 0));
boost::shared_ptr<btRigidBody> leftwall_body(new btRigidBody(leftWall_rbInfo));
leftwall_body->setLinearFactor(btVector3(1,1,0));
leftwall_body->setAngularFactor(btVector3(0,0,1));
// Add the body to the dynamics world.
dynamicsWorld->addRigidBody(leftwall_body.get());
// Create right wall.
btTransform rightWallTransform(btQuaternion(0, 0, 1, 1), btVector3(WIDTH * METERS_PER_PIXEL, HEIGHT / 2 * METERS_PER_PIXEL, 0));
boost::shared_ptr<btDefaultMotionState> rightWallMotionState(new btDefaultMotionState(rightWallTransform));
btRigidBody::btRigidBodyConstructionInfo rightWall_rbInfo(0, rightWallMotionState.get(), groundShape.get(), btVector3(0, 0, 0));
boost::shared_ptr<btRigidBody> rightwall_body(new btRigidBody(rightWall_rbInfo));
rightwall_body->setLinearFactor(btVector3(1,1,0));
rightwall_body->setAngularFactor(btVector3(0,0,1));
// Add the body to the dynamics world.
dynamicsWorld->addRigidBody(rightwall_body.get());
// Create ceiling
btTransform topWallTransform(btQuaternion(0, 0, 0, 1), btVector3(WIDTH / 2 * METERS_PER_PIXEL, 0, 0));
boost::shared_ptr<btDefaultMotionState> topWallMotionState(new btDefaultMotionState(topWallTransform));
btRigidBody::btRigidBodyConstructionInfo topWall_rbInfo(0, topWallMotionState.get(), groundShape.get(), btVector3(0, 0, 0));
boost::shared_ptr<btRigidBody> topwall_body(new btRigidBody(topWall_rbInfo));
topwall_body->setLinearFactor(btVector3(1,1,0));
topwall_body->setAngularFactor(btVector3(0,0,1));
// Add the body to the dynamics world.
dynamicsWorld->addRigidBody(topwall_body.get());
// Create dynamic rigid body.
//btCollisionShape* colShape = new btBoxShape(btVector3(1,1,1));
boost::shared_ptr<btCollisionShape> colShape(new btSphereShape(btScalar(0.6)));
collisionShapes.push_back(colShape.get());
/// Create Dynamic Objects
btTransform startTransform;
startTransform.setIdentity();
btScalar mass(1.f);
//rigidbody is dynamic if and only if mass is non zero, otherwise static
bool isDynamic = (mass != 0.f);
btVector3 localInertia(0, 0, 0);
if (isDynamic)
colShape->calculateLocalInertia(mass,localInertia);
startTransform.setOrigin(btVector3(2, 5, 0));
//using motionstate is recommended, it provides interpolation capabilities, and only synchronizes 'active' objects
boost::shared_ptr<btDefaultMotionState> myMotionState(new btDefaultMotionState(startTransform));
btRigidBody::btRigidBodyConstructionInfo rbInfo(mass,myMotionState.get(),colShape.get(),localInertia);
boost::shared_ptr<btRigidBody> body(new btRigidBody(rbInfo));
body->setLinearFactor(btVector3(1,1,0));
body->setAngularFactor(btVector3(0,0,1));
dynamicsWorld->addRigidBody(body.get());
// Create lulz
boost::ptr_list<btRigidBody> body_list;
boost::ptr_list<btDefaultMotionState> motionstate_list;
boost::ptr_list<btCollisionShape> colshape_list;
for (int i=0;i <= 10; ++i) {
if (i < 5)
colshape_list.push_back(new btSphereShape(btScalar(sf::Randomizer::Random(0.1f, 0.8f))));
else
colshape_list.push_back(new btBoxShape(btVector3(sf::Randomizer::Random(0.1f,0.8f), sf::Randomizer::Random(0.1f,0.8f), 10)));
if (isDynamic)
colshape_list.back().calculateLocalInertia(mass,localInertia);
collisionShapes.push_back(&(colshape_list.back()));
startTransform.setIdentity();
startTransform.setOrigin(btVector3(i,i,0));
motionstate_list.push_back(new btDefaultMotionState(startTransform));
btRigidBody* lol = new btRigidBody(btRigidBody::btRigidBodyConstructionInfo(mass,&(motionstate_list.back()),&(colshape_list.back()),localInertia));
lol->setLinearFactor(btVector3(1,1,0));
lol->setAngularFactor(btVector3(0,0,1));
body_list.push_back(lol);
}
BOOST_FOREACH (btRigidBody& body, body_list) {
dynamicsWorld->addRigidBody(&body);
}
int main()
{
// User-defined main parameters
double t0 = 0; // initial computation time
double T = 20.0; // end of computation time
double h = 0.005; // time step
double position_init = 10.0; // initial position
double velocity_init = 0.0; // initial velocity
double g = 9.81;
double theta = 0.5; // theta for MoreauJeanOSI integrator
// -----------------------------------------
// --- Dynamical systems && interactions ---
// -----------------------------------------
try
{
// ------------
// --- Init ---
// ------------
std::cout << "====> Model loading ..." << std::endl << std::endl;
// -- OneStepIntegrators --
SP::OneStepIntegrator osi;
osi.reset(new MoreauJeanOSI(theta));
// -- Model --
SP::Model model(new Model(t0, T));
std::vector<SP::BulletWeightedShape> shapes;
// note: no rebound with a simple Bullet Box, why ?
// the distance after the broadphase contact detection is negative
// and then stay negative.
// SP::btCollisionShape box(new btBoxShape(btVector3(1,1,1)));
// SP::BulletWeightedShape box1(new BulletWeightedShape(box,1.0));
// This is ok if we build one with btConveHullShape
SP::btCollisionShape box(new btConvexHullShape());
{
std11::static_pointer_cast<btConvexHullShape>(box)->addPoint(btVector3(-1.0, 1.0, -1.0));
std11::static_pointer_cast<btConvexHullShape>(box)->addPoint(btVector3(-1.0, -1.0, -1.0));
std11::static_pointer_cast<btConvexHullShape>(box)->addPoint(btVector3(-1.0, -1.0, 1.0));
std11::static_pointer_cast<btConvexHullShape>(box)->addPoint(btVector3(-1.0, 1.0, 1.0));
std11::static_pointer_cast<btConvexHullShape>(box)->addPoint(btVector3(1.0, 1.0, 1.0));
std11::static_pointer_cast<btConvexHullShape>(box)->addPoint(btVector3(1.0, 1.0, -1.0));
std11::static_pointer_cast<btConvexHullShape>(box)->addPoint(btVector3(1.0, -1.0, -1.0));
std11::static_pointer_cast<btConvexHullShape>(box)->addPoint(btVector3(1.0, -1.0, 1.0));
}
SP::BulletWeightedShape box1(new BulletWeightedShape(box, 1.0));
shapes.push_back(box1);
SP::SiconosVector q0(new SiconosVector(7));
SP::SiconosVector v0(new SiconosVector(6));
v0->zero();
q0->zero();
(*q0)(2) = position_init;
(*q0)(3) = 1.0;
(*v0)(2) = velocity_init;
// -- The dynamical system --
// -- the default contactor is the shape given in the constructor
// -- the contactor id is 0
SP::BulletDS body(new BulletDS(box1, q0, v0));
// -- Set external forces (weight) --
SP::SiconosVector FExt;
FExt.reset(new SiconosVector(3)); //
FExt->zero();
FExt->setValue(2, - g * box1->mass());
body->setFExtPtr(FExt);
// -- Add the dynamical system in the non smooth dynamical system
model->nonSmoothDynamicalSystem()->insertDynamicalSystem(body);
SP::btCollisionObject ground(new btCollisionObject());
ground->setCollisionFlags(btCollisionObject::CF_STATIC_OBJECT);
SP::btCollisionShape groundShape(new btBoxShape(btVector3(30, 30, .5)));
btMatrix3x3 basis;
basis.setIdentity();
ground->getWorldTransform().setBasis(basis);
ground->setCollisionShape(&*groundShape);
ground->getWorldTransform().getOrigin().setZ(-.50);
// ------------------
// --- Simulation ---
// ------------------
// -- Time discretisation --
SP::TimeDiscretisation timedisc(new TimeDiscretisation(t0, h));
// -- OneStepNsProblem --
SP::FrictionContact osnspb(new FrictionContact(3));
// -- Some configuration
osnspb->numericsSolverOptions()->iparam[0] = 1000; // Max number of
// iterations
osnspb->numericsSolverOptions()->dparam[0] = 1e-5; // Tolerance
osnspb->setMaxSize(16384); // max number of
// interactions
osnspb->setMStorageType(1); // Sparse storage
osnspb->setNumericsVerboseMode(0); // 0 silent, 1
// verbose
osnspb->setKeepLambdaAndYState(true); // inject
// previous
// solution
// --- Simulation initialization ---
std::cout << "====> Simulation initialisation ..." << std::endl << std::endl;
int N = ceil((T - t0) / h); // Number of time steps
SP::NonSmoothLaw nslaw(new NewtonImpactFrictionNSL(0.8, 0., 0.0, 3));
// -- The space filter performs broadphase collision detection
SP::BulletSpaceFilter space_filter(new BulletSpaceFilter(model));
// -- insert a non smooth law for contactors id 0
space_filter->insert(nslaw, 0, 0);
// -- add multipoint iterations, this is needed to gather at least
// -- 3 contact points and avoid objects penetration, see Bullet
// -- documentation
space_filter->collisionConfiguration()->setConvexConvexMultipointIterations();
space_filter->collisionConfiguration()->setPlaneConvexMultipointIterations();
// -- The ground is a static object
// -- we give it a group contactor id : 0
space_filter->addStaticObject(ground, 0);
// -- MoreauJeanOSI Time Stepping with Bullet Dynamical Systems
SP::BulletTimeStepping simulation(new BulletTimeStepping(timedisc));
simulation->insertIntegrator(osi);
simulation->insertNonSmoothProblem(osnspb);
model->setSimulation(simulation);
model->initialize();
std::cout << "====> End of initialisation ..." << std::endl << std::endl;
// --- Get the values to be plotted ---
// -> saved in a matrix dataPlot
unsigned int outputSize = 4;
SimpleMatrix dataPlot(N + 1, outputSize);
dataPlot.zero();
SP::SiconosVector q = body->q();
SP::SiconosVector v = body->velocity();
dataPlot(0, 0) = model->t0();
dataPlot(0, 1) = (*q)(2);
dataPlot(0, 2) = (*v)(2);
// --- Time loop ---
std::cout << "====> Start computation ... " << std::endl << std::endl;
// ==== Simulation loop - Writing without explicit event handling =====
int k = 1;
boost::progress_display show_progress(N);
boost::timer time;
time.restart();
while (simulation->hasNextEvent())
{
space_filter->buildInteractions(model->currentTime());
simulation->computeOneStep();
// --- Get values to be plotted ---
dataPlot(k, 0) = simulation->nextTime();
dataPlot(k, 1) = (*q)(2);
dataPlot(k, 2) = (*v)(2);
// If broadphase collision detection shows some contacts then we may
// display contact forces.
if (space_filter->collisionWorld()->getDispatcher()->getNumManifolds() > 0)
{
// we *must* have an indexSet0, filled by Bullet broadphase
// collision detection and an indexSet1, filled by
// TimeStepping::updateIndexSet with the help of Bullet
// getDistance() function
if (model->nonSmoothDynamicalSystem()->topology()->numberOfIndexSet() == 2)
{
SP::InteractionsGraph index1 = simulation->indexSet(1);
// This is the narrow phase contact detection : if
// TimeStepping::updateIndexSet has filled indexSet1 then we
// have some contact forces to display
if (index1->size() > 0)
{
// Four contact points for a cube with a side facing the
// ground. Note : changing Bullet margin for collision
// detection may lead this assertion to be false.
if (index1->size() == 4)
{
InteractionsGraph::VIterator iur = index1->begin();
// different version of bullet may not gives the same
// contact points! So we only keep the summation.
dataPlot(k, 3) =
index1->bundle(*iur)-> lambda(1)->norm2() +
index1->bundle(*++iur)->lambda(1)->norm2() +
index1->bundle(*++iur)->lambda(1)->norm2() +
index1->bundle(*++iur)->lambda(1)->norm2();
}
}
}
}
simulation->nextStep();
++show_progress;
k++;
}
std::cout << std::endl << "End of computation - Number of iterations done: " << k - 1 << std::endl;
std::cout << "Computation Time " << time.elapsed() << std::endl;
// --- Output files ---
std::cout << "====> Output file writing ..." << std::endl;
dataPlot.resize(k, outputSize);
ioMatrix::write("result.dat", "ascii", dataPlot, "noDim");
// Comparison with a reference file
SimpleMatrix dataPlotRef(dataPlot);
dataPlotRef.zero();
ioMatrix::read("result.ref", "ascii", dataPlotRef);
if ((dataPlot - dataPlotRef).normInf() > 1e-12)
{
std::cout << "Warning. The result is rather different from the reference file : "
<< (dataPlot - dataPlotRef).normInf() << std::endl;
return 1;
}
}
catch (SiconosException e)
{
std::cout << e.report() << std::endl;
exit(1);
}
catch (...)
{
std::cout << "Exception caught in BulletBouncingBox" << std::endl;
exit(1);
}
return 0;
}
