[ODE] Inconsistent depth return for dGeomRay

[Shamyl Zakariya]

For reference, I'm on the default ODE 0.39, NOT from CVS. Compiled with 
GCC 3.3 on Mac OS X 10.3

Shamyl Zakariya
   "this is, after all, one of those movies where people spend a great
   deal of time looking at things and pointing."
	From a review of _Fantastic Voyage_

On Jan 13, 2004, at 12:29 AM, Shamyl Zakariya wrote:

> All
>
> I've been trying to implement a kind of range finder class using 
> dGeomRay -- as part of a feedback system for my robotics simulations.
>
> I'm having trouble with the depth returned by dCollide, however -- 
> it's inconsistent, or rather, it seems to sometimes skip geoms in my 
> simulation completely; other times it sees the same geom just fine. 
> It's repeatable, but doesn't follow any pattern I can detect.
>
> The screenshots I'm linking to may clarify what I'm getting at here:
>
> http://home.earthlink.net/~zakariya/ray/0.png
> http://home.earthlink.net/~zakariya/ray/1.png
> http://home.earthlink.net/~zakariya/ray/2.png
> http://home.earthlink.net/~zakariya/ray/3.png
>
> The pink line is the ray, being emitted from the tower on the dark 
> square in the middle; the small pink sphere/lumpy thing is the contact 
> point as reported by dCollide. In some circumstances it collides 
> correctly, in others is passes through and collides with an object 
> behind. For reference, I'm fairly certain the objects in question 
> really are where they're supposed to be, as my robot successfully 
> bumps into them when walking around.
>
> Now, in case I've missed something obvious I'm attaching some code.
>
> Here's the relevant part of my collision callback function:
>
> static void nearCallback (void *data, dGeomID o1, dGeomID o2)
> {
> 	/*
> 		Check to see if this is a ray collision
> 		RangeSensor class's static method SensorForRay
> 		returns a pointer to any RangeSensor implementation
> 		which uses that geom.
> 	*/
> 	
> 	if ( dGeomGetClass( o1 ) == dRayClass || dGeomGetClass( o2 ) == 
> dRayClass )
> 	{
> 		RangeSensor *rangeSensor = NULL;
> 		if ( rangeSensor = RangeSensor::SensorForRay( o1 ) )
> 		{
> 			rangeSensor->measure( o2 );
> 		}
> 		else if ( rangeSensor = RangeSensor::SensorForRay( o2 ) )
> 		{
> 			rangeSensor->measure( o1 );
> 		}
> 		
> 		//don't let rays be processed by default code
> 		return;
> 	}
>
> 	//the rest is irrelevant here
> }
>
> The RangeSensor::measure function is below, it calls dCollide with one 
> contact. I checked to see if using multiple contacts made any 
> difference, but as per the documentation, it always finds only one 
> contact point. Here's the method:
>
> void RangeSensor::measure( dGeomID geom )
> {
> 	dContactGeom contact;	
> 	if ( dCollide( _rayGeom, geom, 1, &contact, sizeof(dContactGeom)) > 0 
> )
> 	{
> 		_contactPos[0] = contact.pos[0];
> 		_contactPos[1] = contact.pos[1];
> 		_contactPos[2] = contact.pos[2];
> 		_contactPos[3] = contact.pos[3];
>
> 		_distance = contact.depth;
> 	}
> }
>
> And, my drawing and stepping methods. For reference, RangeSensor is 
> has step() called on it right before the simulation is stepped.
>
> void RangeSensor::draw( void )
> {
> 	// get the ray's origin and direction
> 	dVector3 start, dir, endpoint;
> 	dGeomRayGet( _rayGeom, start, dir );
>
> 	// crop the distance to either the max length or what was
> 	// calculated in ::measure()
> 	dReal distance = _distance < _maxLength ? _distance : _maxLength;
>
> //	static int count = 0;
> //	if ( !( count++ % 10 ))
> //	{
> //		printf("RangeSensor::draw() ray depth: %f\n", distance );
> //	}
>
> 	// project endpoint of ray
> 	endpoint[0] = start[0] + distance * dir[0];
> 	endpoint[1] = start[1] + distance * dir[1];
> 	endpoint[2] = start[2] + distance * dir[2];
> 	
> 	GLVisualization *graphics = GLVisualization::instance();
> 	graphics->setColor( _color[0], _color[1], _color[2] );
> 	graphics->drawLine( start, endpoint );
> 	
> 	if ( !_contactVisual )
> 	{
> 		// 2 refers to the radius
> 		_contactVisual = graphics->buildSphere( 2 );
> 	}
>
> 	// draw contact point, if ray intersects something
> 	if ( _distance < _maxLength )
> 	{
> 		dMatrix3 R;
> 		dRSetIdentity( R ); // we don't need to orient the contact point
> 		graphics->drawObject( _contactVisual, _contactPos, R );
> 	}
> 	
> }
>
> void RangeSensor::step( void )
> {
>
> 	/*
> 		The anchor and direction vector are in
> 		coordinates relative to _attachedTo, so we've got
> 		to convert them into world coordinates and update
> 		our ray accordingly.
> 	*/
> 		
> 	dVector3 anchor, direction;
> 	
> 	for (int i = 0; i < 3; i++ )
> 	{
> 		anchor[i] = _anchor[i];
> 		direction[i] = _direction[i];
> 	}
> 	
> 	// these functions convert the point and vector
> 	// from values relative to _attachedTo, to
> 	// world coordinates. I know they work correctly.
> 	// _attachedTo is just a class with a geom
> 	getRelPoint( _attachedTo, anchor );
> 	getRelAxis( _attachedTo, direction );
> 	
> 	// update the ray
> 	dGeomRaySet( _rayGeom, anchor[0], anchor[1], anchor[2],
> 				 direction[0], direction[1], direction[2] );
> 				
> 	//reset distance measurement, it will be properly measure
> 	//soon when collision detection is performed
> 	_distance = _maxLength;
> }
>
> I hope somebody can help me here -- this is very frustrating.
>
> Shamyl Zakariya
>   "this is, after all, one of those movies where people spend a great
>   deal of time looking at things and pointing."
> 	From a review of _Fantastic Voyage_
>
> _______________________________________________
> ODE mailing list
> ODE at q12.org
> http://q12.org/mailman/listinfo/ode
>