395 lines
14 KiB
C#
395 lines
14 KiB
C#
using System;
|
|
using Unity.Collections;
|
|
using System.Diagnostics;
|
|
using System.Runtime.InteropServices;
|
|
using Unity.Collections.LowLevel.Unsafe;
|
|
using System.Collections.Generic;
|
|
using System.Reflection;
|
|
using Unity.Burst;
|
|
using Unity.Mathematics;
|
|
|
|
namespace UnityEngine.U2D.Common.UTess
|
|
{
|
|
|
|
////////////////////////////////////////////////////////////////
|
|
/// Convex Hull Generation
|
|
////////////////////////////////////////////////////////////////
|
|
internal struct ConvexHull2D
|
|
{
|
|
|
|
private static readonly float kEpsilon = 0.00001f;
|
|
|
|
struct F3Compare : IComparer<float3>
|
|
{
|
|
public int Compare(float3 x, float3 y)
|
|
{
|
|
if (x.x != y.x)
|
|
return (x.x < y.x) ? -1 : 1;
|
|
return 0;
|
|
}
|
|
}
|
|
|
|
// Distance of Point from Line
|
|
static float DistancePointToLine(float2 pq, float2 p0, float2 p1)
|
|
{
|
|
float2 v = p1 - p0;
|
|
float2 w = pq - p0;
|
|
|
|
float a = math.dot(w, v);
|
|
if (a <= 0)
|
|
return math.length(p0 - pq);
|
|
|
|
float b = math.dot(v, v);
|
|
if (b <= a)
|
|
return math.length(p1 - pq);
|
|
|
|
float c = a / b;
|
|
float2 p = p0 + v * c;
|
|
return math.length(p - pq);
|
|
}
|
|
|
|
static float Sign(float2 p1, float2 p2, float2 p3)
|
|
{
|
|
return (p1.x - p3.x) * (p2.y - p3.y) - (p2.x - p3.x) * (p1.y - p3.y);
|
|
}
|
|
|
|
static bool PointInTriangle(float2 pt, float2 v1, float2 v2, float2 v3)
|
|
{
|
|
float d1, d2, d3;
|
|
bool hasNeg, hasPos;
|
|
|
|
d1 = Sign(pt, v1, v2);
|
|
d2 = Sign(pt, v2, v3);
|
|
d3 = Sign(pt, v3, v1);
|
|
|
|
hasNeg = (d1 < 0) || (d2 < 0) || (d3 < 0);
|
|
hasPos = (d1 > 0) || (d2 > 0) || (d3 > 0);
|
|
|
|
return !(hasNeg && hasPos);
|
|
}
|
|
|
|
// Fetch Points Outside Triangle
|
|
static void FetchPointsOutsideTriangle(ref NativeArray<float2> input, int inputCount, ref NativeArray<float2> output, ref int outputCount, float2 lp, float2 p, float2 rp)
|
|
{
|
|
for (int i = 0; i < inputCount; ++i)
|
|
{
|
|
bool pointInTri = PointInTriangle(input[i], lp, p, rp);
|
|
if (pointInTri)
|
|
continue;
|
|
output[outputCount++] = input[i];
|
|
}
|
|
}
|
|
|
|
// Get Points to Right of Line.
|
|
static void FetchPointsOnRight(ref NativeArray<float2> input, int inputCount, ref NativeArray<float2> output, ref int outputCount, float2 l, float2 r)
|
|
{
|
|
float2 v1 = r - l;
|
|
for (int index = 0; index < inputCount; ++index)
|
|
{
|
|
float2 v2 = r - input[index];
|
|
float xp = v1.x * v2.y - v1.y * v2.x;
|
|
if (xp > 0)
|
|
{
|
|
output[outputCount++] = input[index];
|
|
}
|
|
}
|
|
}
|
|
|
|
// Get Points to the right & left of line
|
|
static unsafe void FetchPoints(float2* input, int inputCount, ref NativeArray<float2> lp, ref int lpCount, ref NativeArray<float2> rp, ref int rpCount, float2 l, float2 r)
|
|
{
|
|
float2 v1 = r - l;
|
|
for (int index = 0; index < inputCount; ++index)
|
|
{
|
|
float2 vx = input[index];
|
|
float2 v2 = r - vx;
|
|
float xp = v1.x * v2.y - v1.y * v2.x;
|
|
if (xp > 0)
|
|
{
|
|
lp[lpCount++] = vx;
|
|
}
|
|
if (xp < 0)
|
|
{
|
|
rp[rpCount++] = vx;
|
|
}
|
|
}
|
|
}
|
|
|
|
// Generate Hull
|
|
static void Generate(ref NativeArray<float2> output, ref int outputCount, ref NativeArray<float2> input, int inputCount, float2 l, float2 r)
|
|
{
|
|
|
|
float2 lp = new float2(l.x, l.y);
|
|
float2 rp = new float2(r.x, r.y);
|
|
float2 pt = new float2(0, 0);
|
|
|
|
// Calculate Pivot.
|
|
float c = 0.00001f;
|
|
float d = c, f = c;
|
|
for (int i = 0; i < inputCount; ++i)
|
|
{
|
|
d = DistancePointToLine(input[i], lp, rp);
|
|
if (d > f)
|
|
{
|
|
pt = input[i];
|
|
f = d;
|
|
}
|
|
}
|
|
|
|
// If we found a Pivot, fetch points outside the Triangle formed by the PivotPoint and Recurse.
|
|
if (f != c)
|
|
{
|
|
// Add to List of Outputs.
|
|
output[outputCount++] = pt;
|
|
|
|
// Get points outside of Triangle.
|
|
int pointCount = 0;
|
|
var pointsOutsideTriangle = new NativeArray<float2>(inputCount, Allocator.Temp);
|
|
FetchPointsOutsideTriangle(ref input, inputCount, ref pointsOutsideTriangle, ref pointCount, l, pt, r);
|
|
|
|
// Get Points Right of l to pt.
|
|
int lpCount = 0;
|
|
var lpPoints = new NativeArray<float2>(pointCount, Allocator.Temp);
|
|
FetchPointsOnRight(ref pointsOutsideTriangle, pointCount, ref lpPoints, ref lpCount, l, pt);
|
|
if (lpCount != 0)
|
|
Generate(ref output, ref outputCount, ref lpPoints, lpCount, l, pt);
|
|
|
|
int rpCount = 0;
|
|
var rpPoints = new NativeArray<float2>(pointCount, Allocator.Temp);
|
|
FetchPointsOnRight(ref pointsOutsideTriangle, pointCount, ref rpPoints, ref rpCount, pt, r);
|
|
if (rpCount != 0)
|
|
Generate(ref output, ref outputCount, ref rpPoints, rpCount, pt, r);
|
|
|
|
// Dispose
|
|
rpPoints.Dispose();
|
|
lpPoints.Dispose();
|
|
pointsOutsideTriangle.Dispose();
|
|
}
|
|
|
|
}
|
|
|
|
static unsafe int CheckSide(float2* convex, int start, int end, float2 p, float2 d)
|
|
{
|
|
int pos = 0, neg = 0;
|
|
|
|
for (int i = start; i < end; i++)
|
|
{
|
|
var nm = convex[i] - p;
|
|
var dt = math.dot(d, nm);
|
|
pos = (dt > 0) ? (pos + 1) : (pos);
|
|
neg = (dt < 0) ? (neg + 1) : (neg);
|
|
if (0 != pos && 0 != neg)
|
|
{
|
|
return 0;
|
|
}
|
|
}
|
|
|
|
// Both Sides.
|
|
return (pos > 0) ? 1 : -1;
|
|
}
|
|
|
|
// Seperating Axis.
|
|
public static bool CheckCollisionSeparatingAxis(ref NativeArray<float2> convex1_, int start1, int end1, ref NativeArray<float2> convex2_, int start2, int end2)
|
|
{
|
|
unsafe
|
|
{
|
|
var convex1 = (float2*)convex1_.GetUnsafeReadOnlyPtr();
|
|
var convex2 = (float2*)convex2_.GetUnsafeReadOnlyPtr();
|
|
|
|
for (int i = start1, j = end1 - 1; i < end1; j = i++)
|
|
{
|
|
var p = convex1[i];
|
|
var d = convex1[i] - convex1[j];
|
|
d = new float2(d.y, -d.x);
|
|
if (CheckSide(convex2, start2, end2, p, d) > 0)
|
|
return false;
|
|
}
|
|
|
|
for (int i = start2, j = end2 - 1; i < end2; j = i++)
|
|
{
|
|
var p = convex2[i];
|
|
var d = convex2[i] - convex2[j];
|
|
d = new float2(d.y, -d.x);
|
|
if (CheckSide(convex1, start1, end1, p, d) > 0)
|
|
return false;
|
|
}
|
|
}
|
|
return true;
|
|
}
|
|
|
|
internal static bool LineLineIntersection(float2 p1, float2 p2, float2 p3, float2 p4, ref float2 result)
|
|
{
|
|
float bx = p2.x - p1.x;
|
|
float by = p2.y - p1.y;
|
|
float dx = p4.x - p3.x;
|
|
float dy = p4.y - p3.y;
|
|
float bDotDPerp = bx * dy - by * dx;
|
|
if (math.abs(bDotDPerp) < kEpsilon)
|
|
{
|
|
return false;
|
|
}
|
|
|
|
float cx = p3.x - p1.x;
|
|
float cy = p3.y - p1.y;
|
|
float t = (cx * dy - cy * dx) / bDotDPerp;
|
|
if ((t >= -kEpsilon) && (t <= 1.0f + kEpsilon))
|
|
{
|
|
result.x = p1.x + t * bx;
|
|
result.y = p1.y + t * by;
|
|
return true;
|
|
}
|
|
return false;
|
|
}
|
|
|
|
// Convex HUll Generator.
|
|
public static unsafe float3 Generate(ref NativeArray<float2> result, ref float4 aabb, ref int pointCount, int seed, Vector2* vertexInput, int vertexCount, float extrude)
|
|
{
|
|
|
|
float2* vertices = (float2*)vertexInput;
|
|
float2* convex = (float2*)result.GetUnsafePtr();
|
|
float2 leftMost, rightMost, topMost, bottomMost, center = float2.zero;
|
|
float lx = Single.MaxValue, ly = Single.MaxValue;
|
|
float3 area = float3.zero;
|
|
leftMost.x = bottomMost.y = Single.MaxValue;
|
|
rightMost.x = topMost.y = Single.MinValue;
|
|
leftMost.y = rightMost.y = topMost.x = bottomMost.x = 0;
|
|
|
|
// Temporary Array for Calc.
|
|
int outputCount = 0;
|
|
var output = new NativeArray<float2>(vertexCount, Allocator.Temp, NativeArrayOptions.UninitializedMemory);
|
|
|
|
// Find the Left and Right EXtremes.
|
|
for (int i = 0; i < vertexCount; ++i)
|
|
{
|
|
leftMost = (leftMost.x > vertices[i].x) ? vertices[i] : leftMost;
|
|
rightMost = (rightMost.x < vertices[i].x) ? vertices[i] : rightMost;
|
|
bottomMost = (bottomMost.y > vertices[i].y) ? vertices[i] : bottomMost;
|
|
topMost = (topMost.y > vertices[i].y) ? vertices[i] : topMost;
|
|
}
|
|
|
|
// Add the Extreme to Output.
|
|
output[outputCount++] = leftMost;
|
|
output[outputCount++] = rightMost;
|
|
|
|
// Get Points for the Edges
|
|
int lpCount = 0, rpCount = 0;
|
|
var lp = new NativeArray<float2>(vertexCount, Allocator.Temp, NativeArrayOptions.UninitializedMemory);
|
|
var rp = new NativeArray<float2>(vertexCount, Allocator.Temp, NativeArrayOptions.UninitializedMemory);
|
|
FetchPoints(vertices, vertexCount, ref lp, ref lpCount, ref rp, ref rpCount, leftMost, rightMost);
|
|
|
|
// Generate Convex Hull
|
|
if (lpCount != 0)
|
|
Generate(ref output, ref outputCount, ref lp, lpCount, leftMost, rightMost);
|
|
if (rpCount != 0)
|
|
Generate(ref output, ref outputCount, ref rp, rpCount, rightMost, leftMost);
|
|
|
|
if (outputCount >= 3)
|
|
{
|
|
// Output. First two points are the pivot points.
|
|
var sortedData = new NativeArray<float3>(outputCount, Allocator.Temp, NativeArrayOptions.UninitializedMemory);
|
|
var sorted = (float3*)sortedData.GetUnsafePtr();
|
|
|
|
var v1 = rightMost - leftMost;
|
|
for (int i = 0; i < outputCount; ++i)
|
|
{
|
|
float3 val = output[i].xyx;
|
|
val.z = 0;
|
|
if (i > 1)
|
|
{
|
|
float2 v2 = rightMost - val.xy;
|
|
val.z = v1.x * v2.y - v1.y * v2.x;
|
|
}
|
|
sorted[i] = val;
|
|
}
|
|
|
|
// Sort by X.
|
|
ModuleHandle.InsertionSort<float3, F3Compare>(sorted, 0, outputCount - 1, new F3Compare());
|
|
|
|
// Copy to Convex.
|
|
convex[pointCount] = leftMost;
|
|
center += convex[pointCount++];
|
|
for (int i = 0; i < outputCount; ++i)
|
|
{
|
|
if (sorted[i].z > 0)
|
|
{
|
|
convex[pointCount] = sorted[i].xy;
|
|
center += convex[pointCount++];
|
|
}
|
|
}
|
|
convex[pointCount] = rightMost;
|
|
center += convex[pointCount++];
|
|
for (int i = outputCount - 1; i > 0; --i)
|
|
{
|
|
if (sorted[i].z < 0)
|
|
{
|
|
convex[pointCount] = sorted[i].xy;
|
|
center += convex[pointCount++];
|
|
}
|
|
}
|
|
center = center / pointCount;
|
|
convex[pointCount++] = leftMost;
|
|
|
|
// Move it to Origin and Extrude.
|
|
leftMost.x = bottomMost.y = Single.MaxValue;
|
|
rightMost.x = topMost.y = Single.MinValue;
|
|
leftMost.y = rightMost.y = topMost.x = bottomMost.x = 0;
|
|
|
|
for (int i = 0; i < pointCount; ++i)
|
|
{
|
|
var v = convex[i];
|
|
var d = math.normalizesafe(v - center);
|
|
convex[i] = center + (d * (math.length(v - center) + extrude));
|
|
|
|
leftMost = (leftMost.x > convex[i].x) ? convex[i] : leftMost;
|
|
rightMost = (rightMost.x < convex[i].x) ? convex[i] : rightMost;
|
|
bottomMost = (bottomMost.y > convex[i].y) ? convex[i] : bottomMost;
|
|
topMost = (topMost.y < convex[i].y) ? convex[i] : topMost;
|
|
}
|
|
|
|
lx = leftMost.x;
|
|
ly = bottomMost.y;
|
|
area.x = rightMost.x - leftMost.x;
|
|
area.y = topMost.y - bottomMost.y;
|
|
center = new float2(center.x - lx, center.y - ly);
|
|
|
|
float minx = 9999999.0f, miny = 9999999.0f;
|
|
float maxx = -9999999.0f, maxy = -9999999.0f;
|
|
|
|
for (int i = 0; i < pointCount; ++i)
|
|
{
|
|
var cx = new float2((int)math.floor(convex[i].x - lx), (int)math.floor(convex[i].y - ly));
|
|
convex[i] = cx;
|
|
minx = (cx.x < minx) ? cx.x : minx;
|
|
maxx = (cx.x > maxx) ? cx.x : maxx;
|
|
miny = (cx.y < miny) ? cx.y : miny;
|
|
maxy = (cx.y > maxy) ? cx.y : maxy;
|
|
|
|
if (i != 0)
|
|
area.z += UnityEngine.U2D.Common.UTess.ModuleHandle.TriangleArea(convex[i], center, convex[i - 1]);
|
|
}
|
|
|
|
// Set the Center and Width/Height
|
|
aabb.z = (maxx - minx / 2.0f);
|
|
aabb.w = (maxy - miny / 2.0f);
|
|
aabb.x = minx + aabb.z;
|
|
aabb.y = miny + aabb.w;
|
|
|
|
|
|
// Dispose.
|
|
sortedData.Dispose();
|
|
}
|
|
else
|
|
{
|
|
Debug.Log("[failed to generate convex hull2d]");
|
|
}
|
|
|
|
rp.Dispose();
|
|
lp.Dispose();
|
|
output.Dispose();
|
|
return area;
|
|
}
|
|
|
|
}
|
|
|
|
}
|