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