Revision: 46558 http://brlcad.svn.sourceforge.net/brlcad/?rev=46558&view=rev Author: abhi2011 Date: 2011-09-02 17:09:17 +0000 (Fri, 02 Sep 2011) Log Message: ----------- Modified simulate to look through all regions in the current DB and add them as rigid bodies to the sim, objects now fallto the ground correctly
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-02 16:54:24 UTC (rev 46557) +++ brlcad/trunk/src/libged/simulate/simphysics.cpp 2011-09-02 17:09:17 UTC (rev 46558) @@ -68,100 +68,228 @@ } + /** - * C++ wrapper for doing physics using bullet - * + * 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 + * */ -extern "C" int -run_simulation(struct bu_vls *result_str, struct simulation_params *sim_params) +int add_rigid_bodies(btDiscreteDynamicsWorld* dynamicsWorld, struct simulation_params *sim_params, + btAlignedObjectArray<btCollisionShape*> collision_shapes) { - int i; - btScalar m[16]; - int num_steps = sim_params->duration; struct rigid_body *current_node; + fastf_t volume; + btScalar mass; - /* Show list of objects to be added to the sim */ - bu_log("The following %d rigid bodies will participate in the simulation : \n", sim_params->num_bodies); + + for (current_node = sim_params->head_node; current_node != NULL; current_node = current_node->next) { - bu_vls_printf(result_str, "Rigid Body : %s\n", current_node->rb_namep); - } - bu_vls_printf(result_str, "Simulation will run for %d steps.\n",num_steps); - bu_vls_printf(result_str, "A 1 kg sphere will be dropped from a height of 50 m.\n"); - bu_vls_printf(result_str, "Every time step is 1/60th of a second long(can be altered).\n"); + // 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 : can 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]))); + btRigidBody::btRigidBodyConstructionInfo + groundRigidBodyCI(0,groundMotionState,groundShape,btVector3(0,0,0)); + btRigidBody* groundRigidBody = new btRigidBody(groundRigidBodyCI); + groundRigidBody->setUserPointer((void *)current_node); + dynamicsWorld->addRigidBody(groundRigidBody); + collision_shapes.push_back(groundShape); - btBroadphaseInterface* broadphase = new btDbvtBroadphase(); + 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); - btDefaultCollisionConfiguration* collisionConfiguration = new btDefaultCollisionConfiguration(); - btCollisionDispatcher* dispatcher = new btCollisionDispatcher(collisionConfiguration); + } + 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); - btSequentialImpulseConstraintSolver* solver = new btSequentialImpulseConstraintSolver; + volume = current_node->bb_dims[0] * current_node->bb_dims[1] * current_node->bb_dims[2]; + mass = volume; // density is 1 - btDiscreteDynamicsWorld* dynamicsWorld = new btDiscreteDynamicsWorld(dispatcher,broadphase,solver,collisionConfiguration); + btVector3 bb_Inertia(0,0,0); + bb_Shape->calculateLocalInertia(mass, bb_Inertia); - dynamicsWorld->setGravity(btVector3(0,-10,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]))); + btRigidBody::btRigidBodyConstructionInfo + bb_RigidBodyCI(mass, bb_MotionState, bb_Shape, bb_Inertia); + btRigidBody* bb_RigidBody = new btRigidBody(bb_RigidBodyCI); + bb_RigidBody->setUserPointer((void *)current_node); + dynamicsWorld->addRigidBody(bb_RigidBody); - btCollisionShape* groundShape = new btStaticPlaneShape(btVector3(0,1,0),1); + bu_vls_printf(sim_params->result_str, "Added rigid body : %s to simulation with mass %f Kg\n", + current_node->rb_namep, mass); - btCollisionShape* fallShape = new btSphereShape(1); + } + } - btDefaultMotionState* groundMotionState = new btDefaultMotionState(btTransform(btQuaternion(0,0,0,1),btVector3(0,-1,0))); - btRigidBody::btRigidBodyConstructionInfo - groundRigidBodyCI(0,groundMotionState,groundShape,btVector3(0,0,0)); - btRigidBody* groundRigidBody = new btRigidBody(groundRigidBodyCI); - dynamicsWorld->addRigidBody(groundRigidBody); + return 0; +} +/** + * Steps the dynamics world according to the simulation parameters + * + */ +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"); - btDefaultMotionState* fallMotionState = - new btDefaultMotionState(btTransform(btQuaternion(0,0,0,1),btVector3(0,50,0))); - btScalar mass = 1; - btVector3 fallInertia(0,0,0); - fallShape->calculateLocalInertia(mass,fallInertia); - btRigidBody::btRigidBodyConstructionInfo fallRigidBodyCI(mass,fallMotionState,fallShape,fallInertia); - btRigidBody* fallRigidBody = new btRigidBody(fallRigidBodyCI); - dynamicsWorld->addRigidBody(fallRigidBody); + for (i=0 ; i < sim_params->duration ; i++) { - bu_vls_printf(result_str, "----- Starting simulation -----\n"); - btTransform trans; - for (i=0 ; i<num_steps ; 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); - fallRigidBody->getMotionState()->getWorldTransform(trans); - bu_vls_printf(result_str, "Step %3d : Sphere height: %f\n", i, trans.getOrigin().getY()); + } + bu_vls_printf(sim_params->result_str, "----- Simulation Complete -----\n"); + + return 0; +} + + +/** + * Get the final transforms and pack off back to libged + * + */ +int get_transforms(btDiscreteDynamicsWorld* dynamicsWorld, struct simulation_params *sim_params) +{ + int i; + btScalar m[16]; + + const int num_bodies = dynamicsWorld->getNumCollisionObjects(); + + for(i=0; i < num_bodies; i++){ + + //Common properties among all rigid bodies + btCollisionObject* bb_ColObj = dynamicsWorld->getCollisionObjectArray()[i]; + btRigidBody* bb_RigidBody = btRigidBody::upcast(bb_ColObj); + //const btCollisionShape* bb_Shape = bb_ColObj->getCollisionShape(); //may be used later + + if( bb_RigidBody && bb_RigidBody->getMotionState()){ + + //Get the motion state and the world transform from it + btDefaultMotionState* bb_MotionState = (btDefaultMotionState*)bb_RigidBody->getMotionState(); + bb_MotionState->m_graphicsWorldTrans.getOpenGLMatrix(m); + //bu_vls_printf(sim_params->result_str, "Position : %f, %f, %f\n", m[12], m[13], m[14]); + + struct rigid_body *current_node = (struct rigid_body *)bb_RigidBody->getUserPointer(); + + if(current_node == NULL){ + bu_vls_printf(sim_params->result_str, "get_transforms : Could not get the user pointer \ + (ground plane perhaps)\n"); + continue; + + } + + //Copy the transform matrix : transpose to convert from column major to row major + current_node->t[0] = m[0]; current_node->t[1] = m[4]; current_node->t[2] = m[8]; current_node->t[3] = m[12]; + current_node->t[4] = m[1]; current_node->t[5] = m[5]; current_node->t[6] = m[9]; current_node->t[7] = m[13]; + current_node->t[8] = m[2]; current_node->t[9] = m[6]; current_node->t[10] = m[10]; current_node->t[11] = m[14]; + current_node->t[12] = m[3]; current_node->t[13] = m[7]; current_node->t[14] = m[11]; current_node->t[15] = m[15]; + + print_matrices(sim_params->result_str, current_node->t, m); + + } } - bu_vls_printf(result_str, "----- Simulation Complete -----\n"); - trans.getOpenGLMatrix(m); + return 0; +} - //Copy the transform matrix : transpose to convert from column major to row major, y to z and vice versa - //Use transform matrix to convert openGL Y axis up to BRL-CAD Z axis up, both are right handed:TODO -/* t[0] = m[0]; t[1] = m[4]; t[2] = m[8]; t[3] = m[12]; - t[4] = m[1]; t[5] = m[5]; t[6] = m[9]; t[7] = m[13]; - t[8] = m[2]; t[9] = m[6]; t[10] = m[10]; t[11] = m[14]; - t[12] = m[3]; t[13] = m[7]; t[14] = m[11]; t[15] = m[15]; - print_matrices(result_str, t, m);*/ +/** + * Cleanup the physics collision shape, rigid body etc + * + */ +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--){ - //Cleanup in order of creation : better ways for cleanup is possible - dynamicsWorld->removeRigidBody(fallRigidBody); - delete fallRigidBody->getMotionState(); - delete fallRigidBody; + btCollisionObject* obj = dynamicsWorld->getCollisionObjectArray()[i]; + btRigidBody* body = btRigidBody::upcast(obj); + if (body && body->getMotionState()){ + delete body->getMotionState(); + } + dynamicsWorld->removeCollisionObject( obj ); + delete obj; + } - dynamicsWorld->removeRigidBody(groundRigidBody); - delete groundRigidBody->getMotionState(); - delete groundRigidBody; - delete fallShape; - delete groundShape; + //delete collision shapes + for (i=0; i<collision_shapes.size(); i++) + { + btCollisionShape* shape = collision_shapes[i]; + delete shape; + } + + //delete dynamics world delete dynamicsWorld; + + return 0; +} + + +/** + * C++ wrapper for doing physics using bullet + * + */ +extern "C" int +run_simulation(struct simulation_params *sim_params) +{ + // Initialize the physics world + btDiscreteDynamicsWorld* dynamicsWorld; + + // Keep the collision shapes, for deletion/cleanup + btAlignedObjectArray<btCollisionShape*> collision_shapes; + + btBroadphaseInterface* broadphase = new btDbvtBroadphase(); + btDefaultCollisionConfiguration* collisionConfiguration = new btDefaultCollisionConfiguration(); + btCollisionDispatcher* dispatcher = new btCollisionDispatcher(collisionConfiguration); + btSequentialImpulseConstraintSolver* solver = new btSequentialImpulseConstraintSolver; + + dynamicsWorld = new btDiscreteDynamicsWorld(dispatcher,broadphase,solver,collisionConfiguration); + + //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); + + //Step the physics the required number of times + step_physics(dynamicsWorld, sim_params); + + //Get the world transforms back into the simulation params struct + get_transforms(dynamicsWorld, sim_params); + + //Clean and free memory used by physics objects + cleanup(dynamicsWorld, collision_shapes); + + //Clean up stuff in here delete solver; + delete dispatcher; delete collisionConfiguration; - delete dispatcher; delete broadphase; + return 0; } Modified: brlcad/trunk/src/libged/simulate/simulate.c =================================================================== --- brlcad/trunk/src/libged/simulate/simulate.c 2011-09-02 16:54:24 UTC (rev 46557) +++ brlcad/trunk/src/libged/simulate/simulate.c 2011-09-02 17:09:17 UTC (rev 46558) @@ -45,7 +45,7 @@ /* The C++ simulation function */ -extern int run_simulation(struct bu_vls *result_str, struct simulation_params *sim_params); +extern int run_simulation(struct simulation_params *sim_params); /** * How to use simulate.Blissfully simple interface, more options will be added soon @@ -53,12 +53,12 @@ static void print_usage(struct bu_vls *str) { - bu_vls_printf(str, "Usage: simulate <steps> <object>\n\n"); - bu_vls_printf(str, "Currently this command aligns <object> to the final position of a falling sphere\n"); - bu_vls_printf(str, "after <steps> number of simulation steps.\n"); - bu_vls_printf(str, "-n <n> <x> <y> <z>\t- Specifies number of steps.\n"); - bu_vls_printf(str, "-f <n> <x> <y> <z>\t- Specifies frequency of update(eg 1/60 Hz).\n"); - bu_vls_printf(str, "-t <x> <y> <z>\t\t - Specifies time for which to run(alternative to -n).\n"); + bu_vls_printf(str, "Usage: simulate <steps>\n\n"); + bu_vls_printf(str, "Currently this command adds all regions in the model database to a \n\ + simulation having only gravity as a force. The objects should fall towards the ground plane XY.\n"); + bu_vls_printf(str, "The positions of the regions are set after <steps> number of simulation steps.\n"); + bu_vls_printf(str, "-f <n> <x> <y> <z>\t- Specifies frequency of update(eg 1/60 Hz)(WIP)\n"); + bu_vls_printf(str, "-t <x> <y> <z>\t\t - Specifies time for which to run(alternative to -n)(WIP)\n"); return; } @@ -69,43 +69,79 @@ */ int kill_copy(struct ged *gedp, struct directory *dp, char* new_name) { - char *cmd_args[5]; - int rv; + char *cmd_args[5]; + int rv; - /* Check if the duplicate already exists, and kill it if so(it maybe of a different shape) */ - if (db_lookup(gedp->ged_wdbp->dbip, new_name, LOOKUP_QUIET) != RT_DIR_NULL) { - bu_log("kill_copy: WARNING \"%s\" already exists, deleting it\n", new_name); - cmd_args[0] = "kill"; - cmd_args[1] = new_name; - cmd_args[2] = (char *)0; + /* Check if the duplicate already exists, and kill it if so(it maybe of a different shape) */ + if (db_lookup(gedp->ged_wdbp->dbip, new_name, LOOKUP_QUIET) != RT_DIR_NULL) { + bu_log("kill_copy: WARNING \"%s\" already exists, deleting it\n", new_name); + cmd_args[0] = "kill"; + cmd_args[1] = new_name; + cmd_args[2] = (char *)0; - if(ged_kill(gedp, 2, (const char **)cmd_args) != GED_OK){ - bu_log("kill_copy: could not delete existing \"%s\"\n", new_name); - return GED_ERROR; - } - } + if(ged_kill(gedp, 2, (const char **)cmd_args) != GED_OK){ + bu_log("kill_copy: could not delete existing \"%s\"\n", new_name); + return GED_ERROR; + } + } - /* Copy the passed prim/comb */ - cmd_args[0] = "copy"; - cmd_args[1] = dp->d_namep; - cmd_args[2] = new_name; - cmd_args[3] = (char *)0; - rv = ged_copy(gedp, 3, (const char **)cmd_args); - if (rv != GED_OK){ - bu_log("kill_copy: could not copy \"%s\" to \"%s\"\n", dp->d_namep, - new_name); - return GED_ERROR; - } + /* Copy the passed prim/comb */ + cmd_args[0] = "copy"; + cmd_args[1] = dp->d_namep; + cmd_args[2] = new_name; + cmd_args[3] = (char *)0; + rv = ged_copy(gedp, 3, (const char **)cmd_args); + if (rv != GED_OK){ + bu_log("kill_copy: could not copy \"%s\" to \"%s\"\n", dp->d_namep, + new_name); + return GED_ERROR; + } - return GED_OK; + return GED_OK; } /** - * Print a struct rigid_body for debugging + * Deletes and recreates the simulation result group + * */ +int create_sim_comb(struct ged *gedp, struct simulation_params *sim_params) +{ + char *cmd_args[5]; + int rv; + + /* Check if the duplicate already exists, and kill it if so */ + if (db_lookup(gedp->ged_wdbp->dbip, sim_params->sim_comb_name, LOOKUP_QUIET) != RT_DIR_NULL) { + bu_log("create_sim_comb: WARNING \"%s\" already exists, deleting it\n", sim_params->sim_comb_name); + cmd_args[0] = "kill"; + cmd_args[1] = sim_params->sim_comb_name; + cmd_args[2] = (char *)0; + + if(ged_kill(gedp, 2, (const char **)cmd_args) != GED_OK){ + bu_log("create_sim_comb: could not delete existing \"%s\"\n", sim_params->sim_comb_name); + return GED_ERROR; + } + } + + /* Setup the empty simulation result group */ + cmd_args[0] = "comb"; + cmd_args[1] = sim_params->sim_comb_name; + cmd_args[2] = (char *)0; + rv = ged_comb(gedp, 2, (const char **)cmd_args); + if (rv != GED_OK){ + bu_log("create_sim_comb: Could not create simulation result group \"%s\"\n", + sim_params->sim_comb_name); + return GED_ERROR; + } + + return GED_OK; +} + +/** + * Prints a struct rigid_body for debugging + */ void print_rigid_body(struct rigid_body *rb) { - bu_log("Rigid Body : \"%s\"\n", rb->rb_namep); + bu_log("Rigid Body : \"%s\"\n", rb->rb_namep); } /** @@ -116,105 +152,203 @@ */ int add_regions(struct ged *gedp, struct simulation_params *sim_params) { - struct directory *dp, *ndp; - char *prefixed_name; - char *prefix = "sim_"; - size_t prefix_len, prefixed_name_len; - point_t rpp_min, rpp_max; - /* char *sim_group = "sim.c"; */ - int i; - struct rigid_body *prev_node = NULL, *current_node; + struct directory *dp, *ndp; + char *prefixed_name; + char *prefix = "sim_"; + size_t prefix_len, prefixed_name_len; + point_t rpp_min, rpp_max; + int i; + struct rigid_body *prev_node = NULL, *current_node; + char *cmd_args[5]; + int rv; + create_sim_comb(gedp, sim_params); + sim_params->num_bodies = 0; - sim_params->num_bodies = 0; - /* Walk the directory list duplicating all regions only, skip some regions */ - for (i = 0; i < RT_DBNHASH; i++) - for (dp = gedp->ged_wdbp->dbip->dbi_Head[i]; dp != RT_DIR_NULL; dp = dp->d_forw) { - if ( (dp->d_flags & RT_DIR_HIDDEN) || /* check for hidden comb/prim */ - !(dp->d_flags & RT_DIR_REGION) /* check if region */ - ) - continue; - if (strstr(dp->d_namep, prefix)){ - bu_log("add_regions: Skipping \"%s\" due to \"%s\" in name\n", - dp->d_namep, prefix); - continue; - } + /* Walk the directory list duplicating all regions only, skip some regions */ + for (i = 0; i < RT_DBNHASH; i++) + for (dp = gedp->ged_wdbp->dbip->dbi_Head[i]; dp != RT_DIR_NULL; dp = dp->d_forw) { + if ( (dp->d_flags & RT_DIR_HIDDEN) || /* check for hidden comb/prim */ + !(dp->d_flags & RT_DIR_REGION) /* check if region */ + ) + continue; - /* Duplicate the region */ - prefix_len = strlen(prefix); - prefixed_name_len = strlen(prefix)+strlen(dp->d_namep)+1; - prefixed_name = (char *)bu_malloc(prefixed_name_len, "Adding sim_ prefix"); - bu_strlcpy(prefixed_name, prefix, prefix_len + 1); - bu_strlcat(prefixed_name + prefix_len, dp->d_namep, prefixed_name_len - prefix_len); + if (strstr(dp->d_namep, prefix)){ + bu_vls_printf(gedp->ged_result_str, "add_regions: Skipping \"%s\" due to \"%s\" in name\n", + dp->d_namep, prefix); + continue; + } - kill_copy(gedp, dp, prefixed_name); - bu_log("add_regions: Copied \"%s\" to \"%s\"\n", dp->d_namep, prefixed_name); + /* Duplicate the region */ + prefix_len = strlen(prefix); + prefixed_name_len = strlen(prefix)+strlen(dp->d_namep)+1; + prefixed_name = (char *)bu_malloc(prefixed_name_len, "Adding sim_ prefix"); + bu_strlcpy(prefixed_name, prefix, prefix_len + 1); + bu_strlcat(prefixed_name + prefix_len, dp->d_namep, prefixed_name_len - prefix_len); - /* 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); - return GED_ERROR; - } + 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); - /* 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, - rpp_min[0], rpp_min[1], rpp_min[2], - rpp_max[0], rpp_max[1], rpp_max[2]); - else{ - bu_log("add_regions: Could not get the BB\n"); + /* 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); + 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, + 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: Could not get the BB\n"); + return GED_ERROR; + } + + /* Add to simulation list */ + current_node = (struct rigid_body *)bu_malloc(sizeof(struct rigid_body), "rigid_body: current_node"); + current_node->index = sim_params->num_bodies; + current_node->rb_namep = bu_strdup(prefixed_name); + current_node->dp = ndp; + VMOVE(current_node->bb_min, rpp_min); + VMOVE(current_node->bb_max, rpp_max); + current_node->next = NULL; + + /* Get BB length, width, height */ + current_node->bb_dims[0] = current_node->bb_max[0] - current_node->bb_min[0]; + 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", + current_node->bb_dims[0], current_node->bb_dims[1], current_node->bb_dims[2]); + + /* Get BB position in 3D space */ + current_node->bb_center[0] = current_node->bb_min[0] + current_node->bb_dims[0]/2; + 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; + + /* Setup the linked list */ + if(prev_node == NULL){ /* first node */ + prev_node = current_node; + sim_params->head_node = current_node; + } + else{ /* past 1st node now */ + prev_node->next = current_node; + prev_node = prev_node->next; + } + + /* Setup the simulation result group union-ing the new objects */ + cmd_args[0] = "comb"; + cmd_args[1] = sim_params->sim_comb_name; + cmd_args[2] = "u"; + cmd_args[3] = prefixed_name; + cmd_args[4] = (char *)0; + rv = ged_comb(gedp, 4, (const char **)cmd_args); + if (rv != GED_OK){ + bu_log("add_regions: Could not add \"%s\" to simulation result group \"%s\"\n", + prefixed_name, sim_params->sim_comb_name); return GED_ERROR; } - /* Add to simulation list */ - current_node = (struct rigid_body *)bu_malloc(sizeof(struct rigid_body), "rigid_body: current_node"); - current_node->rb_namep = bu_strdup(prefixed_name); - VMOVE(current_node->bbmin, rpp_min); - VMOVE(current_node->bbmax, rpp_max); - /*current_node->t */ - current_node->next = NULL; + sim_params->num_bodies++; + } - if(prev_node == NULL){ /* first node */ - prev_node = current_node; - sim_params->head_node = current_node; - } - else{ /* past 1st node now */ - prev_node->next = current_node; - prev_node = prev_node->next; - } - sim_params->num_bodies++; - } - /* Show list of objects to be added to the sim */ -/* bu_log("add_regions: The following %d regions will participate in the sim : \n", sim_params->num_bodies); - for (current_node = sim_params->head_node; current_node != NULL; current_node = current_node->next) { - print_rigid_body(current_node); - }*/ - return GED_OK; + /* Show list of objects to be added to the sim : keep for debugging as of now */ + /* bu_log("add_regions: The following %d regions will participate in the sim : \n", sim_params->num_bodies); + for (current_node = sim_params->head_node; current_node != NULL; current_node = current_node->next) { + print_rigid_body(current_node); + }*/ + return GED_OK; + } -/* Will be used later to apply transforms */ -int apply_transforms() +/** + * 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) { - return GED_OK; + struct rt_db_internal intern; + struct rigid_body *current_node; + mat_t t; + + for (current_node = sim_params->head_node; current_node != NULL; current_node = current_node->next) { + /* 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, to apply rotation */ + t[0] = 1; t[1] = 0; t[2] = 0; t[3] = - (current_node->bb_center[0]); + t[4] = 0; t[5] = 1; t[6] = 0; t[7] = - (current_node->bb_center[1]); + t[8] = 0; t[9] = 0; t[10] = 1; t[11] = - (current_node->bb_center[2]); + t[12] = current_node->t[12]; t[13] = current_node->t[13]; t[14] = current_node->t[14]; t[15] = current_node->t[15]; + + 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*/ + t[0] = current_node->t[0]; t[1] = current_node->t[1]; t[2] = current_node->t[2]; t[3] = 0; + t[4] = current_node->t[4]; t[5] = current_node->t[5]; t[6] = current_node->t[6]; t[7] = 0; + t[8] = current_node->t[8]; t[9] = current_node->t[9]; t[10] = current_node->t[10]; t[11] = 0; + t[12] = current_node->t[12]; t[13] = current_node->t[13]; t[14] = current_node->t[14]; t[15] = current_node->t[15]; + + 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 */ + t[0] = 1; t[1] = 0; t[2] = 0; t[3] = current_node->t[3]; + t[4] = 0; t[5] = 1; t[6] = 0; t[7] = current_node->t[7]; + t[8] = 0; t[9] = 0; t[10] = 1; t[11] = current_node->t[11]; + t[12] = current_node->t[12]; t[13] = current_node->t[13]; t[14] = current_node->t[14]; t[15] = current_node->t[15]; + + if (rt_matrix_transform(&intern, current_node->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: Database write error for '%s', aborting\n", + current_node->dp->d_namep); + return GED_ERROR; + } + } + + + return GED_OK; } /** - * The physics simulation function + * 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[]) { int rv; struct simulation_params sim_params; - static const char *simresult = "simresult.r"; + static const char *sim_comb_name = "sim.c"; + static const char *ground_plane_name = "sim_gp.r"; + struct rigid_body *current_node, *next_node; GED_CHECK_DATABASE_OPEN(gedp, GED_ERROR); GED_CHECK_ARGC_GT_0(gedp, argc, GED_ERROR); @@ -237,27 +371,42 @@ /* 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.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); rv = add_regions(gedp, &sim_params); if (rv != GED_OK){ - bu_vls_printf(gedp->ged_result_str, "%s: Error while adding objects\n", argv[0]); - return GED_ERROR; - } + bu_vls_printf(gedp->ged_result_str, "%s: Error while adding objects\n", argv[0]); + return GED_ERROR; + } - /* Pack up the sim parameters for bullet */ - sim_params.duration = atoi(argv[1]); - sim_params.gedp = gedp; - /* Run the physics simulation */ - rv = run_simulation(gedp->ged_result_str, &sim_params); + 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 */ - /* apply_transforms(); */ + 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; + } - bu_vls_printf(gedp->ged_result_str, "%s: The simulation result is in : %s\n", argv[0], simresult); + /* Free memory in rigid_body list */ + for (current_node = sim_params.head_node; current_node != NULL; ) { + next_node = current_node->next; + bu_free(current_node, "simulate : current_node"); + current_node = next_node; + } + + 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] = (char *)simresult; + argv[1] = sim_comb_name; argv[2] = (char *)0; return GED_OK; @@ -281,8 +430,8 @@ bu_vls_trunc(gedp->ged_result_str, 0); bu_vls_printf(gedp->ged_result_str, "%s : This command is disabled due to the absence of a physics library", - argv[0]); - return GED_ERROR; + argv[0]); + return GED_ERROR; } #endif Modified: brlcad/trunk/src/libged/simulate/simulate.h =================================================================== --- brlcad/trunk/src/libged/simulate/simulate.h 2011-09-02 16:54:24 UTC (rev 46557) +++ brlcad/trunk/src/libged/simulate/simulate.h 2011-09-02 17:09:17 UTC (rev 46558) @@ -29,16 +29,19 @@ #ifndef SIMULATE_H_ #define SIMULATE_H_ -/* Contains information about a single rigid body constructed from BRL-CAD geometry. +/* 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 { - char * rb_namep; /**< @brief pointer to name string */ - point_t bbmin, bbmax; /**< @brief body bb bounds */ - mat_t t; /**< @brief current transformation matrix */ - struct rigid_body *next; /**< @brief link to next body */ + int index; + char *rb_namep; /**< @brief pointer to name string */ + point_t bb_min, bb_max; /**< @brief body bb bounds */ + point_t bb_center, bb_dims; /**< @brief bb center and dimensions */ + mat_t t; /**< @brief current transformation matrix */ + struct directory *dp; /**< @brief directory pointer to the related region */ + struct rigid_body *next; /**< @brief link to next body */ }; /* Contains the simulation parameters, such as number of rigid bodies, @@ -46,10 +49,12 @@ * 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 */ - struct ged *gedp; /**< @brief handle to the libged object to access geometry info */ - struct rigid_body *head_node; /**< @brief link to first rigid body node */ + int duration; /**< @brief contains either the number of steps or the time */ + int num_bodies; /**< @brief number of rigid bodies participating in the sim */ + struct bu_vls *result_str; /**< @brief handle to the libged object to access geometry info */ + 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. ------------------------------------------------------------------------------ Special Offer -- Download ArcSight Logger for FREE! Finally, a world-class log management solution at an even better price-free! And you'll get a free "Love Thy Logs" t-shirt when you download Logger. Secure your free ArcSight Logger TODAY! http://p.sf.net/sfu/arcsisghtdev2dev _______________________________________________ BRL-CAD Source Commits mailing list brlcad-commits@lists.sourceforge.net https://lists.sourceforge.net/lists/listinfo/brlcad-commits