public Grid3DEdge ParseEdge(int index)
{
Grid3DEdge edge = new Grid3DEdge();
int bytesIndex = index * EdgeSize;
edge.from = ByteTool.IntFromBytes(edgeBytes, bytesIndex);
bytesIndex += 4;
edge.to = ByteTool.IntFromBytes(edgeBytes, bytesIndex);
bytesIndex += 4;
edge.cost = ByteTool.IntFromBytes(edgeBytes, bytesIndex);
return(edge);
}
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
}
}
public void AddEdge(Grid3DEdge edge)
{
edgeList.Add(edge);
}
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);
}