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 static void Optimize(Grid3DGraph map, ref List <Int3> path)
{
graphMap = map;
CombinePoints(ref path);
SmoothPath(ref path);
}
public override void OnDrawGizmosSelected(Transform trans)
{
if (graphMap == null)
{
graphMap = new Grid3DGraph();
graphMap.Init(this);
}
Matrix4x4 defaultMatrix = Gizmos.matrix;
Gizmos.matrix = trans.localToWorldMatrix;
Color defaultColor = Gizmos.color;
// begin draw
//var gridSize = new Vector3(cfg.cellSize / 1000f, cfg.cellSize / 1000f, cfg.cellSize / 1000f);
//var minPos = navData.buildConfig.worldMinPos;
//float cellSize = cfg.cellSize / 1000f;
//float cellRadius = cellSize / 2;
Vector3 nodesz = new Vector3(0.05f, 0.05f, 0.05f);
if (spaces != null && drawSpaces)
{
for (int i = 0; i < spaces.Count; ++i)
{
var space = spaces[i];
Gizmos.DrawWireCube((Vector3)space.minPos + (Vector3)(space.cellCount * buildConfig.cellSize) / 2f, (Vector3)(space.cellCount * buildConfig.cellSize));
}
}
if (cells != null && drawNodes)
{
for (int i = 0; i < cells.Count; ++i)
{
var cell = cells[i];
Gizmos.color = red;
Gizmos.DrawWireCube(cell.worldPosition, nodesz);
}
}
if (finalCells != null)
{
for (int i = 0; i < finalCells.Count; ++i)
{
var cell = finalCells[i];
if (cell.worldPos1 == Vector3.zero || cell.worldPos2 == Vector3.zero || cell.worldPos3 == Vector3.zero || cell.worldPos4 == Vector3.zero)
{
continue;
}
Gizmos.color = green;
Gizmos.DrawLine(cell.worldPos1, cell.worldPos2);
Gizmos.DrawLine(cell.worldPos2, cell.worldPos3);
Gizmos.DrawLine(cell.worldPos3, cell.worldPos4);
Gizmos.DrawLine(cell.worldPos4, cell.worldPos1);
}
}
if (graphMap != null && drawGraph)
{
#if UNITY_EDITOR
for (int i = 0; i < graphMap.edgeList.Count; ++i)
{
var edge = graphMap.edgeList[i];
var from = graphMap.nodeDic[edge.from];
var to = graphMap.nodeDic[edge.to];
Gizmos.color = green;
Gizmos.DrawLine((Vector3)from.worldPosition, (Vector3)to.worldPosition);
}
#endif
}
// end draw
Gizmos.color = defaultColor;
Gizmos.matrix = defaultMatrix;
}
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);
}
