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); }