public static bool Linecast(INavmesh graph, Vector3 tmp_origin, Vector3 tmp_end, GraphNode hint, out GraphHitInfo hit, List <GraphNode> trace) { Int3 @int = (Int3)tmp_end; Int3 int2 = (Int3)tmp_origin; hit = default(GraphHitInfo); if (float.IsNaN(tmp_origin.x + tmp_origin.y + tmp_origin.z)) { throw new ArgumentException("origin is NaN"); } if (float.IsNaN(tmp_end.x + tmp_end.y + tmp_end.z)) { throw new ArgumentException("end is NaN"); } TriangleMeshNode triangleMeshNode = hint as TriangleMeshNode; if (triangleMeshNode == null) { triangleMeshNode = ((graph as NavGraph).GetNearest(tmp_origin, NNConstraint.None).node as TriangleMeshNode); if (triangleMeshNode == null) { Debug.LogError("Could not find a valid node to start from"); hit.point = tmp_origin; return(true); } } if (int2 == @int) { hit.node = triangleMeshNode; return(false); } int2 = (Int3)triangleMeshNode.ClosestPointOnNode((Vector3)int2); hit.origin = (Vector3)int2; if (!triangleMeshNode.Walkable) { hit.point = (Vector3)int2; hit.tangentOrigin = (Vector3)int2; return(true); } List <Vector3> list = ListPool <Vector3> .Claim(); List <Vector3> list2 = ListPool <Vector3> .Claim(); int num = 0; while (true) { num++; if (num > 2000) { break; } TriangleMeshNode triangleMeshNode2 = null; if (trace != null) { trace.Add(triangleMeshNode); } if (triangleMeshNode.ContainsPoint(@int)) { goto Block_9; } for (int i = 0; i < triangleMeshNode.connections.Length; i++) { if (triangleMeshNode.connections[i].GraphIndex == triangleMeshNode.GraphIndex) { list.Clear(); list2.Clear(); if (triangleMeshNode.GetPortal(triangleMeshNode.connections[i], list, list2, false)) { Vector3 vector = list[0]; Vector3 vector2 = list2[0]; if (Polygon.LeftNotColinear(vector, vector2, hit.origin) || !Polygon.LeftNotColinear(vector, vector2, tmp_end)) { float num2; float num3; if (Polygon.IntersectionFactor(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 (Polygon.LeftNotColinear(vector3, vector4, hit.origin) || !Polygon.LeftNotColinear(vector3, vector4, tmp_end)) { float num4; float num5; if (Polygon.IntersectionFactor(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); }
public static bool Linecast(INavmesh graph, VInt3 tmp_origin, VInt3 tmp_end, GraphNode hint, out GraphHitInfo hit, List<GraphNode> trace) { VInt3 vInt = tmp_end; VInt3 vInt2 = tmp_origin; hit = default(GraphHitInfo); if (float.IsNaN((float)(tmp_origin.x + tmp_origin.y + tmp_origin.z))) { throw new ArgumentException("origin is NaN"); } if (float.IsNaN((float)(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 (vInt2 == vInt) { hit.node = triangleMeshNode; return false; } vInt2 = (VInt3)triangleMeshNode.ClosestPointOnNode((Vector3)vInt2); hit.origin = vInt2; if (!triangleMeshNode.Walkable) { hit.point = vInt2; hit.tangentOrigin = vInt2; return true; } List<VInt3> list = ListPool<VInt3>.Claim(); List<VInt3> list2 = ListPool<VInt3>.Claim(); int num = 0; while (true) { num++; if (num > 2000) { break; } TriangleMeshNode triangleMeshNode2 = null; if (trace != null) { trace.Add(triangleMeshNode); } if (triangleMeshNode.ContainsPoint(vInt)) { 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)) { VInt3 vInt3 = list.get_Item(0); VInt3 vInt4 = list2.get_Item(0); if (Polygon.LeftNotColinear(vInt3, vInt4, hit.origin) || !Polygon.LeftNotColinear(vInt3, vInt4, tmp_end)) { float num2; float num3; if (Polygon.IntersectionFactor(vInt3, vInt4, 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<VInt3>.Release(list); ListPool<VInt3>.Release(list2); return true; Block_9: ListPool<VInt3>.Release(list); ListPool<VInt3>.Release(list2); return false; Block_18: int vertexCount = triangleMeshNode.GetVertexCount(); for (int j = 0; j < vertexCount; j++) { VInt3 vertex = triangleMeshNode.GetVertex(j); VInt3 vertex2 = triangleMeshNode.GetVertex((j + 1) % vertexCount); if (Polygon.LeftNotColinear(vertex, vertex2, hit.origin) || !Polygon.LeftNotColinear(vertex, vertex2, tmp_end)) { VFactor vFactor; VFactor vFactor2; if (Polygon.IntersectionFactor(vertex, vertex2, hit.origin, tmp_end, out vFactor, out vFactor2)) { if (!vFactor2.IsNegative) { if (!vFactor.IsNegative && vFactor.nom / vFactor.den <= 1L) { VInt3 vInt5 = (vertex2 - vertex) * (float)vFactor.nom; vInt5 = IntMath.Divide(vInt5, vFactor.den); vInt5 += vertex; hit.point = vInt5; hit.node = triangleMeshNode; hit.tangent = vertex2 - vertex; hit.tangentOrigin = vertex; ListPool<VInt3>.Release(list); ListPool<VInt3>.Release(list2); return true; } } } } } Debug.LogWarning("Linecast failing because point not inside node, and line does not hit any edges of it"); ListPool<VInt3>.Release(list); ListPool<VInt3>.Release(list2); return false; }
/** 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 Right or colinear if (!Polygon.LeftNotColinear(a, b, hit.origin)) { if (Polygon.LeftNotColinear(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 (Polygon.IntersectionFactor(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 right or colinear if (!Polygon.LeftNotColinear(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 (Polygon.LeftNotColinear(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 (Polygon.IntersectionFactor(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 IsClockwise(Int3 a, Int3 b, Int3 c) { return(Polygon.LeftNotColinear(a, b, c)); }