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