void IsCdt(Navmesh *lct, int constraint = -1, int vertex = -1)
{
var e = lct->GetEdgeEnumerator(true);
while (e.MoveNext())
{
if (e.Current->Constrained)
{
continue;
}
var o = e.Current->Org->Point;
var d = e.Current->Dest->Point;
if (Math.Contains(o, -lct->Extent, lct->Extent) && Math.Contains(d, -lct->Extent, lct->Extent))
{
var on = e.Current->ONext->Dest->Point;
var dn = e.Current->DNext->Org->Point;
if (Math.CircumcircleContains(o, d, on, dn) && Math.Ccw(dn, d, on) && Math.Ccw(on, o, dn))
{
Debug.Log($"delaunay fail, constraint: {constraint}, vertex:{vertex}, from: {o}, to: {d}");
throw new Exception();
}
}
}
}
// todo use Navmesh.GetLocalClearance
static float GetLocalClearance(double2 a, double2 b, double2 c, Edge *exit, bool lhs)
{
var lba = math.lengthsq(a - b);
var lbc = math.lengthsq(c - b);
var clearance = math.sqrt(math.min(lba, lbc));
var constraint = TryGetConstraint(exit, clearance, b, lhs);
if (constraint == null)
{
return((float)clearance);
}
return((float)math.length(Math.ClosestPointOnLineSegment(b, constraint->Org->Point, constraint->Dest->Point) - b));
}
void InitTriangles(Navmesh *lct)
{
var e = lct->GetEdgeEnumerator(true);
while (e.MoveNext())
{
if (!Math.Contains(e.Current->Org->Point, -lct->Extent, lct->Extent))
{
continue;
}
_triangles.TryAdd(e.Current->TriangleId);
_closed.TryAdd((IntPtr)e.Current);
_closed.TryAdd((IntPtr)e.Current->LNext);
_closed.TryAdd((IntPtr)e.Current->LPrev);
}
}
public void GetClosest(float2 pos, float range, NativeList <Result> items, int maxResults)
{
var min = math.max(0, (int2)((pos - range) / _cellSize));
var max = math.min(_gridSize - 1, (int2)((pos + range) / _cellSize));
var rangeSq = Math.Square(range);
for (int y = min.y; y <= max.y; y++)
{
for (int x = min.x; x <= max.x; x++)
{
if (Get(new int2(x, y), out var l))
{
for (int i = 0; i < l.Length; i++)
{
var item = new Result {
Item = l[i], Dist = math.lengthsq(l[i].Position - pos)
};
if (item.Dist < rangeSq)
{
if (items.Length < maxResults)
{
items.Add(item);
}
var i1 = items.Length - 1;
while (i1 != 0 && item.Dist < items[i1 - 1].Dist)
{
items[i1] = items[i1 - 1];
--i1;
}
items[i1] = item;
if (items.Length == maxResults)
{
rangeSq = items[items.Length - 1].Dist;
}
}
}
}
}
}
}
int TestLocalClearance(Navmesh *lct)
{
var clearanceCalculated = 0;
var e = lct->GetEdgeEnumerator(true);
while (e.MoveNext())
{
if (e.Current->Constrained)
{
continue;
}
var edge = e.Current;
var b = edge->Org->Point;
var c = edge->Dest->Point;
if (!Math.Contains(b, -lct->Extent, lct->Extent) || !Math.Contains(c, -lct->Extent, lct->Extent))
{
continue;
}
var entrance = edge->ONext;
if (!entrance->Constrained)
{
var clearance = GetLocalClearance(entrance->Dest->Point, b, c, edge, false);
if (edge->ClearanceRight == -1)
{
edge->ClearanceRight = clearance;
++clearanceCalculated;
}
else if (!Approx(edge->ClearanceRight, clearance, 1e-6f))
{
var a = entrance->Dest->Point;
var was = edge->ClearanceRight;
Debug.Log($"TestLocalClearance failed. a: {a}, b: {b}, c: {c}, rhs, was: {was}, is: {clearance}");
Assert.IsTrue(false);
}
}
entrance = edge->OPrev;
if (!entrance->Constrained)
{
var clearance = GetLocalClearance(entrance->Dest->Point, b, c, edge, true);
if (edge->ClearanceLeft == -1)
{
edge->ClearanceLeft = clearance;
++clearanceCalculated;
}
else if (!Approx(edge->ClearanceLeft, clearance, 1e-6f))
{
var a = entrance->Dest->Point;
var was = edge->ClearanceLeft;
Debug.Log($"TestLocalClearance failed. a: {a}, b: {b}, c: {c}, lhs, was: {was}, is: {clearance}");
Assert.IsTrue(false);
}
}
}
return(clearanceCalculated);
}
void TestTriangleIds(Navmesh *lct)
{
if (_previousTriangles.IsCreated)
{
_previousTriangles.Dispose();
}
_previousTriangles = _triangles.ToNativeArray();
_triangles.Clear();
_closed.Clear();
_removedTriangles.Clear();
var e = lct->GetEdgeEnumerator(true);
while (e.MoveNext())
{
if (!Math.Contains(e.Current->Org->Point, -lct->Extent, lct->Extent))
{
continue;
}
if (_closed.Contains((IntPtr)e.Current))
{
continue;
}
if (e.Current->TriangleId == 0)
{
Debug.Log("unset Triangle Id");
Assert.IsTrue(false);
}
if (!_triangles.TryAdd(e.Current->TriangleId))
{
Debug.Log("duplicate Triangle Id");
Assert.IsTrue(false);
}
if (e.Current->TriangleId != e.Current->LNext->TriangleId || e.Current->TriangleId != e.Current->LPrev->TriangleId)
{
Debug.Log("inconsistent triangle ids");
Assert.IsTrue(false);
}
_closed.TryAdd((IntPtr)e.Current);
_closed.TryAdd((IntPtr)e.Current->LNext);
_closed.TryAdd((IntPtr)e.Current->LPrev);
}
for (int i = 0; i < _previousTriangles.Length; i++)
{
if (!_triangles.Contains(_previousTriangles[i]))
{
_removedTriangles.Add(_previousTriangles[i]);
}
}
var destroyed = new HashSet <int>(64, Allocator.Temp);
var enumerator = lct->DestroyedTriangles.GetEnumerator();
while (enumerator.MoveNext())
{
destroyed.TryAdd(enumerator.Current);
}
for (int i = 0; i < _removedTriangles.Length; i++)
{
var id = _removedTriangles[i];
if (!destroyed.Remove(id))
{
Debug.Log($"Destroyed triangle {id} not reported");
Assert.IsTrue(false);
}
}
var remaining = destroyed.GetEnumerator();
while (remaining.MoveNext())
{
if (_triangles.Contains(remaining.Current))
{
Debug.Log($"Triangle {remaining.Current} incorrectly reported as destroyed");
Assert.IsTrue(false);
}
}
destroyed.Dispose();
}
void Update()
{
var a = A.position.xz();
var b = B.position.xz();
var c = C.position.xz();
var s = S.position.xz();
var g = G.position.xz();
DebugUtil.DrawCircle(a, b, c, Color.black);
DebugUtil.DrawLine(A.position.xz(), B.position.xz(), Color.black);
DebugUtil.DrawLine(B.position.xz(), C.position.xz(), Color.black);
DebugUtil.DrawLine(C.position.xz(), A.position.xz(), Color.black);
DebugUtil.DrawLine(S.position.xz(), G.position.xz());
DebugUtil.DrawCircle(S.position.xz(), R);
DebugUtil.DrawCircle(G.position.xz(), R);
foreach (var edge in GetEdges())
{
DebugUtil.DrawLine(edge.Item1, edge.Item2, Color.red);
}
FindPath();
void FindPath()
{
var sg = g - s;
var perp = Math.PerpCcw(sg);
var pd = math.normalize(perp) * 2 * R;
var sp = s + perp;
var gp = g + perp;
double2 bl = default;
double2 tl = default;
double2 br = default;
double2 tr = default;
var topFound = false;
var bottomFound = false;
CheckVertex(a);
CheckVertex(b);
CheckVertex(c);
if (!bottomFound)
{
bl = IntersectTri(false, s, sp);
br = IntersectTri(false, g, gp);
}
if (!topFound)
{
tl = IntersectTri(true, s, sp);
tr = IntersectTri(true, g, gp);
}
void CheckVertex(double2 v)
{
if (GeometricPredicates.Orient2DFast(s, sp, v) <= 0 && GeometricPredicates.Orient2DFast(g, gp, v) >= 0)
{
if (GeometricPredicates.Orient2DFast(s, g, v) > 0)
{
tl = Math.ProjectLine(s, sp, v + pd);
tr = Math.ProjectLine(g, gp, v + pd);
topFound = true;
}
else
{
bl = Math.ProjectLine(s, sp, v - pd);
br = Math.ProjectLine(g, gp, v - pd);
bottomFound = true;
}
}
}
double2 IntersectTri(bool up, double2 l0, double2 l1)
{
if (IntersectLineSeg(l0, l1, a, b, out var r))
{
var orient = GeometricPredicates.Orient2DFast(s, g, r);
if ((up ? orient > 0 : orient < 0) || orient == 0 && (up ? math.dot(sg, b - a) < 0 : math.dot(sg, b - a) > 0))
{
return(up ? r + pd : r - pd);
}
}
if (IntersectLineSeg(l0, l1, b, c, out r))
{
var orient = GeometricPredicates.Orient2DFast(s, g, r);
if ((up ? orient > 0 : orient < 0) || orient == 0 && (up ? math.dot(sg, c - b) < 0 : math.dot(sg, c - b) > 0))
{
return(up ? r + pd : r - pd);
}
}
if (IntersectLineSeg(l0, l1, c, a, out r))
{
var orient = GeometricPredicates.Orient2DFast(s, g, r);
if ((up ? orient > 0 : orient < 0) || orient == 0 && (up ? math.dot(sg, a - c) < 0 : math.dot(sg, a - c) > 0))
{
return(up ? r + pd : r - pd);
}
}
throw new BreakDebuggerException();
}
// DebugUtil.DrawLine(bl, br);
// DebugUtil.DrawLine(br, tr);
// DebugUtil.DrawLine(tr, tl);
// DebugUtil.DrawLine(tl, bl);
}
}
