protected override int CalCostH(ProcessingNode node, AStarContext context)
{
Grid3DNode targetNode = context.GetTargetNode().astarNode as Grid3DNode;
Grid3DNode calNode = node.astarNode as Grid3DNode;
int dx = Math.Abs(targetNode.x - calNode.x);
int dy = Math.Abs(targetNode.y - calNode.y);
int dz = Math.Abs(targetNode.z - calNode.z);
dx *= 10;
dy *= 10;
dz *= 10;
int distxz = (int)(dx > dz ? 1.4f * dz + (dx - dz) : 1.4f * dx + (dz - dx));
int dist = distxz + dy;
return(dist);
}
private void ParseNavData(Grid3DNavData navData)
{
navGraphData = navData;
nodeMatrix = new Grid3DNode[navData.buildConfig.cellCount.x, navData.buildConfig.cellCount.y, navData.buildConfig.cellCount.z];
for (int i = 0; i < navData.nodeList.Count; ++i)
{
Grid3DNode node = null;
if (navData.bytesMode)
{
node = navData.ParseNode(i);
}
else
{
node = navData.nodeList[i];
}
nodeMatrix[node.x, node.y, node.z] = node;
this.AddNode(node);
#if UNITY_EDITOR
nodeList.Add(node);
nodeDic.Add(node.id, node);
#endif
}
for (int i = 0; i < navData.edgeList.Count; ++i)
{
Grid3DEdge edge = null;
if (navData.bytesMode)
{
edge = navData.ParseEdge(i);
}
else
{
edge = navData.edgeList[i];
}
Grid3DEdge edgeToFrom = new Grid3DEdge(edge.to, edge.from, edge.cost);
this.AddEdge(edge);
this.AddEdge(edgeToFrom);
#if UNITY_EDITOR
edgeList.Add(edge);
edgeList.Add(edgeToFrom);
#endif
}
}
private void FindPath(ProcessingNode start, ProcessingNode end, AStarContext context)
{
Grid3DGraph graphMap = context.map as Grid3DGraph;
ProcessingNode endNode = DoAStar(context);
context.rawPathNodeCache.Clear();
context.rawPathPoints.Clear();
ProcessingNode pathNode = endNode;
while (pathNode != null)
{
Grid3DNode navNode = pathNode.astarNode as Grid3DNode;
Grid3DNode node = graphMap.GetNodeAt(navNode.x, navNode.y, navNode.z);
context.rawPathNodeCache.Add(navNode);
context.rawPathPoints.Add(node.worldPosition);
pathNode = pathNode.prev;
}
//var points = FindPath3D(startNode.id, endNode.id);
/*Grid3DPathOptimizer.Optimize(graphMap, ref points);
*
* result.Clear();
* for (int i = 0; i < points.Count; ++i)
* {
* var point = points[i];
* var node = graphMap.GetNodeAt(point.x, point.y, point.z);
* result.Add(new Int3(node.worldPosition.x, node.worldPosition.y, node.worldPosition.z));
* }
* if (result.Count >= 2)
* {
* result[0] = from;
* result[result.Count - 1] = to;
* }
* if (result.Count >= 4)
* {
* int cellSize = graphMap.navGraphData.buildConfig.cellSize;
* if ((result[1] - from).sqrMagnitudeLong < cellSize * cellSize)
* result.RemoveAt(1);
* if ((result[result.Count - 2] - to).sqrMagnitudeLong < cellSize * cellSize)
* result.RemoveAt(result.Count - 2);
* }
* return result.Count >= 2;*/
}
public Grid3DNode ParseNode(int index)
{
Grid3DNode node = new Grid3DNode();
int bytesIndex = index * NodeSize;
node.id = ByteTool.IntFromBytes(nodeBytes, bytesIndex);
bytesIndex += 4;
node.x = ByteTool.UshortFromBytes(nodeBytes, bytesIndex);
bytesIndex += 2;
node.y = ByteTool.UshortFromBytes(nodeBytes, bytesIndex);
bytesIndex += 2;
node.z = ByteTool.UshortFromBytes(nodeBytes, bytesIndex);
bytesIndex += 2;
node.worldPosition.x = ByteTool.IntFromBytes(nodeBytes, bytesIndex);
bytesIndex += 4;
node.worldPosition.y = ByteTool.IntFromBytes(nodeBytes, bytesIndex);
bytesIndex += 4;
node.worldPosition.z = ByteTool.IntFromBytes(nodeBytes, bytesIndex);
return(node);
}
public void AddNode(Grid3DNode node)
{
nodeList.Add(node);
nodeDic.Add(node.id, node);
nodeCount = Math.Max(nodeCount, node.id);
}
private void CellsToGraph()
{
navData = ScriptableObject.CreateInstance <Grid3DNavData>();
navData.Init(cfg);
int count = 0;
float cellSize = cfg.cellSize / 1000f;
// make nodes
for (int i = 0; i < rawCells.Count; ++i)
{
count++;
if (count % 10 == 0)
{
EditorUtility.DisplayProgressBar(string.Format("Building graph nodes {0}/{1}", count, rawCells.Count), "", (float)count / rawCells.Count);
}
var cell = rawCells[i];
if (cell == null || !cell.walkable)
{
continue;
}
int x = cell.pos.x;
int y = cell.pos.y;
int z = cell.pos.z;
int id = cell.id;
var node = new Grid3DNode();
node.id = id;
node.x = (ushort)x;
node.y = (ushort)y;
node.z = (ushort)z;
node.worldPosition = new Int3(cell.worldPosition);
//node.walkable = cell.walkable;
navData.AddNode(node);
}
// make edges
HashSet <string> edgeKeySet = new HashSet <string>();
for (int i = 0; i < rawCells.Count; ++i)
{
if (i % 10 == 0)
{
EditorUtility.DisplayProgressBar(string.Format("Building graph edges {0}/{1}", i, rawCells.Count), "", (float)i / rawCells.Count);
}
var cell = rawCells[i];
if (cell == null || !cell.walkable)
{
continue;
}
int x = cell.pos.x;
int y = cell.pos.y;
int z = cell.pos.z;
int id = cell.id;
for (int ix = -1; ix <= 1; ix++)
{
for (int iy = -1; iy <= 1; iy++)
{
for (int iz = -1; iz <= 1; iz++)
{
int realx = x + ix;
int realy = y + iy;
int realz = z + iz;
if (realx >= 0 && realx < cfg.cellCount.x &&
realy >= 0 && realy < cfg.cellCount.y &&
realz >= 0 && realz < cfg.cellCount.z)
{
if (ix == 0 && iz == 0)
{
continue;
}
if (!Enable8Directions && ix != 0 && iz != 0)
{
continue;
}
var neighbor = cellArray[realx, realy, realz];
if (neighbor != null)
{
int nbId = neighbor.id;
int cost = int.MaxValue;
int absDelta = Math.Abs(ix) + Math.Abs(iy) + Math.Abs(iz);
if (neighbor.walkable)
{
// to_from edge has been already added, so ignore it
if (edgeKeySet.Contains(string.Format("{0}_{1}", nbId, id)))
{
continue;
}
else
{
edgeKeySet.Add(string.Format("{0}_{1}", id, nbId));
}
float tan = 0;
float dx = cellSize;
float dy = Math.Abs(neighbor.worldPosition.y - cell.worldPosition.y);
float dz = cellSize;
float distance = (cell.worldPosition - neighbor.worldPosition).magnitude;
cost = (int)Math.Round(distance / (cfg.cellSize / 1000f) * 10);
if (absDelta == 1)
{
if (ix == 0)
{
tan = dy / dz;
}
else if (iz == 0)
{
tan = dy / dx;
}
else
{
tan = 0;
}
}
else if (absDelta == 2)
{
if (iy == 0)
{
tan = dy / cellSize * 1.414f;
}
else
{
tan = dy / cellSize;
}
}
else if (absDelta == 3)
{
tan = dy / cellSize * 1.414f;
}
if (tan > cfg.tanSlope)
{
cost = int.MaxValue;
}
if (cost != int.MaxValue)
{
var edge = new Grid3DEdge(id, nbId, cost);
navData.AddEdge(edge);
}
}
}
}
}
}
}
}
graphMap = new Grid3DGraph();
graphMap.Init(navData);
}
