public RayCastResult CastRay(Point3D position, Vector3D direction, double rayLength, BodyFilterType filterType, params Body[] bodies)
{
//TODO: If the ray starts outside the boundries, then this method will fail. Fix that here, so the user doesn't
// have to worry about it. Also, if the ray length would cause an endpoint outside the bounds
//
// Also, if I fixed the start distance, then I need to add that back into the result, so it's transparent to the caller
CBody[] bodyHandles = null;
if (bodies.Length > 0)
{
// make a list of newton handles
bodyHandles = new CBody[bodies.Length];
for (int i = 0; i < bodies.Length; i++)
{
bodyHandles[i] = bodies[i].NewtonBody;
}
}
//TODO: Always skip the boundry terrains
EventHandler<CWorldRayPreFilterEventArgs> preFilterHandler = null;
if (bodyHandles != null)
{
// This artificial method gets called from within NewtonWorld.WorldRayCast
preFilterHandler = delegate(object sender, CWorldRayPreFilterEventArgs preFilterArgs)
{
switch (filterType)
{
case BodyFilterType.ExcludeBodies:
preFilterArgs.Skip = (Array.IndexOf(bodyHandles, preFilterArgs.Body)) >= 0;
break;
case BodyFilterType.IncludeBodies:
preFilterArgs.Skip = (Array.IndexOf(bodyHandles, preFilterArgs.Body)) < 0;
break;
}
};
}
List<CWorldRayFilterEventArgs> hitTestResults = new List<CWorldRayFilterEventArgs>();
Vector3D posAsVector = position.ToVector(); // newt wants a vector instead of a point3d
// Ask newton to do the hit test (it will invoke the filter delegate, whose implementation is above)
this.NewtonWorld.WorldRayCast(posAsVector, posAsVector + (direction * rayLength),
delegate(object sender, CWorldRayFilterEventArgs filterArgs)
{
hitTestResults.Add(filterArgs);
},
null, preFilterHandler);
if (hitTestResults.Count > 0)
{
// Find the closest one
CWorldRayFilterEventArgs hitTestResult = null;
double distance = double.MaxValue;
for (int i = 0; i < hitTestResults.Count; i++)
{
double d = (hitTestResults[i].IntersetParam * rayLength);
if (d < distance)
{
distance = d;
hitTestResult = hitTestResults[i];
}
}
Body resultBody = UtilityNewt.GetBodyFromUserData(hitTestResult.Body);
if (resultBody == null)
{
throw new ApplicationException("Couldn't get the Body from the CBody.UserData");
}
// Exit Function
return new RayCastResult(resultBody, distance, hitTestResult.HitNormal, hitTestResult.IntersetParam);
}
else
{
// Nothing found
return null;
}
}
/// <summary>
/// NOTE: The ray must be added to the viewport, or this overload won't work
/// NOTE: Taking in a ray instead of simply taking two Point3D's seems overly complex. Why the need for the ray to be added to the viewport? Newton doesn't need that...
/// </summary>
public RayCastResult CastRay(Ray ray, BodyFilterType filterType, params Body[] bodies)
{
Matrix3D localToWorld = MathUtils.GetTransformToWorld(ray);
Vector3D direction;
if (ray.DirectionOrigin == ObjectOrigin.Local)
{
direction = localToWorld.Transform(ray.Direction);
}
else
{
direction = ray.Direction;
}
Point3D position = localToWorld.Transform(new Point3D());
// Call my overload
return CastRay(position, direction, ray.RayLength, filterType, bodies);
}
