Exemple #1
0
    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();
                }
            }
        }
    }
Exemple #2
0
    // 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));
    }
Exemple #3
0
    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);
        }
    }
Exemple #4
0
    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;
                            }
                        }
                    }
                }
            }
        }
    }
Exemple #5
0
    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);
    }
Exemple #6
0
    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();
    }
Exemple #7
0
    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);
        }
    }