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
