[brlcad-commits] SF.net SVN: brlcad:[47049] brlcad/trunk/src/libged/simulate
Open Source Solid Modeling CAD
Brought to you by:
brlcad
From: <abh...@us...> - 2011-10-04 17:47:35
|
Revision: 47049 http://brlcad.svn.sourceforge.net/brlcad/?rev=47049&view=rev Author: abhi2011 Date: 2011-10-04 17:47:22 +0000 (Tue, 04 Oct 2011) Log Message: ----------- Added more code to check the generated manifolds Modified Paths: -------------- brlcad/trunk/src/libged/simulate/simcollisionalgo.cpp brlcad/trunk/src/libged/simulate/simphysics.cpp brlcad/trunk/src/libged/simulate/simulate.c brlcad/trunk/src/libged/simulate/simulate.h Modified: brlcad/trunk/src/libged/simulate/simcollisionalgo.cpp =================================================================== --- brlcad/trunk/src/libged/simulate/simcollisionalgo.cpp 2011-10-04 17:29:55 UTC (rev 47048) +++ brlcad/trunk/src/libged/simulate/simcollisionalgo.cpp 2011-10-04 17:47:22 UTC (rev 47049) @@ -40,10 +40,10 @@ btRTCollisionAlgorithm::btRTCollisionAlgorithm( - btPersistentManifold* mf, - const btCollisionAlgorithmConstructionInfo& ci, - btCollisionObject* obj0, - btCollisionObject* obj1) + btPersistentManifold* mf, + const btCollisionAlgorithmConstructionInfo& ci, + btCollisionObject* obj0, + btCollisionObject* obj1) : btActivatingCollisionAlgorithm(ci, obj0, obj1), m_ownManifold(false), m_manifoldPtr(mf) @@ -66,10 +66,10 @@ void btRTCollisionAlgorithm::processCollision ( - btCollisionObject* body0, - btCollisionObject* body1, - const btDispatcherInfo& dispatchInfo, - btManifoldResult* resultOut) + btCollisionObject* body0, + btCollisionObject* body1, + const btDispatcherInfo& dispatchInfo, + btManifoldResult* resultOut) { if (!m_manifoldPtr) return; @@ -108,6 +108,8 @@ struct sim_manifold *current_manifold = (struct sim_manifold *)bu_malloc(sizeof(struct sim_manifold), "sim_manifold: current_manifold"); current_manifold->next = NULL; + current_manifold->rbA = rbA; + current_manifold->rbB = rbB; if (rbB->first_manifold == NULL) { rbB->first_manifold = current_manifold; Modified: brlcad/trunk/src/libged/simulate/simphysics.cpp =================================================================== --- brlcad/trunk/src/libged/simulate/simphysics.cpp 2011-10-04 17:29:55 UTC (rev 47048) +++ brlcad/trunk/src/libged/simulate/simphysics.cpp 2011-10-04 17:47:22 UTC (rev 47049) @@ -45,22 +45,22 @@ char buffer[800]; sprintf(buffer, "------------Phy : Transformation matrices(%s)--------------\n", - rb_namep); + rb_namep); for (i=0 ; i<4 ; i++) { - for (j=0 ; j<4 ; j++) { + 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); } 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); @@ -89,82 +89,82 @@ for (current_node = sim_params->head_node; current_node != NULL; current_node = current_node->next) { - // Check if we should add a ground plane - if (strcmp(current_node->rb_namep, sim_params->ground_plane_name) == 0) { - // Add a static ground plane : should be controlled by an option : TODO - btCollisionShape* groundShape = new btBoxShape(btVector3(current_node->bb_dims[0]/2, - current_node->bb_dims[1]/2, - current_node->bb_dims[2]/2)); - /*btDefaultMotionState* groundMotionState = new btDefaultMotionState( + // 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* 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])));*/ - btDefaultMotionState* groundMotionState = new btDefaultMotionState(btTransform(btQuaternion(0, 0, 0, 1), + 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); + //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 + btRigidBody::btRigidBodyConstructionInfo groundRigidBodyCI(0, groundMotionState, groundShape, btVector3(0, 0, 0)); - btRigidBody* groundRigidBody = new btRigidBody(groundRigidBodyCI); - groundRigidBody->setUserPointer((void *)current_node); + 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_vls_printf(sim_params->result_str, "Added static ground plane : %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{ - //Nope, its a rigid body - btCollisionShape* bb_Shape = new btBoxShape(btVector3(current_node->bb_dims[0]/2, + } 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); + 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 = 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(current_node->bb_center[0], current_node->bb_center[1], current_node->bb_center[2])));*/ - btDefaultMotionState* bb_MotionState = new btDefaultMotionState(btTransform(btQuaternion(0, 0, 0, 1), + 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); + //Copy the transform matrix + MAT_COPY(m, current_node->m); + bb_MotionState->m_graphicsWorldTrans.setFromOpenGLMatrix(m); - btRigidBody::btRigidBodyConstructionInfo + 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* bb_RigidBody = new btRigidBody(bb_RigidBodyCI); + bb_RigidBody->setUserPointer((void *)current_node); - 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_vls_printf(sim_params->result_str, "Added new rigid body : %s to simulation with mass %f Kg\n", + bu_vls_printf(sim_params->result_str, "Added new rigid body : %s to simulation with mass %f Kg\n", current_node->rb_namep, mass); - } + } } @@ -208,57 +208,57 @@ 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); + //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(); + 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; + 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); + //Copy the transform matrix + MAT_COPY(current_node->m, m); - //print_matrices(current_node->rb_namep, current_node->m, m); + //print_matrices(current_node->rb_namep, current_node->m, m); - //Get the state of the body - current_node->state = bb_RigidBody->getActivationState(); + //Get the state of the body + current_node->state = bb_RigidBody->getActivationState(); - //Get the AABB - bb_Shape->getAabb(bb_MotionState->m_graphicsWorldTrans, aabbMin, aabbMax); + //Get the AABB + bb_Shape->getAabb(bb_MotionState->m_graphicsWorldTrans, aabbMin, aabbMax); - VMOVE(current_node->btbb_min, aabbMin); - VMOVE(current_node->btbb_max, aabbMax); + VMOVE(current_node->btbb_min, aabbMin); + VMOVE(current_node->btbb_max, aabbMax); - // Get BB length, width, height - VSUB2(current_node->btbb_dims, current_node->btbb_max, current_node->btbb_min); + // 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", + 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); + VMOVE(current_node->linear_velocity, v); - v = bb_RigidBody->getAngularVelocity(); - VMOVE(current_node->angular_velocity, v); + v = bb_RigidBody->getAngularVelocity(); + VMOVE(current_node->angular_velocity, v); - } + } } return 0; @@ -278,19 +278,19 @@ for (i=dynamicsWorld->getNumCollisionObjects()-1; i>=0; i--) { - btCollisionObject* obj = dynamicsWorld->getCollisionObjectArray()[i]; - btRigidBody* body = btRigidBody::upcast(obj); - if (body && body->getMotionState()) { - delete body->getMotionState(); - } - dynamicsWorld->removeCollisionObject(obj); - delete obj; + btCollisionObject* obj = dynamicsWorld->getCollisionObjectArray()[i]; + btRigidBody* body = btRigidBody::upcast(obj); + if (body && body->getMotionState()) { + delete body->getMotionState(); + } + dynamicsWorld->removeCollisionObject(obj); + delete obj; } //delete collision shapes for (i=0; i<collision_shapes.size(); i++) { - btCollisionShape* shape = collision_shapes[i]; - delete shape; + btCollisionShape* shape = collision_shapes[i]; + delete shape; } //delete dynamics world @@ -311,25 +311,25 @@ //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); + 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; - btRigidBody* box0 = (btRigidBody*)proxy0->m_clientObject; - btRigidBody* box1 = (btRigidBody*)proxy1->m_clientObject; - if (box0 != NULL && box0 != NULL) { + //This would prevent collision between proxy0 and proxy1 inspite of + //AABB overlap being detected + //collides = false; + btRigidBody* box0 = (btRigidBody*)proxy0->m_clientObject; + btRigidBody* box1 = (btRigidBody*)proxy1->m_clientObject; + if (box0 != NULL && box0 != NULL) { struct rigid_body *upA = (struct rigid_body *)box0->getUserPointer(); struct rigid_body *upB = (struct rigid_body *)box1->getUserPointer(); if (upA != NULL && upB != NULL) bu_log("broadphase_callback: %s & %s has overlapping AABBs", upA->rb_namep, upB->rb_namep); - } + } - //add some additional logic here that modifies 'collides' - return collides; + //add some additional logic here that modifies 'collides' + return collides; } }; @@ -353,47 +353,47 @@ */ for (i=0; i<numManifolds; i++) { - //Get the manifold and the objects which created it - btPersistentManifold* contactManifold = + //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()); + 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 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(); + //Get the number of points in this manifold + numContacts = contactManifold->getNumContacts(); - bu_log("nearphase_callback : Manifold %d of %d, contacts : %d\n", + 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); + //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(); + 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", + 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", + } 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(); + //Can un-comment this line, and then all points are removed + //contactManifold->clearManifold(); } // Only dispatch the Bullet collision information if physics should continue @@ -427,9 +427,9 @@ //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); + BOX_SHAPE_PROXYTYPE, + BOX_SHAPE_PROXYTYPE, + new btRTCollisionAlgorithm::CreateFunc); btSequentialImpulseConstraintSolver* solver = new btSequentialImpulseConstraintSolver; Modified: brlcad/trunk/src/libged/simulate/simulate.c =================================================================== --- brlcad/trunk/src/libged/simulate/simulate.c 2011-10-04 17:29:55 UTC (rev 47048) +++ brlcad/trunk/src/libged/simulate/simulate.c 2011-10-04 17:47:22 UTC (rev 47049) @@ -52,13 +52,13 @@ static void print_usage(struct bu_vls *str) { - 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; + 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,22 +69,22 @@ void print_matrix(char *rb_namep, mat_t t) { - int i, j; - struct bu_vls buffer = BU_VLS_INIT_ZERO; + int i, j; + struct bu_vls buffer = BU_VLS_INIT_ZERO; - bu_vls_sprintf(&buffer, "------------Transformation matrix(%s)--------------\n", - rb_namep); + bu_vls_sprintf(&buffer, "------------Transformation matrix(%s)--------------\n", + rb_namep); - for (i=0 ; i<4 ; i++) { - for (j=0 ; j<4 ; j++) { - bu_vls_sprintf(&buffer, "t[%d]: %f\t", (i*4 + j), t[i*4 + j]); - } - bu_vls_strcat(&buffer, "\n"); + for (i=0 ; i<4 ; i++) { + for (j=0 ; j<4 ; j++) { + bu_vls_sprintf(&buffer, "t[%d]: %f\t", (i*4 + j), t[i*4 + j]); } + bu_vls_strcat(&buffer, "\n"); + } - bu_vls_strcat(&buffer, "-------------------------------------------------------\n"); - bu_log("%V", &buffer); - bu_vls_free(&buffer); + bu_vls_strcat(&buffer, "-------------------------------------------------------\n"); + bu_log("%V", &buffer); + bu_vls_free(&buffer); } @@ -94,7 +94,7 @@ void print_rigid_body(struct rigid_body *rb) { - bu_log("print_rigid_body : \"%s\", state = %d\n", rb->rb_namep, rb->state); + bu_log("print_rigid_body : \"%s\", state = %d\n", rb->rb_namep, rb->state); } @@ -104,27 +104,30 @@ void print_manifold_list(struct rigid_body *rb) { - struct sim_manifold *current_manifold; - int i; + struct sim_manifold *current_manifold; + int i; - bu_log("print_manifold_list: %s\n", rb->rb_namep); + bu_log("print_manifold_list: %s\n", rb->rb_namep); - for (current_manifold = rb->first_manifold; current_manifold != NULL; - current_manifold = current_manifold->next) { - for (i=0; i<current_manifold->num_contacts; i++) { - bu_log("contact %d of %d, (%f, %f, %f):(%f, %f, %f), n(%f, %f, %f)\n", - i+1, current_manifold->num_contacts, - current_manifold->rb_contacts[i].ptA[0], - current_manifold->rb_contacts[i].ptA[1], - current_manifold->rb_contacts[i].ptA[2], - current_manifold->rb_contacts[i].ptB[0], - current_manifold->rb_contacts[i].ptB[1], - current_manifold->rb_contacts[i].ptB[2], - current_manifold->rb_contacts[i].normalWorldOnB[0], - current_manifold->rb_contacts[i].normalWorldOnB[1], - current_manifold->rb_contacts[i].normalWorldOnB[2]); - } + for (current_manifold = rb->first_manifold; current_manifold != NULL; + current_manifold = current_manifold->next) { + bu_log("Manifold between %s and %s(%d contacts):\n", current_manifold->rbA->rb_namep, + current_manifold->rbB->rb_namep, + current_manifold->num_contacts); + for (i=0; i<current_manifold->num_contacts; i++) { + bu_log("%d, (%f, %f, %f):(%f, %f, %f), n(%f, %f, %f)\n", + i+1, + current_manifold->rb_contacts[i].ptA[0], + current_manifold->rb_contacts[i].ptA[1], + current_manifold->rb_contacts[i].ptA[2], + current_manifold->rb_contacts[i].ptB[0], + current_manifold->rb_contacts[i].ptB[1], + current_manifold->rb_contacts[i].ptB[2], + current_manifold->rb_contacts[i].normalWorldOnB[0], + current_manifold->rb_contacts[i].normalWorldOnB[1], + current_manifold->rb_contacts[i].normalWorldOnB[2]); } + } } @@ -134,13 +137,13 @@ void print_command(char* cmd_args[], int num_args) { - int i; - char buffer[200] = ""; - for (i=0; i<num_args; i++) { - sprintf(buffer, "%s %s", buffer, cmd_args[i]); - } + int i; + char buffer[500] = ""; + for (i=0; i<num_args; i++) { + sprintf(buffer, "%s %s", buffer, cmd_args[i]); + } - bu_log(buffer); + bu_log(buffer); } @@ -150,17 +153,17 @@ char* prefix_name(char *prefix, char *original) { - /* Prepare prefixed bounding box primitive name */ - size_t prefix_len, prefixed_name_len; - char *prefixed_name; + /* Prepare prefixed bounding box primitive name */ + size_t prefix_len, prefixed_name_len; + char *prefixed_name; - prefix_len = strlen(prefix); - prefixed_name_len = strlen(prefix)+strlen(original) + 1; - prefixed_name = (char *)bu_malloc(prefixed_name_len, "Adding prefix"); - bu_strlcpy(prefixed_name, prefix, prefix_len + 1); - bu_strlcat(prefixed_name + prefix_len, original, - prefixed_name_len - prefix_len); - return prefixed_name; + prefix_len = strlen(prefix); + prefixed_name_len = strlen(prefix)+strlen(original) + 1; + prefixed_name = (char *)bu_malloc(prefixed_name_len, "Adding prefix"); + bu_strlcpy(prefixed_name, prefix, prefix_len + 1); + bu_strlcat(prefixed_name + prefix_len, original, + prefixed_name_len - prefix_len); + return prefixed_name; } @@ -172,22 +175,22 @@ int kill(struct ged *gedp, char *name) { - char *cmd_args[5]; + char *cmd_args[5]; - /* Check if the duplicate already exists, and kill it if so */ - if (db_lookup(gedp->ged_wdbp->dbip, name, LOOKUP_QUIET) != RT_DIR_NULL) { - bu_log("kill: WARNING \"%s\" exists, deleting it\n", name); - cmd_args[0] = "kill"; - cmd_args[1] = name; - cmd_args[2] = (char *)0; + /* Check if the duplicate already exists, and kill it if so */ + if (db_lookup(gedp->ged_wdbp->dbip, name, LOOKUP_QUIET) != RT_DIR_NULL) { + bu_log("kill: WARNING \"%s\" exists, deleting it\n", name); + cmd_args[0] = "kill"; + cmd_args[1] = name; + cmd_args[2] = (char *)0; - if (ged_kill(gedp, 2, (const char **)cmd_args) != GED_OK) { - bu_log("kill: ERROR Could not delete existing \"%s\"\n", name); - return GED_ERROR; - } + if (ged_kill(gedp, 2, (const char **)cmd_args) != GED_OK) { + bu_log("kill: ERROR Could not delete existing \"%s\"\n", name); + return GED_ERROR; } + } - return GED_OK; + return GED_OK; } @@ -199,27 +202,27 @@ int kill_copy(struct ged *gedp, struct directory *dp, char* new_name) { - char *cmd_args[5]; - int rv; + char *cmd_args[5]; + int rv; - if (kill(gedp, new_name) != GED_OK) { - bu_log("kill_copy: ERROR Could not delete existing \"%s\"\n", new_name); - return GED_ERROR; - } + if (kill(gedp, new_name) != GED_OK) { + bu_log("kill_copy: ERROR 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: ERROR 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: ERROR Could not copy \"%s\" to \"%s\"\n", dp->d_namep, + new_name); + return GED_ERROR; + } - return GED_OK; + return GED_OK; } @@ -231,22 +234,22 @@ int add_to_comb(struct ged *gedp, char *target, char *add) { - char *cmd_args[5]; - int rv; + char *cmd_args[5]; + int rv; - cmd_args[0] = "comb"; - cmd_args[1] = target; - cmd_args[2] = "u"; - cmd_args[3] = add; - cmd_args[4] = (char *)0; - rv = ged_comb(gedp, 4, (const char **)cmd_args); - if (rv != GED_OK) { - bu_log("add_to_comb: ERROR Could not add \"%s\" to the combination \"%s\"\n", - target, add); - return GED_ERROR; - } + cmd_args[0] = "comb"; + cmd_args[1] = target; + cmd_args[2] = "u"; + cmd_args[3] = add; + cmd_args[4] = (char *)0; + rv = ged_comb(gedp, 4, (const char **)cmd_args); + if (rv != GED_OK) { + bu_log("add_to_comb: ERROR Could not add \"%s\" to the combination \"%s\"\n", + target, add); + return GED_ERROR; + } - return GED_OK; + return GED_OK; } @@ -254,13 +257,13 @@ add_physics_attribs(struct rigid_body *current_node) { - VSETALL(current_node->linear_velocity, 0.0f); - VSETALL(current_node->angular_velocity, 0.0f); + VSETALL(current_node->linear_velocity, 0.0f); + VSETALL(current_node->angular_velocity, 0.0f); - current_node->num_manifolds = 0; - current_node->first_manifold = NULL; + current_node->num_manifolds = 0; + current_node->first_manifold = NULL; - return GED_OK; + return GED_OK; } @@ -274,82 +277,82 @@ 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; - 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; + 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; + /* Kill the existing sim comb */ + kill(gedp, sim_params->sim_comb_name); + 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; + /* 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_vls_printf(gedp->ged_result_str, "add_regions: Skipping \"%s\" due to \"%s\" in name\n", - dp->d_namep, prefix); - continue; - } + 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; + } - /* 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); + /* 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); - 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); + 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 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 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; + } - /* 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; - current_node->next = NULL; + /* 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; + current_node->next = NULL; - /* Add physics attribs : one shot get from user */ - add_physics_attribs(current_node); + /* Add physics attribs : one shot get from user */ + add_physics_attribs(current_node); - /* 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 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; + } - /* Add the new region to the simulation result */ - add_to_comb(gedp, sim_params->sim_comb_name, prefixed_name); + /* Add the new region to the simulation result */ + add_to_comb(gedp, sim_params->sim_comb_name, prefixed_name); - sim_params->num_bodies++; - } + sim_params->num_bodies++; + } - /* 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); - }*/ + /* 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; + return GED_OK; } @@ -357,48 +360,48 @@ int get_bb(struct ged *gedp, struct simulation_params *sim_params) { - struct rigid_body *current_node; - point_t rpp_min, rpp_max; + struct rigid_body *current_node; + point_t rpp_min, rpp_max; - /* Free memory in rigid_body list */ - for (current_node = sim_params->head_node; current_node != NULL; current_node = current_node->next) { + /* Free memory in rigid_body list */ + for (current_node = sim_params->head_node; current_node != NULL; current_node = current_node->next) { - /* Get its BB */ - if (rt_bound_internal(gedp->ged_wdbp->dbip, current_node->dp, rpp_min, rpp_max) == 0) - bu_log("get_bb: Got the BB for \"%s\" as \ - min {%f %f %f} max {%f %f %f}\n", current_node->dp->d_namep, - rpp_min[0], rpp_min[1], rpp_min[2], - rpp_max[0], rpp_max[1], rpp_max[2]); - else{ - bu_log("get_bb: ERROR Could not get the BB\n"); - return GED_ERROR; - } + /* Get its BB */ + if (rt_bound_internal(gedp->ged_wdbp->dbip, current_node->dp, rpp_min, rpp_max) == 0) + bu_log("get_bb: Got the BB for \"%s\" as \ + min {%f %f %f} max {%f %f %f}\n", current_node->dp->d_namep, + rpp_min[0], rpp_min[1], rpp_min[2], + rpp_max[0], rpp_max[1], rpp_max[2]); + else{ + bu_log("get_bb: ERROR Could not get the BB\n"); + return GED_ERROR; + } - VMOVE(current_node->bb_min, rpp_min); - VMOVE(current_node->bb_max, rpp_max); + VMOVE(current_node->bb_min, rpp_min); + VMOVE(current_node->bb_max, rpp_max); - /* 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]; + /* 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_log("get_bb: Dimensions of this BB : %f %f %f\n", - current_node->bb_dims[0], current_node->bb_dims[1], current_node->bb_dims[2]); + bu_log("get_bb: 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; + /* 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; - 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]; + 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]; - MAT_COPY(current_node->m_prev, current_node->m); - } + MAT_COPY(current_node->m_prev, current_node->m); + } - return GED_OK; + return GED_OK; } @@ -412,139 +415,139 @@ int insert_AABB(struct ged *gedp, struct simulation_params *sim_params, struct rigid_body *current_node) { - char* cmd_args[28]; - char buffer[20]; - int rv; - char *prefix = "bb_"; - char *prefix_reg = "bb_reg_"; - char *prefixed_name, *prefixed_reg_name; - point_t v; + char* cmd_args[28]; + char buffer[20]; + int rv; + char *prefix = "bb_"; + char *prefix_reg = "bb_reg_"; + char *prefixed_name, *prefixed_reg_name; + point_t v; - /* Prepare prefixed bounding box primitive name */ - prefixed_name = prefix_name(prefix, current_node->rb_namep); + /* Prepare prefixed bounding box primitive name */ + prefixed_name = prefix_name(prefix, current_node->rb_namep); - /* Prepare prefixed bounding box region name */ - prefixed_reg_name = prefix_name(prefix_reg, current_node->rb_namep); + /* Prepare prefixed bounding box region name */ + prefixed_reg_name = prefix_name(prefix_reg, current_node->rb_namep); - /* Delete existing bb prim and region */ - rv = kill(gedp, prefixed_name); - if (rv != GED_OK) { - bu_log("insertAABB: ERROR Could not delete existing bounding box arb8 : %s \ - so NOT attempting to add new bounding box\n", prefixed_name); - return GED_ERROR; - } + /* Delete existing bb prim and region */ + rv = kill(gedp, prefixed_name); + if (rv != GED_OK) { + bu_log("insertAABB: ERROR Could not delete existing bounding box arb8 : %s \ + so NOT attempting to add new bounding box\n", prefixed_name); + return GED_ERROR; + } - rv = kill(gedp, prefixed_reg_name); - if (rv != GED_OK) { - bu_log("insertAABB: ERROR Could not delete existing bounding box region : %s \ - so NOT attempting to add new region\n", prefixed_reg_name); - return GED_ERROR; - } + rv = kill(gedp, prefixed_reg_name); + if (rv != GED_OK) { + bu_log("insertAABB: ERROR Could not delete existing bounding box region : %s \ + so NOT attempting to add new region\n", prefixed_reg_name); + return GED_ERROR; + } - /* Setup the simulation result group union-ing the new objects */ - cmd_args[0] = "in"; - cmd_args[1] = bu_strdup(prefixed_name); - cmd_args[2] = "arb8"; + /* Setup the simulation result group union-ing the new objects */ + cmd_args[0] = "in"; + cmd_args[1] = bu_strdup(prefixed_name); + cmd_args[2] = "arb8"; - /* Front face vertices */ - /* v1 */ - v[0] = current_node->btbb_center[0] + current_node->btbb_dims[0]/2; - v[1] = current_node->btbb_center[1] + current_node->btbb_dims[1]/2; - v[2] = current_node->btbb_center[2] - current_node->btbb_dims[2]/2; - sprintf(buffer, "%f", v[0]); cmd_args[3] = bu_strdup(buffer); - sprintf(buffer, "%f", v[1]); cmd_args[4] = bu_strdup(buffer); - sprintf(buffer, "%f", v[2]); cmd_args[5] = bu_strdup(buffer); + /* Front face vertices */ + /* v1 */ + v[0] = current_node->btbb_center[0] + current_node->btbb_dims[0]/2; + v[1] = current_node->btbb_center[1] + current_node->btbb_dims[1]/2; + v[2] = current_node->btbb_center[2] - current_node->btbb_dims[2]/2; + sprintf(buffer, "%f", v[0]); cmd_args[3] = bu_strdup(buffer); + sprintf(buffer, "%f", v[1]); cmd_args[4] = bu_strdup(buffer); + sprintf(buffer, "%f", v[2]); cmd_args[5] = bu_strdup(buffer); - /* v2 */ - v[0] = current_node->btbb_center[0] + current_node->btbb_dims[0]/2; - v[1] = current_node->btbb_center[1] + current_node->btbb_dims[1]/2; - v[2] = current_node->btbb_center[2] + current_node->btbb_dims[2]/2; - sprintf(buffer, "%f", v[0]); cmd_args[6] = bu_strdup(buffer); - sprintf(buffer, "%f", v[1]); cmd_args[7] = bu_strdup(buffer); - sprintf(buffer, "%f", v[2]); cmd_args[8] = bu_strdup(buffer); + /* v2 */ + v[0] = current_node->btbb_center[0] + current_node->btbb_dims[0]/2; + v[1] = current_node->btbb_center[1] + current_node->btbb_dims[1]/2; + v[2] = current_node->btbb_center[2] + current_node->btbb_dims[2]/2; + sprintf(buffer, "%f", v[0]); cmd_args[6] = bu_strdup(buffer); + sprintf(buffer, "%f", v[1]); cmd_args[7] = bu_strdup(buffer); + sprintf(buffer, "%f", v[2]); cmd_args[8] = bu_strdup(buffer); - /* v3 */ - v[0] = current_node->btbb_center[0] + current_node->btbb_dims[0]/2; - v[1] = current_node->btbb_center[1] - current_node->btbb_dims[1]/2; - v[2] = current_node->btbb_center[2] + current_node->btbb_dims[2]/2; - sprintf(buffer, "%f", v[0]); cmd_args[9] = bu_strdup(buffer); - sprintf(buffer, "%f", v[1]); cmd_args[10] = bu_strdup(buffer); - sprintf(buffer, "%f", v[2]); cmd_args[11] = bu_strdup(buffer); + /* v3 */ + v[0] = current_node->btbb_center[0] + current_node->btbb_dims[0]/2; + v[1] = current_node->btbb_center[1] - current_node->btbb_dims[1]/2; + v[2] = current_node->btbb_center[2] + current_node->btbb_dims[2]/2; + sprintf(buffer, "%f", v[0]); cmd_args[9] = bu_strdup(buffer); + sprintf(buffer, "%f", v[1]); cmd_args[10] = bu_strdup(buffer); + sprintf(buffer, "%f", v[2]); cmd_args[11] = bu_strdup(buffer); - /* v4 */ - v[0] = current_node->btbb_center[0] + current_node->btbb_dims[0]/2; - v[1] = current_node->btbb_center[1] - current_node->btbb_dims[1]/2; - v[2] = current_node->btbb_center[2] - current_node->btbb_dims[2]/2; - sprintf(buffer, "%f", v[0]); cmd_args[12] = bu_strdup(buffer); - sprintf(buffer, "%f", v[1]); cmd_args[13] = bu_strdup(buffer); - sprintf(buffer, "%f", v[2]); cmd_args[14] = bu_strdup(buffer); + /* v4 */ + v[0] = current_node->btbb_center[0] + current_node->btbb_dims[0]/2; + v[1] = current_node->btbb_center[1] - current_node->btbb_dims[1]/2; + v[2] = current_node->btbb_center[2] - current_node->btbb_dims[2]/2; + sprintf(buffer, "%f", v[0]); cmd_args[12] = bu_strdup(buffer); + sprintf(buffer, "%f", v[1]); cmd_args[13] = bu_strdup(buffer); + sprintf(buffer, "%f", v[2]); cmd_args[14] = bu_strdup(buffer); - /* Back face vertices */ - /* v5 */ - v[0] = current_node->btbb_center[0] - current_node->btbb_dims[0]/2; - v[1] = current_node->btbb_center[1] + current_node->btbb_dims[1]/2; - v[2] = current_node->btbb_center[2] - current_node->btbb_dims[2]/2; - sprintf(buffer, "%f", v[0]); cmd_args[15] = bu_strdup(buffer); - sprintf(buffer, "%f", v[1]); cmd_args[16] = bu_strdup(buffer); - sprintf(buffer, "%f", v[2]); cmd_args[17] = bu_strdup(buffer); + /* Back face vertices */ + /* v5 */ + v[0] = current_node->btbb_center[0] - current_node->btbb_dims[0]/2; + v[1] = current_node->btbb_center[1] + current_node->btbb_dims[1]/2; + v[2] = current_node->btbb_center[2] - current_node->btbb_dims[2]/2; + sprintf(buffer, "%f", v[0]); cmd_args[15] = bu_strdup(buffer); + sprintf(buffer, "%f", v[1]); cmd_args[16] = bu_strdup(buffer); + sprintf(buffer, "%f", v[2]); cmd_args[17] = bu_strdup(buffer); - /* v6 */ - v[0] = current_node->btbb_center[0] - current_node->btbb_dims[0]/2; - v[1] = current_node->btbb_center[1] + current_node->btbb_dims[1]/2; - v[2] = current_node->btbb_center[2] + current_node->btbb_dims[2]/2; - sprintf(buffer, "%f", v[0]); cmd_args[18] = bu_strdup(buffer); - sprintf(buffer, "%f", v[1]); cmd_args[19] = bu_strdup(buffer); - sprintf(buffer, "%f", v[2]); cmd_args[20] = bu_strdup(buffer); + /* v6 */ + v[0] = current_node->btbb_center[0] - current_node->btbb_dims[0]/2; + v[1] = current_node->btbb_center[1] + current_node->btbb_dims[1]/2; + v[2] = current_node->btbb_center[2] + current_node->btbb_dims[2]/2; + sprintf(buffer, "%f", v[0]); cmd_args[18] = bu_strdup(buffer); + sprintf(buffer, "%f", v[1]); cmd_args[19] = bu_strdup(buffer); + sprintf(buffer, "%f", v[2]); cmd_args[20] = bu_strdup(buffer); - /* v7 */ - v[0] = current_node->btbb_center[0] - current_node->btbb_dims[0]/2; - v[1] = current_node->btbb_center[1] - current_node->btbb_dims[1]/2; - v[2] = current_node->btbb_center[2] + current_node->btbb_dims[2]/2; - sprintf(buffer, "%f", v[0]); cmd_args[21] = bu_strdup(buffer); - sprintf(buffer, "%f", v[1]); cmd_args[22] = bu_strdup(buffer); - sprintf(buffer, "%f", v[2]); cmd_args[23] = bu_strdup(buffer); + /* v7 */ + v[0] = current_node->btbb_center[0] - current_node->btbb_dims[0]/2; + v[1] = current_node->btbb_center[1] - current_node->btbb_dims[1]/2; + v[2] = current_node->btbb_center[2] + current_node->btbb_dims[2]/2; + sprintf(buffer, "%f", v[0]); cmd_args[21] = bu_strdup(buffer); + sprintf(buffer, "%f", v[1]); cmd_args[22] = bu_strdup(buffer); + sprintf(buffer, "%f", v[2]); cmd_args[23] = bu_strdup(buffer); - /* v8 */ - v[0] = current_node->btbb_center[0] - current_node->btbb_dims[0]/2; - v[1] = current_node->btbb_center[1] - current_node->btbb_dims[1]/2; - v[2] = current_node->btbb_center[2] - current_node->btbb_dims[2]/2; - sprintf(buffer, "%f", v[0]); cmd_args[24] = bu_strdup(buffer); - sprintf(buffer, "%f", v[1]); cmd_args[25] = bu_strdup(buffer); - sprintf(buffer, "%f", v[2]); cmd_args[26] = bu_strdup(buffer); + /* v8 */ + v[0] = current_node->btbb_center[0] - current_node->btbb_dims[0]/2; + v[1] = current_node->btbb_center[1] - current_node->btbb_dims[1]/2; + v[2] = current_node->btbb_center[2] - current_node->btbb_dims[2]/2; + sprintf(buffer, "%f", v[0]); cmd_args[24] = bu_strdup(buffer); + sprintf(buffer, "%f", v[1]); cmd_args[25] = bu_strdup(buffer); + sprintf(buffer, "%f", v[2]); cmd_args[26] = bu_strdup(buffer); - /* Finally make the bb primitive, phew ! */ - cmd_args[27] = (char *)0; - rv = ged_in(gedp, 27, (const char **)cmd_args); - if (rv != GED_OK) { - bu_log("insertAABB: WARNING Could not draw bounding box for \"%s\"\n", - current_node->rb_namep); - } + /* Finally make the bb primitive, phew ! */ + cmd_args[27] = (char *)0; + rv = ged_in(gedp, 27, (const char **)cmd_args); + if (rv != GED_OK) { + bu_log("insertAABB: WARNING Could not draw bounding box for \"%s\"\n", + current_node->rb_namep); + } - /* Make the region for the bb primitive */ - add_to_comb(gedp, prefixed_reg_name, prefixed_name); + /* Make the region for the bb primitive */ + add_to_comb(gedp, prefixed_reg_name, prefixed_name); - /* Adjust the material for region to be almost transparent */ - cmd_args[0] = "mater"; - cmd_args[1] = bu_strdup(prefixed_reg_name); - cmd_args[2] = "plastic tr 0.9"; - cmd_args[3] = "210"; - cmd_args[4] = "0"; - cmd_args[5] = "100"; - cmd_args[6] = "0"; - cmd_args[7] = (char *)0; - rv = ged_mater(gedp, 7, (const char **)cmd_args); - if (rv != GED_OK) { - bu_log("insertAABB: WARNING Could not adjust the material for \"%s\"\n", - prefixed_reg_name); - } + /* Adjust the material for region to be almost transparent */ + cmd_args[0] = "mater"; + cmd_args[1] = bu_strdup(prefixed_reg_name); + cmd_args[2] = "plastic tr 0.9"; + cmd_args[3] = "210"; + cmd_args[4] = "0"; + cmd_args[5] = "100"; + cmd_args[6] = "0"; + cmd_args[7] = (char *)0; + rv = ged_mater(gedp, 7, (const char **)cmd_args); + if (rv != GED_OK) { + bu_log("insertAABB: WARNING Could not adjust the material for \"%s\"\n", + prefixed_reg_name); + } - /* Add the region to the result of the sim so it will be drawn too */ - add_to_comb(gedp, sim_params->sim_comb_name, prefixed_reg_name); + /* Add the region to the result of the sim so it will be drawn too */ + add_to_comb(gedp, sim_params->sim_comb_name, prefixed_reg_name); - bu_free(prefixed_name, "simulate : prefixed_name"); - bu_free(prefixed_reg_name, "simulate : prefixed_reg_name"); + bu_free(prefixed_name, "simulate : prefixed_name"); + bu_free(prefixed_reg_name, "simulate : prefixed_reg_name"); - return GED_OK; + return GED_OK; } @@ -558,166 +561,174 @@ int insert_manifolds(struct ged *gedp, struct simulation_params *sim_params, struct rigid_body *rb) { - char* cmd_args[28]; - char buffer[20]; - int rv, num_args; - char *prefixed_name, *prefixed_reg_name; - char *prefix = "mf_"; - char *prefix_reg = "mf_reg_"; + char* cmd_args[28]; + char buffer[20]; + int rv, num_args; + char *prefixed_name, *prefixed_reg_name; + char *prefix = "mf_"; + char *prefix_reg = "mf_reg_"; + struct bu_vls buffer1 = BU_VLS_INIT_ZERO; + char *name; - struct sim_manifold *current_manifold; - int i; + struct sim_manifold *current_manifold; + int i; - for (current_manifold = rb->first_manifold; current_manifold != NULL; - current_manifold = current_manifold->next) { + for (current_manifold = rb->first_manifold; current_manifold != NULL; + current_manifold = current_manifold->next) { - /* Prepare prefixed bounding box primitive name */ - prefixed_name = prefix_name(prefix, rb->rb_namep); - /* Prepare prefixed bounding box region name */ - prefixed_reg_name = prefix_name(prefix_reg, rb->rb_namep); + if(current_manifold->num_contacts > 0){ + /* Prepare prefixed bounding box primitive name */ + bu_vls_sprintf(&buffer1, "%s_%s", current_manifold->rbA->rb_namep, + current_manifold->rbB->rb_namep); + name = bu_vls_addr(&buffer1); + prefixed_name = prefix_name(prefix, name); - /* Delete existing bb prim and region */ - rv = kill(gedp, prefixed_name); - if (rv != GED_OK) { - bu_log("insert_manifolds: ERROR Could not delete existing bounding box arb8 : %s \ - so NOT attempting to add new bounding box\n", prefixed_name); - return GED_ERROR; - } + /* Prepare prefixed bounding box region name */ + prefixed_reg_name = prefix_name(prefix_reg, name); - rv = kill(gedp, prefixed_reg_name); - if (rv != GED_OK) { - bu_log("insert_manifolds: ERROR Could not delete existing bounding box region : %s \ - so NOT attempting to add new region\n", prefixed_reg_name); - return GED_ERROR; - } + /* Delete existing bb prim and region */ + rv = kill(gedp, prefixed_name); + if (rv != GED_OK) { + bu_log("insert_manifolds: ERROR Could not delete existing bounding box arb8 : %s \ + so NOT attempting to add new bounding box\n", prefixed_name); + return GED_ERROR; + } - /* Setup the simulation result group union-ing the new objects */ - cmd_args[0] = "in"; - cmd_args[1] = bu_strdup(prefixed_name); - cmd_args[2] = (char *)0; - num_args = 2; + rv = kill(gedp, prefixed_reg_name); + if (rv != GED_OK) { + bu_log("insert_manifolds: ERROR Could not delete existing bounding box region : %s \ + so NOT attempting to add new region\n", prefixed_reg_name); + return GED_ERROR; + } - switch(current_manifold->num_contacts) { - case 1: - bu_log("1 contact got, no manifold drawn"); - break; + /* Setup the simulation result group union-ing the new objects */ + cmd_args[0] = "in"; + cmd_args[1] = bu_strdup(prefixed_name); + cmd_args[2] = (char *)0; + num_args = 2; - case 2: - cmd_args[2] = "arb4"; - sprintf(buffer, "%f", current_manifold->rb_contacts[0].ptA[0]); - cmd_args[3] = bu_strdup(buffer); - sprintf(buffer, "%f", current_manifold->rb_contacts[0].ptA[1]); - cmd_args[4] = bu_strdup(buffer); - sprintf(buffer, "%f", current_manifold->rb_contacts[0].ptA[2]); - cmd_args[5] = bu_strdup(buffer); + switch(current_manifold->num_contacts) { + case 1: + bu_log("1 contact got, no manifold drawn"); + break; - sprintf(buffer, "%f", current_manifold->rb_contacts[1].ptA[0]); - cmd_args[6] = bu_strdup(buffer); - sprintf(buffer, "%f", current_manifold->rb_contacts[1].ptA[1]); - cmd_args[7] = bu_strdup(buffer); - sprintf(buffer, "%f", current_manifold->rb_contacts[1].ptA[2]); - cmd_args[8] = bu_strdup(buffer); + case 2: + cmd_args[2] = "arb4"; + sprintf(buffer, "%f", current_manifold->rb_contacts[0].ptA[0]); + cmd_args[3] = bu_strdup(buffer); + sprintf(buffer, "%f", current_manifold->rb_contacts[0].ptA[1]); + cmd_args[4] = bu_strdup(buffer); + sprintf(buffer, "%f", current_manifold->rb_contacts[0].ptA[2]); + cmd_args[5] = bu_strdup(buffer); - sprintf(buffer, "%f", current_manifold->rb_contacts[1].ptB[0]); - cmd_args[9] = bu_strdup(buffer); - sprintf(buffer, "%f", current_manifold->rb_contacts[1].ptB[1]); - cmd_args[10] = bu_strdup(buffer); - sprintf(buffer, "%f", current_manifold->rb_contacts[1].ptB[2]); - cmd_args[11] = bu_strdup(buffer); + sprintf(buffer, "%f", current_manifold->rb_contacts[1].ptA[0]); + cmd_args[6] = bu_strdup(buffer); + sprintf(buffer, "%f", current_manifold->rb_contacts[1].ptA[1]); + cmd_args[7] = bu_strdup(buffer); + sprintf(buffer, "%f", current_manifold->rb_contacts[1].ptA[2]); + cmd_args[8] = bu_strdup(buffer); - sprintf(buffer, "%f", current_manifold->rb_contacts[0].ptB[0]); - cmd_args[12] = bu_strdup(buffer); - sprintf(buffer, "%f", current_manifold->rb_contacts[0].ptB[1]); - cmd_args[13] = bu_strdup(buffer); - sprintf(buffer, "%f", current_manifold->rb_contacts[0].ptB[2]); - cmd_args[14] = bu_strdup(buffer); + sprintf(buffer, "%f", current_manifold->rb_contacts[1].ptB[0]); + cmd_args[9] = bu_strdup(buffer); + sprintf(buffer, "%f", current_manifold->rb_contacts[1].ptB[1]); + cmd_args[10] = bu_strdup(buffer); + sprintf(buffer, "%f", current_manifold->rb_contacts[1].ptB[2]); + cmd_args[11] = bu_strdup(buffer); - cmd_args[15] = (char *)0; - num_args = 15; - break; + sprintf(buffer, "%f", current_manifold->rb_contacts[0].ptB[0]); + cmd_args[12] = bu_strdup(buffer); + sprintf(buffer, "%f", current_manifold->rb_contacts[0].ptB[1]); + cmd_args[13] = bu_strdup(buffer); + sprintf(buffer, "%f", current_manifold->rb_contacts[0].ptB[2]); + cmd_args[14] = bu_strdup(buffer); - case 3: - bu_log("3 contacts got, no manifold drawn"); - break; + cmd_args[15] = (char *)0; + num_args = 15; + break; - case 4: - cmd_args[2] = "arb8"; - for (i=0; i<4; i++) { - sprintf(buffer, "%f", current_manifold->rb_contacts[i].ptA[0]); - cmd_args[3+i*3] = bu_strdup(buffer); - sprintf(buffer, "%f", current_manifold->rb_contacts[i].ptA[1]); - cmd_args[4+i*3] = bu_strdup(buffer); - sprintf(buffer, "%f", current_manifold->rb_contacts[i].ptA[2]); - cmd_args[5+i*3] = bu_strdup(buffer); + case 3: + bu_log("3 contacts got, no manifold drawn"); + break; - sprintf(buffer, "%f", current_manifold->rb_contacts[i].ptB[0]); - cmd_args[15+i*3] = bu_strdup(buffer); - sprintf(buffer, "%f", current_manifold->rb_contacts[i].ptB[1]); - cmd_args[16+i*3] = bu_strdup(buffer); - sprintf(buffer, "%f", current_manifold->rb_contacts[i].ptB[2]); - cmd_args[17+i*3] = bu_strdup(buffer); + case 4: + cmd_args[2] = "arb8"; + for (i=0; i<4; i++) { + sprintf(buffer, "%f", current_manifold->rb_contacts[i].ptA[0]); + cmd_args[3+i*3] = bu_strdup(buffer); + sprintf(buffer, "%f", current_manifold->rb_contacts[i].ptA[1]); + cmd_args[4+i*3] = bu_strdup(buffer); + sprintf(buffer, "%f", current_manifold->rb_contacts[i].ptA[2]); + cmd_args[5+i*3] = bu_strdup(buffer); - /* current_manifold->rb_contacts[i].ptA[0], - current_manifold->rb_contacts[i].ptA[1], - current_manifold->rb_contacts[i].ptA[2], - current_manifold->rb_contacts[i].ptB[0], - current_manifold->rb_contacts[i].ptB[1], - current_manifold->rb_contacts[i].ptB[2], - current_manifold->rb_contacts[i].normalWorldOnB[0], - current_manifold->rb_contacts[i].normalWorldOnB[1], - current_manifold->rb_contacts[i].normalWorldOnB[2]);*/ - } - cmd_args[27] = (char *)0; - num_args = 27; - break; + sprintf(buffer, "%f", current_manifold->rb_contacts[i].ptB[0]); + cmd_args[15+i*3] = bu_strdup(buffer); + sprintf(buffer, "%f", current_manifold->rb_contacts[i].ptB[1]); + cmd_args[16+i*3] = bu_strdup(buffer); + sprintf(buffer, "%f", current_manifold->rb_contacts[i].ptB[2]); + cmd_args[17+i*3] = bu_strdup(buffer); - default: - bu_log("%d contacts got, no manifold drawn", current_manifold->num_contacts); - cmd_args[2] = (char *)0; - num_args = 2; - } + /* current_manifold->rb_contacts[i].ptA[0], + current_manifold->rb_contacts[i].ptA[1], + current_manifold->rb_contacts[i].ptA[2], + current_manifold->rb_contacts[i].ptB[0], + current_manifold->rb_contacts[i].ptB[1], + current_manifold->rb_contacts[i].ptB[2], + current_manifold->rb_contacts[i].normalWorldOnB[0], + current_manifold->rb_contacts[i].normalWorldOnB[1], + current cmd_argsrent_manifold->rb_contacts[i].normalWorldOnB[2]);*/ + } + cmd_args[27] = (char *)0; + num_args = 27; + break; - print_command(cmd_args, num_args); + default: + bu_log("%d contacts got, no manifold drawn", current_manifold->num_contacts); + cmd_args[2] = (char *)0; + num_args = 2; + } - /* Finally make the manifold primitive, if proper command generated */ - if (num_args > 2) { - rv = ged_in(gedp, num_args, (const char **)cmd_args); - if (rv != GED_OK) { - bu_log("insert_manifolds: WARNING Could not draw manifold for \"%s\"\n", - rb->rb_namep); - } + print_command(cmd_args, num_args); - /* Make the region for the bb primitive */ - add_to_comb(gedp, prefixed_reg_name, prefixed_name); + /* Finally mak... [truncated message content] |