Revision: 52516 http://brlcad.svn.sourceforge.net/brlcad/?rev=52516&view=rev Author: Abhi2011 Date: 2012-09-23 14:51:08 +0000 (Sun, 23 Sep 2012) Log Message: ----------- Fixing the simulate function. There are a number of debugging lines still present. These will be removed soon.
Modified Paths: -------------- brlcad/trunk/src/libged/simulate/simphysics.cpp brlcad/trunk/src/libged/simulate/simulate.c brlcad/trunk/src/libged/simulate/simutils.c Modified: brlcad/trunk/src/libged/simulate/simphysics.cpp =================================================================== --- brlcad/trunk/src/libged/simulate/simphysics.cpp 2012-09-21 19:10:55 UTC (rev 52515) +++ brlcad/trunk/src/libged/simulate/simphysics.cpp 2012-09-23 14:51:08 UTC (rev 52516) @@ -54,19 +54,19 @@ rb_namep); for (i=0 ; i<4 ; i++) { - for (j=0 ; j<4 ; j++) { - sprintf(buffer, "%st[%d]: %f\t", buffer, (j*4 + i), t[j*4 + i]); - } - sprintf(buffer, "%s\n", buffer); + for (j=0 ; j<4 ; j++) { + sprintf(buffer, "%st[%d]: %f\t", buffer, (j*4 + i), t[j*4 + i]); + } + sprintf(buffer, "%s\n", buffer); } sprintf(buffer, "%s\n", buffer); for (i=0 ; i<4 ; i++) { - for (j=0 ; j<4 ; j++) { - sprintf(buffer, "%sm[%d]: %f\t", buffer, (j*4 + i), m[j*4 + i]); - } - sprintf(buffer, "%s\n", buffer); + for (j=0 ; j<4 ; j++) { + sprintf(buffer, "%sm[%d]: %f\t", buffer, (j*4 + i), m[j*4 + i]); + } + sprintf(buffer, "%s\n", buffer); } sprintf(buffer, "%s-------------------------------------------------------\n", buffer); @@ -94,95 +94,93 @@ for (current_node = sim_params->head_node; current_node != NULL; current_node = current_node->next) { - current_node->iter = sim_params->iter; + current_node->iter = sim_params->iter; - // Check if we should add a ground plane - if (BU_STR_EQUAL(current_node->rb_namep, sim_params->ground_plane_name)) { - // Add a static ground plane : should be controlled by an option : TODO - btCollisionShape* groundShape = new btBoxShape(btVector3(current_node->bb_dims[0]/2, + // Check if we should add a ground plane - a static rigid body + if (BU_STR_EQUAL(current_node->rb_namep, sim_params->ground_plane_name)) { + // 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)); - //btCollisionShape* groundShape = new btSphereShape(0.5); + //btCollisionShape* groundShape = new btSphereShape(0.5); - btDefaultMotionState* groundMotionState = new btDefaultMotionState( + btDefaultMotionState* groundMotionState = new btDefaultMotionState( btTransform(btQuaternion(0, 0, 0, 1), btVector3(current_node->bb_dims[0]/2, current_node->bb_dims[1]/2, current_node->bb_dims[2]/2) )); - //Copy the transform matrix - MAT_COPY(m, current_node->m); - groundMotionState->m_graphicsWorldTrans.setFromOpenGLMatrix(m); + //Copy the transform matrix + MAT_COPY(m, current_node->m); + groundMotionState->m_graphicsWorldTrans.setFromOpenGLMatrix(m); - /* btCollisionShape* groundShape = new btStaticPlaneShape(btVector3(0, 0, 1), 1); - btDefaultMotionState* groundMotionState = - new btDefaultMotionState(btTransform(btQuaternion(0, 0, 0, 1), btVector3(0, 0, -1)));*/ + /* btCollisionShape* groundShape = new btStaticPlaneShape(btVector3(0, 0, 1), 1); + btDefaultMotionState* groundMotionState = + new btDefaultMotionState(btTransform(btQuaternion(0, 0, 0, 1), btVector3(0, 0, -1)));*/ - btRigidBody::btRigidBodyConstructionInfo - groundRigidBodyCI(0, groundMotionState, groundShape, btVector3(0, 0, 0)); - btRigidBody* groundRigidBody = new btRigidBody(groundRigidBodyCI); - groundRigidBody->setUserPointer((void *)current_node); + 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); + dynamicsWorld->addRigidBody(groundRigidBody); + collision_shapes.push_back(groundShape); - bu_log("Added static ground plane : %s to simulation with mass %f Kg at (%f,%f,%f)\n", - current_node->rb_namep, 0.f, current_node->bb_dims[0]/2, - current_node->bb_dims[1]/2, - current_node->bb_dims[2]/2); + bu_log("Added static ground plane : %s to simulation with mass %f Kg at (%f,%f,%f)\n", + current_node->rb_namep, 0.f, m[12], m[13], m[14]); - } 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)); + } else{ + //Nope, its a dynamic 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)); - //btCollisionShape* bb_Shape = new btSphereShape(0.5); - collision_shapes.push_back(bb_Shape); + //btCollisionShape* bb_Shape = new btSphereShape(0.5); + collision_shapes.push_back(bb_Shape); - volume = current_node->bb_dims[0] * current_node->bb_dims[1] * current_node->bb_dims[2]; - mass = volume; // density is 1 + volume = current_node->bb_dims[0] * current_node->bb_dims[1] * current_node->bb_dims[2]; + mass = 1.0; //volume; // density is 1 - btVector3 bb_Inertia(0, 0, 0); - bb_Shape->calculateLocalInertia(mass, bb_Inertia); + btVector3 bb_Inertia(0, 0, 0); + bb_Shape->calculateLocalInertia(mass, bb_Inertia); - btDefaultMotionState* bb_MotionState = new btDefaultMotionState(btTransform(btQuaternion(0, 0, 0, 1), - btVector3(0, 0, 0))); - /*btDefaultMotionState* bb_MotionState = new btDefaultMotionState( + /*btDefaultMotionState* bb_MotionState = new btDefaultMotionState(btTransform(btQuaternion(0, 0, 0, 1), + btVector3(0, 0, 10)));*/ + + btDefaultMotionState* bb_MotionState = new btDefaultMotionState( btTransform(btQuaternion(0, 0, 0, 1), btVector3(current_node->bb_dims[0]/2, current_node->bb_dims[1]/2, current_node->bb_dims[2]/2) - ));*/ + )); - //Copy the transform matrix - MAT_COPY(m, current_node->m); - bb_MotionState->m_graphicsWorldTrans.setFromOpenGLMatrix(m); + //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); + btRigidBody::btRigidBodyConstructionInfo bb_RigidBodyCI(mass, bb_MotionState, bb_Shape, bb_Inertia); + btRigidBody* bb_RigidBody = new btRigidBody(bb_RigidBodyCI); + bb_RigidBody->setUserPointer((void *)current_node); + bu_log("Setting linear velocity as : %f, %f, %f", + current_node->linear_velocity[0], + current_node->linear_velocity[1], + current_node->linear_velocity[2]); - VMOVE(v, current_node->linear_velocity); - bb_RigidBody->setLinearVelocity(v); + VMOVE(v, current_node->linear_velocity); + bb_RigidBody->setLinearVelocity(v); - VMOVE(v, current_node->angular_velocity); - bb_RigidBody->setAngularVelocity(v); + VMOVE(v, current_node->angular_velocity); + bb_RigidBody->setAngularVelocity(v); - dynamicsWorld->addRigidBody(bb_RigidBody); + dynamicsWorld->addRigidBody(bb_RigidBody); - bu_log("Added new rigid body : %s to simulation with mass %f Kg \ - at (%f,%f,%f)\n", - current_node->rb_namep, mass, - current_node->bb_dims[0]/2, - current_node->bb_dims[1]/2, - current_node->bb_dims[2]/2); + bu_log("Added new rigid body : %s to simulation with mass %f Kg at (%f,%f,%f)\n", + current_node->rb_namep, mass, m[12], m[13], m[14]); - } + } } @@ -197,13 +195,13 @@ int step_physics(btDiscreteDynamicsWorld* dynamicsWorld) { - 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_vls_printf(sim_params->result_str, "Simulation will run for %d steps.\n", sim_params->duration); + //bu_vls_printf(sim_params->result_str, "----- Starting simulation -----\n"); //time step of 1/60th of a second(same as internal fixedTimeStep, maxSubSteps=10 to cover 1/60th sec.) dynamicsWorld->stepSimulation(1/60.f, 10); - bu_vls_printf(sim_params->result_str, "----- Simulation Complete -----\n"); + //bu_vls_printf(sim_params->result_str, "----- Simulation Complete -----\n"); return 0; } @@ -226,57 +224,62 @@ 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 + //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()) { + 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]); + //Get the motion state and the world transform from it + btDefaultMotionState* bb_MotionState = (btDefaultMotionState*)bb_RigidBody->getMotionState(); + bb_MotionState->m_graphicsWorldTrans.getOpenGLMatrix(m); - struct rigid_body *current_node = (struct rigid_body *)bb_RigidBody->getUserPointer(); + bu_log("Position : %f, %f, %f\n", m[12], m[13], m[14]); - if (current_node == NULL) { - bu_vls_printf(sim_params->result_str, "get_transforms : Could not get the user pointer \ - (ground plane perhaps)\n"); - continue; + 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 - MAT_COPY(current_node->m, m); + } - //print_matrices(current_node->rb_namep, current_node->m, m); + //Copy the transform matrix + MAT_COPY(current_node->m, m); - //Get the state of the body - current_node->state = bb_RigidBody->getActivationState(); + print_matrices(current_node->rb_namep, current_node->m, m); - //Get the AABB of those bodies, which do not overlap - bb_Shape->getAabb(bb_MotionState->m_graphicsWorldTrans, aabbMin, aabbMax); + //Get the state of the body + current_node->state = bb_RigidBody->getActivationState(); - VMOVE(current_node->btbb_min, aabbMin); - VMOVE(current_node->btbb_max, aabbMax); + //Get the AABB of those bodies, which do not overlap + bb_Shape->getAabb(bb_MotionState->m_graphicsWorldTrans, aabbMin, aabbMax); - // Get BB length, width, height - VSUB2(current_node->btbb_dims, current_node->btbb_max, current_node->btbb_min); + VMOVE(current_node->btbb_min, aabbMin); + VMOVE(current_node->btbb_max, aabbMax); - bu_vls_printf(sim_params->result_str, "get_transforms: Dimensions of this BB : %f %f %f\n", + // Get BB length, width, height + VSUB2(current_node->btbb_dims, current_node->btbb_max, current_node->btbb_min); + + bu_vls_printf(sim_params->result_str, "get_transforms: Dimensions of this BB : %f %f %f\n", current_node->btbb_dims[0], current_node->btbb_dims[1], current_node->btbb_dims[2]); - //Get BB position in 3D space - VCOMB2(current_node->btbb_center, 1, current_node->btbb_min, 0.5, current_node->btbb_dims) + //Get BB position in 3D space + VCOMB2(current_node->btbb_center, 1, current_node->btbb_min, 0.5, current_node->btbb_dims) - v = bb_RigidBody->getLinearVelocity(); - VMOVE(current_node->linear_velocity, v); + v = bb_RigidBody->getLinearVelocity(); + VMOVE(current_node->linear_velocity, v); - v = bb_RigidBody->getAngularVelocity(); - VMOVE(current_node->angular_velocity, v); + bu_log("Got linear velocity as : %f, %f, %f", current_node->linear_velocity[0], + current_node->linear_velocity[1], + current_node->linear_velocity[2]); - } + v = bb_RigidBody->getAngularVelocity(); + VMOVE(current_node->angular_velocity, v); + + } } return 0; @@ -519,9 +522,9 @@ /*dispatcher->registerCollisionCreateFunc(SPHERE_SHAPE_PROXYTYPE, SPHERE_SHAPE_PROXYTYPE, new btRTCollisionAlgorithm::CreateFunc);*/ - dispatcher->registerCollisionCreateFunc(BOX_SHAPE_PROXYTYPE, + /* dispatcher->registerCollisionCreateFunc(BOX_SHAPE_PROXYTYPE, BOX_SHAPE_PROXYTYPE, - new btRTCollisionAlgorithm::CreateFunc); + new btRTCollisionAlgorithm::CreateFunc);*/ btSequentialImpulseConstraintSolver* solver = new btSequentialImpulseConstraintSolver; @@ -533,8 +536,10 @@ //Add the rigid bodies to the world, including the ground plane add_rigid_bodies(dynamicsWorld, collision_shapes); + bu_log("After add_rigid_bodies()\n"); + //Add a broadphase callback to hook to the AABB detection algos - btOverlapFilterCallback * filterCallback = new broadphase_callback(); +/* btOverlapFilterCallback * filterCallback = new broadphase_callback(); dynamicsWorld->getPairCache()->setOverlapFilterCallback(filterCallback); //Add a nearphase callback to hook to the contact points generation algos @@ -543,19 +548,25 @@ //Investigating the contact pairs used between 2 rigid bodies gContactAddedCallback = contact_added; gContactProcessedCallback = contact_processed; - gContactDestroyedCallback = contact_destroyed; + gContactDestroyedCallback = contact_destroyed;*/ //Step the physics the required number of times step_physics(dynamicsWorld); + bu_log("After step_physics()\n"); + //Get the world transforms back into the simulation params struct get_transforms(dynamicsWorld); + bu_log("After get_transforms()\n"); + //Clean and free memory used by physics objects cleanup(dynamicsWorld, collision_shapes); + bu_log("After cleanup()\n"); + //Clean up stuff in here - delete filterCallback; + //delete filterCallback; delete solver; delete dispatcher; delete collisionConfiguration; Modified: brlcad/trunk/src/libged/simulate/simulate.c =================================================================== --- brlcad/trunk/src/libged/simulate/simulate.c 2012-09-21 19:10:55 UTC (rev 52515) +++ brlcad/trunk/src/libged/simulate/simulate.c 2012-09-23 14:51:08 UTC (rev 52516) @@ -91,18 +91,18 @@ 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 */ + !(dp->d_flags & RT_DIR_REGION) /* check if region */ ) continue; if (strstr(dp->d_namep, prefix)) { - bu_vls_printf(gedp->ged_result_str, "add_regions: Skipping \"%s\" due to \"%s\" in name\n", + bu_vls_printf(gedp->ged_result_str, "add_regions: Skipping \"%s\" due to \"%s\" in name\n", dp->d_namep, prefix); continue; } if (BU_STR_EMPTY(dp->d_namep)) { - bu_vls_printf(gedp->ged_result_str, "add_regions: Skipping \"%s\" due to empty name\n", + bu_vls_printf(gedp->ged_result_str, "add_regions: Skipping \"%s\" due to empty name\n", dp->d_namep); continue; } @@ -116,8 +116,8 @@ /* Get the directory pointer for the object just added */ if ((ndp=db_lookup(gedp->ged_wdbp->dbip, bu_vls_addr(&dp_name_vls), LOOKUP_QUIET)) == RT_DIR_NULL) { - bu_vls_printf(gedp->ged_result_str, "add_regions: db_lookup(%s) failed", bu_vls_addr(&dp_name_vls)); - return GED_ERROR; + bu_vls_printf(gedp->ged_result_str, "add_regions: db_lookup(%s) failed", bu_vls_addr(&dp_name_vls)); + return GED_ERROR; } @@ -141,13 +141,13 @@ /* Setup the linked list */ if (prev_node == NULL) { - /* first node */ - prev_node = current_node; - sim_params->head_node = current_node; + /* 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; + /* past 1st node now */ + prev_node->next = current_node; + prev_node = prev_node->next; } /* Add the new region to the simulation result */ @@ -159,8 +159,8 @@ bu_vls_free(&dp_name_vls); if(sim_params->num_bodies == 0){ - bu_vls_printf(gedp->ged_result_str, "add_regions: ERROR No objects were added\n"); - return GED_ERROR; + bu_vls_printf(gedp->ged_result_str, "add_regions: ERROR No objects were added\n"); + return GED_ERROR; } /* Show list of objects to be added to the sim : keep for debugging as of now */ @@ -252,6 +252,8 @@ return GED_ERROR; } + bu_log("After 1"); + /* Apply inverse rotation with no translation to undo previous iteration's rotation */ MAT_COPY(m, current_node->m_prev); m[12] = 0; @@ -265,6 +267,8 @@ return GED_ERROR; } + bu_log("After 2"); + /*---------------------- Now apply current transformation -------------------------*/ /* Apply rotation with no translation*/ @@ -280,6 +284,8 @@ return GED_ERROR; } + bu_log("After 3"); + /* Translate again without any rotation, to apply final position */ MAT_IDN(m); m[12] = current_node->m[12]; @@ -294,6 +300,8 @@ return GED_ERROR; } + bu_log("After 4"); + /* 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", @@ -301,14 +309,16 @@ return GED_ERROR; } + bu_log("After 5"); + /* Store this world transformation to undo it before next world transformation */ MAT_COPY(current_node->m_prev, current_node->m); - insert_AABB(gedp, sim_params, current_node); + /*insert_AABB(gedp, sim_params, current_node); print_manifold_list(current_node); - insert_manifolds(gedp, sim_params, current_node); + insert_manifolds(gedp, sim_params, current_node);*/ current_node->num_bt_manifolds = 0; @@ -324,11 +334,13 @@ */ int recreate_sim_comb(struct ged *gedp, struct simulation_params *sim_params) { - struct rigid_body *current_node; + struct rigid_body *current_node; + printf("recreate_sim_comb: enter "); + if (sim_kill(gedp, sim_params->sim_comb_name) != GED_OK) { - bu_log("sim_kill_copy: ERROR Could not delete existing \"%s\"\n", sim_params->sim_comb_name); - return GED_ERROR; + bu_log("sim_kill_copy: ERROR Could not delete existing \"%s\"\n", sim_params->sim_comb_name); + return GED_ERROR; } for (current_node = sim_params->head_node; current_node != NULL; @@ -336,6 +348,8 @@ add_to_comb(gedp, sim_params->sim_comb_name, current_node->rb_namep); } + printf("recreate_sim_comb: exit "); + return GED_OK; } @@ -441,11 +455,13 @@ sim_params.rtip->useair = 1; /* Initialize the raytrace world */ - init_raytrace(&sim_params); + //init_raytrace(&sim_params); /* Recreate sim.c to clear AABBs and manifold regions from previous iteration */ recreate_sim_comb(gedp, &sim_params); + bu_log("After recreate_sim_comb()"); + /* Run the physics simulation */ sim_params.iter = i; rv = run_simulation(&sim_params); @@ -454,6 +470,8 @@ return GED_ERROR; } + bu_log("After run_simulation()"); + /* Apply transforms on the participating objects, also shades objects */ rv = apply_transforms(gedp, &sim_params); if (rv != GED_OK) { @@ -461,6 +479,8 @@ return GED_ERROR; } + bu_log("After apply_transforms()"); + /* free the raytrace instance */ rt_free_rti(sim_params.rtip); Modified: brlcad/trunk/src/libged/simulate/simutils.c =================================================================== --- brlcad/trunk/src/libged/simulate/simutils.c 2012-09-21 19:10:55 UTC (rev 52515) +++ brlcad/trunk/src/libged/simulate/simutils.c 2012-09-23 14:51:08 UTC (rev 52516) @@ -669,16 +669,22 @@ sprintf(buffer, "%f", v[1]); cmd_args[25] = bu_strdup(buffer); sprintf(buffer, "%f", v[2]); cmd_args[26] = bu_strdup(buffer); + bu_log("reached here"); + /* Finally make the bb primitive, phew ! */ cmd_args[27] = (char *)0; rv = ged_in(gedp, argc, (const char **)cmd_args); if (rv != GED_OK) { - bu_log("insertAABB: WARNING Could not draw bounding box for \"%s\"\n", + bu_log("insertAABB: WARNING Could not draw bounding box for \"%s\"\n", current_node->rb_namep); } + bu_log("after command"); + bu_free_array(argc, cmd_args, "make_rpp: free cmd_args"); + bu_log("after free"); + /* Make the region for the bb primitive */ add_to_comb(gedp, prefixed_reg_name, prefixed_name); @@ -691,6 +697,8 @@ bu_vls_free(&buffer1); bu_vls_free(&buffer2); + bu_log("reached out"); + return GED_OK; } This was sent by the SourceForge.net collaborative development platform, the world's largest Open Source development site. ------------------------------------------------------------------------------ Everyone hates slow websites. So do we. Make your web apps faster with AppDynamics Download AppDynamics Lite for free today: http://ad.doubleclick.net/clk;258768047;13503038;j? http://info.appdynamics.com/FreeJavaPerformanceDownload.html _______________________________________________ BRL-CAD Source Commits mailing list brlcad-commits@lists.sourceforge.net https://lists.sourceforge.net/lists/listinfo/brlcad-commits