[brlcad-commits] SF.net SVN: brlcad:[47008] brlcad/trunk/src/libged/simulate
Open Source Solid Modeling CAD
Brought to you by:
brlcad
From: <abh...@us...> - 2011-10-01 11:40:29
|
Revision: 47008 http://brlcad.svn.sourceforge.net/brlcad/?rev=47008&view=rev Author: abhi2011 Date: 2011-10-01 11:40:20 +0000 (Sat, 01 Oct 2011) Log Message: ----------- Added code to check the collision points during collision processing Modified Paths: -------------- brlcad/trunk/src/libged/simulate/simcollisionalgo.cpp brlcad/trunk/src/libged/simulate/simphysics.cpp brlcad/trunk/src/libged/simulate/simulate.c Modified: brlcad/trunk/src/libged/simulate/simcollisionalgo.cpp =================================================================== --- brlcad/trunk/src/libged/simulate/simcollisionalgo.cpp 2011-09-30 23:57:07 UTC (rev 47007) +++ brlcad/trunk/src/libged/simulate/simcollisionalgo.cpp 2011-10-01 11:40:20 UTC (rev 47008) @@ -79,8 +79,6 @@ btBoxShape* box0 = (btBoxShape*)col0->getCollisionShape(); btBoxShape* box1 = (btBoxShape*)col1->getCollisionShape(); - - /// report a contact. internally this will be kept persistent, and contact reduction is done resultOut->setPersistentManifold(m_manifoldPtr); #ifndef USE_PERSISTENT_CONTACTS @@ -96,6 +94,43 @@ btBoxBoxDetector detector(box0,box1); detector.getClosestPoints(input,*resultOut,dispatchInfo.m_debugDraw); + + + //------------------- DEBUG --------------------------- + + //Get the user pointers to struct rigid_body, for printing the body name + struct rigid_body *upA = (struct rigid_body *)col0->getUserPointer(); + struct rigid_body *upB = (struct rigid_body *)col1->getUserPointer(); + + if(upA != NULL && upB != NULL){ + + btPersistentManifold* contactManifold = resultOut->getPersistentManifold(); + + bu_log("processCollision(box/box): %s & %s \n", upA->rb_namep, upB->rb_namep); + + //Get the number of points in this manifold + int numContacts = contactManifold->getNumContacts(); + int j; + + bu_log("processCollision : Manifold contacts : %d\n", numContacts); + + //Iterate over the points for this manifold + for (j=0;j<numContacts;j++){ + btManifoldPoint& pt = contactManifold->getContactPoint(j); + + btVector3 ptA = pt.getPositionWorldOnA(); + btVector3 ptB = pt.getPositionWorldOnB(); + + bu_log("processCollision: contact %d of %d, %s(%f, %f, %f) , %s(%f, %f, %f)\n", + j+1, numContacts, + upA->rb_namep, ptA[0], ptA[1], ptA[2], + upB->rb_namep, ptB[0], ptB[1], ptB[2]); + } + } + + + //------------------------------------------------------ + #ifdef USE_PERSISTENT_CONTACTS // refreshContactPoints is only necessary when using persistent contact points. otherwise all points are newly added if (m_ownManifold) Modified: brlcad/trunk/src/libged/simulate/simphysics.cpp =================================================================== --- brlcad/trunk/src/libged/simulate/simphysics.cpp 2011-09-30 23:57:07 UTC (rev 47007) +++ brlcad/trunk/src/libged/simulate/simphysics.cpp 2011-10-01 11:40:20 UTC (rev 47008) @@ -37,6 +37,7 @@ #include "simulate.h" #include <btBulletDynamicsCommon.h> +#include "simcollisionalgo.h" /** * Prints the 16 by 16 transform matrices for debugging @@ -44,28 +45,28 @@ */ void print_matrices(struct simulation_params *sim_params, char *rb_namep, mat_t t, btScalar *m) { - int i, j; + int i, j; - bu_vls_printf(sim_params->result_str, "------------Transformation matrices(%s)--------------\n", - rb_namep); + bu_vls_printf(sim_params->result_str, "------------Transformation matrices(%s)--------------\n", + rb_namep); - for (i=0 ; i<4 ; i++) { - for (j=0 ; j<4 ; j++) { - bu_vls_printf(sim_params->result_str, "t[%d]: %f\t", (j*4 + i), t[j*4 + i] ); - } - bu_vls_printf(sim_params->result_str, "\n"); - } + for (i=0 ; i<4 ; i++) { + for (j=0 ; j<4 ; j++) { + bu_vls_printf(sim_params->result_str, "t[%d]: %f\t", (j*4 + i), t[j*4 + i] ); + } + bu_vls_printf(sim_params->result_str, "\n"); + } - bu_vls_printf(sim_params->result_str, "\n"); + bu_vls_printf(sim_params->result_str, "\n"); - for (i=0 ; i<4 ; i++) { - for (j=0 ; j<4 ; j++) { - bu_vls_printf(sim_params->result_str, "m[%d]: %f\t", (j*4 + i), m[j*4 + i] ); - } - bu_vls_printf(sim_params->result_str, "\n"); - } + for (i=0 ; i<4 ; i++) { + for (j=0 ; j<4 ; j++) { + bu_vls_printf(sim_params->result_str, "m[%d]: %f\t", (j*4 + i), m[j*4 + i] ); + } + bu_vls_printf(sim_params->result_str, "\n"); + } - bu_vls_printf(sim_params->result_str, "-------------------------------------------------------\n"); + bu_vls_printf(sim_params->result_str, "-------------------------------------------------------\n"); } @@ -77,99 +78,99 @@ * */ int add_rigid_bodies(btDiscreteDynamicsWorld* dynamicsWorld, struct simulation_params *sim_params, - btAlignedObjectArray<btCollisionShape*> collision_shapes) + btAlignedObjectArray<btCollisionShape*> collision_shapes) { - struct rigid_body *current_node; - fastf_t volume; - btScalar mass; - btScalar m[16]; - btVector3 v; + struct rigid_body *current_node; + fastf_t volume; + btScalar mass; + btScalar m[16]; + btVector3 v; - for (current_node = sim_params->head_node; current_node != NULL; current_node = current_node->next) { + for (current_node = sim_params->head_node; current_node != NULL; current_node = current_node->next) { - // Check if we should add a ground plane - if(strcmp(current_node->rb_namep, sim_params->ground_plane_name) == 0){ - // Add a static ground plane : should be controlled by an option : TODO - btCollisionShape* groundShape = new btBoxShape(btVector3(current_node->bb_dims[0]/2, - current_node->bb_dims[1]/2, - current_node->bb_dims[2]/2)); - /*btDefaultMotionState* groundMotionState = new btDefaultMotionState( - btTransform(btQuaternion(0,0,0,1), - btVector3(current_node->bb_center[0], - current_node->bb_center[1], - current_node->bb_center[2])));*/ + // Check if we should add a ground plane + if(strcmp(current_node->rb_namep, sim_params->ground_plane_name) == 0){ + // Add a static ground plane : should be controlled by an option : TODO + btCollisionShape* groundShape = new btBoxShape(btVector3(current_node->bb_dims[0]/2, + current_node->bb_dims[1]/2, + current_node->bb_dims[2]/2)); + /*btDefaultMotionState* groundMotionState = new btDefaultMotionState( + btTransform(btQuaternion(0,0,0,1), + btVector3(current_node->bb_center[0], + current_node->bb_center[1], + current_node->bb_center[2])));*/ - btDefaultMotionState* groundMotionState = new btDefaultMotionState(btTransform(btQuaternion(0,0,0,1), - btVector3(0, 0, 0))); + btDefaultMotionState* groundMotionState = new btDefaultMotionState(btTransform(btQuaternion(0,0,0,1), + btVector3(0, 0, 0))); - //Copy the transform matrix - MAT_COPY(m, current_node->m); - groundMotionState->m_graphicsWorldTrans.setFromOpenGLMatrix(m); + //Copy the transform matrix + MAT_COPY(m, current_node->m); + groundMotionState->m_graphicsWorldTrans.setFromOpenGLMatrix(m); - /* btCollisionShape* groundShape = new btStaticPlaneShape(btVector3(0,0,1),1); - btDefaultMotionState* groundMotionState = - new btDefaultMotionState(btTransform(btQuaternion(0,0,0,1),btVector3(0,0,-1)));*/ + /* btCollisionShape* groundShape = new btStaticPlaneShape(btVector3(0,0,1),1); + btDefaultMotionState* groundMotionState = + new btDefaultMotionState(btTransform(btQuaternion(0,0,0,1),btVector3(0,0,-1)));*/ - btRigidBody::btRigidBodyConstructionInfo - groundRigidBodyCI(0,groundMotionState,groundShape,btVector3(0,0,0)); - btRigidBody* groundRigidBody = new btRigidBody(groundRigidBodyCI); - groundRigidBody->setUserPointer((void *)current_node); + btRigidBody::btRigidBodyConstructionInfo + groundRigidBodyCI(0,groundMotionState,groundShape,btVector3(0,0,0)); + btRigidBody* groundRigidBody = new btRigidBody(groundRigidBodyCI); + groundRigidBody->setUserPointer((void *)current_node); - dynamicsWorld->addRigidBody(groundRigidBody); - collision_shapes.push_back(groundShape); + dynamicsWorld->addRigidBody(groundRigidBody); + collision_shapes.push_back(groundShape); - bu_vls_printf(sim_params->result_str, "Added static ground plane : %s to simulation with mass %f Kg\n", - current_node->rb_namep, 0.f); + bu_vls_printf(sim_params->result_str, "Added static ground plane : %s to simulation with mass %f Kg\n", + current_node->rb_namep, 0.f); - } - else{ - //Nope, its a rigid body - btCollisionShape* bb_Shape = new btBoxShape(btVector3(current_node->bb_dims[0]/2, - current_node->bb_dims[1]/2, - current_node->bb_dims[2]/2)); - collision_shapes.push_back(bb_Shape); + } + else{ + //Nope, its a rigid body + btCollisionShape* bb_Shape = new btBoxShape(btVector3(current_node->bb_dims[0]/2, + current_node->bb_dims[1]/2, + current_node->bb_dims[2]/2)); + collision_shapes.push_back(bb_Shape); - volume = current_node->bb_dims[0] * current_node->bb_dims[1] * current_node->bb_dims[2]; - mass = volume; // density is 1 + volume = current_node->bb_dims[0] * current_node->bb_dims[1] * current_node->bb_dims[2]; + mass = volume; // density is 1 - btVector3 bb_Inertia(0,0,0); - bb_Shape->calculateLocalInertia(mass, bb_Inertia); + btVector3 bb_Inertia(0,0,0); + bb_Shape->calculateLocalInertia(mass, bb_Inertia); - /* btDefaultMotionState* bb_MotionState = new btDefaultMotionState(btTransform(btQuaternion(0,0,0,1), - btVector3(current_node->bb_center[0], - current_node->bb_center[1], - current_node->bb_center[2])));*/ - btDefaultMotionState* bb_MotionState = new btDefaultMotionState(btTransform(btQuaternion(0,0,0,1), - btVector3(0, 0, 0))); + /* btDefaultMotionState* bb_MotionState = new btDefaultMotionState(btTransform(btQuaternion(0,0,0,1), + btVector3(current_node->bb_center[0], + current_node->bb_center[1], + current_node->bb_center[2])));*/ + btDefaultMotionState* bb_MotionState = new btDefaultMotionState(btTransform(btQuaternion(0,0,0,1), + btVector3(0, 0, 0))); - //Copy the transform matrix - MAT_COPY(m, current_node->m); - bb_MotionState->m_graphicsWorldTrans.setFromOpenGLMatrix(m); + //Copy the transform matrix + MAT_COPY(m, current_node->m); + bb_MotionState->m_graphicsWorldTrans.setFromOpenGLMatrix(m); - btRigidBody::btRigidBodyConstructionInfo - bb_RigidBodyCI(mass, bb_MotionState, bb_Shape, bb_Inertia); - btRigidBody* bb_RigidBody = new btRigidBody(bb_RigidBodyCI); - bb_RigidBody->setUserPointer((void *)current_node); + btRigidBody::btRigidBodyConstructionInfo + bb_RigidBodyCI(mass, bb_MotionState, bb_Shape, bb_Inertia); + btRigidBody* bb_RigidBody = new btRigidBody(bb_RigidBodyCI); + bb_RigidBody->setUserPointer((void *)current_node); - VMOVE(v, current_node->linear_velocity); - bb_RigidBody->setLinearVelocity(v); + VMOVE(v, current_node->linear_velocity); + bb_RigidBody->setLinearVelocity(v); - VMOVE(v, current_node->angular_velocity); - bb_RigidBody->setAngularVelocity(v); + VMOVE(v, current_node->angular_velocity); + bb_RigidBody->setAngularVelocity(v); - dynamicsWorld->addRigidBody(bb_RigidBody); + dynamicsWorld->addRigidBody(bb_RigidBody); - bu_vls_printf(sim_params->result_str, "Added new rigid body : %s to simulation with mass %f Kg\n", - current_node->rb_namep, mass); + bu_vls_printf(sim_params->result_str, "Added new rigid body : %s to simulation with mass %f Kg\n", + current_node->rb_namep, mass); - } + } - } + } - return 0; + return 0; } /** @@ -178,14 +179,14 @@ */ int step_physics(btDiscreteDynamicsWorld* dynamicsWorld, struct simulation_params *sim_params) { - bu_vls_printf(sim_params->result_str, "Simulation will run for %d steps.\n", sim_params->duration); - bu_vls_printf(sim_params->result_str, "----- Starting simulation -----\n"); + bu_vls_printf(sim_params->result_str, "Simulation will run for %d steps.\n", sim_params->duration); + bu_vls_printf(sim_params->result_str, "----- Starting simulation -----\n"); - //time step of 1/60th of a second(same as internal fixedTimeStep, maxSubSteps=10 to cover 1/60th sec.) - dynamicsWorld->stepSimulation(1/60.f,10); + //time step of 1/60th of a second(same as internal fixedTimeStep, maxSubSteps=10 to cover 1/60th sec.) + dynamicsWorld->stepSimulation(1/60.f,10); - bu_vls_printf(sim_params->result_str, "----- Simulation Complete -----\n"); - return 0; + bu_vls_printf(sim_params->result_str, "----- Simulation Complete -----\n"); + return 0; } @@ -195,71 +196,71 @@ */ int get_transforms(btDiscreteDynamicsWorld* dynamicsWorld, struct simulation_params *sim_params) { - int i; - btScalar m[16]; - btVector3 aabbMin, aabbMax, v; - btTransform identity; + int i; + btScalar m[16]; + btVector3 aabbMin, aabbMax, v; + btTransform identity; - identity.setIdentity(); - const int num_bodies = dynamicsWorld->getNumCollisionObjects(); + identity.setIdentity(); + const int num_bodies = dynamicsWorld->getNumCollisionObjects(); - for(i=0; i < num_bodies; i++){ + for(i=0; i < num_bodies; i++){ - //Common properties among all rigid bodies - btCollisionObject* bb_ColObj = dynamicsWorld->getCollisionObjectArray()[i]; - btRigidBody* bb_RigidBody = btRigidBody::upcast(bb_ColObj); - const btCollisionShape* bb_Shape = bb_ColObj->getCollisionShape(); //may be used later + //Common properties among all rigid bodies + btCollisionObject* bb_ColObj = dynamicsWorld->getCollisionObjectArray()[i]; + btRigidBody* bb_RigidBody = btRigidBody::upcast(bb_ColObj); + const btCollisionShape* bb_Shape = bb_ColObj->getCollisionShape(); //may be used later - if( bb_RigidBody && bb_RigidBody->getMotionState()){ + if( bb_RigidBody && bb_RigidBody->getMotionState()){ - //Get the motion state and the world transform from it - btDefaultMotionState* bb_MotionState = (btDefaultMotionState*)bb_RigidBody->getMotionState(); - bb_MotionState->m_graphicsWorldTrans.getOpenGLMatrix(m); - //bu_vls_printf(sim_params->result_str, "Position : %f, %f, %f\n", m[12], m[13], m[14]); + //Get the motion state and the world transform from it + btDefaultMotionState* bb_MotionState = (btDefaultMotionState*)bb_RigidBody->getMotionState(); + bb_MotionState->m_graphicsWorldTrans.getOpenGLMatrix(m); + //bu_vls_printf(sim_params->result_str, "Position : %f, %f, %f\n", m[12], m[13], m[14]); - struct rigid_body *current_node = (struct rigid_body *)bb_RigidBody->getUserPointer(); + struct rigid_body *current_node = (struct rigid_body *)bb_RigidBody->getUserPointer(); - if(current_node == NULL){ - bu_vls_printf(sim_params->result_str, "get_transforms : Could not get the user pointer \ - (ground plane perhaps)\n"); - continue; + if(current_node == NULL){ + bu_vls_printf(sim_params->result_str, "get_transforms : Could not get the user pointer \ + (ground plane perhaps)\n"); + continue; - } + } - //Copy the transform matrix - MAT_COPY(current_node->m, m); + //Copy the transform matrix + MAT_COPY(current_node->m, m); - //print_matrices(sim_params, current_node->rb_namep, current_node->m, m); + //print_matrices(sim_params, current_node->rb_namep, current_node->m, m); - //Get the state of the body - current_node->state = bb_RigidBody->getActivationState(); + //Get the state of the body + current_node->state = bb_RigidBody->getActivationState(); - //Get the AABB - bb_Shape->getAabb(bb_MotionState->m_graphicsWorldTrans, aabbMin, aabbMax); + //Get the AABB + bb_Shape->getAabb(bb_MotionState->m_graphicsWorldTrans, aabbMin, aabbMax); - VMOVE(current_node->btbb_min, aabbMin); - VMOVE(current_node->btbb_max, aabbMax); + VMOVE(current_node->btbb_min, aabbMin); + VMOVE(current_node->btbb_max, aabbMax); - // Get BB length, width, height - VSUB2(current_node->btbb_dims, current_node->btbb_max, current_node->btbb_min); + // Get BB length, width, height + VSUB2(current_node->btbb_dims, current_node->btbb_max, current_node->btbb_min); - bu_vls_printf(sim_params->result_str, "get_transforms: Dimensions of this BB : %f %f %f\n", - current_node->btbb_dims[0], current_node->btbb_dims[1], current_node->btbb_dims[2]); + bu_vls_printf(sim_params->result_str, "get_transforms: Dimensions of this BB : %f %f %f\n", + current_node->btbb_dims[0], current_node->btbb_dims[1], current_node->btbb_dims[2]); - //Get BB position in 3D space - VCOMB2(current_node->btbb_center, 1, current_node->btbb_min, 0.5, current_node->btbb_dims) + //Get BB position in 3D space + VCOMB2(current_node->btbb_center, 1, current_node->btbb_min, 0.5, current_node->btbb_dims) - v = bb_RigidBody->getLinearVelocity(); - VMOVE(current_node->linear_velocity, v); + v = bb_RigidBody->getLinearVelocity(); + VMOVE(current_node->linear_velocity, v); - v = bb_RigidBody->getAngularVelocity(); - VMOVE(current_node->angular_velocity, v); + v = bb_RigidBody->getAngularVelocity(); + VMOVE(current_node->angular_velocity, v); - } - } + } + } - return 0; + return 0; } @@ -268,84 +269,204 @@ * */ int cleanup(btDiscreteDynamicsWorld* dynamicsWorld, - btAlignedObjectArray<btCollisionShape*> collision_shapes) + btAlignedObjectArray<btCollisionShape*> collision_shapes) { - //remove the rigid bodies from the dynamics world and delete them - int i; + //remove the rigid bodies from the dynamics world and delete them + int i; - for (i=dynamicsWorld->getNumCollisionObjects()-1; i>=0; i--){ + for (i=dynamicsWorld->getNumCollisionObjects()-1; i>=0; i--){ - btCollisionObject* obj = dynamicsWorld->getCollisionObjectArray()[i]; - btRigidBody* body = btRigidBody::upcast(obj); - if (body && body->getMotionState()){ - delete body->getMotionState(); - } - dynamicsWorld->removeCollisionObject( obj ); - delete obj; - } + btCollisionObject* obj = dynamicsWorld->getCollisionObjectArray()[i]; + btRigidBody* body = btRigidBody::upcast(obj); + if (body && body->getMotionState()){ + delete body->getMotionState(); + } + dynamicsWorld->removeCollisionObject( obj ); + delete obj; + } - //delete collision shapes - for (i=0; i<collision_shapes.size(); i++) - { - btCollisionShape* shape = collision_shapes[i]; - delete shape; - } + //delete collision shapes + for (i=0; i<collision_shapes.size(); i++) + { + btCollisionShape* shape = collision_shapes[i]; + delete shape; + } - //delete dynamics world - delete dynamicsWorld; + //delete dynamics world + delete dynamicsWorld; - return 0; + return 0; } +//--------------------- Collision specific code ------------------------ + /** + * Broadphase filter callback struct : used to show the detected AABB overlaps + * + */ +struct broadphase_callback : public btOverlapFilterCallback +{ + //Return true when pairs need collision + virtual bool + needBroadphaseCollision(btBroadphaseProxy* proxy0, btBroadphaseProxy* proxy1) const + { + bool collides = (proxy0->m_collisionFilterGroup & proxy1->m_collisionFilterMask) != 0; + collides = collides && (proxy1->m_collisionFilterGroup & proxy0->m_collisionFilterMask); + + //This would prevent collision between proxy0 and proxy1 inspite of + //AABB overlap being detected + //collides = false; + btRigidBody* box0 = (btRigidBody*)proxy0->m_clientObject; + btRigidBody* box1 = (btRigidBody*)proxy1->m_clientObject; + if(box0 != NULL && box0 != NULL){ + struct rigid_body *upA = (struct rigid_body *)box0->getUserPointer(); + struct rigid_body *upB = (struct rigid_body *)box1->getUserPointer(); + + if(upA != NULL && upB != NULL) + bu_log("broadphase_callback: %s & %s has overlapping AABBs", + upA->rb_namep, upB->rb_namep); + } + + //add some additional logic here that modifies 'collides' + return collides; + } +}; + + +/** + * Narrowphase filter callback : used to show the generated collision points + * + */ +void nearphase_callback(btBroadphasePair& collisionPair, + btCollisionDispatcher& dispatcher, + btDispatcherInfo& dispatchInfo) +{ + + int i, j, numContacts; + int numManifolds = dispatcher.getNumManifolds(); + + /* First iterate through the number of manifolds for the whole dynamics world + * A manifold is a set of contact points containing upto 4 points + * There may be multiple manifolds where objects touch + */ + for (i=0; i<numManifolds; i++){ + + //Get the manifold and the objects which created it + btPersistentManifold* contactManifold = + dispatcher.getManifoldByIndexInternal(i); + btCollisionObject* obA = static_cast<btCollisionObject*>(contactManifold->getBody0()); + btCollisionObject* obB = static_cast<btCollisionObject*>(contactManifold->getBody1()); + + //Get the user pointers to struct rigid_body, for printing the body name + btRigidBody* rbA = btRigidBody::upcast(obA); + btRigidBody* rbB = btRigidBody::upcast(obB); + struct rigid_body *upA = (struct rigid_body *)rbA->getUserPointer(); + struct rigid_body *upB = (struct rigid_body *)rbB->getUserPointer(); + + //Get the number of points in this manifold + numContacts = contactManifold->getNumContacts(); + + bu_log("nearphase_callback : Manifold %d of %d, contacts : %d\n", + i+1, + numManifolds, + numContacts); + + //Iterate over the points for this manifold + for (j=0;j<numContacts;j++){ + btManifoldPoint& pt = contactManifold->getContactPoint(j); + + btVector3 ptA = pt.getPositionWorldOnA(); + btVector3 ptB = pt.getPositionWorldOnB(); + + if(upA == NULL || upB == NULL){ + bu_log("nearphase_callback : contact %d of %d, could not get user pointers\n", + j+1, numContacts); + + } + else{ + bu_log("nearphase_callback: contact %d of %d, %s(%f, %f, %f) , %s(%f, %f, %f)\n", + j+1, numContacts, + upA->rb_namep, ptA[0], ptA[1], ptA[2], + upB->rb_namep, ptB[0], ptB[1], ptB[2]); + } + } + + //Can un-comment this line, and then all points are removed + //contactManifold->clearManifold(); + } + + // Only dispatch the Bullet collision information if physics should continue + dispatcher.defaultNearCallback(collisionPair, dispatcher, dispatchInfo); +} + + +/** * C++ wrapper for doing physics using bullet * */ extern "C" int run_simulation(struct simulation_params *sim_params) { - //int i; + //int i; - //for (i=0 ; i < sim_params->duration ; i++) { + //for (i=0 ; i < sim_params->duration ; i++) { - // Initialize the physics world - btDiscreteDynamicsWorld* dynamicsWorld; + // Initialize the physics world + btDiscreteDynamicsWorld* dynamicsWorld; - // Keep the collision shapes, for deletion/cleanup - btAlignedObjectArray<btCollisionShape*> collision_shapes; + // Keep the collision shapes, for deletion/cleanup + btAlignedObjectArray<btCollisionShape*> collision_shapes; - btBroadphaseInterface* broadphase = new btDbvtBroadphase(); - btDefaultCollisionConfiguration* collisionConfiguration = new btDefaultCollisionConfiguration(); - btCollisionDispatcher* dispatcher = new btCollisionDispatcher(collisionConfiguration); - btSequentialImpulseConstraintSolver* solver = new btSequentialImpulseConstraintSolver; + btBroadphaseInterface* broadphase = new btDbvtBroadphase(); + btDefaultCollisionConfiguration* collisionConfiguration = new btDefaultCollisionConfiguration(); + btCollisionDispatcher* dispatcher = new btCollisionDispatcher(collisionConfiguration); - dynamicsWorld = new btDiscreteDynamicsWorld(dispatcher,broadphase,solver,collisionConfiguration); + //Register custom rt based nearphase algo for box-box collision, + //arbitrary shapes from brlcad are all represented by the box collision shape + //in bullet, the movement will not be like a box however, but according to + //the collisions detected by rt and therefore should follow any arbitrary shape correctly + dispatcher->registerCollisionCreateFunc( + BOX_SHAPE_PROXYTYPE, + BOX_SHAPE_PROXYTYPE, + new btRTCollisionAlgorithm::CreateFunc); - //Set the gravity direction along -ve Z axis - dynamicsWorld->setGravity(btVector3(0, 0, -10)); - //Add the rigid bodies to the world, including the ground plane - add_rigid_bodies(dynamicsWorld, sim_params, collision_shapes); + btSequentialImpulseConstraintSolver* solver = new btSequentialImpulseConstraintSolver; - //Step the physics the required number of times - step_physics(dynamicsWorld, sim_params); + dynamicsWorld = new btDiscreteDynamicsWorld(dispatcher,broadphase,solver,collisionConfiguration); - //Get the world transforms back into the simulation params struct - get_transforms(dynamicsWorld, sim_params); + //Set the gravity direction along -ve Z axis + dynamicsWorld->setGravity(btVector3(0, 0, -10)); - //Clean and free memory used by physics objects - cleanup(dynamicsWorld, collision_shapes); + //Add the rigid bodies to the world, including the ground plane + add_rigid_bodies(dynamicsWorld, sim_params, collision_shapes); - //Clean up stuff in here - delete solver; - delete dispatcher; - delete collisionConfiguration; - delete broadphase; - //} + //Add a broadphase callback to hook to the AABB detection algos + btOverlapFilterCallback * filterCallback = new broadphase_callback(); + dynamicsWorld->getPairCache()->setOverlapFilterCallback(filterCallback); + //Add a nearphase callback to hook to the contact points generation algos + dispatcher->setNearCallback((btNearCallback)nearphase_callback); - return 0; + //Step the physics the required number of times + step_physics(dynamicsWorld, sim_params); + + //Get the world transforms back into the simulation params struct + get_transforms(dynamicsWorld, sim_params); + + //Clean and free memory used by physics objects + cleanup(dynamicsWorld, collision_shapes); + + //Clean up stuff in here + delete solver; + delete dispatcher; + delete collisionConfiguration; + delete broadphase; + //} + + + return 0; } #endif Modified: brlcad/trunk/src/libged/simulate/simulate.c =================================================================== --- brlcad/trunk/src/libged/simulate/simulate.c 2011-09-30 23:57:07 UTC (rev 47007) +++ brlcad/trunk/src/libged/simulate/simulate.c 2011-10-01 11:40:20 UTC (rev 47008) @@ -656,6 +656,8 @@ for (i=0 ; i < sim_params.duration ; i++) { + bu_log("%s: ------------------------- Iteration %d -----------------------\n", argv[0], i); + rv = get_bb(gedp, &sim_params); if (rv != GED_OK){ bu_vls_printf(gedp->ged_result_str, "%s: ERROR while getting bounding boxes\n", argv[0]); This was sent by the SourceForge.net collaborative development platform, the world's largest Open Source development site. |