|
| 1 | +#include <cstdio> |
| 2 | +#include <cstdlib> |
| 3 | +#include <map> |
| 4 | +#include <type_traits> |
| 5 | +#include <vector> |
| 6 | + |
| 7 | +#include <btBulletDynamicsCommon.h> |
| 8 | + |
| 9 | +#include "common.hpp" |
| 10 | + |
| 11 | +constexpr float gravity = 0.0f; |
| 12 | +// How inclined the ground plane is towards +x. |
| 13 | +constexpr float groundXNormal = 0.0f; |
| 14 | +constexpr float initialX = 0.0f; |
| 15 | +constexpr float initialY = 5.0f; |
| 16 | +constexpr float initialZ = 10.0f; |
| 17 | +constexpr float initialLinearVelocityX = 1.0f; |
| 18 | +constexpr float initialLinearVelocityY = 2.0f; |
| 19 | +constexpr float initialLinearVelocityZ = 30.0f; |
| 20 | +constexpr float initialAngularVelocityX = 1.0f; |
| 21 | +constexpr float initialAngularVelocityY = 2.0f; |
| 22 | +constexpr float initialAngularVelocityZ = 3.0f; |
| 23 | +constexpr float timeStep = 1.0f / 60.0f; |
| 24 | +// TODO some combinations of coefficients smaller than 1.0 |
| 25 | +// make the ball go up higher / not lose height. Why? |
| 26 | +constexpr float groundRestitution = 0.9f; |
| 27 | +constexpr float objectRestitution = 0.9f; |
| 28 | +constexpr int nSteps = 50; |
| 29 | + |
| 30 | +std::map<const btCollisionObject*,std::vector<btManifoldPoint*>> objectsCollisions; |
| 31 | +void myTickCallback(btDynamicsWorld *dynamicsWorld, btScalar timeStep) { |
| 32 | + objectsCollisions.clear(); |
| 33 | + int numManifolds = dynamicsWorld->getDispatcher()->getNumManifolds(); |
| 34 | + for (int i = 0; i < numManifolds; i++) { |
| 35 | + btPersistentManifold *contactManifold = dynamicsWorld->getDispatcher()->getManifoldByIndexInternal(i); |
| 36 | + auto *objA = contactManifold->getBody0(); |
| 37 | + auto *objB = contactManifold->getBody1(); |
| 38 | + auto& collisionsA = objectsCollisions[objA]; |
| 39 | + auto& collisionsB = objectsCollisions[objB]; |
| 40 | + int numContacts = contactManifold->getNumContacts(); |
| 41 | + for (int j = 0; j < numContacts; j++) { |
| 42 | + btManifoldPoint& pt = contactManifold->getContactPoint(j); |
| 43 | + collisionsA.push_back(&pt); |
| 44 | + collisionsB.push_back(&pt); |
| 45 | + } |
| 46 | + } |
| 47 | +} |
| 48 | + |
| 49 | +int main() { |
| 50 | + btDefaultCollisionConfiguration *collisionConfiguration |
| 51 | + = new btDefaultCollisionConfiguration(); |
| 52 | + btCollisionDispatcher *dispatcher = new btCollisionDispatcher(collisionConfiguration); |
| 53 | + btBroadphaseInterface *overlappingPairCache = new btDbvtBroadphase(); |
| 54 | + btSequentialImpulseConstraintSolver* solver = new btSequentialImpulseConstraintSolver; |
| 55 | + btDiscreteDynamicsWorld *dynamicsWorld = new btDiscreteDynamicsWorld( |
| 56 | + dispatcher, overlappingPairCache, solver, collisionConfiguration); |
| 57 | + dynamicsWorld->setGravity(btVector3(0, gravity, 0)); |
| 58 | + dynamicsWorld->setInternalTickCallback(myTickCallback); |
| 59 | + btAlignedObjectArray<btCollisionShape*> collisionShapes; |
| 60 | + |
| 61 | + // Object0 |
| 62 | + { |
| 63 | + btCollisionShape *colShape; |
| 64 | +#if 0 |
| 65 | + colShape = new btSphereShape(btScalar(1.0)); |
| 66 | +#else |
| 67 | + // Because of numerical instabilities, the cube bumps all over, |
| 68 | + // moving on the x and z directions as well as y. |
| 69 | + colShape = new btBoxShape( |
| 70 | + btVector3(btScalar(1.0), btScalar(1.0), btScalar(1.0))); |
| 71 | +#endif |
| 72 | + collisionShapes.push_back(colShape); |
| 73 | + btTransform startTransform; |
| 74 | + startTransform.setIdentity(); |
| 75 | + startTransform.setOrigin(btVector3(initialX, initialY, initialZ)); |
| 76 | + btVector3 localInertia(0, 0, 0); |
| 77 | + btScalar mass(1.0f); |
| 78 | + colShape->calculateLocalInertia(mass, localInertia); |
| 79 | + btDefaultMotionState *myMotionState = new btDefaultMotionState(startTransform); |
| 80 | + btRigidBody *body = new btRigidBody(btRigidBody::btRigidBodyConstructionInfo( |
| 81 | + mass, myMotionState, colShape, localInertia)); |
| 82 | + body->setRestitution(objectRestitution); |
| 83 | + body->setLinearVelocity(btVector3(initialLinearVelocityX, initialLinearVelocityY, initialLinearVelocityZ)); |
| 84 | + body->setAngularVelocity(btVector3(initialAngularVelocityX, initialAngularVelocityY, initialAngularVelocityZ)); |
| 85 | + dynamicsWorld->addRigidBody(body); |
| 86 | + } |
| 87 | + |
| 88 | + // Object1 |
| 89 | + { |
| 90 | + btCollisionShape *colShape; |
| 91 | +#if 0 |
| 92 | + colShape = new btSphereShape(btScalar(1.0)); |
| 93 | +#else |
| 94 | + // Because of numerical instabilities, the cube bumps all over, |
| 95 | + // moving on the x and z directions as well as y. |
| 96 | + colShape = new btBoxShape( |
| 97 | + btVector3(btScalar(1.0), btScalar(1.0), btScalar(1.0))); |
| 98 | +#endif |
| 99 | + collisionShapes.push_back(colShape); |
| 100 | + btTransform startTransform; |
| 101 | + startTransform.setIdentity(); |
| 102 | + startTransform.setOrigin(btVector3(initialX, initialY, initialZ)); |
| 103 | + btVector3 localInertia(0, 0, 0); |
| 104 | + btScalar mass(1.0f); |
| 105 | + colShape->calculateLocalInertia(mass, localInertia); |
| 106 | + btDefaultMotionState *myMotionState = new btDefaultMotionState(startTransform); |
| 107 | + btRigidBody *body = new btRigidBody(btRigidBody::btRigidBodyConstructionInfo( |
| 108 | + mass, myMotionState, colShape, localInertia)); |
| 109 | + body->setRestitution(objectRestitution); |
| 110 | + body->setLinearVelocity(btVector3(initialLinearVelocityX, initialLinearVelocityY, initialLinearVelocityZ)); |
| 111 | + body->setAngularVelocity(btVector3(initialAngularVelocityX, initialAngularVelocityY, initialAngularVelocityZ)); |
| 112 | + dynamicsWorld->addRigidBody(body); |
| 113 | + } |
| 114 | + |
| 115 | + // Main loop. |
| 116 | + std::printf(COMMON_PRINTF_HEADER "\n"); |
| 117 | + for (std::remove_const<decltype(nSteps)>::type step = 0; step < nSteps; ++step) { |
| 118 | + dynamicsWorld->stepSimulation(timeStep); |
| 119 | + auto nCollisionObjects = dynamicsWorld->getNumCollisionObjects(); |
| 120 | + for (decltype(nCollisionObjects) objectIndex = 0; objectIndex < nCollisionObjects; ++objectIndex) { |
| 121 | + btRigidBody *body = btRigidBody::upcast(dynamicsWorld->getCollisionObjectArray()[objectIndex]); |
| 122 | + commonPrintBodyState(body, step, objectIndex); |
| 123 | + } |
| 124 | + std::printf("\n"); |
| 125 | + } |
| 126 | + |
| 127 | + // Cleanup. |
| 128 | + for (int i = dynamicsWorld->getNumCollisionObjects() - 1; i >= 0; --i) { |
| 129 | + btCollisionObject* obj = dynamicsWorld->getCollisionObjectArray()[i]; |
| 130 | + btRigidBody* body = btRigidBody::upcast(obj); |
| 131 | + if (body && body->getMotionState()) { |
| 132 | + delete body->getMotionState(); |
| 133 | + } |
| 134 | + dynamicsWorld->removeCollisionObject(obj); |
| 135 | + delete obj; |
| 136 | + } |
| 137 | + for (int i = 0; i < collisionShapes.size(); ++i) { |
| 138 | + delete collisionShapes[i]; |
| 139 | + } |
| 140 | + delete dynamicsWorld; |
| 141 | + delete solver; |
| 142 | + delete overlappingPairCache; |
| 143 | + delete dispatcher; |
| 144 | + delete collisionConfiguration; |
| 145 | + collisionShapes.clear(); |
| 146 | +} |
0 commit comments