示例#1
0
 public static bool SegmentsIntersectXZ(Int3 start1, Int3 end1, Int3 start2, Int3 end2)
 {
     return(VectorMath.RightOrColinearXZ(start1, end1, start2) != VectorMath.RightOrColinearXZ(start1, end1, end2) && VectorMath.RightOrColinearXZ(start2, end2, start1) != VectorMath.RightOrColinearXZ(start2, end2, end1));
 }
示例#2
0
        bool FindNextCorners(Vector3 origin, int startIndex, List <Vector3> funnelPath, int numCorners, out bool lastCorner)
        {
            lastCorner = false;

            if (left == null)
            {
                throw new System.Exception("left list is null");
            }
            if (right == null)
            {
                throw new System.Exception("right list is null");
            }
            if (funnelPath == null)
            {
                throw new System.ArgumentNullException("funnelPath");
            }

            if (left.Count != right.Count)
            {
                throw new System.ArgumentException("left and right lists must have equal length");
            }

            int diagonalCount = left.Count;

            if (diagonalCount == 0)
            {
                throw new System.ArgumentException("no diagonals");
            }

            if (diagonalCount - startIndex < 3)
            {
                //Direct path
                funnelPath.Add(left[diagonalCount - 1]);
                lastCorner = true;
                return(true);
            }

                        #if ASTARDEBUG
            for (int i = startIndex; i < left.Count - 1; i++)
            {
                Debug.DrawLine(left[i], left[i + 1], Color.red);
                Debug.DrawLine(right[i], right[i + 1], Color.magenta);
                Debug.DrawRay(right[i], Vector3.up, Color.magenta);
            }
            for (int i = 0; i < left.Count; i++)
            {
                Debug.DrawLine(right[i], left[i], Color.cyan);
            }
                        #endif

            //Remove identical vertices
            while (left[startIndex + 1] == left[startIndex + 2] && right[startIndex + 1] == right[startIndex + 2])
            {
                //System.Console.WriteLine ("Removing identical left and right");
                //left.RemoveAt (1);
                //right.RemoveAt (1);
                startIndex++;

                if (diagonalCount - startIndex <= 3)
                {
                    return(false);
                }
            }

            Vector3 swPoint = left[startIndex + 2];
            if (swPoint == left[startIndex + 1])
            {
                swPoint = right[startIndex + 2];
            }


            //Test
            while (VectorMath.IsColinearXZ(origin, left[startIndex + 1], right[startIndex + 1]) || VectorMath.RightOrColinearXZ(left[startIndex + 1], right[startIndex + 1], swPoint) == VectorMath.RightOrColinearXZ(left[startIndex + 1], right[startIndex + 1], origin))
            {
        #if ASTARDEBUG
                Debug.DrawLine(left[startIndex + 1], right[startIndex + 1], new Color(0, 0, 0, 0.5F));
                Debug.DrawLine(origin, swPoint, new Color(0, 0, 0, 0.5F));
        #endif
                //left.RemoveAt (1);
                //right.RemoveAt (1);
                startIndex++;

                if (diagonalCount - startIndex < 3)
                {
                    //Debug.Log ("#2 " + left.Count + " - " + startIndex + " = " + (left.Count-startIndex));
                    //Direct path
                    funnelPath.Add(left[diagonalCount - 1]);
                    lastCorner = true;
                    return(true);
                }

                swPoint = left[startIndex + 2];
                if (swPoint == left[startIndex + 1])
                {
                    swPoint = right[startIndex + 2];
                }
            }


            //funnelPath.Add (origin);

            Vector3 portalApex  = origin;
            Vector3 portalLeft  = left[startIndex + 1];
            Vector3 portalRight = right[startIndex + 1];

            int apexIndex  = startIndex + 0;
            int rightIndex = startIndex + 1;
            int leftIndex  = startIndex + 1;

            for (int i = startIndex + 2; i < diagonalCount; i++)
            {
                if (funnelPath.Count >= numCorners)
                {
                    return(true);
                }

                if (funnelPath.Count > 2000)
                {
                    Debug.LogWarning("Avoiding infinite loop. Remove this check if you have this long paths.");
                    break;
                }

                Vector3 pLeft  = left[i];
                Vector3 pRight = right[i];

                /*Debug.DrawLine (portalApex,portalLeft,Color.red);
                 * Debug.DrawLine (portalApex,portalRight,Color.yellow);
                 * Debug.DrawLine (portalApex,left,Color.cyan);
                 * Debug.DrawLine (portalApex,right,Color.cyan);*/

                if (VectorMath.SignedTriangleAreaTimes2XZ(portalApex, portalRight, pRight) >= 0)
                {
                    if (portalApex == portalRight || VectorMath.SignedTriangleAreaTimes2XZ(portalApex, portalLeft, pRight) <= 0)
                    {
                        portalRight = pRight;
                        rightIndex  = i;
                    }
                    else
                    {
                        funnelPath.Add(portalLeft);
                        portalApex = portalLeft;
                        apexIndex  = leftIndex;

                        portalLeft  = portalApex;
                        portalRight = portalApex;

                        leftIndex  = apexIndex;
                        rightIndex = apexIndex;

                        i = apexIndex;

                        continue;
                    }
                }

                if (VectorMath.SignedTriangleAreaTimes2XZ(portalApex, portalLeft, pLeft) <= 0)
                {
                    if (portalApex == portalLeft || VectorMath.SignedTriangleAreaTimes2XZ(portalApex, portalRight, pLeft) >= 0)
                    {
                        portalLeft = pLeft;
                        leftIndex  = i;
                    }
                    else
                    {
                        funnelPath.Add(portalRight);
                        portalApex = portalRight;
                        apexIndex  = rightIndex;

                        portalLeft  = portalApex;
                        portalRight = portalApex;

                        leftIndex  = apexIndex;
                        rightIndex = apexIndex;

                        i = apexIndex;

                        continue;
                    }
                }
            }

            lastCorner = true;
            funnelPath.Add(left[diagonalCount - 1]);

            return(true);
        }
示例#3
0
        public override Vector3 ClosestPointOnNodeXZ(Vector3 _p)
        {
            // Get the object holding the vertex data for this node
            // This is usually a graph or a recast graph tile
            INavmeshHolder g = GetNavmeshHolder(GraphIndex);

            // Get all 3 vertices for this node
            Int3 tp1 = g.GetVertex(v0);
            Int3 tp2 = g.GetVertex(v1);
            Int3 tp3 = g.GetVertex(v2);

            // We need the point as an Int3
            var p = (Int3)_p;

            // Save the original y coordinate, we will return a point with the same y coordinate
            int oy = p.y;

            // Assumes the triangle vertices are laid out in (counter?)clockwise order

            tp1.y = 0;
            tp2.y = 0;
            tp3.y = 0;
            p.y   = 0;

            if ((long)(tp2.x - tp1.x) * (long)(p.z - tp1.z) - (long)(p.x - tp1.x) * (long)(tp2.z - tp1.z) > 0)
            {
                float f = Mathf.Clamp01(VectorMath.ClosestPointOnLineFactor(tp1, tp2, p));
                return(new Vector3(tp1.x + (tp2.x - tp1.x) * f, oy, tp1.z + (tp2.z - tp1.z) * f) * Int3.PrecisionFactor);
            }
            else if ((long)(tp3.x - tp2.x) * (long)(p.z - tp2.z) - (long)(p.x - tp2.x) * (long)(tp3.z - tp2.z) > 0)
            {
                float f = Mathf.Clamp01(VectorMath.ClosestPointOnLineFactor(tp2, tp3, p));
                return(new Vector3(tp2.x + (tp3.x - tp2.x) * f, oy, tp2.z + (tp3.z - tp2.z) * f) * Int3.PrecisionFactor);
            }
            else if ((long)(tp1.x - tp3.x) * (long)(p.z - tp3.z) - (long)(p.x - tp3.x) * (long)(tp1.z - tp3.z) > 0)
            {
                float f = Mathf.Clamp01(VectorMath.ClosestPointOnLineFactor(tp3, tp1, p));
                return(new Vector3(tp3.x + (tp1.x - tp3.x) * f, oy, tp3.z + (tp1.z - tp3.z) * f) * Int3.PrecisionFactor);
            }
            else
            {
                return(_p);
            }

            /*
             * Equivalent to the above, but the above uses manual inlining
             * if (!VectorMath.RightOrColinearXZ (tp1, tp2, p)) {
             *  float f = Mathf.Clamp01 (VectorMath.ClosestPointOnLineFactor (tp1, tp2, p));
             *  return new Vector3(tp1.x + (tp2.x-tp1.x)*f, oy, tp1.z + (tp2.z-tp1.z)*f)*Int3.PrecisionFactor;
             * } else if (!VectorMath.RightOrColinearXZ (tp2, tp3, p)) {
             *  float f = Mathf.Clamp01 (VectorMath.ClosestPointOnLineFactor (tp2, tp3, p));
             *  return new Vector3(tp2.x + (tp3.x-tp2.x)*f, oy, tp2.z + (tp3.z-tp2.z)*f)*Int3.PrecisionFactor;
             * } else if (!VectorMath.RightOrColinearXZ (tp3, tp1, p)) {
             *  float f = Mathf.Clamp01 (VectorMath.ClosestPointOnLineFactor (tp3, tp1, p));
             *  return new Vector3(tp3.x + (tp1.x-tp3.x)*f, oy, tp3.z + (tp1.z-tp3.z)*f)*Int3.PrecisionFactor;
             * } else {
             *  return _p;
             * }*/

            /* Almost equivalent to the above, but this is slower
             * Vector3 tp1 = (Vector3)g.GetVertex(v0);
             * Vector3 tp2 = (Vector3)g.GetVertex(v1);
             * Vector3 tp3 = (Vector3)g.GetVertex(v2);
             * tp1.y = 0;
             * tp2.y = 0;
             * tp3.y = 0;
             * _p.y = 0;
             * return Pathfinding.Polygon.ClosestPointOnTriangle (tp1,tp2,tp3,_p);*/
        }
示例#4
0
        /** Returns if there is an obstacle between \a origin and \a end on the graph.
         * \param [in] graph The graph to perform the search on
         * \param [in] tmp_origin Point to start from
         * \param [in] tmp_end Point to linecast to
         * \param [out] hit Contains info on what was hit, see GraphHitInfo
         * \param [in] hint You need to pass the node closest to the start point, if null, a search for the closest node will be done
         * \param trace If a list is passed, then it will be filled with all nodes the linecast traverses
         * This is not the same as Physics.Linecast, this function traverses the \b graph and looks for collisions instead of checking for collider intersection.
         * \astarpro */
        public static bool Linecast(INavmesh graph, Vector3 tmp_origin, Vector3 tmp_end, GraphNode hint, out GraphHitInfo hit, List <GraphNode> trace)
        {
            var end    = (Int3)tmp_end;
            var origin = (Int3)tmp_origin;

            hit = new GraphHitInfo();

            if (float.IsNaN(tmp_origin.x + tmp_origin.y + tmp_origin.z))
            {
                throw new System.ArgumentException("origin is NaN");
            }
            if (float.IsNaN(tmp_end.x + tmp_end.y + tmp_end.z))
            {
                throw new System.ArgumentException("end is NaN");
            }

            var node = hint as TriangleMeshNode;

            if (node == null)
            {
                node = (graph as NavGraph).GetNearest(tmp_origin, NNConstraint.None).node as TriangleMeshNode;

                if (node == null)
                {
                    Debug.LogError("Could not find a valid node to start from");
                    hit.point = tmp_origin;
                    return(true);
                }
            }

            if (origin == end)
            {
                hit.node = node;
                return(false);
            }

            origin     = (Int3)node.ClosestPointOnNode((Vector3)origin);
            hit.origin = (Vector3)origin;

            if (!node.Walkable)
            {
                hit.point         = (Vector3)origin;
                hit.tangentOrigin = (Vector3)origin;
                return(true);
            }


            List <Vector3> left = Pathfinding.Util.ListPool <Vector3> .Claim();         //new List<Vector3>(1);

            List <Vector3> right = Pathfinding.Util.ListPool <Vector3> .Claim();        //new List<Vector3>(1);

            int counter = 0;

            while (true)
            {
                counter++;
                if (counter > 2000)
                {
                    Debug.LogError("Linecast was stuck in infinite loop. Breaking.");
                    Pathfinding.Util.ListPool <Vector3> .Release(left);

                    Pathfinding.Util.ListPool <Vector3> .Release(right);

                    return(true);
                }

                TriangleMeshNode newNode = null;

                if (trace != null)
                {
                    trace.Add(node);
                }

                if (node.ContainsPoint(end))
                {
                    Pathfinding.Util.ListPool <Vector3> .Release(left);

                    Pathfinding.Util.ListPool <Vector3> .Release(right);

                    return(false);
                }

                for (int i = 0; i < node.connections.Length; i++)
                {
                    //Nodes on other graphs should not be considered
                    //They might even be of other types (not MeshNode)
                    if (node.connections[i].GraphIndex != node.GraphIndex)
                    {
                        continue;
                    }

                    left.Clear();
                    right.Clear();

                    if (!node.GetPortal(node.connections[i], left, right, false))
                    {
                        continue;
                    }

                    Vector3 a = left[0];
                    Vector3 b = right[0];

                    //i.e Left or colinear
                    if (!VectorMath.RightXZ(a, b, hit.origin))
                    {
                        if (VectorMath.RightXZ(a, b, tmp_end))
                        {
                            //Since polygons are laid out in clockwise order, the ray would intersect (if intersecting) this edge going in to the node, not going out from it
                            continue;
                        }
                    }

                    float factor1, factor2;

                    if (VectorMath.LineIntersectionFactorXZ(a, b, hit.origin, tmp_end, out factor1, out factor2))
                    {
                        //Intersection behind the start
                        if (factor2 < 0)
                        {
                            continue;
                        }

                        if (factor1 >= 0 && factor1 <= 1)
                        {
                            newNode = node.connections[i] as TriangleMeshNode;
                            break;
                        }
                    }
                }

                if (newNode == null)
                {
                    //Possible edge hit
                    int vs = node.GetVertexCount();

                    for (int i = 0; i < vs; i++)
                    {
                        var a = (Vector3)node.GetVertex(i);
                        var b = (Vector3)node.GetVertex((i + 1) % vs);


                        //i.e left or colinear
                        if (!VectorMath.RightXZ(a, b, hit.origin))
                        {
                            //Since polygons are laid out in clockwise order, the ray would intersect (if intersecting) this edge going in to the node, not going out from it
                            if (VectorMath.RightXZ(a, b, tmp_end))
                            {
                                //Since polygons are laid out in clockwise order, the ray would intersect (if intersecting) this edge going in to the node, not going out from it
                                continue;
                            }
                        }

                        float factor1, factor2;
                        if (VectorMath.LineIntersectionFactorXZ(a, b, hit.origin, tmp_end, out factor1, out factor2))
                        {
                            if (factor2 < 0)
                            {
                                continue;
                            }

                            if (factor1 >= 0 && factor1 <= 1)
                            {
                                Vector3 intersectionPoint = a + (b - a) * factor1;
                                hit.point         = intersectionPoint;
                                hit.node          = node;
                                hit.tangent       = b - a;
                                hit.tangentOrigin = a;

                                Pathfinding.Util.ListPool <Vector3> .Release(left);

                                Pathfinding.Util.ListPool <Vector3> .Release(right);

                                return(true);
                            }
                        }
                    }

                    //Ok, this is wrong...
                    Debug.LogWarning("Linecast failing because point not inside node, and line does not hit any edges of it");

                    Pathfinding.Util.ListPool <Vector3> .Release(left);

                    Pathfinding.Util.ListPool <Vector3> .Release(right);

                    return(false);
                }

                node = newNode;
            }
        }
示例#5
0
        /** Generates a navmesh. Based on the supplied vertices and triangles */
        void GenerateNodes(Vector3[] vectorVertices, int[] triangles, out Vector3[] originalVertices, out Int3[] vertices)
        {
            UnityEngine.Profiling.Profiler.BeginSample("Init");

            if (vectorVertices.Length == 0 || triangles.Length == 0)
            {
                originalVertices = vectorVertices;
                vertices         = new Int3[0];
                nodes            = new TriangleMeshNode[0];
                return;
            }

            vertices = new Int3[vectorVertices.Length];

            int c = 0;

            for (int i = 0; i < vertices.Length; i++)
            {
                vertices[i] = (Int3)matrix.MultiplyPoint3x4(vectorVertices[i]);
            }

            var hashedVerts = new Dictionary <Int3, int>();

            var newVertices = new int[vertices.Length];

            UnityEngine.Profiling.Profiler.EndSample();
            UnityEngine.Profiling.Profiler.BeginSample("Hashing");

            for (int i = 0; i < vertices.Length; i++)
            {
                if (!hashedVerts.ContainsKey(vertices[i]))
                {
                    newVertices[c] = i;
                    hashedVerts.Add(vertices[i], c);
                    c++;
                }
            }

            for (int x = 0; x < triangles.Length; x++)
            {
                Int3 vertex = vertices[triangles[x]];

                triangles[x] = hashedVerts[vertex];
            }

            Int3[] totalIntVertices = vertices;
            vertices         = new Int3[c];
            originalVertices = new Vector3[c];
            for (int i = 0; i < c; i++)
            {
                vertices[i]         = totalIntVertices[newVertices[i]];
                originalVertices[i] = vectorVertices[newVertices[i]];
            }

            UnityEngine.Profiling.Profiler.EndSample();
            UnityEngine.Profiling.Profiler.BeginSample("Constructing Nodes");

            nodes = new TriangleMeshNode[triangles.Length / 3];

            int graphIndex = active.astarData.GetGraphIndex(this);

            // Does not have to set this, it is set in ScanInternal
            //TriangleMeshNode.SetNavmeshHolder ((int)graphIndex,this);

            for (int i = 0; i < nodes.Length; i++)
            {
                nodes[i] = new TriangleMeshNode(active);
                TriangleMeshNode node = nodes[i];                //new MeshNode ();

                node.GraphIndex = (uint)graphIndex;
                node.Penalty    = initialPenalty;
                node.Walkable   = true;


                node.v0 = triangles[i * 3];
                node.v1 = triangles[i * 3 + 1];
                node.v2 = triangles[i * 3 + 2];

                if (!VectorMath.IsClockwiseXZ(vertices[node.v0], vertices[node.v1], vertices[node.v2]))
                {
                    //Debug.DrawLine (vertices[node.v0],vertices[node.v1],Color.red);
                    //Debug.DrawLine (vertices[node.v1],vertices[node.v2],Color.red);
                    //Debug.DrawLine (vertices[node.v2],vertices[node.v0],Color.red);

                    int tmp = node.v0;
                    node.v0 = node.v2;
                    node.v2 = tmp;
                }

                if (VectorMath.IsColinearXZ(vertices[node.v0], vertices[node.v1], vertices[node.v2]))
                {
                    Debug.DrawLine((Vector3)vertices[node.v0], (Vector3)vertices[node.v1], Color.red);
                    Debug.DrawLine((Vector3)vertices[node.v1], (Vector3)vertices[node.v2], Color.red);
                    Debug.DrawLine((Vector3)vertices[node.v2], (Vector3)vertices[node.v0], Color.red);
                }

                // Make sure position is correctly set
                node.UpdatePositionFromVertices();
            }

            UnityEngine.Profiling.Profiler.EndSample();

            var sides = new Dictionary <Int2, TriangleMeshNode>();

            for (int i = 0, j = 0; i < triangles.Length; j += 1, i += 3)
            {
                sides[new Int2(triangles[i + 0], triangles[i + 1])] = nodes[j];
                sides[new Int2(triangles[i + 1], triangles[i + 2])] = nodes[j];
                sides[new Int2(triangles[i + 2], triangles[i + 0])] = nodes[j];
            }

            UnityEngine.Profiling.Profiler.BeginSample("Connecting Nodes");

            var connections     = new List <MeshNode>();
            var connectionCosts = new List <uint>();

            for (int i = 0, j = 0; i < triangles.Length; j += 1, i += 3)
            {
                connections.Clear();
                connectionCosts.Clear();

                TriangleMeshNode node = nodes[j];

                for (int q = 0; q < 3; q++)
                {
                    TriangleMeshNode other;
                    if (sides.TryGetValue(new Int2(triangles[i + ((q + 1) % 3)], triangles[i + q]), out other))
                    {
                        connections.Add(other);
                        connectionCosts.Add((uint)(node.position - other.position).costMagnitude);
                    }
                }

                node.connections     = connections.ToArray();
                node.connectionCosts = connectionCosts.ToArray();
            }

            UnityEngine.Profiling.Profiler.EndSample();
            UnityEngine.Profiling.Profiler.BeginSample("Rebuilding BBTree");

            RebuildBBTree(this);

            UnityEngine.Profiling.Profiler.EndSample();

#if ASTARDEBUG
            for (int i = 0; i < nodes.Length; i++)
            {
                TriangleMeshNode node = nodes[i] as TriangleMeshNode;

                float a1 = VectorMath.SignedTriangleAreaTimes2XZ((Vector3)vertices[node.v0], (Vector3)vertices[node.v1], (Vector3)vertices[node.v2]);

                long a2 = VectorMath.SignedTriangleAreaTimes2XZ(vertices[node.v0], vertices[node.v1], vertices[node.v2]);
                if (a1 * a2 < 0)
                {
                    Debug.LogError(a1 + " " + a2);
                }


                if (VectorMath.IsClockwiseXZ(vertices[node.v0], vertices[node.v1], vertices[node.v2]))
                {
                    Debug.DrawLine((Vector3)vertices[node.v0], (Vector3)vertices[node.v1], Color.green);
                    Debug.DrawLine((Vector3)vertices[node.v1], (Vector3)vertices[node.v2], Color.green);
                    Debug.DrawLine((Vector3)vertices[node.v2], (Vector3)vertices[node.v0], Color.green);
                }
                else
                {
                    Debug.DrawLine((Vector3)vertices[node.v0], (Vector3)vertices[node.v1], Color.red);
                    Debug.DrawLine((Vector3)vertices[node.v1], (Vector3)vertices[node.v2], Color.red);
                    Debug.DrawLine((Vector3)vertices[node.v2], (Vector3)vertices[node.v0], Color.red);
                }
            }
#endif
        }
        // Token: 0x0600251A RID: 9498 RVA: 0x0019D24C File Offset: 0x0019B44C
        public static void UpdateArea(GraphUpdateObject o, INavmeshHolder graph)
        {
            Bounds  bounds = graph.transform.InverseTransform(o.bounds);
            IntRect irect  = new IntRect(Mathf.FloorToInt(bounds.min.x * 1000f), Mathf.FloorToInt(bounds.min.z * 1000f), Mathf.CeilToInt(bounds.max.x * 1000f), Mathf.CeilToInt(bounds.max.z * 1000f));
            Int3    a      = new Int3(irect.xmin, 0, irect.ymin);
            Int3    b      = new Int3(irect.xmin, 0, irect.ymax);
            Int3    c      = new Int3(irect.xmax, 0, irect.ymin);
            Int3    d      = new Int3(irect.xmax, 0, irect.ymax);
            int     ymin   = ((Int3)bounds.min).y;
            int     ymax   = ((Int3)bounds.max).y;

            graph.GetNodes(delegate(GraphNode _node)
            {
                TriangleMeshNode triangleMeshNode = _node as TriangleMeshNode;
                bool flag = false;
                int num   = 0;
                int num2  = 0;
                int num3  = 0;
                int num4  = 0;
                for (int i = 0; i < 3; i++)
                {
                    Int3 vertexInGraphSpace = triangleMeshNode.GetVertexInGraphSpace(i);
                    if (irect.Contains(vertexInGraphSpace.x, vertexInGraphSpace.z))
                    {
                        flag = true;
                        break;
                    }
                    if (vertexInGraphSpace.x < irect.xmin)
                    {
                        num++;
                    }
                    if (vertexInGraphSpace.x > irect.xmax)
                    {
                        num2++;
                    }
                    if (vertexInGraphSpace.z < irect.ymin)
                    {
                        num3++;
                    }
                    if (vertexInGraphSpace.z > irect.ymax)
                    {
                        num4++;
                    }
                }
                if (!flag && (num == 3 || num2 == 3 || num3 == 3 || num4 == 3))
                {
                    return;
                }
                for (int j = 0; j < 3; j++)
                {
                    int i2 = (j > 1) ? 0 : (j + 1);
                    Int3 vertexInGraphSpace2 = triangleMeshNode.GetVertexInGraphSpace(j);
                    Int3 vertexInGraphSpace3 = triangleMeshNode.GetVertexInGraphSpace(i2);
                    if (VectorMath.SegmentsIntersectXZ(a, b, vertexInGraphSpace2, vertexInGraphSpace3))
                    {
                        flag = true;
                        break;
                    }
                    if (VectorMath.SegmentsIntersectXZ(a, c, vertexInGraphSpace2, vertexInGraphSpace3))
                    {
                        flag = true;
                        break;
                    }
                    if (VectorMath.SegmentsIntersectXZ(c, d, vertexInGraphSpace2, vertexInGraphSpace3))
                    {
                        flag = true;
                        break;
                    }
                    if (VectorMath.SegmentsIntersectXZ(d, b, vertexInGraphSpace2, vertexInGraphSpace3))
                    {
                        flag = true;
                        break;
                    }
                }
                if (flag || triangleMeshNode.ContainsPointInGraphSpace(a) || triangleMeshNode.ContainsPointInGraphSpace(b) || triangleMeshNode.ContainsPointInGraphSpace(c) || triangleMeshNode.ContainsPointInGraphSpace(d))
                {
                    flag = true;
                }
                if (!flag)
                {
                    return;
                }
                int num5 = 0;
                int num6 = 0;
                for (int k = 0; k < 3; k++)
                {
                    Int3 vertexInGraphSpace4 = triangleMeshNode.GetVertexInGraphSpace(k);
                    if (vertexInGraphSpace4.y < ymin)
                    {
                        num6++;
                    }
                    if (vertexInGraphSpace4.y > ymax)
                    {
                        num5++;
                    }
                }
                if (num6 == 3 || num5 == 3)
                {
                    return;
                }
                o.WillUpdateNode(triangleMeshNode);
                o.Apply(triangleMeshNode);
            });
        }
示例#7
0
        // Update is called once per frame
        void LateUpdate()
        {
            if (prevNode == null)
            {
                //Good Game
                //var nninfo = AstarPath.active.GetNearest(transform.position);
                var nninfo = AstarPath.active.GetNearest((VInt3)transform.position);
                prevNode = nninfo.node;
                //Good Game
                //prevPos = transform.position;
                prevPos = (VInt3)transform.position;
            }

            if (prevNode == null)
            {
                return;
            }

            if (prevNode != null)
            {
                var graph = AstarData.GetGraph(prevNode) as IRaycastableGraph;
                if (graph != null)
                {
                    GraphHitInfo hit;
                    //Good Game
                    //if (graph.Linecast(prevPos, transform.position, prevNode, out hit)) {
                    if (graph.Linecast(prevPos, (VInt3)transform.position, prevNode, out hit))
                    {
                        //Good Game
                        //hit.point.y = transform.position.y;
                        hit.point.y = (int)transform.position.y;
                        //Good Game
                        //Vector3 closest = VectorMath.ClosestPointOnLine(hit.tangentOrigin, hit.tangentOrigin+hit.tangent, transform.position);
                        Vector3 closest = VectorMath.ClosestPointOnLine((Vector3)hit.tangentOrigin, (Vector3)(hit.tangentOrigin + hit.tangent), transform.position);
                        //Good Game
                        //Vector3 ohit = hit.point;
                        Vector3 ohit = (Vector3)hit.point;
                        ohit = ohit + Vector3.ClampMagnitude((Vector3)hit.node.position - ohit, 0.008f);
                        //Good Game

                        /*if (graph.Linecast(ohit, closest, hit.node, out hit)) {
                         *                              hit.point.y = transform.position.y;
                         *                              transform.position = hit.point;*/
                        if (graph.Linecast((VInt3)ohit, (VInt3)closest, hit.node, out hit))
                        {
                            hit.point.y        = (int)transform.position.y;
                            transform.position = (Vector3)hit.point;
                        }
                        else
                        {
                            closest.y = transform.position.y;

                            transform.position = closest;
                        }
                    }
                    prevNode = hit.node;
                }
            }

            //Good Game
            //prevPos = transform.position;
            prevPos = (VInt3)transform.position;
        }
示例#8
0
 public static bool ContainsPoint(TriangleMeshNode node, Vector3 pos, Int3[] vertices)
 {
     if (!VectorMath.IsClockwiseMarginXZ((Vector3)vertices[node.v0], (Vector3)vertices[node.v1], (Vector3)vertices[node.v2]))
     {
         Debug.LogError("Noes!");
     }
     return(VectorMath.IsClockwiseMarginXZ((Vector3)vertices[node.v0], (Vector3)vertices[node.v1], pos) && VectorMath.IsClockwiseMarginXZ((Vector3)vertices[node.v1], (Vector3)vertices[node.v2], pos) && VectorMath.IsClockwiseMarginXZ((Vector3)vertices[node.v2], (Vector3)vertices[node.v0], pos));
 }
示例#9
0
        private void GenerateNodes(Vector3[] vectorVertices, int[] triangles, out Vector3[] originalVertices, out Int3[] vertices)
        {
            if (vectorVertices.Length == 0 || triangles.Length == 0)
            {
                originalVertices = vectorVertices;
                vertices         = new Int3[0];
                this.nodes       = new TriangleMeshNode[0];
                return;
            }
            vertices = new Int3[vectorVertices.Length];
            int num = 0;

            for (int i = 0; i < vertices.Length; i++)
            {
                vertices[i] = (Int3)this.matrix.MultiplyPoint3x4(vectorVertices[i]);
            }
            Dictionary <Int3, int> dictionary = new Dictionary <Int3, int>();

            int[] array = new int[vertices.Length];
            for (int j = 0; j < vertices.Length; j++)
            {
                if (!dictionary.ContainsKey(vertices[j]))
                {
                    array[num] = j;
                    dictionary.Add(vertices[j], num);
                    num++;
                }
            }
            for (int k = 0; k < triangles.Length; k++)
            {
                Int3 key = vertices[triangles[k]];
                triangles[k] = dictionary[key];
            }
            Int3[] array2 = vertices;
            vertices         = new Int3[num];
            originalVertices = new Vector3[num];
            for (int l = 0; l < num; l++)
            {
                vertices[l]         = array2[array[l]];
                originalVertices[l] = vectorVertices[array[l]];
            }
            this.nodes = new TriangleMeshNode[triangles.Length / 3];
            int graphIndex = this.active.astarData.GetGraphIndex(this);

            for (int m = 0; m < this.nodes.Length; m++)
            {
                this.nodes[m] = new TriangleMeshNode(this.active);
                TriangleMeshNode triangleMeshNode = this.nodes[m];
                triangleMeshNode.GraphIndex = (uint)graphIndex;
                triangleMeshNode.Penalty    = this.initialPenalty;
                triangleMeshNode.Walkable   = true;
                triangleMeshNode.v0         = triangles[m * 3];
                triangleMeshNode.v1         = triangles[m * 3 + 1];
                triangleMeshNode.v2         = triangles[m * 3 + 2];
                if (!VectorMath.IsClockwiseXZ(vertices[triangleMeshNode.v0], vertices[triangleMeshNode.v1], vertices[triangleMeshNode.v2]))
                {
                    int v = triangleMeshNode.v0;
                    triangleMeshNode.v0 = triangleMeshNode.v2;
                    triangleMeshNode.v2 = v;
                }
                if (VectorMath.IsColinearXZ(vertices[triangleMeshNode.v0], vertices[triangleMeshNode.v1], vertices[triangleMeshNode.v2]))
                {
                    Debug.DrawLine((Vector3)vertices[triangleMeshNode.v0], (Vector3)vertices[triangleMeshNode.v1], Color.red);
                    Debug.DrawLine((Vector3)vertices[triangleMeshNode.v1], (Vector3)vertices[triangleMeshNode.v2], Color.red);
                    Debug.DrawLine((Vector3)vertices[triangleMeshNode.v2], (Vector3)vertices[triangleMeshNode.v0], Color.red);
                }
                triangleMeshNode.UpdatePositionFromVertices();
            }
            Dictionary <Int2, TriangleMeshNode> dictionary2 = new Dictionary <Int2, TriangleMeshNode>();
            int n    = 0;
            int num2 = 0;

            while (n < triangles.Length)
            {
                dictionary2[new Int2(triangles[n], triangles[n + 1])]     = this.nodes[num2];
                dictionary2[new Int2(triangles[n + 1], triangles[n + 2])] = this.nodes[num2];
                dictionary2[new Int2(triangles[n + 2], triangles[n])]     = this.nodes[num2];
                num2++;
                n += 3;
            }
            List <MeshNode> list  = new List <MeshNode>();
            List <uint>     list2 = new List <uint>();
            int             num3  = 0;
            int             num4  = 0;

            while (num3 < triangles.Length)
            {
                list.Clear();
                list2.Clear();
                TriangleMeshNode triangleMeshNode2 = this.nodes[num4];
                for (int num5 = 0; num5 < 3; num5++)
                {
                    TriangleMeshNode triangleMeshNode3;
                    if (dictionary2.TryGetValue(new Int2(triangles[num3 + (num5 + 1) % 3], triangles[num3 + num5]), out triangleMeshNode3))
                    {
                        list.Add(triangleMeshNode3);
                        list2.Add((uint)(triangleMeshNode2.position - triangleMeshNode3.position).costMagnitude);
                    }
                }
                triangleMeshNode2.connections     = list.ToArray();
                triangleMeshNode2.connectionCosts = list2.ToArray();
                num4++;
                num3 += 3;
            }
            NavMeshGraph.RebuildBBTree(this);
        }
示例#10
0
        public static void UpdateArea(GraphUpdateObject o, INavmesh graph)
        {
            Bounds  bounds = o.bounds;
            Rect    r      = Rect.MinMaxRect(bounds.min.x, bounds.min.z, bounds.max.x, bounds.max.z);
            IntRect r2     = new IntRect(Mathf.FloorToInt(bounds.min.x * 1000f), Mathf.FloorToInt(bounds.min.z * 1000f), Mathf.FloorToInt(bounds.max.x * 1000f), Mathf.FloorToInt(bounds.max.z * 1000f));
            Int3    a      = new Int3(r2.xmin, 0, r2.ymin);
            Int3    b      = new Int3(r2.xmin, 0, r2.ymax);
            Int3    c      = new Int3(r2.xmax, 0, r2.ymin);
            Int3    d      = new Int3(r2.xmax, 0, r2.ymax);
            int     ymin   = ((Int3)bounds.min).y;
            int     ymax   = ((Int3)bounds.max).y;

            graph.GetNodes(delegate(GraphNode _node)
            {
                TriangleMeshNode triangleMeshNode = _node as TriangleMeshNode;
                bool flag = false;
                int num   = 0;
                int num2  = 0;
                int num3  = 0;
                int num4  = 0;
                for (int i = 0; i < 3; i++)
                {
                    Int3 vertex    = triangleMeshNode.GetVertex(i);
                    Vector3 vector = (Vector3)vertex;
                    if (r2.Contains(vertex.x, vertex.z))
                    {
                        flag = true;
                        break;
                    }
                    if (vector.x < r.xMin)
                    {
                        num++;
                    }
                    if (vector.x > r.xMax)
                    {
                        num2++;
                    }
                    if (vector.z < r.yMin)
                    {
                        num3++;
                    }
                    if (vector.z > r.yMax)
                    {
                        num4++;
                    }
                }
                if (!flag && (num == 3 || num2 == 3 || num3 == 3 || num4 == 3))
                {
                    return(true);
                }
                for (int j = 0; j < 3; j++)
                {
                    int i2       = (j <= 1) ? (j + 1) : 0;
                    Int3 vertex2 = triangleMeshNode.GetVertex(j);
                    Int3 vertex3 = triangleMeshNode.GetVertex(i2);
                    if (VectorMath.SegmentsIntersectXZ(a, b, vertex2, vertex3))
                    {
                        flag = true;
                        break;
                    }
                    if (VectorMath.SegmentsIntersectXZ(a, c, vertex2, vertex3))
                    {
                        flag = true;
                        break;
                    }
                    if (VectorMath.SegmentsIntersectXZ(c, d, vertex2, vertex3))
                    {
                        flag = true;
                        break;
                    }
                    if (VectorMath.SegmentsIntersectXZ(d, b, vertex2, vertex3))
                    {
                        flag = true;
                        break;
                    }
                }
                if (flag || triangleMeshNode.ContainsPoint(a) || triangleMeshNode.ContainsPoint(b) || triangleMeshNode.ContainsPoint(c) || triangleMeshNode.ContainsPoint(d))
                {
                    flag = true;
                }
                if (!flag)
                {
                    return(true);
                }
                int num5 = 0;
                int num6 = 0;
                for (int k = 0; k < 3; k++)
                {
                    Int3 vertex4 = triangleMeshNode.GetVertex(k);
                    if (vertex4.y < ymin)
                    {
                        num6++;
                    }
                    if (vertex4.y > ymax)
                    {
                        num5++;
                    }
                }
                if (num6 == 3 || num5 == 3)
                {
                    return(true);
                }
                o.WillUpdateNode(triangleMeshNode);
                o.Apply(triangleMeshNode);
                return(true);
            });
        }
示例#11
0
 public bool ContainsPoint(TriangleMeshNode node, Vector3 pos)
 {
     return(VectorMath.IsClockwiseXZ((Vector3)this.vertices[node.v0], (Vector3)this.vertices[node.v1], pos) && VectorMath.IsClockwiseXZ((Vector3)this.vertices[node.v1], (Vector3)this.vertices[node.v2], pos) && VectorMath.IsClockwiseXZ((Vector3)this.vertices[node.v2], (Vector3)this.vertices[node.v0], pos));
 }
示例#12
0
        public static bool Linecast(INavmesh graph, Vector3 tmp_origin, Vector3 tmp_end, GraphNode hint, out GraphHitInfo hit, List <GraphNode> trace)
        {
            Int3 @int = (Int3)tmp_end;
            Int3 int2 = (Int3)tmp_origin;

            hit = default(GraphHitInfo);
            if (float.IsNaN(tmp_origin.x + tmp_origin.y + tmp_origin.z))
            {
                throw new ArgumentException("origin is NaN");
            }
            if (float.IsNaN(tmp_end.x + tmp_end.y + tmp_end.z))
            {
                throw new ArgumentException("end is NaN");
            }
            TriangleMeshNode triangleMeshNode = hint as TriangleMeshNode;

            if (triangleMeshNode == null)
            {
                triangleMeshNode = ((graph as NavGraph).GetNearest(tmp_origin, NNConstraint.None).node as TriangleMeshNode);
                if (triangleMeshNode == null)
                {
                    Debug.LogError("Could not find a valid node to start from");
                    hit.point = tmp_origin;
                    return(true);
                }
            }
            if (int2 == @int)
            {
                hit.node = triangleMeshNode;
                return(false);
            }
            int2       = (Int3)triangleMeshNode.ClosestPointOnNode((Vector3)int2);
            hit.origin = (Vector3)int2;
            if (!triangleMeshNode.Walkable)
            {
                hit.point         = (Vector3)int2;
                hit.tangentOrigin = (Vector3)int2;
                return(true);
            }
            List <Vector3> list = ListPool <Vector3> .Claim();

            List <Vector3> list2 = ListPool <Vector3> .Claim();

            int num = 0;

            while (true)
            {
                num++;
                if (num > 2000)
                {
                    break;
                }
                TriangleMeshNode triangleMeshNode2 = null;
                if (trace != null)
                {
                    trace.Add(triangleMeshNode);
                }
                if (triangleMeshNode.ContainsPoint(@int))
                {
                    goto Block_9;
                }
                for (int i = 0; i < triangleMeshNode.connections.Length; i++)
                {
                    if (triangleMeshNode.connections[i].GraphIndex == triangleMeshNode.GraphIndex)
                    {
                        list.Clear();
                        list2.Clear();
                        if (triangleMeshNode.GetPortal(triangleMeshNode.connections[i], list, list2, false))
                        {
                            Vector3 vector  = list[0];
                            Vector3 vector2 = list2[0];
                            if (VectorMath.RightXZ(vector, vector2, hit.origin) || !VectorMath.RightXZ(vector, vector2, tmp_end))
                            {
                                float num2;
                                float num3;
                                if (VectorMath.LineIntersectionFactorXZ(vector, vector2, hit.origin, tmp_end, out num2, out num3))
                                {
                                    if (num3 >= 0f)
                                    {
                                        if (num2 >= 0f && num2 <= 1f)
                                        {
                                            triangleMeshNode2 = (triangleMeshNode.connections[i] as TriangleMeshNode);
                                            break;
                                        }
                                    }
                                }
                            }
                        }
                    }
                }
                if (triangleMeshNode2 == null)
                {
                    goto Block_18;
                }
                triangleMeshNode = triangleMeshNode2;
            }
            Debug.LogError("Linecast was stuck in infinite loop. Breaking.");
            ListPool <Vector3> .Release(list);

            ListPool <Vector3> .Release(list2);

            return(true);

Block_9:
            ListPool <Vector3> .Release(list);

            ListPool <Vector3> .Release(list2);

            return(false);

Block_18:
            int vertexCount = triangleMeshNode.GetVertexCount();

            for (int j = 0; j < vertexCount; j++)
            {
                Vector3 vector3 = (Vector3)triangleMeshNode.GetVertex(j);
                Vector3 vector4 = (Vector3)triangleMeshNode.GetVertex((j + 1) % vertexCount);
                if (VectorMath.RightXZ(vector3, vector4, hit.origin) || !VectorMath.RightXZ(vector3, vector4, tmp_end))
                {
                    float num4;
                    float num5;
                    if (VectorMath.LineIntersectionFactorXZ(vector3, vector4, hit.origin, tmp_end, out num4, out num5))
                    {
                        if (num5 >= 0f)
                        {
                            if (num4 >= 0f && num4 <= 1f)
                            {
                                Vector3 point = vector3 + (vector4 - vector3) * num4;
                                hit.point         = point;
                                hit.node          = triangleMeshNode;
                                hit.tangent       = vector4 - vector3;
                                hit.tangentOrigin = vector3;
                                ListPool <Vector3> .Release(list);

                                ListPool <Vector3> .Release(list2);

                                return(true);
                            }
                        }
                    }
                }
            }
            Debug.LogWarning("Linecast failing because point not inside node, and line does not hit any edges of it");
            ListPool <Vector3> .Release(list);

            ListPool <Vector3> .Release(list2);

            return(false);
        }
示例#13
0
        Vector3 Snap(ABPath path, Exactness mode, bool start, out bool forceAddPoint, out int closestConnectionIndex)
        {
            var index   = start ? 0 : path.path.Count - 1;
            var node    = path.path[index];
            var nodePos = (Vector3)node.position;

            closestConnectionIndex = 0;

            forceAddPoint = false;

            switch (mode)
            {
            case Exactness.ClosestOnNode:
                return(start ? path.startPoint : path.endPoint);

            case Exactness.SnapToNode:
                return(nodePos);

            case Exactness.Original:
            case Exactness.Interpolate:
            case Exactness.NodeConnection:
                Vector3 relevantPoint;
                if (start)
                {
                    relevantPoint = adjustStartPoint != null?adjustStartPoint() : path.originalStartPoint;
                }
                else
                {
                    relevantPoint = path.originalEndPoint;
                }

                switch (mode)
                {
                case Exactness.Original:
                    return(GetClampedPoint(nodePos, relevantPoint, node));

                case Exactness.Interpolate:
                    // Adjacent node to either the start node or the end node in the path
                    var adjacentNode = path.path[Mathf.Clamp(index + (start ? 1 : -1), 0, path.path.Count - 1)];
                    return(VectorMath.ClosestPointOnSegment(nodePos, (Vector3)adjacentNode.position,
                                                            relevantPoint));

                case Exactness.NodeConnection:
                    // This code uses some tricks to avoid allocations
                    // even though it uses delegates heavily
                    // The connectionBufferAddDelegate delegate simply adds whatever node
                    // it is called with to the connectionBuffer
                    connectionBuffer            = connectionBuffer ?? new List <GraphNode>();
                    connectionBufferAddDelegate = connectionBufferAddDelegate ??
                                                  (System.Action <GraphNode>)connectionBuffer.Add;

                    // Adjacent node to either the start node or the end node in the path
                    adjacentNode = path.path[Mathf.Clamp(index + (start ? 1 : -1), 0, path.path.Count - 1)];

                    // Add all neighbours of #node to the connectionBuffer
                    node.GetConnections(connectionBufferAddDelegate);
                    var bestPos  = nodePos;
                    var bestDist = float.PositiveInfinity;

                    // Loop through all neighbours
                    // Do it in reverse order because the length of the connectionBuffer
                    // will change during iteration
                    for (int i = connectionBuffer.Count - 1; i >= 0; i--)
                    {
                        var neighbour = connectionBuffer[i];

                        // Find the closest point on the connection between the nodes
                        // and check if the distance to that point is lower than the previous best
                        var closest = VectorMath.ClosestPointOnSegment(nodePos, (Vector3)neighbour.position,
                                                                       relevantPoint);

                        var dist = (closest - relevantPoint).sqrMagnitude;
                        if (dist < bestDist)
                        {
                            bestPos  = closest;
                            bestDist = dist;
                            closestConnectionIndex = i;

                            // If this node is not the adjacent node
                            // then the path should go through the start node as well
                            forceAddPoint = neighbour != adjacentNode;
                        }
                    }

                    connectionBuffer.Clear();
                    return(bestPos);

                default:
                    throw new System.ArgumentException(
                              "Cannot reach this point, but the compiler is not smart enough to realize that.");
                }

            default:
                throw new System.ArgumentException("Invalid mode");
            }
        }
示例#14
0
        public static Vector2 LineIntersectionPoint(Vector2 start1, Vector2 end1, Vector2 start2, Vector2 end2)
        {
            bool flag;

            return(VectorMath.LineIntersectionPoint(start1, end1, start2, end2, out flag));
        }
示例#15
0
        void TraverseFunnel(RichFunnel fn, float deltaTime, out Vector3 nextPosition, out Quaternion nextRotation)
        {
            // Clamp the current position to the navmesh
            // and update the list of upcoming corners in the path
            // and store that in the 'nextCorners' field
            var     position3D = UpdateTarget(fn);
            float   elevation;
            Vector2 position = movementPlane.ToPlane(position3D, out elevation);

            // Only find nearby walls every 5th frame to improve performance
            if (Time.frameCount % 5 == 0 && wallForce > 0 && wallDist > 0)
            {
                wallBuffer.Clear();
                fn.FindWalls(wallBuffer, wallDist);
            }

            // Target point
            steeringTarget = nextCorners[0];
            Vector2 targetPoint = movementPlane.ToPlane(steeringTarget);
            // Direction to target
            Vector2 dir = targetPoint - position;

            // Normalized direction to the target
            Vector2 normdir = VectorMath.Normalize(dir, out distanceToSteeringTarget);
            // Calculate force from walls
            Vector2 wallForceVector = CalculateWallForce(position, elevation, normdir);
            Vector2 targetVelocity;

            if (approachingPartEndpoint)
            {
                targetVelocity = slowdownTime > 0 ? Vector2.zero : normdir * maxSpeed;

                // Reduce the wall avoidance force as we get closer to our target
                wallForceVector *= System.Math.Min(distanceToSteeringTarget / 0.5f, 1);

                if (distanceToSteeringTarget <= endReachedDistance)
                {
                    // Reached the end of the path or an off mesh link
                    NextPart();
                }
            }
            else
            {
                var nextNextCorner = nextCorners.Count > 1 ? movementPlane.ToPlane(nextCorners[1]) : position + 2 * dir;
                targetVelocity = (nextNextCorner - targetPoint).normalized * maxSpeed;
            }

            var     forwards = movementPlane.ToPlane(simulatedRotation * (rotationIn2D ? Vector3.up : Vector3.forward));
            Vector2 accel    = MovementUtilities.CalculateAccelerationToReachPoint(targetPoint - position, targetVelocity, velocity2D, acceleration, rotationSpeed, maxSpeed, forwards);

            // Update the velocity using the acceleration
            velocity2D += (accel + wallForceVector * wallForce) * deltaTime;

            // Distance to the end of the path (almost as the crow flies)
            //Good Game
            //var distanceToEndOfPath = distanceToSteeringTarget + Vector3.Distance(steeringTarget, fn.exactEnd);
            var distanceToEndOfPath = distanceToSteeringTarget + Vector3.Distance(steeringTarget, (Vector3)fn.exactEnd);
            var slowdownFactor      = distanceToEndOfPath < maxSpeed *slowdownTime?Mathf.Sqrt(distanceToEndOfPath / (maxSpeed *slowdownTime)) : 1;

            FinalMovement(position3D, deltaTime, distanceToEndOfPath, slowdownFactor, out nextPosition, out nextRotation);
        }
        public List <Vector3> SmoothOffsetSimple(List <Vector3> path)
        {
            if (path.Count <= 2 || iterations <= 0)
            {
                return(path);
            }

            if (iterations > 12)
            {
                Debug.LogWarning("A very high iteration count was passed, won't let this one through");
                return(path);
            }

            int maxLength = (path.Count - 2) * (int)Mathf.Pow(2, iterations) + 2;

            List <Vector3> subdivided = ListPool <Vector3> .Claim(maxLength);

            List <Vector3> subdivided2 = ListPool <Vector3> .Claim(maxLength);

            for (int i = 0; i < maxLength; i++)
            {
                subdivided.Add(Vector3.zero); subdivided2.Add(Vector3.zero);
            }

            for (int i = 0; i < path.Count; i++)
            {
                subdivided[i] = path[i];
            }

            for (int iteration = 0; iteration < iterations; iteration++)
            {
                int currentPathLength = (path.Count - 2) * (int)Mathf.Pow(2, iteration) + 2;

                //Switch the arrays
                List <Vector3> tmp = subdivided;
                subdivided  = subdivided2;
                subdivided2 = tmp;

                const float nextMultiplier = 1F;

                for (int i = 0; i < currentPathLength - 1; i++)
                {
                    Vector3 current = subdivided2[i];
                    Vector3 next    = subdivided2[i + 1];

                    Vector3 normal = Vector3.Cross(next - current, Vector3.up);
                    normal = normal.normalized;

                    bool firstRight  = false;
                    bool secondRight = false;
                    bool setFirst    = false;
                    bool setSecond   = false;
                    if (i != 0 && !VectorMath.IsColinearXZ(current, next, subdivided2[i - 1]))
                    {
                        setFirst   = true;
                        firstRight = VectorMath.RightOrColinearXZ(current, next, subdivided2[i - 1]);
                    }
                    if (i < currentPathLength - 1 && !VectorMath.IsColinearXZ(current, next, subdivided2[i + 2]))
                    {
                        setSecond   = true;
                        secondRight = VectorMath.RightOrColinearXZ(current, next, subdivided2[i + 2]);
                    }

                    if (setFirst)
                    {
                        subdivided[i * 2] = current + (firstRight ? normal * offset * nextMultiplier : -normal * offset * nextMultiplier);
                    }
                    else
                    {
                        subdivided[i * 2] = current;
                    }

                    if (setSecond)
                    {
                        subdivided[i * 2 + 1] = next + (secondRight ? normal * offset * nextMultiplier : -normal * offset * nextMultiplier);
                    }
                    else
                    {
                        subdivided[i * 2 + 1] = next;
                    }
                }

                subdivided[(path.Count - 2) * (int)Mathf.Pow(2, iteration + 1) + 2 - 1] = subdivided2[currentPathLength - 1];
            }

            ListPool <Vector3> .Release(ref subdivided2);

            return(subdivided);
        }
示例#17
0
        public static void UpdateArea(GraphUpdateObject o, INavmeshHolder graph)
        {
            Bounds bounds = graph.transform.InverseTransform(o.bounds);

            // Bounding rectangle with integer coordinates
            var irect = new IntRect(
                Mathf.FloorToInt(bounds.min.x * Int3.Precision),
                Mathf.FloorToInt(bounds.min.z * Int3.Precision),
                Mathf.CeilToInt(bounds.max.x * Int3.Precision),
                Mathf.CeilToInt(bounds.max.z * Int3.Precision)
                );

            // Corners of the bounding rectangle
            var a = new Int3(irect.xmin, 0, irect.ymin);
            var b = new Int3(irect.xmin, 0, irect.ymax);
            var c = new Int3(irect.xmax, 0, irect.ymin);
            var d = new Int3(irect.xmax, 0, irect.ymax);

            var ymin = ((Int3)bounds.min).y;
            var ymax = ((Int3)bounds.max).y;

            // Loop through all nodes and check if they intersect the bounding box
            graph.GetNodes(_node => {
                var node = _node as TriangleMeshNode;

                bool inside = false;

                int allLeft   = 0;
                int allRight  = 0;
                int allTop    = 0;
                int allBottom = 0;

                // Check bounding box rect in XZ plane
                for (int v = 0; v < 3; v++)
                {
                    Int3 p = node.GetVertexInGraphSpace(v);

                    if (irect.Contains(p.x, p.z))
                    {
                        inside = true;
                        break;
                    }

                    if (p.x < irect.xmin)
                    {
                        allLeft++;
                    }
                    if (p.x > irect.xmax)
                    {
                        allRight++;
                    }
                    if (p.z < irect.ymin)
                    {
                        allTop++;
                    }
                    if (p.z > irect.ymax)
                    {
                        allBottom++;
                    }
                }

                if (!inside && (allLeft == 3 || allRight == 3 || allTop == 3 || allBottom == 3))
                {
                    return;
                }

                // Check if the polygon edges intersect the bounding rect
                for (int v = 0; v < 3; v++)
                {
                    int v2 = v > 1 ? 0 : v + 1;

                    Int3 vert1 = node.GetVertexInGraphSpace(v);
                    Int3 vert2 = node.GetVertexInGraphSpace(v2);

                    if (VectorMath.SegmentsIntersectXZ(a, b, vert1, vert2))
                    {
                        inside = true; break;
                    }
                    if (VectorMath.SegmentsIntersectXZ(a, c, vert1, vert2))
                    {
                        inside = true; break;
                    }
                    if (VectorMath.SegmentsIntersectXZ(c, d, vert1, vert2))
                    {
                        inside = true; break;
                    }
                    if (VectorMath.SegmentsIntersectXZ(d, b, vert1, vert2))
                    {
                        inside = true; break;
                    }
                }

                // Check if the node contains any corner of the bounding rect
                if (inside || node.ContainsPointInGraphSpace(a) || node.ContainsPointInGraphSpace(b) || node.ContainsPointInGraphSpace(c) || node.ContainsPointInGraphSpace(d))
                {
                    inside = true;
                }

                if (!inside)
                {
                    return;
                }

                int allAbove = 0;
                int allBelow = 0;

                // Check y coordinate
                for (int v = 0; v < 3; v++)
                {
                    Int3 p = node.GetVertexInGraphSpace(v);
                    if (p.y < ymin)
                    {
                        allBelow++;
                    }
                    if (p.y > ymax)
                    {
                        allAbove++;
                    }
                }

                // Polygon is either completely above the bounding box or completely below it
                if (allBelow == 3 || allAbove == 3)
                {
                    return;
                }

                // Triangle is inside the bounding box!
                // Update it!
                o.WillUpdateNode(node);
                o.Apply(node);
            });
        }
示例#18
0
        private Vector3 Snap(ABPath path, Exactness mode, bool start, out bool forceAddPoint)
        {
            Vector3   originalEndPoint;
            GraphNode node2;
            int       num      = !start ? (path.path.Count - 1) : 0;
            GraphNode hint     = path.path[num];
            Vector3   position = (Vector3)hint.position;

            forceAddPoint = false;
            switch (mode)
            {
            case Exactness.SnapToNode:
                return(position);

            case Exactness.Original:
            case Exactness.Interpolate:
            case Exactness.NodeConnection:
                if (!start)
                {
                    originalEndPoint = path.originalEndPoint;
                    break;
                }
                originalEndPoint = (this.adjustStartPoint == null) ? path.originalStartPoint : this.adjustStartPoint();
                break;

            case Exactness.ClosestOnNode:
                return(this.GetClampedPoint(position, !start ? path.endPoint : path.startPoint, hint));

            default:
                throw new ArgumentException("Invalid mode");
            }
            switch (mode)
            {
            case Exactness.Original:
                return(this.GetClampedPoint(position, originalEndPoint, hint));

            case Exactness.Interpolate:
            {
                Vector3 point = this.GetClampedPoint(position, originalEndPoint, hint);
                node2 = path.path[Mathf.Clamp(num + (!start ? -1 : 1), 0, path.path.Count - 1)];
                return(VectorMath.ClosestPointOnSegment(position, (Vector3)node2.position, point));
            }

            case Exactness.NodeConnection:
            {
                if (this.connectionBuffer == null)
                {
                }
                this.connectionBuffer = new List <GraphNode>();
                if (this.connectionBufferAddDelegate == null)
                {
                }
                this.connectionBufferAddDelegate = new GraphNodeDelegate(this.connectionBuffer.Add);
                node2 = path.path[Mathf.Clamp(num + (!start ? -1 : 1), 0, path.path.Count - 1)];
                hint.GetConnections(this.connectionBufferAddDelegate);
                Vector3 vector4          = position;
                float   positiveInfinity = float.PositiveInfinity;
                for (int i = this.connectionBuffer.Count - 1; i >= 0; i--)
                {
                    GraphNode node3        = this.connectionBuffer[i];
                    Vector3   vector5      = VectorMath.ClosestPointOnSegment(position, (Vector3)node3.position, originalEndPoint);
                    Vector3   vector6      = vector5 - originalEndPoint;
                    float     sqrMagnitude = vector6.sqrMagnitude;
                    if (sqrMagnitude < positiveInfinity)
                    {
                        vector4          = vector5;
                        positiveInfinity = sqrMagnitude;
                        forceAddPoint    = node3 != node2;
                    }
                }
                this.connectionBuffer.Clear();
                return(vector4);
            }
            }
            throw new ArgumentException("Cannot reach this point, but the compiler is not smart enough to realize that.");
        }
示例#19
0
        /** Updates an area in the list graph.
         * Recalculates possibly affected connections, i.e all connectionlines passing trough the bounds of the \a guo will be recalculated
         * \astarpro */
        public void UpdateArea(GraphUpdateObject guo)
        {
            if (nodes == null)
            {
                return;
            }

            for (int i = 0; i < nodeCount; i++)
            {
                if (guo.bounds.Contains((Vector3)nodes[i].position))
                {
                    guo.WillUpdateNode(nodes[i]);
                    guo.Apply(nodes[i]);
                }
            }

            if (guo.updatePhysics)
            {
                //Use a copy of the bounding box, we should not change the GUO's bounding box since it might be used for other graph updates
                Bounds bounds = guo.bounds;

                if (thickRaycast)
                {
                    //Expand the bounding box to account for the thick raycast
                    bounds.Expand(thickRaycastRadius * 2);
                }

                //Create two temporary arrays used for holding new connections and costs
                List <Connection> tmp_arr = Pathfinding.Util.ListPool <Connection> .Claim();

                for (int i = 0; i < nodeCount; i++)
                {
                    PointNode node    = nodes[i];
                    var       nodePos = (Vector3)node.position;

                    List <Connection> conn = null;

                    for (int j = 0; j < nodeCount; j++)
                    {
                        if (j == i)
                        {
                            continue;
                        }

                        var otherNodePos = (Vector3)nodes[j].position;
                        // Check if this connection intersects the bounding box.
                        // If it does we need to recalculate that connection.
                        if (VectorMath.SegmentIntersectsBounds(bounds, nodePos, otherNodePos))
                        {
                            float     dist;
                            PointNode other           = nodes[j];
                            bool      contains        = node.ContainsConnection(other);
                            bool      validConnection = IsValidConnection(node, other, out dist);

                            if (!contains && validConnection)
                            {
                                // A new connection should be added

                                if (conn == null)
                                {
                                    tmp_arr.Clear();
                                    conn = tmp_arr;
                                    conn.AddRange(node.connections);
                                }

                                uint cost = (uint)Mathf.RoundToInt(dist * Int3.FloatPrecision);
                                conn.Add(new Connection {
                                    node = other, cost = cost
                                });
                            }
                            else if (contains && !validConnection)
                            {
                                // A connection should be removed

                                if (conn == null)
                                {
                                    tmp_arr.Clear();
                                    conn = tmp_arr;
                                    conn.AddRange(node.connections);
                                }

                                for (int q = 0; q < conn.Count; q++)
                                {
                                    if (conn[q].node == other)
                                    {
                                        conn.RemoveAt(q);
                                        break;
                                    }
                                }
                            }
                        }
                    }

                    // Save the new connections if any were changed
                    if (conn != null)
                    {
                        node.connections = conn.ToArray();
                    }
                }

                // Release buffers back to the pool
                Pathfinding.Util.ListPool <Connection> .Release(tmp_arr);
            }
        }
示例#20
0
        public static bool RunFunnel(List <Vector3> left, List <Vector3> right, List <Vector3> funnelPath)
        {
            if (left == null)
            {
                throw new ArgumentNullException("left");
            }
            if (right == null)
            {
                throw new ArgumentNullException("right");
            }
            if (funnelPath == null)
            {
                throw new ArgumentNullException("funnelPath");
            }
            if (left.Count != right.Count)
            {
                throw new ArgumentException("left and right lists must have equal length");
            }
            while (left[1] == left[2] && right[1] == right[2])
            {
                left.RemoveAt(1);
                right.RemoveAt(1);
                if (left.Count <= 3)
                {
                    return(false);
                }
            }
            Vector3 vector = left[2];

            if (vector == left[1])
            {
                vector = right[2];
            }
            while (VectorMath.IsColinearXZ(left[0], left[1], right[1]) || VectorMath.RightOrColinearXZ(left[1], right[1], vector) == VectorMath.RightOrColinearXZ(left[1], right[1], left[0]))
            {
                left.RemoveAt(1);
                right.RemoveAt(1);
                if (left.Count <= 3)
                {
                    return(false);
                }
                vector = left[2];
                if (vector == left[1])
                {
                    vector = right[2];
                }
            }
            if (!VectorMath.IsClockwiseXZ(left[0], left[1], right[1]) && !VectorMath.IsColinearXZ(left[0], left[1], right[1]))
            {
                List <Vector3> list = left;
                left  = right;
                right = list;
            }
            funnelPath.Add(left[0]);
            Vector3 vector2 = left[0];
            Vector3 vector3 = left[1];
            Vector3 vector4 = right[1];
            int     num     = 1;
            int     num2    = 1;
            int     i       = 2;

            while (i < left.Count)
            {
                if (funnelPath.Count > 2000)
                {
                    Debug.LogWarning("Avoiding infinite loop. Remove this check if you have this long paths.");
                    break;
                }
                Vector3 vector5 = left[i];
                Vector3 vector6 = right[i];
                if (VectorMath.SignedTriangleAreaTimes2XZ(vector2, vector4, vector6) < 0f)
                {
                    goto IL_26B;
                }
                if (vector2 == vector4 || VectorMath.SignedTriangleAreaTimes2XZ(vector2, vector3, vector6) <= 0f)
                {
                    vector4 = vector6;
                    num     = i;
                    goto IL_26B;
                }
                funnelPath.Add(vector3);
                vector2 = vector3;
                int num3 = num2;
                vector3 = vector2;
                vector4 = vector2;
                num2    = num3;
                num     = num3;
                i       = num3;
IL_2CF:
                i++;
                continue;
IL_26B:
                if (VectorMath.SignedTriangleAreaTimes2XZ(vector2, vector3, vector5) > 0f)
                {
                    goto IL_2CF;
                }
                if (vector2 == vector3 || VectorMath.SignedTriangleAreaTimes2XZ(vector2, vector4, vector5) >= 0f)
                {
                    vector3 = vector5;
                    num2    = i;
                    goto IL_2CF;
                }
                funnelPath.Add(vector4);
                vector2 = vector4;
                num3    = num;
                vector3 = vector2;
                vector4 = vector2;
                num2    = num3;
                num     = num3;
                i       = num3;
                goto IL_2CF;
            }
            funnelPath.Add(left[left.Count - 1]);
            return(true);
        }
示例#21
0
        // Token: 0x06002619 RID: 9753 RVA: 0x001A76DC File Offset: 0x001A58DC
        void IUpdatableGraph.UpdateArea(GraphUpdateObject guo)
        {
            if (this.nodes == null)
            {
                return;
            }
            for (int i = 0; i < this.nodeCount; i++)
            {
                PointNode pointNode = this.nodes[i];
                if (guo.bounds.Contains((Vector3)pointNode.position))
                {
                    guo.WillUpdateNode(pointNode);
                    guo.Apply(pointNode);
                }
            }
            if (guo.updatePhysics)
            {
                Bounds bounds = guo.bounds;
                if (this.thickRaycast)
                {
                    bounds.Expand(this.thickRaycastRadius * 2f);
                }
                List <Connection> list = ListPool <Connection> .Claim();

                for (int j = 0; j < this.nodeCount; j++)
                {
                    PointNode         pointNode2 = this.nodes[j];
                    Vector3           a          = (Vector3)pointNode2.position;
                    List <Connection> list2      = null;
                    for (int k = 0; k < this.nodeCount; k++)
                    {
                        if (k != j)
                        {
                            Vector3 b = (Vector3)this.nodes[k].position;
                            if (VectorMath.SegmentIntersectsBounds(bounds, a, b))
                            {
                                PointNode pointNode3 = this.nodes[k];
                                bool      flag       = pointNode2.ContainsConnection(pointNode3);
                                float     num;
                                bool      flag2 = this.IsValidConnection(pointNode2, pointNode3, out num);
                                if (list2 == null && flag != flag2)
                                {
                                    list.Clear();
                                    list2 = list;
                                    list2.AddRange(pointNode2.connections);
                                }
                                if (!flag && flag2)
                                {
                                    uint cost = (uint)Mathf.RoundToInt(num * 1000f);
                                    list2.Add(new Connection(pointNode3, cost, byte.MaxValue));
                                }
                                else if (flag && !flag2)
                                {
                                    for (int l = 0; l < list2.Count; l++)
                                    {
                                        if (list2[l].node == pointNode3)
                                        {
                                            list2.RemoveAt(l);
                                            break;
                                        }
                                    }
                                }
                            }
                        }
                    }
                    if (list2 != null)
                    {
                        pointNode2.connections = list2.ToArray();
                    }
                }
                ListPool <Connection> .Release(ref list);
            }
        }
示例#22
0
        /// <summary>TODO: This is the area in XZ space, use full 3D space for higher correctness maybe?</summary>
        public override float SurfaceArea()
        {
            var holder = GetNavmeshHolder(GraphIndex);

            return(System.Math.Abs(VectorMath.SignedTriangleAreaTimes2XZ(holder.GetVertex(v0), holder.GetVertex(v1), holder.GetVertex(v2))) * 0.5f);
        }
示例#23
0
        public static void UpdateArea(GraphUpdateObject o, INavmesh graph)
        {
            Bounds bounds = o.bounds;

            // Bounding rectangle with floating point coordinates
            Rect r = Rect.MinMaxRect(bounds.min.x, bounds.min.z, bounds.max.x, bounds.max.z);

            // Bounding rectangle with int coordinates
            var r2 = new IntRect(
                Mathf.FloorToInt(bounds.min.x * Int3.Precision),
                Mathf.FloorToInt(bounds.min.z * Int3.Precision),
                Mathf.FloorToInt(bounds.max.x * Int3.Precision),
                Mathf.FloorToInt(bounds.max.z * Int3.Precision)
                );

            // Corners of the bounding rectangle
            var a = new Int3(r2.xmin, 0, r2.ymin);
            var b = new Int3(r2.xmin, 0, r2.ymax);
            var c = new Int3(r2.xmax, 0, r2.ymin);
            var d = new Int3(r2.xmax, 0, r2.ymax);

            var ymin = ((Int3)bounds.min).y;
            var ymax = ((Int3)bounds.max).y;

            // Loop through all nodes
            graph.GetNodes(_node => {
                var node = _node as TriangleMeshNode;

                bool inside = false;

                int allLeft   = 0;
                int allRight  = 0;
                int allTop    = 0;
                int allBottom = 0;

                // Check bounding box rect in XZ plane
                for (int v = 0; v < 3; v++)
                {
                    Int3 p   = node.GetVertex(v);
                    var vert = (Vector3)p;

                    if (r2.Contains(p.x, p.z))
                    {
                        inside = true;
                        break;
                    }

                    if (vert.x < r.xMin)
                    {
                        allLeft++;
                    }
                    if (vert.x > r.xMax)
                    {
                        allRight++;
                    }
                    if (vert.z < r.yMin)
                    {
                        allTop++;
                    }
                    if (vert.z > r.yMax)
                    {
                        allBottom++;
                    }
                }

                if (!inside)
                {
                    if (allLeft == 3 || allRight == 3 || allTop == 3 || allBottom == 3)
                    {
                        return(true);
                    }
                }

                // Check if the polygon edges intersect the bounding rect
                for (int v = 0; v < 3; v++)
                {
                    int v2 = v > 1 ? 0 : v + 1;

                    Int3 vert1 = node.GetVertex(v);
                    Int3 vert2 = node.GetVertex(v2);

                    if (VectorMath.SegmentsIntersectXZ(a, b, vert1, vert2))
                    {
                        inside = true; break;
                    }
                    if (VectorMath.SegmentsIntersectXZ(a, c, vert1, vert2))
                    {
                        inside = true; break;
                    }
                    if (VectorMath.SegmentsIntersectXZ(c, d, vert1, vert2))
                    {
                        inside = true; break;
                    }
                    if (VectorMath.SegmentsIntersectXZ(d, b, vert1, vert2))
                    {
                        inside = true; break;
                    }
                }

                // Check if the node contains any corner of the bounding rect
                if (inside || node.ContainsPoint(a) || node.ContainsPoint(b) || node.ContainsPoint(c) || node.ContainsPoint(d))
                {
                    inside = true;
                }

                if (!inside)
                {
                    return(true);
                }

                int allAbove = 0;
                int allBelow = 0;

                // Check y coordinate
                for (int v = 0; v < 3; v++)
                {
                    Int3 p = node.GetVertex(v);
                    if (p.y < ymin)
                    {
                        allBelow++;
                    }
                    if (p.y > ymax)
                    {
                        allAbove++;
                    }
                }

                // Polygon is either completely above the bounding box or completely below it
                if (allBelow == 3 || allAbove == 3)
                {
                    return(true);
                }

                // Triangle is inside the bounding box!
                // Update it!
                o.WillUpdateNode(node);
                o.Apply(node);
                return(true);
            });
        }
示例#24
0
文件: NodeLink3.cs 项目: CCGLP/Fast
        public void Apply(bool forceNewCheck)
        {
            //TODO
            //This function assumes that connections from the n1,n2 nodes never need to be removed in the future (e.g because the nodes move or something)
            NNConstraint nn = NNConstraint.None;

            nn.distanceXZ = true;
            int graph = (int)startNode.GraphIndex;

            //Search all graphs but the one which start and end nodes are on
            nn.graphMask = ~(1 << graph);

            bool same = true;

            if (true)
            {
                NNInfo n1 = AstarPath.active.GetNearest(StartTransform.position, nn);
                same          &= n1.node == connectedNode1 && n1.node != null;
                connectedNode1 = n1.node as MeshNode;
                clamped1       = n1.clampedPosition;
                if (connectedNode1 != null)
                {
                    Debug.DrawRay((Vector3)connectedNode1.position, Vector3.up * 5, Color.red);
                }
            }

            if (true)
            {
                NNInfo n2 = AstarPath.active.GetNearest(EndTransform.position, nn);
                same          &= n2.node == connectedNode2 && n2.node != null;
                connectedNode2 = n2.node as MeshNode;
                clamped2       = n2.clampedPosition;
                if (connectedNode2 != null)
                {
                    Debug.DrawRay((Vector3)connectedNode2.position, Vector3.up * 5, Color.cyan);
                }
            }

            if (connectedNode2 == null || connectedNode1 == null)
            {
                return;
            }

            startNode.SetPosition((Int3)StartTransform.position);
            endNode.SetPosition((Int3)EndTransform.position);

            if (same && !forceNewCheck)
            {
                return;
            }

            RemoveConnections(startNode);
            RemoveConnections(endNode);

            uint cost = (uint)Mathf.RoundToInt(((Int3)(StartTransform.position - EndTransform.position)).costMagnitude * costFactor);

            startNode.AddConnection(endNode, cost);
            endNode.AddConnection(startNode, cost);

            Int3 dir = connectedNode2.position - connectedNode1.position;

            for (int a = 0; a < connectedNode1.GetVertexCount(); a++)
            {
                Int3 va1 = connectedNode1.GetVertex(a);
                Int3 va2 = connectedNode1.GetVertex((a + 1) % connectedNode1.GetVertexCount());

                if (Int3.DotLong((va2 - va1).Normal2D(), dir) > 0)
                {
                    continue;
                }

                for (int b = 0; b < connectedNode2.GetVertexCount(); b++)
                {
                    Int3 vb1 = connectedNode2.GetVertex(b);
                    Int3 vb2 = connectedNode2.GetVertex((b + 1) % connectedNode2.GetVertexCount());

                    if (Int3.DotLong((vb2 - vb1).Normal2D(), dir) < 0)
                    {
                        continue;
                    }


                    //Debug.DrawLine ((Vector3)va1, (Vector3)va2, Color.magenta);
                    //Debug.DrawLine ((Vector3)vb1, (Vector3)vb2, Color.cyan);
                    //Debug.Break ();

                    if (Int3.Angle((vb2 - vb1), (va2 - va1)) > (170.0 / 360.0f) * Mathf.PI * 2)
                    {
                        float t1 = 0;
                        float t2 = 1;

                        t2 = System.Math.Min(t2, VectorMath.ClosestPointOnLineFactor(va1, va2, vb1));
                        t1 = System.Math.Max(t1, VectorMath.ClosestPointOnLineFactor(va1, va2, vb2));

                        if (t2 < t1)
                        {
                            Debug.LogError("Wait wut!? " + t1 + " " + t2 + " " + va1 + " " + va2 + " " + vb1 + " " + vb2 + "\nTODO, fix this error");
                        }
                        else
                        {
                            Vector3 pa = (Vector3)(va2 - va1) * t1 + (Vector3)va1;
                            Vector3 pb = (Vector3)(va2 - va1) * t2 + (Vector3)va1;

                            startNode.portalA = pa;
                            startNode.portalB = pb;

                            endNode.portalA = pb;
                            endNode.portalB = pa;

                            //Add connections between nodes, or replace old connections if existing
                            connectedNode1.AddConnection(startNode, (uint)Mathf.RoundToInt(((Int3)(clamped1 - StartTransform.position)).costMagnitude * costFactor));
                            connectedNode2.AddConnection(endNode, (uint)Mathf.RoundToInt(((Int3)(clamped2 - EndTransform.position)).costMagnitude * costFactor));

                            startNode.AddConnection(connectedNode1, (uint)Mathf.RoundToInt(((Int3)(clamped1 - StartTransform.position)).costMagnitude * costFactor));
                            endNode.AddConnection(connectedNode2, (uint)Mathf.RoundToInt(((Int3)(clamped2 - EndTransform.position)).costMagnitude * costFactor));

                            return;
                        }
                    }
                }
            }
        }
示例#25
0
        /// <summary>
        /// Searches for the node the agent is inside.
        /// This will also clamp the position to the navmesh
        /// and repair the funnel cooridor if the agent moves slightly outside it.
        ///
        /// Returns: True if nodes along the path have been destroyed so that a path recalculation is required
        /// </summary>
        bool ClampToNavmeshInternal(ref Vector3 position)
        {
            var previousNode = nodes[currentNode];

            if (previousNode.Destroyed)
            {
                return(true);
            }

            // Check if we are in the same node as we were in during the last frame and otherwise do a more extensive search
            if (previousNode.ContainsPoint(position))
            {
                return(false);
            }

            // This part of the code is relatively seldom called
            // Most of the time we are still on the same node as during the previous frame

            var que        = navmeshClampQueue;
            var allVisited = navmeshClampList;
            var parent     = navmeshClampDict;

            previousNode.TemporaryFlag1 = true;
            parent[previousNode]        = null;
            que.Enqueue(previousNode);
            allVisited.Add(previousNode);

            float            bestDistance = float.PositiveInfinity;
            Vector3          bestPoint    = position;
            TriangleMeshNode bestNode     = null;

            while (que.Count > 0)
            {
                var node = que.Dequeue();

                // Snap to the closest point in XZ space (keep the Y coordinate)
                // If we would have snapped to the closest point in 3D space, the agent
                // might slow down when traversing slopes
                var closest = node.ClosestPointOnNodeXZ(position);
                var dist    = VectorMath.MagnitudeXZ(closest - position);

                // Check if this node is any closer than the previous best node.
                // Allow for a small margin to both avoid floating point errors and to allow
                // moving past very small local minima.
                if (dist <= bestDistance * 1.05f + 0.001f)
                {
                    if (dist < bestDistance)
                    {
                        bestDistance = dist;
                        bestPoint    = closest;
                        bestNode     = node;
                    }

                    for (int i = 0; i < node.connections.Length; i++)
                    {
                        var neighbour = node.connections[i].node as TriangleMeshNode;
                        if (neighbour != null && !neighbour.TemporaryFlag1)
                        {
                            neighbour.TemporaryFlag1 = true;
                            parent[neighbour]        = node;
                            que.Enqueue(neighbour);
                            allVisited.Add(neighbour);
                        }
                    }
                }
            }

            if (bestNode == null)
            {
                Debug.LogError(previousNode.position + " " + previousNode.ClosestPointOnNode(position) + " " + position + " " + bestDistance);
            }

            UnityEngine.Assertions.Assert.IsNotNull(bestNode);

            for (int i = 0; i < allVisited.Count; i++)
            {
                allVisited[i].TemporaryFlag1 = false;
            }
            allVisited.ClearFast();

            var closestNodeInPath = nodes.IndexOf(bestNode);

            // Move the x and z coordinates of the chararacter but not the y coordinate
            // because the navmesh surface may not line up with the ground
            position.x = bestPoint.x;
            position.z = bestPoint.z;

            // Check if the closest node
            // was on the path already or if we need to adjust it
            if (closestNodeInPath == -1)
            {
                // Reuse this list, because why not.
                var prefix = navmeshClampList;

                while (closestNodeInPath == -1)
                {
                    prefix.Add(bestNode);
                    bestNode          = parent[bestNode];
                    closestNodeInPath = nodes.IndexOf(bestNode);
                }

                // We have found a node containing the position, but it is outside the funnel
                // Recalculate the funnel to include this node
                exactStart = position;
                UpdateFunnelCorridor(closestNodeInPath, prefix);

                prefix.ClearFast();

                // Restart from the first node in the updated path
                currentNode = 0;
            }
            else
            {
                currentNode = closestNodeInPath;
            }

            parent.Clear();
            // Do a quick check to see if the next node in the path has been destroyed
            // If that is the case then we should plan a new path immediately
            return(currentNode + 1 < nodes.Count && nodes[currentNode + 1].Destroyed);
        }
示例#26
0
        public static bool RunFunnel(List <Vector3> left, List <Vector3> right, List <Vector3> funnelPath)
        {
            if (left == null)
            {
                throw new ArgumentNullException("left");
            }
            if (right == null)
            {
                throw new ArgumentNullException("right");
            }
            if (funnelPath == null)
            {
                throw new ArgumentNullException("funnelPath");
            }
            if (left.Count != right.Count)
            {
                throw new ArgumentException("left and right lists must have equal length");
            }
            if (left.Count <= 3)
            {
                return(false);
            }
            while ((left[1] == left[2]) && (right[1] == right[2]))
            {
                left.RemoveAt(1);
                right.RemoveAt(1);
                if (left.Count <= 3)
                {
                    return(false);
                }
            }
            Vector3 p = left[2];

            if (p == left[1])
            {
                p = right[2];
            }
            while (VectorMath.IsColinearXZ(left[0], left[1], right[1]) || (VectorMath.RightOrColinearXZ(left[1], right[1], p) == VectorMath.RightOrColinearXZ(left[1], right[1], left[0])))
            {
                left.RemoveAt(1);
                right.RemoveAt(1);
                if (left.Count <= 3)
                {
                    return(false);
                }
                p = left[2];
                if (p == left[1])
                {
                    p = right[2];
                }
            }
            if (!VectorMath.IsClockwiseXZ(left[0], left[1], right[1]) && !VectorMath.IsColinearXZ(left[0], left[1], right[1]))
            {
                List <Vector3> list = left;
                left  = right;
                right = list;
            }
            funnelPath.Add(left[0]);
            Vector3 a       = left[0];
            Vector3 b       = left[1];
            Vector3 vector4 = right[1];
            int     num     = 0;
            int     num2    = 1;
            int     num3    = 1;

            for (int i = 2; i < left.Count; i++)
            {
                if (funnelPath.Count > 0x7d0)
                {
                    Debug.LogWarning("Avoiding infinite loop. Remove this check if you have this long paths.");
                    break;
                }
                Vector3 c       = left[i];
                Vector3 vector6 = right[i];
                if (VectorMath.SignedTriangleAreaTimes2XZ(a, vector4, vector6) >= 0f)
                {
                    if ((a == vector4) || (VectorMath.SignedTriangleAreaTimes2XZ(a, b, vector6) <= 0f))
                    {
                        vector4 = vector6;
                        num2    = i;
                    }
                    else
                    {
                        funnelPath.Add(b);
                        a       = b;
                        num     = num3;
                        b       = a;
                        vector4 = a;
                        num3    = num;
                        num2    = num;
                        i       = num;
                        continue;
                    }
                }
                if (VectorMath.SignedTriangleAreaTimes2XZ(a, b, c) <= 0f)
                {
                    if ((a == b) || (VectorMath.SignedTriangleAreaTimes2XZ(a, vector4, c) >= 0f))
                    {
                        b    = c;
                        num3 = i;
                    }
                    else
                    {
                        funnelPath.Add(vector4);
                        a       = vector4;
                        num     = num2;
                        b       = a;
                        vector4 = a;
                        num3    = num;
                        num2    = num;
                        i       = num;
                    }
                }
            }
            funnelPath.Add(left[left.Count - 1]);
            return(true);
        }
示例#27
0
        /** Updates an area in the list graph.
         * Recalculates possibly affected connections, i.e all connectionlines passing trough the bounds of the \a guo will be recalculated
         * \astarpro */
        public void UpdateArea(GraphUpdateObject guo)
        {
            if (nodes == null)
            {
                return;
            }

            for (int i = 0; i < nodeCount; i++)
            {
                if (guo.bounds.Contains((Vector3)nodes[i].position))
                {
                    guo.WillUpdateNode(nodes[i]);
                    guo.Apply(nodes[i]);
                }
            }

            if (guo.updatePhysics)
            {
                //Use a copy of the bounding box, we should not change the GUO's bounding box since it might be used for other graph updates
                Bounds bounds = guo.bounds;

                if (thickRaycast)
                {
                    //Expand the bounding box to account for the thick raycast
                    bounds.Expand(thickRaycastRadius * 2);
                }

                //Create two temporary arrays used for holding new connections and costs
                List <GraphNode> tmp_arr = Pathfinding.Util.ListPool <GraphNode> .Claim();

                List <uint> tmp_arr2 = Pathfinding.Util.ListPool <uint> .Claim();

                for (int i = 0; i < nodeCount; i++)
                {
                    PointNode node = nodes[i];
                    var       a    = (Vector3)node.position;

                    List <GraphNode> conn  = null;
                    List <uint>      costs = null;

                    for (int j = 0; j < nodeCount; j++)
                    {
                        if (j == i)
                        {
                            continue;
                        }

                        var b = (Vector3)nodes[j].position;
                        if (VectorMath.SegmentIntersectsBounds(bounds, a, b))
                        {
                            float     dist;
                            PointNode other           = nodes[j];
                            bool      contains        = node.ContainsConnection(other);
                            bool      validConnection = IsValidConnection(node, other, out dist);

                            if (!contains && validConnection)
                            {
                                // A new connection should be added

                                if (conn == null)
                                {
                                    tmp_arr.Clear();
                                    tmp_arr2.Clear();
                                    conn  = tmp_arr;
                                    costs = tmp_arr2;
                                    conn.AddRange(node.connections);
                                    costs.AddRange(node.connectionCosts);
                                }

                                uint cost = (uint)Mathf.RoundToInt(dist * Int3.FloatPrecision);
                                conn.Add(other);
                                costs.Add(cost);
                            }
                            else if (contains && !validConnection)
                            {
                                // A connection should be removed

                                if (conn == null)
                                {
                                    tmp_arr.Clear();
                                    tmp_arr2.Clear();
                                    conn  = tmp_arr;
                                    costs = tmp_arr2;
                                    conn.AddRange(node.connections);
                                    costs.AddRange(node.connectionCosts);
                                }

                                int p = conn.IndexOf(other);

                                //Shouldn't have to check for it, but who knows what might go wrong
                                if (p != -1)
                                {
                                    conn.RemoveAt(p);
                                    costs.RemoveAt(p);
                                }
                            }
                        }
                    }

                    // Save the new connections if any were changed
                    if (conn != null)
                    {
                        node.connections     = conn.ToArray();
                        node.connectionCosts = costs.ToArray();
                    }
                }

                // Release buffers back to the pool
                Pathfinding.Util.ListPool <GraphNode> .Release(tmp_arr);

                Pathfinding.Util.ListPool <uint> .Release(tmp_arr2);
            }
        }
示例#28
0
        public bool FindNextCorners(Vector3 origin, int startIndex, List <Vector3> funnelPath, int numCorners, out bool lastCorner)
        {
            lastCorner = false;
            if (this.left == null)
            {
                throw new Exception("left list is null");
            }
            if (this.right == null)
            {
                throw new Exception("right list is null");
            }
            if (funnelPath == null)
            {
                throw new ArgumentNullException("funnelPath");
            }
            if (this.left.Count != this.right.Count)
            {
                throw new ArgumentException("left and right lists must have equal length");
            }
            int count = this.left.Count;

            if (count == 0)
            {
                throw new ArgumentException("no diagonals");
            }
            if ((count - startIndex) < 3)
            {
                funnelPath.Add(this.left[count - 1]);
                lastCorner = true;
                return(true);
            }
            while ((this.left[startIndex + 1] == this.left[startIndex + 2]) && (this.right[startIndex + 1] == this.right[startIndex + 2]))
            {
                startIndex++;
                if ((count - startIndex) <= 3)
                {
                    return(false);
                }
            }
            Vector3 p = this.left[startIndex + 2];

            if (p == this.left[startIndex + 1])
            {
                p = this.right[startIndex + 2];
            }
            while (VectorMath.IsColinearXZ(origin, this.left[startIndex + 1], this.right[startIndex + 1]) || (VectorMath.RightOrColinearXZ(this.left[startIndex + 1], this.right[startIndex + 1], p) == VectorMath.RightOrColinearXZ(this.left[startIndex + 1], this.right[startIndex + 1], origin)))
            {
                startIndex++;
                if ((count - startIndex) < 3)
                {
                    funnelPath.Add(this.left[count - 1]);
                    lastCorner = true;
                    return(true);
                }
                p = this.left[startIndex + 2];
                if (p == this.left[startIndex + 1])
                {
                    p = this.right[startIndex + 2];
                }
            }
            Vector3 a       = origin;
            Vector3 b       = this.left[startIndex + 1];
            Vector3 vector4 = this.right[startIndex + 1];
            int     num2    = startIndex;
            int     num3    = startIndex + 1;
            int     num4    = startIndex + 1;

            for (int i = startIndex + 2; i < count; i++)
            {
                if (funnelPath.Count >= numCorners)
                {
                    return(true);
                }
                if (funnelPath.Count > 0x7d0)
                {
                    Debug.LogWarning("Avoiding infinite loop. Remove this check if you have this long paths.");
                    break;
                }
                Vector3 c       = this.left[i];
                Vector3 vector6 = this.right[i];
                if (VectorMath.SignedTriangleAreaTimes2XZ(a, vector4, vector6) >= 0f)
                {
                    if ((a == vector4) || (VectorMath.SignedTriangleAreaTimes2XZ(a, b, vector6) <= 0f))
                    {
                        vector4 = vector6;
                        num3    = i;
                    }
                    else
                    {
                        funnelPath.Add(b);
                        a       = b;
                        num2    = num4;
                        b       = a;
                        vector4 = a;
                        num4    = num2;
                        num3    = num2;
                        i       = num2;
                        continue;
                    }
                }
                if (VectorMath.SignedTriangleAreaTimes2XZ(a, b, c) <= 0f)
                {
                    if ((a == b) || (VectorMath.SignedTriangleAreaTimes2XZ(a, vector4, c) >= 0f))
                    {
                        b    = c;
                        num4 = i;
                    }
                    else
                    {
                        funnelPath.Add(vector4);
                        a       = vector4;
                        num2    = num3;
                        b       = a;
                        vector4 = a;
                        num4    = num2;
                        num3    = num2;
                        i       = num2;
                    }
                }
            }
            lastCorner = true;
            funnelPath.Add(this.left[count - 1]);
            return(true);
        }
示例#29
0
        void CalculateMeshContour()
        {
            if (mesh == null)
            {
                return;
            }

            edges.Clear();
            pointers.Clear();

            Vector3[] verts = mesh.vertices;
            int[]     tris  = mesh.triangles;
            for (int i = 0; i < tris.Length; i += 3)
            {
                // Make sure it is clockwise
                if (VectorMath.IsClockwiseXZ(verts[tris[i + 0]], verts[tris[i + 1]], verts[tris[i + 2]]))
                {
                    int tmp = tris[i + 0];
                    tris[i + 0] = tris[i + 2];
                    tris[i + 2] = tmp;
                }

                edges[new Int2(tris[i + 0], tris[i + 1])] = i;
                edges[new Int2(tris[i + 1], tris[i + 2])] = i;
                edges[new Int2(tris[i + 2], tris[i + 0])] = i;
            }

            // Construct a list of pointers along all edges
            for (int i = 0; i < tris.Length; i += 3)
            {
                for (int j = 0; j < 3; j++)
                {
                    if (!edges.ContainsKey(new Int2(tris[i + ((j + 1) % 3)], tris[i + ((j + 0) % 3)])))
                    {
                        pointers[tris[i + ((j + 0) % 3)]] = tris[i + ((j + 1) % 3)];
                    }
                }
            }

            var contourBuffer = new List <Vector3[]>();

            List <Vector3> buffer = Pathfinding.Util.ListPool <Vector3> .Claim();

            // Follow edge pointers to generate the contours
            for (int i = 0; i < verts.Length; i++)
            {
                if (pointers.ContainsKey(i))
                {
                    buffer.Clear();

                    int s = i;
                    do
                    {
                        int tmp = pointers[s];

                        //This path has been taken before
                        if (tmp == -1)
                        {
                            break;
                        }

                        pointers[s] = -1;
                        buffer.Add(verts[s]);
                        s = tmp;

                        if (s == -1)
                        {
                            Debug.LogError("Invalid Mesh '" + mesh.name + " in " + gameObject.name);
                            break;
                        }
                    } while (s != i);

                    if (buffer.Count > 0)
                    {
                        contourBuffer.Add(buffer.ToArray());
                    }
                }
            }

            // Return lists to the pool
            Pathfinding.Util.ListPool <Vector3> .Release(ref buffer);

            contours = contourBuffer.ToArray();
        }
示例#30
0
 public static bool IsClockwiseOrColinear(Int2 a, Int2 b, Int2 c)
 {
     return(VectorMath.RightOrColinear(a, b, c));
 }