[Opal-commits] opal/src/ODE ODEJoint.cpp,1.27,1.28 ODESimulator.cpp,1.84,1.85

[tylerstreeter <tyl...@us...>]

Update of /cvsroot/opal/opal/src/ODE
In directory sc8-pr-cvs1.sourceforge.net:/tmp/cvs-serv11795/src/ODE

Modified Files:
	ODEJoint.cpp ODESimulator.cpp 
Log Message:
added contact groups; added separate event handlers for collisions and joint break events; commented out mass ratio stuff

Index: ODESimulator.cpp
===================================================================
RCS file: /cvsroot/opal/opal/src/ODE/ODESimulator.cpp,v
retrieving revision 1.84
retrieving revision 1.85
diff -C2 -d -r1.84 -r1.85
*** ODESimulator.cpp	5 Mar 2005 21:26:24 -0000	1.84
--- ODESimulator.cpp	8 Mar 2005 16:10:24 -0000	1.85
***************
*** 76,80 ****
  
  		setSolverAccuracy(defaults::solverAccuracy);
- 		//setDefaultSleepiness(defaults::sleepiness);
  		mCollisionCount = 0;
  	}
--- 76,79 ----
***************
*** 258,264 ****
  				dSpaceCollide2(o0, o1, data, &internal_collisionCallback);
  				
- 				// Colliding all geoms internal to the space.
  				if (dGeomIsSpace(o0))
  				{
  					dSpaceCollide((dSpaceID)o0, data, 
  						&internal_collisionCallback);
--- 257,263 ----
  				dSpaceCollide2(o0, o1, data, &internal_collisionCallback);
  				
  				if (dGeomIsSpace(o0))
  				{
+ 					// Colliding all geoms internal to the space.
  					dSpaceCollide((dSpaceID)o0, data, 
  						&internal_collisionCallback);
***************
*** 267,270 ****
--- 266,270 ----
  				if (dGeomIsSpace(o1))
  				{
+ 					// Colliding all geoms internal to the space.
  					dSpaceCollide((dSpaceID)o1, data, 
  						&internal_collisionCallback);
***************
*** 275,279 ****
  				// Colliding two geoms.
  
! 				// Use category/collide bitfields here?
  
  				dBodyID o0BodyID = dGeomGetBody(o0);
--- 275,308 ----
  				// Colliding two geoms.
  
! 				// The following is a set of special cases where we might 
! 				// not want to do collision detection (but not all are 
! 				// enforced here for various reasons): 
! 				// 1. Two static Solids (neither geom has a body): this is 
! 				//    not enforced because sometimes you might want two 
! 				//    static Solids to generate collision events.
! 				// 2. Two Shapes that are part of the same Solid (they 
! 				//    share a body): this is not enforced because ODE 
! 				//    already takes care of it.
! 				// 3. Two sleeping Solids (note: both must have bodies to 
! 				//	  check this): this is enforced.  ODE should handle 
! 				//	  this, but it doesn't.
! 				// 4. Two Solids connected by a fixed Joint: this is 
! 				//    enforced.
! 				// 5. Two Solids connected by a Joint (besides ODE 
! 				//    contact joints, which never generate 
! 				//    contacts) (note: both must have 
! 				//    bodies to check this): this is enforced for now; it 
! 				//    might be nice to have a Joint option that says whether 
! 				//    the Joint's two 
! 				//    Solids generate contacts with each other... I'll 
! 				//    mark that as a TODO; if this is implemented, be sure 
! 				//    to update the comments for 
! 				//    Simulator::setupContactGroups.
! 				// 6. Solid0 is static and Solid1 is sleeping: not enforced 
! 				//    because sometimes you might want to wake up a sleeping 
! 				//    Solid with a static Solid.
! 				// 7. Solid1 is static and Solid0 is sleeping: not enforced 
! 				//    because sometimes you might want to wake up a sleeping 
! 				//    Solid with a static Solid.
  
  				dBodyID o0BodyID = dGeomGetBody(o0);
***************
*** 285,309 ****
  				}
  
! 				// Don't do collision detection for the following cases:
! 				// 1. two static solids (neither geom has a body)
! 				// 2. two shapes that are part of the same solid (they share a 
! 				//	body)
! 				// 3. two sleeping solids - note: both must have bodies to 
! 				//	check this
! 				// 4. two joined solids (but not contact joints); note: both 
! 				//	must have bodies to check this
! 				// 5. solid0 is static and solid1 is sleeping
! 				// 6. solid1 is static and solid0 is sleeping
! 				// Note: cases 5 and 6 will keep static solids from waking up 
! 				// sleeping solids
  
! 				if ((0 == o0BodyID && 0 == o1BodyID) //case 1
! 					//|| (o0BodyID == o1BodyID) //case 2 -> handled by ODE
! 					|| (bothHaveBodies && !dBodyIsEnabled(o0BodyID) && 
  						!dBodyIsEnabled(o1BodyID)) //case 3
! 					|| (bothHaveBodies && dAreConnectedExcluding(o0BodyID, 
! 						o1BodyID, dJointTypeContact)) //case 4
! 					|| (0 == o0BodyID && !dBodyIsEnabled(o1BodyID)) //case 5
! 					|| (0 == o1BodyID && !dBodyIsEnabled(o0BodyID)) //case 6
  					)
  				{
--- 314,332 ----
  				}
  
! 				// TODO: find a way to get access to the Joint* for #5...
! 				// check if ODE has user data* for joints.  If this 
! 				// doesn't work, just have a boolean in the Simulator that 
! 				// affects all Joints.
  
! 				if (//(0 == o0BodyID && 0 == o1BodyID) //case 1
! 					//|| (o0BodyID == o1BodyID) //case 2
! 					(bothHaveBodies && !dBodyIsEnabled(o0BodyID) && 
  						!dBodyIsEnabled(o1BodyID)) //case 3
! 					//|| (connected by a fixed joint) // case 4 TODO
! 					|| (bothHaveBodies && dAreConnectedExcluding(
! 						o0BodyID, o1BodyID, dJointTypeContact)) //&& TODO
! 						//jointContactsEnabled) //case 5
! 					//|| (0 == o0BodyID && !dBodyIsEnabled(o1BodyID)) //case 6
! 					//|| (0 == o1BodyID && !dBodyIsEnabled(o0BodyID)) //case 7
  					)
  				{
***************
*** 321,324 ****
--- 344,349 ----
  					sizeof(dContactGeom));
  
+ 				// If the two objects didn't make any contacts, they weren't 
+ 				// touching, so just return.
  				if (0 == numContacts)
  				{
***************
*** 327,342 ****
  
  				dContact tempContact;
! 				bool generateContacts0 = true; // Default to true.
! 				bool generateContacts1 = true; // Default to true.
! 				GeomData* geomData0 = ((GeomData*)dGeomGetData(o0));
  				GeomData* geomData1 = ((GeomData*)dGeomGetData(o1));
  				Solid* solid0 = geomData0->solid;
  				Solid* solid1 = geomData1->solid;
  
! 				// Early check to save some time.
! 				if (solid0->getEventHandler() || solid1->getEventHandler())
  				{
! 					// Call the event handlers.  Note: we only use one 
! 					// contact point per collision; just use the first one 
  					// in the contact array.  The order of the Solids 
  					// passed to the event handlers is important: the first 
--- 352,385 ----
  
  				dContact tempContact;
! 				//bool generateContacts0 = true; // Default to true.
! 				//bool generateContacts1 = true; // Default to true.
! 				GeomData* geomData0 = (GeomData*)dGeomGetData(o0);
  				GeomData* geomData1 = ((GeomData*)dGeomGetData(o1));
+ 				const ShapeData* shape0 = geomData0->shape;
+ 				const ShapeData* shape1 = geomData1->shape;
  				Solid* solid0 = geomData0->solid;
  				Solid* solid1 = geomData1->solid;
  
! 				// We only need to check for "one side" of the contact group 
! 				// here because the groups are always setup both ways (i.e. 
! 				// the interaction between object 0's contact group and 
! 				// object 1's contact group is always symmetric).
! 				bool makeContacts = false;
! 				unsigned long int group1Bit = 1 << shape1->contactGroup;
! 				if (sim->internal_getContactGroupFlags(shape0->contactGroup)
! 					& group1Bit)
  				{
! 					makeContacts = true;
! 				}
! 
! 				// If at least one of the Solids has a CollisionEventHandler, 
! 				// send it a CollisionEvent.
! 				CollisionEventHandler* handler0 = solid0->getEventHandler();
! 				CollisionEventHandler* handler1 = solid1->getEventHandler();
! 
! 				if (handler0 || handler1)
! 				{
! 					// Call the CollisionEventHandlers.  Note that we only 
! 					// use one contact point per collision: just the first one 
  					// in the contact array.  The order of the Solids 
  					// passed to the event handlers is important: the first 
***************
*** 345,350 ****
  
  					CollisionEvent e;
! 					e.solid0 = solid0;
! 					e.solid1 = solid1;
  					e.pos[0] = contactArray[0].pos[0];
  					e.pos[1] = contactArray[0].pos[1];
--- 388,393 ----
  
  					CollisionEvent e;
! 					e.thisSolid = solid0;
! 					e.otherSolid = solid1;
  					e.pos[0] = contactArray[0].pos[0];
  					e.pos[1] = contactArray[0].pos[1];
***************
*** 355,392 ****
  					e.depth = contactArray[0].depth;
  
! 					EventHandler* eventHandler = solid0->getEventHandler();
! 					if (eventHandler)
  					{
! 						generateContacts0 = 
! 							eventHandler->handleCollisionEvent(e);
  					}
  
! 					e.normal *= -1; // Invert normal.
! 					e.solid0 = solid1; // Swap solid pointers.
! 					e.solid1 = solid0;
! 
! 					eventHandler = solid1->getEventHandler();
! 					if (eventHandler)
  					{
! 						generateContacts1 = 
! 							eventHandler->handleCollisionEvent(e);
  					}
  				}
  
! 				// Handle one-sided contacts for two cases: 1) only one of 
! 				// the above event handlers actually wants contacts generated, 
! 				// 2) if the mass ratio is above some threshold, treat it as 
! 				// a one-sided collision solution: treat the more massive 
! 				// object as static, calculate the collision like normal 
! 				// (with the massive one being static), then also add the 
! 				// massive object's velocity to the smaller one (velocity 
! 				// calculated at the point of collision).
  
! 				if (true == generateContacts0 || true == generateContacts1)
  				{
  					for(int i=0; i<numContacts; ++i)
  					{
! 						const Material* m0 = &(geomData0->shape->material);
! 						const Material* m1 = &(geomData1->shape->material);
  
  						tempContact.surface.mode = dContactBounce | 
--- 398,464 ----
  					e.depth = contactArray[0].depth;
  
! 					if (handler0)
  					{
! 						handler0->handleCollisionEvent(e);
  					}
  
! 					if (handler1)
  					{
! 						// For the other Solid's CollisionEventHandler, we need 
! 						// to invert the normal and swap the Solid pointers.
! 						e.normal *= -1;
! 						e.thisSolid = solid1;
! 						e.otherSolid = solid0;
! 						handler1->handleCollisionEvent(e);
  					}
  				}
  
! 				// Old version...
! 				//// Early check to save some time.
! 				//if (solid0->getEventHandler() || solid1->getEventHandler())
! 				//{
! 				//	// Call the event handlers.  Note: we only use one 
! 				//	// contact point per collision; just use the first one 
! 				//	// in the contact array.  The order of the Solids 
! 				//	// passed to the event handlers is important: the first 
! 				//	// one should be the one whose event handler is 
! 				//	// getting called.
  
! 				//	CollisionEvent e;
! 				//	e.solid0 = solid0;
! 				//	e.solid1 = solid1;
! 				//	e.pos[0] = contactArray[0].pos[0];
! 				//	e.pos[1] = contactArray[0].pos[1];
! 				//	e.pos[2] = contactArray[0].pos[2];
! 				//	e.normal[0] = contactArray[0].normal[0];
! 				//	e.normal[1] = contactArray[0].normal[1];
! 				//	e.normal[2] = contactArray[0].normal[2];
! 				//	e.depth = contactArray[0].depth;
! 
! 				//	EventHandler* eventHandler = solid0->getEventHandler();
! 				//	if (eventHandler)
! 				//	{
! 				//		generateContacts0 = 
! 				//			eventHandler->handleCollisionEvent(e);
! 				//	}
! 
! 				//	e.normal *= -1; // Invert normal.
! 				//	e.solid0 = solid1; // Swap solid pointers.
! 				//	e.solid1 = solid0;
! 
! 				//	eventHandler = solid1->getEventHandler();
! 				//	if (eventHandler)
! 				//	{
! 				//		generateContacts1 = 
! 				//			eventHandler->handleCollisionEvent(e);
! 				//	}
! 				//}
! 
! 				if (makeContacts)
  				{
  					for(int i=0; i<numContacts; ++i)
  					{
! 						const Material* m0 = &(shape0->material);
! 						const Material* m1 = &(shape1->material);
  
  						tempContact.surface.mode = dContactBounce | 
***************
*** 448,545 ****
  							theWorldID, theJointGroupID, &tempContact);
  
! 						if (!bothHaveBodies)
! 						{
! 							// at least one object is static, so just handle 
! 							// things like normal
! 							dJointAttach(contactJoint, o0BodyID, o1BodyID);
! 						}
! 						else
! 						{
! 							// calculate mass ratio (use mass1 / mass2); if 
! 							// the ratio is too high, mass1 is too large; if 
! 							// the ratio is too low, mass2 is too large
! 							real massRatio = 0;
! 							dMass mass0, mass1;
! 							dBodyGetMass(o0BodyID, &mass0);
! 							dBodyGetMass(o1BodyID, &mass1);
! 							massRatio = mass0.mass / mass1.mass;
  
! 							// here we handle all the different collision 
! 							// cases: one solid or the other or both may want 
! 							// contacts generated; also, the mass ratio may 
! 							// be outside the acceptable range
  
! 							if (true == generateContacts0 && true == 
! 								generateContacts1)
! 							{
! 								// both want contacts, neither wants to be 
! 								// static
! 								if (massRatio > defaults::ode::maxMassRatio)
! 								{
! 									// ratio is too high - mass0 is too large, 
! 									// treat solid0 as static
! 									dBodyEnable(o1BodyID);
! 									dJointAttach(contactJoint, 0, o1BodyID);
! 								}
! 								else if (massRatio < 
! 									defaults::ode::minMassRatio)
! 								{
! 									// ratio is too low - mass1 is too large, 
! 									// treat solid1 as static
! 									dBodyEnable(o0BodyID);
! 									dJointAttach(contactJoint, o0BodyID, 0);
! 								}
! 								else
! 								{
! 									//ratio is good - no static objects
! 									dJointAttach(contactJoint, o0BodyID, 
! 										o1BodyID);
! 								}
! 							}
! 							else if (true == generateContacts0)
! 							{
! 								// solid0 wants contacts, solid1 wants to be 
! 								// static
! 								if (massRatio > defaults::ode::maxMassRatio)
! 								{
! 									// ratio is too high - mass0 is too large, 
! 									// treat solid0 and solid1 as static
! 									// i.e. don't generate a contact joint
! 								}
! 								else
! 								{
! 									// this block handles two cases which have 
! 									// the same result:
! 									// 1. ratio is too low - mass1 is too 
! 									//	large, treat solid1 as static
! 									// 2. ratio is good - treat solid1 as 
! 									//	static
! 									dBodyEnable(o0BodyID);
! 									dJointAttach(contactJoint, o0BodyID, 0);
! 								}
! 							}
! 							else //generateContacts1 must be true
! 							{
! 								// solid1 wants contacts, solid0 wants to be 
! 								// static
! 								if (massRatio < defaults::ode::minMassRatio)
! 								{
! 									// ratio is too low - mass1 is too large, 
! 									// treat solid0 and solid1 as static
! 									// i.e. don't generate a contact joint
! 								}						
! 								else
! 								{
! 									// this block handles two cases which have 
! 									// the same result:
! 									// 1. ratio is too high - mass0 is too 
! 									//	large, treat solid0 as static
! 									// 2. ratio is good - treat solid0 as 
! 									//	static
! 									dBodyEnable(o1BodyID);
! 									dJointAttach(contactJoint, 0, o1BodyID);
! 								}
! 							}
! 						}
  					}
  				}
--- 520,648 ----
  							theWorldID, theJointGroupID, &tempContact);
  
! 						// Note: the following line of code replaces the 
! 						// rest of this function which is commented out.  
! 						// TODO: test this and make sure the mass ratio 
! 						// issues are unimportant and that we never need 
! 						// "one-sided" contacts between two Solids.
! 						dJointAttach(contactJoint, o0BodyID, o1BodyID);
  
! 						//if (!bothHaveBodies)
! 						//{
! 						//	// at least one object is static, so just handle 
! 						//	// things like normal
! 						//	dJointAttach(contactJoint, o0BodyID, o1BodyID);
! 						//}
! 						//else
! 						//{
! 						//	// TODO: We probably need to remove the following chunk of 
! 						//	// code.  The first case is obsolete since now both sides 
! 						//	// always get contacts, if at all.  (There isn't really a 
! 						//	// good reason to have one side use contacts but not the 
! 						//	// other; the side not wanting contacts would appear to be 
! 						//	// static, so just make it static in the first place.  On 
! 						//	// the other hand, this may end up being desirable if 
! 						//	// an object should be moved by some objects but not 
! 						//	// others, and the "others" *do* collid with the first 
! 						//	// object... but this situation may never come up.  The 
! 						//	// second case, using mass ratios to determine whether two 
! 						//	// objects should collide, might not be an issue.  Mass 
! 						//	// ratios might only be a problem when the two objects are 
! 						//	// constantly connected with a Joint.
  
! 						//	// Handle one-sided contacts for two cases: 1) only one of 
! 						//	// the above event handlers actually wants contacts generated, 
! 						//	// 2) if the mass ratio is above some threshold, treat it as 
! 						//	// a one-sided collision solution: treat the more massive 
! 						//	// object as static, calculate the collision like normal 
! 						//	// (with the massive one being static), then also add the 
! 						//	// massive object's velocity to the smaller one (velocity 
! 						//	// calculated at the point of collision).
! 
! 						//	// calculate mass ratio (use mass1 / mass2); if 
! 						//	// the ratio is too high, mass1 is too large; if 
! 						//	// the ratio is too low, mass2 is too large
! 						//	real massRatio = 0;
! 						//	dMass mass0, mass1;
! 						//	dBodyGetMass(o0BodyID, &mass0);
! 						//	dBodyGetMass(o1BodyID, &mass1);
! 						//	massRatio = mass0.mass / mass1.mass;
! 
! 						//	// here we handle all the different collision 
! 						//	// cases: one solid or the other or both may want 
! 						//	// contacts generated; also, the mass ratio may 
! 						//	// be outside the acceptable range
! 
! 						//	if (true == generateContacts0 && true == 
! 						//		generateContacts1)
! 						//	{
! 						//		// both want contacts, neither wants to be 
! 						//		// static
! 						//		if (massRatio > defaults::ode::maxMassRatio)
! 						//		{
! 						//			// ratio is too high - mass0 is too large, 
! 						//			// treat solid0 as static
! 						//			dBodyEnable(o1BodyID);
! 						//			dJointAttach(contactJoint, 0, o1BodyID);
! 						//		}
! 						//		else if (massRatio < 
! 						//			defaults::ode::minMassRatio)
! 						//		{
! 						//			// ratio is too low - mass1 is too large, 
! 						//			// treat solid1 as static
! 						//			dBodyEnable(o0BodyID);
! 						//			dJointAttach(contactJoint, o0BodyID, 0);
! 						//		}
! 						//		else
! 						//		{
! 						//			//ratio is good - no static objects
! 						//			dJointAttach(contactJoint, o0BodyID, 
! 						//				o1BodyID);
! 						//		}
! 						//	}
! 						//	else if (true == generateContacts0)
! 						//	{
! 						//		// solid0 wants contacts, solid1 wants to be 
! 						//		// static
! 						//		if (massRatio > defaults::ode::maxMassRatio)
! 						//		{
! 						//			// ratio is too high - mass0 is too large, 
! 						//			// treat solid0 and solid1 as static
! 						//			// i.e. don't generate a contact joint
! 						//		}
! 						//		else
! 						//		{
! 						//			// this block handles two cases which have 
! 						//			// the same result:
! 						//			// 1. ratio is too low - mass1 is too 
! 						//			//	large, treat solid1 as static
! 						//			// 2. ratio is good - treat solid1 as 
! 						//			//	static
! 						//			dBodyEnable(o0BodyID);
! 						//			dJointAttach(contactJoint, o0BodyID, 0);
! 						//		}
! 						//	}
! 						//	else //generateContacts1 must be true
! 						//	{
! 						//		// solid1 wants contacts, solid0 wants to be 
! 						//		// static
! 						//		if (massRatio < defaults::ode::minMassRatio)
! 						//		{
! 						//			// ratio is too low - mass1 is too large, 
! 						//			// treat solid0 and solid1 as static
! 						//			// i.e. don't generate a contact joint
! 						//		}						
! 						//		else
! 						//		{
! 						//			// this block handles two cases which have 
! 						//			// the same result:
! 						//			// 1. ratio is too high - mass0 is too 
! 						//			//	large, treat solid0 as static
! 						//			// 2. ratio is good - treat solid0 as 
! 						//			//	static
! 						//			dBodyEnable(o1BodyID);
! 						//			dJointAttach(contactJoint, 0, o1BodyID);
! 						//		}
! 						//	}
! 						//}
  					}
  				}

Index: ODEJoint.cpp
===================================================================
RCS file: /cvsroot/opal/opal/src/ODE/ODEJoint.cpp,v
retrieving revision 1.27
retrieving revision 1.28
diff -C2 -d -r1.27 -r1.28
*** ODEJoint.cpp	1 Mar 2005 05:04:23 -0000	1.27
--- ODEJoint.cpp	8 Mar 2005 16:10:24 -0000	1.28
***************
*** 59,63 ****
  
  			// Reset event handler.
! 			mEventHandler = NULL;
  		}
  
--- 59,63 ----
  
  			// Reset event handler.
! 			mJointBreakEventHandler = NULL;
  		}