[brlcad-commits] SF.net SVN: brlcad:[46890] brlcad/trunk/src/libged/simulate
Open Source Solid Modeling CAD
Brought to you by:
brlcad
From: <abh...@us...> - 2011-09-25 17:32:02
|
Revision: 46890 http://brlcad.svn.sourceforge.net/brlcad/?rev=46890&view=rev Author: abhi2011 Date: 2011-09-25 17:31:54 +0000 (Sun, 25 Sep 2011) Log Message: ----------- fixing some functions after I had to revert back to resolve some positioning issues, modifying the code step by step now Modified Paths: -------------- 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/simphysics.cpp =================================================================== --- brlcad/trunk/src/libged/simulate/simphysics.cpp 2011-09-25 07:39:03 UTC (rev 46889) +++ brlcad/trunk/src/libged/simulate/simphysics.cpp 2011-09-25 17:31:54 UTC (rev 46890) @@ -32,18 +32,12 @@ #include <iostream> -#include "bu.h" -#include "raytrace.h" #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 * @@ -76,104 +70,7 @@ } -//--------------------- 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 @@ -183,213 +80,108 @@ btAlignedObjectArray<btCollisionShape*> collision_shapes) { struct rigid_body *current_node; - btVector3 v1, v2, v3; + fastf_t volume; btScalar mass; + btScalar m[16]; + + for (current_node = sim_params->head_node; current_node != NULL; current_node = current_node->next) { - // Check if the rigid body is static or dynamic(mass=0 or not) - mass = current_node->mass; - if(ZERO(mass)){ - //Static body - btCollisionShape* bb_Shape = new btBoxShape(btVector3(current_node->bb_dims[0]/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* bb_MotionState = new btDefaultMotionState( + /*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]))); + current_node->bb_center[2])));*/ + 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)));*/ + btRigidBody::btRigidBodyConstructionInfo - bb_RigidBodyCI(0, bb_MotionState, bb_Shape, btVector3(0,0,0)); - btRigidBody* bb_RigidBody = new btRigidBody(bb_RigidBodyCI); - bb_RigidBody->setUserPointer((void *)current_node); - dynamicsWorld->addRigidBody(bb_RigidBody); - collision_shapes.push_back(bb_Shape); + 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); - bu_log("Added static body : %s to simulation with mass %f Kg\n", + 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 if(mass > 0){ - //Dynamic Body + 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); - /* Set the mass from user parameters */ + 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); - btDefaultMotionState* bb_MotionState = new btDefaultMotionState(btTransform(btQuaternion(0,0,0,1), + /* 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]))); + 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); btRigidBody* bb_RigidBody = new btRigidBody(bb_RigidBodyCI); bb_RigidBody->setUserPointer((void *)current_node); - /* Add various other simulation properties from user parameters*/ - /* Custom Force : will become a torque if force_position is non-zero in body relative co-ords*/ - v1[0] = current_node->force[0]; - v1[1] = current_node->force[1]; - v1[2] = current_node->force[2]; - v1[0] = current_node->force_position[0]; - v1[1] = current_node->force_position[1]; - v1[2] = current_node->force_position[2]; - switch(current_node->persist_force){ - case PERSIST_FORCE_ONCE: - bb_RigidBody->applyForce(v1, v2); - current_node->persist_force = PERSIST_FORCE_IGNORE; - break; - - case PERSIST_FORCE_ALWAYS: - bb_RigidBody->applyForce(v1, v2); - break; - - case PERSIST_FORCE_IGNORE: - break; - default: - ; - } - - /* Linear Velocity */ - v1[0] = current_node->linear_velocity[0]; - v1[1] = current_node->linear_velocity[1]; - v1[2] = current_node->linear_velocity[2]; - bb_RigidBody->setLinearVelocity(v1); - - /* Angular Velocity */ - v1[0] = current_node->angular_velocity[0]; - v1[1] = current_node->angular_velocity[1]; - v1[2] = current_node->angular_velocity[2]; - bb_RigidBody->setAngularVelocity(v1); - dynamicsWorld->addRigidBody(bb_RigidBody); - v1 = bb_RigidBody->getLinearVelocity(); - v2 = bb_RigidBody->getAngularVelocity(); + 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_log("Added rigid body %s to simulation with mass %f Kg, lv(%f,%f,%f), av(%f,%f,%f)\n", - current_node->rb_namep, mass, v1[0], v1[1], v1[2], v2[0], v2[1], v2[2]); - } - else - bu_log("Negative mass of %f Kg detected for %s,\ - exotic forms of matter are not yet supported. Object ignored.\n", mass, current_node->rb_namep); } return 0; } - /** - * 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 simulation_params *sim_params) -{ - struct rt_db_internal intern; - struct rigid_body *current_node; - struct db_i *dbip = sim_params->dbip; - mat_t t, m; - struct directory *dp; - - for (current_node = sim_params->head_node; current_node != NULL; current_node = current_node->next) { - - dp = current_node->dp; - - /* Get the internal representation of the object */ - if ( !rt_db_lookup_internal(dbip, dp->d_namep, &dp, &intern, LOOKUP_NOISY, &rt_uniresource)){ - bu_log("apply_transforms: rt_db_lookup_internal(%s) failed to get the internal form", - dp->d_namep); - return SIM_ERROR; - } - - - /* Translate to origin without any rotation, before applying rotation */ - MAT_IDN(m); - m[12] = - (current_node->bb_center[0]); - m[13] = - (current_node->bb_center[1]); - m[14] = - (current_node->bb_center[2]); - MAT_TRANSPOSE(t, m); - if (rt_matrix_transform(&intern, t, &intern, 0, dbip, &rt_uniresource) < 0){ - bu_log("apply_transforms: ERROR rt_matrix_transform(%s) failed while \ - translating to origin!\n", - current_node->dp->d_namep); - return SIM_ERROR; - } - - /* Apply rotation with no translation*/ - MAT_COPY(m, current_node->m); - m[12] = 0; - m[13] = 0; - m[14] = 0; - MAT_TRANSPOSE(t, m); - if (rt_matrix_transform(&intern, t, &intern, 0, dbip, &rt_uniresource) < 0){ - bu_log("apply_transforms: ERROR rt_matrix_transform(%s) failed while \ - applying rotation\n", - current_node->dp->d_namep); - return SIM_ERROR; - } - - /* Translate again without any rotation, to apply final position */ - MAT_IDN(m); - m[12] = current_node->m[12]; - m[13] = current_node->m[13]; - m[14] = current_node->m[14]; - MAT_TRANSPOSE(t, m); - if (rt_matrix_transform(&intern, t, &intern, 0, dbip, &rt_uniresource) < 0){ - bu_log("apply_transforms: ERROR rt_matrix_transform(%s) failed while \ - translating to final position\n", - current_node->dp->d_namep); - return SIM_ERROR; - } - - /* Write the modified solid to the db so it can be redrawn at the new position & orientation by Mged */ - if (rt_db_put_internal(current_node->dp, dbip, &intern, &rt_uniresource) < 0) { - bu_log("apply_transforms: ERROR Database write error for '%s', aborting\n", - current_node->dp->d_namep); - return SIM_ERROR; - } - - } - - return SIM_OK; -} - - -/** - * Step the dynamics world according to the simulation parameters just once + * Steps the dynamics world according to the simulation parameters * */ int step_physics(btDiscreteDynamicsWorld* dynamicsWorld, struct simulation_params *sim_params) { - int rv, i; + 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("----- Starting simulation -----\n"); - for (i=0 ; i < sim_params->duration ; i++) { //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); - - /* Apply transforms on the participating objects, also shades objects */ - rv = apply_transforms(sim_params); - if (rv != SIM_OK){ - bu_log("step_physics: ERROR while applying transforms\n"); - return SIM_ERROR; - } } - bu_log("----- Simulation Complete -----\n"); + bu_vls_printf(sim_params->result_str, "----- Simulation Complete -----\n"); - return SIM_OK; + return 0; } @@ -406,7 +198,6 @@ identity.setIdentity(); const int num_bodies = dynamicsWorld->getNumCollisionObjects(); - btVector3 v; for(i=0; i < num_bodies; i++){ @@ -426,15 +217,14 @@ 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\n"); - return SIM_ERROR; + bu_vls_printf(sim_params->result_str, "get_transforms : Could not get the user pointer \ + (ground plane perhaps)\n"); + continue; + } //Copy the transform matrix - current_node->m[0] = m[0]; current_node->m[4] = m[4]; current_node->m[8] = m[8]; current_node->m[12] = m[12]; - current_node->m[1] = m[1]; current_node->m[5] = m[5]; current_node->m[9] = m[9]; current_node->m[13] = m[13]; - current_node->m[2] = m[2]; current_node->m[6] = m[6]; current_node->m[10] = m[10]; current_node->m[14] = m[14]; - current_node->m[3] = m[3]; current_node->m[7] = m[7]; current_node->m[11] = m[11]; current_node->m[15] = m[15]; + MAT_COPY(current_node->m, m); //print_matrices(sim_params, current_node->rb_namep, current_node->m, m); @@ -444,44 +234,28 @@ //Get the AABB bb_Shape->getAabb(bb_MotionState->m_graphicsWorldTrans, aabbMin, aabbMax); - current_node->btbb_min[0] = aabbMin[0]; - current_node->btbb_min[1] = aabbMin[1]; - current_node->btbb_min[2] = aabbMin[2]; + VMOVE(current_node->btbb_min, aabbMin); + VMOVE(current_node->btbb_max, aabbMax); - current_node->btbb_max[0] = aabbMax[0]; - current_node->btbb_max[1] = aabbMax[1]; - current_node->btbb_max[2] = aabbMax[2]; - // Get BB length, width, height - current_node->btbb_dims[0] = current_node->btbb_max[0] - current_node->btbb_min[0]; + VSUB2(current_node->btbb_dims, current_node->btbb_max, current_node->btbb_min); + /* current_node->btbb_dims[0] = current_node->btbb_max[0] - current_node->btbb_min[0]; current_node->btbb_dims[1] = current_node->btbb_max[1] - current_node->btbb_min[1]; - current_node->btbb_dims[2] = current_node->btbb_max[2] - current_node->btbb_min[2]; + current_node->btbb_dims[2] = current_node->btbb_max[2] - current_node->btbb_min[2];*/ - bu_log("get_transforms: Dimensions of this BB : %f %f %f\n", + 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 - current_node->btbb_center[0] = current_node->btbb_min[0] + current_node->btbb_dims[0]/2; + VCOMB2(current_node->btbb_center, 1, current_node->btbb_min, 0.5, current_node->btbb_dims) + /* current_node->btbb_center[0] = current_node->btbb_min[0] + current_node->btbb_dims[0]/2; current_node->btbb_center[1] = current_node->btbb_min[1] + current_node->btbb_dims[1]/2; - current_node->btbb_center[2] = current_node->btbb_min[2] + current_node->btbb_dims[2]/2; + current_node->btbb_center[2] = current_node->btbb_min[2] + current_node->btbb_dims[2]/2;*/ - /* Get the other sim parameters which should be persisted */ - /* Linear Velocity */ - v = bb_RigidBody->getLinearVelocity(); - current_node->linear_velocity[0] = v[0]; - current_node->linear_velocity[1] = v[1]; - current_node->linear_velocity[2] = v[2]; - - /* Angular Velocity */ - v = bb_RigidBody->getAngularVelocity(); - current_node->angular_velocity[0] = v[0]; - current_node->angular_velocity[1] = v[1]; - current_node->angular_velocity[2] = v[2]; - } } - return SIM_OK; + return 0; } @@ -516,7 +290,7 @@ //delete dynamics world delete dynamicsWorld; - return SIM_OK; + return 0; } @@ -533,22 +307,9 @@ // 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); @@ -559,23 +320,16 @@ //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 & AABBs back into the simulation params struct + //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 this function + //Clean up stuff in here delete solver; delete dispatcher; delete collisionConfiguration; Modified: brlcad/trunk/src/libged/simulate/simulate.c =================================================================== --- brlcad/trunk/src/libged/simulate/simulate.c 2011-09-25 07:39:03 UTC (rev 46889) +++ brlcad/trunk/src/libged/simulate/simulate.c 2011-09-25 17:31:54 UTC (rev 46890) @@ -22,8 +22,7 @@ * 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 + * * */ @@ -48,29 +47,7 @@ /* The C++ simulation function */ extern int run_simulation(struct simulation_params *sim_params); -/* Physics attributes that are currently checked for in regions */ -#define MAX_SIM_ATTRIBS 6 -/* DO NOT CHANGE THE ORDER of attribs in sim_attrib as the code assumes the current order - * add new sim attribs at the bottom and increase MAX_SIM_ATTRIBS - */ -static const char* sim_attrib[MAX_SIM_ATTRIBS] = { - "simulate:mass", - "simulate:force", - "simulate:linear_velocity", - "simulate:angular_velocity", - "simulate:restitution", - "simulate:friction" -}; - -/* Maximum allowed values of scalars and components of vectors - * These are to check for correctness of user set values, not imposed by physics - */ -static const fastf_t MAX_MASS = 500000; -static const fastf_t MAX_FORCE_COMP = 100000; -static const fastf_t MAX_LINEAR_VELOCITY_COMP = 100000; -static const fastf_t MAX_ANGULAR_VELOCITY_COMP = 100000; - /** * How to use simulate.Blissfully simple interface, more options will be added soon */ @@ -199,136 +176,6 @@ /** - * Parses a string containing 3 floating point components into a vector vect_t - * - */ -int parse_vector(vect_t vec, const char *str) -{ - char *argv[ELEMENTS_PER_VECT]; - char *end; - VSETALL(vec, 0); - - if(strlen(str)){ - bu_argv_from_string((char **)argv, ELEMENTS_PER_VECT, (char *)str); - - vec[0] = strtod(argv[0], &end); - vec[1] = strtod(argv[1], &end); - vec[2] = strtod(argv[2], &end); - } - - /*bu_log("parse_vector: str = \"%s\" , vec = (%f, %f, %f)\n", str, vec[0], vec[1], vec[2]);*/ - - - return GED_OK; -} - - -/** - * Adds physics attributes to a node of the rigid_body list to be dispatched to physics - * - */ -int add_physics_attributes(struct ged *gedp, struct rigid_body *current_node, struct directory *ndp) -{ - unsigned int i; - struct bu_attribute_value_set avs; - struct bu_attribute_value_pair *avpp; - const char *val, *val_cpy; - fastf_t simulate_mass; - vect_t simulate_force, simulate_linear_velocity, simulate_angular_velocity; - - bu_avs_init_empty(&avs); - if (db5_get_attributes(gedp->ged_wdbp->dbip, &avs, ndp)) { - bu_vls_printf(gedp->ged_result_str, "add_regions : ERROR : Cannot get attributes for object %s\n", - ndp->d_namep); - return GED_ERROR; - } - - /* Set sim attributes to default values */ - current_node->mass = 0; - VSETALL(current_node->force, 0); - VSETALL(current_node->force_position, 0); - VSETALL(current_node->linear_velocity, 0); - VSETALL(current_node->angular_velocity, 0); - - /* Sort attribute-value set array by attribute name */ - qsort(&avs.avp[0], avs.count, sizeof(struct bu_attribute_value_pair), _ged_cmpattr); - - for (i = 0; i < MAX_SIM_ATTRIBS; i++) { - val = bu_avs_get(&avs, sim_attrib[i]); - if (val) { - bu_log("add_regions : Object %s has %s = %s\n", ndp->d_namep, sim_attrib[i], val); - val_cpy = bu_strdup(val); - - switch(i){ - /* Mass */ - case 0: - simulate_mass = atoi(val_cpy); - if(simulate_mass < 0 || simulate_mass > MAX_MASS){ - bu_log("add_regions : WARNING %s should be between %f and %f, set to %f\n", - sim_attrib[i], 0.0, MAX_MASS, 0.0); - current_node->mass = 0; - } - else - current_node->mass = simulate_mass; - break; - /* Custom Force */ - case 1: - parse_vector(simulate_force, val_cpy); - if(abs(simulate_force[0]) > MAX_FORCE_COMP || - abs(simulate_force[1]) > MAX_FORCE_COMP || - abs(simulate_force[2]) > MAX_FORCE_COMP){ - bu_log("add_regions : WARNING %s should be between %f and %f, set to %f\n", - sim_attrib[i], 0.0, MAX_FORCE_COMP, 0.0); - VSETALL(current_node->force, 0); - } - else - VMOVE(current_node->force, simulate_force); - break; - /* Linear Velocity */ - case 2: - parse_vector(simulate_linear_velocity, val_cpy); - if(abs(simulate_linear_velocity[0]) > MAX_LINEAR_VELOCITY_COMP || - abs(simulate_linear_velocity[1]) > MAX_LINEAR_VELOCITY_COMP || - abs(simulate_linear_velocity[2]) > MAX_LINEAR_VELOCITY_COMP){ - bu_log("add_regions : WARNING %s should be between %f and %f, set to %f\n", - sim_attrib[i], 0.0, MAX_LINEAR_VELOCITY_COMP, 0.0); - VSETALL(current_node->linear_velocity, 0); - } - else - VMOVE(current_node->linear_velocity, simulate_linear_velocity); - break; - /* Angular Velocity */ - case 3: - parse_vector(simulate_angular_velocity, val_cpy); - if(abs(simulate_angular_velocity[0]) > MAX_ANGULAR_VELOCITY_COMP || - abs(simulate_angular_velocity[1]) > MAX_ANGULAR_VELOCITY_COMP || - abs(simulate_angular_velocity[2]) > MAX_ANGULAR_VELOCITY_COMP){ - bu_log("add_regions : WARNING %s should be between %f and %f, set to %f\n", - sim_attrib[i], 0.0, MAX_ANGULAR_VELOCITY_COMP, 0.0); - VSETALL(current_node->angular_velocity, 0); - } - else - VMOVE(current_node->angular_velocity, simulate_angular_velocity); - break; - default: - bu_log("add_regions : WARNING : simulation attribute %s not recognized!\n", - sim_attrib[i]); - } /* switch */ - } /* if */ - } /* for */ - - /* Just list all the attributes */ - for (i = 0, avpp = avs.avp; i < avs.count; i++, avpp++) { - bu_log("Attribute : %s {%s} ", avpp->name, avpp->value); - } - - bu_avs_free(&avs); - - return GED_OK; - -} - -/** * 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 @@ -341,10 +188,9 @@ char *prefix = "sim_"; size_t prefix_len, prefixed_name_len; point_t rpp_min, rpp_max; - unsigned int i; + int i; struct rigid_body *prev_node = NULL, *current_node; - /* Kill the existing sim comb */ kill(gedp, sim_params->sim_comb_name); sim_params->num_bodies = 0; @@ -358,7 +204,7 @@ continue; if (strstr(dp->d_namep, prefix)){ - bu_log("add_regions: Skipping \"%s\" due to \"%s\" in name\n", + bu_vls_printf(gedp->ged_result_str, "add_regions: Skipping \"%s\" due to \"%s\" in name\n", dp->d_namep, prefix); continue; } @@ -371,21 +217,22 @@ bu_strlcat(prefixed_name + prefix_len, dp->d_namep, prefixed_name_len - prefix_len); kill_copy(gedp, dp, prefixed_name); - bu_log("add_regions: Copied \"%s\" to \"%s\"\n", dp->d_namep, prefixed_name); + bu_vls_printf(gedp->ged_result_str, "add_regions: Copied \"%s\" to \"%s\"\n", dp->d_namep, prefixed_name); - /* Get the directory pointer for the duplicate object just added */ + /* 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_log("add_regions: db_lookup(%s) failed", prefixed_name); + bu_vls_printf(gedp->ged_result_str, "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_log("add_regions: Got the BB for \"%s\" as min {%f %f %f} max {%f %f %f}\n", ndp->d_namep, + 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, rpp_min[0], rpp_min[1], rpp_min[2], rpp_max[0], rpp_max[1], rpp_max[2]); else{ - bu_log("add_regions: ERROR Could not get the BB\n"); + bu_vls_printf(gedp->ged_result_str, "add_regions: ERROR Could not get the BB\n"); return GED_ERROR; } @@ -403,7 +250,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_log("add_regions: Dimensions of this BB : %f %f %f\n", + bu_vls_printf(gedp->ged_result_str, "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 */ @@ -411,8 +258,10 @@ current_node->bb_center[1] = current_node->bb_min[1] + current_node->bb_dims[1]/2; current_node->bb_center[2] = current_node->bb_min[2] + current_node->bb_dims[2]/2; - /* Get its physics attributes */ - add_physics_attributes(gedp, current_node, ndp); + MAT_IDN(current_node->m); + current_node->m[12] = current_node->bb_center[0]; + current_node->m[13] = current_node->bb_center[1]; + current_node->m[14] = current_node->bb_center[2]; /* Setup the linked list */ if(prev_node == NULL){ /* first node */ @@ -657,6 +506,115 @@ /** + * 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) +{ + struct rt_db_internal intern; + struct rigid_body *current_node; + mat_t t , m; + /*int rv;*/ + + for (current_node = sim_params->head_node; current_node != NULL; current_node = current_node->next) { + + if(strcmp(current_node->rb_namep, sim_params->ground_plane_name) == 0) + continue; + + /* Get the internal representation of the object */ + GED_DB_GET_INTERNAL(gedp, &intern, current_node->dp, bn_mat_identity, &rt_uniresource, GED_ERROR); + + /* Translate to origin without any rotation, before applying rotation */ + MAT_IDN(m); + m[12] = - (current_node->bb_center[0]); + m[13] = - (current_node->bb_center[1]); + m[14] = - (current_node->bb_center[2]); + MAT_TRANSPOSE(t, m); + if (rt_matrix_transform(&intern, t, &intern, 0, gedp->ged_wdbp->dbip, &rt_uniresource) < 0){ + bu_vls_printf(gedp->ged_result_str, "apply_transforms: ERROR rt_matrix_transform(%s) failed while \ + translating to origin!\n", + current_node->dp->d_namep); + return GED_ERROR; + } + + /* Apply rotation with no translation*/ + MAT_COPY(m, current_node->m); + m[12] = 0; + m[13] = 0; + m[14] = 0; + MAT_TRANSPOSE(t, m); + if (rt_matrix_transform(&intern, t, &intern, 0, gedp->ged_wdbp->dbip, &rt_uniresource) < 0){ + bu_vls_printf(gedp->ged_result_str, "apply_transforms: ERROR rt_matrix_transform(%s) failed while \ + applying rotation\n", + current_node->dp->d_namep); + return GED_ERROR; + } + + /* Translate again without any rotation, to apply final position */ + MAT_IDN(m); + m[12] = current_node->m[12]; + m[13] = current_node->m[13]; + m[14] = current_node->m[14]; + MAT_TRANSPOSE(t, m); + if (rt_matrix_transform(&intern, t, &intern, 0, gedp->ged_wdbp->dbip, &rt_uniresource) < 0){ + bu_vls_printf(gedp->ged_result_str, "apply_transforms: ERROR rt_matrix_transform(%s) failed while \ + translating to final position\n", + current_node->dp->d_namep); + return GED_ERROR; + } + + /* Write the modified solid to the db so it can be redrawn at the new position & orientation by Mged */ + if (rt_db_put_internal(current_node->dp, gedp->ged_wdbp->dbip, &intern, &rt_uniresource) < 0) { + bu_vls_printf(gedp->ged_result_str, "apply_transforms: ERROR Database write error for '%s', aborting\n", + current_node->dp->d_namep); + return GED_ERROR; + } + + /* Apply the proper shader to match the object state : useful for debugging */ +/* switch(current_node->state){ + case ACTIVE_TAG: + rv = apply_color(gedp, current_node->rb_namep, 255, 255, 0); + break; + + case ISLAND_SLEEPING: + rv = apply_color(gedp, current_node->rb_namep, 255, 0, 255); + break; + + case WANTS_DEACTIVATION: + rv = apply_color(gedp, current_node->rb_namep, 0, 255, 0); + break; + + case DISABLE_DEACTIVATION: + rv = apply_color(gedp, current_node->rb_namep, 255, 0, 0); + break; + + case DISABLE_SIMULATION: + rv = apply_color(gedp, current_node->rb_namep, 132, 255, 0); + break; + + default: + rv = apply_color(gedp, current_node->rb_namep, 0, 135, 233); + } + + if (rv != GED_OK){ + 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); */ + + /* print_rigid_body(current_node); */ + + } + + return GED_OK; +} + + +/** * The libged physics simulation function : * Check flags, adds regions to simulation parameters, runs the simulation * applies the transforms, frees memory @@ -686,12 +644,12 @@ 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 */ - sim_params.duration = atoi(argv[1]); - sim_params.dbip = gedp->ged_wdbp->dbip; + sim_params.duration = atoi(argv[1]); sim_params.result_str = gedp->ged_result_str; sim_params.sim_comb_name = bu_strdup(sim_comb_name); sim_params.ground_plane_name = bu_strdup(ground_plane_name); @@ -701,13 +659,21 @@ return GED_ERROR; } - /* This call will run physics for 1 step and put the resultant vel/forces into sim_params */ - rv = run_simulation(&sim_params); - if (rv != GED_OK){ + + /* Run the physics simulation */ + rv = run_simulation(&sim_params); + if (rv != GED_OK){ bu_vls_printf(gedp->ged_result_str, "%s: ERROR while running the simulation\n", argv[0]); return GED_ERROR; } + /* Apply transforms on the participating objects, also shades objects */ + rv = apply_transforms(gedp, &sim_params); + if (rv != GED_OK){ + bu_vls_printf(gedp->ged_result_str, "%s: ERROR while applying transforms\n", argv[0]); + return GED_ERROR; + } + /* Free memory in rigid_body list */ for (current_node = sim_params.head_node; current_node != NULL; ) { next_node = current_node->next; Modified: brlcad/trunk/src/libged/simulate/simulate.h =================================================================== --- brlcad/trunk/src/libged/simulate/simulate.h 2011-09-25 07:39:03 UTC (rev 46889) +++ brlcad/trunk/src/libged/simulate/simulate.h 2011-09-25 17:31:54 UTC (rev 46890) @@ -24,9 +24,6 @@ * Declares structures for passing simulation parameters and * hold info regarding rigid bodies * - * TODO : Add support for multiple manifolds for complex structures : - * may need a more comprehensive structure. - * */ #ifndef SIMULATE_H_ @@ -40,57 +37,32 @@ #define DISABLE_DEACTIVATION 4 #define DISABLE_SIMULATION 5 -//Force persistence over multiple steps -#define PERSIST_FORCE_ONCE 1 -#define PERSIST_FORCE_ALWAYS 2 -#define PERSIST_FORCE_IGNORE 3 - -//Force persistence over multiple steps -#define SIM_ERROR 0 -#define SIM_OK 1 - /* 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 */ struct rigid_body { - - /* Set by libged before taking this step */ - int index; + int index; char *rb_namep; /**< @brief pointer to name string */ - point_t bb_min, bb_max; /**< @brief body bb bounds in meters */ - point_t bb_center, bb_dims; /**< @brief bb center and dimensions in meters*/ + point_t bb_min, bb_max; /**< @brief body bb bounds */ + 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; /**< @brief transformation matrix from Bullet */ + int state; /**< @brief rigid body state from Bullet */ struct directory *dp; /**< @brief directory pointer to the related region */ - point_t contact[4]; /**< @brief contact manifold got from rt */ - point_t num_contacts; /**< @brief number of points inserted into contact[] */ struct rigid_body *next; /**< @brief link to next body */ - vect_t force; /**< @brief force to be applied before stepping sim */ - vect_t force_position; /**< @brief apply at this body rel. pos.: non-zero means torque */ - int persist_force; /**< @brief the above force should be applied on all time steps */ - fastf_t mass; /**< @brief mass in Kg of body */ - fastf_t restitution; /**< @brief coeff. of restitution(bounciness) of body */ - fastf_t friction; /**< @brief coeff. of friction of body */ - - /* Set by Bullet at the end of current step */ - int state; /**< @brief rigid body state after this step */ - point_t btbb_min, btbb_max; /**< @brief body bb bounds after this step in meters*/ - point_t btbb_center, btbb_dims; /**< @brief Bullet bb center and dimensions */ - mat_t m; /**< @brief transformation matrix after this step */ - - /* 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 */ }; -/* Contains the simulation parameters, such as number of rigid bodies and - * the head node of the linked list containing the bodies. +/* 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. */ struct simulation_params { + int duration; /**< @brief contains either the number of steps or the time */ int num_bodies; /**< @brief number of rigid bodies participating in the sim */ - int duration; struct bu_vls *result_str; /**< @brief handle to the libged object to access geometry info */ - struct db_i *dbip; char *sim_comb_name; /**< @brief name of the group which contains all sim regions*/ char *ground_plane_name; /**< @brief name of the ground plane region */ struct rigid_body *head_node; /**< @brief link to first rigid body node */ This was sent by the SourceForge.net collaborative development platform, the world's largest Open Source development site. |