float CalculateEdgeCost(PseudoGridCell src, PseudoGridCell tgt)
{
switch(edgeCostAlgorithm) {
case EdgeCostAlgorithm.EUCLIDEAN_DISTANCE:
return Vector3.Distance(tgt.transform.position,
src.transform.position);
case EdgeCostAlgorithm.ONE_PLUS_POS_SLOPE:
return 1.0f + Mathf.Max(0f,
Vector3.Dot((tgt.transform.position - src.transform.position),
transform.up));
case EdgeCostAlgorithm.ONE:
break;
}
return 1.0f;
}
public override NavMeshNode Quantize(Vector3 pos)
{
// FIXME this is slow
float minDist = float.PositiveInfinity;
PseudoGridCell nearest = null;
foreach (PseudoGridCell cell in cells)
{
float dist = (pos - cell.transform.position).sqrMagnitude;
if (dist < minDist)
{
nearest = cell;
minDist = dist;
}
}
return(nearest);
}
float CalculateEdgeCost(PseudoGridCell src, PseudoGridCell tgt)
{
switch (edgeCostAlgorithm)
{
case EdgeCostAlgorithm.EUCLIDEAN_DISTANCE:
return(Vector3.Distance(tgt.transform.position,
src.transform.position));
case EdgeCostAlgorithm.ONE_PLUS_POS_SLOPE:
return(1.0f + Mathf.Max(0f,
Vector3.Dot((tgt.transform.position - src.transform.position),
transform.up)));
case EdgeCostAlgorithm.ONE:
break;
}
return(1.0f);
}
public void FindCells()
{
List <GameObject> del = new List <GameObject>();
foreach (Transform child in transform)
{
del.Add(child.gameObject);
}
foreach (GameObject obj in del)
{
DestroyImmediate(obj);
}
Sample sampler = null;
switch (samplingType)
{
case SamplerType.CENTER: sampler = SampleCenter; break;
case SamplerType.CORNERS_AVG: sampler = SampleCornersAvg; break;
case SamplerType.CORNERS_MIN: sampler = SampleCornersMin; break;
case SamplerType.CORNERS_MAX: sampler = SampleCornersMax; break;
case SamplerType.MONTE_CARLO: sampler = SampleMonteCarlo; break;
}
float xSize = transform.localScale.x / xRes;
float ySize = transform.localScale.z / yRes;
float thickness = transform.localScale.y * 0.1f;
float xOff = xSize * 0.5f;
float yOff = ySize * 0.5f;
Vector3 down = transform.up * -transform.localScale.y;
Vector3 top = Vector3.up * transform.localScale.y * 0.5f;
Vector3 scale = new Vector3(xSize, thickness, ySize);
Vector3 start = new Vector3(-xOff * xRes + xOff, 0.0f, -yOff * yRes + yOff);
float localMaxHeight = maxHeight / transform.localScale.y;
PseudoGridCell[,] cells = new PseudoGridCell[yRes, xRes];
// Node creation pass
for (int y = 0; y < yRes; y++)
{
for (int x = 0; x < xRes; x++)
{
Vector3 rayPos = transform.position + transform.rotation *
(new Vector3(xSize * x, 0f, ySize * y) + start + top);
Vector3 pos = Vector3.zero;
if (!sampler(rayPos, down, out pos) && noHitNoCell)
{
continue;
}
Vector3 localPos = transform.InverseTransformPoint(pos);
if (localPos.y + 0.5f > localMaxHeight)
{
continue;
}
GameObject cell = new GameObject("Cell (" + x + "," + y + ")");
cell.transform.position = pos;
cell.transform.rotation = transform.rotation;
cell.transform.parent = transform;
cell.transform.localScale = scale;
cell.gameObject.layer = gameObject.layer;
BoxCollider box = cell.AddComponent <BoxCollider>();
box.isTrigger = true;
Vector3 s = transform.localScale;
box.size = new Vector3(1.0f / s.x, 1.0f / s.y, 1.0f / s.z);
cells[y, x] = cell.AddComponent <PseudoGridCell>();
}
}
// Edge creation pass
List <GameObjectEdge> edges = new List <GameObjectEdge>();
for (int y = 0; y < yRes; y++)
{
for (int x = 0; x < xRes; x++)
{
edges.Clear();
PseudoGridCell src = cells[y, x];
if (src == null)
{
continue;
}
for (int dy = -1; dy <= 1; dy++)
{
for (int dx = -1; dx <= 1; dx++)
{
if (gridType == GridType.FOUR_GRID &&
Mathf.Abs(dx) + Mathf.Abs(dy) >= 2)
{
continue;
}
int x2 = x + dx, y2 = y + dy;
// Skip the center
if ((dx == 0 && dy == 0) || x2 < 0 || x2 >= cells.GetLength(1) ||
y2 < 0 || y2 >= cells.GetLength(0))
{
// TODO add 4-/8- criteria
continue;
}
PseudoGridCell dest = cells[y2, x2];
if (dest == null)
{
continue;
}
Vector3 diff = dest.transform.position - src.transform.position;
float slope = Mathf.Abs(Vector3.Dot(diff, transform.up));
if (slope < maxSlope)
{
edges.Add(new GameObjectEdge(dest,
CalculateEdgeCost(src, dest)));
}
}
}
src.edges = edges.ToArray();
}
}
if (removeOrphans)
{
for (int y = 0; y < yRes; y++)
{
for (int x = 0; x < xRes; x++)
{
if (cells[y, x] == null)
{
continue;
}
if (cells[y, x].edges.Length == 0)
{
DestroyImmediate(cells[y, x]);
}
}
}
}
}
public void FindCells()
{
List<GameObject> del = new List<GameObject>();
foreach(Transform child in transform) {
del.Add(child.gameObject);
}
foreach(GameObject obj in del) {
DestroyImmediate(obj);
}
Sample sampler = null;
switch(samplingType) {
case SamplerType.CENTER: sampler = SampleCenter; break;
case SamplerType.CORNERS_AVG: sampler = SampleCornersAvg; break;
case SamplerType.CORNERS_MIN: sampler = SampleCornersMin; break;
case SamplerType.CORNERS_MAX: sampler = SampleCornersMax; break;
case SamplerType.MONTE_CARLO: sampler = SampleMonteCarlo; break;
}
float xSize = transform.localScale.x / xRes;
float ySize = transform.localScale.z / yRes;
float thickness = transform.localScale.y * 0.1f;
float xOff = xSize * 0.5f;
float yOff = ySize * 0.5f;
Vector3 down = transform.up * -transform.localScale.y;
Vector3 top = Vector3.up * transform.localScale.y * 0.5f;
Vector3 scale = new Vector3(xSize, thickness, ySize);
Vector3 start = new Vector3(-xOff * xRes + xOff, 0.0f, -yOff * yRes + yOff);
float localMaxHeight = maxHeight / transform.localScale.y;
PseudoGridCell[,] cells = new PseudoGridCell[yRes, xRes];
// Node creation pass
for(int y = 0; y < yRes ; y ++) {
for(int x = 0; x < xRes ; x ++) {
Vector3 rayPos = transform.position + transform.rotation *
(new Vector3(xSize * x, 0f, ySize * y) + start + top);
Vector3 pos = Vector3.zero;
if(!sampler(rayPos, down, out pos) && noHitNoCell) {
continue;
}
Vector3 localPos = transform.InverseTransformPoint(pos);
if(localPos.y + 0.5f > localMaxHeight) {
continue;
}
GameObject cell = new GameObject("Cell (" + x + "," + y + ")");
cell.transform.position = pos;
cell.transform.rotation = transform.rotation;
cell.transform.parent = transform;
cell.transform.localScale = scale;
cell.gameObject.layer = gameObject.layer;
BoxCollider box = cell.AddComponent<BoxCollider>();
box.isTrigger = true;
Vector3 s = transform.localScale;
box.size = new Vector3(1.0f/s.x, 1.0f/s.y, 1.0f/s.z);
cells[y, x] = cell.AddComponent<PseudoGridCell>();
}
}
// Edge creation pass
List<GameObjectEdge> edges = new List<GameObjectEdge>();
for(int y = 0; y < yRes ; y ++) {
for(int x = 0; x < xRes ; x ++) {
edges.Clear();
PseudoGridCell src = cells[y, x];
if(src == null) {
continue;
}
for(int dy = -1; dy <= 1; dy ++) {
for(int dx = -1; dx <= 1; dx ++) {
if(gridType == GridType.FOUR_GRID
&& Mathf.Abs(dx) + Mathf.Abs(dy) >= 2)
{
continue;
}
int x2 = x + dx, y2 = y + dy;
// Skip the center
if((dx == 0 && dy == 0) || x2 < 0 || x2 >= cells.GetLength(1)
|| y2 < 0 || y2 >= cells.GetLength(0))
{
// TODO add 4-/8- criteria
continue;
}
PseudoGridCell dest = cells[y2, x2];
if(dest == null) {
continue;
}
Vector3 diff = dest.transform.position - src.transform.position;
float slope = Mathf.Abs(Vector3.Dot(diff, transform.up));
if(slope < maxSlope) {
edges.Add(new GameObjectEdge(dest,
CalculateEdgeCost(src, dest)));
}
}
}
src.edges = edges.ToArray();
}
}
if(removeOrphans) {
for(int y = 0; y < yRes ; y ++) {
for(int x = 0; x < xRes ; x ++) {
if(cells[y, x] == null) {
continue;
}
if(cells[y, x].edges.Length == 0) {
DestroyImmediate(cells[y, x]);
}
}
}
}
}
