public Grid(int row, int column)
{
this.random = new Random();
this.Row = row;
this.Column = column;
matrizNodes = new Node[this.Row,this.Column];
Nodes = new Node[this.Row * this.Column];
for (var i = 0; i < this.Row; i++)
{
for (var j = 0; j < this.Column; j++)
{
Nodes[i * this.Column + j] = new Node(i, j);
}
}
for (int x = 0; x < matrizNodes.GetLength(0); x++)
{
for (int y = 0; y < matrizNodes.GetLength(1); y++)
{
var index = x * matrizNodes.GetLength(1) + y;
matrizNodes[x, y] = Nodes[index];
}
}
}
/// <summary>
/// Creates a number of nodes with the correct type for the graph.
/// </summary>
/// <remarks>Called by graph generators and when deserializing a graph with nodes.
/// Override this function if you do not use the default <see cref="Node"/> class.</remarks>
/// <param name="number">
/// A <see cref="System.Int32"/>
/// </param>
/// <returns>
/// A <see cref="Node[]"/>
/// </returns>
public virtual Node[] CreateNodes (int number) {
Node[] tmp = new Node[number];
for (int i=0;i<number;i++) {
tmp[i] = new Node ();
}
return tmp;
}
/** Creates a number of nodes with the correct type for the graph.
This should not set the #nodes array, only return the nodes.
Called by graph generators and when deserializing a graph with nodes.
Override this function if you do not use the default Pathfinding.Node class.
*/
public virtual Node[] CreateNodes (int number) {
Node[] tmp = new Node[number];
for (int i=0;i<number;i++) {
tmp[i] = new Node ();
tmp[i].penalty = initialPenalty;
}
return tmp;
}
/// <summary>
/// This will be called on the same time as Awake on the gameObject which the AstarPath script is attached to. (remember, not in the editor)
/// Use this for any initialization code which can't be placed in Scan
/// </summary>
//public override void Awake() {
// base.Awake();
//}
// IMPROVE not really necessary. I just override this NavGraph method to add the debug line at the bottom. Otherwise its identical
public override NNInfo GetNearest(Vector3 position, NNConstraint constraint, Node hint) {
if (nodes == null) {
return new NNInfo();
}
float maxDistSqr = constraint.constrainDistance ? AstarPath.active.maxNearestNodeDistanceSqr : float.PositiveInfinity;
float minDist = float.PositiveInfinity;
Node minNode = null;
float minConstDist = float.PositiveInfinity;
Node minConstNode = null;
for (int i = 0; i < nodes.Length; i++) {
Node node = nodes[i];
float dist = (position - (Vector3)node.position).sqrMagnitude;
if (dist < minDist) {
minDist = dist;
minNode = node;
}
if (dist < minConstDist && dist < maxDistSqr && constraint.Suitable(node)) {
minConstDist = dist;
minConstNode = node;
}
}
NNInfo nnInfo = new NNInfo(minNode);
nnInfo.constrainedNode = minConstNode;
if (minConstNode != null) {
nnInfo.constClampedPosition = (Vector3)minConstNode.position;
}
else if (minNode != null) {
nnInfo.constrainedNode = minNode;
nnInfo.constClampedPosition = (Vector3)minNode.position;
}
#region Debugging
if (nnInfo.constrainedNode == null) {
float closestDistance;
Node closestNode = __FindClosestNode(position, out closestDistance);
D.Warn("Can't find node close enough to {0}. ClosestNode is {1} away.", position, closestDistance);
}
else {
D.Log("Closest Node is at {0}, {1} from {2}.", (Vector3)nnInfo.constrainedNode.position, Mathf.Sqrt(minDist), position);
}
//D.Log("GetNearest() constraint.Suitable = {0}.", constraint.Suitable(nnInfo.node));
#endregion
return nnInfo;
}
/** 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
*/
public static bool UpdateGraphsNoBlock (GraphUpdateObject guo, Node node1, Node node2, bool alwaysRevert = false) {
List<Node> buffer = ListPool<Node>.Claim ();
buffer.Add (node1);
buffer.Add (node2);
bool worked = UpdateGraphsNoBlock (guo,buffer, alwaysRevert);
ListPool<Node>.Release (buffer);
return worked;
}
public void DrawPath(Node goal,ref Grid grid,ref Tree[] _trees)
{
Point startingPoint = StateToPoint(nodeState.ORIGIN);
int pointX = startingPoint.X;
int pointY = startingPoint.Y;
path.Add(origin);
if (pointX == -1 && pointY == -1)
{
return;
}
while (true)
{
Point lowestPoint = Point.Zero;
int lowest = 10000;
foreach (Point movePoint in CheckStep(pointX, pointY))
{
int count = _squares[movePoint.X, movePoint.Y].distanceSteps;
if (count < lowest)
{
lowest = count;
lowestPoint.X = movePoint.X;
lowestPoint.Y = movePoint.Y;
}
}
if (lowest != 10000)
{
_squares[lowestPoint.X, lowestPoint.Y].hasPath = true;
_squares[lowestPoint.X, lowestPoint.Y].ChangeTexture(ImageLibrary.getInstance().getImage("Clear"));
path.Add(_squares[lowestPoint.X, lowestPoint.Y]);
pointX = lowestPoint.X;
pointY = lowestPoint.Y;
}
else
{
Search(origin, goal, ref grid, this.game, ref _trees, ref this.path);
return;
}
if (_squares[pointX, pointY].state == nodeState.GOAL)
{
_squares[pointX, pointY].ChangeTexture(ImageLibrary.getInstance().getImage("Origin"));
origin.ChangeTexture(ImageLibrary.getInstance().getImage("Origin"));
path.Add(_squares[pointX, pointY]);
CreateTrees(ref _trees);
break;
}
}
}
//TODO can be optimized by having a "Taken" flag in a Node class
private bool IsTaken(Node node)
{
foreach (var group in _groups.Values)
{
if (group.CurrentNode == node.GridPosition)
{
return(true);
}
}
return(false);
}
protected void CreateMap(ref bool[,] boolArray)
{
this.dimension = new Point(boolArray.GetLength(0), boolArray.GetLength(1));
map = new Node[dimension.X, dimension.Y];
for (int x = 0; x < this.dimension.X; x++)
for (int y = 0; y < this.dimension.Y; y++)
{
map[x, y] = new Node(new Point(x, y), boolArray[x, y], this.start, this.end, this.distanceType);
}
}
public Node(int i, int j, int x = 0, int y = 0, int w = 50, int h = 50, Node father = null)
{
I = i;
J = j;
W = w;
H = h;
X = W + I * W + I;
Y = H + J * H + J;
Father = father;
}
public Character(Game game, Vector3 scale, Vector3 rotation, Vector3 position, Texture2D texture, Camera camera, Node target)
{
this.scale = scale;
this.target = target;
this.camera = camera;
this.world = Matrix.CreateScale(scale) * Matrix.CreateFromYawPitchRoll(rotation.Y, rotation.X, rotation.Z) * Matrix.CreateTranslation(position);
this.position = position;
this.rotation = rotation;
this.texture = texture;
Vector3[] _verts = new Vector3[]
{
new Vector3(-1, 0, -1),
new Vector3(-1, 0, 1),
new Vector3(1, 0, -1),
new Vector3(1,0,1)
};
int[] triangles = new int[]
{
0,1,2,
2,1,3
};
Vector2[] uv = new Vector2[]
{
new Vector2(0, 0),
new Vector2(0, 1),
new Vector2(1, 0),
new Vector2(1,0),
new Vector2(0, 1),
new Vector2(1, 1)
};
int[] trianglesuv = new int[]
{
0,1,2,
3,4,5
};
ConvertMesh myMesh = new ConvertMesh();
verts = myMesh.returnTriangle(_verts, triangles, uv, trianglesuv);
vertexBuffer = new VertexBuffer(game.GraphicsDevice, typeof(VertexPositionTexture), verts.Length, BufferUsage.None);
vertexBuffer.SetData<VertexPositionTexture>(verts);
effect = new BasicEffect(game.GraphicsDevice);
}
public static int GetDistance(Node from, Node to)
{
int distX = Mathf.Abs(from.gridPos.x - to.gridPos.x);
int distY = Mathf.Abs(from.gridPos.y - to.gridPos.y);
if (distX > distY)
{
return 14 * distY + 10 * (distX - distY);
}
else
{
return 14 * distX + 10 * (distY - distX);
}
}
/** Increases the capacity of the nodes array to hold more layers.
* After this function has been called and new nodes have been set up, the AstarPath.DataUpdate function must be called.
*/
public void AddLayers(int count)
{
int newLayerCount = layerCount + count;
if (newLayerCount > LevelGridNode.MaxLayerCount) {
Debug.LogError ("Too many layers, a maximum of LevelGridNode.MaxLayerCount are allowed (required "+newLayerCount+")");
return;
}
Node[] tmp = nodes;
nodes = new Node[width*depth*newLayerCount];
for (int i=0;i<tmp.Length;i++) nodes[i] = tmp[i];
layerCount = newLayerCount;
}
public void Raffle(ref List<Node> list, Node origem, Node destino)
{
list.Clear();
for (int i = 0; i < matrizNodes.GetLength(0); i++)
{
for (int j = 0; j < matrizNodes.GetLength(1); j++)
{
matrizNodes[i, j].EstadoNode = EstadoNode.nenhum;
matrizNodes[i, j].IsPath = false;
matrizNodes[i, j].DistanciaPassos = 10000;
}
}
for (int i = 0; i < matrizNodes.GetLength(0); i++)
{
for (int j = 0; j < matrizNodes.GetLength(1); j++)
{
matrizNodes[i, j].EstadoNode = EstadoNode.nenhum;
if (matrizNodes[i,j] == origem)
{
matrizNodes[i, j].EstadoNode = EstadoNode.origem;
}
else if (matrizNodes[i, j] == destino)
{
matrizNodes[i, j].EstadoNode = EstadoNode.alvo;
}
}
}
for (var i = 0; i < matrizNodes.GetLength(0); i++)
{
for (var j = 0; j < matrizNodes.GetLength(1); j++)
{
if (this.random.Next(100) < 30)
{
var index = j * this.Column + i;
if (this.matrizNodes[i,j].EstadoNode != EstadoNode.origem &&
this.matrizNodes[i, j].EstadoNode != EstadoNode.alvo)
{
this.matrizNodes[i, j].EstadoNode = EstadoNode.parede;
list.Add(this.matrizNodes[i, j]);
}
}
}
}
}
private bool Intersects(Group group, Node node)
{
//Heuristic approach to get the time stamp
var minDistance = Utils.GetMinimumDistance(group.CurrentNode, node.GridPosition);
var maxDistance = Utils.GetMaximumDistance(group.CurrentNode, node.GridPosition);
foreach (var otherGroup in _groups.Values)
{
if (group == otherGroup)
{
continue;
}
//The group is stationary
if ((otherGroup.Path == null || otherGroup.Path.IsEmpty) && otherGroup.IsAt(node))
{
return(true);
}
/*
* //The group contains it on it's path
* if (otherGroup.Path != null)
* {
* for (var i = minDistance - 1; i < maxDistance; i++)
* {
* if (otherGroup.Path.GetNodeAtTime(i) == node)
* {
* return true;
* }
* }
* }*/
var intersects = false;
if (otherGroup.Path != null)
{
intersects = _checkWholePath ? otherGroup.Path.Contains(node) : otherGroup.Path.GetTheLastExpectedPosition().Node == node;
}
if (intersects)
{
return(true);
}
}
return(false);
}
public Path GetPathInDirection(Node fromNode, Node toNode, Group group, bool[][] visitedMap, GridRestriction gridRestriction = null)
{
Path path = null;
var queue = new Queue <Node>();
queue.Enqueue(toNode);
while (queue.Any())
{
//If path found stop search
if (path != null)
{
break;
}
var node = queue.Dequeue();
//If already visited the node skip to the next one
if (visitedMap[node.GridPosition.x][node.GridPosition.y])
{
continue;
}
if (node.IsPlayerWalkable && !Intersects(group, node))
{
var potentialPath = Pathfinder.FindPath(fromNode.GridPosition, node.GridPosition,
GameManager.Instance.GridMap, true);
if (potentialPath != null && (gridRestriction == null || gridRestriction.IsValid(node)))
{
path = new Path(potentialPath);
}
}
visitedMap[node.GridPosition.x][node.GridPosition.y] = true;
var neighbours = node.GetWalkableNeighbours();
foreach (var neighbour in neighbours)
{
queue.Enqueue(neighbour);
}
}
return(path);
}
private bool OtherGroupIsOnWayTo(Node node, Group thisGroup)
{
foreach (var group in _groups.Values)
{
if (group == thisGroup)
{
continue;
}
if (!group.IsAt(node) && group.Path != null && group.Path.Contains(node))
{
return(true);
}
}
return(false);
}
public void PathfindForOrigin(Node alvo, ref Grid grid)
{
Point pontoInicial = ProcurarNode(alvo.EstadoNode);
int alvoX = pontoInicial.X;
int alvoY = pontoInicial.Y;
if (alvoX == -1 || alvoY == -1)
{
return;
}
matrizNodes[alvoX, alvoY].DistanciaPassos = 0;
while (true)
{
bool verificando = false;
foreach (Point pontoAtual in TodosNodes())
{
int x = pontoAtual.X;
int y = pontoAtual.Y;
if (CampoAberto(x,y))
{
int andandoAqui = matrizNodes[x, y].DistanciaPassos;
foreach (Point pontoDeLocomocao in ValidarMovimentos(x, y))
{
int nX = pontoDeLocomocao.X;
int nY = pontoDeLocomocao.Y;
int novoAndar = andandoAqui + 1;
if (matrizNodes[nX, nY].DistanciaPassos > novoAndar)
{
matrizNodes[nX, nY].DistanciaPassos = novoAndar;
verificando = true;
}
}
}
}
if (!verificando)
{
break;
}
}
}
public Grid(Game game,Camera mainCamera,int Rows, int Columns)
{
this.Rows = Rows;
this.Columns = Columns;
nodes = new Node[Rows, Columns];
for (int x = 0; x < Columns; x++)
{
for (int y = 0; y < Rows; y++)
{
float positionX = 1 * x;
float positionY = 1 * y;
nodes[x, y] = new Node(game, new Vector3(1.25f, 0, 1.25f), new Vector3(0, 0, 0), new Vector3(positionX*2.5f, 0, positionY*2.5f),ImageLibrary.getInstance().getImage("Floor"), mainCamera);
nodes[x, y].x = x;
nodes[x, y].y = y;
}
}
}
public static void Create(float nodeRadius, Vector2 worldSize, Vector2 center, LayerMask unwalkableMask)
{
Grid.nodeSize = nodeRadius * 2;
int gridSizeX = Mathf.RoundToInt(worldSize.x / nodeSize);
int gridSizeY = Mathf.RoundToInt(worldSize.y / nodeSize);
gridSize = new Coords(gridSizeX, gridSizeY);
grid = new Node[gridSize.x, gridSize.y];
Vector2 bottomLeft = center - (Vector2.right * (worldSize.x / 2)) - (Vector2.up * (worldSize.y / 2));
for (int x = 0; x < gridSize.x; x++)
{
for (int y = 0; y < gridSize.y; y++)
{
Vector2 worldPoint = bottomLeft + Vector2.right * (x * nodeSize + nodeRadius) + Vector2.up * (y * nodeSize + nodeRadius);
bool walkable = !(Physics2D.OverlapCircle(worldPoint, nodeRadius, unwalkableMask));
grid[x, y] = new Node(walkable, worldPoint, x, y);
}
}
}
/** Returns the graph which contains the specified node. The graph must be in the #graphs array.
* \returns Returns the graph which contains the node. Null if the graph wasn't found */
public static NavGraph GetGraph(Node node)
{
if (node == null) {
return null;
}
AstarPath script = AstarPath.active;
if (script == null) return null;
AstarData data = script.astarData;
if (data == null) return null;
if (data.graphs == null) return null;
int graphIndex = node.graphIndex;
if (graphIndex < 0 || graphIndex >= data.graphs.Length) {
return null;
}
return data.graphs[graphIndex];
}
//One node uses approximately 4*3 + 4 + 4 + (connections ? 1 + (4+4)*numConnections) ? 20 + 1 + 8*numConnections ? 21 + 8*3 ? 45 bytes serialized
/** Serializes one node to the stream.
* \astarpro */
private void SerializeNode(Node node, BinaryWriter stream)
{
throw new NotSupportedException ("This function has been deprecated");
}
public void DeSerializeNodes (Node[] nodes, AstarSerializer serializer) {
//NavMeshGraph.DeSerializeMeshNodes (this as INavmesh, nodes, serializer);
}
/** Deserializes one node from the stream into the specified graphs and to the specified graph index.
* \astarpro */
private void DeSerializeNode(Node node, NavGraph[] graphs, int graphIndex, BinaryReader stream)
{
//NavGraph graph = graphs[graphIndex];
if (mask == SMask.SaveNodePositions) {
node.position = new Int3(
stream.ReadInt32 (), //X
stream.ReadInt32 (), //Y
stream.ReadInt32 () //Z
);
}
if (mask != SMask.RunLengthEncoding) {
node.penalty = (uint)stream.ReadInt32 ();
node.flags = stream.ReadInt32 ();
}
if (mask == SMask.SaveNodeConnections) {
if (tmpConnections == null) {
tmpConnections = new List<Node> ();
tmpConnectionCosts = new List<int> ();
} else {
tmpConnections.Clear ();
tmpConnectionCosts.Clear ();
}
int numConn = (int)stream.ReadByte ();
for (int i=0;i<numConn;i++) {
int nodeIndex = stream.ReadInt32 ();
//Graph index as in, which graph
int nodeGraphIndex = (nodeIndex >> 26) & 0x3F;
nodeIndex &= 0x3FFFFFF;
int cost = stream.ReadInt32 ();
bool containsLink = false;
if (nodeGraphIndex != graphIndex) {
containsLink = stream.ReadBoolean ();
}
if (graphRefGuids[graphIndex] != -1) {
Node other = active.astarData.GetNode (graphRefGuids[nodeGraphIndex],nodeIndex, graphs);
//Shouldn't really have to check for this, but just in case of corrupt serialization data
if (other != null) {
tmpConnections.Add (other);
if (mask == SMask.SaveNodeConnectionCosts) {
tmpConnectionCosts.Add (cost);
}
if (containsLink) {
other.AddConnection (node,cost);
}
}
}
}
node.connections = tmpConnections.ToArray ();
if (mask == SMask.SaveNodeConnectionCosts) {
node.connectionCosts = tmpConnectionCosts.ToArray ();
} else {
node.connectionCosts = new int[node.connections.Length];
}
}
}
public new void AddPortal(Node n1, Node n2, List<Vector3> left, List<Vector3> right)
{
//Not implemented
}
/** Returns if the connection between \a a and \a b is valid.
* Checks for obstructions using raycasts (if enabled) and checks for height differences.\n
* As a bonus, it outputs the distance between the nodes too if the connection is valid */
public bool IsValidConnection (Node a, Node b, out float dist) {
dist = 0;
if (!a.walkable || !b.walkable) return false;
Vector3 dir = (Vector3)(a.position-b.position);
if (
(!Mathf.Approximately (limits.x,0) && Mathf.Abs (dir.x) > limits.x) ||
(!Mathf.Approximately (limits.y,0) && Mathf.Abs (dir.y) > limits.y) ||
(!Mathf.Approximately (limits.z,0) && Mathf.Abs (dir.z) > limits.z))
{
return false;
}
dist = dir.magnitude;
if (maxDistance == 0 || dist < maxDistance) {
if (raycast) {
Ray ray = new Ray ((Vector3)a.position,(Vector3)(b.position-a.position));
Ray invertRay = new Ray ((Vector3)b.position,(Vector3)(a.position-b.position));
if (thickRaycast) {
if (!Physics.SphereCast (ray,thickRaycastRadius,dist,mask) && !Physics.SphereCast (invertRay,thickRaycastRadius,dist,mask)) {
return true;
}
} else {
if (!Physics.Raycast (ray,dist,mask) && !Physics.Raycast (invertRay,dist,mask)) {
return true;
}
}
} else {
return true;
}
}
return false;
}
/** Returns if \a _b is visible from \a _a 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 new bool Linecast(Vector3 _a, Vector3 _b, Node hint, out GraphHitInfo hit)
{
return SnappedLinecast (_a,_b,hint,out hit);
}
/** Calculates the grid connections for a single node */
public virtual void CalculateConnections(Node[] nodes, int x, int z, GridNode node)
{
//Reset all connections
node.flags = node.flags & -256;
//All connections are disabled if the node is not walkable
if (!node.walkable) {
return;
}
int index = node.GetIndex ();
if (corners == null) {
corners = new int[4];
} else {
for (int i = 0;i<4;i++) {
corners[i] = 0;
}
}
for (int i=0, j = 3; i<4; j = i, i++) {
int nx = x + neighbourXOffsets[i];
int nz = z + neighbourZOffsets[i];
if (nx < 0 || nz < 0 || nx >= width || nz >= depth) {
continue;
}
GridNode other = nodes[index+neighbourOffsets[i]] as GridNode;
if (IsValidConnection (node, other)) {
node.SetConnectionRaw (i,1);
corners[i]++;
corners[j]++;
}
}
if (neighbours == NumNeighbours.Eight) {
if (cutCorners) {
for (int i=0; i<4; i++) {
if (corners[i] >= 1) {
int nx = x + neighbourXOffsets[i+4];
int nz = z + neighbourZOffsets[i+4];
if (nx < 0 || nz < 0 || nx >= width || nz >= depth) {
continue;
}
GridNode other = nodes[index+neighbourOffsets[i+4]] as GridNode;
if (IsValidConnection (node,other)) {
node.SetConnectionRaw (i+4,1);
}
}
}
} else {
for (int i=0; i<4; i++) {
//We don't need to check if it is out of bounds because if both of the other neighbours are inside the bounds this one must be too
if (corners[i] == 2) {
GridNode other = nodes[index+neighbourOffsets[i+4]] as GridNode;
if (IsValidConnection (node,other)) {
node.SetConnectionRaw (i+4,1);
}
}
}
}
}
}
public override NNInfo GetNearest(Vector3 position, NNConstraint constraint, Node hint = null)
{
if (nodes == null || depth*width*layerCount != nodes.Length) {
//Debug.LogError ("NavGraph hasn't been generated yet");
return new NNInfo ();
}
position = inverseMatrix.MultiplyPoint3x4 (position);
int x = Mathf.Clamp (Mathf.RoundToInt (position.x-0.5F) , 0, width-1);
int z = Mathf.Clamp (Mathf.RoundToInt (position.z-0.5F) , 0, depth-1);
int index = width*z+x;
float minDist = float.PositiveInfinity;
Node minNode = null;
for (int i=0;i<layerCount;i++) {
Node node = nodes[index + width*depth*i];
if (node != null) {
float dist = ((Vector3)node.position - position).sqrMagnitude;
if (dist < minDist) {
minDist = dist;
minNode = node;
}
}
}
return new NNInfo(minNode);
}
/** Returns if \a _b is visible from \a _a on the graph.
* \param [in] _a Point to linecast from
* \param [in] _b Point to linecast to
* \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 new bool Linecast(Vector3 _a, Vector3 _b, Node hint)
{
GraphHitInfo hit;
return Linecast (_a,_b,hint, out hit);
}
//public void CalculateConnections (Node[] nodes, LinkedLevelCell[] linkedCells, Node node, int x, int z, int nodeIndex) {
public void CalculateConnections(Node[] nodes, Node node, int x, int z, int layerIndex)
{
if (node == null) return;
LevelGridNode lgn = (LevelGridNode)node;
lgn.ResetAllGridConnections ();
if (!node.walkable) {
return;
}
float height = 0;
if (layerIndex == layerCount-1 || nodes[lgn.GetIndex () + width*depth*(layerIndex+1)] == null) {
height = float.PositiveInfinity;
} else {
height = System.Math.Abs (lgn.position.y - nodes[lgn.GetIndex()+width*depth*(layerIndex+1)].position.y)*Int3.PrecisionFactor;
}
for (int dir=0;dir<4;dir++) {
int nx = x + neighbourXOffsets[dir];
int nz = z + neighbourZOffsets[dir];
//Check for out-of-bounds
if (nx < 0 || nz < 0 || nx >= width || nz >= depth) {
continue;
}
//Calculate new index
int nIndex = nz*width+nx;
int conn = LevelGridNode.NoConnection;
for (int i=0;i<layerCount;i++) {
Node other = nodes[nIndex + width*depth*i];
if (other != null && other.walkable) {
float otherHeight = 0;
//Is there a node above this one
if (i == layerCount-1 || nodes[nIndex+width*depth*(i+1)] == null) {
otherHeight = float.PositiveInfinity;
} else {
otherHeight = System.Math.Abs (other.position.y - nodes[nIndex+width*depth*(i+1)].position.y)*Int3.PrecisionFactor;
}
float bottom = Mathf.Max (other.position.y*Int3.PrecisionFactor,lgn.position.y*Int3.PrecisionFactor);
float top = Mathf.Min (other.position.y*Int3.PrecisionFactor+otherHeight,lgn.position.y*Int3.PrecisionFactor+height);
float dist = top-bottom;
if (dist >= characterHeight && Mathf.Abs (other.position.y-lgn.position.y)*Int3.PrecisionFactor <= maxClimb) {
//Debug.DrawLine (lgn.position,other.position,new Color (0,1,0,0.5F));
conn = i;
}
}
}
lgn.SetConnectionValue (dir,conn);
/*LinkedLevelCell llc = linkedCells[nIndex];
//LinkedLevelNode lln = llc.first;
int conn = 0xF;
//float minDist = Mathf.Infinity;
for (int i=0;i<llc.count;i++) {
LevelGridNode other = (LevelGridNode)nodes[llc.index+i];
if (!other.walkable) {
continue;
}
float bottom = Mathf.Max (other.position.y*Int3.PrecisionFactor,lgn.position.y*Int3.PrecisionFactor);
float top = Mathf.Min (other.position.y*Int3.PrecisionFactor+other.height,lgn.position.y*Int3.PrecisionFactor+lgn.height);
float dist = top-bottom;
//if (z == 3) {
// Debug.DrawRay (lgn.position,Vector3.up*(dist >= characterHeight ? 2 : 0)*0.9F,Color.yellow);
//}
//Debug.DrawLine ((Vector3)lgn.position+Vector3.up,(Vector3)other.position+Vector3.up,new Color (1,0,0,0.5F));
//if (Mathf.Abs (other.position.y-lgn.position.y)*Int3.PrecisionFactor > maxClimb) {
// Debug.DrawLine (lgn.position,other.position,new Color (1,0,0,0.5F));
//}
if (dist >= characterHeight && Mathf.Abs (other.position.y-lgn.position.y)*Int3.PrecisionFactor <= maxClimb) {
if (i >= 0xF) {
Debug.LogError ("Too many layers");
continue;
}
//Debug.DrawLine (lgn.position,other.position,new Color (0,1,0,0.5F));
conn = i;
}
}
lgn.SetConnection (dir,conn);
*/
//Debug.Log ("Yey");
//Debug.DrawLine (node.position,minNode.position,Color.yellow);
}
}
/** 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;
}
/** Returns if \a _b is visible from \a _a 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, Node hint, out GraphHitInfo hit)
{
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;
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;
}*/
Node node = nodes[z*width+x];
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.success = false;
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;
}
}
}
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, Node hint)
{
if (nodes == null || depth*width != nodes.Length) {
//Debug.LogError ("NavGraph hasn't been generated yet");
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]);
//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 void DeSerializeNodes(Node[] nodes, AstarSerializer serializer)
{
/*for (int i=0;i<nodes.Length;i++) {
GridNode node = nodes[i] as GridNode;
if (node == null) {
Debug.LogError ("DeSerialization Error : Couldn't cast the node to the appropriate type - GridGenerator");
return;
}*/
GenerateMatrix ();
SetUpOffsetsAndCosts ();
if (nodes == null || nodes.Length == 0) {
return;
}
nodes = new GridNode[nodes.Length];
int gridIndex = GridNode.SetGridGraph (this);
int numberSize = (int)serializer.readerStream.ReadByte ();
for (int z = 0; z < depth; z ++) {
for (int x = 0; x < width; x++) {
GridNode node = nodes[z*width+x] as GridNode;
nodes[z*width+x] = node;
if (node == null) {
Debug.LogError ("DeSerialization Error : Couldn't cast the node to the appropriate type - GridGenerator");
return;
}
node.SetIndex (z*width+x);
node.SetGridIndex (gridIndex);
float yPos = 0;
//if (serializer.mask == AstarSerializer.SMask.SaveNodePositions) {
//Needs to multiply with precision factor because the position will be scaled by Int3.Precision later (Vector3 --> Int3 conversion)
if (numberSize == 0) {
yPos = serializer.readerStream.ReadInt16 ();
} else {
yPos = serializer.readerStream.ReadInt32 ();
}
//}
node.position = (Int3)matrix.MultiplyPoint3x4 (new Vector3 (x+0.5F,yPos,z+0.5F));
}
}
}