Ejemplo n.º 1
0
 public static void SimplifyPath2(IRaycastableGraph rcg, List<GraphNode> nodes, int start, int end, List<GraphNode> result, Vector3 startPoint, Vector3 endPoint)
 {
     int count = result.Count;
     if (end <= start + 1)
     {
         result.Add(nodes[start]);
         result.Add(nodes[end]);
         return;
     }
     GraphHitInfo graphHitInfo;
     if (rcg.Linecast(startPoint, endPoint, nodes[start], out graphHitInfo, result) || result[result.Count - 1] != nodes[end])
     {
         result.RemoveRange(count, result.Count - count);
         int num = -1;
         float num2 = float.PositiveInfinity;
         for (int i = start + 1; i < end; i++)
         {
             float num3 = AstarMath.DistancePointSegmentStrict(startPoint, endPoint, (Vector3)nodes[i].position);
             if (num == -1 || num3 < num2)
             {
                 num = i;
                 num2 = num3;
             }
         }
         RichFunnel.SimplifyPath2(rcg, nodes, start, num, result, startPoint, (Vector3)nodes[num].position);
         result.RemoveAt(result.Count - 1);
         RichFunnel.SimplifyPath2(rcg, nodes, num, end, result, (Vector3)nodes[num].position, endPoint);
     }
 }
Ejemplo n.º 2
0
        // Token: 0x060007EF RID: 2031 RVA: 0x0004C030 File Offset: 0x0004A430
        public static void GetPointsAroundPointWorld(Vector3 p, IRaycastableGraph g, List <Vector3> previousPoints, float radius, float clearanceRadius)
        {
            if (previousPoints.Count == 0)
            {
                return;
            }
            Vector3 vector = Vector3.zero;

            for (int i = 0; i < previousPoints.Count; i++)
            {
                vector += previousPoints[i];
            }
            vector /= (float)previousPoints.Count;
            for (int j = 0; j < previousPoints.Count; j++)
            {
                int index;
                previousPoints[index = j] = previousPoints[index] - vector;
            }
            PathUtilities.GetPointsAroundPoint(p, g, previousPoints, radius, clearanceRadius);
        }
Ejemplo n.º 3
0
 public bool ValidateLine(GraphNode n1, GraphNode n2, Vector3 v1, Vector3 v2)
 {
     if (this.useRaycasting)
     {
         if (this.thickRaycast && (this.thickRaycastRadius > 0f))
         {
             Vector3 vector = v2 - v1;
             if (Physics.SphereCast(new Ray(v1 + this.raycastOffset, v2 - v1), this.thickRaycastRadius, vector.magnitude, (int)this.mask))
             {
                 return(false);
             }
         }
         else if (Physics.Linecast(v1 + this.raycastOffset, v2 + this.raycastOffset, (int)this.mask))
         {
             return(false);
         }
     }
     if (this.useGraphRaycasting && (n1 == null))
     {
         n1 = AstarPath.active.GetNearest(v1).node;
         n2 = AstarPath.active.GetNearest(v2).node;
     }
     if ((this.useGraphRaycasting && (n1 != null)) && (n2 != null))
     {
         NavGraph graph  = AstarData.GetGraph(n1);
         NavGraph graph2 = AstarData.GetGraph(n2);
         if (graph != graph2)
         {
             return(false);
         }
         if (graph != null)
         {
             IRaycastableGraph graph3 = graph as IRaycastableGraph;
             if (graph3 != null)
             {
                 return(!graph3.Linecast(v1, v2, n1));
             }
         }
     }
     return(true);
 }
Ejemplo n.º 4
0
 // Token: 0x06002440 RID: 9280 RVA: 0x00196CB8 File Offset: 0x00194EB8
 private void LateUpdate()
 {
     if (this.prevNode == null)
     {
         NNInfo nearest = AstarPath.active.GetNearest(base.transform.position);
         this.prevNode = nearest.node;
         this.prevPos  = base.transform.position;
     }
     if (this.prevNode == null)
     {
         return;
     }
     if (this.prevNode != null)
     {
         IRaycastableGraph raycastableGraph = AstarData.GetGraph(this.prevNode) as IRaycastableGraph;
         if (raycastableGraph != null)
         {
             GraphHitInfo graphHitInfo;
             if (raycastableGraph.Linecast(this.prevPos, base.transform.position, this.prevNode, out graphHitInfo))
             {
                 graphHitInfo.point.y = base.transform.position.y;
                 Vector3 vector  = VectorMath.ClosestPointOnLine(graphHitInfo.tangentOrigin, graphHitInfo.tangentOrigin + graphHitInfo.tangent, base.transform.position);
                 Vector3 vector2 = graphHitInfo.point;
                 vector2 += Vector3.ClampMagnitude((Vector3)graphHitInfo.node.position - vector2, 0.008f);
                 if (raycastableGraph.Linecast(vector2, vector, graphHitInfo.node, out graphHitInfo))
                 {
                     graphHitInfo.point.y    = base.transform.position.y;
                     base.transform.position = graphHitInfo.point;
                 }
                 else
                 {
                     vector.y = base.transform.position.y;
                     base.transform.position = vector;
                 }
             }
             this.prevNode = graphHitInfo.node;
         }
     }
     this.prevPos = base.transform.position;
 }
Ejemplo n.º 5
0
		public bool ValidateLine(GraphNode n1, GraphNode n2, Vector3 v1, Vector3 v2)
		{
			if (this.useRaycasting)
			{
				if (this.thickRaycast && this.thickRaycastRadius > 0f)
				{
					if (Physics.SphereCast(new Ray(v1 + this.raycastOffset, v2 - v1), this.thickRaycastRadius, (v2 - v1).magnitude, this.mask))
					{
						return false;
					}
				}
				else if (Physics.Linecast(v1 + this.raycastOffset, v2 + this.raycastOffset, this.mask))
				{
					return false;
				}
			}
			if (this.useGraphRaycasting && n1 == null)
			{
				n1 = AstarPath.active.GetNearest(v1).node;
				n2 = AstarPath.active.GetNearest(v2).node;
			}
			if (this.useGraphRaycasting && n1 != null && n2 != null)
			{
				NavGraph graph = AstarData.GetGraph(n1);
				NavGraph graph2 = AstarData.GetGraph(n2);
				if (graph != graph2)
				{
					return false;
				}
				if (graph != null)
				{
					IRaycastableGraph raycastableGraph = graph as IRaycastableGraph;
					if (raycastableGraph != null)
					{
						return !raycastableGraph.Linecast(v1, v2, n1);
					}
				}
			}
			return true;
		}
Ejemplo n.º 6
0
        /** Will calculate a number of points around \a p which are on the graph and are separated by \a clearance from each other.
         * This is like GetPointsAroundPoint except that \a previousPoints are treated as being in world space.
         * The average of the points will be found and then that will be treated as the group center.
         */
        public static void GetPointsAroundPointWorld(Vector3 p, IRaycastableGraph g, List <Vector3> previousPoints, float radius, float clearanceRadius)
        {
            if (previousPoints.Count == 0)
            {
                return;
            }

            Vector3 avg = Vector3.zero;

            for (int i = 0; i < previousPoints.Count; i++)
            {
                avg += previousPoints[i];
            }
            avg /= previousPoints.Count;

            for (int i = 0; i < previousPoints.Count; i++)
            {
                previousPoints[i] -= avg;
            }

            GetPointsAroundPoint(p, g, previousPoints, radius, clearanceRadius);
        }
Ejemplo n.º 7
0
    public Vector3 GetClampedPoint(Vector3 from, Vector3 to, Node hint)
    {
        //float minDistance = Mathf.Infinity;
        Vector3 minPoint = to;

        if (useRaycasting)
        {
            RaycastHit hit;
            if (Physics.Linecast(from, to, out hit, mask))
            {
                minPoint = hit.point;
                //minDistance = hit.distance;
            }
        }

        if (useGraphRaycasting && hint != null)
        {
            NavGraph graph = AstarData.GetGraph(hint);

            if (graph != null)
            {
                IRaycastableGraph rayGraph = graph as IRaycastableGraph;

                if (rayGraph != null)
                {
                    GraphHitInfo hit;

                    if (rayGraph.Linecast(from, minPoint, hint, out hit))
                    {
                        //if ((hit.point-from).magnitude < minDistance) {
                        minPoint = hit.point;
                        //}
                    }
                }
            }
        }

        return(minPoint);
    }
Ejemplo n.º 8
0
 private void LateUpdate()
 {
     if (this.prevNode == null)
     {
         NNInfo nearest = AstarPath.active.GetNearest(base.transform.position);
         this.prevNode = nearest.node;
         this.prevPos  = base.transform.position;
     }
     if (this.prevNode != null)
     {
         if (this.prevNode != null)
         {
             IRaycastableGraph graph = AstarData.GetGraph(this.prevNode) as IRaycastableGraph;
             if (graph != null)
             {
                 GraphHitInfo info2;
                 if (graph.Linecast(this.prevPos, base.transform.position, this.prevNode, out info2))
                 {
                     info2.point.y = base.transform.position.y;
                     Vector3 end   = VectorMath.ClosestPointOnLine(info2.tangentOrigin, info2.tangentOrigin + info2.tangent, base.transform.position);
                     Vector3 point = info2.point;
                     point += Vector3.ClampMagnitude(((Vector3)info2.node.position) - point, 0.008f);
                     if (graph.Linecast(point, end, info2.node, out info2))
                     {
                         info2.point.y           = base.transform.position.y;
                         base.transform.position = info2.point;
                     }
                     else
                     {
                         end.y = base.transform.position.y;
                         base.transform.position = end;
                     }
                 }
                 this.prevNode = info2.node;
             }
         }
         this.prevPos = base.transform.position;
     }
 }
Ejemplo n.º 9
0
        public static void SimplifyPath2(IRaycastableGraph rcg, List <GraphNode> nodes, int start, int end, List <GraphNode> result, Vector3 startPoint, Vector3 endPoint)
        {
            int resCount = result.Count;

            if (end <= start + 1)
            {
                result.Add(nodes[start]);
                result.Add(nodes[end]);
                //t--;
                return;
            }

            GraphHitInfo hit;

            if ((rcg.Linecast(startPoint, endPoint, nodes[start], out hit, result) || result[result.Count - 1] != nodes[end]))
            {
                //Obstacle
                //Refine further
                result.RemoveRange(resCount, result.Count - resCount);

                int   minDistNode = -1;
                float minDist     = float.PositiveInfinity;
                for (int i = start + 1; i < end; i++)
                {
                    float dist = VectorMath.SqrDistancePointSegment(startPoint, endPoint, (Vector3)nodes[i].position);
                    if (minDistNode == -1 || dist < minDist)
                    {
                        minDistNode = i;
                        minDist     = dist;
                    }
                }

                SimplifyPath2(rcg, nodes, start, minDistNode, result, startPoint, (Vector3)nodes[minDistNode].position);
                //Remove start node of next part so that it is not added twice
                result.RemoveAt(result.Count - 1);
                SimplifyPath2(rcg, nodes, minDistNode, end, result, (Vector3)nodes[minDistNode].position, endPoint);
            }
        }
Ejemplo n.º 10
0
    // will return false if any part of the line is off the nav mesh
    // if 'clampFromInNavMesh' is true, we make sure the from position is on the nav mesh before doing checks
    // (done in 2d, does not currently support nav meshes on top of one and other)
    public static bool IsVisibleOnRecastGraph(Vector3 from, Vector3 to, bool clampFromInNavMesh = false, NNInfo?nearestInfo = null)
    {
        if (null == AstarPath.active)
        {
            return(false);
        }

        NNInfo fromInfo = nearestInfo ?? AstarPath.active.GetNearest(from);

        if (null == fromInfo.node)
        {
            return(false);
        }

        if (clampFromInNavMesh)
        {
            from = CalculatePointOnRecastGraph(from, 0.1f, fromInfo);
        }

        NavGraph graph = AstarData.GetGraph(fromInfo.node);

        if (graph != null)
        {
            IRaycastableGraph rayGraph = graph as IRaycastableGraph;
            if (rayGraph != null)
            {
                GraphHitInfo hit;
                if (rayGraph.Linecast(from, to, fromInfo.node, out hit))
                {
                    return(false); // hit an obstacle
                }
                return(true);      // no nav mesh exit
            }
            return(false);         // no recast graph
        }
        return(false);             // no nav mesh
    }
        // Token: 0x060006F5 RID: 1781 RVA: 0x000444A0 File Offset: 0x000428A0
        public Vector3 GetClampedPoint(Vector3 from, Vector3 to, GraphNode hint)
        {
            Vector3    vector = to;
            RaycastHit raycastHit;

            if (this.useRaycasting && Physics.Linecast(from, to, out raycastHit, this.mask))
            {
                vector = raycastHit.point;
            }
            if (this.useGraphRaycasting && hint != null)
            {
                NavGraph graph = AstarData.GetGraph(hint);
                if (graph != null)
                {
                    IRaycastableGraph raycastableGraph = graph as IRaycastableGraph;
                    GraphHitInfo      graphHitInfo;
                    if (raycastableGraph != null && raycastableGraph.Linecast(from, vector, hint, out graphHitInfo))
                    {
                        vector = graphHitInfo.point;
                    }
                }
            }
            return(vector);
        }
Ejemplo n.º 12
0
 public static void SimplifyPath3(IRaycastableGraph rcg, List<GraphNode> nodes, int start, int end, List<GraphNode> result, Vector3 startPoint, Vector3 endPoint, int depth = 0)
 {
     if (start == end)
     {
         result.Add(nodes[start]);
         return;
     }
     if (start + 1 == end)
     {
         result.Add(nodes[start]);
         result.Add(nodes[end]);
         return;
     }
     int count = result.Count;
     GraphHitInfo graphHitInfo;
     if (rcg.Linecast(startPoint, endPoint, nodes[start], out graphHitInfo, result) || result[result.Count - 1] != nodes[end])
     {
         result.RemoveRange(count, result.Count - count);
         int num = 0;
         float num2 = 0f;
         for (int i = start + 1; i < end - 1; i++)
         {
             float num3 = AstarMath.DistancePointSegmentStrict(startPoint, endPoint, (Vector3)nodes[i].position);
             if (num3 > num2)
             {
                 num = i;
                 num2 = num3;
             }
         }
         int num4 = (num + start) / 2;
         int num5 = (num + end) / 2;
         if (num4 == num5)
         {
             RichFunnel.SimplifyPath3(rcg, nodes, start, num4, result, startPoint, (Vector3)nodes[num4].position, 0);
             result.RemoveAt(result.Count - 1);
             RichFunnel.SimplifyPath3(rcg, nodes, num4, end, result, (Vector3)nodes[num4].position, endPoint, depth + 1);
         }
         else
         {
             RichFunnel.SimplifyPath3(rcg, nodes, start, num4, result, startPoint, (Vector3)nodes[num4].position, depth + 1);
             result.RemoveAt(result.Count - 1);
             RichFunnel.SimplifyPath3(rcg, nodes, num4, num5, result, (Vector3)nodes[num4].position, (Vector3)nodes[num5].position, depth + 1);
             result.RemoveAt(result.Count - 1);
             RichFunnel.SimplifyPath3(rcg, nodes, num5, end, result, (Vector3)nodes[num5].position, endPoint, depth + 1);
         }
     }
 }
Ejemplo n.º 13
0
		/** Simplifies a funnel path using linecasting.
		 * Running time is roughly O(n^2 log n) in the worst case (where n = end-start)
		 * Actually it depends on how the graph looks, so in theory the actual upper limit on the worst case running time is O(n*m log n) (where n = end-start and m = nodes in the graph)
		 * but O(n^2 log n) is a much more realistic worst case limit.
		 *
		 * Requires #graph to implement IRaycastableGraph
		 */
		public void SimplifyPath (IRaycastableGraph graph, List<GraphNode> nodes, int start, int end, List<GraphNode> result, Vector3 startPoint, Vector3 endPoint) {

			if (graph == null) throw new System.ArgumentNullException ("graph");

			if (start > end) {
				throw new System.ArgumentException ("start >= end");
			}

			int ostart = start;

			int count = 0;
			while (true) {

				if (count++ > 1000) {
					Debug.LogError("!!!");
					break;
				}

				if (start == end) {
					result.Add (nodes[end]);
					return;
				}

				int resCount = result.Count;

				int mx = end+1;
				int mn = start+1;
				bool anySucceded = false;
				while (mx > mn+1) {

					int mid = (mx+mn)/2;

					GraphHitInfo hit;
					Vector3 sp = start == ostart ? startPoint : (Vector3)nodes[start].position;
					Vector3 ep = mid == end ? endPoint : (Vector3)nodes[mid].position;

					if (graph.Linecast (sp, ep, nodes[start], out hit)) {
						mx = mid;
					} else {
						anySucceded = true;
						mn = mid;
					}
				}

				if (!anySucceded) {
					result.Add(nodes[start]);

					//It is guaranteed that mn = start+1
					start = mn;
				} else {

					//Need to redo the linecast to get the trace
					GraphHitInfo hit;
					Vector3 sp = start == ostart ? startPoint : (Vector3)nodes[start].position;
					Vector3 ep = mn == end ? endPoint : (Vector3)nodes[mn].position;
					graph.Linecast (sp, ep, nodes[start], out hit, result);

					long penaltySum = 0;
					long penaltySum2 = 0;
					for (int i=start;i<=mn;i++) {
						penaltySum += nodes[i].Penalty + (path.seeker != null ? path.seeker.tagPenalties[nodes[i].Tag] : 0);
					}

					for (int i=resCount;i<result.Count;i++) {
						penaltySum2 += result[i].Penalty + (path.seeker != null ? path.seeker.tagPenalties[result[i].Tag] : 0);
					}

					// Allow 40% more penalty on average per node
					if ((penaltySum*1.4*(mn-start+1)) < (penaltySum2*(result.Count-resCount)) || result[result.Count-1] != nodes[mn]) {
						//Debug.Log ((penaltySum*1.4*(mn-start+1)) + " < "+ (penaltySum2*(result.Count-resCount)));
						//Debug.DrawLine ((Vector3)nodes[start].Position, (Vector3)nodes[mn].Position, Color.red);
						//Linecast hit the wrong node
						result.RemoveRange (resCount, result.Count-resCount);

						result.Add(nodes[start]);
						//Debug.Break();
						start = start+1;
					} else {
						//Debug.Log ("!! " + (penaltySum*1.4*(mn-start+1)) + " < "+ (penaltySum2*(result.Count-resCount)));
						//Debug.DrawLine ((Vector3)nodes[start].Position, (Vector3)nodes[mn].Position, Color.green);
						//Debug.Break ();
						//Remove nodes[end]
						result.RemoveAt(result.Count-1);
						start = mn;
					}
				}
			}
		}
Ejemplo n.º 14
0
		public static void SimplifyPath2 (IRaycastableGraph rcg, List<GraphNode> nodes, int start, int end, List<GraphNode> result, Vector3 startPoint, Vector3 endPoint) {

			int resCount = result.Count;

			if (end <= start+1) {
				result.Add (nodes[start]);
				result.Add (nodes[end]);
				//t--;
				return;
			}

			GraphHitInfo hit;
			if ((rcg.Linecast (startPoint, endPoint, nodes[start], out hit, result) || result[result.Count-1] != nodes[end])) {
				//Obstacle
				//Refine further
				result.RemoveRange(resCount,result.Count-resCount);

				int minDistNode = -1;
				float minDist = float.PositiveInfinity;
				for (int i=start+1;i<end;i++) {
					float dist = AstarMath.DistancePointSegmentStrict (startPoint, endPoint, (Vector3)nodes[i].position);
					if (minDistNode == -1 || dist < minDist) {
						minDistNode = i;
						minDist = dist;
					}
				}

				SimplifyPath2 (rcg, nodes, start, minDistNode, result, startPoint, (Vector3)nodes[minDistNode].position);
				//Remove start node of next part so that it is not added twice
				result.RemoveAt (result.Count-1);
				SimplifyPath2 (rcg, nodes, minDistNode, end, result, (Vector3)nodes[minDistNode].position, endPoint);
			}

		}
Ejemplo n.º 15
0
		public static void SimplifyPath3 (IRaycastableGraph rcg, List<GraphNode> nodes, int start, int end, List<GraphNode> result, Vector3 startPoint, Vector3 endPoint, int depth = 0) {

			if (start == end) {
				result.Add (nodes[start]);
				return;
			}
			if (start+1 == end) {
				result.Add (nodes[start]);
				result.Add (nodes[end]);
				return;
			}

			int resCount = result.Count;

			GraphHitInfo hit;
			bool linecast = rcg.Linecast (startPoint, endPoint, nodes[start], out hit, result);
			if (linecast || result[result.Count-1] != nodes[end]) {

				//Debug.DrawLine (startPoint, endPoint, Color.black);
				//Obstacle
				//Refine further
				result.RemoveRange(resCount,result.Count-resCount);

				int maxDistNode = 0;
				float maxDist = 0;
				for (int i=start+1;i<end-1;i++) {
					float dist = AstarMath.DistancePointSegmentStrict (startPoint, endPoint, (Vector3)nodes[i].position);
					if (dist > maxDist) {
						maxDistNode = i;
						maxDist = dist;
					}
				}

				int mid1 = (maxDistNode+start)/2;
				int mid2 = (maxDistNode+end)/2;

				if (mid1 == mid2) {
					SimplifyPath3 (rcg, nodes, start, mid1, result, startPoint, (Vector3)nodes[mid1].position);
					//Remove start node of next part so that it is not added twice
					result.RemoveAt (result.Count-1);
					SimplifyPath3 (rcg, nodes, mid1, end, result, (Vector3)nodes[mid1].position, endPoint, depth+1);
				} else {
					SimplifyPath3 (rcg, nodes, start, mid1, result, startPoint, (Vector3)nodes[mid1].position,depth+1);

					//Remove start node of next part so that it is not added twice
					result.RemoveAt (result.Count-1);
					SimplifyPath3 (rcg, nodes, mid1, mid2, result, (Vector3)nodes[mid1].position, (Vector3)nodes[mid2].position,depth+1);

					//Remove start node of next part so that it is not added twice
					result.RemoveAt (result.Count-1);
					SimplifyPath3 (rcg, nodes, mid2, end, result, (Vector3)nodes[mid2].position, endPoint,depth+1);
				}
			}
		}
Ejemplo n.º 16
0
        private void SimplifyPath(IRaycastableGraph graph, List <GraphNode> nodes, int start, int end, List <GraphNode> result, Vector3 startPoint, Vector3 endPoint)
        {
            if (graph == null)
            {
                throw new ArgumentNullException("graph");
            }
            if (start > end)
            {
                throw new ArgumentException("start >= end");
            }
            int num  = start;
            int num2 = 0;

            while (num2++ <= 1000)
            {
                if (start == end)
                {
                    result.Add(nodes[end]);
                    return;
                }
                int  count = result.Count;
                int  i     = end + 1;
                int  num3  = start + 1;
                bool flag  = false;
                while (i > num3 + 1)
                {
                    int          num4   = (i + num3) / 2;
                    Vector3      start2 = (start != num) ? ((Vector3)nodes[start].position) : startPoint;
                    Vector3      end2   = (num4 != end) ? ((Vector3)nodes[num4].position) : endPoint;
                    GraphHitInfo graphHitInfo;
                    if (graph.Linecast(start2, end2, nodes[start], out graphHitInfo))
                    {
                        i = num4;
                    }
                    else
                    {
                        flag = true;
                        num3 = num4;
                    }
                }
                if (!flag)
                {
                    result.Add(nodes[start]);
                    start = num3;
                }
                else
                {
                    Vector3      start3 = (start != num) ? ((Vector3)nodes[start].position) : startPoint;
                    Vector3      end3   = (num3 != end) ? ((Vector3)nodes[num3].position) : endPoint;
                    GraphHitInfo graphHitInfo2;
                    graph.Linecast(start3, end3, nodes[start], out graphHitInfo2, result);
                    long num5 = 0L;
                    long num6 = 0L;
                    for (int j = start; j <= num3; j++)
                    {
                        num5 += (long)((ulong)nodes[j].Penalty + (ulong)((long)((!(this.path.seeker != null)) ? 0 : this.path.seeker.tagPenalties[(int)((UIntPtr)nodes[j].Tag)])));
                    }
                    for (int k = count; k < result.Count; k++)
                    {
                        num6 += (long)((ulong)result[k].Penalty + (ulong)((long)((!(this.path.seeker != null)) ? 0 : this.path.seeker.tagPenalties[(int)((UIntPtr)result[k].Tag)])));
                    }
                    if ((double)num5 * 1.4 * (double)(num3 - start + 1) < (double)(num6 * (long)(result.Count - count)) || result[result.Count - 1] != nodes[num3])
                    {
                        result.RemoveRange(count, result.Count - count);
                        result.Add(nodes[start]);
                        start++;
                    }
                    else
                    {
                        result.RemoveAt(result.Count - 1);
                        start = num3;
                    }
                }
            }
            Debug.LogError("Was the path really long or have we got cought in an infinite loop?");
        }
Ejemplo n.º 17
0
        public void BuildFunnelCorridor(List <GraphNode> nodes, int start, int end)
        {
            this.exactStart = (nodes[start] as MeshNode).ClosestPointOnNode(this.exactStart);
            this.exactEnd   = (nodes[end] as MeshNode).ClosestPointOnNode(this.exactEnd);
            this.left.Clear();
            this.right.Clear();
            this.left.Add(this.exactStart);
            this.right.Add(this.exactStart);
            this.nodes.Clear();
            IRaycastableGraph graph = this.graph as IRaycastableGraph;

            if ((graph == null) || (this.funnelSimplificationMode == FunnelSimplification.None))
            {
                if (this.nodes.Capacity < (end - start))
                {
                    this.nodes.Capacity = end - start;
                }
                for (int j = start; j <= end; j++)
                {
                    TriangleMeshNode item = nodes[j] as TriangleMeshNode;
                    if (item != null)
                    {
                        this.nodes.Add(item);
                    }
                }
            }
            else
            {
                List <GraphNode> result = ListPool <GraphNode> .Claim(end - start);

                switch (this.funnelSimplificationMode)
                {
                case FunnelSimplification.Iterative:
                    this.SimplifyPath(graph, nodes, start, end, result, this.exactStart, this.exactEnd);
                    break;

                case FunnelSimplification.RecursiveBinary:
                    SimplifyPath2(graph, nodes, start, end, result, this.exactStart, this.exactEnd);
                    break;

                case FunnelSimplification.RecursiveTrinary:
                    SimplifyPath3(graph, nodes, start, end, result, this.exactStart, this.exactEnd, 0);
                    break;
                }
                if (this.nodes.Capacity < result.Count)
                {
                    this.nodes.Capacity = result.Count;
                }
                for (int k = 0; k < result.Count; k++)
                {
                    TriangleMeshNode node = result[k] as TriangleMeshNode;
                    if (node != null)
                    {
                        this.nodes.Add(node);
                    }
                }
                ListPool <GraphNode> .Release(result);
            }
            for (int i = 0; i < (this.nodes.Count - 1); i++)
            {
                this.nodes[i].GetPortal(this.nodes[i + 1], this.left, this.right, false);
            }
            this.left.Add(this.exactEnd);
            this.right.Add(this.exactEnd);
        }
Ejemplo n.º 18
0
		/** Will calculate a number of points around \a p which are on the graph and are separated by \a clearance from each other.
		 * This is like GetPointsAroundPoint except that \a previousPoints are treated as being in world space.
		 * The average of the points will be found and then that will be treated as the group center.
		 */
		public static void GetPointsAroundPointWorld (Vector3 p, IRaycastableGraph g, List<Vector3> previousPoints, float radius, float clearanceRadius) {
			if ( previousPoints.Count == 0 ) return;

			var avg = Vector3.zero;
			for ( var i = 0; i < previousPoints.Count; i++ ) avg += previousPoints[i];
			avg /= previousPoints.Count;

			for ( var i = 0; i < previousPoints.Count; i++ ) previousPoints[i] -= avg;

			GetPointsAroundPoint ( p, g, previousPoints, radius, clearanceRadius );
		}
Ejemplo n.º 19
0
        /// <summary>
        /// Simplifies a funnel path using linecasting.
        /// Running time is roughly O(n^2 log n) in the worst case (where n = end-start)
        /// Actually it depends on how the graph looks, so in theory the actual upper limit on the worst case running time is O(n*m log n) (where n = end-start and m = nodes in the graph)
        /// but O(n^2 log n) is a much more realistic worst case limit.
        ///
        /// Requires <see cref="graph"/> to implement IRaycastableGraph
        /// </summary>
        public static void Simplify(PathPart part, IRaycastableGraph graph, List <GraphNode> nodes, List <GraphNode> result, int[] tagPenalties, int traversableTags)
        {
            var start      = part.startIndex;
            var end        = part.endIndex;
            var startPoint = part.startPoint;
            var endPoint   = part.endPoint;

            if (graph == null)
            {
                throw new System.ArgumentNullException(nameof(graph));
            }

            if (start > end)
            {
                throw new System.ArgumentException("start > end");
            }

            // Do a straight line of sight check to see if the path can be simplified to a single line
            {
                GraphHitInfo hit;
                if (!graph.Linecast(startPoint, endPoint, nodes[start], out hit) && hit.node == nodes[end])
                {
                    graph.Linecast(startPoint, endPoint, nodes[start], out hit, result);

                    long penaltySum  = 0;
                    long penaltySum2 = 0;
                    for (int i = start; i <= end; i++)
                    {
                        penaltySum += nodes[i].Penalty + tagPenalties[nodes[i].Tag];
                    }

                    bool walkable = true;
                    for (int i = 0; i < result.Count; i++)
                    {
                        penaltySum2 += result[i].Penalty + tagPenalties[result[i].Tag];
                        walkable    &= ((traversableTags >> (int)result[i].Tag) & 1) == 1;
                    }

                    // Allow 40% more penalty on average per node
                    if (!walkable || (penaltySum * 1.4 * result.Count) < (penaltySum2 * (end - start + 1)))
                    {
                        // The straight line penalties are much higher than the original path.
                        // Revert the simplification
                        result.Clear();
                    }
                    else
                    {
                        // The straight line simplification looks good.
                        // We are done here.
                        return;
                    }
                }
            }

            int ostart = start;

            int count = 0;

            while (true)
            {
                if (count++ > 1000)
                {
                    Debug.LogError("Was the path really long or have we got cought in an infinite loop?");
                    break;
                }

                if (start == end)
                {
                    result.Add(nodes[end]);
                    return;
                }

                int resCount = result.Count;

                // Run a binary search to find the furthest node that we have a clear line of sight to
                int  mx          = end + 1;
                int  mn          = start + 1;
                bool anySucceded = false;
                while (mx > mn + 1)
                {
                    int mid = (mx + mn) / 2;

                    GraphHitInfo hit;
                    Vector3      sp = start == ostart ? startPoint : (Vector3)nodes[start].position;
                    Vector3      ep = mid == end ? endPoint : (Vector3)nodes[mid].position;

                    // Check if there is an obstacle between these points, or if there is no obstacle, but we didn't end up at the right node.
                    // The second case can happen for example in buildings with multiple floors.
                    if (graph.Linecast(sp, ep, nodes[start], out hit) || hit.node != nodes[mid])
                    {
                        mx = mid;
                    }
                    else
                    {
                        anySucceded = true;
                        mn          = mid;
                    }
                }

                if (!anySucceded)
                {
                    result.Add(nodes[start]);

                    // It is guaranteed that mn = start+1
                    start = mn;
                }
                else
                {
                    // Replace a part of the path with the straight path to the furthest node we had line of sight to.
                    // Need to redo the linecast to get the trace (i.e. list of nodes along the line of sight).
                    GraphHitInfo hit;
                    Vector3      sp = start == ostart ? startPoint : (Vector3)nodes[start].position;
                    Vector3      ep = mn == end ? endPoint : (Vector3)nodes[mn].position;
                    graph.Linecast(sp, ep, nodes[start], out hit, result);

                    long penaltySum  = 0;
                    long penaltySum2 = 0;
                    for (int i = start; i <= mn; i++)
                    {
                        penaltySum += nodes[i].Penalty + tagPenalties[nodes[i].Tag];
                    }

                    bool walkable = true;
                    for (int i = resCount; i < result.Count; i++)
                    {
                        penaltySum2 += result[i].Penalty + tagPenalties[result[i].Tag];
                        walkable    &= ((traversableTags >> (int)result[i].Tag) & 1) == 1;
                    }

                    // Allow 40% more penalty on average per node
                    if (!walkable || (penaltySum * 1.4 * (result.Count - resCount)) < (penaltySum2 * (mn - start + 1)) || result[result.Count - 1] != nodes[mn])
                    {
                        //Debug.DrawLine ((Vector3)nodes[start].Position, (Vector3)nodes[mn].Position, Color.red);
                        // Linecast hit the wrong node or it is a lot more expensive than the original path
                        result.RemoveRange(resCount, result.Count - resCount);

                        result.Add(nodes[start]);
                        //Debug.Break();
                        start = start + 1;
                    }
                    else
                    {
                        //Debug.DrawLine ((Vector3)nodes[start].Position, (Vector3)nodes[mn].Position, Color.green);
                        //Remove nodes[end]
                        result.RemoveAt(result.Count - 1);
                        start = mn;
                    }
                }
            }
        }
Ejemplo n.º 20
0
        /** Build a funnel corridor from a node list slice.
         * The nodes are assumed to be of type TriangleMeshNode.
         *
         * \param start Start index in the nodes array
         * \param end End index in the nodes array, this index is inclusive
         */
        public void BuildFunnelCorridor(List <GraphNode> nodes, int start, int end)
        {
            //Make sure start and end points are on the correct nodes
            exactStart = (nodes[start] as MeshNode).ClosestPointOnNode(exactStart);
            exactEnd   = (nodes[end] as MeshNode).ClosestPointOnNode(exactEnd);

            left.Clear();
            right.Clear();
            left.Add(exactStart);
            right.Add(exactStart);


            this.nodes.Clear();

            IRaycastableGraph rcg = graph as IRaycastableGraph;

            if (rcg != null && funnelSimplificationMode != FunnelSimplification.None)
            {
                List <GraphNode> tmp = Pathfinding.Util.ListPool <GraphNode> .Claim(end - start);

                switch (funnelSimplificationMode)
                {
                case FunnelSimplification.Iterative:
                    SimplifyPath(rcg, nodes, start, end, tmp, exactStart, exactEnd);
                    break;

                case FunnelSimplification.RecursiveBinary:
                    SimplifyPath2(rcg, nodes, start, end, tmp, exactStart, exactEnd);
                    break;

                case FunnelSimplification.RecursiveTrinary:
                    SimplifyPath3(rcg, nodes, start, end, tmp, exactStart, exactEnd);
                    break;
                }

                if (this.nodes.Capacity < tmp.Count)
                {
                    this.nodes.Capacity = tmp.Count;
                }

                for (int i = 0; i < tmp.Count; i++)
                {
                    //Guaranteed to be TriangleMeshNodes since they are all in the same graph
                    TriangleMeshNode nd = tmp[i] as TriangleMeshNode;
                    if (nd != null)
                    {
                        this.nodes.Add(nd);
                    }
                }

                Pathfinding.Util.ListPool <GraphNode> .Release(tmp);
            }
            else
            {
                if (this.nodes.Capacity < end - start)
                {
                    this.nodes.Capacity = (end - start);
                }
                for (int i = start; i <= end; i++)
                {
                    //Guaranteed to be TriangleMeshNodes since they are all in the same graph
                    TriangleMeshNode nd = nodes[i] as TriangleMeshNode;
                    if (nd != null)
                    {
                        this.nodes.Add(nd);
                    }
                }
            }

            for (int i = 0; i < this.nodes.Count - 1; i++)
            {
                /** \todo should use return value in future versions */
                this.nodes[i].GetPortal(this.nodes[i + 1], left, right, false);
            }

            left.Add(exactEnd);
            right.Add(exactEnd);
        }
Ejemplo n.º 21
0
        public void SimplifyPath(IRaycastableGraph graph, List <GraphNode> nodes, int start, int end, List <GraphNode> result, Vector3 startPoint, Vector3 endPoint)
        {
            if (graph == null)
            {
                throw new ArgumentNullException("graph");
            }
            if (start > end)
            {
                throw new ArgumentException("start >= end");
            }
            int num  = start;
            int num2 = 0;

Label_0028:
            if (num2++ > 0x3e8)
            {
                Debug.LogError("!!!");
            }
            else if (start == end)
            {
                result.Add(nodes[end]);
            }
            else
            {
                int  count = result.Count;
                int  num4  = end + 1;
                int  num5  = start + 1;
                bool flag  = false;
                while (num4 > (num5 + 1))
                {
                    GraphHitInfo info;
                    int          num6    = (num4 + num5) / 2;
                    Vector3      vector  = (start != num) ? ((Vector3)nodes[start].position) : startPoint;
                    Vector3      vector2 = (num6 != end) ? ((Vector3)nodes[num6].position) : endPoint;
                    if (graph.Linecast(vector, vector2, nodes[start], out info))
                    {
                        num4 = num6;
                    }
                    else
                    {
                        flag = true;
                        num5 = num6;
                    }
                }
                if (!flag)
                {
                    result.Add(nodes[start]);
                    start = num5;
                }
                else
                {
                    GraphHitInfo info2;
                    Vector3      vector3 = (start != num) ? ((Vector3)nodes[start].position) : startPoint;
                    Vector3      vector4 = (num5 != end) ? ((Vector3)nodes[num5].position) : endPoint;
                    graph.Linecast(vector3, vector4, nodes[start], out info2, result);
                    long num7 = 0L;
                    long num8 = 0L;
                    for (int i = start; i <= num5; i++)
                    {
                        num7 += nodes[i].Penalty + ((this.path.seeker == null) ? ((long)0) : ((long)this.path.seeker.tagPenalties[nodes[i].Tag]));
                    }
                    for (int j = count; j < result.Count; j++)
                    {
                        num8 += result[j].Penalty + ((this.path.seeker == null) ? ((long)0) : ((long)this.path.seeker.tagPenalties[result[j].Tag]));
                    }
                    if ((((num7 * 1.4) * ((num5 - start) + 1)) < (num8 * (result.Count - count))) || (result[result.Count - 1] != nodes[num5]))
                    {
                        result.RemoveRange(count, result.Count - count);
                        result.Add(nodes[start]);
                        start++;
                    }
                    else
                    {
                        result.RemoveAt(result.Count - 1);
                        start = num5;
                    }
                }
                goto Label_0028;
            }
        }
Ejemplo n.º 22
0
        // Token: 0x060027A4 RID: 10148 RVA: 0x001B33F8 File Offset: 0x001B15F8
        public static void GetPointsAroundPoint(Vector3 center, IRaycastableGraph g, List <Vector3> previousPoints, float radius, float clearanceRadius)
        {
            if (g == null)
            {
                throw new ArgumentNullException("g");
            }
            NavGraph navGraph = g as NavGraph;

            if (navGraph == null)
            {
                throw new ArgumentException("g is not a NavGraph");
            }
            NNInfoInternal nearestForce = navGraph.GetNearestForce(center, NNConstraint.Default);

            center = nearestForce.clampedPosition;
            if (nearestForce.node == null)
            {
                return;
            }
            radius           = Mathf.Max(radius, 1.4142f * clearanceRadius * Mathf.Sqrt((float)previousPoints.Count));
            clearanceRadius *= clearanceRadius;
            int i = 0;

            while (i < previousPoints.Count)
            {
                Vector3 vector    = previousPoints[i];
                float   magnitude = vector.magnitude;
                if (magnitude > 0f)
                {
                    vector /= magnitude;
                }
                float num = radius;
                vector *= num;
                int     num2 = 0;
                Vector3 vector2;
                for (;;)
                {
                    vector2 = center + vector;
                    GraphHitInfo graphHitInfo;
                    if (g.Linecast(center, vector2, nearestForce.node, out graphHitInfo))
                    {
                        if (graphHitInfo.point == Vector3.zero)
                        {
                            num2++;
                            if (num2 > 8)
                            {
                                goto Block_7;
                            }
                        }
                        else
                        {
                            vector2 = graphHitInfo.point;
                        }
                    }
                    bool flag = false;
                    for (float num3 = 0.1f; num3 <= 1f; num3 += 0.05f)
                    {
                        Vector3 vector3 = Vector3.Lerp(center, vector2, num3);
                        flag = true;
                        for (int j = 0; j < i; j++)
                        {
                            if ((previousPoints[j] - vector3).sqrMagnitude < clearanceRadius)
                            {
                                flag = false;
                                break;
                            }
                        }
                        if (flag || num2 > 8)
                        {
                            flag = true;
                            previousPoints[i] = vector3;
                            break;
                        }
                    }
                    if (flag)
                    {
                        break;
                    }
                    clearanceRadius *= 0.9f;
                    vector           = UnityEngine.Random.onUnitSphere * Mathf.Lerp(num, radius, (float)(num2 / 5));
                    vector.y         = 0f;
                    num2++;
                }
IL_19D:
                i++;
                continue;
Block_7:
                previousPoints[i] = vector2;
                goto IL_19D;
            }
        }
Ejemplo n.º 23
0
        public void BuildFunnelCorridor(List <GraphNode> nodes, int start, int end)
        {
            this.exactStart = (nodes[start] as MeshNode).ClosestPointOnNode(this.exactStart);
            this.exactEnd   = (nodes[end] as MeshNode).ClosestPointOnNode(this.exactEnd);
            this.left.Clear();
            this.right.Clear();
            this.left.Add(this.exactStart);
            this.right.Add(this.exactStart);
            this.nodes.Clear();
            IRaycastableGraph raycastableGraph = this.graph as IRaycastableGraph;

            if (raycastableGraph != null && this.funnelSimplificationMode != RichFunnel.FunnelSimplification.None)
            {
                List <GraphNode> list = ListPool <GraphNode> .Claim(end - start);

                switch (this.funnelSimplificationMode)
                {
                case RichFunnel.FunnelSimplification.Iterative:
                    this.SimplifyPath(raycastableGraph, nodes, start, end, list, this.exactStart, this.exactEnd);
                    break;

                case RichFunnel.FunnelSimplification.RecursiveBinary:
                    RichFunnel.SimplifyPath2(raycastableGraph, nodes, start, end, list, this.exactStart, this.exactEnd);
                    break;

                case RichFunnel.FunnelSimplification.RecursiveTrinary:
                    RichFunnel.SimplifyPath3(raycastableGraph, nodes, start, end, list, this.exactStart, this.exactEnd, 0);
                    break;
                }
                if (this.nodes.Capacity < list.Count)
                {
                    this.nodes.Capacity = list.Count;
                }
                for (int i = 0; i < list.Count; i++)
                {
                    TriangleMeshNode triangleMeshNode = list[i] as TriangleMeshNode;
                    if (triangleMeshNode != null)
                    {
                        this.nodes.Add(triangleMeshNode);
                    }
                }
                ListPool <GraphNode> .Release(list);
            }
            else
            {
                if (this.nodes.Capacity < end - start)
                {
                    this.nodes.Capacity = end - start;
                }
                for (int j = start; j <= end; j++)
                {
                    TriangleMeshNode triangleMeshNode2 = nodes[j] as TriangleMeshNode;
                    if (triangleMeshNode2 != null)
                    {
                        this.nodes.Add(triangleMeshNode2);
                    }
                }
            }
            for (int k = 0; k < this.nodes.Count - 1; k++)
            {
                this.nodes[k].GetPortal(this.nodes[k + 1], this.left, this.right, false);
            }
            this.left.Add(this.exactEnd);
            this.right.Add(this.exactEnd);
        }
Ejemplo n.º 24
0
        public static void SimplifyPath3(IRaycastableGraph rcg, List <GraphNode> nodes, int start, int end, List <GraphNode> result, Vector3 startPoint, Vector3 endPoint, int depth = 0)
        {
            if (start == end)
            {
                result.Add(nodes[start]);
                return;
            }
            if (start + 1 == end)
            {
                result.Add(nodes[start]);
                result.Add(nodes[end]);
                return;
            }

            int resCount = result.Count;

            GraphHitInfo hit;
            bool         linecast = rcg.Linecast(startPoint, endPoint, nodes[start], out hit, result);

            if (linecast || result[result.Count - 1] != nodes[end])
            {
                //Debug.DrawLine (startPoint, endPoint, Color.black);
                //Obstacle
                //Refine further
                result.RemoveRange(resCount, result.Count - resCount);

                int   maxDistNode = 0;
                float maxDist     = 0;
                for (int i = start + 1; i < end - 1; i++)
                {
                    float dist = VectorMath.SqrDistancePointSegment(startPoint, endPoint, (Vector3)nodes[i].position);
                    if (dist > maxDist)
                    {
                        maxDistNode = i;
                        maxDist     = dist;
                    }
                }

                int mid1 = (maxDistNode + start) / 2;
                int mid2 = (maxDistNode + end) / 2;

                if (mid1 == mid2)
                {
                    SimplifyPath3(rcg, nodes, start, mid1, result, startPoint, (Vector3)nodes[mid1].position);
                    //Remove start node of next part so that it is not added twice
                    result.RemoveAt(result.Count - 1);
                    SimplifyPath3(rcg, nodes, mid1, end, result, (Vector3)nodes[mid1].position, endPoint, depth + 1);
                }
                else
                {
                    SimplifyPath3(rcg, nodes, start, mid1, result, startPoint, (Vector3)nodes[mid1].position, depth + 1);

                    //Remove start node of next part so that it is not added twice
                    result.RemoveAt(result.Count - 1);
                    SimplifyPath3(rcg, nodes, mid1, mid2, result, (Vector3)nodes[mid1].position, (Vector3)nodes[mid2].position, depth + 1);

                    //Remove start node of next part so that it is not added twice
                    result.RemoveAt(result.Count - 1);
                    SimplifyPath3(rcg, nodes, mid2, end, result, (Vector3)nodes[mid2].position, endPoint, depth + 1);
                }
            }
        }
Ejemplo n.º 25
0
        /** Will calculate a number of points around \a center which are on the graph and are separated by \a clearance from each other.
         * The maximum distance from \a center to any point will be \a radius.
         * Points will first be tried to be laid out as \a previousPoints and if that fails, random points will be selected.
         * This is great if you want to pick a number of target points for group movement. If you pass all current agent points from e.g the group's average position
         * this method will return target points so that the units move very little within the group, this is often aesthetically pleasing and reduces jitter if using
         * some kind of local avoidance.
         *
         * \param center The point to generate points around
         * \param g The graph to use for linecasting. If you are only using one graph, you can get this by AstarPath.active.graphs[0] as IRaycastableGraph.
         * Note that not all graphs are raycastable, recast, navmesh and grid graphs are raycastable. On recast and navmesh it works the best.
         * \param previousPoints The points to use for reference. Note that these should not be in world space. They are treated as relative to \a center.
         *      The new points will overwrite the existing points in the list. The result will be in world space, not relative to \a center.
         * \param radius The final points will be at most this distance from \a center.
         * \param clearanceRadius The points will if possible be at least this distance from each other.
         *
         * \todo Write unit tests
         */
        public static void GetPointsAroundPoint(Vector3 center, IRaycastableGraph g, List <Vector3> previousPoints, float radius, float clearanceRadius)
        {
            if (g == null)
            {
                throw new System.ArgumentNullException("g");
            }

            var graph = g as NavGraph;

            if (graph == null)
            {
                throw new System.ArgumentException("g is not a NavGraph");
            }

            NNInfoInternal nn = graph.GetNearestForce(center, NNConstraint.Default);

            center = nn.clampedPosition;

            if (nn.node == null)
            {
                // No valid point to start from
                return;
            }


            // Make sure the enclosing circle has a radius which can pack circles with packing density 0.5
            radius           = Mathf.Max(radius, 1.4142f * clearanceRadius * Mathf.Sqrt(previousPoints.Count)); //Mathf.Sqrt(previousPoints.Count*clearanceRadius*2));
            clearanceRadius *= clearanceRadius;

            for (int i = 0; i < previousPoints.Count; i++)
            {
                Vector3 dir  = previousPoints[i];
                float   magn = dir.magnitude;

                if (magn > 0)
                {
                    dir /= magn;
                }

                float newMagn = radius;                //magn > radius ? radius : magn;
                dir *= newMagn;

                GraphHitInfo hit;

                int tests = 0;
                while (true)
                {
                    Vector3 pt = center + dir;

                    if (g.Linecast(center, pt, nn.node, out hit))
                    {
                        if (hit.point == PF.Vector3.zero)
                        {
                            // Oops, linecast actually failed completely
                            // try again unless we have tried lots of times
                            // then we just continue anyway
                            tests++;
                            if (tests > 8)
                            {
                                previousPoints[i] = pt;
                                break;
                            }
                        }
                        else
                        {
                            pt = hit.point;
                        }
                    }

                    bool worked = false;

                    for (float q = 0.1f; q <= 1.0f; q += 0.05f)
                    {
                        Vector3 qt = Vector3.Lerp(center, pt, q);
                        worked = true;
                        for (int j = 0; j < i; j++)
                        {
                            if ((previousPoints[j] - qt).sqrMagnitude < clearanceRadius)
                            {
                                worked = false;
                                break;
                            }
                        }

                        // Abort after 8 tests or when we have found a valid point
                        if (worked || tests > 8)
                        {
                            worked            = true;
                            previousPoints[i] = qt;
                            break;
                        }
                    }

                    // Break out of nested loop
                    if (worked)
                    {
                        break;
                    }

                    // If we could not find a valid point, reduce the clearance radius slightly to improve
                    // the chances next time
                    clearanceRadius *= 0.9f;
                    // This will pick points in 2D closer to the edge of the circle with a higher probability
                    dir   = Random.onUnitSphere * Mathf.Lerp(newMagn, radius, tests / 5);
                    dir.y = 0;
                    tests++;
                }
            }
        }
Ejemplo n.º 26
0
        /** Simplifies a funnel path using linecasting.
         * Running time is roughly O(n^2 log n) in the worst case (where n = end-start)
         * Actually it depends on how the graph looks, so in theory the actual upper limit on the worst case running time is O(n*m log n) (where n = end-start and m = nodes in the graph)
         * but O(n^2 log n) is a much more realistic worst case limit.
         *
         * Requires #graph to implement IRaycastableGraph
         */
        public void SimplifyPath(IRaycastableGraph graph, List <GraphNode> nodes, int start, int end, List <GraphNode> result, Vector3 startPoint, Vector3 endPoint)
        {
            if (graph == null)
            {
                throw new System.ArgumentNullException("graph");
            }

            if (start > end)
            {
                throw new System.ArgumentException("start >= end");
            }

            int ostart = start;

            int count = 0;

            while (true)
            {
                if (count++ > 1000)
                {
                    Debug.LogError("!!!");
                    break;
                }

                if (start == end)
                {
                    result.Add(nodes[end]);
                    return;
                }

                int resCount = result.Count;

                int  mx          = end + 1;
                int  mn          = start + 1;
                bool anySucceded = false;
                while (mx > mn + 1)
                {
                    int mid = (mx + mn) / 2;

                    GraphHitInfo hit;
                    Vector3      sp = start == ostart ? startPoint : (Vector3)nodes[start].position;
                    Vector3      ep = mid == end ? endPoint : (Vector3)nodes[mid].position;

                    if (graph.Linecast(sp, ep, nodes[start], out hit))
                    {
                        mx = mid;
                    }
                    else
                    {
                        anySucceded = true;
                        mn          = mid;
                    }
                }

                if (!anySucceded)
                {
                    result.Add(nodes[start]);

                    //It is guaranteed that mn = start+1
                    start = mn;
                }
                else
                {
                    //Need to redo the linecast to get the trace
                    GraphHitInfo hit;
                    Vector3      sp = start == ostart ? startPoint : (Vector3)nodes[start].position;
                    Vector3      ep = mn == end ? endPoint : (Vector3)nodes[mn].position;
                    graph.Linecast(sp, ep, nodes[start], out hit, result);

                    long penaltySum  = 0;
                    long penaltySum2 = 0;
                    for (int i = start; i <= mn; i++)
                    {
                        penaltySum += nodes[i].Penalty + (path.seeker != null ? path.seeker.tagPenalties[nodes[i].Tag] : 0);
                    }

                    for (int i = resCount; i < result.Count; i++)
                    {
                        penaltySum2 += result[i].Penalty + (path.seeker != null ? path.seeker.tagPenalties[result[i].Tag] : 0);
                    }

                    // Allow 40% more penalty on average per node
                    if ((penaltySum * 1.4 * (mn - start + 1)) < (penaltySum2 * (result.Count - resCount)) || result[result.Count - 1] != nodes[mn])
                    {
                        //Debug.Log ((penaltySum*1.4*(mn-start+1)) + " < "+ (penaltySum2*(result.Count-resCount)));
                        //Debug.DrawLine ((Vector3)nodes[start].Position, (Vector3)nodes[mn].Position, Color.red);
                        //Linecast hit the wrong node
                        result.RemoveRange(resCount, result.Count - resCount);

                        result.Add(nodes[start]);
                        //Debug.Break();
                        start = start + 1;
                    }
                    else
                    {
                        //Debug.Log ("!! " + (penaltySum*1.4*(mn-start+1)) + " < "+ (penaltySum2*(result.Count-resCount)));
                        //Debug.DrawLine ((Vector3)nodes[start].Position, (Vector3)nodes[mn].Position, Color.green);
                        //Debug.Break ();
                        //Remove nodes[end]
                        result.RemoveAt(result.Count - 1);
                        start = mn;
                    }
                }
            }
        }
Ejemplo n.º 27
0
        public static void GetPointsAroundPoint(Vector3 p, IRaycastableGraph g, List <Vector3> previousPoints, float radius, float clearanceRadius)
        {
            if (g == null)
            {
                throw new ArgumentNullException("g");
            }
            NavGraph navGraph = g as NavGraph;

            if (navGraph == null)
            {
                throw new ArgumentException("g is not a NavGraph");
            }
            NNInfoInternal nearestForce = navGraph.GetNearestForce(p, NNConstraint.Default);

            p = nearestForce.clampedPosition;
            if (nearestForce.node == null)
            {
                return;
            }
            radius           = Mathf.Max(radius, 1.4142f * clearanceRadius * Mathf.Sqrt((float)previousPoints.Count));
            clearanceRadius *= clearanceRadius;
            for (int i = 0; i < previousPoints.Count; i++)
            {
                Vector3 vector    = previousPoints[i];
                float   magnitude = vector.magnitude;
                if (magnitude > 0f)
                {
                    vector /= magnitude;
                }
                float num = radius;
                vector *= num;
                bool flag = false;
                int  num2 = 0;
                do
                {
                    Vector3      vector2 = p + vector;
                    GraphHitInfo graphHitInfo;
                    if (g.Linecast(p, vector2, nearestForce.node, out graphHitInfo))
                    {
                        vector2 = graphHitInfo.point;
                    }
                    for (float num3 = 0.1f; num3 <= 1f; num3 += 0.05f)
                    {
                        Vector3 vector3 = (vector2 - p) * num3 + p;
                        flag = true;
                        for (int j = 0; j < i; j++)
                        {
                            if ((previousPoints[j] - vector3).sqrMagnitude < clearanceRadius)
                            {
                                flag = false;
                                break;
                            }
                        }
                        if (flag)
                        {
                            previousPoints[i] = vector3;
                            break;
                        }
                    }
                    if (!flag)
                    {
                        if (num2 > 8)
                        {
                            flag = true;
                        }
                        else
                        {
                            clearanceRadius *= 0.9f;
                            vector           = UnityEngine.Random.onUnitSphere * Mathf.Lerp(num, radius, (float)(num2 / 5));
                            vector.y         = 0f;
                            num2++;
                        }
                    }
                }while (!flag);
            }
        }
Ejemplo n.º 28
0
		/** Will calculate a number of points around \a p which are on the graph and are separated by \a clearance from each other.
		 * The maximum distance from \a p to any point will be \a radius.
		 * Points will first be tried to be laid out as \a previousPoints and if that fails, random points will be selected.
		 * This is great if you want to pick a number of target points for group movement. If you pass all current agent points from e.g the group's average position
		 * this method will return target points so that the units move very little within the group, this is often aesthetically pleasing and reduces jitter if using
		 * some kind of local avoidance.
		 * 
		 * \param g The graph to use for linecasting. If you are only using one graph, you can get this by AstarPath.active.graphs[0] as IRaycastableGraph.
		 * Note that not all graphs are raycastable, recast, navmesh and grid graphs are raycastable. On recast and navmesh it works the best.
		 * \param previousPoints The points to use for reference. Note that these should not be in world space. They are treated as relative to \a p.
		 */
		public static void GetPointsAroundPoint (Vector3 p, IRaycastableGraph g, List<Vector3> previousPoints, float radius, float clearanceRadius) {
			
			if (g == null) throw new ArgumentNullException ("g");
			
			var graph = g as NavGraph;
			
			if (graph == null) throw new ArgumentException ("g is not a NavGraph");
			
			var nn = graph.GetNearestForce (p, NNConstraint.Default);
			p = nn.clampedPosition;
			
			if (nn.node == null) {
				// No valid point to start from
				return;
			}
			
			
			// Make sure the enclosing circle has a radius which can pack circles with packing density 0.5
			radius = Mathf.Max (radius, 1.4142f*clearanceRadius*Mathf.Sqrt(previousPoints.Count));//Mathf.Sqrt(previousPoints.Count*clearanceRadius*2));
			clearanceRadius *= clearanceRadius;
			
			for (var i=0;i<previousPoints.Count;i++) {
				
				var dir = previousPoints[i];
				var magn = dir.magnitude;
				
				if (magn > 0) dir /= magn;
			
				var newMagn = radius;//magn > radius ? radius : magn;
				dir *= newMagn;
				
				var worked = false;
				
				GraphHitInfo hit;
				
				var tests = 0;
				do {
					
					var pt = p + dir;

					if (g.Linecast (p, pt, nn.node, out hit)) {
						pt = hit.point;
					}
					
					for (var q = 0.1f; q <= 1.0f; q+= 0.05f) {
						var qt = (pt - p)*q + p;
						worked = true;
						for (var j=0;j<i;j++) {
							if ((previousPoints[j] - qt).sqrMagnitude < clearanceRadius) {
								worked = false;
								break;
							}
						}
						
						if (worked) {
							previousPoints[i] = qt;
							break;
						}
					}
					
					if (!worked) {

						// Abort after 8 tries
						if (tests > 8) {
							worked = true;
						} else {
							clearanceRadius *= 0.9f;
							// This will pick points in 2D closer to the edge of the circle with a higher probability
							dir = Random.onUnitSphere * Mathf.Lerp (newMagn, radius, tests / 5);
							dir.y = 0;
							tests++;
						}
					}
				} while (!worked);
			}
			
		}
Ejemplo n.º 29
0
 public void SimplifyPath(IRaycastableGraph graph, List<GraphNode> nodes, int start, int end, List<GraphNode> result, Vector3 startPoint, Vector3 endPoint)
 {
     if (graph == null)
     {
         throw new ArgumentNullException("graph");
     }
     if (start > end)
     {
         throw new ArgumentException("start >= end");
     }
     int num = start;
     int num2 = 0;
     while (num2++ <= 1000)
     {
         if (start == end)
         {
             result.Add(nodes[end]);
             return;
         }
         int count = result.Count;
         int i = end + 1;
         int num3 = start + 1;
         bool flag = false;
         while (i > num3 + 1)
         {
             int num4 = (i + num3) / 2;
             Vector3 start2 = (start != num) ? ((Vector3)nodes[start].position) : startPoint;
             Vector3 end2 = (num4 != end) ? ((Vector3)nodes[num4].position) : endPoint;
             GraphHitInfo graphHitInfo;
             if (graph.Linecast(start2, end2, nodes[start], out graphHitInfo))
             {
                 i = num4;
             }
             else
             {
                 flag = true;
                 num3 = num4;
             }
         }
         if (!flag)
         {
             result.Add(nodes[start]);
             start = num3;
         }
         else
         {
             Vector3 start3 = (start != num) ? ((Vector3)nodes[start].position) : startPoint;
             Vector3 end3 = (num3 != end) ? ((Vector3)nodes[num3].position) : endPoint;
             GraphHitInfo graphHitInfo2;
             graph.Linecast(start3, end3, nodes[start], out graphHitInfo2, result);
             long num5 = 0L;
             long num6 = 0L;
             for (int j = start; j <= num3; j++)
             {
                 num5 += (long)((ulong)nodes[j].Penalty + (ulong)((long)((!(this.path.seeker != null)) ? 0 : this.path.seeker.tagPenalties[(int)((UIntPtr)nodes[j].Tag)])));
             }
             for (int k = count; k < result.Count; k++)
             {
                 num6 += (long)((ulong)result[k].Penalty + (ulong)((long)((!(this.path.seeker != null)) ? 0 : this.path.seeker.tagPenalties[(int)((UIntPtr)result[k].Tag)])));
             }
             if ((double)num5 * 1.4 * (double)(num3 - start + 1) < (double)(num6 * (long)(result.Count - count)) || result[result.Count - 1] != nodes[num3])
             {
                 result.RemoveRange(count, result.Count - count);
                 result.Add(nodes[start]);
                 start++;
             }
             else
             {
                 result.RemoveAt(result.Count - 1);
                 start = num3;
             }
         }
     }
     Debug.LogError("!!!");
 }