[ODE] Inconsistent depth return for dGeomRay

[Shamyl Zakariya]

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_