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); }