/// 基礎的な剛体群を生成し、世界への放り込みます 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; }