[Opal-commits] opal/src/ODE ODESimulator.cpp,1.93,1.94 ODESimulator.h,1.67,1.68

SourceForge Headquarters 1320 Columbia Street Suite 310 San Diego, CA 92101 +1 (858) 422-6466

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

Modified Files:
	ODESimulator.cpp ODESimulator.h 
Log Message:
made the RaycastSensor and VolumeSensor use contact groups

Index: ODESimulator.h
===================================================================
RCS file: /cvsroot/opal/opal/src/ODE/ODESimulator.h,v
retrieving revision 1.67
retrieving revision 1.68
diff -C2 -d -r1.67 -r1.68
*** ODESimulator.h	23 Mar 2005 20:24:54 -0000	1.67
--- ODESimulator.h	25 Mar 2005 02:44:21 -0000	1.68
***************
*** 99,103 ****
  		virtual const RaycastResult& OPAL_CALL internal_fireRay(
  			const Rayr& r, real length, const Solid* attachedSolid, 
! 			unsigned long int rayContactGroup);

  		/// Helper function used for ray casting.  
--- 99,106 ----
  		virtual const RaycastResult& OPAL_CALL internal_fireRay(
  			const Rayr& r, real length, const Solid* attachedSolid, 
! 			unsigned int rayContactGroup);
! 
! 		virtual const VolumeQueryResult& OPAL_CALL internal_queryVolume(
! 			const Solid* volume, const Solid* attachedSolid);

  		/// Helper function used for ray casting.  
***************
*** 108,116 ****
  		virtual void OPAL_CALL internal_addCollidedSolid(Solid* solid);

! 		virtual const VolumeQueryResult& OPAL_CALL internal_queryVolume(
! 			const Solid* volume, const Solid* attachedSolid);

  	protected:
- 
  		/// The ODE world ID used by this Simulator.  
  		dWorldID mWorldID;
--- 111,118 ----
  		virtual void OPAL_CALL internal_addCollidedSolid(Solid* solid);

! 		/// Helper function used for ray casting.  
! 		virtual unsigned int OPAL_CALL internal_getRayContactGroup();

  	protected:
  		/// The ODE world ID used by this Simulator.  
  		dWorldID mWorldID;
***************
*** 120,124 ****

  		/// The ODE joint constraint group.  
! 		dJointGroupID mContactGroupID;

  		/// The type of constraint solver to use.  
--- 122,126 ----

  		/// The ODE joint constraint group.  
! 		dJointGroupID mContactJointGroupID;

  		/// The type of constraint solver to use.  
***************
*** 140,143 ****
--- 142,149 ----
  		/// query doesn't collide with the attached Solid.
  		const Solid* mSensorSolid;
+ 
+ 		/// A temporary variable that lets rays to use contact groups.  This 
+ 		/// allows them to limit which Shapes they collide with.
+ 		unsigned int mRayContactGroup;
  	private:
  	};

Index: ODESimulator.cpp
===================================================================
RCS file: /cvsroot/opal/opal/src/ODE/ODESimulator.cpp,v
retrieving revision 1.93
retrieving revision 1.94
diff -C2 -d -r1.93 -r1.94
*** ODESimulator.cpp	23 Mar 2005 20:24:48 -0000	1.93
--- ODESimulator.cpp	25 Mar 2005 02:44:20 -0000	1.94
***************
*** 54,58 ****

  		// Create the ODE contact joint group.
! 		mContactGroupID = dJointGroupCreate(0);

  		// Set the ODE global CFM value that will be used by all Joints 
--- 54,58 ----

  		// Create the ODE contact joint group.
! 		mContactJointGroupID = dJointGroupCreate(0);

  		// Set the ODE global CFM value that will be used by all Joints 
***************
*** 78,81 ****
--- 78,82 ----
  		// "mRaycastResult" is initialized in its own constructor.
  		mSensorSolid = NULL;
+ 		mRayContactGroup = defaults::shape::contactGroup;
  	}

***************
*** 112,116 ****
  		dSpaceID rootSpaceID = mRootSpaceID;
  		dWorldID worldID = mWorldID;
! 		dJointGroupID contactGroupID = mContactGroupID;

  		delete this;
--- 113,117 ----
  		dSpaceID rootSpaceID = mRootSpaceID;
  		dWorldID worldID = mWorldID;
! 		dJointGroupID contactJointGroupID = mContactJointGroupID;

  		delete this;
***************
*** 121,125 ****
  		dSpaceDestroy(rootSpaceID);
  		dWorldDestroy(worldID);
! 		dJointGroupDestroy(contactGroupID);
  		dCloseODE();
  	}
--- 122,126 ----
  		dSpaceDestroy(rootSpaceID);
  		dWorldDestroy(worldID);
! 		dJointGroupDestroy(contactJointGroupID);
  		dCloseODE();
  	}
***************
*** 267,271 ****

  		//Remove all joints from the contact group.
! 		dJointGroupEmpty(mContactGroupID);
  	}

--- 268,272 ----

  		//Remove all joints from the contact group.
! 		dJointGroupEmpty(mContactJointGroupID);
  	}

***************
*** 321,327 ****
--- 322,334 ----
  				//    because sometimes you might want to wake up a sleeping 
  				//    Solid with a static Solid.
+ 				// 8. The two Solids' contact groups do not generate 
+ 				//    contacts when they collide AND neither Solid has a 
+ 				//    CollisionEventHandler.

+ 				// Get the geoms' ODE body IDs.
  				dBodyID o0BodyID = dGeomGetBody(o0);
  				dBodyID o1BodyID = dGeomGetBody(o1);
+ 
+ 				// Check if both Solids are dynamic (i.e. have ODE bodies).
  				bool bothHaveBodies = true;
  				if (0 == o0BodyID || 0 == o1BodyID)
***************
*** 330,333 ****
--- 337,342 ----
  				}

+ 				// If the two Solids are connected by a common Joint, get 
+ 				// a pointer to that Joint.
  				Joint* commonJoint = NULL;
  				if (bothHaveBodies && dAreConnectedExcluding(o0BodyID, 
***************
*** 339,342 ****
--- 348,379 ----
  				}

+ 				// Get pointers to the geoms' GeomData structures.
+ 				GeomData* geomData0 = (GeomData*)dGeomGetData(o0);
+ 				GeomData* geomData1 = ((GeomData*)dGeomGetData(o1));
+ 
+ 				// Get pointers to the geoms' ShapeData structures.
+ 				const ShapeData* shape0 = geomData0->shape;
+ 				const ShapeData* shape1 = geomData1->shape;
+ 
+ 				// Get a pointer to the ODESimulator.
+ 				ODESimulator* sim = (ODESimulator*)data;
+ 
+ 				// Check if the two Solids' contact groups generate contacts 
+ 				// when they collide.
+ 				bool makeContacts = sim->groupsMakeContacts(
+ 					shape0->contactGroup, shape1->contactGroup);
+ 
+ 				// Get pointers to the geoms' Solids.
+ 				Solid* solid0 = geomData0->solid;
+ 				Solid* solid1 = geomData1->solid;
+ 
+ 				// Get pointers to the two Solids' CollisionEventHandlers.  
+ 				// These will be NULL if the Solids don't use 
+ 				// CollisionEventHandlers.
+ 				CollisionEventHandler* handler0 = 
+ 					solid0->getCollisionEventHandler();
+ 				CollisionEventHandler* handler1 = 
+ 					solid1->getCollisionEventHandler();
+ 
  				if (//(0 == o0BodyID && 0 == o1BodyID) //case 1
  					//|| (o0BodyID == o1BodyID) //case 2
***************
*** 349,352 ****
--- 386,390 ----
  					//|| (0 == o0BodyID && !dBodyIsEnabled(o1BodyID)) //case 6
  					//|| (0 == o1BodyID && !dBodyIsEnabled(o0BodyID)) //case 7
+ 					|| (!makeContacts && !(handler0 || handler1))
  					)
  				{
***************
*** 354,359 ****
  				}

! 				// Now actually test for collision between the two geoms.
! 				ODESimulator* sim = (ODESimulator*)data;
  				dWorldID theWorldID = sim->internal_getWorldID();
  				dJointGroupID theJointGroupID = 
--- 392,397 ----
  				}

! 				// Now actually test for collision between the two geoms.  
! 				// This is one of the more expensive operations.
  				dWorldID theWorldID = sim->internal_getWorldID();
  				dJointGroupID theJointGroupID = 
***************
*** 371,403 ****
  				}

- 				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->getCollisionEventHandler();
- 				CollisionEventHandler* handler1 = 
- 					solid1->getCollisionEventHandler();
- 
  				if (handler0 || handler1)
  				{
--- 409,414 ----
***************
*** 486,489 ****
--- 497,501 ----
  						const Material* m1 = &(shape1->material);

+ 						dContact tempContact;
  						tempContact.surface.mode = dContactBounce | 
  							dContactSoftERP;// | dContactSoftCFM;
***************
*** 710,715 ****
  		}

- 		// Note: o0 should always be the main object (the geom to check against
- 		// everything else).
  		void internal_volumeCollisionCallback(void* data, dGeomID o0, 
  			dGeomID o1)
--- 722,725 ----
***************
*** 725,735 ****
  				// Colliding two geoms.

! 				dBodyID o0BodyID = dGeomGetBody(o0);
! 				dBodyID o1BodyID = dGeomGetBody(o1);
! 				bool bothHaveBodies = true;
! 				if (0 == o0BodyID || 0 == o1BodyID)
! 				{
! 					bothHaveBodies = false;
! 				}

  				// don't do collision detection for the following case:
--- 735,745 ----
  				// Colliding two geoms.

! 				//dBodyID o0BodyID = dGeomGetBody(o0);
! 				//dBodyID o1BodyID = dGeomGetBody(o1);
! 				//bool bothHaveBodies = true;
! 				//if (0 == o0BodyID || 0 == o1BodyID)
! 				//{
! 				//	bothHaveBodies = false;
! 				//}

  				// don't do collision detection for the following case:
***************
*** 743,747 ****
  				//}

! 				//now actually test for collision between the two geoms
  				dContactGeom contactArray[1];
  				int numContacts = dCollide(o0, o1, 1, contactArray, 
--- 753,778 ----
  				//}

! 				// Get a pointer to the ODESimulator.
! 				ODESimulator* sim = (ODESimulator*)data;
! 
! 				// Get pointers to the two geoms' GeomData structure.  
! 				GeomData* geomData0 = ((GeomData*)dGeomGetData(o0));
! 				GeomData* geomData1 = ((GeomData*)dGeomGetData(o1));
! 
! 				// Get pointers to the geoms' ShapeData structures.
! 				const ShapeData* shape0 = geomData0->shape;
! 				const ShapeData* shape1 = geomData1->shape;
! 
! 				// Check if the two Solids' contact groups generate contacts 
! 				// when they collide.
! 				bool makeContacts = sim->groupsMakeContacts(
! 					shape0->contactGroup, shape1->contactGroup);
! 				if (!makeContacts)
! 				{
! 					return;
! 				}
! 
! 				// Now actually test for collision between the two geoms.  
! 				// This is one of the more expensive operations.
  				dContactGeom contactArray[1];
  				int numContacts = dCollide(o0, o1, 1, contactArray, 
***************
*** 754,767 ****
  				else
  				{
! 					//these two geoms must be intersecting
! 					ODESimulator* sim = (ODESimulator*)data;

! 					// TODO: not sure if we can know that o1 is the volume 
! 					// object, so we'll
! 					// just call this twice for now
! 					GeomData* geomData0 = ((GeomData*)dGeomGetData(o0));
! 					GeomData* geomData1 = ((GeomData*)dGeomGetData(o1));
  					Solid* solid0 = geomData0->solid;
  					Solid* solid1 = geomData1->solid;
  					sim->internal_addCollidedSolid(solid0);
  					sim->internal_addCollidedSolid(solid1);
--- 785,799 ----
  				else
  				{
! 					// These two geoms must be intersecting.

! 					// Get pointers to the geoms' Solids.
  					Solid* solid0 = geomData0->solid;
  					Solid* solid1 = geomData1->solid;
+ 
+ 					// Not sure at this point if we can know that o1 is the 
+ 					// volume object, so we'll just call this twice.  It 
+ 					// will automatically keep from adding the same Solid 
+ 					// multiple times by using its collision count.  Later, 
+ 					// the volume Solid will be removed from this list.
  					sim->internal_addCollidedSolid(solid0);
  					sim->internal_addCollidedSolid(solid1);
***************
*** 770,774 ****
  		}

- 		// Note: o1 is the ray geom.
  		void internal_raycastCollisionCallback(void* data, dGeomID o0, 
  			dGeomID o1)
--- 802,805 ----
***************
*** 784,788 ****
  				// Colliding two geoms.

! 				// Now actually test for collision between the two geoms.
  				dContactGeom contactArray[1];
  				int numContacts = dCollide(o0, o1, 1, contactArray, 
--- 815,848 ----
  				// Colliding two geoms.

! 				// Get a pointer to the ODESimulator.
! 				ODESimulator* sim = (ODESimulator*)data;
! 
! 				// Get pointers to the two geoms' GeomData structure.  
! 				GeomData* geomData0 = ((GeomData*)dGeomGetData(o0));
! 				GeomData* geomData1 = ((GeomData*)dGeomGetData(o1));
! 
! 				// Only one of these (the one NOT belonging to the ray geom) 
! 				// will be non-NULL.
! 				unsigned int geomContactGroup = defaults::shape::contactGroup;
! 				if (geomData0)
! 				{
! 					geomContactGroup = geomData0->shape->contactGroup;
! 				}
! 				else
! 				{
! 					geomContactGroup = geomData1->shape->contactGroup;
! 				}
! 
! 				// Check if the two Solids' contact groups generate contacts 
! 				// when they collide.
! 				bool makeContacts = sim->groupsMakeContacts(
! 					geomContactGroup, sim->internal_getRayContactGroup());
! 				if (!makeContacts)
! 				{
! 					return;
! 				}
! 
! 				// Now actually test for collision between the two geoms.  
! 				// This is one of the more expensive operations.
  				dContactGeom contactArray[1];
  				int numContacts = dCollide(o0, o1, 1, contactArray, 
***************
*** 795,812 ****
  				else
  				{
! 					// These two geoms must be intersecting.
! 					ODESimulator* sim = (ODESimulator*)data;

- 					GeomData* geomData0 = ((GeomData*)dGeomGetData(o0));
  					if(geomData0)
  					{
- 						Point3r intersection(contactArray[0].pos[0], 
- 							contactArray[0].pos[1], contactArray[0].pos[2]);
- 						Vec3r normal(contactArray[0].normal[0], 
- 							contactArray[0].normal[1], 
- 							contactArray[0].normal[2]);
  						sim->internal_setRaycastResult(geomData0->solid, 
  							intersection, normal, contactArray[0].depth);
  					}
  				}
  			}
--- 855,878 ----
  				else
  				{
! 					// These two geoms must be intersecting.  Again, only 
! 					// one will be part of a Solid we want to store; the 
! 					// other is the ray.
! 
! 					Point3r intersection(contactArray[0].pos[0], 
! 						contactArray[0].pos[1], contactArray[0].pos[2]);
! 					Vec3r normal(contactArray[0].normal[0], 
! 						contactArray[0].normal[1], 
! 						contactArray[0].normal[2]);

  					if(geomData0)
  					{
  						sim->internal_setRaycastResult(geomData0->solid, 
  							intersection, normal, contactArray[0].depth);
  					}
+ 					else
+ 					{
+ 						sim->internal_setRaycastResult(geomData1->solid, 
+ 							intersection, normal, contactArray[0].depth);
+ 					}
  				}
  			}
***************
*** 853,856 ****
--- 919,927 ----
  	}

+ 	unsigned int ODESimulator::internal_getRayContactGroup()
+ 	{
+ 		return mRayContactGroup;
+ 	}
+ 
  	//helper function for collision callback
  	//void createOneSidedContact(dJointID contactJoint, dBodyID movingObject, 
***************
*** 875,879 ****
  	const RaycastResult& ODESimulator::internal_fireRay(const Rayr& r, 
  		real length, const Solid* attachedSolid, 
! 		unsigned long int rayContactGroup)
  	{
  		Point3r origin = r.getOrigin();
--- 946,950 ----
  	const RaycastResult& ODESimulator::internal_fireRay(const Rayr& r, 
  		real length, const Solid* attachedSolid, 
! 		unsigned int rayContactGroup)
  	{
  		Point3r origin = r.getOrigin();
***************
*** 885,894 ****
  		mRaycastResult.distance = 0;
  		mSensorSolid = attachedSolid;

! 		// TODO: use rayContactGroup to limit collision check
! 
  		dGeomID rayGeomID = dCreateRay(mRootSpaceID, length);
  		dGeomRaySet(rayGeomID, origin[0], origin[1], origin[2], dir[0], 
  			dir[1], dir[2]);

  		// Check for collisions.  This will fill mRaycastResult with valid 
--- 956,968 ----
  		mRaycastResult.distance = 0;
  		mSensorSolid = attachedSolid;
+ 		mRayContactGroup = rayContactGroup;

! 		// Create an ODE ray geom.  Make sure its user data pointer is 
! 		// NULL because this is used in the collision callback to 
! 		// distinguish the ray from other geoms.
  		dGeomID rayGeomID = dCreateRay(mRootSpaceID, length);
  		dGeomRaySet(rayGeomID, origin[0], origin[1], origin[2], dir[0], 
  			dir[1], dir[2]);
+ 		dGeomSetData(rayGeomID, NULL);

  		// Check for collisions.  This will fill mRaycastResult with valid 
***************
*** 911,916 ****
  			((ODESolid*)volume)->internal_getGeomDataList();

- 		// TODO: use the volume's contact group to limit collision check
- 
  		// Check for collisions with each of the volume Solid's geoms.  
  		// This will fill up mVolumeQueryResult with those Solids that 
--- 985,988 ----
***************
*** 1028,1032 ****
  	dJointGroupID ODESimulator::internal_getJointGroupID()const
  	{
! 		return mContactGroupID;
  	}
  }
--- 1100,1104 ----
  	dJointGroupID ODESimulator::internal_getJointGroupID()const
  	{
! 		return mContactJointGroupID;
  	}
  }