protected override RayCastResult[] OnRayCastPiercing(Ray ray, int contactGroup)
{
if (float.IsNaN(ray.Origin.X))
{
Log.Fatal("PhysicsWorld.RayCast: Single.IsNaN(ray.Origin.X)");
}
if (float.IsNaN(ray.Direction.X))
{
Log.Fatal("PhysicsWorld.RayCast: Single.IsNaN(ray.Direction.X)");
}
if (ray.Direction == Vec3.Zero)
{
return(emptyPiercingRayCastResult);
}
Vec3 dirNormal = ray.Direction;
float length = dirNormal.Normalize();
Ode.dGeomRaySet(rayCastGeomID, ray.Origin.X, ray.Origin.Y, ray.Origin.Z,
dirNormal.X, dirNormal.Y, dirNormal.Z);
Ode.dGeomRaySetLength(rayCastGeomID, length);
int count;
IntPtr data;
Ode.DoRayCastPiercing(neoAxisAdditionsID, contactGroup, out count, out data);
if (count == 0)
{
return(emptyPiercingRayCastResult);
}
RayCastResult[] array = new RayCastResult[count];
unsafe
{
Ode.RayCastResult *pointer = (Ode.RayCastResult *)data;
for (int n = 0; n < count; n++)
{
RayCastResult result = new RayCastResult();
result.Shape = shapesDictionary[pointer->shapeDictionaryIndex].shape;
result.Position = Convert.ToNet(pointer->position);
result.Normal = Convert.ToNet(pointer->normal);
result.Distance = pointer->distance;
result.TriangleID = pointer->triangleID;
array[n] = result;
pointer++;
}
}
//sort by distance
ArrayUtils.SelectionSort(array, rayCastResultDistanceComparer);
return(array);
}