Revision: 47037 http://brlcad.svn.sourceforge.net/brlcad/?rev=47037&view=rev Author: brlcad Date: 2011-10-03 20:12:29 +0000 (Mon, 03 Oct 2011) Log Message: ----------- more mass ws indent style comment cleanup. very hard to follow when even the file itself is so self-inconsistent. update copyright year to 2011 as year of inception.
Modified Paths: -------------- brlcad/trunk/src/libged/simulate/simcollisionalgo.cpp brlcad/trunk/src/libged/simulate/simcollisionalgo.h brlcad/trunk/src/libged/simulate/simphysics.cpp brlcad/trunk/src/libged/simulate/simulate.c brlcad/trunk/src/libged/simulate/simulate.h Modified: brlcad/trunk/src/libged/simulate/simcollisionalgo.cpp =================================================================== --- brlcad/trunk/src/libged/simulate/simcollisionalgo.cpp 2011-10-03 19:53:38 UTC (rev 47036) +++ brlcad/trunk/src/libged/simulate/simcollisionalgo.cpp 2011-10-03 20:12:29 UTC (rev 47037) @@ -1,7 +1,7 @@ -/* S I M C O L L I S I O N A L G O . C P P +/* S I M C O L L I S I O N A L G O . C P P * BRL-CAD * - * Copyright (c) 2008-2011 United States Government as represented by + * Copyright (c) 2011 United States Government as represented by * the U.S. Army Research Laboratory. * * This library is free software; you can redistribute it and/or @@ -17,13 +17,10 @@ * License along with this file; see the file named COPYING for more * information. */ -/** @file libged/simulate/simcollisionalgo.cpp - * - * - * Routines related to performing collision detection using rt - * This is a custom algorithm that replaces the box-box collision algorithm - * in bullet - * +/* + * Routines related to performing collision detection using rt. This + * is a custom algorithm that replaces the box-box collision algorithm + * in Bullet. */ #include "common.h" @@ -38,161 +35,145 @@ #include "BulletCollision/CollisionDispatch/btCollisionObject.h" #include "BulletCollision/CollisionDispatch/btBoxBoxDetector.h" + #define USE_PERSISTENT_CONTACTS 1 + btRTCollisionAlgorithm::btRTCollisionAlgorithm( - btPersistentManifold* mf, - const btCollisionAlgorithmConstructionInfo& ci, - btCollisionObject* obj0, - btCollisionObject* obj1) -: btActivatingCollisionAlgorithm(ci,obj0,obj1), -m_ownManifold(false), -m_manifoldPtr(mf) + btPersistentManifold* mf, + const btCollisionAlgorithmConstructionInfo& ci, + btCollisionObject* obj0, + btCollisionObject* obj1) + : btActivatingCollisionAlgorithm(ci, obj0, obj1), + m_ownManifold(false), + m_manifoldPtr(mf) { - if (!m_manifoldPtr && m_dispatcher->needsCollision(obj0,obj1)) - { - m_manifoldPtr = m_dispatcher->getNewManifold(obj0,obj1); - m_ownManifold = true; - } + if (!m_manifoldPtr && m_dispatcher->needsCollision(obj0, obj1)) { + m_manifoldPtr = m_dispatcher->getNewManifold(obj0, obj1); + m_ownManifold = true; + } } + btRTCollisionAlgorithm::~btRTCollisionAlgorithm() { - if (m_ownManifold) - { - if (m_manifoldPtr) - m_dispatcher->releaseManifold(m_manifoldPtr); - } + if (m_ownManifold) { + if (m_manifoldPtr) + m_dispatcher->releaseManifold(m_manifoldPtr); + } } -void btRTCollisionAlgorithm::processCollision ( - btCollisionObject* body0, - btCollisionObject* body1, - const btDispatcherInfo& dispatchInfo, - btManifoldResult* resultOut) + +void +btRTCollisionAlgorithm::processCollision ( + btCollisionObject* body0, + btCollisionObject* body1, + const btDispatcherInfo& dispatchInfo, + btManifoldResult* resultOut) { - if (!m_manifoldPtr) - return; + if (!m_manifoldPtr) + return; - btCollisionObject* col0 = body0; - btCollisionObject* col1 = body1; - btBoxShape* box0 = (btBoxShape*)col0->getCollisionShape(); - btBoxShape* box1 = (btBoxShape*)col1->getCollisionShape(); + btCollisionObject* col0 = body0; + btCollisionObject* col1 = body1; + 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); + /// report a contact. internally this will be kept persistent, and contact reduction is done + resultOut->setPersistentManifold(m_manifoldPtr); #ifndef USE_PERSISTENT_CONTACTS - m_manifoldPtr->clearManifold(); + m_manifoldPtr->clearManifold(); #endif //USE_PERSISTENT_CONTACTS - btDiscreteCollisionDetectorInterface::ClosestPointInput input; - input.m_maximumDistanceSquared = BT_LARGE_FLOAT; - input.m_transformA = body0->getWorldTransform(); - input.m_transformB = body1->getWorldTransform(); + btDiscreteCollisionDetectorInterface::ClosestPointInput input; + input.m_maximumDistanceSquared = BT_LARGE_FLOAT; + input.m_transformA = body0->getWorldTransform(); + input.m_transformB = body1->getWorldTransform(); - //This part will get replaced with a call to rt - btBoxBoxDetector detector(box0,box1); - detector.getClosestPoints(input,*resultOut,dispatchInfo.m_debugDraw); + //This part will get replaced with a call to rt + btBoxBoxDetector detector(box0, box1); + detector.getClosestPoints(input, *resultOut, dispatchInfo.m_debugDraw); + //------------------- DEBUG --------------------------- - //------------------- DEBUG --------------------------- - //Get the user pointers to struct rigid_body, for printing the body name - struct rigid_body *rbA = (struct rigid_body *)col0->getUserPointer(); - struct rigid_body *rbB = (struct rigid_body *)col1->getUserPointer(); + struct rigid_body *rbA = (struct rigid_body *)col0->getUserPointer(); + struct rigid_body *rbB = (struct rigid_body *)col1->getUserPointer(); - if(rbA != NULL && rbB != NULL){ + if (rbA != NULL && rbB != NULL) { - btPersistentManifold* contactManifold = resultOut->getPersistentManifold(); + btPersistentManifold* contactManifold = resultOut->getPersistentManifold(); - struct sim_manifold *current_manifold = - (struct sim_manifold *)bu_malloc(sizeof(struct sim_manifold), "sim_manifold: current_manifold"); - current_manifold->next = NULL; + struct sim_manifold *current_manifold = + (struct sim_manifold *)bu_malloc(sizeof(struct sim_manifold), "sim_manifold: current_manifold"); + current_manifold->next = NULL; - if(rbB->first_manifold == NULL){ - rbB->first_manifold = current_manifold; - } - else{ - //Go upto the last manifold, keeping a ptr 1 node behind - struct sim_manifold *p1 = rbB->first_manifold, *p2; - while(p1 != NULL){ - p2 = p1; - p1 = p1->next; - } + if (rbB->first_manifold == NULL) { + rbB->first_manifold = current_manifold; + } else{ + //Go upto the last manifold, keeping a ptr 1 node behind + struct sim_manifold *p1 = rbB->first_manifold, *p2; + while (p1 != NULL) { + p2 = p1; + p1 = p1->next; + } - p2->next = current_manifold; - //print_manifold_list(rb->first_manifold); - } - rbB->num_manifolds++; + p2->next = current_manifold; + //print_manifold_list(rb->first_manifold); + } + rbB->num_manifolds++; - bu_log("processCollision(box/box): %s & %s \n", rbA->rb_namep, rbB->rb_namep); + bu_log("processCollision(box/box): %s & %s \n", rbA->rb_namep, rbB->rb_namep); - //Get the number of points in this manifold - int num_contacts = contactManifold->getNumContacts(); - current_manifold->num_contacts = num_contacts; - int i; + //Get the number of points in this manifold + int num_contacts = contactManifold->getNumContacts(); + current_manifold->num_contacts = num_contacts; + int i; - bu_log("processCollision : Manifold contacts : %d\n", num_contacts); + bu_log("processCollision : Manifold contacts : %d\n", num_contacts); - //Iterate over the points for this manifold - for (i=0; i<num_contacts; i++){ - btManifoldPoint& pt = contactManifold->getContactPoint(i); + //Iterate over the points for this manifold + for (i=0; i<num_contacts; i++) { + btManifoldPoint& pt = contactManifold->getContactPoint(i); - btVector3 ptA = pt.getPositionWorldOnA(); - btVector3 ptB = pt.getPositionWorldOnB(); + btVector3 ptA = pt.getPositionWorldOnA(); + btVector3 ptB = pt.getPositionWorldOnB(); - VMOVE(current_manifold->rb_contacts[i].ptA, ptA); - VMOVE(current_manifold->rb_contacts[i].ptB, ptB); - VMOVE(current_manifold->rb_contacts[i].normalWorldOnB, pt.m_normalWorldOnB); + VMOVE(current_manifold->rb_contacts[i].ptA, ptA); + VMOVE(current_manifold->rb_contacts[i].ptB, ptB); + VMOVE(current_manifold->rb_contacts[i].normalWorldOnB, pt.m_normalWorldOnB); - bu_log("%d, %s(%f, %f, %f) , %s(%f, %f, %f), n(%f, %f, %f)\n", - i+1, - rbA->rb_namep, ptA[0], ptA[1], ptA[2], - rbB->rb_namep, ptB[0], ptB[1], ptB[2], - pt.m_normalWorldOnB[0], pt.m_normalWorldOnB[1], pt.m_normalWorldOnB[2]); + bu_log("%d, %s(%f, %f, %f) , %s(%f, %f, %f), n(%f, %f, %f)\n", + i+1, + rbA->rb_namep, ptA[0], ptA[1], ptA[2], + rbB->rb_namep, ptB[0], ptB[1], ptB[2], + pt.m_normalWorldOnB[0], pt.m_normalWorldOnB[1], pt.m_normalWorldOnB[2]); - } } + } - //------------------------------------------------------ + //------------------------------------------------------ #ifdef USE_PERSISTENT_CONTACTS - // refreshContactPoints is only necessary when using persistent contact points. otherwise all points are newly added - if (m_ownManifold) - { - resultOut->refreshContactPoints(); - } + // refreshContactPoints is only necessary when using persistent contact points. otherwise all points are newly added + if (m_ownManifold) { + resultOut->refreshContactPoints(); + } #endif //USE_PERSISTENT_CONTACTS } -btScalar btRTCollisionAlgorithm::calculateTimeOfImpact(btCollisionObject* /*body0*/,btCollisionObject* /*body1*/,const btDispatcherInfo& /*dispatchInfo*/,btManifoldResult* /*resultOut*/) + +btScalar +btRTCollisionAlgorithm::calculateTimeOfImpact(btCollisionObject* /*body0*/, btCollisionObject* /*body1*/, const btDispatcherInfo& /*dispatchInfo*/, btManifoldResult* /*resultOut*/) { - //not yet - return 1.f; + //not yet + return 1.f; } - - - - - - - - - - - - - - - - - - #endif // Local Variables: Modified: brlcad/trunk/src/libged/simulate/simcollisionalgo.h =================================================================== --- brlcad/trunk/src/libged/simulate/simcollisionalgo.h 2011-10-03 19:53:38 UTC (rev 47036) +++ brlcad/trunk/src/libged/simulate/simcollisionalgo.h 2011-10-03 20:12:29 UTC (rev 47037) @@ -1,7 +1,7 @@ -/* S I M C O L L I S I O N A L G O . H +/* S I M C O L L I S I O N A L G O . H * BRL-CAD * - * Copyright (c) 2008-2011 United States Government as represented by + * Copyright (c) 2011 United States Government as represented by * the U.S. Army Research Laboratory. * * This library is free software; you can redistribute it and/or @@ -17,11 +17,9 @@ * License along with this file; see the file named COPYING for more * information. */ -/** @file libged/simulate/simcollisionalgo.h - * - * - * Routines related to performing collision detection using rt - * This is a custom algorithm that replaces the box-box collision algorithm +/* + * Routines related to performing collision detection using rt. This + * is a custom algorithm that replaces the box-box collision algorithm * in bullet * */ @@ -52,39 +50,39 @@ ///Raytrace based collision detection class btRTCollisionAlgorithm : public btActivatingCollisionAlgorithm { - bool m_ownManifold; - btPersistentManifold* m_manifoldPtr; + bool m_ownManifold; + btPersistentManifold* m_manifoldPtr; public: - btRTCollisionAlgorithm(const btCollisionAlgorithmConstructionInfo& ci) - : btActivatingCollisionAlgorithm(ci) {} + btRTCollisionAlgorithm(const btCollisionAlgorithmConstructionInfo& ci) + : btActivatingCollisionAlgorithm(ci) {} - virtual void processCollision (btCollisionObject* body0,btCollisionObject* body1,const btDispatcherInfo& dispatchInfo,btManifoldResult* resultOut); + virtual void processCollision (btCollisionObject* body0, btCollisionObject* body1, const btDispatcherInfo& dispatchInfo, btManifoldResult* resultOut); - virtual btScalar calculateTimeOfImpact(btCollisionObject* body0,btCollisionObject* body1,const btDispatcherInfo& dispatchInfo,btManifoldResult* resultOut); + virtual btScalar calculateTimeOfImpact(btCollisionObject* body0, btCollisionObject* body1, const btDispatcherInfo& dispatchInfo, btManifoldResult* resultOut); - btRTCollisionAlgorithm(btPersistentManifold* mf,const btCollisionAlgorithmConstructionInfo& ci,btCollisionObject* body0,btCollisionObject* body1); + btRTCollisionAlgorithm(btPersistentManifold* mf, const btCollisionAlgorithmConstructionInfo& ci, btCollisionObject* body0, btCollisionObject* body1); - virtual ~btRTCollisionAlgorithm(); + virtual ~btRTCollisionAlgorithm(); - virtual void getAllContactManifolds(btManifoldArray& manifoldArray) - { - if (m_manifoldPtr && m_ownManifold) - { - manifoldArray.push_back(m_manifoldPtr); - } + virtual void + getAllContactManifolds(btManifoldArray& manifoldArray) + { + if (m_manifoldPtr && m_ownManifold) { + manifoldArray.push_back(m_manifoldPtr); } + } - struct CreateFunc :public btCollisionAlgorithmCreateFunc + struct CreateFunc : public btCollisionAlgorithmCreateFunc + { + virtual btCollisionAlgorithm* CreateCollisionAlgorithm(btCollisionAlgorithmConstructionInfo& ci, btCollisionObject* body0, btCollisionObject* body1) { - virtual btCollisionAlgorithm* CreateCollisionAlgorithm(btCollisionAlgorithmConstructionInfo& ci, btCollisionObject* body0,btCollisionObject* body1) - { - int bbsize = sizeof(btRTCollisionAlgorithm); - void* ptr = ci.m_dispatcher1->allocateCollisionAlgorithm(bbsize); - return new(ptr) btRTCollisionAlgorithm(0,ci,body0,body1); - } - }; + int bbsize = sizeof(btRTCollisionAlgorithm); + void* ptr = ci.m_dispatcher1->allocateCollisionAlgorithm(bbsize); + return new(ptr) btRTCollisionAlgorithm(0, ci, body0, body1); + } + }; }; Modified: brlcad/trunk/src/libged/simulate/simphysics.cpp =================================================================== --- brlcad/trunk/src/libged/simulate/simphysics.cpp 2011-10-03 19:53:38 UTC (rev 47036) +++ brlcad/trunk/src/libged/simulate/simphysics.cpp 2011-10-03 20:12:29 UTC (rev 47037) @@ -1,7 +1,7 @@ -/* S I M P H Y S I C S . C P P +/* S I M P H Y S I C S . C P P * BRL-CAD * - * Copyright (c) 2008-2011 United States Government as represented by + * Copyright (c) 2011 United States Government as represented by * the U.S. Army Research Laboratory. * * This library is free software; you can redistribute it and/or @@ -17,13 +17,8 @@ * License along with this file; see the file named COPYING for more * information. */ -/** @file libged/simulate/simphysics.cpp - * - * +/* * Routines related to performing physics on the passed regions - * - * - * */ #include "common.h" @@ -43,7 +38,8 @@ * Prints the 16 by 16 transform matrices for debugging * */ -void print_matrices(char *rb_namep, mat_t t, btScalar *m) +void +print_matrices(char *rb_namep, mat_t t, btScalar *m) { int i, j; char buffer[800]; @@ -53,7 +49,7 @@ for (i=0 ; i<4 ; i++) { for (j=0 ; j<4 ; j++) { - sprintf(buffer, "%st[%d]: %f\t", buffer, (j*4 + i), t[j*4 + i] ); + sprintf(buffer, "%st[%d]: %f\t", buffer, (j*4 + i), t[j*4 + i]); } sprintf(buffer, "%s\n", buffer); } @@ -62,7 +58,7 @@ for (i=0 ; i<4 ; i++) { for (j=0 ; j<4 ; j++) { - sprintf(buffer, "%sm[%d]: %f\t", buffer, (j*4 + i), m[j*4 + i] ); + sprintf(buffer, "%sm[%d]: %f\t", buffer, (j*4 + i), m[j*4 + i]); } sprintf(buffer, "%s\n", buffer); } @@ -79,8 +75,10 @@ * The plane will be static and have the same dimensions as the region * */ -int add_rigid_bodies(btDiscreteDynamicsWorld* dynamicsWorld, struct simulation_params *sim_params, - btAlignedObjectArray<btCollisionShape*> collision_shapes) +int +add_rigid_bodies(btDiscreteDynamicsWorld* dynamicsWorld, + struct simulation_params *sim_params, + btAlignedObjectArray<btCollisionShape*> collision_shapes) { struct rigid_body *current_node; fastf_t volume; @@ -89,34 +87,33 @@ btVector3 v; - 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){ + 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])));*/ + 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); - /* 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)); + groundRigidBodyCI(0, groundMotionState, groundShape, btVector3(0, 0, 0)); btRigidBody* groundRigidBody = new btRigidBody(groundRigidBodyCI); groundRigidBody->setUserPointer((void *)current_node); @@ -124,35 +121,34 @@ 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); + current_node->rb_namep, 0.f); - } - else{ + } 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)); + 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 - btVector3 bb_Inertia(0,0,0); + 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); btRigidBody::btRigidBodyConstructionInfo - bb_RigidBodyCI(mass, bb_MotionState, bb_Shape, bb_Inertia); + bb_RigidBodyCI(mass, bb_MotionState, bb_Shape, bb_Inertia); btRigidBody* bb_RigidBody = new btRigidBody(bb_RigidBodyCI); bb_RigidBody->setUserPointer((void *)current_node); @@ -166,7 +162,7 @@ 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); + current_node->rb_namep, mass); } @@ -175,17 +171,19 @@ return 0; } + /** * Steps the dynamics world according to the simulation parameters * */ -int step_physics(btDiscreteDynamicsWorld* dynamicsWorld, struct simulation_params *sim_params) +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"); //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); + dynamicsWorld->stepSimulation(1/60.f, 10); bu_vls_printf(sim_params->result_str, "----- Simulation Complete -----\n"); return 0; @@ -196,25 +194,26 @@ * Get the final transforms and pack off back to libged * */ -int get_transforms(btDiscreteDynamicsWorld* dynamicsWorld, struct simulation_params *sim_params) +int +get_transforms(btDiscreteDynamicsWorld* dynamicsWorld, struct simulation_params *sim_params) { int i; btScalar m[16]; btVector3 aabbMin, aabbMax, v; - btTransform identity; + btTransform identity; 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); + 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(); @@ -223,7 +222,7 @@ struct rigid_body *current_node = (struct rigid_body *)bb_RigidBody->getUserPointer(); - if(current_node == NULL){ + if (current_node == NULL) { bu_vls_printf(sim_params->result_str, "get_transforms : Could not get the user pointer \ (ground plane perhaps)\n"); continue; @@ -248,12 +247,12 @@ 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]); + 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) - v = bb_RigidBody->getLinearVelocity(); + v = bb_RigidBody->getLinearVelocity(); VMOVE(current_node->linear_velocity, v); v = bb_RigidBody->getAngularVelocity(); @@ -270,26 +269,26 @@ * Cleanup the physics collision shapes, rigid bodies etc * */ -int cleanup(btDiscreteDynamicsWorld* dynamicsWorld, - btAlignedObjectArray<btCollisionShape*> collision_shapes) +int +cleanup(btDiscreteDynamicsWorld* dynamicsWorld, + btAlignedObjectArray<btCollisionShape*> collision_shapes) { //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()){ + if (body && body->getMotionState()) { delete body->getMotionState(); } - dynamicsWorld->removeCollisionObject( obj ); + dynamicsWorld->removeCollisionObject(obj); delete obj; } //delete collision shapes - for (i=0; i<collision_shapes.size(); i++) - { + for (i=0; i<collision_shapes.size(); i++) { btCollisionShape* shape = collision_shapes[i]; delete shape; } @@ -311,8 +310,7 @@ { //Return true when pairs need collision virtual bool - needBroadphaseCollision(btBroadphaseProxy* proxy0, btBroadphaseProxy* proxy1) const - { + needBroadphaseCollision(btBroadphaseProxy* proxy0, btBroadphaseProxy* proxy1) const { bool collides = (proxy0->m_collisionFilterGroup & proxy1->m_collisionFilterMask) != 0; collides = collides && (proxy1->m_collisionFilterGroup & proxy0->m_collisionFilterMask); @@ -321,13 +319,13 @@ //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 (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); + 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' @@ -340,9 +338,10 @@ * Narrowphase filter callback : used to show the generated collision points * */ -void nearphase_callback(btBroadphasePair& collisionPair, - btCollisionDispatcher& dispatcher, - btDispatcherInfo& dispatchInfo) +void +nearphase_callback(btBroadphasePair& collisionPair, + btCollisionDispatcher& dispatcher, + btDispatcherInfo& dispatchInfo) { int i, j, numContacts; @@ -352,17 +351,17 @@ * 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++){ + for (i=0; i<numManifolds; i++) { //Get the manifold and the objects which created it btPersistentManifold* contactManifold = - dispatcher.getManifoldByIndexInternal(i); + 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); + 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(); @@ -370,27 +369,26 @@ numContacts = contactManifold->getNumContacts(); bu_log("nearphase_callback : Manifold %d of %d, contacts : %d\n", - i+1, - numManifolds, - numContacts); + i+1, + numManifolds, + numContacts); //Iterate over the points for this manifold - for (j=0;j<numContacts;j++){ + for (j=0;j<numContacts;j++) { btManifoldPoint& pt = contactManifold->getContactPoint(j); btVector3 ptA = pt.getPositionWorldOnA(); btVector3 ptB = pt.getPositionWorldOnB(); - if(upA == NULL || upB == NULL){ + if (upA == NULL || upB == NULL) { bu_log("nearphase_callback : contact %d of %d, could not get user pointers\n", - j+1, numContacts); + j+1, numContacts); - } - else{ + } 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]); + j+1, numContacts, + upA->rb_namep, ptA[0], ptA[1], ptA[2], + upB->rb_namep, ptB[0], ptB[1], ptB[2]); } } @@ -405,7 +403,7 @@ /** * C++ wrapper for doing physics using bullet - * + * */ extern "C" int run_simulation(struct simulation_params *sim_params) @@ -414,63 +412,64 @@ //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); + btBroadphaseInterface* broadphase = new btDbvtBroadphase(); + 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); + //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; + btSequentialImpulseConstraintSolver* solver = new btSequentialImpulseConstraintSolver; - dynamicsWorld = new btDiscreteDynamicsWorld(dispatcher,broadphase,solver,collisionConfiguration); + dynamicsWorld = new btDiscreteDynamicsWorld(dispatcher, broadphase, solver, collisionConfiguration); - //Set the gravity direction along -ve Z axis - dynamicsWorld->setGravity(btVector3(0, 0, -10)); + //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); + //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 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); + //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); + //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); + //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 and free memory used by physics objects + cleanup(dynamicsWorld, collision_shapes); - //Clean up stuff in here - delete solver; - delete dispatcher; - delete collisionConfiguration; - delete broadphase; + //Clean up stuff in here + delete solver; + delete dispatcher; + delete collisionConfiguration; + delete broadphase; //} return 0; } + #endif // Local Variables: Modified: brlcad/trunk/src/libged/simulate/simulate.c =================================================================== --- brlcad/trunk/src/libged/simulate/simulate.c 2011-10-03 19:53:38 UTC (rev 47036) +++ brlcad/trunk/src/libged/simulate/simulate.c 2011-10-03 20:12:29 UTC (rev 47037) @@ -1,7 +1,7 @@ /* S I M U L A T E . C * BRL-CAD * - * Copyright (c) 2008-2011 United States Government as represented by + * Copyright (c) 2011 United States Government as represented by * the U.S. Army Research Laboratory. * * This library is free software; you can redistribute it and/or @@ -17,13 +17,11 @@ * License along with this file; see the file named COPYING for more * information. */ -/** @file libged/simulate/simulate.c - * +/* * The simulate command. * * Routines related to performing physics on passed objects only * - * */ #include "common.h" @@ -68,7 +66,8 @@ * Prints a 16 by 16 transform matrix for debugging * */ -void print_matrix(char *rb_namep, mat_t t) +void +print_matrix(char *rb_namep, mat_t t) { int i, j; struct bu_vls buffer = BU_VLS_INIT_ZERO; @@ -92,7 +91,8 @@ /** * Prints a struct rigid_body for debugging, more members will be printed later */ -void print_rigid_body(struct rigid_body *rb) +void +print_rigid_body(struct rigid_body *rb) { bu_log("print_rigid_body : \"%s\", state = %d\n", rb->rb_namep, rb->state); } @@ -101,7 +101,8 @@ /** * Prints the list of contacts in each manifold of a rigid body */ -void print_manifold_list(struct rigid_body *rb) +void +print_manifold_list(struct rigid_body *rb) { struct sim_manifold *current_manifold; int i; @@ -130,7 +131,8 @@ /** * Prints the args of a command to be executed using libged */ -void print_command(char* cmd_args[], int num_args) +void +print_command(char* cmd_args[], int num_args) { int i; char buffer[200] = ""; @@ -145,7 +147,9 @@ /** * Used to prefix a name, requires memory to be freed by caller */ -char* prefix_name(char *prefix, char *original) { +char* +prefix_name(char *prefix, char *original) +{ /* Prepare prefixed bounding box primitive name */ size_t prefix_len, prefixed_name_len; char *prefixed_name; @@ -161,10 +165,12 @@ /** - * Deletes a prim/comb if it exists - * TODO : lower to librt + * Deletes a prim/comb if it exists. + * + * TODO: lower to librt */ -int kill(struct ged *gedp, char *name) +int +kill(struct ged *gedp, char *name) { char *cmd_args[5]; @@ -186,10 +192,12 @@ /** - * Deletes and duplicates the prim/comb passed in dp as new_name + * Deletes and duplicates the prim/comb passed in dp as new_name. + * * TODO : lower to librt */ -int kill_copy(struct ged *gedp, struct directory *dp, char* new_name) +int +kill_copy(struct ged *gedp, struct directory *dp, char* new_name) { char *cmd_args[5]; int rv; @@ -216,10 +224,12 @@ /** - * Adds a prim/comb to an existing comb or creates it if not existing - * TODO : lower to librt + * Adds a prim/comb to an existing comb or creates it if not existing. + * + * TODO: lower to librt */ -int add_to_comb(struct ged *gedp, char *target, char *add) +int +add_to_comb(struct ged *gedp, char *target, char *add) { char *cmd_args[5]; int rv; @@ -240,7 +250,8 @@ } -int add_physics_attribs(struct rigid_body *current_node) +int +add_physics_attribs(struct rigid_body *current_node) { VSETALL(current_node->linear_velocity, 0.0f); @@ -255,11 +266,13 @@ /** * Add the list of regions in the model to the rigid bodies list in - * simulation parameters. This function will duplicate the existing regions - * prefixing "sim_" to the new region and putting them all under a new - * comb "sim.c". It will skip over any existing regions with "sim_" in the name + * simulation parameters. This function will duplicate the existing + * regions prefixing "sim_" to the new region and putting them all + * under a new comb "sim.c". It will skip over any existing regions + * with "sim_" in the name */ -int add_regions(struct ged *gedp, struct simulation_params *sim_params) +int +add_regions(struct ged *gedp, struct simulation_params *sim_params) { struct directory *dp, *ndp; char *prefixed_name; @@ -341,7 +354,8 @@ } -int get_bb(struct ged *gedp, struct simulation_params *sim_params) +int +get_bb(struct ged *gedp, struct simulation_params *sim_params) { struct rigid_body *current_node; point_t rpp_min, rpp_max; @@ -390,13 +404,13 @@ /** * This function draws the bounding box around a comb as reported by - * Bullet - * TODO : this should be used with a debugging flag - * TODO : this function will soon be lowered to librt + * Bullet. * + * TODO: this should be used with a debugging flag + * TODO: this function will soon be lowered to librt */ -int insert_AABB(struct ged *gedp, struct simulation_params *sim_params, - struct rigid_body *current_node) +int +insert_AABB(struct ged *gedp, struct simulation_params *sim_params, struct rigid_body *current_node) { char* cmd_args[28]; char buffer[20]; @@ -536,14 +550,13 @@ /** - * This function inserts a manifold comb as reported by - * Bullet - * TODO : this should be used with a debugging flag - * TODO : this function will be lowered to librt + * This function inserts a manifold comb as reported by Bullet. * + * TODO: this should be used with a debugging flag + * TODO: this function will be lowered to librt */ -int insert_manifolds(struct ged *gedp, struct simulation_params *sim_params, - struct rigid_body *rb) +int +insert_manifolds(struct ged *gedp, struct simulation_params *sim_params, struct rigid_body *rb) { char* cmd_args[28]; char buffer[20]; @@ -711,12 +724,15 @@ /** * This function colors the passed comb. It's for showing the current * state of the object inside the physics engine. + * * TODO : this should be used with a debugging flag - * */ -int apply_color(struct ged *gedp, char* rb_namep, unsigned char r, - unsigned char g, - unsigned char b) +int +apply_color(struct ged *gedp, + char* rb_namep, + unsigned char r, + unsigned char g, + unsigned char b) { struct directory *dp = NULL; struct rt_comb_internal *comb = NULL; @@ -766,11 +782,13 @@ /** - * This function takes the transforms present in the current node and applies them - * in 3 steps : translate to origin, apply the rotation, then translate to final - * position with respect to origin(as obtained from physics) + * This function takes the transforms present in the current node and + * applies them in 3 steps : translate to origin, apply the rotation, + * then translate to final position with respect to origin(as obtained + * from physics) */ -int apply_transforms(struct ged *gedp, struct simulation_params *sim_params) +int +apply_transforms(struct ged *gedp, struct simulation_params *sim_params) { struct rt_db_internal intern; struct rigid_body *current_node; @@ -883,11 +901,11 @@ /** - * Frees the list of manifolds as generated by Bullet: this list is used - * to compare with rt generated manifold for accuracy - * + * Frees the list of manifolds as generated by Bullet. This list is + * used to compare with rt generated manifold for accuracy */ -int free_manifold_lists(struct simulation_params *sim_params) +int +free_manifold_lists(struct simulation_params *sim_params) { /* Free memory in manifold list */ struct sim_manifold *current_manifold, *next_manifold; @@ -913,9 +931,10 @@ /** - * The libged physics simulation function : - * Check flags, adds regions to simulation parameters, runs the simulation - * applies the transforms, frees memory + * The libged physics simulation function. + * + * Check flags, adds regions to simulation parameters, runs the + * simulation applies the transforms, frees memory */ int ged_simulate(struct ged *gedp, int argc, const char *argv[]) @@ -942,7 +961,6 @@ bu_vls_printf(gedp->ged_result_str, "Usage: %s <steps>", argv[0]); return GED_ERROR; } - /* Make a list containing the bb and existing transforms of all the objects in the model * which will participate in the simulation @@ -1001,7 +1019,7 @@ bu_vls_printf(gedp->ged_result_str, "%s: The simulation result is in group : %s\n", argv[0], sim_comb_name); - + /* Draw the result : inserting it in argv[1] will cause it to be automatically drawn in the cmd_wrapper */ argv[1] = sim_comb_name; argv[2] = (char *)0; Modified: brlcad/trunk/src/libged/simulate/simulate.h =================================================================== --- brlcad/trunk/src/libged/simulate/simulate.h 2011-10-03 19:53:38 UTC (rev 47036) +++ brlcad/trunk/src/libged/simulate/simulate.h 2011-10-03 20:12:29 UTC (rev 47037) @@ -1,7 +1,7 @@ -/* S I M U L A T E . H +/* S I M U L A T E . H * BRL-CAD * - * Copyright (c) 2008-2011 United States Government as represented by + * Copyright (c) 2011 United States Government as represented by * the U.S. Army Research Laboratory. * * This library is free software; you can redistribute it and/or @@ -17,8 +17,7 @@ * License along with this file; see the file named COPYING for more * information. */ -/** @file libged/simulate/simulate.h - * +/* * The simulate command. * * Declares structures for passing simulation parameters and @@ -40,22 +39,26 @@ #define MAX_CONTACTS_PER_MANIFOLD 4 struct sim_contact { - vect_t ptA, ptB; - vect_t normalWorldOnB; + vect_t ptA, ptB; + vect_t normalWorldOnB; }; + struct sim_manifold { - int num_contacts; - int indexA, indexB; - char *nameA, *nameB; - struct sim_contact rb_contacts[MAX_CONTACTS_PER_MANIFOLD]; - struct sim_manifold *next; + int num_contacts; + int indexA, indexB; + char *nameA, *nameB; + struct sim_contact rb_contacts[MAX_CONTACTS_PER_MANIFOLD]; + struct sim_manifold *next; }; -/* Contains information about a single rigid body constructed from a BRL-CAD region. - * This structure is the node of a linked list containing the geometry to be added to the sim - * Only the bb is currently present, but physical properties like elasticity, custom forces - * will be added later: TODO + +/* Contains information about a single rigid body constructed from a + * BRL-CAD region. This structure is the node of a linked list + * containing the geometry to be added to the sim. + * + * TODO: Only the bb is currently present, but physical properties + * like elasticity, custom forces will be added later. */ struct rigid_body { int index; @@ -64,23 +67,24 @@ point_t bb_center, bb_dims; /**< @brief bb center and dimensions */ point_t btbb_min, btbb_max; /**< @brief Bullet body bb bounds */ point_t btbb_center, btbb_dims; /**< @brief Bullet bb center and dimensions */ - mat_t m, m_prev; /**< @brief transformation matrix from Bullet */ - int state; /**< @brief rigid body state from Bullet */ + mat_t m, m_prev; /**< @brief transformation matrix from Bullet */ + int state; /**< @brief rigid body state from Bullet */ struct directory *dp; /**< @brief directory pointer to the related region */ struct rigid_body *next; /**< @brief link to next body */ /* Can be set by libged or Bullet(checked and inserted into sim) */ - vect_t linear_velocity; /**< @brief linear velocity components */ - vect_t angular_velocity; /**< @brief angular velocity components */ + vect_t linear_velocity; /**< @brief linear velocity components */ + vect_t angular_velocity; /**< @brief angular velocity components */ /* Manifolds in which this body participates and is body B, only body B has manifold info*/ - int num_manifolds; /**< @brief angular velocity components */ + int num_manifolds; /**< @brief angular velocity components */ struct sim_manifold *first_manifold; /**< @brief angular velocity components */ }; + /* Contains the simulation parameters, such as number of rigid bodies, - * the head node of the linked list containing the bodies and time/steps for - * which the simulation will be run. + * the head node of the linked list containing the bodies and + * time/steps for which the simulation will be run. */ struct simulation_params { int duration; /**< @brief contains either the number of steps or the time */ @@ -92,8 +96,6 @@ }; - - #endif /* SIMULATE_H_ */ This was sent by the SourceForge.net collaborative development platform, the world's largest Open Source development site. ------------------------------------------------------------------------------ All the data continuously generated in your IT infrastructure contains a definitive record of customers, application performance, security threats, fraudulent activity and more. Splunk takes this data and makes sense of it. Business sense. IT sense. Common sense. http://p.sf.net/sfu/splunk-d2dcopy1 _______________________________________________ BRL-CAD Source Commits mailing list brlcad-commits@lists.sourceforge.net https://lists.sourceforge.net/lists/listinfo/brlcad-commits