public static bool IsOverlappingVertical(NavQuad a, NavQuad b, float widthOverlap, float heightOverlap)
{
Vector3 aHalf = a.scale * 0.5f;
Vector3 bHalf = b.scale * 0.5f;
Vector3 aMin = a.position - aHalf;
Vector3 aMax = a.position + aHalf;
Vector3 bMin = b.position - bHalf;
Vector3 bMax = b.position + bHalf;
// check y
if ((aMax.y - bMin.y) >= heightOverlap && (bMax.y - aMin.y) >= heightOverlap)
{
// check x or z
if (a.scale.x > a.scale.z && b.scale.x > b.scale.z)
{
return((aMax.x - bMin.x) >= widthOverlap && (bMax.x - aMin.x) >= widthOverlap);
}
else if (a.scale.z > a.scale.x && b.scale.z > b.scale.x)
{
return((aMax.z - bMin.z) >= widthOverlap && (bMax.z - aMin.z) >= widthOverlap);
}
else
{
throw new System.InvalidOperationException();
}
}
else
{
return(false);
}
}
public void Walk()
{
NavQuad startNavQuad = Services.MapManager.GetNavQuadClosestToPosition(startingLocation);
NavQuad endNavQuad = Services.MapManager.GetNavQuadClosestToPosition(endLocation);
path = AStarSearch.ShortestPath(startNavQuad, endNavQuad, false);
}
public static List <NavQuad> ShortestPath(NavQuad start, NavQuad goal, bool raw)
{
List <NavQuad> path = new List <NavQuad>();
Dictionary <NavQuad, NavQuad> cameFrom = new Dictionary <NavQuad, NavQuad>();
Dictionary <NavQuad, float> costSoFar = new Dictionary <NavQuad, float>();
NavQuad estimatedClosestQuad = start;
PriorityQueue <NavQuad> frontier = new PriorityQueue <NavQuad>();
frontier.Enqueue(start, 0);
cameFrom[start] = start;
costSoFar[start] = 0;
while (frontier.Count > 0)
{
NavQuad current = frontier.Dequeue();
if (Heuristic(current, goal) < Heuristic(estimatedClosestQuad, goal))
{
estimatedClosestQuad = current;
}
if (current == goal)
{
break;
}
foreach (NavQuad next in current.neighbors)
{
if (!next.IsImpassable())
{
float newCost;
if (raw)
{
newCost = costSoFar[current] + 1;
}
else
{
newCost = costSoFar[current] + next.movementCost;
}
if (!costSoFar.ContainsKey(next) || newCost < costSoFar[next])
{
costSoFar[next] = newCost;
float priority = newCost + Heuristic(next, goal);
frontier.Enqueue(next, priority);
cameFrom[next] = current;
}
}
}
}
NavQuad pathNode = estimatedClosestQuad;
while (pathNode != start)
{
path.Add(pathNode);
pathNode = cameFrom[pathNode];
}
path.Reverse();
return(path);
}
private List <NavQuad> GetList(NavQuad quad)
{
if (!quadsTolinks.ContainsKey(quad))
{
quadsTolinks.Add(quad, new List <NavQuad>());
}
return(quadsTolinks[quad]);
}
private NavLink(NavQuad from, NavQuad to, Position2 point0, Position2 point1)
{
this.From = from;
this.To = to;
this.P0 = point0;
this.P1 = point1;
this.Distance = (from.Center - to.Center).Length;
}
public void AddLink(NavQuad a, NavQuad b)
{
GetList(b);
var aList = GetList(a);
if (!aList.Contains(b))
{
aList.Add(b);
}
}
private IEnumerable <NavQuad> GetAdjacentNodesForIslandSearch(NavQuad node)
{
foreach (var adj in GetList(node))
{
if (GetList(adj).Contains(node))
{
yield return(adj);
}
}
}
public NavigationIsland GetIslandFromQuad(NavQuad quad)
{
if (quadsToIslands.TryGetValue(quad, out var island))
{
return(island);
}
else
{
return(null);
}
}
public static List <NavQuad> FindAllAvailableGoals(NavQuad start, float movementAvailable, bool raw)
{
List <NavQuad> availableGoals = new List <NavQuad>();
if (movementAvailable == 0)
{
return(availableGoals);
}
Dictionary <NavQuad, NavQuad> cameFrom = new Dictionary <NavQuad, NavQuad>();
Dictionary <NavQuad, float> costSoFar = new Dictionary <NavQuad, float>();
Queue <NavQuad> frontier = new Queue <NavQuad>();
frontier.Enqueue(start);
cameFrom[start] = start;
costSoFar[start] = 0;
while (frontier.Count > 0)
{
NavQuad current = frontier.Dequeue();
if (costSoFar[current] <= movementAvailable)
{
if (current != start)
{
availableGoals.Add(current);
}
foreach (NavQuad next in current.neighbors)
{
if (!next.IsImpassable() || raw)
{
float newCost;
if (raw)
{
newCost = costSoFar[current] + 1;
}
else
{
newCost = costSoFar[current] + next.movementCost;
}
if (!costSoFar.ContainsKey(next) || newCost < costSoFar[next])
{
costSoFar[next] = newCost;
frontier.Enqueue(next);
cameFrom[next] = current;
}
}
}
}
}
return(availableGoals);
}
public NavQuad GetNavQuadClosestToPosition(Vector3 pos)
{
NavQuad closestNavQuad = null;
float closestDist = Mathf.Infinity;
foreach (NavQuad nq in navQuads)
{
if (Vector3.Distance(nq.position, pos) < closestDist)
{
closestNavQuad = nq;
closestDist = Vector3.Distance(nq.position, pos);
}
}
return(closestNavQuad);
}
public NavQuad GetQuadForPosition(Vector3 position)
{
NavQuad bestQuad = null;
float bestQuadDistance = Mathf.Infinity;
foreach (var quad in quadsTolinks.Keys)
{
if (quad.ContainsPoint(position, out float f))
{
if (f >= 0 && f < bestQuadDistance)
{
(bestQuadDistance, bestQuad) = (f, quad);
}
}
}
return(bestQuad);
}
public void Generate()
{
var navMesh = new NavMesh(this.game);
var intersectionMap = new Dictionary <Intersection, MergedIntersection>();
var intersectionSideMap = new Dictionary <MergedIntersection, Game.Street[]>();
var intersectionQuadMap = new Dictionary <MergedIntersection, NavQuad>();
foreach (var mergedIntersection in this.game.GetList <MergedIntersection>())
{
var streets = new Game.Street[4];
foreach (var street in mergedIntersection.Streets)
{
var thisNode = mergedIntersection
.Intersections.First(i => i == street.Node1 || i == street.Node2);
var otherNode = street.OtherNode(thisNode);
var diff = otherNode.Position - thisNode.Position;
if (diff.X == 0.U())
{
streets[diff.Y > 0.U() ? Block.Side.North : Block.Side.South] = street;
}
else if (diff.Y == 0.U())
{
streets[diff.X > 0.U() ? Block.Side.East : Block.Side.West] = street;
}
else
{
throw new Exception("");
}
}
var padding = 0.3.U();
var cornerSW = getIntersectionCorner(streets, Block.Corner.SouthWest);
var cornerSE = getIntersectionCorner(streets, Block.Corner.SouthEast);
var cornerNW = getIntersectionCorner(streets, Block.Corner.NorthWest);
var cornerNE = getIntersectionCorner(streets, Block.Corner.NorthEast);
var navQuad = new NavQuad(
cornerSW + new Difference2(padding, padding),
cornerSE + new Difference2(-padding, padding),
cornerNW + new Difference2(padding, -padding),
cornerNE + new Difference2(-padding, -padding)
);
navMesh.Add(navQuad);
intersectionSideMap.Add(mergedIntersection, streets);
intersectionQuadMap.Add(mergedIntersection, navQuad);
foreach (var intersection in mergedIntersection.Intersections)
{
intersectionMap.Add(intersection, mergedIntersection);
}
}
foreach (var mergedIntersection in this.game.GetList <MergedIntersection>())
{
var intersectionQuad = intersectionQuadMap[mergedIntersection];
foreach (var street in mergedIntersection.Streets
.Where(s => mergedIntersection.Intersections.Contains(s.Node1)))
{
var side = (Block.Side)Array.IndexOf(intersectionSideMap[mergedIntersection], street);
var quad1 = intersectionQuad;
var quad2 = intersectionQuadMap[intersectionMap[street.Node2]];
if (side == Block.Side.East || side == Block.Side.West)
{
if (side == Block.Side.East)
{
Do.Swap(ref quad1, ref quad2);
}
var navQuad = new NavQuad(quad2.SE, quad1.SW, quad2.NE, quad1.NW);
NavLink.CreatePair(navQuad, quad1, quad1.SW, quad1.NW);
NavLink.CreatePair(navQuad, quad2, quad2.SE, quad2.NE);
navMesh.Add(navQuad);
}
else if (side == Block.Side.South || side == Block.Side.North)
{
if (side == Block.Side.South)
{
Do.Swap(ref quad1, ref quad2);
}
var navQuad = new NavQuad(quad1.NW, quad1.NE, quad2.SW, quad2.SE);
NavLink.CreatePair(navQuad, quad1, quad1.NW, quad1.NE);
NavLink.CreatePair(navQuad, quad2, quad2.SW, quad2.SE);
navMesh.Add(navQuad);
}
}
}
}
public void SetAdjacentBoundry(NavCellBoundry adjacent, NavMeshGenerationSettings settings)
{
if (adjacent == null)
{
connections = null;
islandsToIslands = null;
return;
}
else
{
lock (adjacentBoundryCalculationLock) lock (adjacent.adjacentBoundryCalculationLock)
{
// set adjacent connection
this.connectedBoundry = adjacent;
adjacent.connectedBoundry = this;
// create maps for algorithm
Dictionary <NavQuad, List <int> > thisToThatMap = new Dictionary <NavQuad, List <int> >();
Dictionary <NavQuad, List <int> > thatToThisMap = new Dictionary <NavQuad, List <int> >();
Dictionary <NavigationIsland, List <NavigationIsland> > thisToThatIslands
= new Dictionary <NavigationIsland, List <NavigationIsland> >();
Dictionary <NavigationIsland, List <NavigationIsland> > thatToThisIslands
= new Dictionary <NavigationIsland, List <NavigationIsland> >();
// intialize maps
foreach (var quad in this.quads)
{
thisToThatMap.Add(quad, new List <int>());
}
foreach (var quad in adjacent.quads)
{
thatToThisMap.Add(quad, new List <int>());
}
// The multiplication by 0.999999 helps combat floating point rounding errors
float requiredWidth = settings.orientation.VoxelSize * 0.5f;
float requiredHeight = settings.crouchHeightInVoxels * settings.orientation.VoxelSize * 0.5f;
for (int i = 0; i < quads.Length; i++)
{
NavQuad thisQuad = quads[i];
for (int j = 0; j < adjacent.quads.Length; j++)
{
NavQuad otherQuad = adjacent.quads[j];
if (NavQuad.IsOverlappingVertical(thisQuad, otherQuad, requiredWidth, requiredHeight))
{
float aHeight = thisQuad.position.y - thisQuad.scale.y * 0.5f;
float bHeight = otherQuad.position.y - otherQuad.scale.y * 0.5f;
if (settings.CanClimbTo(aHeight, bHeight))
{
thisToThatMap[thisQuad].Add(j);
}
else if (settings.CanDropTo(aHeight, bHeight))
{
thisToThatMap[thisQuad].Add(j);
}
if (settings.CanClimbTo(bHeight, aHeight))
{
thatToThisMap[otherQuad].Add(i);
}
else if (settings.CanDropTo(bHeight, aHeight))
{
thatToThisMap[otherQuad].Add(i);
}
}
}
}
connections = thisToThatMap.ToDictionary(x => x.Key, x => x.Value.ToArray());
adjacent.connections = thatToThisMap.ToDictionary(x => x.Key, x => x.Value.ToArray());
// link this boundries islands to that boundries islands
foreach (var quad in this.quads)
{
var thisIsland = boundryQuadsToIslands[quad];
// ensure map contains island
if (!thisToThatIslands.ContainsKey(thisIsland))
{
thisToThatIslands.Add(thisIsland, new List <NavigationIsland>());
}
// add link in map
var thisList = thisToThatIslands[thisIsland];
foreach (var other in GetConnectedQuads(quad))
{
// get islands
var otherIsland = adjacent.boundryQuadsToIslands[other];
if (!thisList.Contains(otherIsland))
{
thisList.Add(otherIsland);
}
}
}
// link that boundries islands to this boundries islands
foreach (var other in adjacent.quads)
{
var otherIsland = adjacent.boundryQuadsToIslands[other];
// ensure map contains island
if (!thatToThisIslands.ContainsKey(otherIsland))
{
thatToThisIslands.Add(otherIsland, new List <NavigationIsland>());
}
var otherList = thatToThisIslands[otherIsland];
foreach (var quad in adjacent.GetConnectedQuads(other))
{
// get islands
var thisIsland = boundryQuadsToIslands[quad];
// add link in map
if (!otherList.Contains(thisIsland))
{
otherList.Add(thisIsland);
}
}
}
this.islandsToIslands = thisToThatIslands;
adjacent.islandsToIslands = thatToThisIslands;
}
}
}
public IEnumerable <NavQuad> GetConnectedQuads(NavQuad quad)
{
if (connections != null)
{
if (connections.TryGetValue(quad, out int[] adjacentIndicies))
void GenerateNavQuads()
{
int quadsPerTileSqrt = Mathf.RoundToInt(Mathf.Sqrt(NavQuad.quadsPerTile));
navQuads = new NavQuad[quadsPerTileSqrt * mapWidth, quadsPerTileSqrt *mapLength];
//make NavQuads
foreach (Tile tile in map)
{
BoxCollider boxCol = tile.boxCol;
for (int i = 0; i < quadsPerTileSqrt; i++)
{
for (int j = 0; j < quadsPerTileSqrt; j++)
{
Vector3 pos = new Vector3(
boxCol.bounds.min.x + (boxCol.bounds.size.x * ((float)i / quadsPerTileSqrt)),
transform.position.y,
boxCol.bounds.min.z + (boxCol.bounds.size.z * ((float)j / quadsPerTileSqrt)));
NavQuad navQuad = new NavQuad(pos);
int x = tile.coord.x * quadsPerTileSqrt + i;
int y = tile.coord.y * quadsPerTileSqrt + j;
navQuads[x, y] = (navQuad);
tile.navQuads.Add(navQuad);
}
}
}
//find all neighbors of each NavQuads
for (int i = 0; i < quadsPerTileSqrt * mapWidth; i++)
{
for (int j = 0; j < quadsPerTileSqrt * mapLength; j++)
{
// Debug.Log(i+", "+j+" "+navQuads[i, j]);
navQuads[i, j].neighbors = new List <NavQuad>();
if (i > 0)
{
navQuads[i, j].neighbors.Add(navQuads[i - 1, j]);
}
if (i < quadsPerTileSqrt * mapWidth - 1)
{
navQuads[i, j].neighbors.Add(navQuads[i + 1, j]);
}
if (j > 0)
{
navQuads[i, j].neighbors.Add(navQuads[i, j - 1]);
}
if (j < quadsPerTileSqrt * mapLength - 1)
{
navQuads[i, j].neighbors.Add(navQuads[i, j + 1]);
}
/* diagonals */
if (i > 0 && j > 0)
{
navQuads[i, j].neighbors.Add(navQuads[i - 1, j - 1]);
}
if (i > 0 && j < quadsPerTileSqrt * mapLength - 1)
{
navQuads[i, j].neighbors.Add(navQuads[i - 1, j + 1]);
}
if (j > 0 && i < quadsPerTileSqrt * mapWidth - 1)
{
navQuads[i, j].neighbors.Add(navQuads[i + 1, j - 1]);
}
if (i < quadsPerTileSqrt * mapWidth - 1 && j < quadsPerTileSqrt * mapLength - 1)
{
navQuads[i, j].neighbors.Add(navQuads[i + 1, j + 1]);
}
}
}
}
public static void CreatePair(NavQuad quad1, NavQuad quad2, Position2 point0, Position2 point1)
{
quad1.Add(new NavLink(quad1, quad2, point0, point1));
quad2.Add(new NavLink(quad2, quad1, point0, point1));
}
public void Generate()
{
var navMesh = new NavMesh(this.game);
var intersectionMap = new Dictionary<Intersection, MergedIntersection>();
var intersectionSideMap = new Dictionary<MergedIntersection, Game.Street[]>();
var intersectionQuadMap = new Dictionary<MergedIntersection, NavQuad>();
foreach (var mergedIntersection in this.game.GetList<MergedIntersection>())
{
var streets = new Game.Street[4];
foreach (var street in mergedIntersection.Streets)
{
var thisNode = mergedIntersection
.Intersections.First(i => i == street.Node1 || i == street.Node2);
var otherNode = street.OtherNode(thisNode);
var diff = otherNode.Position - thisNode.Position;
if (diff.X == 0.U())
{
streets[diff.Y > 0.U() ? Block.Side.North : Block.Side.South] = street;
}
else if (diff.Y == 0.U())
{
streets[diff.X > 0.U() ? Block.Side.East : Block.Side.West] = street;
}
else
{
throw new Exception("");
}
}
var padding = 0.3.U();
var cornerSW = getIntersectionCorner(streets, Block.Corner.SouthWest);
var cornerSE = getIntersectionCorner(streets, Block.Corner.SouthEast);
var cornerNW = getIntersectionCorner(streets, Block.Corner.NorthWest);
var cornerNE = getIntersectionCorner(streets, Block.Corner.NorthEast);
var navQuad = new NavQuad(
cornerSW + new Difference2(padding, padding),
cornerSE + new Difference2(-padding, padding),
cornerNW + new Difference2(padding, -padding),
cornerNE + new Difference2(-padding, -padding)
);
navMesh.Add(navQuad);
intersectionSideMap.Add(mergedIntersection, streets);
intersectionQuadMap.Add(mergedIntersection, navQuad);
foreach (var intersection in mergedIntersection.Intersections)
{
intersectionMap.Add(intersection, mergedIntersection);
}
}
foreach (var mergedIntersection in this.game.GetList<MergedIntersection>())
{
var intersectionQuad = intersectionQuadMap[mergedIntersection];
foreach (var street in mergedIntersection.Streets
.Where(s => mergedIntersection.Intersections.Contains(s.Node1)))
{
var side = (Block.Side)Array.IndexOf(intersectionSideMap[mergedIntersection], street);
var quad1 = intersectionQuad;
var quad2 = intersectionQuadMap[intersectionMap[street.Node2]];
if (side == Block.Side.East || side == Block.Side.West)
{
if (side == Block.Side.East)
{
Do.Swap(ref quad1, ref quad2);
}
var navQuad = new NavQuad(quad2.SE, quad1.SW, quad2.NE, quad1.NW);
NavLink.CreatePair(navQuad, quad1, quad1.SW, quad1.NW);
NavLink.CreatePair(navQuad, quad2, quad2.SE, quad2.NE);
navMesh.Add(navQuad);
}
else if (side == Block.Side.South || side == Block.Side.North)
{
if (side == Block.Side.South)
{
Do.Swap(ref quad1, ref quad2);
}
var navQuad = new NavQuad(quad1.NW, quad1.NE, quad2.SW, quad2.SE);
NavLink.CreatePair(navQuad, quad1, quad1.NW, quad1.NE);
NavLink.CreatePair(navQuad, quad2, quad2.SW, quad2.SE);
navMesh.Add(navQuad);
}
}
}
}
public static float Distance(NavQuad a, NavQuad b)
{
return(a.Distance(b));
}
public float Distance(NavQuad other)
{
return(Vector3.Distance(position, other.position));
}
public static float Heuristic(NavQuad a, NavQuad b)
{
return(NavQuad.Distance(a, b));
}
