public NavigationGridlet[] FindGridletPath(NavigationGridlet start, NavigationGridlet end)
{
Dictionary<NavigationGridlet, int> scoresByGridlet = new Dictionary<NavigationGridlet, int>();
scoresByGridlet.Add(start, 0);
var s = new Stack<KeyValuePair<NavigationGridlet, int>>();
s.Push(new KeyValuePair<NavigationGridlet, int>(start, 0));
bool success = false;
while (s.Any()) {
var kvp = s.Pop();
foreach (var neighbor in kvp.Key.Neighbors) {
if (!scoresByGridlet.ContainsKey(neighbor) && neighbor.IsEnabled) {
scoresByGridlet.Add(neighbor, kvp.Value + 1);
s.Push(new KeyValuePair<NavigationGridlet, int>(neighbor, kvp.Value + 1));
if (neighbor == end) {
success = true;
break;
}
}
}
}
if (!success) {
Console.WriteLine("GRidlet pathing failed!");
return null;
} else {
var current = end;
List<NavigationGridlet> path = new List<NavigationGridlet>();
while (current != start) {
path.Add(current);
int minimumDistance = Int32.MaxValue;
NavigationGridlet minimumNeighbor = null;
foreach (var neighbor in current.Neighbors) {
int score;
if (scoresByGridlet.TryGetValue(neighbor, out score)) {
if (minimumDistance > score) {
minimumDistance = score;
minimumNeighbor = neighbor;
}
}
}
current = minimumNeighbor;
}
path.Add(start);
var result = path.ToArray();
Array.Reverse(result);
return result;
}
}
public NavigationGridlet Quad(float cx, float cy, float cz, float pitch, float yaw, float roll, int xLength, int yLength)
{
var cells = Util.Generate(xLength * yLength, i => new NavigationGridletCell(i, i % xLength, i / xLength));
for (var i = 0; i < cells.Length; i++) {
var zzx = i % xLength - xLength / 2;
var zzy = i / xLength - yLength / 2;
cells[i].Height = 1;
// cells[i].Height = (float)(Math.Sqrt(zzx * zzx + zzy * zzy) / 2); //(float)StaticRandom.NextDouble(2);
}
for (var y = 0; y < yLength; y++) {
cells[y * xLength].Flags = CellFlags.Edge;
cells[(y + 1) * xLength - 1].Flags = CellFlags.Edge;
}
for (var x = 1; x < xLength - 1; x++) {
cells[x].Flags = CellFlags.Edge;
cells[x + xLength * (yLength - 1)].Flags = CellFlags.Edge;
}
var gridlet = new NavigationGridlet { X = cx, Y = cy, Z = cz, XLength = xLength, YLength = yLength, Cells = cells, IsEnabled = true };
gridlet.Orientation = Matrix.RotationZ(yaw) * Matrix.RotationY(pitch) * Matrix.RotationZ(roll);
foreach (var cell in cells) {
cell.Gridlet = gridlet;
}
gridlet.Initialize();
for (var x = 20; x < 30; x++) {
for (var y = 20; y < 30; y++) {
cells[x + y * xLength].Flags |= CellFlags.Blocked;
}
}
for (var x = 40; x < 50; x++) {
for (var y = 20; y < 30; y++) {
cells[x + y * xLength].Flags |= CellFlags.Blocked;
}
}
return gridlet;
}
public NavigationGridletElement(NavigationGridlet gridlet)
{
this.gridlet = gridlet;
}
public void AddNeighbor(NavigationGridlet gridlet)
{
Neighbors.Add(gridlet);
}
private NavigationPath LocalPathlet(NavigationGridlet gridlet, Vector3 start, Vector3 end)
{
// var triangles = gridlet.Mesh;
return new NavigationPath(new[] { start, end });
}
private List<NavigationPath> GetConnectorToConnectorPathlets(Vector3 startPosition, Vector3 endPosition, int currentGridletIndex, NavigationGridlet[] gridletPath)
{
var currentGridlet = gridletPath[currentGridletIndex];
var destinationGridlet = gridletPath[currentGridletIndex + 1];
var edges = currentGridlet.EdgeCells.Where(c => c.Neighbors.Any(n => n.Gridlet == destinationGridlet)).ToArray();
var currentObb = currentGridlet.OrientedBoundingBox;
var result = new List<NavigationPath>();
foreach (var connectingEdge in edges) {
var neighbors = connectingEdge.Neighbors.Where(n => n.Gridlet == destinationGridlet).ToArray();
foreach (var neighbor in neighbors) {
var neighborObb = neighbor.OrientedBoundingBox;
var pathlet = new NavigationPath(new[] { startPosition, neighborObb.Center });
result.Add(pathlet);
}
}
return result;
}
public GridletComponent(NavigationGrid grid, NavigationGridlet gridlet)
{
this.grid = grid;
this.gridlet = gridlet;
}