/// <summary> /// Calculate the shortest path through the funnel. /// /// If the unwrap option is disabled the funnel will simply be projected onto the XZ plane. /// If the unwrap option is enabled then the funnel may be oriented arbitrarily and may have twists and bends. /// This makes it possible to support the funnel algorithm in XY space as well as in more complicated cases, such /// as on curved worlds. /// [Open online documentation to see images] /// /// [Open online documentation to see images] /// /// See: Unwrap /// </summary> /// <param name="funnel">The portals of the funnel. The first and last vertices portals must be single points (so for example left[0] == right[0]).</param> /// <param name="unwrap">Determines if twists and bends should be straightened out before running the funnel algorithm.</param> /// <param name="splitAtEveryPortal">If true, then a vertex will be inserted every time the path crosses a portal /// instead of only at the corners of the path. The result will have exactly one vertex per portal if this is enabled. /// This may introduce vertices with the same position in the output (esp. in corners where many portals meet).</param> public static List <Vector3> Calculate(FunnelPortals funnel, bool unwrap, bool splitAtEveryPortal) { if (funnel.left.Count != funnel.right.Count) { throw new System.ArgumentException("funnel.left.Count != funnel.right.Count"); } // Get arrays at least as large as the number of portals var leftArr = ArrayPool <Vector2> .Claim(funnel.left.Count); var rightArr = ArrayPool <Vector2> .Claim(funnel.left.Count); if (unwrap) { Unwrap(funnel, leftArr, rightArr); } else { // Copy to arrays for (int i = 0; i < funnel.left.Count; i++) { leftArr[i] = ToXZ(funnel.left[i]); rightArr[i] = ToXZ(funnel.right[i]); } } int startIndex = FixFunnel(leftArr, rightArr, funnel.left.Count); var intermediateResult = ListPool <int> .Claim(); if (startIndex == -1) { // If funnel algorithm failed, fall back to a simple line intermediateResult.Add(0); intermediateResult.Add(funnel.left.Count - 1); } else { bool lastCorner; Calculate(leftArr, rightArr, funnel.left.Count, startIndex, intermediateResult, int.MaxValue, out lastCorner); } // Get list for the final result var result = ListPool <Vector3> .Claim(intermediateResult.Count); Vector2 prev2D = leftArr[0]; var prevIdx = 0; for (int i = 0; i < intermediateResult.Count; i++) { var idx = intermediateResult[i]; if (splitAtEveryPortal) { // Check intersections with every portal segment var next2D = idx >= 0 ? leftArr[idx] : rightArr[-idx]; for (int j = prevIdx + 1; j < System.Math.Abs(idx); j++) { var factor = VectorMath.LineIntersectionFactorXZ(FromXZ(leftArr[j]), FromXZ(rightArr[j]), FromXZ(prev2D), FromXZ(next2D)); result.Add(Vector3.Lerp(funnel.left[j], funnel.right[j], factor)); } prevIdx = Mathf.Abs(idx); prev2D = next2D; } if (idx >= 0) { result.Add(funnel.left[idx]); } else { result.Add(funnel.right[-idx]); } } // Release lists back to the pool ListPool <int> .Release(ref intermediateResult); ArrayPool <Vector2> .Release(ref leftArr); ArrayPool <Vector2> .Release(ref rightArr); return(result); }
public static float IntersectionFactor(Vector3 start1, Vector3 end1, Vector3 start2, Vector3 end2) { return(VectorMath.LineIntersectionFactorXZ(start1, end1, start2, end2)); }
public static bool IntersectionFactor(Vector3 start1, Vector3 end1, Vector3 start2, Vector3 end2, out float factor1, out float factor2) { return(VectorMath.LineIntersectionFactorXZ(start1, end1, start2, end2, out factor1, out factor2)); }
public static List <Vector3> Calculate(Funnel.FunnelPortals funnel, bool unwrap, bool splitAtEveryPortal) { Vector2[] array = new Vector2[funnel.left.Count]; Vector2[] array2 = new Vector2[funnel.left.Count]; if (unwrap) { Funnel.Unwrap(funnel, array, array2); } else { for (int i = 0; i < array.Length; i++) { array[i] = Funnel.ToXZ(funnel.left[i]); array2[i] = Funnel.ToXZ(funnel.right[i]); } } Vector2[] array3 = array; int num = Funnel.FixFunnel(ref array, ref array2); List <Vector3> list = funnel.left; List <Vector3> list2 = funnel.right; if (array3 != array) { list = funnel.right; list2 = funnel.left; } List <int> list3 = ListPool <int> .Claim(); if (num == -1) { list3.Add(0); list3.Add(funnel.left.Count - 1); } else { bool flag; Funnel.Calculate(array, array2, num, list3, int.MaxValue, out flag); } List <Vector3> list4 = ListPool <Vector3> .Claim(list3.Count); Vector2 p = array[0]; int num2 = 0; for (int j = 0; j < list3.Count; j++) { int num3 = list3[j]; if (splitAtEveryPortal) { Vector2 vector = (num3 >= 0) ? array[num3] : array2[-num3]; for (int k = num2 + 1; k < Math.Abs(num3); k++) { float t = VectorMath.LineIntersectionFactorXZ(Funnel.FromXZ(array[k]), Funnel.FromXZ(array2[k]), Funnel.FromXZ(p), Funnel.FromXZ(vector)); list4.Add(Vector3.Lerp(list[k], list2[k], t)); } num2 = Mathf.Abs(num3); p = vector; } if (num3 >= 0) { list4.Add(list[num3]); } else { list4.Add(list2[-num3]); } } ListPool <Vector3> .Release(funnel.left); ListPool <Vector3> .Release(funnel.right); ListPool <int> .Release(list3); return(list4); }
/** Calculate the shortest path through the funnel. * \param funnel The portals of the funnel. The first and last vertices portals must be single points (so for example left[0] == right[0]). * \param unwrap Determines if twists and bends should be straightened out before running the funnel algorithm. * \param splitAtEveryPortal If true, then a vertex will be inserted every time the path crosses a portal * instead of only at the corners of the path. The result will have exactly one vertex per portal if this is enabled. * This may introduce vertices with the same position in the output (esp. in corners where many portals meet). * * If the unwrap option is disabled the funnel will simply be projected onto the XZ plane. * If the unwrap option is enabled then the funnel may be oriented arbitrarily and may have twists and bends. * This makes it possible to support the funnel algorithm in XY space as well as in more complicated cases, such * as on curved worlds. * \shadowimage{funnel_unwrap_illustration.png} * * \shadowimage{funnel_split_at_every_portal.png} * * \see Unwrap */ public static List <Vector3> Calculate(FunnelPortals funnel, bool unwrap, bool splitAtEveryPortal) { var leftArr = new Vector2[funnel.left.Count]; var rightArr = new Vector2[funnel.left.Count]; if (unwrap) { Unwrap(funnel, leftArr, rightArr); } else { // Copy to arrays for (int i = 0; i < leftArr.Length; i++) { leftArr[i] = ToXZ(funnel.left[i]); rightArr[i] = ToXZ(funnel.right[i]); } } var origLeft = leftArr; int startIndex = FixFunnel(ref leftArr, ref rightArr); var left3D = funnel.left; var right3D = funnel.right; if (origLeft != leftArr) { // Flipped order left3D = funnel.right; right3D = funnel.left; } var intermediateResult = ListPool <int> .Claim(); if (startIndex == -1) { // If funnel algorithm failed, degrade to simple line intermediateResult.Add(0); intermediateResult.Add(funnel.left.Count - 1); } else { bool lastCorner; Calculate(leftArr, rightArr, startIndex, intermediateResult, int.MaxValue, out lastCorner); } // Get list for the final result var result = ListPool <Vector3> .Claim(intermediateResult.Count); Vector2 prev2D = leftArr[0]; var prevIdx = 0; for (int i = 0; i < intermediateResult.Count; i++) { var idx = intermediateResult[i]; if (splitAtEveryPortal) { // Check intersections with every portal segment var next2D = idx >= 0 ? leftArr[idx] : rightArr[-idx]; for (int j = prevIdx + 1; j < System.Math.Abs(idx); j++) { var factor = VectorMath.LineIntersectionFactorXZ(FromXZ(leftArr[j]), FromXZ(rightArr[j]), FromXZ(prev2D), FromXZ(next2D)); result.Add(Vector3.Lerp(left3D[j], right3D[j], factor)); } prevIdx = Mathf.Abs(idx); prev2D = next2D; } if (idx >= 0) { result.Add(left3D[idx]); } else { result.Add(right3D[-idx]); } } // Release lists back to the pool ListPool <Vector3> .Release(funnel.left); ListPool <Vector3> .Release(funnel.right); ListPool <int> .Release(intermediateResult); return(result); }
/** 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; } }
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; for (;;) { 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); }
// Token: 0x06002797 RID: 10135 RVA: 0x001B2B78 File Offset: 0x001B0D78 public static List <Vector3> Calculate(Funnel.FunnelPortals funnel, bool unwrap, bool splitAtEveryPortal) { if (funnel.left.Count != funnel.right.Count) { throw new ArgumentException("funnel.left.Count != funnel.right.Count"); } Vector2[] array = ArrayPool <Vector2> .Claim(funnel.left.Count); Vector2[] array2 = ArrayPool <Vector2> .Claim(funnel.left.Count); if (unwrap) { Funnel.Unwrap(funnel, array, array2); } else { for (int i = 0; i < funnel.left.Count; i++) { array[i] = Funnel.ToXZ(funnel.left[i]); array2[i] = Funnel.ToXZ(funnel.right[i]); } } int num = Funnel.FixFunnel(array, array2, funnel.left.Count); List <int> list = ListPool <int> .Claim(); if (num == -1) { list.Add(0); list.Add(funnel.left.Count - 1); } else { bool flag; Funnel.Calculate(array, array2, funnel.left.Count, num, list, int.MaxValue, out flag); } List <Vector3> list2 = ListPool <Vector3> .Claim(list.Count); Vector2 p = array[0]; int num2 = 0; for (int j = 0; j < list.Count; j++) { int num3 = list[j]; if (splitAtEveryPortal) { Vector2 vector = (num3 >= 0) ? array[num3] : array2[-num3]; for (int k = num2 + 1; k < Math.Abs(num3); k++) { float t = VectorMath.LineIntersectionFactorXZ(Funnel.FromXZ(array[k]), Funnel.FromXZ(array2[k]), Funnel.FromXZ(p), Funnel.FromXZ(vector)); list2.Add(Vector3.Lerp(funnel.left[k], funnel.right[k], t)); } num2 = Mathf.Abs(num3); p = vector; } if (num3 >= 0) { list2.Add(funnel.left[num3]); } else { list2.Add(funnel.right[-num3]); } } ListPool <int> .Release(ref list); ArrayPool <Vector2> .Release(ref array, false); ArrayPool <Vector2> .Release(ref array2, false); return(list2); }