/// <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);
}
