Beispiel #1
0
    /** Applies constrained movement from \a startPos to \a endPos.
     * The result is stored in \a clampedPos.
     * Returns the new current node */
    public GraphNode ClampAlongNavmesh(Vector3 startPos, GraphNode _startNode, Vector3 endPos, out Vector3 clampedPos)
    {
        ConvexMeshNode startNode = (ConvexMeshNode)_startNode;

        clampedPos = endPos;

        Stack <ConvexMeshNode> stack  = tmpStack;               // Tiny stack
        List <ConvexMeshNode>  closed = tmpClosed;              // Tiny closed list

        stack.Clear();
        closed.Clear();

        Vector3        bestPos, p;
        float          bestDist = float.PositiveInfinity;
        float          d;
        ConvexMeshNode bestRef = null;
        // Search constraint
        Vector3 searchPos    = (startPos + endPos) / 2;
        float   searchRadius = Mathfx.MagnitudeXZ(startPos, endPos) / 2;

        // Init
        bestPos = startPos;
        stack.Push(startNode);
        closed.Add(startNode);         // Self ref, start maker.

        INavmesh graph = AstarData.GetGraph(startNode) as INavmesh;

        if (graph == null)
        {
            //Debug.LogError ("Null graph, or the graph was no NavMeshGraph");
            return(startNode);
        }

#if ASTARDEBUG
        Debug.DrawLine(startPos, endPos, Color.blue);
#endif

        while (stack.Count > 0)
        {
            // Pop front.
            ConvexMeshNode cur = stack.Pop();

            // If target is inside the cur, stop search.
            if (NavMeshGraph.ContainsPoint(cur, endPos, graph.vertices))
            {
#if ASTARDEBUG
                Debug.DrawRay(endPos, Vector3.up, Color.red);
#endif
                bestRef = cur;
                bestPos = endPos;
                break;
            }
            // Follow edges or keep track of nearest point on blocking edge.
            for (int i = 0, j = 2; i < 3; j = i++)
            {
                int sp = cur.GetVertexIndex(j);
                int sq = cur.GetVertexIndex(i);

                bool           blocking = true;
                ConvexMeshNode conn     = null;

                for (int q = 0; q < cur.connections.Length; q++)
                {
                    conn = cur.connections[q] as ConvexMeshNode;
                    if (conn == null)
                    {
                        continue;
                    }

                    for (int i2 = 0, j2 = 2; i2 < 3; j2 = i2++)
                    {
                        int sp2 = conn.GetVertexIndex(j2);
                        int sq2 = conn.GetVertexIndex(i2);
                        if ((sp2 == sp && sq2 == sq) || (sp2 == sq && sq2 == sp))
                        {
                            blocking = false;
                            break;
                        }
                    }

                    if (!blocking)
                    {
                        break;
                    }
                }

                //Node neiRef = cur->nei[j];

                if (blocking)
                {
                    // Blocked edge, calc distance.
                    p = Mathfx.NearestPointStrictXZ((Vector3)graph.vertices[sp], (Vector3)graph.vertices[sq], endPos);

#if ASTARDEBUG
                    Debug.DrawLine((Vector3)graph.vertices[sp] + Vector3.up * 0.1F, (Vector3)graph.vertices[sq] + Vector3.up * 0.1F, Color.black);
#endif
                    d = Mathfx.MagnitudeXZ(p, endPos);
                    if (d < bestDist)
                    {
                        // Update nearest distance.
                        bestPos  = p;
                        bestDist = d;
                        bestRef  = cur;
                    }
                }
                else
                {
                    // Skip already visited.
                    if (closed.Contains(conn))
                    {
                        continue;
                    }
                    // Store to closed with parent for trace back.
                    closed.Add(conn);
#if ASTARDEBUG
                    Debug.DrawLine((Vector3)cur.position, (Vector3)conn.position, Color.black);
                    Debug.DrawLine((Vector3)graph.vertices[sp] + Vector3.up * 0.1F, (Vector3)graph.vertices[sq] + Vector3.up * 0.1F, Color.blue);
#endif

                    // Non-blocked edge, follow if within search radius.
                    p = Mathfx.NearestPointStrictXZ((Vector3)graph.vertices[sp], (Vector3)graph.vertices[sq], searchPos);

                    d = Mathfx.MagnitudeXZ(p, searchPos);
                    if (d <= searchRadius)
                    {
#if ASTARDEBUG
                        Debug.DrawLine((Vector3)searchPos - Vector3.up * 0.1F, p - Vector3.up * 0.1F, Color.cyan);
#endif
                        stack.Push(conn);
                    }
#if ASTARDEBUG
                    else
                    {
                        Debug.DrawLine((Vector3)searchPos - Vector3.up * 0.1F, p - Vector3.up * 0.1F, Color.red);
                    }
#endif
                }
            }
        }
        // Trace back and store visited curgons.

        /* followVisited(bestRef,visited,closed);
         * // Store best movement position.*/
        clampedPos = bestPos;
        // Return number of visited curs.
        return(bestRef);       //visited.size();
    }