private void ExtractTriangles()
{
if ((this.subMeshIndex >= 0) && (this.mesh.subMeshCount >= 0))
{
int submesh = Mathf.Min(this.subMeshIndex, this.mesh.subMeshCount - 1);
int[] triangles = this.mesh.GetTriangles(submesh);
Vector3[] vertices = this.mesh.vertices;
Vector2[] uv = this.mesh.uv;
Vector2[] vectorArray3 = this.mesh.uv2;
if (triangles.Length > 0)
{
for (int i = (triangles.Length / 3) - 1; i >= 0; i--)
{
P3D_Triangle item = P3D_Triangle.Spawn();
int index = triangles[i * 3];
int num5 = triangles[(i * 3) + 1];
int num6 = triangles[(i * 3) + 2];
item.PointA = vertices[index];
item.PointB = vertices[num5];
item.PointC = vertices[num6];
item.Coord1A = uv[index];
item.Coord1B = uv[num5];
item.Coord1C = uv[num6];
if (vectorArray3.Length > 0)
{
item.Coord2A = vectorArray3[index];
item.Coord2B = vectorArray3[num5];
item.Coord2C = vectorArray3[num6];
}
this.triangles.Add(item);
}
}
}
}
public static bool ClosestBarycentric(Vector3 point, P3D_Triangle triangle, ref Vector3 weights, ref float distanceSqr)
{
Vector3 pointA = triangle.PointA;
Vector3 pointB = triangle.PointB;
Vector3 pointC = triangle.PointC;
Quaternion quaternion = Quaternion.Inverse(Quaternion.LookRotation(-Vector3.Cross(pointA - pointB, pointA - pointC)));
Vector3 a = (Vector3)(quaternion * pointA);
Vector3 b = (Vector3)(quaternion * pointB);
Vector3 vector6 = (Vector3)(quaternion * pointC);
Vector3 p = (Vector3)(quaternion * point);
if (!PointRightOfLine(a, b, p) || (!PointRightOfLine(b, vector6, p) || !PointRightOfLine(vector6, a, p)))
{
return(false);
}
Vector3 lhs = b - a;
Vector3 rhs = vector6 - a;
Vector3 vector10 = p - a;
float num = Vector2.Dot(lhs, lhs);
float num2 = Vector2.Dot(lhs, rhs);
float num3 = Vector2.Dot(rhs, rhs);
float num4 = Vector2.Dot(vector10, lhs);
float num5 = Vector2.Dot(vector10, rhs);
float num6 = Reciprocal((num * num3) - (num2 * num2));
weights.y = ((num3 * num4) - (num2 * num5)) * num6;
weights.z = ((num * num5) - (num2 * num4)) * num6;
weights.x = (1f - weights.y) - weights.z;
Vector3 vector11 = (Vector3)(((weights.x * pointA) + (weights.y * pointB)) + (weights.z * pointC));
distanceSqr = (point - vector11).sqrMagnitude;
return(true);
}
private P3D_Result GetResult(P3D_Triangle triangle, Vector3 weights, float distance01)
{
ClearResults();
AddToResults(triangle, weights, distance01);
return(results[0]);
}
private void AddToResults(P3D_Triangle triangle, Vector3 weights, float distance01)
{
P3D_Result item = P3D_Result.Spawn();
item.Triangle = triangle;
item.Weights = weights;
item.Distance01 = distance01;
results.Add(item);
}
// This adds a triangle result to the results list
private void AddToResults(P3D_Triangle triangle, Vector3 weights, float distance01)
{
var result = P3D_Result.Spawn();
result.Triangle = triangle;
result.Weights = weights;
result.Distance01 = distance01;
results.Add(result);
}
private void SortTriangle(P3D_Triangle triangle, ref int minIndex, ref int maxIndex, bool abovePivot)
{
if (abovePivot == true)
{
triangles[maxIndex - 1] = triangle; maxIndex -= 1;
}
else
{
triangles[minIndex] = triangle; minIndex += 1;
}
}
public static P3D_Triangle Spawn()
{
if (pool.Count <= 0)
{
return(new P3D_Triangle());
}
int index = pool.Count - 1;
P3D_Triangle triangle = pool[index];
pool.RemoveAt(index);
return(triangle);
}
private void SortTriangle(P3D_Triangle triangle, ref int minIndex, ref int maxIndex, bool abovePivot)
{
if (abovePivot)
{
this.triangles[maxIndex - 1] = triangle;
maxIndex--;
}
else
{
this.triangles[minIndex] = triangle;
minIndex++;
}
}
// This will compare a triangle against a point, and return the barycentric weights of the closest point
public static float ClosestBarycentric(Vector3 point, P3D_Triangle triangle, out Vector3 weights)
{
// Project point to triangle space
var a = triangle.PointA;
var b = triangle.PointB;
var c = triangle.PointC;
var r = Quaternion.Inverse(Quaternion.LookRotation(-Vector3.Cross(a - b, a - c)));
var ra = r * a;
var rb = r * b;
var rc = r * c;
var rp = r * point;
// Calculate weights to line segments?
if (PointLeftOfLine(ra, rb, rp) == true)
{
var weight = ClosestBarycentric(rp, ra, rb); weights = new Vector3(1.0f - weight, weight, 0.0f);
}
else if (PointLeftOfLine(rb, rc, rp) == true)
{
var weight = ClosestBarycentric(rp, rb, rc); weights = new Vector3(0.0f, 1.0f - weight, weight);
}
else if (PointLeftOfLine(rc, ra, rp) == true)
{
var weight = ClosestBarycentric(rp, rc, ra); weights = new Vector3(weight, 0.0f, 1.0f - weight);
}
// Calculate weight to triangle?
else
{
var v0 = rb - ra;
var v1 = rc - ra;
var v2 = rp - ra;
var d00 = Vector2.Dot(v0, v0);
var d01 = Vector2.Dot(v0, v1);
var d11 = Vector2.Dot(v1, v1);
var d20 = Vector2.Dot(v2, v0);
var d21 = Vector2.Dot(v2, v1);
var denom = P3D_Helper.Reciprocal(d00 * d11 - d01 * d01);
weights.y = (d11 * d20 - d01 * d21) * denom;
weights.z = (d00 * d21 - d01 * d20) * denom;
weights.x = 1.0f - weights.y - weights.z;
}
// Return square distance from point to closest point
var closest = weights.x * a + weights.y * b + weights.z * c;
return((point - closest).sqrMagnitude);
}
public void Clear()
{
this.mesh = null;
this.vertexCount = 0;
this.subMeshIndex = 0;
for (int i = this.triangles.Count - 1; i >= 0; i--)
{
P3D_Triangle.Despawn(this.triangles[i]);
}
this.triangles.Clear();
for (int j = this.nodes.Count - 1; j >= 0; j--)
{
P3D_Node.Despawn(this.nodes[j]);
}
this.nodes.Clear();
}
public static float ClosestBarycentric(Vector3 point, P3D_Triangle triangle, out Vector3 weights)
{
Vector3 pointA = triangle.PointA;
Vector3 pointB = triangle.PointB;
Vector3 pointC = triangle.PointC;
Quaternion quaternion = Quaternion.Inverse(Quaternion.LookRotation(-Vector3.Cross(pointA - pointB, pointA - pointC)));
Vector3 a = (Vector3)(quaternion * pointA);
Vector3 b = (Vector3)(quaternion * pointB);
Vector3 vector6 = (Vector3)(quaternion * pointC);
Vector3 p = (Vector3)(quaternion * point);
if (PointLeftOfLine(a, b, p))
{
float y = ClosestBarycentric(p, a, b);
weights = new Vector3(1f - y, y, 0f);
}
else if (PointLeftOfLine(b, vector6, p))
{
float z = ClosestBarycentric(p, b, vector6);
weights = new Vector3(0f, 1f - z, z);
}
else if (PointLeftOfLine(vector6, a, p))
{
float x = ClosestBarycentric(p, vector6, a);
weights = new Vector3(x, 0f, 1f - x);
}
else
{
Vector3 lhs = b - a;
Vector3 rhs = vector6 - a;
Vector3 vector10 = p - a;
float num4 = Vector2.Dot(lhs, lhs);
float num5 = Vector2.Dot(lhs, rhs);
float num6 = Vector2.Dot(rhs, rhs);
float num7 = Vector2.Dot(vector10, lhs);
float num8 = Vector2.Dot(vector10, rhs);
float num9 = Reciprocal((num4 * num6) - (num5 * num5));
weights.y = ((num6 * num7) - (num5 * num8)) * num9;
weights.z = ((num4 * num8) - (num5 * num7)) * num9;
weights.x = (1f - weights.y) - weights.z;
}
Vector3 vector11 = (Vector3)(((weights.x * pointA) + (weights.y * pointB)) + (weights.z * pointC));
return((point - vector11).sqrMagnitude);
}
private void SortTriangles(int minIndex, int maxIndex)
{
potentials.Clear();
Vector3 min = this.triangles[minIndex].Min;
Vector3 max = this.triangles[minIndex].Max;
Vector3 zero = Vector3.zero;
for (int i = minIndex; i < maxIndex; i++)
{
P3D_Triangle item = this.triangles[i];
min = Vector3.Min(min, item.Min);
max = Vector3.Max(max, item.Max);
zero += (item.PointA + item.PointB) + item.PointC;
potentials.Add(item);
}
Vector3 vector4 = max - min;
if ((vector4.x > vector4.y) && (vector4.x > vector4.z))
{
float num2 = P3D_Helper.Divide(zero.x, this.triangles.Count * 3f);
for (int j = potentials.Count - 1; j >= 0; j--)
{
P3D_Triangle triangle = potentials[j];
this.SortTriangle(triangle, ref minIndex, ref maxIndex, triangle.MidX >= num2);
}
}
else if ((vector4.y > vector4.x) && (vector4.y > vector4.z))
{
float num4 = P3D_Helper.Divide(zero.y, this.triangles.Count * 3f);
for (int j = potentials.Count - 1; j >= 0; j--)
{
P3D_Triangle triangle = potentials[j];
this.SortTriangle(triangle, ref minIndex, ref maxIndex, triangle.MidY >= num4);
}
}
else
{
float num6 = P3D_Helper.Divide(zero.z, this.triangles.Count * 3f);
for (int j = potentials.Count - 1; j >= 0; j--)
{
P3D_Triangle triangle = potentials[j];
this.SortTriangle(triangle, ref minIndex, ref maxIndex, triangle.MidZ >= num6);
}
}
}
public List <P3D_Result> FindBetweenAll(Vector3 startPoint, Vector3 endPoint)
{
this.ClearResults();
if (this.IsReady)
{
this.BeginSearchBetween(startPoint, endPoint);
for (int i = potentials.Count - 1; i >= 0; i--)
{
P3D_Triangle triangle = potentials[i];
Vector3 weights = new Vector3();
float num2 = 0f;
if (P3D_Helper.IntersectBarycentric(startPoint, endPoint, triangle, out weights, out num2))
{
this.AddToResults(triangle, weights, num2);
}
}
}
return(results);
}
// This will intersect a triangle against a line segment, and return the barycentric weights of the hit point
public static bool IntersectBarycentric(Vector3 start, Vector3 end, P3D_Triangle triangle, out Vector3 weights, out float distance01)
{
weights = default(Vector3);
distance01 = 0.0f;
var e1 = triangle.Edge1;
var e2 = triangle.Edge2;
var d = end - start;
var p = Vector3.Cross(d, e2);
var det = Vector3.Dot(e1, p);
if (Mathf.Abs(det) < float.Epsilon)
{
return(false);
}
var detRecip = 1.0f / det;
var t = start - triangle.PointA;
weights.x = Vector3.Dot(t, p) * detRecip;
if (weights.x < -float.Epsilon || weights.x > 1.0f + float.Epsilon)
{
return(false);
}
var q = Vector3.Cross(t, e1);
weights.y = Vector3.Dot(d, q) * detRecip;
var xy = weights.x + weights.y;
if (weights.y < -float.Epsilon || xy > 1.0f + float.Epsilon)
{
return(false);
}
weights = new Vector3(1.0f - xy, weights.x, weights.y);
distance01 = Vector3.Dot(e2, q) * detRecip;
return(distance01 >= 0.0f && distance01 <= 1.0f);
}
// This allows you to reset this class
public void Clear()
{
mesh = null;
vertexCount = 0;
materialIndex = 0;
for (var i = triangles.Count - 1; i >= 0; i--)
{
P3D_Triangle.Despawn(triangles[i]);
}
triangles.Clear();
for (var i = nodes.Count - 1; i >= 0; i--)
{
P3D_Node.Despawn(nodes[i]);
}
nodes.Clear();
}
public List <P3D_Result> FindPerpendicularAll(Vector3 point, float maxDistance)
{
this.ClearResults();
if (this.IsReady && (maxDistance > 0f))
{
float maxDistanceSqr = maxDistance * maxDistance;
this.BeginSearchDistance(point, maxDistanceSqr);
for (int i = potentials.Count - 1; i >= 0; i--)
{
P3D_Triangle triangle = potentials[i];
Vector3 weights = new Vector3();
float distanceSqr = 0f;
if (P3D_Helper.ClosestBarycentric(point, triangle, ref weights, ref distanceSqr) && (distanceSqr <= maxDistanceSqr))
{
this.AddToResults(triangle, weights, Mathf.Sqrt(distanceSqr) / maxDistance);
}
}
}
return(results);
}
// This will extract all triangles from the current mesh
private void ExtractTriangles()
{
if (materialIndex >= 0 && mesh.subMeshCount >= 0)
{
var submeshIndex = Mathf.Min(materialIndex, mesh.subMeshCount - 1);
var indices = mesh.GetTriangles(submeshIndex);
var allPositions = mesh.vertices;
var allCoords1 = mesh.uv;
var allCoords2 = mesh.uv2;
if (indices.Length > 0)
{
var triangleCount = indices.Length / 3;
for (var i = triangleCount - 1; i >= 0; i--)
{
var triangle = P3D_Triangle.Spawn();
var a = indices[i * 3 + 0];
var b = indices[i * 3 + 1];
var c = indices[i * 3 + 2];
triangle.PointA = allPositions[a];
triangle.PointB = allPositions[b];
triangle.PointC = allPositions[c];
triangle.Coord1A = allCoords1[a];
triangle.Coord1B = allCoords1[b];
triangle.Coord1C = allCoords1[c];
if (allCoords2.Length > 0)
{
triangle.Coord2A = allCoords2[a];
triangle.Coord2B = allCoords2[b];
triangle.Coord2C = allCoords2[c];
}
triangles.Add(triangle);
}
}
}
}
public void CalculateBound(List <P3D_Triangle> triangles)
{
if ((triangles.Count > 0) && (this.TriangleCount > 0))
{
Vector3 min = triangles[this.TriangleIndex].Min;
Vector3 max = triangles[this.TriangleIndex].Max;
int num = (this.TriangleIndex + this.TriangleCount) - 1;
while (true)
{
if (num <= this.TriangleIndex)
{
this.Bound.SetMinMax(min, max);
break;
}
P3D_Triangle triangle = triangles[num];
min = Vector3.Min(min, triangle.Min);
max = Vector3.Max(max, triangle.Max);
num--;
}
}
}
public static bool ClosestBarycentric(Vector3 point, P3D_Triangle triangle, ref Vector3 weights, ref float distanceSqr)
{
// Project point to triangle space
var a = triangle.PointA;
var b = triangle.PointB;
var c = triangle.PointC;
var r = Quaternion.Inverse(Quaternion.LookRotation(-Vector3.Cross(a - b, a - c)));
var ra = r * a;
var rb = r * b;
var rc = r * c;
var rp = r * point;
// Point is over triangle face?
if (PointRightOfLine(ra, rb, rp) == true && PointRightOfLine(rb, rc, rp) == true && PointRightOfLine(rc, ra, rp) == true)
{
var v0 = rb - ra;
var v1 = rc - ra;
var v2 = rp - ra;
var d00 = Vector2.Dot(v0, v0);
var d01 = Vector2.Dot(v0, v1);
var d11 = Vector2.Dot(v1, v1);
var d20 = Vector2.Dot(v2, v0);
var d21 = Vector2.Dot(v2, v1);
var denom = P3D_Helper.Reciprocal(d00 * d11 - d01 * d01);
weights.y = (d11 * d20 - d01 * d21) * denom;
weights.z = (d00 * d21 - d01 * d20) * denom;
weights.x = 1.0f - weights.y - weights.z;
var closest = weights.x * a + weights.y * b + weights.z * c;
distanceSqr = (point - closest).sqrMagnitude;
return(true);
}
return(false);
}
public static bool IntersectBarycentric(Vector3 start, Vector3 end, P3D_Triangle triangle, out Vector3 weights, out float distance01)
{
Vector3 vector = new Vector3();
weights = vector;
distance01 = 0f;
Vector3 lhs = triangle.Edge1;
Vector3 rhs = triangle.Edge2;
Vector3 vector4 = end - start;
Vector3 vector5 = Vector3.Cross(vector4, rhs);
float f = Vector3.Dot(lhs, vector5);
if (Mathf.Abs(f) < float.Epsilon)
{
return(false);
}
float num2 = 1f / f;
Vector3 vector6 = start - triangle.PointA;
weights.x = Vector3.Dot(vector6, vector5) * num2;
if ((weights.x < -1.401298E-45f) || (weights.x > 1f))
{
return(false);
}
Vector3 vector7 = Vector3.Cross(vector6, lhs);
weights.y = Vector3.Dot(vector4, vector7) * num2;
float num3 = weights.x + weights.y;
if ((weights.y < -1.401298E-45f) || (num3 > 1f))
{
return(false);
}
weights = new Vector3(1f - num3, weights.x, weights.y);
distance01 = Vector3.Dot(rhs, vector7) * num2;
return((distance01 >= 0f) && (distance01 <= 1f));
}
public P3D_Result FindPerpendicularNearest(Vector3 point, float maxDistance)
{
this.ClearResults();
if (this.IsReady && (maxDistance > 0f))
{
float maxDistanceSqr = maxDistance * maxDistance;
P3D_Triangle triangle = null;
Vector3 vector2 = new Vector3();
Vector3 weights = vector2;
this.BeginSearchDistance(point, maxDistanceSqr);
int num2 = potentials.Count - 1;
while (true)
{
if (num2 < 0)
{
if (triangle == null)
{
break;
}
return(this.GetResult(triangle, weights, Mathf.Sqrt(maxDistanceSqr) / maxDistance));
}
P3D_Triangle triangle2 = potentials[num2];
vector2 = new Vector3();
Vector3 vector3 = vector2;
float distanceSqr = 0f;
if (P3D_Helper.ClosestBarycentric(point, triangle2, ref vector3, ref distanceSqr) && (distanceSqr <= maxDistanceSqr))
{
maxDistanceSqr = distanceSqr;
triangle = triangle2;
weights = vector3;
}
num2--;
}
}
return(null);
}
public P3D_Result FindBetweenNearest(Vector3 startPoint, Vector3 endPoint)
{
this.ClearResults();
if (this.IsReady)
{
float positiveInfinity = float.PositiveInfinity;
P3D_Triangle triangle = null;
Vector3 vector2 = new Vector3();
Vector3 weights = vector2;
this.BeginSearchBetween(startPoint, endPoint);
int num2 = potentials.Count - 1;
while (true)
{
if (num2 < 0)
{
if (triangle == null)
{
break;
}
return(this.GetResult(triangle, weights, positiveInfinity));
}
P3D_Triangle triangle2 = potentials[num2];
vector2 = new Vector3();
Vector3 vector3 = vector2;
float num3 = 0f;
if (P3D_Helper.IntersectBarycentric(startPoint, endPoint, triangle2, out vector3, out num3) && (num3 < positiveInfinity))
{
positiveInfinity = num3;
triangle = triangle2;
weights = vector3;
}
num2--;
}
}
return(null);
}
public static P3D_Triangle Despawn(P3D_Triangle triangle)
{
pool.Add(triangle);
return(null);
}
