Revision: 46729 http://brlcad.svn.sourceforge.net/brlcad/?rev=46729&view=rev Author: abhi2011 Date: 2011-09-16 06:07:40 +0000 (Fri, 16 Sep 2011) Log Message: ----------- Began rt integration into the bullet collision pipeline, added callback to show aabb overlaps and contact points
Modified Paths: -------------- brlcad/trunk/src/libged/CMakeLists.txt brlcad/trunk/src/libged/simulate/simphysics.cpp brlcad/trunk/src/libged/simulate/simulate.c Modified: brlcad/trunk/src/libged/CMakeLists.txt =================================================================== --- brlcad/trunk/src/libged/CMakeLists.txt 2011-09-16 05:02:51 UTC (rev 46728) +++ brlcad/trunk/src/libged/CMakeLists.txt 2011-09-16 06:07:40 UTC (rev 46729) @@ -22,10 +22,10 @@ # verification (sh/cmakecheck.sh) will fail on dist-hook. IF(BULLET_FOUND) GED_ADD_CMD(LIBGED_SIM_SOURCES BULLET_INCLUDE_DIR BULLET_LIBRARIES) - SET(SIM_SRCS simulate/simphysics.cpp simulate/simulate.c) + SET(SIM_SRCS simulate/simphysics.cpp simulate/simulate.c simulate/simcollisionalgo.cpp simulate/simcollisionalgo.h) ELSE(BULLET_FOUND) SET(SIM_SRCS "") - SET(ged_ignore_files simulate/simphysics.cpp simulate/simulate.c) + SET(ged_ignore_files simulate/simphysics.cpp simulate/simulate.c simulate/simcollisionalgo.cpp simulate/simcollisionalgo.h) ENDIF(BULLET_FOUND) set(LIBGED_SOURCES @@ -336,6 +336,7 @@ ged_private.h qray.h simulate/simulate.h + simulate/simcollisionalgo.h wdb_qray.h ) CMAKEFILES(${ged_ignore_files}) Modified: brlcad/trunk/src/libged/simulate/simphysics.cpp =================================================================== --- brlcad/trunk/src/libged/simulate/simphysics.cpp 2011-09-16 05:02:51 UTC (rev 46728) +++ brlcad/trunk/src/libged/simulate/simphysics.cpp 2011-09-16 06:07:40 UTC (rev 46729) @@ -35,9 +35,13 @@ #include "db.h" #include "vmath.h" #include "simulate.h" +#include "simcollisionalgo.h" #include <btBulletDynamicsCommon.h> + +//--------------------- Printing functions for debugging ------------------------ + /** * Prints the 16 by 16 transform matrices for debugging * @@ -70,7 +74,104 @@ } +//--------------------- 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; + //proxy0->m_clientObject + + //bu_log("broadphase_callback: These 2 objects have overlapping AABBs"); + + //add some additional logic here that modified '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); +} + + +//--------------------- Physics simulation management -------------------------- + +/** * Adds rigid bodies to the dynamics world from the BRL-CAD geometry, * will add a ground plane if a region by the name of sim_gp.r is detected * The plane will be static and have the same dimensions as the region @@ -155,16 +256,21 @@ int step_physics(btDiscreteDynamicsWorld* dynamicsWorld, struct simulation_params *sim_params) { int i; - 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_log("Simulation will run for %d steps.\n", sim_params->duration); + bu_log("----- Starting simulation -----\n"); for (i=0 ; i < sim_params->duration ; i++) { + //bu_log("---------------Step : %d -------------\n", i+1); + //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); + + /* Modify collision points after narrowphase collisions */ + //narrowphase_collisions(dynamicsWorld); } - bu_vls_printf(sim_params->result_str, "----- Simulation Complete -----\n"); + bu_log("----- Simulation Complete -----\n"); return 0; } @@ -298,9 +404,22 @@ // Keep the collision shapes, for deletion/cleanup btAlignedObjectArray<btCollisionShape*> collision_shapes; + // Setup broadphase collision algo btBroadphaseInterface* broadphase = new btDbvtBroadphase(); + + //Rest of the setup towards a dynamics world creation btDefaultCollisionConfiguration* collisionConfiguration = new btDefaultCollisionConfiguration(); btCollisionDispatcher* dispatcher = new btCollisionDispatcher(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); + btSequentialImpulseConstraintSolver* solver = new btSequentialImpulseConstraintSolver; dynamicsWorld = new btDiscreteDynamicsWorld(dispatcher,broadphase,solver,collisionConfiguration); @@ -311,16 +430,23 @@ //Add the rigid bodies to the world, including the ground plane add_rigid_bodies(dynamicsWorld, sim_params, collision_shapes); + //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); + //Step the physics the required number of times step_physics(dynamicsWorld, sim_params); - //Get the world transforms back into the simulation params struct + //Get the world transforms & AABBs 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 + //Clean up stuff in this function delete solver; delete dispatcher; delete collisionConfiguration; Modified: brlcad/trunk/src/libged/simulate/simulate.c =================================================================== --- brlcad/trunk/src/libged/simulate/simulate.c 2011-09-16 05:02:51 UTC (rev 46728) +++ brlcad/trunk/src/libged/simulate/simulate.c 2011-09-16 06:07:40 UTC (rev 46729) @@ -22,7 +22,8 @@ * The simulate command. * * Routines related to performing physics on passed objects only - * + * TODO : Adds flags to control AABB and object state display + * TODO : Finish integrating rt into collision detection * */ @@ -204,7 +205,7 @@ continue; if (strstr(dp->d_namep, prefix)){ - bu_vls_printf(gedp->ged_result_str, "add_regions: Skipping \"%s\" due to \"%s\" in name\n", + bu_log("add_regions: Skipping \"%s\" due to \"%s\" in name\n", dp->d_namep, prefix); continue; } @@ -217,22 +218,22 @@ bu_strlcat(prefixed_name + prefix_len, dp->d_namep, prefixed_name_len - prefix_len); kill_copy(gedp, dp, prefixed_name); - bu_vls_printf(gedp->ged_result_str, "add_regions: Copied \"%s\" to \"%s\"\n", dp->d_namep, prefixed_name); + bu_log("add_regions: Copied \"%s\" to \"%s\"\n", dp->d_namep, prefixed_name); /* Get the directory pointer for the object just added */ if ((ndp=db_lookup(gedp->ged_wdbp->dbip, prefixed_name, LOOKUP_QUIET)) == RT_DIR_NULL) { - bu_vls_printf(gedp->ged_result_str, "add_regions: db_lookup(%s) failed", prefixed_name); + bu_log("add_regions: db_lookup(%s) failed", prefixed_name); return GED_ERROR; } /* Get its BB */ if(rt_bound_internal(gedp->ged_wdbp->dbip, ndp, rpp_min, rpp_max) == 0) - bu_vls_printf(gedp->ged_result_str, "add_regions: Got the BB for \"%s\" as \ - min {%f %f %f} max {%f %f %f}\n", ndp->d_namep, + bu_log("add_regions: Got the BB for \"%s\" as \ + min {%f %f %f} max {%f %f %f}\n", ndp->d_namep, rpp_min[0], rpp_min[1], rpp_min[2], rpp_max[0], rpp_max[1], rpp_max[2]); else{ - bu_vls_printf(gedp->ged_result_str, "add_regions: ERROR Could not get the BB\n"); + bu_log("add_regions: ERROR Could not get the BB\n"); return GED_ERROR; } @@ -250,7 +251,7 @@ current_node->bb_dims[1] = current_node->bb_max[1] - current_node->bb_min[1]; current_node->bb_dims[2] = current_node->bb_max[2] - current_node->bb_min[2]; - bu_vls_printf(gedp->ged_result_str, "add_regions: Dimensions of this BB : %f %f %f\n", + bu_log("add_regions: Dimensions of this BB : %f %f %f\n", current_node->bb_dims[0], current_node->bb_dims[1], current_node->bb_dims[2]); /* Get BB position in 3D space */ @@ -510,7 +511,7 @@ struct rt_db_internal intern; struct rigid_body *current_node; mat_t t , m; - /*int rv;*/ + /* int rv; */ for (current_node = sim_params->head_node; current_node != NULL; current_node = current_node->next) { @@ -567,7 +568,7 @@ } /* Apply the proper shader to match the object state : useful for debugging */ -/* switch(current_node->state){ + /* switch(current_node->state){ case ACTIVE_TAG: rv = apply_color(gedp, current_node->rb_namep, 255, 255, 0); break; @@ -596,7 +597,7 @@ bu_vls_printf(gedp->ged_result_str, "apply_transforms: WARNING Could not set \ the state color for %s\n", current_node->rb_namep); } -*/ + */ /* This will be enabled by a flag later */ /* insertAABB(gedp, sim_params, current_node); */ This was sent by the SourceForge.net collaborative development platform, the world's largest Open Source development site. ------------------------------------------------------------------------------ BlackBerry® DevCon Americas, Oct. 18-20, San Francisco, CA http://p.sf.net/sfu/rim-devcon-copy2 _______________________________________________ BRL-CAD Source Commits mailing list brlcad-commits@lists.sourceforge.net https://lists.sourceforge.net/lists/listinfo/brlcad-commits