Vector3 ObjectCollisionTest(Vector3 rayStart, Vector3 rayEnd, SimulationObject obj)
{
const float OO_THREE = 1f / 3f;
Vector3 closestPoint = rayEnd;
if (rayStart == rayEnd)
{
Logger.DebugLog("RayStart is equal to RayEnd, returning given location");
return(closestPoint);
}
Vector3 direction = Vector3.Normalize(rayEnd - rayStart);
Ray ray = new Ray(rayStart, direction);
// Get the mesh that has been transformed into world-space
SimpleMesh mesh = null;
if (obj.Prim.ParentID != 0)
{
SimulationObject parent;
if (server.Scene.TryGetObject(obj.Prim.ParentID, out parent))
{
mesh = obj.GetWorldMesh(DetailLevel.Low, parent);
}
}
else
{
mesh = obj.GetWorldMesh(DetailLevel.Low, null);
}
if (mesh != null)
{
// Iterate through all of the triangles in the mesh, doing a ray-triangle intersection
float closestDistance = Single.MaxValue;
for (int i = 0; i < mesh.Indices.Count; i += 3)
{
Vector3 point0 = mesh.Vertices[mesh.Indices[i + 0]].Position;
Vector3 point1 = mesh.Vertices[mesh.Indices[i + 1]].Position;
Vector3 point2 = mesh.Vertices[mesh.Indices[i + 2]].Position;
if (RayTriangleIntersection(rayStart, direction, point0, point1, point2))
{
// HACK: Find the barycenter of this triangle. Would be better to have
// RayTriangleIntersection return the exact collision point
Vector3 center = (point0 + point1 + point2) * OO_THREE;
Logger.DebugLog("Collision hit with triangle at " + center);
if ((center - rayStart).Length() < closestDistance)
{
closestPoint = center;
}
}
}
}
return(closestPoint);
}
