private List <VertexPositionColor> TestAStar(CompiledNavMesh compiledMesh, Vector3 startVec, Vector3 endVec, int maxFall) { var startNode = compiledMesh.FindFloorUnderPoint(startVec.X, startVec.Y, startVec.Z + 2, maxFall); var goalNode = compiledMesh.FindFloorUnderPoint(endVec.X, endVec.Y, endVec.Z + 2, maxFall); if (startNode.blockIndex == goalNode.blockIndex) { return(null); } var exactEnd = endVec; // TODO - caller should provide this var start = compiledMesh.WorldPointFromNode(startNode); var goal = compiledMesh.WorldPointFromNode(goalNode); var floorLineVertices = new List <VertexPositionColor>(); floorLineVertices.Add(new VertexPositionColor(start + new Vector3(0, 0, 5), Color.Lime)); floorLineVertices.Add(new VertexPositionColor(start, Color.Lime)); floorLineVertices.Add(new VertexPositionColor(goal + new Vector3(0, 0, 5), Color.Red)); floorLineVertices.Add(new VertexPositionColor(goal, Color.Red)); var path = compiledMesh.AStar(startNode, goalNode); var currentNode = goalNode; while (true) { if (!path.ContainsKey(currentNode)) { throw new InvalidOperationException(); } var next = path[currentNode]; // walks backwards from goal, so really 'prev' if (next.Equals(default(CompiledNavMesh.NodeId))) { break; } var ep0 = compiledMesh.WorldPointFromNode(currentNode); var ep1 = compiledMesh.WorldPointFromNode(next); floorLineVertices.Add(new VertexPositionColor(ep0 + new Vector3(0, 0, 0.1f), Color.White)); floorLineVertices.Add(new VertexPositionColor(ep1 + new Vector3(0, 0, 0.1f), Color.White)); currentNode = next; } // overlay the reduced path on top of original AStar results... // findPath runs AStar again but with path point reduction var pathPointList = compiledMesh.findPath(startNode, goalNode, exactEnd); for (int i = 1; i < pathPointList.Count; i++) { floorLineVertices.Add(new VertexPositionColor(pathPointList[i] + new Vector3(0, 0, 0.2f), Color.Blue)); floorLineVertices.Add(new VertexPositionColor(pathPointList[i - 1] + new Vector3(0, 0, 0.2f), Color.Blue)); } return(floorLineVertices); }
private List <VertexPositionColor> RenderFloorLines(CompiledNavMesh compiledMesh, Vector3 clipCenter, int clipSpan) { var floorLineVertices = new List <VertexPositionColor>(); // DEBUG MARKERS // floorLineVertices.Add(new VertexPositionColor(new Vector3(126, 65, 144.3f + 3), Color.Lime)); // floorLineVertices.Add(new VertexPositionColor(new Vector3(126, 65, 144.3f), Color.Lime)); // floorLineVertices.Add(new VertexPositionColor(new Vector3(127, 64, 145.09f + 3), Color.Red)); // floorLineVertices.Add(new VertexPositionColor(new Vector3(127, 64, 145.09f), Color.Red)); //============================================= // TESTING COMPILED MESH GETNEIGHBOR //start... // floorLineVertices.Add(new VertexPositionColor(new Vector3(133, 103, 150), Color.White)); // floorLineVertices.Add(new VertexPositionColor(new Vector3(133, 103, 144), Color.White)); /* * var curNode = compiledMesh.FindFloorUnderPoint(132.9f, 102.9f, 149, 20); * Vector3 v = compiledMesh.WorldFromBlockIndex(curNode.blockIndex, 144); * * floorLineVertices.Add(new VertexPositionColor(v + new Vector3(0,0,5), Color.Lime)); * floorLineVertices.Add(new VertexPositionColor(v, Color.Lime)); * * curNode = compiledMesh.GetNeighbor(curNode, 0, 1); * v = compiledMesh.WorldFromBlockIndex(curNode.blockIndex, 144); * floorLineVertices.Add(new VertexPositionColor(v + new Vector3(0, 0, 5), Color.Cyan)); * floorLineVertices.Add(new VertexPositionColor(v, Color.Lime)); * * curNode = compiledMesh.GetNeighbor(curNode, 0, 1); * v = compiledMesh.WorldFromBlockIndex(curNode.blockIndex, 144); * floorLineVertices.Add(new VertexPositionColor(v + new Vector3(0, 0, 5), Color.Yellow)); * floorLineVertices.Add(new VertexPositionColor(v, Color.Lime)); * * curNode = compiledMesh.GetNeighbor(curNode, 1, 1); * v = compiledMesh.WorldFromBlockIndex(curNode.blockIndex, 144); * floorLineVertices.Add(new VertexPositionColor(v + new Vector3(0, 0, 5), Color.White)); * floorLineVertices.Add(new VertexPositionColor(v, Color.Lime)); */ // END TEST //============================================= //============================================= // TEST A* //TestAStar(compiledMesh, new Vector3(141, 21, 145), new Vector3(140, 28, 144), 10, floorLineVertices); //TestAStar(compiledMesh, new Vector3(155, 56, 144), new Vector3(169, 23, 144), 10, floorLineVertices); // TestAStar(compiledMesh, new Vector3(130, 101, 145), new Vector3(125, 106, 147), 10, floorLineVertices); // END A* //============================================= int clipX1 = 0; int clipY1 = 0; int clipX2 = compiledMesh.BlockWidth; int clipY2 = compiledMesh.BlockHeight; if (clipSpan != 0) { var node = compiledMesh.FindFloorUnderPoint(clipCenter.X, clipCenter.Y, clipCenter.Z, 50); if (node.blockIndex != 0 || node.directionFlags != 0) { var pt = compiledMesh.BlockXYFromIndex(node.blockIndex); clipX1 = Math.Max(0, pt.X - clipSpan); clipY1 = Math.Max(0, pt.Y - clipSpan); clipX2 = Math.Min(clipX2, pt.X + clipSpan); clipY2 = Math.Min(clipY2, pt.Y + clipSpan); } } //var pt1 = compiledMesh.WorldFromBlockIndex(2206555, 115); //var pt2 = compiledMesh.WorldFromBlockIndex(2208535, 115); //var DBGdir = compiledMesh.getDirectionToNeighborByIndex(2206555, 2208535); //Util.DrawBoundingBox(BoundingBox.CreateFromPoints(new[] { new Vector3(pt1.X - 0.1f, pt1.Y - 0.1f, 115), new Vector3(pt1.X + 0.1f, pt1.Y + 0.1f, 122) }), floorLineVertices, Color.Lime); //Util.DrawBoundingBox(BoundingBox.CreateFromPoints(new[] { new Vector3(pt2.X - 0.1f, pt2.Y - 0.1f, 115), new Vector3(pt2.X + 0.1f, pt2.Y + 0.1f, 122) }), floorLineVertices, Color.Red); for (int bY = clipY1; bY < clipY2; bY++) { for (int bX = clipX1; bX < clipX2; bX++) { float x = compiledMesh.X1 + bX * compiledMesh.Step; float y = compiledMesh.Y1 + bY * compiledMesh.Step; compiledMesh.ForeachHeightAtXY(bX, bY, (directionFlags, z) => { /*Color xcolor = Color.Lime; * * //if (bX == 28 && bY == 0) Debugger.Break(); * * if (y > 221 && y < 223 && x > 346 && x < 348) * xcolor = Color.White; * else if (y > 221 && y < 223 && x > 347 && x < 349) * xcolor = Color.Blue; * else if (y >= 221 && y <= 223 && x >= 346 && x <= 348) * xcolor = Color.Red;*/ // DRAW ELEVATION MARKER /* floorLineVertices.Add(new VertexPositionColor(new Vector3(x - 0.1f, y, z), xcolor)); * floorLineVertices.Add(new VertexPositionColor(new Vector3(x, y + 0.1f, z), xcolor)); * * floorLineVertices.Add(new VertexPositionColor(new Vector3(x, y + 0.1f, z), xcolor)); * floorLineVertices.Add(new VertexPositionColor(new Vector3(x + 0.1f, y, z), xcolor)); * * floorLineVertices.Add(new VertexPositionColor(new Vector3(x + 0.1f, y, z), xcolor)); * floorLineVertices.Add(new VertexPositionColor(new Vector3(x, y - 0.1f, z), xcolor)); * * floorLineVertices.Add(new VertexPositionColor(new Vector3(x, y - 0.1f, z), xcolor)); * floorLineVertices.Add(new VertexPositionColor(new Vector3(x - 0.1f, y, z), xcolor)); */ // EDGE CONNECTIONS // draw lines through the compass points... float halfstep = compiledMesh.Step; float n; n = compiledMesh.GetEdge(bX, bY, z, directionFlags, NavMeshUtil.DIRECTION_LEFT); if (n >= 0) { var test = Color.Orange; if (Math.Abs(z - n) >= 0.9f) { test = Color.Red; } floorLineVertices.Add(new VertexPositionColor(new Vector3(x, y, z), test)); floorLineVertices.Add(new VertexPositionColor(new Vector3(x - halfstep, y, n), test)); } n = compiledMesh.GetEdge(bX, bY, z, directionFlags, NavMeshUtil.DIRECTION_TL); if (n >= 0) { var test = Color.Orange; if (Math.Abs(z - n) / (new Vector2(x, y) - new Vector2(x - halfstep, y + halfstep)).Length() >= 1.4f) { test = Color.Magenta; } floorLineVertices.Add(new VertexPositionColor(new Vector3(x, y, z), test)); floorLineVertices.Add(new VertexPositionColor(new Vector3(x - halfstep, y + halfstep, n), test)); } n = compiledMesh.GetEdge(bX, bY, z, directionFlags, NavMeshUtil.DIRECTION_TOP); if (n >= 0) { var test = Color.Orange; if (Math.Abs(z - n) >= 0.9f) { test = Color.Red; } floorLineVertices.Add(new VertexPositionColor(new Vector3(x, y, z), test)); floorLineVertices.Add(new VertexPositionColor(new Vector3(x, y + halfstep, n), test)); } n = compiledMesh.GetEdge(bX, bY, z, directionFlags, NavMeshUtil.DIRECTION_TR); if (n >= 0) { var test = Color.Orange; if (Math.Abs(z - n) / (new Vector2(x, y) - new Vector2(x + halfstep, y + halfstep)).Length() >= 1.4f) { test = Color.Purple; } floorLineVertices.Add(new VertexPositionColor(new Vector3(x, y, z), test)); floorLineVertices.Add(new VertexPositionColor(new Vector3(x + halfstep, y + halfstep, n), test)); } }); } } return(floorLineVertices); }