public static bool IsClockwiseXZ(Int3 a, Int3 b, Int3 c)
{
return(VectorMath.RightXZ(a, b, c));
}
public static bool LeftNotColinear(Vector3 a, Vector3 b, Vector3 p)
{
return(VectorMath.RightXZ(a, b, p));
}
/** 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 LeftNotColinear(Int3 a, Int3 b, Int3 p)
{
return(VectorMath.RightXZ(a, b, p));
}
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);
}
