// Update is called once per frame
void LateUpdate () {
if (prevNode == null) {
NNInfo nninfo = AstarPath.active.GetNearest(transform.position);
prevNode = nninfo.node;
prevPos = transform.position;
}
if (prevNode == null) {
return;
}
if (prevNode != null) {
var graph = AstarData.GetGraph(prevNode) as IRaycastableGraph;
if (graph != null) {
GraphHitInfo hit;
if (graph.Linecast(prevPos, transform.position, prevNode, out hit)) {
hit.point.y = transform.position.y;
Vector3 closest = VectorMath.ClosestPointOnLine(hit.tangentOrigin, hit.tangentOrigin+hit.tangent, transform.position);
Vector3 ohit = hit.point;
ohit = ohit + Vector3.ClampMagnitude((Vector3)hit.node.position-ohit, 0.008f);
if (graph.Linecast(ohit, closest, hit.node, out hit)) {
hit.point.y = transform.position.y;
transform.position = hit.point;
} else {
closest.y = transform.position.y;
transform.position = closest;
}
}
prevNode = hit.node;
}
}
prevPos = transform.position;
}
public GraphNode GetOther ( GraphNode a ) {
if ( this.connections.Length < 2 ) return null;
if ( this.connections.Length != 2 ) throw new System.Exception ("Invalid NodeLink3Node. Expected 2 connections, found " + this.connections.Length);
return a == connections[0] ? (connections[1] as NodeLink3Node).GetOtherInternal(this) : (connections[0] as NodeLink3Node).GetOtherInternal(this);
}
public override bool GetPortal (GraphNode other, List<Vector3> left, List<Vector3> right, bool backwards)
{
if ( this.connections.Length < 2 ) return false;
if ( this.connections.Length != 2 ) throw new System.Exception ("Invalid NodeLink3Node. Expected 2 connections, found " + this.connections.Length);
//if ( other != connections[0] || other != connections[1] ) return false;
if ( left != null ) {
//Debug.DrawLine ( portalA, portalB, Color.red);
left.Add ( portalA );
right.Add ( portalB );
/*
Vector3 normal = link.transform.forward;
Vector3 tangent = Vector3.Dot (normal, (Vector3)(other.Position - this.Position) ) > 0 ? link.transform.right*0.5f : -link.transform.right*0.5f;
Debug.DrawLine ( link.transform.position -tangent * link.portalWidth, link.transform.position +tangent * link.portalWidth, Color.red);
Debug.DrawRay ( link.transform.position -tangent * link.portalWidth, Vector3.up*5, Color.red);
Debug.Break ();
left.Add ( link.transform.position -tangent * link.portalWidth );
right.Add (link.transform.position +tangent * link.portalWidth );*/
}
return true;
}
public static FloodPath Construct (GraphNode start, OnPathDelegate callback = null) {
if (start == null) throw new ArgumentNullException("start");
var p = PathPool.GetPath<FloodPath>();
p.Setup(start, callback);
return p;
}
public override void AddConnection(GraphNode node, uint cost)
{
if (this.connections != null)
{
for (int i = 0; i < this.connections.Length; i++)
{
if (this.connections[i] == node)
{
this.connectionCosts[i] = cost;
return;
}
}
}
int num = (this.connections == null) ? 0 : this.connections.Length;
GraphNode[] array = new GraphNode[num + 1];
uint[] array2 = new uint[num + 1];
for (int j = 0; j < num; j++)
{
array[j] = this.connections[j];
array2[j] = this.connectionCosts[j];
}
array[num] = node;
array2[num] = cost;
this.connections = array;
this.connectionCosts = array2;
}
public override void Execute()
{
if(Path.path.Count >= Index)
{ return; }
Node = Path.path[Index];
}
/** Returns all nodes reachable from the seed node.
* This function performs a BFS (breadth-first-search) or flood fill of the graph and returns all nodes which can be reached from
* the seed node. In almost all cases this will be identical to returning all nodes which have the same area as the seed node.
* In the editor areas are displayed as different colors of the nodes.
* The only case where it will not be so is when there is a one way path from some part of the area to the seed node
* but no path from the seed node to that part of the graph.
*
* The returned list is sorted by node distance from the seed node
* i.e distance is measured in the number of nodes the shortest path from \a seed to that node would pass through.
* Note that the distance measurement does not take heuristics, penalties or tag penalties.
*
* Depending on the number of reachable nodes, this function can take quite some time to calculate
* so don't use it too often or it might affect the framerate of your game.
*
* \param seed The node to start the search from
* \param tagMask Optional mask for tags. This is a bitmask.
*
* \returns A List<Node> containing all nodes reachable from the seed node.
* For better memory management the returned list should be pooled, see Pathfinding.Util.ListPool
*/
public static List<GraphNode> GetReachableNodes (GraphNode seed, int tagMask = -1) {
var stack = StackPool<GraphNode>.Claim ();
var list = ListPool<GraphNode>.Claim ();
/** \todo Pool */
var map = new HashSet<GraphNode>();
GraphNodeDelegate callback;
if (tagMask == -1) {
callback = delegate (GraphNode node) {
if (node.Walkable && map.Add (node)) {
list.Add (node);
stack.Push (node);
}
};
} else {
callback = delegate (GraphNode node) {
if (node.Walkable && ((tagMask >> (int)node.Tag) & 0x1) != 0 && map.Add (node)) {
list.Add (node);
stack.Push (node);
}
};
}
callback (seed);
while (stack.Count > 0) {
stack.Pop ().GetConnections (callback);
}
StackPool<GraphNode>.Release (stack);
return list;
}
/** Add a connection from this node to the specified node.
* If the connection already exists, the cost will simply be updated and
* no extra connection added.
*
* \note Only adds a one-way connection. Consider calling the same function on the other node
* to get a two-way connection.
*/
public override void AddConnection (GraphNode node, uint cost) {
if (connections != null) {
for (int i=0;i<connections.Length;i++) {
if (connections[i] == node) {
connectionCosts[i] = cost;
return;
}
}
}
int connLength = connections != null ? connections.Length : 0;
GraphNode[] newconns = new GraphNode[connLength+1];
uint[] newconncosts = new uint[connLength+1];
for (int i=0;i<connLength;i++) {
newconns[i] = connections[i];
newconncosts[i] = connectionCosts[i];
}
newconns[connLength] = node;
newconncosts[connLength] = cost;
connections = newconns;
connectionCosts = newconncosts;
}
public override void Execute()
{
var closestResult = AstarPath.active.GetNearest(Position);
if (closestResult.node == null) { return; }
NearestNode = closestResult.node;
}
/** Returns all nodes reachable from the seed node.
* This function performs a BFS (breadth-first-search) or flood fill of the graph and returns all nodes which can be reached from
* the seed node. In almost all cases this will be identical to returning all nodes which have the same area as the seed node.
* In the editor areas are displayed as different colors of the nodes.
* The only case where it will not be so is when there is a one way path from some part of the area to the seed node
* but no path from the seed node to that part of the graph.
*
* The returned list is sorted by node distance from the seed node
* i.e distance is measured in the number of nodes the shortest path from \a seed to that node would pass through.
* Note that the distance measurement does not take heuristics, penalties or tag penalties.
*
* Depending on the number of reachable nodes, this function can take quite some time to calculate
* so don't use it too often or it might affect the framerate of your game.
*
* \param seed The node to start the search from
* \param tagMask Optional mask for tags. This is a bitmask.
*
* \returns A List<Node> containing all nodes reachable from the seed node.
* For better memory management the returned list should be pooled, see Pathfinding.Util.ListPool
*/
public static List<GraphNode> GetReachableNodes (GraphNode seed, int tagMask = -1) {
Stack<GraphNode> stack = Pathfinding.Util.StackPool<GraphNode>.Claim ();
List<GraphNode> list = Pathfinding.Util.ListPool<GraphNode>.Claim ();
HashSet<GraphNode> map = new HashSet<GraphNode>();
GraphNodeDelegate callback;
if (tagMask == -1) {
callback = delegate (GraphNode node) {
if (node.Walkable && map.Add (node)) {
list.Add (node);
stack.Push (node);
}
};
} else {
callback = delegate (GraphNode node) {
if (node.Walkable && ((tagMask >> (int)node.Tag) & 0x1) != 0 && map.Add (node)) {
list.Add (node);
stack.Push (node);
}
};
}
callback (seed);
while (stack.Count > 0) {
stack.Pop ().GetConnections (callback);
}
Pathfinding.Util.StackPool<GraphNode>.Release (stack);
return list;
}
/** Updates graphs and checks if all nodes are still reachable from each other.
* Graphs are updated, then a check is made to see if the nodes are still reachable from each other.
* If they are not, the graphs are reverted to before the update and \a false is returned.\n
* This is slower than a normal graph update.
* All queued graph updates and thread safe callbacks will be flushed during this function.
*
* \note This might return true for small areas even if there is no possible path if AstarPath.minAreaSize is greater than zero (0).
* So when using this, it is recommended to set AstarPath.minAreaSize to 0 (A* Inspector -> Settings -> Pathfinding)
*
* \param guo The GraphUpdateObject to update the graphs with
* \param node1 Node which should have a valid path to \a node2. All nodes should be walkable or \a false will be returned.
* \param node2 Node which should have a valid path to \a node1. All nodes should be walkable or \a false will be returned.
* \param alwaysRevert If true, reverts the graphs to the old state even if no blocking ocurred
*
* \returns True if the given nodes are still reachable from each other after the \a guo has been applied. False otherwise.
*
\code
var guo = new GraphUpdateObject (tower.GetComponent<Collider>.bounds);
var spawnPointNode = AstarPath.active.GetNearest (spawnPoint.position).node;
var goalNode = AstarPath.active.GetNearest (goalNode.position).node;
if (GraphUpdateUtilities.UpdateGraphsNoBlock (guo, spawnPointNode, goalNode, false)) {
// Valid tower position
// Since the last parameter (which is called "alwaysRevert") in the method call was false
// The graph is now updated and the game can just continue
} else {
// Invalid tower position. It blocks the path between the spawn point and the goal
// The effect on the graph has been reverted
Destroy (tower);
}
\endcode
*/
public static bool UpdateGraphsNoBlock (GraphUpdateObject guo, GraphNode node1, GraphNode node2, bool alwaysRevert = false) {
List<GraphNode> buffer = ListPool<GraphNode>.Claim ();
buffer.Add (node1);
buffer.Add (node2);
bool worked = UpdateGraphsNoBlock (guo, buffer, alwaysRevert);
ListPool<GraphNode>.Release (buffer);
return worked;
}
public GraphHitInfo(Vector3 point)
{
this.tangentOrigin = Vector3.zero;
this.origin = Vector3.zero;
this.point = point;
this.node = null;
this.tangent = Vector3.zero;
}
/** Adds a node to the heap */
public void Add(GraphNode node) {
if (node == null) {
Debug.Log ("Sending null node to BinaryHeap");
return;
}
if (numberOfItems == binaryHeap.Length) {
Debug.Log ("Forced to discard nodes because of binary heap size limit, please consider increasing the size ("+numberOfItems +" "+binaryHeap.Length+")");
numberOfItems--;
}
binaryHeap[numberOfItems] = node;
//node.heapIndex = numberOfItems;//Heap index
int bubbleIndex = numberOfItems;
uint nodeF = node.f;
while (bubbleIndex != 1) {
int parentIndex = bubbleIndex / 2;
/*if (binaryHeap[parentIndex] == null) {
Debug.Log ("WUT!!");
return;
}*/
if (nodeF <= binaryHeap[parentIndex].f) {
//binaryHeap[bubbleIndex].f <= binaryHeap[parentIndex].f) { /** \todo Wouldn't it be more efficient with '<' instead of '<=' ? * /
//Node tmpValue = binaryHeap[parentIndex];
//tmpValue.heapIndex = bubbleIndex;//HeapIndex
binaryHeap[bubbleIndex] = binaryHeap[parentIndex];
binaryHeap[parentIndex] = node;//binaryHeap[bubbleIndex];
//binaryHeap[bubbleIndex].heapIndex = bubbleIndex; //Heap index
//binaryHeap[parentIndex].heapIndex = parentIndex; //Heap index
bubbleIndex = parentIndex;
} else {
/*if (binaryHeap[bubbleIndex].f <= binaryHeap[parentIndex].f) { /** \todo Wouldn't it be more efficient with '<' instead of '<=' ? *
Node tmpValue = binaryHeap[parentIndex];
//tmpValue.heapIndex = bubbleIndex;//HeapIndex
binaryHeap[parentIndex] = binaryHeap[bubbleIndex];
binaryHeap[bubbleIndex] = tmpValue;
bubbleIndex = parentIndex;
} else {*/
break;
}
}
numberOfItems++;
}
public static bool InSearchTree(GraphNode node, Path path)
{
if (path == null || path.pathHandler == null)
{
return true;
}
PathNode pathNode = path.pathHandler.GetPathNode(node);
return pathNode.pathID == path.pathID;
}
public override bool ContainsConnection(GraphNode node)
{
for (int i = 0; i < this.connections.Length; i++)
{
if (this.connections[i] == node)
{
return true;
}
}
return false;
}
public override void Execute()
{
if(Path.vectorPath.Count == 0) { return; }
var closestNode = Path.FindClosestNodeTo(PositionToCheck);
OutputNode = closestNode;
if (DistanceToClosest > 0 || DistanceToClosest < 0)
{
var nodePosition = (Vector3) closestNode.position;
DistanceToClosest = Vector3.Distance(nodePosition, PositionToCheck);
}
}
/** Returns all nodes up to a given node-distance from the seed node.
* This function performs a BFS (breadth-first-search) or flood fill of the graph and returns all nodes within a specified node distance which can be reached from
* the seed node. In almost all cases when \a depth is large enough this will be identical to returning all nodes which have the same area as the seed node.
* In the editor areas are displayed as different colors of the nodes.
* The only case where it will not be so is when there is a one way path from some part of the area to the seed node
* but no path from the seed node to that part of the graph.
*
* The returned list is sorted by node distance from the seed node
* i.e distance is measured in the number of nodes the shortest path from \a seed to that node would pass through.
* Note that the distance measurement does not take heuristics, penalties or tag penalties.
*
* Depending on the number of nodes, this function can take quite some time to calculate
* so don't use it too often or it might affect the framerate of your game.
*
* \param seed The node to start the search from.
* \param depth The maximum node-distance from the seed node.
* \param tagMask Optional mask for tags. This is a bitmask.
*
* \returns A List<Node> containing all nodes reachable up to a specified node distance from the seed node.
* For better memory management the returned list should be pooled, see Pathfinding.Util.ListPool
*
* \warning This method is not thread safe. Only use it from the Unity thread (i.e normal game code).
*/
public static List<GraphNode> BFS(GraphNode seed, int depth, int tagMask = -1)
{
BFSQueue = BFSQueue ?? new Queue<GraphNode>();
var que = BFSQueue;
BFSMap = BFSMap ?? new Dictionary<GraphNode,int>();
var map = BFSMap;
// Even though we clear at the end of this function, it is good to
// do it here as well in case the previous invocation of the method
// threw an exception for some reason
// and didn't clear the que and map
que.Clear ();
map.Clear ();
List<GraphNode> result = ListPool<GraphNode>.Claim ();
int currentDist = -1;
GraphNodeDelegate callback;
if (tagMask == -1) {
callback = node => {
if (node.Walkable && !map.ContainsKey (node)) {
map.Add (node, currentDist+1);
result.Add (node);
que.Enqueue (node);
}
};
} else {
callback = node => {
if (node.Walkable && ((tagMask >> (int)node.Tag) & 0x1) != 0 && !map.ContainsKey (node)) {
map.Add (node, currentDist+1);
result.Add (node);
que.Enqueue (node);
}
};
}
callback (seed);
while (que.Count > 0 ) {
GraphNode n = que.Dequeue ();
currentDist = map[n];
if (currentDist >= depth) break;
n.GetConnections (callback);
}
que.Clear ();
map.Clear ();
return result;
}
static int CanTraverse(IntPtr L)
{
try
{
ToLua.CheckArgsCount(L, 2);
Pathfinding.Path obj = (Pathfinding.Path)ToLua.CheckObject <Pathfinding.Path>(L, 1);
Pathfinding.GraphNode arg0 = (Pathfinding.GraphNode)ToLua.CheckObject <Pathfinding.GraphNode>(L, 2);
bool o = obj.CanTraverse(arg0);
LuaDLL.lua_pushboolean(L, o);
return(1);
}
catch (Exception e)
{
return(LuaDLL.toluaL_exception(L, e));
}
}
static int CalculateHScore(IntPtr L)
{
try
{
ToLua.CheckArgsCount(L, 2);
Pathfinding.Path obj = (Pathfinding.Path)ToLua.CheckObject <Pathfinding.Path>(L, 1);
Pathfinding.GraphNode arg0 = (Pathfinding.GraphNode)ToLua.CheckObject <Pathfinding.GraphNode>(L, 2);
uint o = obj.CalculateHScore(arg0);
LuaDLL.lua_pushnumber(L, o);
return(1);
}
catch (Exception e)
{
return(LuaDLL.toluaL_exception(L, e));
}
}
public void Trace(GraphNode from)
{
GraphNode graphNode = from;
int num = 0;
while (graphNode != null)
{
this.path.Add(graphNode);
this.vectorPath.Add((Vector3)graphNode.position);
graphNode = this.flood.GetParent(graphNode);
num++;
if (num > 1024)
{
Debug.LogWarning("Inifinity loop? >1024 node path. Remove this message if you really have that long paths (FloodPathTracer.cs, Trace function)");
break;
}
}
}
public RichSpecial Initialize(NodeLink2 nodeLink, GraphNode first)
{
this.nodeLink = nodeLink;
if (first == nodeLink.StartNode)
{
this.first = nodeLink.StartTransform;
this.second = nodeLink.EndTransform;
this.reverse = false;
}
else
{
this.first = nodeLink.EndTransform;
this.second = nodeLink.StartTransform;
this.reverse = true;
}
return this;
}
public Pathfinding.GraphNode GetClosestNode(Vector3 position)
{
Pathfinding.GraphNode closestNode = null;
float minDistance = float.MaxValue;
for (int nodeIndex = 0; nodeIndex < _nodes.Count; ++nodeIndex)
{
Pathfinding.GraphNode node = _nodes[nodeIndex];
float distance = Vector3.Distance((Vector3)node.position, position);
if (minDistance > distance)
{
closestNode = node;
minDistance = distance;
}
}
return(closestNode);
}
static int GetConnectionSpecialCost(IntPtr L)
{
try
{
ToLua.CheckArgsCount(L, 4);
Pathfinding.Path obj = (Pathfinding.Path)ToLua.CheckObject <Pathfinding.Path>(L, 1);
Pathfinding.GraphNode arg0 = (Pathfinding.GraphNode)ToLua.CheckObject <Pathfinding.GraphNode>(L, 2);
Pathfinding.GraphNode arg1 = (Pathfinding.GraphNode)ToLua.CheckObject <Pathfinding.GraphNode>(L, 3);
uint arg2 = (uint)LuaDLL.luaL_checknumber(L, 4);
uint o = obj.GetConnectionSpecialCost(arg0, arg1, arg2);
LuaDLL.lua_pushnumber(L, o);
return(1);
}
catch (Exception e)
{
return(LuaDLL.toluaL_exception(L, e));
}
}
/** Returns all nodes reachable from the seed node.
* This function performs a BFS (breadth-first-search) or flood fill of the graph and returns all nodes which can be reached from
* the seed node. In almost all cases this will be identical to returning all nodes which have the same area as the seed node.
* In the editor areas are displayed as different colors of the nodes.
* The only case where it will not be so is when there is a one way path from some part of the area to the seed node
* but no path from the seed node to that part of the graph.
*
* The returned list is sorted by node distance from the seed node
* i.e distance is measured in the number of nodes the shortest path from \a seed to that node would pass through.
* Note that the distance measurement does not take heuristics, penalties or tag penalties.
*
* Depending on the number of reachable nodes, this function can take quite some time to calculate
* so don't use it too often or it might affect the framerate of your game.
*
* \param seed The node to start the search from
* \param tagMask Optional mask for tags. This is a bitmask.
*
* \returns A List<Node> containing all nodes reachable from the seed node.
* For better memory management the returned list should be pooled, see Pathfinding.Util.ListPool
*/
public static List<GraphNode> GetReachableNodes (GraphNode seed, int tagMask = -1) {
#if ASTAR_PROFILE
System.Diagnostics.Stopwatch watch = new System.Diagnostics.Stopwatch();
watch.Start ();
#endif
Stack<GraphNode> stack = Pathfinding.Util.StackPool<GraphNode>.Claim ();
List<GraphNode> list = Pathfinding.Util.ListPool<GraphNode>.Claim ();
HashSet<GraphNode> map = new HashSet<GraphNode>();
GraphNodeDelegate callback;
if (tagMask == -1) {
callback = delegate (GraphNode node) {
if (node.Walkable && map.Add (node)) {
list.Add (node);
stack.Push (node);
}
};
} else {
callback = delegate (GraphNode node) {
if (node.Walkable && ((tagMask >> (int)node.Tag) & 0x1) != 0 && map.Add (node)) {
list.Add (node);
stack.Push (node);
}
};
}
callback (seed);
while (stack.Count > 0) {
stack.Pop ().GetConnections (callback);
}
Pathfinding.Util.StackPool<GraphNode>.Release (stack);
#if ASTAR_PROFILE
watch.Stop ();
Debug.Log ((1000*watch.Elapsed.TotalSeconds).ToString("0.0 ms"));
#endif
return list;
}
void selectTargetNode()
{
selectTargetTransform();
if (_targetTransform == null)
{
return;
}
TargetNodes targetNodes = _targetTransform.GetComponent <TargetNodes>();
Pathfinding.GraphNode targetNode = targetNodes.GetClosestNode(_transform.position);
if (targetNode == null)
{
return;
}
_aiPath.destination = (Vector3)targetNode.position;
}
public bool Contains (GraphNode node) {
Vector3 point = (Vector3)node.position;
//Debug.DrawRay (node.position,-Vector3.up*2,Color.magenta);
if (convex) {
if (_convexPoints == null) return false;
for (int i=0,j=_convexPoints.Length-1;i<_convexPoints.Length;j=i,i++) {
if (Polygon.Left (_convexPoints[i],_convexPoints[j],point)) return false;
}
} else {
if (_points == null) return false;
return Polygon.ContainsPoint (_points,point);
}
//Debug.DrawRay (node.position,Vector3.up*2,Color.blue);
return true;
}
public PathNode GetPathNode(GraphNode node)
{
int nodeIndex = node.NodeIndex;
return(this.nodes[nodeIndex >> 10][nodeIndex & 1023]);
}
public override bool ContainsConnection (GraphNode node) {
for (int i=0;i<connections.Length;i++) if (connections[i] == node) return true;
return false;
}
public void AddPortal(GraphNode n1, GraphNode n2, List <Vector3> left, List <Vector3> right)
{
}
/** Returns if there is an obstacle between \a origin and \a end on the graph.
* \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
* \param trace If a list is passed, then it will be filled with all nodes the linecast traverses
* This is not the same as Physics.Linecast, this function traverses the graph and looks for collisions.
* \astarpro */
public bool Linecast (Vector3 _a, Vector3 _b, GraphNode hint, out GraphHitInfo hit, List<GraphNode> trace) {
hit = new GraphHitInfo ();
//
//Node n2 = GetNearest (_b,NNConstraint.None);
_a = inverseMatrix.MultiplyPoint3x4 (_a);
_a.x -= 0.5F;
_a.z -= 0.5F;
_b = inverseMatrix.MultiplyPoint3x4 (_b);
_b.x -= 0.5F;
_b.z -= 0.5F;
//Grid coordinates
//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));
//Clamping is needed
if (_a.x < -0.5F || _a.z < -0.5F || _a.x >= width-0.5F || _a.z >= depth-0.5F ||
_b.x < -0.5F || _b.z < -0.5F || _b.x >= width-0.5F || _b.z >= depth-0.5F) {
//Bounding points of the grid
Vector3 p1 = new Vector3 (-0.5F ,0, -0.5F);
Vector3 p2 = new Vector3 (-0.5F ,0, depth-0.5F);
Vector3 p3 = new Vector3 (width-0.5F,0, depth-0.5F);
Vector3 p4 = new Vector3 (width-0.5F,0, -0.5F);
int intersectCount = 0;
bool intersect = false;
Vector3 intersection = Polygon.SegmentIntersectionPoint (p1,p2,_a,_b, out intersect);
if (intersect) {
//Debug.Log ("Intersection with p1 and p2 "+_a+" "+_b+" - Intersection: "+intersection);
intersectCount++;
if (!Polygon.Left (p1,p2,_a)) {
_a = intersection;
} else {
_b = intersection;
}
}
intersection = Polygon.SegmentIntersectionPoint (p2,p3,_a,_b, out intersect);
if (intersect) {
//Debug.Log ("Intersection with p2 and p3 "+_a+" "+_b+" - Intersection: "+intersection);
intersectCount++;
if (!Polygon.Left (p2,p3,_a)) {
_a = intersection;
} else {
_b = intersection;
}
}
intersection = Polygon.SegmentIntersectionPoint (p3,p4,_a,_b, out intersect);
if (intersect) {
//Debug.Log ("Intersection with p3 and p4 "+_a+" "+_b+" - Intersection: "+intersection);
intersectCount++;
if (!Polygon.Left (p3,p4,_a)) {
_a = intersection;
} else {
_b = intersection;
}
}
intersection = Polygon.SegmentIntersectionPoint (p4,p1,_a,_b, out intersect);
if (intersect) {
//Debug.Log ("Intersection with p4 and p1 "+_a+" "+_b+" - Intersection: "+intersection);
intersectCount++;
if (!Polygon.Left (p4,p1,_a)) {
_a = intersection;
} else {
_b = intersection;
}
}
if (intersectCount == 0) {
//The line does not intersect with the grid
return false;
}
}
Vector3 dir = _b-_a;
float magn = dir.magnitude;
if (magn == 0) {
//Zero length line
return false;
}
float sampleLength = 0.2F;
float newMagn = nodeSize * sampleLength;
newMagn -= nodeSize * 0.02F;
dir = (dir / magn) * newMagn;
//Floor to int, number of samples on the line
int its = (int)(magn / newMagn);
//Debug.Log ("Num Its: "+its+" "+dir);
Vector3 originOffset = _a + dir * nodeSize * 0.01F;
GraphNode prevNode = null;
for (int i=0;i <= its;i++) {
Vector3 p = originOffset + dir * i;
int x = Mathf.RoundToInt (p.x);
int z = Mathf.RoundToInt (p.z);
x = x < 0 ? 0 : (x >= width ? width-1 : x);
z = z < 0 ? 0 : (z >= depth ? depth-1 : z);
/*if (x < 0 || z < 0 || x >= width || z >= depth) {
Debug.LogError ("Point Out Of Bounds "+"("+x+", "+z+") "+p+" in iteration "+i+" of "+its+" With a direction magn "+dir.magnitude+"\nA: "+_a+"\nB: "+_b);
throw new System.IndexOutOfRangeException ("The point "+x+","+z+ " is outside the bounds of the grid");
break;
}*/
GraphNode node = nodes[z*width+x];
if (node == prevNode) continue;
if (!node.Walkable) {
if (i > 0) {
hit.point = matrix.MultiplyPoint3x4 (originOffset + dir * (i-1)+new Vector3 (0.5F,0,0.5F));
} else {
hit.point = matrix.MultiplyPoint3x4 (_a+new Vector3 (0.5F,0,0.5F));
}
hit.origin = matrix.MultiplyPoint3x4 (_a+new Vector3 (0.5F,0,0.5F));
hit.node = node;
return true;
}
if (i > its-1) {
if (Mathf.Abs (p.x-_b.x) <= 0.50001F || Mathf.Abs (p.z - _b.z) <= 0.50001F) {
return false;
}
}
if (trace != null) trace.Add (node);
prevNode = node;
}
return false;
}
//public void GenerateBounds () {
//bounds.center = offset+new Vector3 (0,height*0.5F,0);
//bounds.size = new Vector3 (width*scale,height,depth*scale);
//}
/** \todo Set clamped position for Grid Graph */
public override NNInfo GetNearest (Vector3 position, NNConstraint constraint, GraphNode hint) {
if (nodes == null || depth*width != nodes.Length) {
return new NNInfo ();
}
position = inverseMatrix.MultiplyPoint3x4 (position);
float xf = position.x-0.5F;
float zf = position.z-0.5f;
int x = Mathf.Clamp (Mathf.RoundToInt (xf) , 0, width-1);
int z = Mathf.Clamp (Mathf.RoundToInt (zf) , 0, depth-1);
NNInfo nn = new NNInfo(nodes[z*width+x]);
float y = inverseMatrix.MultiplyPoint3x4((Vector3)nodes[z*width+x].position).y;
nn.clampedPosition = matrix.MultiplyPoint3x4 (new Vector3(Mathf.Clamp(xf,x-0.5f,x+0.5f)+0.5f,y,Mathf.Clamp(zf,z-0.5f,z+0.5f)+0.5f));
//Set clamped position
//nn.clampedPosition = new Vector3(Mathf.Clamp (xf,x-0.5f,x+0.5f),position.y,Mathf.Clamp (zf,z-0.5f,z+0.5f));
//nn.clampedPosition = matrix.MultiplyPoint3x4 (nn.clampedPosition);
return nn;
}
public override string DebugString(PathLog logMode)
{
if (logMode == PathLog.None || (!error && logMode == PathLog.OnlyErrors))
{
return("");
}
System.Text.StringBuilder text = pathHandler.DebugStringBuilder;
text.Length = 0;
text.Append(error ? "Path Failed : " : "Path Completed : ");
text.Append("Computation Time ");
text.Append((duration).ToString(logMode == PathLog.Heavy ? "0.000" : "0.00"));
text.Append(" ms Searched Nodes ");
text.Append(searchedNodes);
if (!error)
{
text.Append("\nLast Found Path Length ");
text.Append(path == null ? "Null" : path.Count.ToString());
if (logMode == PathLog.Heavy)
{
text.Append("\nSearch Iterations " + searchIterations);
text.Append("\nPaths (").Append(targetsFound.Length).Append("):");
for (int i = 0; i < targetsFound.Length; i++)
{
text.Append("\n\n Path "+ i).Append(" Found: ").Append(targetsFound[i]);
GraphNode node = nodePaths[i] == null ? null : nodePaths[i][nodePaths[i].Count - 1];
text.Append("\n Length: ");
text.Append(nodePaths[i].Count);
if (node != null)
{
PathNode nodeR = pathHandler.GetPathNode(endNode);
if (nodeR != null)
{
text.Append("\n End Node");
text.Append("\n G: ");
text.Append(nodeR.G);
text.Append("\n H: ");
text.Append(nodeR.H);
text.Append("\n F: ");
text.Append(nodeR.F);
text.Append("\n Point: ");
text.Append(((Vector3)endPoint).ToString());
text.Append("\n Graph: ");
text.Append(endNode.GraphIndex);
}
else
{
text.Append("\n End Node: Null");
}
}
}
text.Append("\nStart Node");
text.Append("\n Point: ");
text.Append(((Vector3)endPoint).ToString());
text.Append("\n Graph: ");
text.Append(startNode.GraphIndex);
text.Append("\nBinary Heap size at completion: ");
text.AppendLine(pathHandler.GetHeap() == null ? "Null" : (pathHandler.GetHeap().numberOfItems - 2).ToString()); // -2 because numberOfItems includes the next item to be added and item zero is not used
}
}
if (error)
{
text.Append("\nError: ");
text.Append(errorLog);
text.AppendLine();
}
if (logMode == PathLog.Heavy && !AstarPath.IsUsingMultithreading)
{
text.Append("\nCallback references ");
if (callback != null)
{
text.Append(callback.Target.GetType().FullName).AppendLine();
}
else
{
text.AppendLine("NULL");
}
}
text.Append("\nPath Number ");
text.Append(pathID);
return(text.ToString());
}
public void FloodFill(GraphNode seed)
{
this.FloodFill(seed, this.lastUniqueAreaIndex + 1u);
this.lastUniqueAreaIndex += 1u;
}
public TowerPrototype _towerToPlace = null; // If null, none selected.
//private Vector3 _outOfTheWay = new Vector3(20, 0, 0);
// Use this for initialization
void Start()
{
Active = this;
_spawnPoint = AstarPath.active.GetNearest(SpawnPointObject.transform.position).node;
_goalPoint = AstarPath.active.GetNearest(GoalNodeObject.transform.position).node;
}
private void RemoveConnections(GraphNode node)
{
node.ClearConnections(true);
}
public override void RemoveConnection(GraphNode node)
{
throw new NotImplementedException();
}
public override void AddConnection(GraphNode node, uint cost)
{
throw new NotImplementedException();
}
public override NNInfo GetNearest(Vector3 position, NNConstraint constraint, GraphNode hint)
{
return(GetNearest(this, nodes, position, constraint, accurateNearestNode));
}
void RemoveConnections(GraphNode node)
{
//TODO, might be better to replace connection
node.ClearConnections(true);
}
public override void ReturnPath()
{
if (error)
{
if (callbacks != null)
{
for (int i = 0; i < callbacks.Length; i++)
{
if (callbacks[i] != null)
{
callbacks[i] (this);
}
}
}
if (callback != null)
{
callback(this);
}
return;
}
bool anySucceded = false;
Vector3 _originalStartPoint = originalStartPoint;
Vector3 _startPoint = startPoint;
GraphNode _startNode = startNode;
for (int i = 0; i < nodePaths.Length; i++)
{
path = nodePaths[i];
if (path != null)
{
CompleteState = PathCompleteState.Complete;
anySucceded = true;
}
else
{
CompleteState = PathCompleteState.Error;
}
if (callbacks != null && callbacks[i] != null)
{
vectorPath = vectorPaths[i];
//=== SEGMENT - should be identical to the one a few rows below (except "i")
if (inverted)
{
endPoint = _startPoint;
endNode = _startNode; //path[0];
startNode = targetNodes[i]; //path[path.Length-1];
startPoint = targetPoints[i];
originalEndPoint = _originalStartPoint;
originalStartPoint = originalTargetPoints[i];
}
else
{
endPoint = targetPoints[i];
originalEndPoint = originalTargetPoints[i];
endNode = targetNodes[i]; //path[path.Length-1];
}
callbacks[i] (this);
//In case a modifier changed the vectorPath, update the array of all vectorPaths
vectorPaths[i] = vectorPath;
}
}
if (anySucceded)
{
CompleteState = PathCompleteState.Complete;
if (!pathsForAll)
{
path = nodePaths[chosenTarget];
vectorPath = vectorPaths[chosenTarget];
//=== SEGMENT - should be identical to the one a few rows above (except "chosenTarget")
if (inverted)
{
endPoint = _startPoint;
endNode = _startNode;
startNode = targetNodes[chosenTarget];
startPoint = targetPoints[chosenTarget];
originalEndPoint = _originalStartPoint;
originalStartPoint = originalTargetPoints[chosenTarget];
}
else
{
endPoint = targetPoints[chosenTarget];
originalEndPoint = originalTargetPoints[chosenTarget];
endNode = targetNodes[chosenTarget];
}
}
}
else
{
CompleteState = PathCompleteState.Error;
}
if (callback != null)
{
callback(this);
}
}
public void RecalculateCosts()
{
if (this.pivots == null)
{
this.RecalculatePivots();
}
if (this.mode == HeuristicOptimizationMode.None)
{
return;
}
this.pivotCount = 0;
for (int i = 0; i < this.pivots.Length; i++)
{
if (this.pivots[i] != null && (this.pivots[i].Destroyed || !this.pivots[i].Walkable))
{
throw new Exception("Invalid pivot nodes (destroyed or unwalkable)");
}
}
if (this.mode != HeuristicOptimizationMode.RandomSpreadOut)
{
for (int j = 0; j < this.pivots.Length; j++)
{
if (this.pivots[j] == null)
{
throw new Exception("Invalid pivot nodes (null)");
}
}
}
Debug.Log("Recalculating costs...");
this.pivotCount = this.pivots.Length;
Action <int> startCostCalculation = null;
int numComplete = 0;
OnPathDelegate onComplete = delegate(Path path)
{
numComplete++;
if (numComplete == this.pivotCount)
{
Debug.Log("Grid graph special case!");
this.ApplyGridGraphEndpointSpecialCase();
}
};
startCostCalculation = delegate(int k)
{
GraphNode pivot = this.pivots[k];
FloodPath fp = null;
fp = FloodPath.Construct(pivot, onComplete);
fp.immediateCallback = delegate(Path _p)
{
_p.Claim(this);
MeshNode meshNode = pivot as MeshNode;
uint costOffset = 0u;
int k;
if (meshNode != null && meshNode.connectionCosts != null)
{
for (k = 0; k < meshNode.connectionCosts.Length; k++)
{
costOffset = Math.Max(costOffset, meshNode.connectionCosts[k]);
}
}
NavGraph[] graphs = AstarPath.active.graphs;
for (int m = graphs.Length - 1; m >= 0; m--)
{
graphs[m].GetNodes(delegate(GraphNode node)
{
int num6 = node.NodeIndex * this.pivotCount + k;
this.EnsureCapacity(num6);
PathNode pathNode = fp.pathHandler.GetPathNode(node);
if (costOffset > 0u)
{
this.costs[num6] = ((pathNode.pathID != fp.pathID || pathNode.parent == null) ? 0u : Math.Max(pathNode.parent.G - costOffset, 0u));
}
else
{
this.costs[num6] = ((pathNode.pathID != fp.pathID) ? 0u : pathNode.G);
}
return(true);
});
}
if (this.mode == HeuristicOptimizationMode.RandomSpreadOut && k < this.pivots.Length - 1)
{
if (this.pivots[k + 1] == null)
{
int num = -1;
uint num2 = 0u;
int num3 = this.maxNodeIndex / this.pivotCount;
for (int n = 1; n < num3; n++)
{
uint num4 = 1073741824u;
for (int num5 = 0; num5 <= k; num5++)
{
num4 = Math.Min(num4, this.costs[n * this.pivotCount + num5]);
}
GraphNode node2 = fp.pathHandler.GetPathNode(n).node;
if ((num4 > num2 || num == -1) && node2 != null && !node2.Destroyed && node2.Walkable)
{
num = n;
num2 = num4;
}
}
if (num == -1)
{
Debug.LogError("Failed generating random pivot points for heuristic optimizations");
return;
}
this.pivots[k + 1] = fp.pathHandler.GetPathNode(num).node;
}
startCostCalculation(k + 1);
}
_p.Release(this, false);
};
AstarPath.StartPath(fp, true);
};
if (this.mode != HeuristicOptimizationMode.RandomSpreadOut)
{
for (int l = 0; l < this.pivots.Length; l++)
{
startCostCalculation(l);
}
}
else
{
startCostCalculation(0);
}
this.dirty = 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
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 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;
}
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;
}
}
public override void Open(Path path, PathNode pathNode, PathHandler handler)
{
GridGraph gg = GetGridGraph(GraphIndex);
int[] neighbourOffsets = gg.neighbourOffsets;
uint[] neighbourCosts = gg.neighbourCosts;
GridNode[] nodes = gg.nodes;
ushort pid = handler.PathID;
for (int i = 0; i < 8; i++)
{
if (GetConnectionInternal(i))
{
GridNode other = nodes[nodeInGridIndex + neighbourOffsets[i]];
if (!path.CanTraverse(other))
{
continue;
}
PathNode otherPN = handler.GetPathNode(other);
if (otherPN.pathID != pid)
{
otherPN.parent = pathNode;
otherPN.pathID = pid;
otherPN.cost = neighbourCosts[i];
otherPN.H = path.CalculateHScore(other);
other.UpdateG(path, otherPN);
//Debug.Log ("G " + otherPN.G + " F " + otherPN.F);
handler.PushNode(otherPN);
//Debug.DrawRay ((Vector3)otherPN.node.Position, Vector3.up,Color.blue);
}
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 = neighbourCosts[i];
if (pathNode.G + tmpCost + path.GetTraversalCost(other) < otherPN.G)
{
//Debug.Log ("Path better from " + NodeIndex + " to " + otherPN.node.NodeIndex + " " + (pathNode.G+tmpCost+path.GetTraversalCost(other)) + " < " + otherPN.G);
otherPN.cost = tmpCost;
otherPN.parent = pathNode;
other.UpdateRecursiveG(path, otherPN, handler);
//Or if the path from this node ("other") to the current ("current") is better
}
else if (otherPN.G + tmpCost + path.GetTraversalCost(this) < pathNode.G)
{
//Debug.Log ("Path better from " + otherPN.node.NodeIndex + " to " + NodeIndex + " " + (otherPN.G+tmpCost+path.GetTraversalCost (this)) + " < " + pathNode.G);
pathNode.parent = otherPN;
pathNode.cost = tmpCost;
UpdateRecursiveG(path, pathNode, handler);
}
}
}
}
#if ASTAR_GRID_CUSTOM_CONNECTIONS
if (connections != null)
{
for (int i = 0; i < connections.Length; i++)
{
GraphNode other = connections[i];
if (!path.CanTraverse(other))
{
continue;
}
PathNode otherPN = handler.GetPathNode(other);
if (otherPN.pathID != pid)
{
otherPN.parent = pathNode;
otherPN.pathID = pid;
otherPN.cost = connectionCosts[i];
otherPN.H = path.CalculateHScore(other);
other.UpdateG(path, otherPN);
//Debug.Log ("G " + otherPN.G + " F " + otherPN.F);
handler.PushNode(otherPN);
//Debug.DrawRay ((Vector3)otherPN.node.Position, Vector3.up,Color.blue);
}
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 = connectionCosts[i];
if (pathNode.G + tmpCost + path.GetTraversalCost(other) < otherPN.G)
{
//Debug.Log ("Path better from " + NodeIndex + " to " + otherPN.node.NodeIndex + " " + (pathNode.G+tmpCost+path.GetTraversalCost(other)) + " < " + otherPN.G);
otherPN.cost = tmpCost;
otherPN.parent = pathNode;
other.UpdateRecursiveG(path, otherPN, handler);
//Or if the path from this node ("other") to the current ("current") is better
}
else if (otherPN.G + tmpCost + path.GetTraversalCost(this) < pathNode.G && other.ContainsConnection(this))
{
//Debug.Log ("Path better from " + otherPN.node.NodeIndex + " to " + NodeIndex + " " + (otherPN.G+tmpCost+path.GetTraversalCost (this)) + " < " + pathNode.G);
pathNode.parent = otherPN;
pathNode.cost = tmpCost;
UpdateRecursiveG(path, pathNode, handler);
}
}
}
}
#endif
}
public uint GetTraversalCost(Path path, GraphNode node)
{
// Same as default implementation
return(path.GetTagPenalty((int)node.Tag) + node.Penalty);
}
protected override bool EndPointGridGraphSpecialCase(GraphNode endNode)
{
return(false);
}
public override void RemoveConnection(GraphNode node)
{
throw new System.NotImplementedException("GridNodes do not have support for adding manual connections");
}
public override bool GetPortal(GraphNode other, List <Vector3> left, List <Vector3> right, bool backwards)
{
if (backwards)
{
return(true);
}
GridGraph gg = GetGridGraph(GraphIndex);
int[] neighbourOffsets = gg.neighbourOffsets;
GridNode[] nodes = gg.nodes;
for (int i = 0; i < 4; i++)
{
if (GetConnectionInternal(i) && other == nodes[nodeInGridIndex + neighbourOffsets[i]])
{
Vector3 middle = ((Vector3)(position + other.position)) * 0.5f;
Vector3 cross = Vector3.Cross(gg.collision.up, (Vector3)(other.position - position));
cross.Normalize();
cross *= gg.nodeSize * 0.5f;
left.Add(middle - cross);
right.Add(middle + cross);
return(true);
}
}
for (int i = 4; i < 8; i++)
{
if (GetConnectionInternal(i) && other == nodes[nodeInGridIndex + neighbourOffsets[i]])
{
bool rClear = false;
bool lClear = false;
if (GetConnectionInternal(i - 4))
{
GridNode n2 = nodes[nodeInGridIndex + neighbourOffsets[i - 4]];
if (n2.Walkable && n2.GetConnectionInternal((i - 4 + 1) % 4))
{
rClear = true;
}
}
if (GetConnectionInternal((i - 4 + 1) % 4))
{
GridNode n2 = nodes[nodeInGridIndex + neighbourOffsets[(i - 4 + 1) % 4]];
if (n2.Walkable && n2.GetConnectionInternal(i - 4))
{
lClear = true;
}
}
Vector3 middle = ((Vector3)(position + other.position)) * 0.5f;
Vector3 cross = Vector3.Cross(gg.collision.up, (Vector3)(other.position - position));
cross.Normalize();
cross *= gg.nodeSize * 1.4142f;
left.Add(middle - (lClear ? cross : Vector3.zero));
right.Add(middle + (rClear ? cross : Vector3.zero));
return(true);
}
}
return(false);
}
/** May be called by graph nodes to get a special cost for some connections.
* Nodes may call it when PathNode.flag2 is set to true, for example mesh nodes, which have
* a very large area can be marked on the start and end nodes, this method will be called
* to get the actual cost for moving from the start position to its neighbours instead
* of as would otherwise be the case, from the start node's position to its neighbours.
* The position of a node and the actual start point on the node can vary quite a lot.
*
* The default behaviour of this method is to return the previous cost of the connection,
* essentiall making no change at all.
*
* This method should return the same regardless of the order of a and b.
* That is f(a,b) == f(b,a) should hold.
*
* \param a Moving from this node
* \param b Moving to this node
* \param currentCost The cost of moving between the nodes. Return this value if there is no meaningful special cost to return.
*/
internal virtual uint GetConnectionSpecialCost(GraphNode a, GraphNode b, uint currentCost)
{
return(currentCost);
}
/** Returns if there is an obstacle between \a origin and \a end on the graph.
* \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 Linecast (Vector3 _a, Vector3 _b, GraphNode hint, out GraphHitInfo hit) {
return Linecast (_a, _b, hint, out hit, null);
}
/** Internal method to clean up node data */
public void DestroyNode (GraphNode node) {
PathNode pn = GetPathNode (node);
//Clean up reference to help GC
pn.node = null;
pn.parent = null;
}
/** 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 PathNode GetPathNode ( GraphNode node ) {
//Get the index of the node
int ind = node.NodeIndex;
return nodes[ind >> BucketSizeLog2][ind & BucketIndexMask];
}
/** Removes any connection from this node to the specified node.
* If no such connection exists, nothing will be done.
*
* \note This only removes the connection from this node to the other node.
* You may want to call the same function on the other node to remove its eventual connection
* to this node.
*/
public override void RemoveConnection (GraphNode node) {
if (connections == null) return;
for (int i=0;i<connections.Length;i++) {
if (connections[i] == node) {
int connLength = connections.Length;
GraphNode[] newconns = new GraphNode[connLength-1];
uint[] newconncosts = new uint[connLength-1];
for (int j=0;j<i;j++) {
newconns[j] = connections[j];
newconncosts[j] = connectionCosts[j];
}
for (int j=i+1;j<connLength;j++) {
newconns[j-1] = connections[j];
newconncosts[j-1] = connectionCosts[j];
}
connections = newconns;
connectionCosts = newconncosts;
return;
}
}
}
/// <summary>
/// Returns if there is a walkable path from node1 to node2.
/// This method is extremely fast because it only uses precalculated information.
///
/// <code>
/// GraphNode node1 = AstarPath.active.GetNearest(point1, NNConstraint.Default).node;
/// GraphNode node2 = AstarPath.active.GetNearest(point2, NNConstraint.Default).node;
///
/// if (PathUtilities.IsPathPossible(node1, node2)) {
/// // Yay, there is a path between those two nodes
/// }
/// </code>
///
/// See: graph-updates (view in online documentation for working links)
/// See: <see cref="AstarPath.GetNearest"/>
/// </summary>
public static bool IsPathPossible(GraphNode node1, GraphNode node2)
{
return(node1.Walkable && node2.Walkable && node1.Area == node2.Area);
}
/// <summary>
/// Check if a straight path between v1 and v2 is valid.
/// If both n1 and n2 are supplied it is assumed that the line goes from the center of n1 to the center of n2 and a more optimized graph linecast may be done.
/// </summary>
protected bool ValidateLine(GraphNode n1, GraphNode n2, Vector3 v1, Vector3 v2)
{
if (useRaycasting)
{
// Use raycasting to check if a straight path between v1 and v2 is valid
if (use2DPhysics)
{
if (thickRaycast && thickRaycastRadius > 0 && Physics2D.CircleCast(v1 + raycastOffset,
thickRaycastRadius, v2 - v1, (v2 - v1).magnitude, mask))
{
return(false);
}
if (Physics2D.Linecast(v1 + raycastOffset, v2 + raycastOffset, mask))
{
return(false);
}
}
else
{
// Perform a thick raycast (if enabled)
if (thickRaycast && thickRaycastRadius > 0 && Physics.SphereCast(
new Ray(v1 + raycastOffset, v2 - v1), thickRaycastRadius, (v2 - v1).magnitude, mask))
{
return(false);
}
// Perform a normal raycast
// This is done even if a thick raycast is also done because thick raycasts do not report collisions for
// colliders that overlapped the (imaginary) sphere at the origin of the thick raycast.
// If this raycast was not done then some obstacles could be missed.
if (Physics.Linecast(v1 + raycastOffset, v2 + raycastOffset, mask))
{
return(false);
}
}
}
if (useGraphRaycasting)
{
if (n1 == null)
{
n1 = AstarPath.active.GetNearest(v1).node;
}
if (n2 == null)
{
n2 = AstarPath.active.GetNearest(v2).node;
}
if (n1 != null && n2 != null)
{
// Use graph raycasting to check if a straight path between v1 and v2 is valid
var graph = n1.Graph;
var graph2 = n2.Graph;
if (graph != graph2)
{
return(false);
}
var rayGraph = graph as IRaycastableGraph;
if (rayGraph != null)
{
return(!rayGraph.Linecast(v1, v2, n1));
}
}
}
return(true);
}
/** 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)
{
var 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) {
var 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);
var nninfo = new NNInfo(minNode);
//Find the point closest to the nearest triangle
if (nninfo.node != null)
{
var node = nninfo.node as TriangleMeshNode; //minNode2 as MeshNode;
Vector3 clP = node.ClosestPointOnNode(position);
nninfo.clampedPosition = clP;
}
nninfo.constrainedNode = minConstNode;
if (nninfo.constrainedNode != null)
{
var node = nninfo.constrainedNode as TriangleMeshNode; //minNode2 as MeshNode;
Vector3 clP = node.ClosestPointOnNode(position);
nninfo.constClampedPosition = clP;
}
return(nninfo);
}
/** Internal method to initialize node data */
public void InitializeNode (GraphNode node) {
//Get the index of the node
int ind = node.NodeIndex;
int bucketNumber = ind >> BucketSizeLog2;
int bucketIndex = ind & BucketIndexMask;
if (bucketNumber >= nodes.Length) {
//At least increase the size to:
//Current size * 1.5
//Current size + 2 or
//bucketNumber
PathNode[][] newNodes = new PathNode[System.Math.Max (System.Math.Max (nodes.Length*3 / 2,bucketNumber+1), nodes.Length+2)][];
for (int i=0;i<nodes.Length;i++) newNodes[i] = nodes[i];
//Debug.Log ("Resizing Bucket List from " + nodes.Length + " to " + newNodes.Length + " (bucketNumber="+bucketNumber+")");
bool[] newBucketNew = new bool[newNodes.Length];
for (int i=0;i<nodes.Length;i++) newBucketNew[i] = bucketNew[i];
bool[] newBucketCreated = new bool[newNodes.Length];
for (int i=0;i<nodes.Length;i++) newBucketCreated[i] = bucketCreated[i];
nodes = newNodes;
bucketNew = newBucketNew;
bucketCreated = newBucketCreated;
}
if (nodes[bucketNumber] == null) {
PathNode[] ns;
if (bucketCache.Count > 0) {
ns = bucketCache.Pop();
} else {
ns = new PathNode[BucketSize];
for (int i=0;i<BucketSize;i++) ns[i] = new PathNode ();
}
nodes[bucketNumber] = ns;
if (!bucketCreated[bucketNumber]) {
bucketNew[bucketNumber] = true;
bucketCreated[bucketNumber] = true;
}
filledBuckets++;
}
PathNode pn = nodes[bucketNumber][bucketIndex];
pn.node = node;
}
public NNInfo(NNInfoInternal internalInfo)
{
node = internalInfo.node;
position = internalInfo.clampedPosition;
}
/* Color to use for gizmos.
* Returns a color to be used for the specified node with the current debug settings (editor only)
*/
public virtual Color NodeColor(GraphNode node, PathHandler data)
{
Color c = AstarColor.NodeConnection;
bool colSet = false;
if (node == null)
{
return(AstarColor.NodeConnection);
}
switch (AstarPath.active.debugMode)
{
case GraphDebugMode.Areas:
c = AstarColor.GetAreaColor(node.Area);
colSet = true;
break;
case GraphDebugMode.Penalty:
c = Color.Lerp(AstarColor.ConnectionLowLerp, AstarColor.ConnectionHighLerp, (float)node.Penalty / (float)AstarPath.active.debugRoof);
colSet = true;
break;
case GraphDebugMode.Tags:
c = AstarMath.IntToColor((int)node.Tag, 0.5F);
colSet = true;
break;
/* Wasn't really usefull
* case GraphDebugMode.Position:
* float r = Mathf.PingPong (node.position.x/10000F,1F) + Mathf.PingPong (node.position.x/300000F,1F);
* float g = Mathf.PingPong (node.position.y/10000F,1F) + Mathf.PingPong (node.position.y/200000F,1F);
* float b = Mathf.PingPong (node.position.z/10000F,1F) + Mathf.PingPong (node.position.z/100000F,1F);
*
*
* c = new Color (r,g,b);
* break;
*/
}
if (!colSet)
{
if (data == null)
{
return(AstarColor.NodeConnection);
}
PathNode nodeR = data.GetPathNode(node);
switch (AstarPath.active.debugMode)
{
case GraphDebugMode.G:
//c = Mathfx.IntToColor (node.g,0.5F);
c = Color.Lerp(AstarColor.ConnectionLowLerp, AstarColor.ConnectionHighLerp, (float)nodeR.G / (float)AstarPath.active.debugRoof);
break;
case GraphDebugMode.H:
c = Color.Lerp(AstarColor.ConnectionLowLerp, AstarColor.ConnectionHighLerp, (float)nodeR.H / (float)AstarPath.active.debugRoof);
break;
case GraphDebugMode.F:
c = Color.Lerp(AstarColor.ConnectionLowLerp, AstarColor.ConnectionHighLerp, (float)nodeR.F / (float)AstarPath.active.debugRoof);
break;
}
}
c.a *= 0.5F;
return(c);
}
/// <summary>
/// Returns all nodes up to a given node-distance from the seed node.
/// This function performs a BFS (<a href="https://en.wikipedia.org/wiki/Breadth-first_search">breadth-first search</a>) or flood fill of the graph and returns all nodes within a specified node distance which can be reached from
/// the seed node. In almost all cases when depth is large enough this will be identical to returning all nodes which have the same area as the seed node.
/// In the editor areas are displayed as different colors of the nodes.
/// The only case where it will not be so is when there is a one way path from some part of the area to the seed node
/// but no path from the seed node to that part of the graph.
///
/// The returned list is sorted by node distance from the seed node
/// i.e distance is measured in the number of nodes the shortest path from seed to that node would pass through.
/// Note that the distance measurement does not take heuristics, penalties or tag penalties.
///
/// Depending on the number of nodes, this function can take quite some time to calculate
/// so don't use it too often or it might affect the framerate of your game.
///
/// Returns: A List<GraphNode> containing all nodes reachable up to a specified node distance from the seed node.
/// For better memory management the returned list should be pooled, see Pathfinding.Util.ListPool
///
/// Warning: This method is not thread safe. Only use it from the Unity thread (i.e normal game code).
///
/// The video below shows the BFS result with varying values of depth. Points are sampled on the nodes using <see cref="GetPointsOnNodes"/>.
/// [Open online documentation to see videos]
/// </summary>
/// <param name="seed">The node to start the search from.</param>
/// <param name="depth">The maximum node-distance from the seed node.</param>
/// <param name="tagMask">Optional mask for tags. This is a bitmask.</param>
/// <param name="filter">Optional filter for which nodes to search. You can combine this with depth = int.MaxValue and tagMask = -1 to make the filter determine everything.
/// Only walkable nodes are searched regardless of the filter. If the filter function returns false the node will be treated as unwalkable.</param>
public static List <GraphNode> BFS(GraphNode seed, int depth, int tagMask = -1, System.Func <GraphNode, bool> filter = null)
{
#if ASTAR_PROFILE
System.Diagnostics.Stopwatch watch = new System.Diagnostics.Stopwatch();
watch.Start();
#endif
BFSQueue = BFSQueue ?? new Queue <GraphNode>();
var que = BFSQueue;
BFSMap = BFSMap ?? new Dictionary <GraphNode, int>();
var map = BFSMap;
// Even though we clear at the end of this function, it is good to
// do it here as well in case the previous invocation of the method
// threw an exception for some reason
// and didn't clear the que and map
que.Clear();
map.Clear();
List <GraphNode> result = ListPool <GraphNode> .Claim();
int currentDist = -1;
System.Action <GraphNode> callback;
if (tagMask == -1)
{
callback = node => {
if (node.Walkable && !map.ContainsKey(node))
{
if (filter != null && !filter(node))
{
return;
}
map.Add(node, currentDist + 1);
result.Add(node);
que.Enqueue(node);
}
};
}
else
{
callback = node => {
if (node.Walkable && ((tagMask >> (int)node.Tag) & 0x1) != 0 && !map.ContainsKey(node))
{
if (filter != null && !filter(node))
{
return;
}
map.Add(node, currentDist + 1);
result.Add(node);
que.Enqueue(node);
}
};
}
callback(seed);
while (que.Count > 0)
{
GraphNode n = que.Dequeue();
currentDist = map[n];
if (currentDist >= depth)
{
break;
}
n.GetConnections(callback);
}
que.Clear();
map.Clear();
#if ASTAR_PROFILE
watch.Stop();
Debug.Log((1000 * watch.Elapsed.TotalSeconds).ToString("0.0 ms"));
#endif
return(result);
}
