예제 #1
0
파일: NavMesh.cs 프로젝트: wpszz/WPNavMesh
        public float FindClosestEdge2D(Vector2 pos, out Vector2 hit)
        {
            hit = new Vector2();
            int      polyCount = polyNodes.Length;
            float    min       = float.MaxValue;
            PolyNode node      = null;
            float    tmpMin;
            PolyNode tmpNode;

            for (int i = 0; i < polyCount; i++)
            {
                tmpNode = polyNodes[i];
                tmpMin  = Vector2.Distance(pos, tmpNode.center);
                if (tmpMin < min)
                {
                    min  = tmpMin;
                    node = tmpNode;
                }
            }

            min = float.MaxValue;
            int     vertCount = node.vertexs.Length;
            Vector2 tmpHit;
            Vector2 tmpDir;

            for (int i = 0; i < vertCount; i++)
            {
                int j = i + 1;
                if (j >= vertCount)
                {
                    j = 0;
                }

                if (Math.CalculateSegmentIntersect2D(pos, node.center, node.vertexs[i], node.vertexs[j], out tmpHit))
                {
                    tmpMin = Vector2.Distance(node.center, tmpHit);
                    if (tmpMin <= float.Epsilon)
                    {
                        // overlap with center
                        min = 0f;
                        break;
                    }

                    // move to center slightly
                    tmpDir  = node.center - tmpHit;
                    tmpDir /= tmpMin;
                    tmpHit += tmpDir * 0.001f;

                    if (Math.IsInsideConvexPoly(tmpHit, node.vertexs))
                    {
                        tmpMin = Vector2.Distance(pos, tmpHit);
                        if (tmpMin < min)
                        {
                            min = tmpMin;
                            hit = tmpHit;
                        }
                    }
                }
            }

            return(min);
        }
예제 #2
0
        public static bool Serialize(NavMeshBuilder builder, out string json)
        {
            builder.Refresh();

            json = "";

            List <Vector3> allVerts = new List <Vector3>();
            List <int[]>   allPolys = new List <int[]>();

            foreach (var poly in builder.polys)
            {
                int[] indexs = new int[poly.vertices.Count];
                for (int i = 0; i < indexs.Length; i++)
                {
                    indexs[i] = allVerts.Count + i;
                }
                allVerts.AddRange(poly.vertices);
                allPolys.Add(indexs);
            }

            List <Vector3>        newVerts      = new List <Vector3>();
            Dictionary <int, int> indexOldToNew = new Dictionary <int, int>();

            for (int i = 0; i < allVerts.Count; i++)
            {
                int existedIndex = -1;
                for (int j = 0; j < newVerts.Count; j++)
                {
                    if (Vector3.Distance(allVerts[i], newVerts[j]) < 0.01)
                    {
                        existedIndex = j;
                        break;
                    }
                }
                if (existedIndex < 0)
                {
                    indexOldToNew[i] = newVerts.Count;
                    newVerts.Add(allVerts[i]);
                }
                else
                {
                    indexOldToNew[i] = existedIndex;
                }
            }
            foreach (var indexs in allPolys)
            {
                for (int i = 0; i < indexs.Length; i++)
                {
                    indexs[i] = indexOldToNew[indexs[i]];
                }
            }

            int polyCount = allPolys.Count;

            PolyNode[] polyNodes = new PolyNode[polyCount];
            for (int i = 0; i < polyCount; i++)
            {
                var      indexs = allPolys[i];
                PolyNode node   = new PolyNode();
                node.vertexs = new Vector2[indexs.Length];
                for (int j = 0; j < indexs.Length; j++)
                {
                    node.vertexs[j] = new Vector2(newVerts[indexs[j]].x, newVerts[indexs[j]].z);
                }
                node.center  = Math.CalculatePolyCenter2D(node.vertexs);
                polyNodes[i] = node;
                node.index   = i;

                // only support convex ploygons
                if (!Math.IsConvexPoly(node.vertexs))
                {
                    Debug.LogError("Polygon " + builder.polys[i].name + " is not a convex polygon.", builder.polys[i]);
                    return(false);
                }

                // make sure clockwise order
                if (!Math.IsClockwise2D(node.vertexs[0], node.vertexs[1], node.vertexs[2]))
                {
                    System.Array.Reverse(node.vertexs);
                    System.Array.Reverse(indexs);
                }
            }
            // find neighbors
            List <PolyNode> neighbors     = new List <PolyNode>();
            List <int>      neighborEdges = new List <int>();

            for (int i = 0; i < polyCount; i++)
            {
                var indexs = allPolys[i];
                neighbors.Clear();
                neighborEdges.Clear();
                for (int j = 0; j < polyCount; j++)
                {
                    if (i == j)
                    {
                        continue;
                    }
                    var indexs2      = allPolys[j];
                    int neighborEdge = -1;
                    for (int m = 0; m < indexs.Length; m++)
                    {
                        int e1 = indexs[m];
                        int e2 = indexs[m + 1 >= indexs.Length ? 0 : m + 1];
                        for (int n = 0; n < indexs2.Length; n++)
                        {
                            int e3 = indexs2[n];
                            int e4 = indexs2[n + 1 >= indexs2.Length ? 0 : n + 1];
                            if (e1 == e3 && e2 == e4 || e1 == e4 && e2 == e3)
                            {
                                neighborEdge = m;
                                break;
                            }
                        }
                        if (neighborEdge >= 0)
                        {
                            break;
                        }
                    }
                    if (neighborEdge >= 0)
                    {
                        neighbors.Add(polyNodes[j]);
                        neighborEdges.Add(neighborEdge);
                    }
                }
                polyNodes[i].neighbors     = neighbors.ToArray();
                polyNodes[i].neighborEdges = neighborEdges.ToArray();
            }

            NavMesh navMesh = new NavMesh();

            navMesh.polyNodes = polyNodes;

            json = NavMesh.Serialize(navMesh);
            Debug.Log(json);

            return(true);
        }
예제 #3
0
        public static void FindPath2D(Vector2 startPos2D, Vector2 endPos2D, PolyNode nodeAStar, List <Vector2> path)
        {
            path.Clear();

            PolyNode origin = nodeAStar;
            PolyNode prev = origin;
            PolyNode current = prev.parent;
            PolyNode next = null;
            PolyNode nodeP1 = current;
            PolyNode nodeP2 = current;
            Vector2  p0 = endPos2D;
            Vector2  p1 = Vector2.zero;
            Vector2  p2 = Vector2.zero;
            Vector2  p3 = Vector2.zero;
            Vector2  p4 = Vector2.zero;
            int      e1, e2, e3, e4;
            bool     flag1, flag2, flag3, flag4;
            bool     findCorner = false;

            if (current != null)
            {
                e2 = current.neighborEdges[prev.parentEdge];
                e1 = e2 + 1 >= current.vertexs.Length ? 0 : e2 + 1;
                p1 = current.vertexs[e1];
                p2 = current.vertexs[e2];

                while (true)
                {
                    next = current.parent;
                    if (next == null)
                    {
                        flag1 = Math.IsClockwiseMargin2D(p0, p1, startPos2D);
                        flag2 = Math.IsClockwiseMargin2D(p0, startPos2D, p2);

                        if (flag1 && flag2)
                        {
                            // startPos2D is inside of the ∠p0_p1_p2
                            break;
                        }

                        if (!flag1)
                        {
                            // p1 is the next corner
                            path.Add(p0);
                            p0     = p1;
                            origin = nodeP1;
                        }
                        else if (!flag2)
                        {
                            // p2 is the next corner
                            path.Add(p0);
                            p0     = p2;
                            origin = nodeP2;
                        }

                        //reset vector checking from current node
                        prev    = origin;
                        current = prev.parent;
                        if (current == null)
                        {
                            // origin is the last node
                            break;
                        }
                        nodeP1 = current;
                        nodeP2 = current;
                        e2     = current.neighborEdges[prev.parentEdge];
                        e1     = e2 + 1 >= current.vertexs.Length ? 0 : e2 + 1;
                        p1     = current.vertexs[e1];
                        p2     = current.vertexs[e2];
                        continue;
                    }

                    // compare next points in the edge
                    e4 = next.neighborEdges[current.parentEdge];
                    e3 = e4 + 1 >= next.vertexs.Length ? 0 : e4 + 1;
                    p3 = next.vertexs[e3];
                    p4 = next.vertexs[e4];

                    flag1 = Math.IsClockwiseMargin2D(p0, p1, p3);
                    flag2 = Math.IsClockwiseMargin2D(p0, p3, p2);
                    flag3 = Math.IsClockwiseMargin2D(p0, p1, p4);
                    flag4 = Math.IsClockwiseMargin2D(p0, p4, p2);

                    if (flag1 && flag2)
                    {
                        // p3 is inside of the ∠p0_p1_p2
                        p1     = p3;
                        nodeP1 = next;
                    }

                    if (flag3 && flag4)
                    {
                        // p4 is inside of the ∠p0_p1_p2
                        p2     = p4;
                        nodeP2 = next;
                    }

                    findCorner = false;

                    if (!flag2 && !flag4 || flag1 && !flag2 && flag3)
                    {
                        // flag1 && !flag2 && flag3 is the especial case
                        // p2 is the next corner
                        path.Add(p0);
                        p0         = p2;
                        origin     = nodeP2;
                        findCorner = true;
                    }
                    else if (!flag1 && !flag3 || flag1 && flag2 && !flag3)
                    {
                        // flag1 && flag2 && !flag3 is the especial case
                        // p1 is the next corner
                        path.Add(p0);
                        p0         = p1;
                        origin     = nodeP1;
                        findCorner = true;
                    }

                    if (findCorner)
                    {
                        // reset vector checking from current node
                        prev    = origin;
                        current = prev.parent;
                        if (current == null)
                        {
                            // origin is the last node
                            break;
                        }
                        nodeP1 = current;
                        nodeP2 = current;
                        e2     = current.neighborEdges[prev.parentEdge];
                        e1     = e2 + 1 >= current.vertexs.Length ? 0 : e2 + 1;
                        p1     = current.vertexs[e1];
                        p2     = current.vertexs[e2];
                    }
                    else
                    {
                        // move to next
                        prev    = current;
                        current = next;
                    }
                }
            }

            path.Add(p0);
            path.Add(startPos2D);

            // reverse because nodeAStar is the end of the node path.
            path.Reverse();
        }
예제 #4
0
        void OnDrawGizmos()
        {
            if (navMesh != null)
            {
                float showHeight = testHeight + offsetHeight;

                Gizmos.color = Color.red;
                for (int i = 0; i < navMesh.polyNodes.Length; i++)
                {
                    PolyNode node = navMesh.polyNodes[i];
                    Vector3  size = Vector3.one * 0.1f;
                    if (testPathNodeEnd != null)
                    {
                        PolyNode tmp = testPathNodeEnd;
                        do
                        {
                            if (tmp == node)
                            {
                                size.y = 3;
                                break;
                            }
                            tmp = tmp.parent;
                        }while (tmp != null);
                    }
                    Gizmos.DrawCube(new Vector3(node.center.x, showHeight, node.center.y), size);
                }

                Gizmos.color = Color.green;
                float   width       = testMax.x - testMin.x;
                float   height      = testMax.y - testMin.y;
                int     row         = splitCount;
                int     column      = splitCount;
                float   deltaColumn = width / column;
                float   deltaRow    = height / row;
                Vector2 p           = Vector2.zero;
                for (int i = 0; i < row; i++)
                {
                    for (int j = 0; j < column; j++)
                    {
                        p.x = testMin.x + deltaColumn * j;
                        p.y = testMin.y + deltaRow * i;
                        if (navMesh.GetInsidePolyIndex(p) >= 0)
                        {
                            Gizmos.DrawRay(new Vector3(p.x, showHeight, p.y), Vector3.up);
                        }
                    }
                }

                if (testPath != null)
                {
                    Gizmos.color = Color.blue;
                    for (int i = 0; i < testPath.Length; i++)
                    {
                        Gizmos.DrawCube(new Vector3(testPath[i].x, showHeight, testPath[i].y), new Vector3(0.2f, 1f, 0.2f));
                        if (i + 1 < testPath.Length)
                        {
                            Gizmos.DrawLine(new Vector3(testPath[i].x, showHeight + 0.1f, testPath[i].y),
                                            new Vector3(testPath[i + 1].x, showHeight + 0.1f, testPath[i + 1].y));
                        }
                    }
                }

                if (testStartClosestDis > 0)
                {
                    Gizmos.color = Color.yellow;
                    Gizmos.DrawCube(new Vector3(testStartClosest.x, showHeight + 0.1f, testStartClosest.y), new Vector3(0.2f, 10, 0.2f));
                }
                if (testEndClosestDis > 0)
                {
                    Gizmos.color = Color.magenta;
                    Gizmos.DrawCube(new Vector3(testEndClosest.x, showHeight + 0.1f, testEndClosest.y), new Vector3(0.2f, 10, 0.2f));
                }
            }
        }