static int GetHTarget(IntPtr L) { try { ToLua.CheckArgsCount(L, 1); Pathfinding.Path obj = (Pathfinding.Path)ToLua.CheckObject <Pathfinding.Path>(L, 1); Pathfinding.Int3 o = obj.GetHTarget(); ToLua.PushValue(L, o); return(1); } catch (Exception e) { return(LuaDLL.toluaL_exception(L, e)); } }
public static float DistancePointSegment(Int3 a, Int3 b, Int3 p) { float num = (float)(b.x - a.x); float num2 = (float)(b.z - a.z); float num3 = (float)(p.x - a.x); float num4 = (float)(p.z - a.z); float num5 = num * num + num2 * num2; float num6 = num * num3 + num2 * num4; if (num5 > 0f) { num6 /= num5; } if (num6 < 0f) { num6 = 0f; } else if (num6 > 1f) { num6 = 1f; } num3 = (float)a.x + num6 * num - (float)p.x; num4 = (float)a.z + num6 * num2 - (float)p.z; return num3 * num3 + num4 * num4; }
public override bool ContainsPoint (Int3 p) { // Get the object holding the vertex data for this node // This is usually a graph or a recast graph tile INavmeshHolder navmeshHolder = GetNavmeshHolder(GraphIndex); // Get all 3 vertices for this node Int3 a = navmeshHolder.GetVertex(v0); Int3 b = navmeshHolder.GetVertex(v1); Int3 c = navmeshHolder.GetVertex(v2); if ((long)(b.x - a.x) * (long)(p.z - a.z) - (long)(p.x - a.x) * (long)(b.z - a.z) > 0) return false; if ((long)(c.x - b.x) * (long)(p.z - b.z) - (long)(p.x - b.x) * (long)(c.z - b.z) > 0) return false; if ((long)(a.x - c.x) * (long)(p.z - c.z) - (long)(p.x - c.x) * (long)(a.z - c.z) > 0) return false; return true; // Equivalent code, but the above code is faster //return Polygon.IsClockwiseMargin (a,b, p) && Polygon.IsClockwiseMargin (b,c, p) && Polygon.IsClockwiseMargin (c,a, p); //return Polygon.ContainsPoint(g.GetVertex(v0),g.GetVertex(v1),g.GetVertex(v2),p); }
/** This performs a linear search through all polygons returning the closest one. * This will fill the NNInfo with .node for the closest node not necessarily complying with the NNConstraint, and .constrainedNode with the closest node * complying with the NNConstraint. * \see GetNearestForce(Node[],Int3[],Vector3,NNConstraint,bool) */ public static NNInfo GetNearestForceBoth(NavGraph graph, INavmeshHolder navmesh, Vector3 position, NNConstraint constraint, bool accurateNearestNode) { Int3 pos = (Int3)position; float minDist = -1; GraphNode minNode = null; float minConstDist = -1; GraphNode minConstNode = null; float maxDistSqr = constraint.constrainDistance ? AstarPath.active.maxNearestNodeDistanceSqr : float.PositiveInfinity; GraphNodeDelegateCancelable del = delegate(GraphNode _node) { TriangleMeshNode node = _node as TriangleMeshNode; if (accurateNearestNode) { Vector3 closest = node.ClosestPointOnNode(position); float dist = ((Vector3)pos - closest).sqrMagnitude; if (minNode == null || dist < minDist) { minDist = dist; minNode = node; } if (dist < maxDistSqr && constraint.Suitable(node)) { if (minConstNode == null || dist < minConstDist) { minConstDist = dist; minConstNode = node; } } } else { if (!node.ContainsPoint((Int3)position)) { float dist = (node.position - pos).sqrMagnitude; if (minNode == null || dist < minDist) { minDist = dist; minNode = node; } if (dist < maxDistSqr && constraint.Suitable(node)) { if (minConstNode == null || dist < minConstDist) { minConstDist = dist; minConstNode = node; } } } else { int dist = AstarMath.Abs(node.position.y - pos.y); if (minNode == null || dist < minDist) { minDist = dist; minNode = node; } if (dist < maxDistSqr && constraint.Suitable(node)) { if (minConstNode == null || dist < minConstDist) { minConstDist = dist; minConstNode = node; } } } } return(true); }; graph.GetNodes(del); NNInfo nninfo = new NNInfo(minNode); //Find the point closest to the nearest triangle if (nninfo.node != null) { TriangleMeshNode node = nninfo.node as TriangleMeshNode; //minNode2 as MeshNode; Vector3 clP = node.ClosestPointOnNode(position); nninfo.clampedPosition = clP; } nninfo.constrainedNode = minConstNode; if (nninfo.constrainedNode != null) { TriangleMeshNode node = nninfo.constrainedNode as TriangleMeshNode; //minNode2 as MeshNode; Vector3 clP = node.ClosestPointOnNode(position); nninfo.constClampedPosition = clP; } return(nninfo); }
/** Opens the nodes connected to this node. This is a base call and can be called by node classes overriding the Open function to open all connections in the #connections array. * \see #connections * \see Open */ public void BaseOpen (NodeRunData nodeRunData, NodeRun nodeR, Int3 targetPosition, Path path) { if (connections == null) return; for (int i=0;i<connections.Length;i++) { Node conNode = connections[i]; if (!path.CanTraverse (conNode)) { continue; } NodeRun nodeR2 = conNode.GetNodeRun (nodeRunData); if (nodeR2.pathID != nodeRunData.pathID) { nodeR2.parent = nodeR; nodeR2.pathID = nodeRunData.pathID; nodeR2.cost = (uint)connectionCosts[i]; conNode.UpdateH (targetPosition, path.heuristic, path.heuristicScale, nodeR2); conNode.UpdateG (nodeR2, nodeRunData); nodeRunData.open.Add (nodeR2); //Debug.DrawLine (position,node.position,Color.cyan); //Debug.Log ("Opening Node "+node.position.ToString ()+" "+g+" "+node.cost+" "+node.g+" "+node.f); } else { //If not we can test if the path from the current node to this one is a better one then the one already used uint tmpCost = (uint)connectionCosts[i]; if (nodeR.g+tmpCost+conNode.penalty #if !ASTAR_NoTagPenalty + path.GetTagPenalty(conNode.tags) #endif < nodeR2.g) { nodeR2.cost = tmpCost; nodeR2.parent = nodeR; conNode.UpdateAllG (nodeR2,nodeRunData); nodeRunData.open.Add (nodeR2); } else if (nodeR2.g+tmpCost+penalty #if !ASTAR_NoTagPenalty + path.GetTagPenalty(tags) #endif < nodeR.g) {//Or if the path from this node ("node") to the current ("current") is better bool contains = conNode.ContainsConnection (this); //Make sure we don't travel along the wrong direction of a one way link now, make sure the Current node can be moved to from the other Node. /*if (node.connections != null) { for (int y=0;y<node.connections.Length;y++) { if (node.connections[y] == this) { contains = true; break; } } }*/ if (!contains) { continue; } nodeR.parent = nodeR2; nodeR.cost = tmpCost; UpdateAllG (nodeR,nodeRunData); nodeRunData.open.Add (nodeR); } } } }
/** Prepares the path. Searches for start and end nodes and does some simple checking if a path is at all possible */ public virtual void Prepare() { System.DateTime startTime = System.DateTime.Now; //@pathStartTime = startTime; maxFrameTime = AstarPath.active.maxFrameTime; //maxAngle = NmaxAngle; //angleCost = NangleCost; //stepByStep = NstepByStep; //unitRadius = 0;//BETA, Not used NNInfo startNNInfo = AstarPath.active.GetNearest(startPoint, nnConstraint, startHint); //Tell the NNConstraint which node was found as the start node if it is a PathNNConstraint and not a normal NNConstraint PathNNConstraint pathNNConstraint = nnConstraint as PathNNConstraint; if (pathNNConstraint != null) { pathNNConstraint.SetStart(startNNInfo.node); } startPoint = startNNInfo.clampedPosition; startIntPoint = (Int3)startPoint; startNode = startNNInfo.node; if (hasEndPoint) { NNInfo endNNInfo = AstarPath.active.GetNearest(endPoint, nnConstraint, endHint); endPoint = endNNInfo.clampedPosition; hTarget = (Int3)endPoint; endNode = endNNInfo.node; } if (startNode == null || (hasEndPoint == true && endNode == null)) { LogError("Couldn't find close nodes to either the start or the end (start = " + (startNode != null) + " end = " + (endNode != null) + ")"); duration += (System.DateTime.Now.Ticks - startTime.Ticks) * 0.0001F; return; } if (!startNode.walkable) { LogError("The node closest to the start point is not walkable"); duration += (System.DateTime.Now.Ticks - startTime.Ticks) * 0.0001F; return; } if (hasEndPoint && !endNode.walkable) { LogError("The node closest to the start point is not walkable"); duration += (System.DateTime.Now.Ticks - startTime.Ticks) * 0.0001F; return; } if (hasEndPoint && startNode.area != endNode.area) { LogError("There is no valid path to the target (start area: " + startNode.area + ", target area: " + endNode.area + ")"); //Debug.DrawLine (startNode.position,endNode.position,Color.cyan); duration += (System.DateTime.Now.Ticks - startTime.Ticks) * 0.0001F; return; } duration += (System.DateTime.Now.Ticks - startTime.Ticks) * 0.0001F; }
public new override void Open(NodeRunData nodeRunData, NodeRun nodeR, Int3 targetPosition, Path path) { BaseOpen (nodeRunData, nodeR, targetPosition, path); LayerGridGraph graph = gridGraphs[indices >> 24]; int[] neighbourOffsets = graph.neighbourOffsets; int[] neighbourCosts = graph.neighbourCosts; Node[] nodes = graph.nodes; int index = GetIndex();//indices & 0xFFFFFF; for (int i=0;i<4;i++) { int conn = GetConnectionValue(i);//(gridConnections >> i*4) & 0xF; if (conn != LevelGridNode.NoConnection) { Node node = nodes[index+neighbourOffsets[i] + graph.width*graph.depth*conn]; if (!path.CanTraverse (node)) { continue; } NodeRun nodeR2 = node.GetNodeRun (nodeRunData); if (nodeR2.pathID != nodeRunData.pathID) { nodeR2.parent = nodeR; nodeR2.pathID = nodeRunData.pathID; nodeR2.cost = (uint)neighbourCosts[i]; node.UpdateH (targetPosition, path.heuristic, path.heuristicScale, nodeR2); node.UpdateG (nodeR2, nodeRunData); nodeRunData.open.Add (nodeR2); } else { //If not we can test if the path from the current node to this one is a better one then the one already used uint tmpCost = (uint)neighbourCosts[i]; if (nodeR.g+tmpCost+node.penalty #if !NoTagPenalty + path.GetTagPenalty(node.tags) #endif < nodeR2.g) { nodeR2.cost = tmpCost; nodeR2.parent = nodeR; //TODO!!!!! ?? node.UpdateAllG (nodeR2,nodeRunData); nodeRunData.open.Add (nodeR2); } else if (nodeR2.g+tmpCost+penalty #if !NoTagPenalty + path.GetTagPenalty(tags) #endif < nodeR.g) {//Or if the path from this node ("node") to the current ("current") is better bool contains = node.ContainsConnection (this); //Make sure we don't travel along the wrong direction of a one way link now, make sure the Current node can be moved to from the other Node. /*if (node.connections != null) { for (int y=0;y<node.connections.Length;y++) { if (node.connections[y] == this) { contains = true; break; } } }*/ if (!contains) { continue; } nodeR.parent = nodeR2; nodeR.cost = tmpCost; //TODO!!!!!!! ?? UpdateAllG (nodeR,nodeRunData); nodeRunData.open.Add (nodeR); } } } } }
public Vector3 ClosestPoint (Vector3 p, Int3[] vertices) { return Polygon.ClosesPointOnTriangle (vertices[v1],vertices[v2],vertices[v3],p); }
/** Generates a navmesh. Based on the supplied vertices and triangles. Memory usage is about O(n) */ public void GenerateNodes(Vector3[] vectorVertices, int[] triangles, out Vector3[] originalVertices, out Int3[] vertices) { UnityEngine.Profiling.Profiler.BeginSample("Init"); if (vectorVertices.Length == 0 || triangles.Length == 0) { originalVertices = vectorVertices; vertices = new Int3[0]; //graph.CreateNodes (0); nodes = new TriangleMeshNode[0]; return; } vertices = new Int3[vectorVertices.Length]; //Backup the original vertices //for (int i=0;i<vectorVertices.Length;i++) { // vectorVertices[i] = graph.matrix.MultiplyPoint (vectorVertices[i]); //} int c = 0; for (int i = 0; i < vertices.Length; i++) { vertices[i] = (Int3)matrix.MultiplyPoint3x4(vectorVertices[i]); } Dictionary <Int3, int> hashedVerts = new Dictionary <Int3, int> (); int[] newVertices = new int[vertices.Length]; UnityEngine.Profiling.Profiler.EndSample(); UnityEngine.Profiling.Profiler.BeginSample("Hashing"); for (int i = 0; i < vertices.Length; i++) { if (!hashedVerts.ContainsKey(vertices[i])) { newVertices[c] = i; hashedVerts.Add(vertices[i], c); c++; } // else { //Debug.Log ("Hash Duplicate "+hash+" "+vertices[i].ToString ()); //} } /*newVertices[c] = vertices.Length-1; * * if (!hashedVerts.ContainsKey (vertices[newVertices[c]])) { * * hashedVerts.Add (vertices[newVertices[c]], c); * c++; * }*/ for (int x = 0; x < triangles.Length; x++) { Int3 vertex = vertices[triangles[x]]; triangles[x] = hashedVerts[vertex]; } /*for (int i=0;i<triangles.Length;i += 3) { * * Vector3 offset = Vector3.forward*i*0.01F; * Debug.DrawLine (newVertices[triangles[i]]+offset,newVertices[triangles[i+1]]+offset,Color.blue); * Debug.DrawLine (newVertices[triangles[i+1]]+offset,newVertices[triangles[i+2]]+offset,Color.blue); * Debug.DrawLine (newVertices[triangles[i+2]]+offset,newVertices[triangles[i]]+offset,Color.blue); * }*/ Int3[] totalIntVertices = vertices; vertices = new Int3[c]; originalVertices = new Vector3[c]; for (int i = 0; i < c; i++) { vertices[i] = totalIntVertices[newVertices[i]]; //(Int3)graph.matrix.MultiplyPoint (vectorVertices[i]); originalVertices[i] = (Vector3)vectorVertices[newVertices[i]]; //vectorVertices[newVertices[i]]; } UnityEngine.Profiling.Profiler.EndSample(); UnityEngine.Profiling.Profiler.BeginSample("Constructing Nodes"); //graph.CreateNodes (triangles.Length/3);//new Node[triangles.Length/3]; nodes = new TriangleMeshNode[triangles.Length / 3]; for (int i = 0; i < nodes.Length; i++) { nodes[i] = new TriangleMeshNode(active); TriangleMeshNode node = nodes[i]; //new MeshNode (); node.Penalty = initialPenalty; node.Walkable = true; node.v0 = triangles[i * 3]; node.v1 = triangles[i * 3 + 1]; node.v2 = triangles[i * 3 + 2]; if (!Polygon.IsClockwise(vertices[node.v0], vertices[node.v1], vertices[node.v2])) { //Debug.DrawLine (vertices[node.v0],vertices[node.v1],Color.red); //Debug.DrawLine (vertices[node.v1],vertices[node.v2],Color.red); //Debug.DrawLine (vertices[node.v2],vertices[node.v0],Color.red); int tmp = node.v0; node.v0 = node.v2; node.v2 = tmp; } if (Polygon.IsColinear(vertices[node.v0], vertices[node.v1], vertices[node.v2])) { Debug.DrawLine((Vector3)vertices[node.v0], (Vector3)vertices[node.v1], Color.red); Debug.DrawLine((Vector3)vertices[node.v1], (Vector3)vertices[node.v2], Color.red); Debug.DrawLine((Vector3)vertices[node.v2], (Vector3)vertices[node.v0], Color.red); } // Make sure position is correctly set node.UpdatePositionFromVertices(); } UnityEngine.Profiling.Profiler.EndSample(); Dictionary <Int2, TriangleMeshNode> sides = new Dictionary <Int2, TriangleMeshNode>(); for (int i = 0, j = 0; i < triangles.Length; j += 1, i += 3) { sides[new Int2(triangles[i + 0], triangles[i + 1])] = nodes[j]; sides[new Int2(triangles[i + 1], triangles[i + 2])] = nodes[j]; sides[new Int2(triangles[i + 2], triangles[i + 0])] = nodes[j]; } UnityEngine.Profiling.Profiler.BeginSample("Connecting Nodes"); List <MeshNode> connections = new List <MeshNode> (); List <uint> connectionCosts = new List <uint> (); int identicalError = 0; for (int i = 0, j = 0; i < triangles.Length; j += 1, i += 3) { connections.Clear(); connectionCosts.Clear(); //Int3 indices = new Int3(triangles[i],triangles[i+1],triangles[i+2]); TriangleMeshNode node = nodes[j]; for (int q = 0; q < 3; q++) { TriangleMeshNode other; if (sides.TryGetValue(new Int2(triangles[i + ((q + 1) % 3)], triangles[i + q]), out other)) { connections.Add(other); connectionCosts.Add((uint)(node.position - other.position).costMagnitude); } } node.connections = connections.ToArray(); node.connectionCosts = connectionCosts.ToArray(); } if (identicalError > 0) { Debug.LogError("One or more triangles are identical to other triangles, this is not a good thing to have in a navmesh\nIncreasing the scale of the mesh might help\nNumber of triangles with error: " + identicalError + "\n"); } UnityEngine.Profiling.Profiler.EndSample(); UnityEngine.Profiling.Profiler.BeginSample("Rebuilding BBTree"); RebuildBBTree(this); UnityEngine.Profiling.Profiler.EndSample(); //Debug.Log ("Graph Generation - NavMesh - Time to compute graph "+((Time.realtimeSinceStartup-startTime)*1000F).ToString ("0")+"ms"); }
public bool GetPortal(GraphNode toNode, System.Collections.Generic.List <Vector3> left, System.Collections.Generic.List <Vector3> right, bool backwards, out int aIndex, out int bIndex) { aIndex = -1; bIndex = -1; //If the nodes are in different graphs, this function has no idea on how to find a shared edge. if (backwards || toNode.GraphIndex != GraphIndex) { return(false); } // Since the nodes are in the same graph, they are both TriangleMeshNodes // So we don't need to care about other types of nodes var toTriNode = toNode as TriangleMeshNode; var edge = SharedEdge(toTriNode); // A connection was found, but it specifically didn't use an edge if (edge == 0xFF) { return(false); } // No connection was found between the nodes // Check if there is a node link that connects them if (edge == -1) { #if !ASTAR_NO_POINT_GRAPH if (connections != null) { for (int i = 0; i < connections.Length; i++) { if (connections[i].node.GraphIndex != GraphIndex) { var mid = connections[i].node as NodeLink3Node; if (mid != null && mid.GetOther(this) == toTriNode) { // We have found a node which is connected through a NodeLink3Node mid.GetPortal(toTriNode, left, right, false); return(true); } } } } #endif return(false); } aIndex = edge; bIndex = (edge + 1) % GetVertexCount(); // Get the vertices of the shared edge for the first node Int3 v1a = GetVertex(edge); Int3 v1b = GetVertex((edge + 1) % GetVertexCount()); // Get tile indices int tileIndex1 = (GetVertexIndex(0) >> NavmeshBase.TileIndexOffset) & NavmeshBase.TileIndexMask; int tileIndex2 = (toTriNode.GetVertexIndex(0) >> NavmeshBase.TileIndexOffset) & NavmeshBase.TileIndexMask; if (tileIndex1 != tileIndex2) { // When the nodes are in different tiles, the edges might not be completely identical // so another technique is needed. // Get the tile coordinates, from them we can figure out which edge is going to be shared int x1, x2, z1, z2, coord; INavmeshHolder nm = GetNavmeshHolder(GraphIndex); nm.GetTileCoordinates(tileIndex1, out x1, out z1); nm.GetTileCoordinates(tileIndex2, out x2, out z2); if (System.Math.Abs(x1 - x2) == 1) { coord = 2; } else if (System.Math.Abs(z1 - z2) == 1) { coord = 0; } else { return(false); // Tiles are not adjacent. This is likely a custom connection between two nodes. } var otherEdge = toTriNode.SharedEdge(this); // A connection was found, but it specifically didn't use an edge. This is odd since the connection in the other direction did use an edge if (otherEdge == 0xFF) { throw new System.Exception( "Connection used edge in one direction, but not in the other direction. Has the wrong overload of AddConnection been used?"); } // If it is -1 then it must be a one-way connection. Fall back to using the whole edge if (otherEdge != -1) { // When the nodes are in different tiles, they might not share exactly the same edge // so we clamp the portal to the segment of the edges which they both have. int mincoord = System.Math.Min(v1a[coord], v1b[coord]); int maxcoord = System.Math.Max(v1a[coord], v1b[coord]); // Get the vertices of the shared edge for the second node Int3 v2a = toTriNode.GetVertex(otherEdge); Int3 v2b = toTriNode.GetVertex((otherEdge + 1) % toTriNode.GetVertexCount()); mincoord = System.Math.Max(mincoord, System.Math.Min(v2a[coord], v2b[coord])); maxcoord = System.Math.Min(maxcoord, System.Math.Max(v2a[coord], v2b[coord])); if (v1a[coord] < v1b[coord]) { v1a[coord] = mincoord; v1b[coord] = maxcoord; } else { v1a[coord] = maxcoord; v1b[coord] = mincoord; } } } if (left != null) { // All triangles should be laid out in clockwise order so v1b is the rightmost vertex (seen from this node) left.Add((Vector3)v1a); right.Add((Vector3)v1b); } return(true); }
public new bool SnappedLinecast(Vector3 _a, Vector3 _b, GraphNode hint, out GraphHitInfo hit) { hit = default(GraphHitInfo); LevelGridNode levelGridNode = base.GetNearest(_a, NNConstraint.None).node as LevelGridNode; LevelGridNode levelGridNode2 = base.GetNearest(_b, NNConstraint.None).node as LevelGridNode; if (levelGridNode == null || levelGridNode2 == null) { hit.node = null; hit.point = _a; return(true); } _a = this.inverseMatrix.MultiplyPoint3x4((Vector3)levelGridNode.position); _a.x -= 0.5f; _a.z -= 0.5f; _b = this.inverseMatrix.MultiplyPoint3x4((Vector3)levelGridNode2.position); _b.x -= 0.5f; _b.z -= 0.5f; Int3 ob = new Int3(Mathf.RoundToInt(_a.x), Mathf.RoundToInt(_a.y), Mathf.RoundToInt(_a.z)); Int3 @int = new Int3(Mathf.RoundToInt(_b.x), Mathf.RoundToInt(_b.y), Mathf.RoundToInt(_b.z)); hit.origin = (Vector3)ob; if (!levelGridNode.Walkable) { hit.node = levelGridNode; hit.point = this.matrix.MultiplyPoint3x4(new Vector3((float)ob.x + 0.5f, 0f, (float)ob.z + 0.5f)); hit.point.y = ((Vector3)hit.node.position).y; return(true); } int num = Mathf.Abs(ob.x - @int.x); int num2 = Mathf.Abs(ob.z - @int.z); LevelGridNode levelGridNode4; for (LevelGridNode levelGridNode3 = levelGridNode; levelGridNode3 != levelGridNode2; levelGridNode3 = levelGridNode4) { if (levelGridNode3.NodeInGridIndex == levelGridNode2.NodeInGridIndex) { hit.node = levelGridNode3; hit.point = (Vector3)levelGridNode3.position; return(true); } num = Math.Abs(ob.x - @int.x); num2 = Math.Abs(ob.z - @int.z); int num3 = 0; if (num >= num2) { num3 = ((@int.x <= ob.x) ? 3 : 1); } else if (num2 > num) { num3 = ((@int.z <= ob.z) ? 0 : 2); } if (!this.CheckConnection(levelGridNode3, num3)) { hit.node = levelGridNode3; hit.point = (Vector3)levelGridNode3.position; return(true); } levelGridNode4 = this.nodes[levelGridNode3.NodeInGridIndex + this.neighbourOffsets[num3] + this.width * this.depth * levelGridNode3.GetConnectionValue(num3)]; if (!levelGridNode4.Walkable) { hit.node = levelGridNode4; hit.point = (Vector3)levelGridNode4.position; return(true); } ob = (Int3)this.inverseMatrix.MultiplyPoint3x4((Vector3)levelGridNode4.position); } return(false); }
/** Prepares the path. Searches for start and end nodes and does some simple checking if a path is at all possible */ public override void Prepare() { AstarProfiler.StartProfile("Get Nearest"); //Initialize the NNConstraint nnConstraint.tags = enabledTags; NNInfo startNNInfo = AstarPath.active.GetNearest(startPoint, nnConstraint, startHint); //Tell the NNConstraint which node was found as the start node if it is a PathNNConstraint and not a normal NNConstraint var pathNNConstraint = nnConstraint as PathNNConstraint; if (pathNNConstraint != null) { pathNNConstraint.SetStart(startNNInfo.node); } startPoint = startNNInfo.clampedPosition; startIntPoint = (Int3)startPoint; startNode = startNNInfo.node; //If it is declared that this path type has an end point //Some path types might want to use most of the ABPath code, but will not have an explicit end point at this stage if (hasEndPoint) { NNInfo endNNInfo = AstarPath.active.GetNearest(endPoint, nnConstraint, endHint); endPoint = endNNInfo.clampedPosition; // Note, other methods assume hTarget is (Int3)endPoint hTarget = (Int3)endPoint; endNode = endNNInfo.node; hTargetNode = endNode; } AstarProfiler.EndProfile(); #if ASTARDEBUG if (startNode != null) { Debug.DrawLine((Vector3)startNode.position, startPoint, Color.blue); } if (endNode != null) { Debug.DrawLine((Vector3)endNode.position, endPoint, Color.blue); } #endif if (startNode == null && (hasEndPoint && endNode == null)) { Error(); LogError("Couldn't find close nodes to the start point or the end point"); return; } if (startNode == null) { Error(); LogError("Couldn't find a close node to the start point"); return; } if (endNode == null && hasEndPoint) { Error(); LogError("Couldn't find a close node to the end point"); return; } if (!startNode.Walkable) { #if ASTARDEBUG Debug.DrawRay(startPoint, Vector3.up, Color.red); Debug.DrawLine(startPoint, (Vector3)startNode.position, Color.red); #endif Error(); LogError("The node closest to the start point is not walkable"); return; } if (hasEndPoint && !endNode.Walkable) { Error(); LogError("The node closest to the end point is not walkable"); return; } if (hasEndPoint && startNode.Area != endNode.Area) { Error(); LogError("There is no valid path to the target (start area: " + startNode.Area + ", target area: " + endNode.Area + ")"); return; } }
public static void UpdateArea(GraphUpdateObject o, INavmeshHolder graph) { Bounds bounds = graph.transform.InverseTransform(o.bounds); // Bounding rectangle with integer coordinates var irect = new IntRect( Mathf.FloorToInt(bounds.min.x * Int3.Precision), Mathf.FloorToInt(bounds.min.z * Int3.Precision), Mathf.CeilToInt(bounds.max.x * Int3.Precision), Mathf.CeilToInt(bounds.max.z * Int3.Precision) ); // Corners of the bounding rectangle var a = new Int3(irect.xmin, 0, irect.ymin); var b = new Int3(irect.xmin, 0, irect.ymax); var c = new Int3(irect.xmax, 0, irect.ymin); var d = new Int3(irect.xmax, 0, irect.ymax); var ymin = ((Int3)bounds.min).y; var ymax = ((Int3)bounds.max).y; // Loop through all nodes and check if they intersect the bounding box graph.GetNodes(_node => { var node = _node as TriangleMeshNode; bool inside = false; int allLeft = 0; int allRight = 0; int allTop = 0; int allBottom = 0; // Check bounding box rect in XZ plane for (int v = 0; v < 3; v++) { Int3 p = node.GetVertexInGraphSpace(v); if (irect.Contains(p.x, p.z)) { inside = true; break; } if (p.x < irect.xmin) { allLeft++; } if (p.x > irect.xmax) { allRight++; } if (p.z < irect.ymin) { allTop++; } if (p.z > irect.ymax) { allBottom++; } } if (!inside && (allLeft == 3 || allRight == 3 || allTop == 3 || allBottom == 3)) { return; } // Check if the polygon edges intersect the bounding rect for (int v = 0; v < 3; v++) { int v2 = v > 1 ? 0 : v + 1; Int3 vert1 = node.GetVertexInGraphSpace(v); Int3 vert2 = node.GetVertexInGraphSpace(v2); if (VectorMath.SegmentsIntersectXZ(a, b, vert1, vert2)) { inside = true; break; } if (VectorMath.SegmentsIntersectXZ(a, c, vert1, vert2)) { inside = true; break; } if (VectorMath.SegmentsIntersectXZ(c, d, vert1, vert2)) { inside = true; break; } if (VectorMath.SegmentsIntersectXZ(d, b, vert1, vert2)) { inside = true; break; } } // Check if the node contains any corner of the bounding rect if (inside || node.ContainsPointInGraphSpace(a) || node.ContainsPointInGraphSpace(b) || node.ContainsPointInGraphSpace(c) || node.ContainsPointInGraphSpace(d)) { inside = true; } if (!inside) { return; } int allAbove = 0; int allBelow = 0; // Check y coordinate for (int v = 0; v < 3; v++) { Int3 p = node.GetVertexInGraphSpace(v); if (p.y < ymin) { allBelow++; } if (p.y > ymax) { allAbove++; } } // Polygon is either completely above the bounding box or completely below it if (allBelow == 3 || allAbove == 3) { return; } // Triangle is inside the bounding box! // Update it! o.WillUpdateNode(node); o.Apply(node); }); }
// Token: 0x060022FE RID: 8958 RVA: 0x00192AE0 File Offset: 0x00190CE0 public void Apply(bool forceNewCheck) { NNConstraint none = NNConstraint.None; none.distanceXZ = true; int graphIndex = (int)this.startNode.GraphIndex; none.graphMask = ~(1 << graphIndex); bool flag = true; NNInfo nearest = AstarPath.active.GetNearest(this.StartTransform.position, none); flag &= (nearest.node == this.connectedNode1 && nearest.node != null); this.connectedNode1 = (nearest.node as MeshNode); this.clamped1 = nearest.position; if (this.connectedNode1 != null) { Debug.DrawRay((Vector3)this.connectedNode1.position, Vector3.up * 5f, Color.red); } NNInfo nearest2 = AstarPath.active.GetNearest(this.EndTransform.position, none); flag &= (nearest2.node == this.connectedNode2 && nearest2.node != null); this.connectedNode2 = (nearest2.node as MeshNode); this.clamped2 = nearest2.position; if (this.connectedNode2 != null) { Debug.DrawRay((Vector3)this.connectedNode2.position, Vector3.up * 5f, Color.cyan); } if (this.connectedNode2 == null || this.connectedNode1 == null) { return; } this.startNode.SetPosition((Int3)this.StartTransform.position); this.endNode.SetPosition((Int3)this.EndTransform.position); if (flag && !forceNewCheck) { return; } this.RemoveConnections(this.startNode); this.RemoveConnections(this.endNode); uint cost = (uint)Mathf.RoundToInt((float)((Int3)(this.StartTransform.position - this.EndTransform.position)).costMagnitude * this.costFactor); this.startNode.AddConnection(this.endNode, cost); this.endNode.AddConnection(this.startNode, cost); Int3 rhs = this.connectedNode2.position - this.connectedNode1.position; for (int i = 0; i < this.connectedNode1.GetVertexCount(); i++) { Int3 vertex = this.connectedNode1.GetVertex(i); Int3 vertex2 = this.connectedNode1.GetVertex((i + 1) % this.connectedNode1.GetVertexCount()); if (Int3.DotLong((vertex2 - vertex).Normal2D(), rhs) <= 0L) { for (int j = 0; j < this.connectedNode2.GetVertexCount(); j++) { Int3 vertex3 = this.connectedNode2.GetVertex(j); Int3 vertex4 = this.connectedNode2.GetVertex((j + 1) % this.connectedNode2.GetVertexCount()); if (Int3.DotLong((vertex4 - vertex3).Normal2D(), rhs) >= 0L && (double)Int3.Angle(vertex4 - vertex3, vertex2 - vertex) > 2.967059810956319) { float num = 0f; float num2 = 1f; num2 = Math.Min(num2, VectorMath.ClosestPointOnLineFactor(vertex, vertex2, vertex3)); num = Math.Max(num, VectorMath.ClosestPointOnLineFactor(vertex, vertex2, vertex4)); if (num2 >= num) { Vector3 vector = (Vector3)(vertex2 - vertex) * num + (Vector3)vertex; Vector3 vector2 = (Vector3)(vertex2 - vertex) * num2 + (Vector3)vertex; this.startNode.portalA = vector; this.startNode.portalB = vector2; this.endNode.portalA = vector2; this.endNode.portalB = vector; this.connectedNode1.AddConnection(this.startNode, (uint)Mathf.RoundToInt((float)((Int3)(this.clamped1 - this.StartTransform.position)).costMagnitude * this.costFactor)); this.connectedNode2.AddConnection(this.endNode, (uint)Mathf.RoundToInt((float)((Int3)(this.clamped2 - this.EndTransform.position)).costMagnitude * this.costFactor)); this.startNode.AddConnection(this.connectedNode1, (uint)Mathf.RoundToInt((float)((Int3)(this.clamped1 - this.StartTransform.position)).costMagnitude * this.costFactor)); this.endNode.AddConnection(this.connectedNode2, (uint)Mathf.RoundToInt((float)((Int3)(this.clamped2 - this.EndTransform.position)).costMagnitude * this.costFactor)); return; } Debug.LogError(string.Concat(new object[] { "Something went wrong! ", num, " ", num2, " ", vertex, " ", vertex2, " ", vertex3, " ", vertex4, "\nTODO, how can this happen?" })); } } } } }
public static Vector3 IntPointToV3(IntPoint p) { Int3 ob = new Int3((int)p.X, 0, (int)p.Y); return((Vector3)ob); }
public static IntPoint V3ToIntPoint(Vector3 p) { Int3 @int = (Int3)p; return(new IntPoint((long)@int.x, (long)@int.z)); }
/** This performs a linear search through all polygons returning the closest one */ public static NNInfo GetNearestForce (Node[] nodes, Int3[] vertices, Vector3 position, NNConstraint constraint) { Int3 pos = (Int3)position; //Replacement for Infinity, the maximum value a int can hold int minDist = -1; Node minNode = null; float minDist2 = -1; Node minNode2 = null; int minConstDist = -1; Node minNodeConst = null; float minConstDist2 = -1; Node minNodeConst2 = null; //int rnd = (int)Random.Range (0,10000); //int skipped = 0; for (int i=0;i<nodes.Length;i++) { MeshNode node = nodes[i] as MeshNode; if (!Polygon.IsClockwise (vertices[node.v1],vertices[node.v2],pos) || !Polygon.IsClockwise (vertices[node.v2],vertices[node.v3],pos) || !Polygon.IsClockwise (vertices[node.v3],vertices[node.v1],pos)) { //Polygon.TriangleArea2 (vertices[node.v1],vertices[node.v2],pos) >= 0 || Polygon.TriangleArea2 (vertices[node.v2],vertices[node.v3],pos) >= 0 || Polygon.TriangleArea2 (vertices[node.v3],vertices[node.v1],pos) >= 0) { /*if (minDist2 != -1) { float d1 = (node.position-vertices[node.v1]).sqrMagnitude; d1 = Mathf.Min (d1,(node.position-vertices[node.v1]).sqrMagnitude); d1 = Mathf.Min (d1,(node.position-vertices[node.v1]).sqrMagnitude); //The closest distance possible from the current node to 'pos' d1 = (node.position-pos).sqrMagnitude-d1; if (d1 > minDist2) { skipped++; continue; } }*/ /*float dist2 = Mathfx.DistancePointSegment2 (pos.x,pos.z,vertices[node.v1].x,vertices[node.v1].z,vertices[node.v2].x,vertices[node.v2].z); dist2 = Mathfx.Min (dist2,Mathfx.DistancePointSegment2 (pos.x,pos.z,vertices[node.v1].x,vertices[node.v1].z,vertices[node.v3].x,vertices[node.v3].z)); dist2 = Mathfx.Min (dist2,Mathfx.DistancePointSegment2 (pos.x,pos.z,vertices[node.v3].x,vertices[node.v3].z,vertices[node.v2].x,vertices[node.v2].z));*/ float dist2 = (node.position-pos).sqrMagnitude; if (minDist2 == -1 || dist2 < minDist2) { minDist2 = dist2; minNode2 = node; } if (constraint.Suitable (node)) { if (minConstDist2 == -1 || dist2 < minConstDist2) { minConstDist2 = dist2; minNodeConst2 = node; } } continue; } int dist = Mathfx.Abs (node.position.y-pos.y); if (minDist == -1 || dist < minDist) { minDist = dist; minNode = node; } if (constraint.Suitable (node)) { if (minConstDist == -1 || dist < minConstDist) { minConstDist = dist; minNodeConst = node; } } } NNInfo nninfo = new NNInfo (minNode == null ? minNode2 : minNode, minNode == null ? NearestNodePriority.Low : NearestNodePriority.High); //Find the point closest to the nearest triangle //if (minNode == null) { if (nninfo.node != null) { MeshNode node = nninfo.node as MeshNode;//minNode2 as MeshNode; Vector3[] triangle = new Vector3[3] {vertices[node.v1],vertices[node.v2],vertices[node.v3]}; Vector3 clP = Polygon.ClosesPointOnTriangle (triangle,position); nninfo.clampedPosition = clP; } nninfo.constrainedNode = minNodeConst == null ? minNodeConst2 : minNodeConst; if (nninfo.constrainedNode != null) { MeshNode node = nninfo.constrainedNode as MeshNode;//minNode2 as MeshNode; Vector3[] triangle = new Vector3[3] {vertices[node.v1],vertices[node.v2],vertices[node.v3]}; Vector3 clP = Polygon.ClosesPointOnTriangle (triangle,position); nninfo.constClampedPosition = clP; } return nninfo; }
/** Returns if the point is inside the node in XZ space */ public static bool ContainsPoint (TriangleMeshNode node, Vector3 pos, Int3[] vertices) { if (!Polygon.IsClockwiseMargin ((Vector3)vertices[node.v0],(Vector3)vertices[node.v1], (Vector3)vertices[node.v2])) { Debug.LogError ("Noes!"); } if ( Polygon.IsClockwiseMargin ((Vector3)vertices[node.v0],(Vector3)vertices[node.v1], pos) && Polygon.IsClockwiseMargin ((Vector3)vertices[node.v1],(Vector3)vertices[node.v2], pos) && Polygon.IsClockwiseMargin ((Vector3)vertices[node.v2],(Vector3)vertices[node.v0], pos)) { return true; } return false; }
/** Returns if the point is inside the node in XZ space */ public static bool ContainsPoint (MeshNode node, Vector3 pos, Int3[] vertices) { if (Polygon.IsClockwiseMargin ((Vector3)vertices[node.v1],(Vector3)vertices[node.v2], pos) && Polygon.IsClockwiseMargin ((Vector3)vertices[node.v2],(Vector3)vertices[node.v3], pos) && Polygon.IsClockwiseMargin ((Vector3)vertices[node.v3],(Vector3)vertices[node.v1], pos)) { return true; } return false; }
/** Reset all values to their default values. * All inheriting path types must implement this function, resetting ALL their variables to enable recycling of paths. * Call this base function in inheriting types with base.Reset (); */ public override void Reset () { base.Reset (); startNode = null; endNode = null; startHint = null; endHint = null; originalStartPoint = Vector3.zero; originalEndPoint = Vector3.zero; startPoint = Vector3.zero; endPoint = Vector3.zero; calculatePartial = false; partialBestTarget = null; startIntPoint = new Int3(); hTarget = new Int3(); endNodeCosts = null; }
public static void UpdateArea (GraphUpdateObject o, INavmesh graph) { //System.DateTime startTime = System.DateTime.UtcNow; Bounds bounds = o.bounds; Rect r = Rect.MinMaxRect (bounds.min.x,bounds.min.z,bounds.max.x,bounds.max.z); IntRect r2 = new IntRect( Mathf.FloorToInt(bounds.min.x*Int3.Precision), Mathf.FloorToInt(bounds.min.z*Int3.Precision), Mathf.FloorToInt(bounds.max.x*Int3.Precision), Mathf.FloorToInt(bounds.max.z*Int3.Precision) ); /*Vector3 a = new Vector3 (r.xMin,0,r.yMin);// -1 -1 Vector3 b = new Vector3 (r.xMin,0,r.yMax);// -1 1 Vector3 c = new Vector3 (r.xMax,0,r.yMin);// 1 -1 Vector3 d = new Vector3 (r.xMax,0,r.yMax);// 1 1 */ Int3 a = new Int3(r2.xmin,0,r2.ymin); Int3 b = new Int3(r2.xmin,0,r2.ymax); Int3 c = new Int3(r2.xmax,0,r2.ymin); Int3 d = new Int3(r2.xmax,0,r2.ymax); Int3 ia = (Int3)a; Int3 ib = (Int3)b; Int3 ic = (Int3)c; Int3 id = (Int3)d; #if ASTARDEBUG Debug.DrawLine (a,b,Color.white); Debug.DrawLine (a,c,Color.white); Debug.DrawLine (c,d,Color.white); Debug.DrawLine (d,b,Color.white); #endif //for (int i=0;i<nodes.Length;i++) { graph.GetNodes (delegate (GraphNode _node) { TriangleMeshNode node = _node as TriangleMeshNode; bool inside = false; int allLeft = 0; int allRight = 0; int allTop = 0; int allBottom = 0; for (int v=0;v<3;v++) { Int3 p = node.GetVertex(v); Vector3 vert = (Vector3)p; //Vector2 vert2D = new Vector2 (vert.x,vert.z); if (r2.Contains (p.x,p.z)) { //Debug.DrawRay (vert,Vector3.up*10,Color.yellow); inside = true; break; } if (vert.x < r.xMin) allLeft++; if (vert.x > r.xMax) allRight++; if (vert.z < r.yMin) allTop++; if (vert.z > r.yMax) allBottom++; //if (!bounds.Contains (node[v]) { // inside = false; // break; //} } if (!inside) { if (allLeft == 3 || allRight == 3 || allTop == 3 || allBottom == 3) { return true; } } //Debug.DrawLine ((Vector3)node.GetVertex(0),(Vector3)node.GetVertex(1),Color.yellow); //Debug.DrawLine ((Vector3)node.GetVertex(1),(Vector3)node.GetVertex(2),Color.yellow); //Debug.DrawLine ((Vector3)node.GetVertex(2),(Vector3)node.GetVertex(0),Color.yellow); for (int v=0;v<3;v++) { int v2 = v > 1 ? 0 : v+1; Int3 vert1 = node.GetVertex(v); Int3 vert2 = node.GetVertex(v2); if (Polygon.Intersects (a,b,vert1,vert2)) { inside = true; break; } if (Polygon.Intersects (a,c,vert1,vert2)) { inside = true; break; } if (Polygon.Intersects (c,d,vert1,vert2)) { inside = true; break; } if (Polygon.Intersects (d,b,vert1,vert2)) { inside = true; break; } } if (node.ContainsPoint (ia) || node.ContainsPoint (ib) || node.ContainsPoint (ic) || node.ContainsPoint (id)) { inside = true; } if (!inside) { return true; } o.WillUpdateNode(node); o.Apply (node); /*Debug.DrawLine ((Vector3)node.GetVertex(0),(Vector3)node.GetVertex(1),Color.blue); Debug.DrawLine ((Vector3)node.GetVertex(1),(Vector3)node.GetVertex(2),Color.blue); Debug.DrawLine ((Vector3)node.GetVertex(2),(Vector3)node.GetVertex(0),Color.blue); Debug.Break ();*/ return true; }); //System.DateTime endTime = System.DateTime.UtcNow; //float theTime = (endTime-startTime).Ticks*0.0001F; //Debug.Log ("Intersecting bounds with navmesh took "+theTime.ToString ("0.000")+" ms"); }
public static void UpdateArea(GraphUpdateObject o, INavmesh graph) { //System.DateTime startTime = System.DateTime.UtcNow; Bounds bounds = o.bounds; Rect r = Rect.MinMaxRect(bounds.min.x, bounds.min.z, bounds.max.x, bounds.max.z); IntRect r2 = new IntRect( Mathf.FloorToInt(bounds.min.x * Int3.Precision), Mathf.FloorToInt(bounds.min.z * Int3.Precision), Mathf.FloorToInt(bounds.max.x * Int3.Precision), Mathf.FloorToInt(bounds.max.z * Int3.Precision) ); /*Vector3 a = new Vector3 (r.xMin,0,r.yMin);// -1 -1 * Vector3 b = new Vector3 (r.xMin,0,r.yMax);// -1 1 * Vector3 c = new Vector3 (r.xMax,0,r.yMin);// 1 -1 * Vector3 d = new Vector3 (r.xMax,0,r.yMax);// 1 1 */ Int3 a = new Int3(r2.xmin, 0, r2.ymin); Int3 b = new Int3(r2.xmin, 0, r2.ymax); Int3 c = new Int3(r2.xmax, 0, r2.ymin); Int3 d = new Int3(r2.xmax, 0, r2.ymax); Int3 ia = (Int3)a; Int3 ib = (Int3)b; Int3 ic = (Int3)c; Int3 id = (Int3)d; //for (int i=0;i<nodes.Length;i++) { graph.GetNodes(delegate(GraphNode _node) { TriangleMeshNode node = _node as TriangleMeshNode; bool inside = false; int allLeft = 0; int allRight = 0; int allTop = 0; int allBottom = 0; for (int v = 0; v < 3; v++) { Int3 p = node.GetVertex(v); Vector3 vert = (Vector3)p; //Vector2 vert2D = new Vector2 (vert.x,vert.z); if (r2.Contains(p.x, p.z)) { //Debug.DrawRay (vert,Vector3.up*10,Color.yellow); inside = true; break; } if (vert.x < r.xMin) { allLeft++; } if (vert.x > r.xMax) { allRight++; } if (vert.z < r.yMin) { allTop++; } if (vert.z > r.yMax) { allBottom++; } //if (!bounds.Contains (node[v]) { // inside = false; // break; //} } if (!inside) { if (allLeft == 3 || allRight == 3 || allTop == 3 || allBottom == 3) { return(true); } } //Debug.DrawLine ((Vector3)node.GetVertex(0),(Vector3)node.GetVertex(1),Color.yellow); //Debug.DrawLine ((Vector3)node.GetVertex(1),(Vector3)node.GetVertex(2),Color.yellow); //Debug.DrawLine ((Vector3)node.GetVertex(2),(Vector3)node.GetVertex(0),Color.yellow); for (int v = 0; v < 3; v++) { int v2 = v > 1 ? 0 : v + 1; Int3 vert1 = node.GetVertex(v); Int3 vert2 = node.GetVertex(v2); if (Polygon.Intersects(a, b, vert1, vert2)) { inside = true; break; } if (Polygon.Intersects(a, c, vert1, vert2)) { inside = true; break; } if (Polygon.Intersects(c, d, vert1, vert2)) { inside = true; break; } if (Polygon.Intersects(d, b, vert1, vert2)) { inside = true; break; } } if (node.ContainsPoint(ia) || node.ContainsPoint(ib) || node.ContainsPoint(ic) || node.ContainsPoint(id)) { inside = true; } if (!inside) { return(true); } o.WillUpdateNode(node); o.Apply(node); /*Debug.DrawLine ((Vector3)node.GetVertex(0),(Vector3)node.GetVertex(1),Color.blue); * Debug.DrawLine ((Vector3)node.GetVertex(1),(Vector3)node.GetVertex(2),Color.blue); * Debug.DrawLine ((Vector3)node.GetVertex(2),(Vector3)node.GetVertex(0),Color.blue); * Debug.Break ();*/ return(true); }); //System.DateTime endTime = System.DateTime.UtcNow; //float theTime = (endTime-startTime).Ticks*0.0001F; //Debug.Log ("Intersecting bounds with navmesh took "+theTime.ToString ("0.000")+" ms"); }
public virtual void Reset() { if (object.ReferenceEquals(AstarPath.active, null)) { throw new NullReferenceException("No AstarPath object found in the scene. Make sure there is one or do not create paths in Awake"); } this.hasBeenReset = true; this.state = PathState.Created; this.releasedNotSilent = false; this.pathHandler = null; this.callback = null; this._errorLog = string.Empty; this.pathCompleteState = PathCompleteState.NotCalculated; this.path = ListPool<GraphNode>.Claim(); this.vectorPath = ListPool<Vector3>.Claim(); this.currentR = null; this.duration = 0f; this.searchIterations = 0; this.searchedNodes = 0; this.nnConstraint = PathNNConstraint.Default; this.next = null; this.heuristic = AstarPath.active.heuristic; this.heuristicScale = AstarPath.active.heuristicScale; this.enabledTags = -1; this.tagPenalties = null; this.callTime = DateTime.UtcNow; this.pathID = AstarPath.active.GetNextPathID(); this.hTarget = Int3.zero; this.hTargetNode = null; }
public static void UpdateArea(GraphUpdateObject o, INavmesh graph) { Bounds bounds = o.bounds; // Bounding rectangle with floating point coordinates Rect r = Rect.MinMaxRect(bounds.min.x, bounds.min.z, bounds.max.x, bounds.max.z); // Bounding rectangle with int coordinates var r2 = new IntRect( Mathf.FloorToInt(bounds.min.x * Int3.Precision), Mathf.FloorToInt(bounds.min.z * Int3.Precision), Mathf.FloorToInt(bounds.max.x * Int3.Precision), Mathf.FloorToInt(bounds.max.z * Int3.Precision) ); // Corners of the bounding rectangle var a = new Int3(r2.xmin, 0, r2.ymin); var b = new Int3(r2.xmin, 0, r2.ymax); var c = new Int3(r2.xmax, 0, r2.ymin); var d = new Int3(r2.xmax, 0, r2.ymax); var ymin = ((Int3)bounds.min).y; var ymax = ((Int3)bounds.max).y; // Loop through all nodes graph.GetNodes(_node => { var node = _node as TriangleMeshNode; bool inside = false; int allLeft = 0; int allRight = 0; int allTop = 0; int allBottom = 0; // Check bounding box rect in XZ plane for (int v = 0; v < 3; v++) { Int3 p = node.GetVertex(v); var vert = (Vector3)p; if (r2.Contains(p.x, p.z)) { inside = true; break; } if (vert.x < r.xMin) { allLeft++; } if (vert.x > r.xMax) { allRight++; } if (vert.z < r.yMin) { allTop++; } if (vert.z > r.yMax) { allBottom++; } } if (!inside) { if (allLeft == 3 || allRight == 3 || allTop == 3 || allBottom == 3) { return(true); } } // Check if the polygon edges intersect the bounding rect for (int v = 0; v < 3; v++) { int v2 = v > 1 ? 0 : v + 1; Int3 vert1 = node.GetVertex(v); Int3 vert2 = node.GetVertex(v2); if (VectorMath.SegmentsIntersectXZ(a, b, vert1, vert2)) { inside = true; break; } if (VectorMath.SegmentsIntersectXZ(a, c, vert1, vert2)) { inside = true; break; } if (VectorMath.SegmentsIntersectXZ(c, d, vert1, vert2)) { inside = true; break; } if (VectorMath.SegmentsIntersectXZ(d, b, vert1, vert2)) { inside = true; break; } } // Check if the node contains any corner of the bounding rect if (inside || node.ContainsPoint(a) || node.ContainsPoint(b) || node.ContainsPoint(c) || node.ContainsPoint(d)) { inside = true; } if (!inside) { return(true); } int allAbove = 0; int allBelow = 0; // Check y coordinate for (int v = 0; v < 3; v++) { Int3 p = node.GetVertex(v); if (p.y < ymin) { allBelow++; } if (p.y > ymax) { allAbove++; } } // Polygon is either completely above the bounding box or completely below it if (allBelow == 3 || allAbove == 3) { return(true); } // Triangle is inside the bounding box! // Update it! o.WillUpdateNode(node); o.Apply(node); return(true); }); }
public void SetPosition (Int3 position) { this.position = position; }
/** Generates a navmesh. Based on the supplied vertices and triangles */ void GenerateNodes(Vector3[] vectorVertices, int[] triangles, out Vector3[] originalVertices, out Int3[] vertices) { Profiler.BeginSample("Init"); if (vectorVertices.Length == 0 || triangles.Length == 0) { originalVertices = vectorVertices; vertices = new Int3[0]; nodes = new TriangleMeshNode[0]; return; } vertices = new Int3[vectorVertices.Length]; int c = 0; for (int i = 0; i < vertices.Length; i++) { vertices[i] = (Int3)matrix.MultiplyPoint3x4(vectorVertices[i]); } var hashedVerts = new Dictionary <Int3, int>(); var newVertices = new int[vertices.Length]; Profiler.EndSample(); Profiler.BeginSample("Hashing"); for (int i = 0; i < vertices.Length; i++) { if (!hashedVerts.ContainsKey(vertices[i])) { newVertices[c] = i; hashedVerts.Add(vertices[i], c); c++; } } for (int x = 0; x < triangles.Length; x++) { Int3 vertex = vertices[triangles[x]]; triangles[x] = hashedVerts[vertex]; } Int3[] totalIntVertices = vertices; vertices = new Int3[c]; originalVertices = new Vector3[c]; for (int i = 0; i < c; i++) { vertices[i] = totalIntVertices[newVertices[i]]; originalVertices[i] = vectorVertices[newVertices[i]]; } Profiler.EndSample(); Profiler.BeginSample("Constructing Nodes"); nodes = new TriangleMeshNode[triangles.Length / 3]; int graphIndex = active.astarData.GetGraphIndex(this); // Does not have to set this, it is set in ScanInternal //TriangleMeshNode.SetNavmeshHolder ((int)graphIndex,this); for (int i = 0; i < nodes.Length; i++) { nodes[i] = new TriangleMeshNode(active); TriangleMeshNode node = nodes[i]; //new MeshNode (); node.GraphIndex = (uint)graphIndex; node.Penalty = initialPenalty; node.Walkable = true; node.v0 = triangles[i * 3]; node.v1 = triangles[i * 3 + 1]; node.v2 = triangles[i * 3 + 2]; if (!VectorMath.IsClockwiseXZ(vertices[node.v0], vertices[node.v1], vertices[node.v2])) { //Debug.DrawLine (vertices[node.v0],vertices[node.v1],Color.red); //Debug.DrawLine (vertices[node.v1],vertices[node.v2],Color.red); //Debug.DrawLine (vertices[node.v2],vertices[node.v0],Color.red); int tmp = node.v0; node.v0 = node.v2; node.v2 = tmp; } if (VectorMath.IsColinearXZ(vertices[node.v0], vertices[node.v1], vertices[node.v2])) { Debug.DrawLine((Vector3)vertices[node.v0], (Vector3)vertices[node.v1], Color.red); Debug.DrawLine((Vector3)vertices[node.v1], (Vector3)vertices[node.v2], Color.red); Debug.DrawLine((Vector3)vertices[node.v2], (Vector3)vertices[node.v0], Color.red); } // Make sure position is correctly set node.UpdatePositionFromVertices(); } Profiler.EndSample(); var sides = new Dictionary <Int2, TriangleMeshNode>(); for (int i = 0, j = 0; i < triangles.Length; j += 1, i += 3) { sides[new Int2(triangles[i + 0], triangles[i + 1])] = nodes[j]; sides[new Int2(triangles[i + 1], triangles[i + 2])] = nodes[j]; sides[new Int2(triangles[i + 2], triangles[i + 0])] = nodes[j]; } Profiler.BeginSample("Connecting Nodes"); var connections = new List <MeshNode>(); var connectionCosts = new List <uint>(); for (int i = 0, j = 0; i < triangles.Length; j += 1, i += 3) { connections.Clear(); connectionCosts.Clear(); TriangleMeshNode node = nodes[j]; for (int q = 0; q < 3; q++) { TriangleMeshNode other; if (sides.TryGetValue(new Int2(triangles[i + ((q + 1) % 3)], triangles[i + q]), out other)) { connections.Add(other); connectionCosts.Add((uint)(node.position - other.position).costMagnitude); } } node.connections = connections.ToArray(); node.connectionCosts = connectionCosts.ToArray(); } Profiler.EndSample(); Profiler.BeginSample("Rebuilding BBTree"); RebuildBBTree(this); Profiler.EndSample(); }
/** Reset all values to their default values. * * \note All inheriting path types (e.g ConstantPath, RandomPath, etc.) which declare their own variables need to * override this function, resetting ALL their variables to enable recycling of paths. * If this is not done, trying to use that path type for pooling might result in weird behaviour. * The best way is to reset to default values the variables declared in the extended path type and then * call this base function in inheriting types with base.Reset (). * * \warning This function should not be called manually. */ public virtual void Reset () { if (System.Object.ReferenceEquals (AstarPath.active, null)) throw new System.NullReferenceException ("No AstarPath object found in the scene. " + "Make sure there is one or do not create paths in Awake"); hasBeenReset = true; state = (int)PathState.Created; releasedNotSilent = false; pathHandler = null; callback = null; _errorLog = ""; pathCompleteState = PathCompleteState.NotCalculated; path = Pathfinding.Util.ListPool<GraphNode>.Claim(); vectorPath = Pathfinding.Util.ListPool<Vector3>.Claim(); currentR = null; duration = 0; searchIterations = 0; searchedNodes = 0; //calltime nnConstraint = PathNNConstraint.Default; next = null; heuristic = AstarPath.active.heuristic; heuristicScale = AstarPath.active.heuristicScale; enabledTags = -1; tagPenalties = null; callTime = System.DateTime.UtcNow; pathID = AstarPath.active.GetNextPathID (); hTarget = Int3.zero; hTargetNode = null; }
public virtual int[] InitialOpen(BinaryHeapM open, Int3 targetPosition, Int3 position, Path path, bool doOpen) { return(BaseInitialOpen(open, targetPosition, position, path, doOpen)); }
//public override Int3 Position {get { return position; } } public void SetPosition (Int3 value) { position = value; }
public virtual void Open(NodeRunData nodeRunData, NodeRun nodeR, Int3 targetPosition, Path path) { BaseOpen(nodeRunData, nodeR, targetPosition, path); }
public virtual void Open (NodeRunData nodeRunData, NodeRun nodeR, Int3 targetPosition, Path path) { BaseOpen (nodeRunData,nodeR, targetPosition,path); }
/** Opens the nodes connected to this node. This is a base call and can be called by node classes overriding the Open function to open all connections in the #connections array. * \see #connections * \see Open */ public void BaseOpen(NodeRunData nodeRunData, NodeRun nodeR, Int3 targetPosition, Path path) { if (connections == null) { return; } for (int i = 0; i < connections.Length; i++) { Node conNode = connections[i]; if (!path.CanTraverse(conNode)) { continue; } NodeRun nodeR2 = conNode.GetNodeRun(nodeRunData); if (nodeR2.pathID != nodeRunData.pathID) { nodeR2.parent = nodeR; nodeR2.pathID = nodeRunData.pathID; nodeR2.cost = (uint)connectionCosts[i]; conNode.UpdateH(targetPosition, path.heuristic, path.heuristicScale, nodeR2); conNode.UpdateG(nodeR2, nodeRunData); nodeRunData.open.Add(nodeR2); //Debug.DrawLine (position,node.position,Color.cyan); //Debug.Log ("Opening Node "+node.position.ToString ()+" "+g+" "+node.cost+" "+node.g+" "+node.f); } else { //If not we can test if the path from the current node to this one is a better one then the one already used uint tmpCost = (uint)connectionCosts[i]; if (nodeR.g + tmpCost + conNode.penalty + path.GetTagPenalty(conNode.tags) < nodeR2.g) { nodeR2.cost = tmpCost; nodeR2.parent = nodeR; conNode.UpdateAllG(nodeR2, nodeRunData); nodeRunData.open.Add(nodeR2); } else if (nodeR2.g + tmpCost + penalty + path.GetTagPenalty(tags) < nodeR.g) //Or if the path from this node ("node") to the current ("current") is better { bool contains = conNode.ContainsConnection(this); //Make sure we don't travel along the wrong direction of a one way link now, make sure the Current node can be moved to from the other Node. /*if (node.connections != null) { * for (int y=0;y<node.connections.Length;y++) { * if (node.connections[y] == this) { * contains = true; * break; * } * } * }*/ if (!contains) { continue; } nodeR.parent = nodeR2; nodeR.cost = tmpCost; UpdateAllG(nodeR, nodeRunData); nodeRunData.open.Add(nodeR); } } } }
/** Prepares the path. Searches for start and end nodes and does some simple checking if a path is at all possible */ public override void Prepare() { AstarProfiler.StartProfile ("Get Nearest"); //Initialize the NNConstraint nnConstraint.tags = enabledTags; NNInfo startNNInfo = AstarPath.active.GetNearest (startPoint,nnConstraint, startHint); //Tell the NNConstraint which node was found as the start node if it is a PathNNConstraint and not a normal NNConstraint PathNNConstraint pathNNConstraint = nnConstraint as PathNNConstraint; if (pathNNConstraint != null) { pathNNConstraint.SetStart (startNNInfo.node); } startPoint = startNNInfo.clampedPosition; startIntPoint = (Int3)startPoint; startNode = startNNInfo.node; //If it is declared that this path type has an end point //Some path types might want to use most of the ABPath code, but will not have an explicit end point at start if (hasEndPoint) { NNInfo endNNInfo = AstarPath.active.GetNearest (endPoint,nnConstraint, endHint); endPoint = endNNInfo.clampedPosition; hTarget = (Int3)endPoint; endNode = endNNInfo.node; } AstarProfiler.EndProfile (); if (startNode == null && (hasEndPoint && endNode == null)) { Error (); LogError ("Couldn't find close nodes to the start point or the end point"); return; } if (startNode == null) { Error (); LogError ("Couldn't find a close node to the start point"); return; } if (endNode == null && hasEndPoint) { Error (); LogError ("Couldn't find a close node to the end point"); return; } if (!startNode.walkable) { Error (); LogError ("The node closest to the start point is not walkable"); return; } if (hasEndPoint && !endNode.walkable) { Error (); LogError ("The node closest to the end point is not walkable"); return; } if (hasEndPoint && startNode.area != endNode.area) { Error (); LogError ("There is no valid path to the target (start area: "+startNode.area+", target area: "+endNode.area+")"); return; } }
/** Add a node to the graph at the specified position. * \note Vector3 can be casted to Int3 using (Int3)myVector. * * \note This needs to be called when it is safe to update nodes, which is * - when scanning * - during a graph update * - inside a callback registered using AstarPath.RegisterSafeUpdate */ public PointNode AddNode(Int3 position) { return(AddNode(new PointNode(active), position)); }
public void SetPosition (Int3 p) { position = p; }
public static long SignedTriangleAreaTimes2XZ(Int3 a, Int3 b, Int3 c) { return((long)(b.x - a.x) * (long)(c.z - a.z) - (long)(c.x - a.x) * (long)(b.z - a.z)); }
/** Returns the closest point of the node */ public static Vector3 ClosestPointOnNode (MeshNode node, Int3[] vertices, Vector3 pos) { return Polygon.ClosesPointOnTriangle (vertices[node[0]],vertices[node[1]],vertices[node[2]],pos); }
public static bool RightXZ(Int3 a, Int3 b, Int3 p) { return((long)(b.x - a.x) * (long)(p.z - a.z) - (long)(p.x - a.x) * (long)(b.z - a.z) < 0L); }
/** Generates a navmesh. Based on the supplied vertices and triangles. Memory usage is about O(n) */ public static void GenerateNodes (NavGraph graph, Vector3[] vectorVertices, int[] triangles, out Vector3[] originalVertices, out Int3[] vertices) { if (!(graph is INavmesh)) { Debug.LogError ("The specified graph does not implement interface 'INavmesh'"); originalVertices = vectorVertices; vertices = new Int3[0]; graph.nodes = graph.CreateNodes (0); return; } if (vectorVertices.Length == 0 || triangles.Length == 0) { originalVertices = vectorVertices; vertices = new Int3[0]; graph.nodes = graph.CreateNodes (0); return; } vertices = new Int3[vectorVertices.Length]; //Backup the original vertices //for (int i=0;i<vectorVertices.Length;i++) { // vectorVertices[i] = graph.matrix.MultiplyPoint (vectorVertices[i]); //} int c = 0; /*int maxX = 0; int maxZ = 0; //Almost infinity int minX = 0xFFFFFFF; int minZ = 0xFFFFFFF;*/ for (int i=0;i<vertices.Length;i++) { vertices[i] = (Int3)graph.matrix.MultiplyPoint (vectorVertices[i]); /*maxX = Mathfx.Max (vertices[i].x, maxX); maxZ = Mathfx.Max (vertices[i].z, maxZ); minX = Mathfx.Min (vertices[i].x, minX); minZ = Mathfx.Min (vertices[i].z, minZ);*/ } //maxX = maxX-minX; //maxZ = maxZ-minZ; Dictionary<Int3,int> hashedVerts = new Dictionary<Int3,int> (); int[] newVertices = new int[vertices.Length]; for (int i=0;i<vertices.Length-1;i++) { //int hash = Mathfx.ComputeVertexHash (vertices[i].x,vertices[i].y,vertices[i].z); //(vertices[i].x-minX)+(vertices[i].z-minX)*maxX+vertices[i].y*maxX*maxZ; //if (sortedVertices[i] != sortedVertices[i+1]) { if (!hashedVerts.ContainsKey (vertices[i])) { newVertices[c] = i; hashedVerts.Add (vertices[i], c); c++; }// else { //Debug.Log ("Hash Duplicate "+hash+" "+vertices[i].ToString ()); //} } newVertices[c] = vertices.Length-1; //int hash2 = (newVertices[c].x-minX)+(newVertices[c].z-minX)*maxX+newVertices[c].y*maxX*maxZ; //int hash2 = Mathfx.ComputeVertexHash (newVertices[c].x,newVertices[c].y,newVertices[c].z); if (!hashedVerts.ContainsKey (vertices[newVertices[c]])) { hashedVerts.Add (vertices[newVertices[c]], c); c++; } for (int x=0;x<triangles.Length;x++) { Int3 vertex = vertices[triangles[x]]; //int hash3 = (vertex.x-minX)+(vertex.z-minX)*maxX+vertex.y*maxX*maxZ; //int hash3 = Mathfx.ComputeVertexHash (vertex.x,vertex.y,vertex.z); //for (int y=0;y<newVertices.Length;y++) { triangles[x] = hashedVerts[vertex]; } /*for (int i=0;i<triangles.Length;i += 3) { Vector3 offset = Vector3.forward*i*0.01F; Debug.DrawLine (newVertices[triangles[i]]+offset,newVertices[triangles[i+1]]+offset,Color.blue); Debug.DrawLine (newVertices[triangles[i+1]]+offset,newVertices[triangles[i+2]]+offset,Color.blue); Debug.DrawLine (newVertices[triangles[i+2]]+offset,newVertices[triangles[i]]+offset,Color.blue); }*/ //Debug.Log ("NavMesh - Old vertice count "+vertices.Length+", new vertice count "+c+" "+maxX+" "+maxZ+" "+maxX*maxZ); Int3[] totalIntVertices = vertices; vertices = new Int3[c]; originalVertices = new Vector3[c]; for (int i=0;i<c;i++) { vertices[i] = totalIntVertices[newVertices[i]];//(Int3)graph.matrix.MultiplyPoint (vectorVertices[i]); originalVertices[i] = vertices[i];//vectorVertices[newVertices[i]]; } Node[] nodes = graph.CreateNodes (triangles.Length/3);//new Node[triangles.Length/3]; graph.nodes = nodes; for (int i=0;i<nodes.Length;i++) { MeshNode node = (MeshNode)nodes[i];//new MeshNode (); node.walkable = true; node.position = (vertices[triangles[i*3]] + vertices[triangles[i*3+1]] + vertices[triangles[i*3+2]])/3F; node.v1 = triangles[i*3]; node.v2 = triangles[i*3+1]; node.v3 = triangles[i*3+2]; if (!Polygon.IsClockwise (vertices[node.v1],vertices[node.v2],vertices[node.v3])) { //Debug.DrawLine (vertices[node.v1],vertices[node.v2],Color.red); //Debug.DrawLine (vertices[node.v2],vertices[node.v3],Color.red); //Debug.DrawLine (vertices[node.v3],vertices[node.v1],Color.red); int tmp = node.v1; node.v1 = node.v3; node.v3 = tmp; } if (Polygon.IsColinear (vertices[node.v1],vertices[node.v2],vertices[node.v3])) { Debug.DrawLine (vertices[node.v1],vertices[node.v2],Color.red); Debug.DrawLine (vertices[node.v2],vertices[node.v3],Color.red); Debug.DrawLine (vertices[node.v3],vertices[node.v1],Color.red); } nodes[i] = node; } List<Node> connections = new List<Node> (); List<int> connectionCosts = new List<int> (); int identicalError = 0; for (int i=0;i<triangles.Length;i+=3) { connections.Clear (); connectionCosts.Clear (); //Int3 indices = new Int3(triangles[i],triangles[i+1],triangles[i+2]); Node node = nodes[i/3]; for (int x=0;x<triangles.Length;x+=3) { if (x == i) { continue; } int count = 0; if (triangles[x] == triangles[i]) { count++; } if (triangles[x+1] == triangles[i]) { count++; } if (triangles[x+2] == triangles[i]) { count++; } if (triangles[x] == triangles[i+1]) { count++; } if (triangles[x+1] == triangles[i+1]) { count++; } if (triangles[x+2] == triangles[i+1]) { count++; } if (triangles[x] == triangles[i+2]) { count++; } if (triangles[x+1] == triangles[i+2]) { count++; } if (triangles[x+2] == triangles[i+2]) { count++; } if (count >= 3) { identicalError++; Debug.DrawLine (vertices[triangles[x]],vertices[triangles[x+1]],Color.red); Debug.DrawLine (vertices[triangles[x]],vertices[triangles[x+2]],Color.red); Debug.DrawLine (vertices[triangles[x+2]],vertices[triangles[x+1]],Color.red); } if (count == 2) { Node other = nodes[x/3]; connections.Add (other); connectionCosts.Add (Mathf.RoundToInt ((node.position-other.position).magnitude)); } } node.connections = connections.ToArray (); node.connectionCosts = connectionCosts.ToArray (); } if (identicalError > 0) { Debug.LogError ("One or more triangles are identical to other triangles, this is not a good thing to have in a navmesh\nIncreasing the scale of the mesh might help\nNumber of triangles with error: "+identicalError+"\n"); } RebuildBBTree (graph); //Debug.Log ("Graph Generation - NavMesh - Time to compute graph "+((Time.realtimeSinceStartup-startTime)*1000F).ToString ("0")+"ms"); }
public static bool RightOrColinearXZ(Int3 a, Int3 b, Int3 p) { return((long)(b.x - a.x) * (long)(p.z - a.z) - (long)(p.x - a.x) * (long)(b.z - a.z) <= 0L); }
public override void DeserializeExtraInfo (GraphSerializationContext ctx) { uint graphIndex = (uint)active.astarData.GetGraphIndex(this); TriangleMeshNode.SetNavmeshHolder ((int)graphIndex,this); int c1 = ctx.reader.ReadInt32(); int c2 = ctx.reader.ReadInt32(); if (c1 == -1) { nodes = new TriangleMeshNode[0]; _vertices = new Int3[0]; originalVertices = new Vector3[0]; } nodes = new TriangleMeshNode[c1]; _vertices = new Int3[c2]; originalVertices = new Vector3[c2]; for (int i=0;i<c2;i++) { _vertices[i] = new Int3(ctx.reader.ReadInt32(), ctx.reader.ReadInt32(), ctx.reader.ReadInt32()); originalVertices[i] = new Vector3(ctx.reader.ReadSingle(), ctx.reader.ReadSingle(), ctx.reader.ReadSingle()); } bbTree = new BBTree(this); for (int i=0;i<c1;i++) { nodes[i] = new TriangleMeshNode(active); TriangleMeshNode node = nodes[i]; node.DeserializeNode(ctx); node.GraphIndex = graphIndex; node.UpdatePositionFromVertices(); bbTree.Insert (node); } }
public static bool IsClockwiseOrColinearXZ(Int3 a, Int3 b, Int3 c) { return(VectorMath.RightOrColinearXZ(a, b, c)); }
/** Returns the closest point of the node */ public static Vector3 ClosestPointOnNode (TriangleMeshNode node, Int3[] vertices, Vector3 pos) { return Polygon.ClosestPointOnTriangle ((Vector3)vertices[node.v0],(Vector3)vertices[node.v1],(Vector3)vertices[node.v2],pos); }
public static bool IsColinearXZ(Int3 a, Int3 b, Int3 c) { return((long)(b.x - a.x) * (long)(c.z - a.z) - (long)(c.x - a.x) * (long)(b.z - a.z) == 0L); }
/** Generates a navmesh. Based on the supplied vertices and triangles. Memory usage is about O(n) */ public void GenerateNodes (Vector3[] vectorVertices, int[] triangles, out Vector3[] originalVertices, out Int3[] vertices) { if (vectorVertices.Length == 0 || triangles.Length == 0) { originalVertices = vectorVertices; vertices = new Int3[0]; //graph.CreateNodes (0); nodes = new TriangleMeshNode[0]; return; } vertices = new Int3[vectorVertices.Length]; //Backup the original vertices //for (int i=0;i<vectorVertices.Length;i++) { // vectorVertices[i] = graph.matrix.MultiplyPoint (vectorVertices[i]); //} int c = 0; /*int maxX = 0; int maxZ = 0; //Almost infinity int minX = 0xFFFFFFF; int minZ = 0xFFFFFFF;*/ for (int i=0;i<vertices.Length;i++) { vertices[i] = (Int3)matrix.MultiplyPoint3x4 (vectorVertices[i]); /*maxX = Mathfx.Max (vertices[i].x, maxX); maxZ = Mathfx.Max (vertices[i].z, maxZ); minX = Mathfx.Min (vertices[i].x, minX); minZ = Mathfx.Min (vertices[i].z, minZ);*/ } //maxX = maxX-minX; //maxZ = maxZ-minZ; Dictionary<Int3,int> hashedVerts = new Dictionary<Int3,int> (); int[] newVertices = new int[vertices.Length]; for (int i=0;i<vertices.Length-1;i++) { //int hash = Mathfx.ComputeVertexHash (vertices[i].x,vertices[i].y,vertices[i].z); //(vertices[i].x-minX)+(vertices[i].z-minX)*maxX+vertices[i].y*maxX*maxZ; //if (sortedVertices[i] != sortedVertices[i+1]) { if (!hashedVerts.ContainsKey (vertices[i])) { newVertices[c] = i; hashedVerts.Add (vertices[i], c); c++; }// else { //Debug.Log ("Hash Duplicate "+hash+" "+vertices[i].ToString ()); //} } newVertices[c] = vertices.Length-1; //int hash2 = (newVertices[c].x-minX)+(newVertices[c].z-minX)*maxX+newVertices[c].y*maxX*maxZ; //int hash2 = Mathfx.ComputeVertexHash (newVertices[c].x,newVertices[c].y,newVertices[c].z); if (!hashedVerts.ContainsKey (vertices[newVertices[c]])) { hashedVerts.Add (vertices[newVertices[c]], c); c++; } for (int x=0;x<triangles.Length;x++) { Int3 vertex = vertices[triangles[x]]; //int hash3 = (vertex.x-minX)+(vertex.z-minX)*maxX+vertex.y*maxX*maxZ; //int hash3 = Mathfx.ComputeVertexHash (vertex.x,vertex.y,vertex.z); //for (int y=0;y<newVertices.Length;y++) { triangles[x] = hashedVerts[vertex]; } /*for (int i=0;i<triangles.Length;i += 3) { Vector3 offset = Vector3.forward*i*0.01F; Debug.DrawLine (newVertices[triangles[i]]+offset,newVertices[triangles[i+1]]+offset,Color.blue); Debug.DrawLine (newVertices[triangles[i+1]]+offset,newVertices[triangles[i+2]]+offset,Color.blue); Debug.DrawLine (newVertices[triangles[i+2]]+offset,newVertices[triangles[i]]+offset,Color.blue); }*/ //Debug.Log ("NavMesh - Old vertice count "+vertices.Length+", new vertice count "+c+" "+maxX+" "+maxZ+" "+maxX*maxZ); Int3[] totalIntVertices = vertices; vertices = new Int3[c]; originalVertices = new Vector3[c]; for (int i=0;i<c;i++) { vertices[i] = totalIntVertices[newVertices[i]];//(Int3)graph.matrix.MultiplyPoint (vectorVertices[i]); originalVertices[i] = (Vector3)vectorVertices[newVertices[i]];//vectorVertices[newVertices[i]]; } //graph.CreateNodes (triangles.Length/3);//new Node[triangles.Length/3]; nodes = new TriangleMeshNode[triangles.Length/3]; for (int i=0;i<nodes.Length;i++) { nodes[i] = new TriangleMeshNode(active); TriangleMeshNode node = nodes[i];//new MeshNode (); node.Penalty = initialPenalty; node.Walkable = true; node.v0 = triangles[i*3]; node.v1 = triangles[i*3+1]; node.v2 = triangles[i*3+2]; if (!Polygon.IsClockwise (vertices[node.v0],vertices[node.v1],vertices[node.v2])) { //Debug.DrawLine (vertices[node.v0],vertices[node.v1],Color.red); //Debug.DrawLine (vertices[node.v1],vertices[node.v2],Color.red); //Debug.DrawLine (vertices[node.v2],vertices[node.v0],Color.red); int tmp = node.v0; node.v0 = node.v2; node.v2 = tmp; } if (Polygon.IsColinear (vertices[node.v0],vertices[node.v1],vertices[node.v2])) { Debug.DrawLine ((Vector3)vertices[node.v0],(Vector3)vertices[node.v1],Color.red); Debug.DrawLine ((Vector3)vertices[node.v1],(Vector3)vertices[node.v2],Color.red); Debug.DrawLine ((Vector3)vertices[node.v2],(Vector3)vertices[node.v0],Color.red); } // Make sure position is correctly set node.UpdatePositionFromVertices(); } List<MeshNode> connections = new List<MeshNode> (); List<uint> connectionCosts = new List<uint> (); int identicalError = 0; for (int i=0;i<triangles.Length;i+=3) { connections.Clear (); connectionCosts.Clear (); //Int3 indices = new Int3(triangles[i],triangles[i+1],triangles[i+2]); TriangleMeshNode node = nodes[i/3]; for (int x=0;x<triangles.Length;x+=3) { if (x == i) { continue; } int count = 0; if (triangles[x] == triangles[i]) { count++; } if (triangles[x+1] == triangles[i]) { count++; } if (triangles[x+2] == triangles[i]) { count++; } if (triangles[x] == triangles[i+1]) { count++; } if (triangles[x+1] == triangles[i+1]) { count++; } if (triangles[x+2] == triangles[i+1]) { count++; } if (triangles[x] == triangles[i+2]) { count++; } if (triangles[x+1] == triangles[i+2]) { count++; } if (triangles[x+2] == triangles[i+2]) { count++; } if (count >= 3) { identicalError++; Debug.DrawLine ((Vector3)vertices[triangles[x]],(Vector3)vertices[triangles[x+1]],Color.red); Debug.DrawLine ((Vector3)vertices[triangles[x]],(Vector3)vertices[triangles[x+2]],Color.red); Debug.DrawLine ((Vector3)vertices[triangles[x+2]],(Vector3)vertices[triangles[x+1]],Color.red); } if (count == 2) { GraphNode other = nodes[x/3]; connections.Add (other as MeshNode); connectionCosts.Add ((uint)(node.position-other.position).costMagnitude); } } node.connections = connections.ToArray (); node.connectionCosts = connectionCosts.ToArray (); } if (identicalError > 0) { Debug.LogError ("One or more triangles are identical to other triangles, this is not a good thing to have in a navmesh\nIncreasing the scale of the mesh might help\nNumber of triangles with error: "+identicalError+"\n"); } RebuildBBTree (this); #if ASTARDEBUG for (int i=0;i<nodes.Length;i++) { TriangleMeshNode node = nodes[i] as TriangleMeshNode; float a1 = Polygon.TriangleArea2 ((Vector3)vertices[node.v0],(Vector3)vertices[node.v1],(Vector3)vertices[node.v2]); long a2 = Polygon.TriangleArea2 (vertices[node.v0],vertices[node.v1],vertices[node.v2]); if (a1 * a2 < 0) Debug.LogError (a1+ " " + a2); if (Polygon.IsClockwise (vertices[node.v0],vertices[node.v1],vertices[node.v2])) { Debug.DrawLine ((Vector3)vertices[node.v0],(Vector3)vertices[node.v1],Color.green); Debug.DrawLine ((Vector3)vertices[node.v1],(Vector3)vertices[node.v2],Color.green); Debug.DrawLine ((Vector3)vertices[node.v2],(Vector3)vertices[node.v0],Color.green); } else { Debug.DrawLine ((Vector3)vertices[node.v0],(Vector3)vertices[node.v1],Color.red); Debug.DrawLine ((Vector3)vertices[node.v1],(Vector3)vertices[node.v2],Color.red); Debug.DrawLine ((Vector3)vertices[node.v2],(Vector3)vertices[node.v0],Color.red); } } #endif //Debug.Log ("Graph Generation - NavMesh - Time to compute graph "+((Time.realtimeSinceStartup-startTime)*1000F).ToString ("0")+"ms"); }
public static bool IsColinearAlmostXZ(Int3 a, Int3 b, Int3 c) { long num = (long)(b.x - a.x) * (long)(c.z - a.z) - (long)(c.x - a.x) * (long)(b.z - a.z); return(num > -1L && num < 1L); }
/** Checks if \a p is inside the node in XZ space * * The default implementation uses XZ space and is in large part got from the website linked below * \author http://unifycommunity.com/wiki/index.php?title=PolyContainsPoint (Eric5h5) * * The TriangleMeshNode overrides this and implements faster code for that case. */ public virtual bool ContainsPoint(Int3 p) { bool inside = false; int count = GetVertexCount(); for (int i = 0, j=count-1; i < count; j = i++) { if ( ((GetVertex(i).z <= p.z && p.z < GetVertex(j).z) || (GetVertex(j).z <= p.z && p.z < GetVertex(i).z)) && (p.x < (GetVertex(j).x - GetVertex(i).x) * (p.z - GetVertex(i).z) / (GetVertex(j).z - GetVertex(i).z) + GetVertex(i).x)) inside = !inside; } return inside; }
public static bool SegmentsIntersectXZ(Int3 start1, Int3 end1, Int3 start2, Int3 end2) { return(VectorMath.RightOrColinearXZ(start1, end1, start2) != VectorMath.RightOrColinearXZ(start1, end1, end2) && VectorMath.RightOrColinearXZ(start2, end2, start1) != VectorMath.RightOrColinearXZ(start2, end2, end1)); }
/** Prepares the path. Searches for start and end nodes and does some simple checking if a path is at all possible */ public override void Prepare () { AstarProfiler.StartProfile ("Get Nearest"); //Initialize the NNConstraint nnConstraint.tags = enabledTags; NNInfo startNNInfo = AstarPath.active.GetNearest (startPoint,nnConstraint, startHint); //Tell the NNConstraint which node was found as the start node if it is a PathNNConstraint and not a normal NNConstraint var pathNNConstraint = nnConstraint as PathNNConstraint; if (pathNNConstraint != null) { pathNNConstraint.SetStart (startNNInfo.node); } startPoint = startNNInfo.clampedPosition; startIntPoint = (Int3)startPoint; startNode = startNNInfo.node; //If it is declared that this path type has an end point //Some path types might want to use most of the ABPath code, but will not have an explicit end point at this stage if (hasEndPoint) { NNInfo endNNInfo = AstarPath.active.GetNearest (endPoint,nnConstraint, endHint); endPoint = endNNInfo.clampedPosition; // Note, other methods assume hTarget is (Int3)endPoint hTarget = (Int3)endPoint; endNode = endNNInfo.node; hTargetNode = endNode; } AstarProfiler.EndProfile (); #if ASTARDEBUG if (startNode != null) Debug.DrawLine ((Vector3)startNode.position,startPoint,Color.blue); if (endNode != null) Debug.DrawLine ((Vector3)endNode.position,endPoint,Color.blue); #endif if (startNode == null && (hasEndPoint && endNode == null)) { Error (); LogError ("Couldn't find close nodes to the start point or the end point"); return; } if (startNode == null) { Error (); LogError ("Couldn't find a close node to the start point"); return; } if (endNode == null && hasEndPoint) { Error (); LogError ("Couldn't find a close node to the end point"); return; } if (!startNode.Walkable) { #if ASTARDEBUG Debug.DrawRay (startPoint,Vector3.up,Color.red); Debug.DrawLine (startPoint,(Vector3)startNode.position,Color.red); #endif Error (); LogError ("The node closest to the start point is not walkable"); return; } if (hasEndPoint && !endNode.Walkable) { Error (); LogError ("The node closest to the end point is not walkable"); return; } if (hasEndPoint && startNode.Area != endNode.Area) { Error (); LogError ("There is no valid path to the target (start area: "+startNode.Area+", target area: "+endNode.Area+")"); return; } }
public static bool RaySegmentIntersectXZ(Int3 start1, Int3 end1, Int3 start2, Int3 end2) { Int3 @int = end1 - start1; Int3 int2 = end2 - start2; long num = (long)(int2.z * @int.x - int2.x * @int.z); if (num == 0L) { return(false); } long num2 = (long)(int2.x * (start1.z - start2.z) - int2.z * (start1.x - start2.x)); long num3 = (long)(@int.x * (start1.z - start2.z) - @int.z * (start1.x - start2.x)); return((num2 < 0L ^ num < 0L) && (num3 < 0L ^ num < 0L) && (num < 0L || num3 <= num) && (num >= 0L || num3 > num)); }
public override void DeserializeNode (GraphSerializationContext ctx) { base.DeserializeNode (ctx); position = new Int3(ctx.reader.ReadInt32(), ctx.reader.ReadInt32(), ctx.reader.ReadInt32()); gridFlags = ctx.reader.ReadUInt16(); #if ASTAR_LEVELGRIDNODE_FEW_LAYERS gridConnections = ctx.reader.ReadUInt16(); #else gridConnections = ctx.reader.ReadUInt32(); #endif }
public static bool LineIntersectionFactorXZ(Int3 start1, Int3 end1, Int3 start2, Int3 end2, out float factor1, out float factor2) { Int3 @int = end1 - start1; Int3 int2 = end2 - start2; long num = (long)(int2.z * @int.x - int2.x * @int.z); if (num == 0L) { factor1 = 0f; factor2 = 0f; return(false); } long num2 = (long)(int2.x * (start1.z - start2.z) - int2.z * (start1.x - start2.x)); long num3 = (long)(@int.x * (start1.z - start2.z) - @int.z * (start1.x - start2.x)); factor1 = (float)num2 / (float)num; factor2 = (float)num3 / (float)num; return(true); }
/** Sets the start and end points. * Sets #originalStartPoint, #originalEndPoint, #startPoint, #endPoint, #startIntPoint and #hTarget (to \a end ) */ public virtual void UpdateStartEnd(Vector3 start, Vector3 end) { originalStartPoint = start; originalEndPoint = end; startPoint = start; endPoint = end; startIntPoint = (Int3)start; hTarget = (Int3)end; }
public static float LineRayIntersectionFactorXZ(Int3 start1, Int3 end1, Int3 start2, Int3 end2) { Int3 @int = end1 - start1; Int3 int2 = end2 - start2; int num = int2.z * @int.x - int2.x * @int.z; if (num == 0) { return(float.NaN); } int num2 = int2.x * (start1.z - start2.z) - int2.z * (start1.x - start2.x); int num3 = @int.x * (start1.z - start2.z) - @int.z * (start1.x - start2.x); if ((float)num3 / (float)num < 0f) { return(float.NaN); } return((float)num2 / (float)num); }
/** Returns if \a _b is visible from \a _a on the graph. * This function is different from the other Linecast functions since it 1) snaps the start and end positions directly to the graph * and it uses Bresenham's line drawing algorithm as opposed to the others which use sampling at fixed intervals. * If you only care about if one \b node can see another \b node, then this function is great, but if you need more precision than one node, * use the normal linecast functions * \param [in] _a Point to linecast from * \param [in] _b Point to linecast to * \param [out] hit Contains info on what was hit, see GraphHitInfo * \param [in] hint (deprecated) If you have some idea of what the start node might be (the one close to \a _a), pass it to hint since it can enable faster lookups. * * This is not the same as Physics.Linecast, this function traverses the graph and looks for collisions. * \astarpro */ public new bool SnappedLinecast(Vector3 _a, Vector3 _b, Node hint, out GraphHitInfo hit) { hit = new GraphHitInfo (); //System.DateTime startTime = System.DateTime.UtcNow; LevelGridNode n1 = GetNearest (_a,NNConstraint.None).node as LevelGridNode; LevelGridNode n2 = GetNearest (_b,NNConstraint.None).node as LevelGridNode; if (n1 == null || n2 == null) { hit.node = null; hit.point = _a; return true; } _a = inverseMatrix.MultiplyPoint3x4 ((Vector3)n1.position); _a.x -= 0.5F; _a.z -= 0.5F; _b = inverseMatrix.MultiplyPoint3x4 ((Vector3)n2.position); _b.x -= 0.5F; _b.z -= 0.5F; Int3 a = new Int3 (Mathf.RoundToInt (_a.x),Mathf.RoundToInt (_a.y),Mathf.RoundToInt (_a.z)); Int3 b = new Int3 (Mathf.RoundToInt (_b.x),Mathf.RoundToInt (_b.y),Mathf.RoundToInt (_b.z)); hit.origin = (Vector3)a; //Debug.DrawLine (matrix.MultiplyPoint3x4 (a*100),matrix.MultiplyPoint3x4 (b*100),Color.yellow); if (!n1.walkable) {//nodes[a.z*width+a.x].walkable) { hit.node = n1;//nodes[a.z*width+a.x]; hit.point = matrix.MultiplyPoint3x4 (new Vector3 (a.x+0.5F,0,a.z+0.5F)); hit.point.y = ((Vector3)hit.node.position).y; return true; } int dx = Mathf.Abs (a.x-b.x); int dz = Mathf.Abs (a.z-b.z); LevelGridNode currentNode = n1; while (true) { if (currentNode == n2) { //a.x == b.x && a.z == b.z) { //System.DateTime endTime2 = System.DateTime.UtcNow; //float theTime2 = (endTime2-startTime).Ticks*0.0001F; //Debug.Log ("Grid Linecast : Time "+theTime2.ToString ("0.00")); return false; } //The nodes are at the same position in the graph when seen from above if (currentNode.GetIndex() == n2.GetIndex()) { hit.node = currentNode; hit.point = (Vector3)currentNode.position; return true; } dx = System.Math.Abs(a.x-b.x); dz = System.Math.Abs(a.z-b.z); int dir = 0; if (dx >= dz) { dir = b.x>a.x ? 1 : 3; } else if (dz > dx) { dir = b.z>a.z ? 2 : 0; } if (CheckConnection (currentNode,dir)) { LevelGridNode other = nodes[currentNode.GetIndex()+neighbourOffsets[dir] + width*depth*currentNode.GetConnectionValue(dir)] as LevelGridNode; if (!other.walkable) { hit.node = other; hit.point = (Vector3)other.position; return true; } //Debug.DrawLine (matrix.MultiplyPoint3x4 (a*100),matrix.MultiplyPoint3x4 (newPos*100)); a = (Int3)inverseMatrix.MultiplyPoint3x4 ((Vector3)other.position); currentNode = other; } else { hit.node = currentNode; hit.point = (Vector3)currentNode.position;//matrix.MultiplyPoint3x4 (new Vector3 (a.x+0.5F,0,a.z+0.5F)); return true; } /*int e2 = err*2; Int3 newPos = a; if (e2 > -dz) { err = err-dz; dir = sx; newPos.x += sx; } if (e2 < dx) { err = err+dx; dir += width*sz; newPos.z += sz; } if (dir == 0) { Debug.LogError ("Offset is zero, this should not happen"); return false; } for (int i=0;i<neighbourOffsets.Length;i++) { if (neighbourOffsets[i] == dir) { if (CheckConnection (nodes[a.z*width+a.x] as LevelGridNode,i)) { if (!nodes[newPos.z*width+newPos.x].walkable) { hit.node = nodes[a.z*width+a.x]; hit.point = matrix.MultiplyPoint3x4 (new Vector3 (a.x+0.5F,0,a.z+0.5F)); hit.point.y = ((Vector3)hit.node.position).y; return true; } //Debug.DrawLine (matrix.MultiplyPoint3x4 (a*100),matrix.MultiplyPoint3x4 (newPos*100)); a = newPos; break; } else { hit.node = nodes[a.z*width+a.x]; hit.point = matrix.MultiplyPoint3x4 (new Vector3 (a.x+0.5F,0,a.z+0.5F)); hit.point.y = ((Vector3)hit.node.position).y; return true; } } }*/ } //Debug.DrawLine (_a,_b,Color.green); //hit.success = true; }
public void SetPosition(Int3 value) { position = value; }
/** Returns if \a _b is visible from \a _a on the graph. * This function is different from the other Linecast functions since it 1) snaps the start and end positions directly to the graph * and it uses Bresenham's line drawing algorithm as opposed to the others which use sampling at fixed intervals. * If you only care about if one \b node can see another \b node, then this function is great, but if you need more precision than one node, * use the normal linecast functions * * \param [in] _a Point to linecast from * \param [in] _b Point to linecast to * \param [out] hit Contains info on what was hit, see GraphHitInfo * \param [in] hint If you have some idea of what the start node might be (the one close to \a _a), pass it to hint since it can enable faster lookups * * This is not the same as Physics.Linecast, this function traverses the graph and looks for collisions. * \astarpro */ public bool SnappedLinecast (Vector3 _a, Vector3 _b, GraphNode hint, out GraphHitInfo hit) { hit = new GraphHitInfo (); //System.DateTime startTime = System.DateTime.UtcNow; GraphNode n1 = GetNearest (_a,NNConstraint.None).node; GraphNode n2 = GetNearest (_b,NNConstraint.None).node; _a = inverseMatrix.MultiplyPoint3x4 ((Vector3)n1.position); _a.x -= 0.5F; _a.z -= 0.5F; _b = inverseMatrix.MultiplyPoint3x4 ((Vector3)n2.position); _b.x -= 0.5F; _b.z -= 0.5F; Int3 a = new Int3 (Mathf.RoundToInt (_a.x),Mathf.RoundToInt (_a.y),Mathf.RoundToInt (_a.z)); Int3 b = new Int3 (Mathf.RoundToInt (_b.x),Mathf.RoundToInt (_b.y),Mathf.RoundToInt (_b.z)); hit.origin = (Vector3)a; //Debug.DrawLine (matrix.MultiplyPoint3x4 (a*100),matrix.MultiplyPoint3x4 (b*100),Color.yellow); if (!nodes[a.z*width+a.x].Walkable) { hit.node = nodes[a.z*width+a.x]; hit.point = matrix.MultiplyPoint3x4 (new Vector3 (a.x+0.5F,0,a.z+0.5F)); hit.point.y = ((Vector3)hit.node.position).y; return true; } int dx = Mathf.Abs (a.x-b.x); int dz = Mathf.Abs (a.z-b.z); int sx = 0; int sz = 0; if (a.x < b.x) { sx = 1; } else { sx = -1; } if (a.z < b.z) { sz = 1; } else { sz = -1; } int err = dx-dz; while (true) { if (a.x == b.x && a.z == b.z) { //System.DateTime endTime2 = System.DateTime.UtcNow; //float theTime2 = (endTime2-startTime).Ticks*0.0001F; //Debug.Log ("Grid Linecast : Time "+theTime2.ToString ("0.00")); return false; } int e2 = err*2; int dir = 0; Int3 newPos = a; if (e2 > -dz) { err = err-dz; dir = sx; newPos.x += sx; } if (e2 < dx) { err = err+dx; dir += width*sz; newPos.z += sz; } if (dir == 0) { Debug.LogError ("Offset is zero, this should not happen"); return false; } for (int i=0;i<neighbourOffsets.Length;i++) { if (neighbourOffsets[i] == dir) { if (CheckConnection (nodes[a.z*width+a.x] as GridNode,i)) { if (!nodes[newPos.z*width+newPos.x].Walkable) { hit.node = nodes[a.z*width+a.x]; hit.point = matrix.MultiplyPoint3x4 (new Vector3 (a.x+0.5F,0,a.z+0.5F)); hit.point.y = ((Vector3)hit.node.position).y; return true; } //Debug.DrawLine (matrix.MultiplyPoint3x4 (a*100),matrix.MultiplyPoint3x4 (newPos*100)); a = newPos; break; } else { hit.node = nodes[a.z*width+a.x]; hit.point = matrix.MultiplyPoint3x4 (new Vector3 (a.x+0.5F,0,a.z+0.5F)); hit.point.y = ((Vector3)hit.node.position).y; return true; } } } } //Debug.DrawLine (_a,_b,Color.green); //hit.success = true; //return false; }
public static void UpdateArea(GraphUpdateObject o, INavmesh graph) { Bounds bounds = o.bounds; Rect rect = Rect.MinMaxRect(bounds.min.x, bounds.min.z, bounds.max.x, bounds.max.z); IntRect irect = new IntRect(Mathf.FloorToInt(bounds.min.x * 1000f), Mathf.FloorToInt(bounds.min.z * 1000f), Mathf.FloorToInt(bounds.max.x * 1000f), Mathf.FloorToInt(bounds.max.z * 1000f)); Int3 a = new Int3(irect.xmin, 0, irect.ymin); Int3 b = new Int3(irect.xmin, 0, irect.ymax); Int3 c = new Int3(irect.xmax, 0, irect.ymin); Int3 d = new Int3(irect.xmax, 0, irect.ymax); int ymin = ((Int3)bounds.min).y; int ymax = ((Int3)bounds.max).y; graph.GetNodes(delegate(GraphNode _node) { TriangleMeshNode triangleMeshNode = _node as TriangleMeshNode; bool flag = false; int num = 0; int num2 = 0; int num3 = 0; int num4 = 0; for (int i = 0; i < 3; i++) { Int3 vertex = triangleMeshNode.GetVertex(i); Vector3 vector = (Vector3)vertex; if (irect.Contains(vertex.x, vertex.z)) { flag = true; break; } if (vector.x < rect.xMin) { num++; } if (vector.x > rect.xMax) { num2++; } if (vector.z < rect.yMin) { num3++; } if (vector.z > rect.yMax) { num4++; } } if (!flag && (num == 3 || num2 == 3 || num3 == 3 || num4 == 3)) { return; } for (int j = 0; j < 3; j++) { int i2 = (j <= 1) ? (j + 1) : 0; Int3 vertex2 = triangleMeshNode.GetVertex(j); Int3 vertex3 = triangleMeshNode.GetVertex(i2); if (VectorMath.SegmentsIntersectXZ(a, b, vertex2, vertex3)) { flag = true; break; } if (VectorMath.SegmentsIntersectXZ(a, c, vertex2, vertex3)) { flag = true; break; } if (VectorMath.SegmentsIntersectXZ(c, d, vertex2, vertex3)) { flag = true; break; } if (VectorMath.SegmentsIntersectXZ(d, b, vertex2, vertex3)) { flag = true; break; } } if (flag || triangleMeshNode.ContainsPoint(a) || triangleMeshNode.ContainsPoint(b) || triangleMeshNode.ContainsPoint(c) || triangleMeshNode.ContainsPoint(d)) { flag = true; } if (!flag) { return; } int num5 = 0; int num6 = 0; for (int k = 0; k < 3; k++) { Int3 vertex4 = triangleMeshNode.GetVertex(k); if (vertex4.y < ymin) { num6++; } if (vertex4.y > ymax) { num5++; } } if (num6 == 3 || num5 == 3) { return; } o.WillUpdateNode(triangleMeshNode); o.Apply(triangleMeshNode); }); }
public override void DeserializeNode (GraphSerializationContext ctx) { base.DeserializeNode (ctx); position = new Int3 (ctx.reader.ReadInt32(), ctx.reader.ReadInt32(), ctx.reader.ReadInt32()); }
protected override void Prepare() { AstarProfiler.StartProfile("Get Nearest"); nnConstraint.tags = enabledTags; var startNNInfo = AstarPath.active.GetNearest(startPoint, nnConstraint); //Tell the NNConstraint which node was found as the start node if it is a PathNNConstraint and not a normal NNConstraint var pathNNConstraint = nnConstraint as PathNNConstraint; if (pathNNConstraint != null) { pathNNConstraint.SetStart(startNNInfo.node); } startPoint = startNNInfo.position; startIntPoint = (Int3)startPoint; startNode = startNNInfo.node; if (startNode == null) { FailWithError("Couldn't find a node close to the start point"); return; } if (!CanTraverse(startNode)) { FailWithError("The node closest to the start point could not be traversed"); return; } // If it is declared that this path type has an end point if (hasEndPoint) { var endNNInfo = AstarPath.active.GetNearest(endPoint, nnConstraint); endPoint = endNNInfo.position; endNode = endNNInfo.node; if (endNode == null) { FailWithError("Couldn't find a node close to the end point"); return; } // This should not trigger unless the user has modified the NNConstraint if (!CanTraverse(endNode)) { FailWithError("The node closest to the end point could not be traversed"); return; } // This should not trigger unless the user has modified the NNConstraint if (startNode.Area != endNode.Area) { FailWithError("There is no valid path to the target"); return; } #if !ASTAR_NO_GRID_GRAPH if (!EndPointGridGraphSpecialCase(endNNInfo.node)) #endif { // Note, other methods assume hTarget is (Int3)endPoint hTarget = (Int3)endPoint; hTargetNode = endNode; // Mark end node with flag1 to mark it as a target point pathHandler.GetPathNode(endNode).flag1 = true; } } AstarProfiler.EndProfile(); }