First draft. Lots of incredibly obvious optimizations to do. Going to build a tree and see how it improves. Currently I get 9.5 FPS with 100 checks per frame (I get 2000 regularly in the scene).
using UnityEngine;
using System.Collections;
using System;
public class BSPTree : MonoBehaviour {
[SerializeField]
int tests;
[SerializeField]
MeshFilter testMesh;
private Mesh mesh;
// Use this for initialization
void Start () {
mesh = testMesh.mesh;
triangles = new int[mesh.triangles.Length];
normals = new Vector3[mesh.normals.Length];
vertices = new Vector3[mesh.vertices.Length];
triangleLength = triangles.Length;
Array.Copy(mesh.triangles, triangles, mesh.triangles.Length);
Array.Copy(mesh.normals, normals, mesh.normals.Length);
Array.Copy(mesh.vertices, vertices, mesh.vertices.Length);
// DrawMesh(mesh, testMesh.transform);
}
// Update is called once per frame
void Update () {
for (int i = 0; i < tests; i++)
{
FindNearestPoint();
}
}
void FindNearestPoint()
{
Vector3 testPosition = transform.position;
Vector3 closestPoint = ClosestPointOn(testMesh, testPosition);
//DebugDraw.DrawMarker(closestPoint, 3.0f, Color.red, 0, false);
}
int[] triangles;
Vector3[] vertices;
Vector3[] normals;
int triangleLength;
Vector3 ClosestPointOn(MeshFilter filter, Vector3 to)
{
float shortestDistance = float.MaxValue;
int shortestTriangle = 0;
Vector3 shortestPoint = Vector3.zero;
// Iterate through all triangles
for (int i = 0; i < triangleLength; i += 3)
{
Vector3 p1 = TransformPoint(vertices[triangles[i]], filter.transform);
Vector3 p2 = TransformPoint(vertices[triangles[i + 1]], filter.transform);
Vector3 p3 = TransformPoint(vertices[triangles[i + 2]], filter.transform);
Vector3 normal = Vector3.Cross((p2 - p1).normalized, (p3 - p1).normalized).normalized;
Vector3 centroid = (p1 + p2 + p3) * 0.3333f;
Vector3 projected = Math3d.ProjectPointOnPlane(normal, centroid, to);
Vector3 uvw = Barycentric(projected, p1, p2, p3);
Vector3 nearest = ProjectPointOntoBarycentric(uvw, p1, p2, p3, projected);
float distance = (to - nearest).sqrMagnitude;
if (distance < shortestDistance)
{
shortestDistance = distance;
shortestTriangle = i;
shortestPoint = nearest;
}
}
// DrawTriangle(mesh.vertices[mesh.triangles[shortestTriangle]], mesh.vertices[mesh.triangles[shortestTriangle + 1]], mesh.vertices[mesh.triangles[shortestTriangle + 2]], filter.transform, Color.cyan);
return shortestPoint;
}
Vector3 Barycentric(Vector3 p, Vector3 a, Vector3 b, Vector3 c)
{
Vector3 v0 = b - a, v1 = c - a, v2 = p - a;
float d00 = Vector3.Dot(v0, v0);
float d01 = Vector3.Dot(v0, v1);
float d11 = Vector3.Dot(v1, v1);
float d20 = Vector3.Dot(v2, v0);
float d21 = Vector3.Dot(v2, v1);
float denom = d00 * d11 - d01 * d01;
float v = (d11 * d20 - d01 * d21) / denom;
float w = (d00 * d21 - d01 * d20) / denom;
float u = 1.0f - v - w;
return new Vector3(u, v, w);
}
Vector3 ProjectPointOntoBarycentric(Vector3 barycentric, Vector3 p1, Vector3 p2, Vector3 p3, Vector3 p)
{
float u = barycentric.x;
float v = barycentric.y;
float w = barycentric.z;
Vector3 projected = Vector3.zero;
if (u > 0 && v > 0 && w < 0)
{
projected = Math3d.ProjectPointOnLineSegment(p1, p2, p);
}
else if (u < 0 && v > 0 && w > 0)
{
projected = Math3d.ProjectPointOnLineSegment(p2, p3, p);
}
else if (u > 0 && v < 0 && w > 0)
{
projected = Math3d.ProjectPointOnLineSegment(p3, p1, p);
}
else if (u > 0 && v < 0 && w < 0)
{
projected = p1;
}
else if (u < 0 && v > 0 && w < 0)
{
projected = p2;
}
else if (u < 0 && v < 0 && w > 0)
{
projected = p3;
}
else
{
projected = p;
}
return projected;
}
Vector3 TransformPoint(Vector3 p, Transform t)
{
return t.rotation * p + t.position;
}
void DrawMesh(Mesh mesh, Transform t)
{
for (int i = 0; i < mesh.triangles.Length; i += 3)
{
DrawTriangle(mesh.vertices[mesh.triangles[i]], mesh.vertices[mesh.triangles[i + 1]], mesh.vertices[mesh.triangles[i + 2]], t);
}
}
void DrawTriangle(Vector3 a, Vector3 b, Vector3 c)
{
Debug.DrawLine(a, b, Color.cyan);
Debug.DrawLine(b, c, Color.cyan);
Debug.DrawLine(c, a, Color.cyan);
}
void DrawTriangle(Vector3 a, Vector3 b, Vector3 c, Color color)
{
Debug.DrawLine(a, b, color);
Debug.DrawLine(b, c, color);
Debug.DrawLine(c, a, color);
}
void DrawTriangle(Vector3 a, Vector3 b, Vector3 c, Transform t)
{
a = t.rotation * a + t.position;
b = t.rotation * b + t.position;
c = t.rotation * c + t.position;
Debug.DrawLine(a, b, Color.black);
Debug.DrawLine(b, c, Color.black);
Debug.DrawLine(c, a, Color.black);
}
void DrawTriangle(Vector3 a, Vector3 b, Vector3 c, Transform t, Color color)
{
a = t.rotation * a + t.position;
b = t.rotation * b + t.position;
c = t.rotation * c + t.position;
Debug.DrawLine(a, b, color);
Debug.DrawLine(b, c, color);
Debug.DrawLine(c, a, color);
}
void OnDrawGizmos()
{
Gizmos.color = Color.red;
Gizmos.DrawSphere(transform.position, 0.25f);
}
}
