public void addNeighbor(INavCell other)
{
if (!neighbors.Contains(other))
{
neighbors.Add(other);
}
}
override public void setProperties(INavCell cell)
{
if (gameObject.GetComponent <BoxCollider>().bounds.Contains(cell.center))
{
cell.properties.Add("OnFire", true);
Debug.Log(cell.properties.ContainsKey("OnFire"));
}
}
public bool isAdjacent(INavCell other)
{
if (!(other is Triangle))
{
return(false);
}
return(this.isAdjacent((Triangle)other));
}
private float NotOnFireCost(INavCell source, INavCell target)
{
float distance = Vector3.Distance(source.center, target.center);
if (target.properties.ContainsKey("OnFire"))
{
//Debug.Log("burning");
return(30000.0f + distance);
}
else
{
return(distance);
}
}
private float OnFireCost(INavCell source, INavCell target)
{
float distance = Vector3.Distance(source.center, target.center);
return(distance);
}
public static List <Vector3> generatePath(GameObject source,
GameObject target,
CostFunction computeCost = null,
HeuristicFunction computeHeuristic = null)
{
if (computeCost == null)
{
computeCost = euclideanCost;
}
if (computeHeuristic == null)
{
computeHeuristic = euclideanHeuristic;
}
string costFunctionID = computeCost.Method.ToString();
if (levelNavMesh == null)
{
Debug.Log("Could not find a CustomNavMesh");
List <Vector3> l = new List <Vector3>();
l.Add(target.transform.position);
return(l);
}
//Debug.Log("Checking cache...");
if (lastLocationCache.ContainsKey(target))
{
if (Vector3.Distance(target.transform.position, lastLocationCache[target]) > REPATH_THRESHOLD)
{
visitedDistancesCache.Remove(target);
}
}
Dictionary <INavCell, float> visitedDistances;
if (visitedDistancesCache.ContainsKey(target))
{
var funcToDistances = visitedDistancesCache[target];
if (funcToDistances.ContainsKey(costFunctionID))
{
visitedDistances = funcToDistances[costFunctionID];
}
else
{
//Debug.Log("Cannot find cost function");
visitedDistances = new Dictionary <INavCell, float>();
}
}
else
{
visitedDistances = new Dictionary <INavCell, float>();
}
//Debug.Log("Generate path starting...");
// Backwards A*
INavCell targetCell = levelNavMesh.getClosestNavCell(target.transform.position);
Vector3 startPosition = source.transform.position;
List <INavCell> startCells = levelNavMesh.getClosestNavCell(startPosition).neighbors;
if (!startCells.Exists(cell => visitedDistances.ContainsKey(cell)))
{
//DateTime startTime = DateTime.UtcNow;
PriorityQueue <INavCell> pQueue = new PriorityQueue <INavCell>();
if (visitedDistances.Count == 0)
{
pQueue.Add(0.0f + computeHeuristic(targetCell.center, startPosition), targetCell);
}
else
{
foreach (var kvp in visitedDistances)
{
INavCell cell = kvp.Key;
float gValue = kvp.Value;
foreach (INavCell neighbor in cell.neighbors)
{
if (visitedDistances.ContainsKey(neighbor))
{
continue;
}
// Edge weight
float weight = computeCost(neighbor, cell);
// A* Heuristic -- Euclidean distance
float heuristic = computeHeuristic(neighbor.center, startPosition);
pQueue.Add(gValue + weight + heuristic, neighbor);
}
}
}
PriorityQueue <INavCell> .Node currentNode = pQueue.RemoveMin();
// Debug
//GameObject aStarVisited = new GameObject("A-Star Visits");
//float counter = 0;
while (!startCells.Contains(currentNode.Value)) //&& counter < 10.0f)
{
INavCell currentNavCell = currentNode.Value;
//Update our visited things
if (!visitedDistances.ContainsKey(currentNavCell))
{
// Hmm, by adding the heuristic to the priority,
// we now have that priority != distance traveled
float gValue = currentNode.Priority - computeHeuristic(currentNavCell.center, startPosition);
visitedDistances.Add(currentNavCell, gValue);
// Debug
//GameObject marker = GameObject.CreatePrimitive(PrimitiveType.Sphere);
//Destroy(marker.GetComponent<Collider>());
//marker.transform.position = currentNavCell.center;
//marker.transform.parent = aStarVisited.transform;
List <INavCell> neighbors = currentNavCell.neighbors;
foreach (INavCell neighbor in neighbors)
{
// Edge weight
float weight = computeCost(neighbor, currentNavCell);
// A* Heuristic -- Euclidean distance
float heuristic = computeHeuristic(neighbor.center, startPosition);
pQueue.Add(gValue + weight + heuristic, neighbor);
}
}
//Now move onto the next thing in the PQ
currentNode = pQueue.RemoveMin();
}
//final addition of the last node
visitedDistances.Add(currentNode.Value, currentNode.Priority - computeHeuristic(currentNode.Value.center, startPosition));
//Debug.Log("A* search done in " + (DateTime.UtcNow - startTime).Milliseconds);
// Caching
if (!visitedDistancesCache.ContainsKey(target))
{
var funcToDist = new Dictionary <string, Dictionary <INavCell, float> >();
funcToDist.Add(costFunctionID, visitedDistances);
visitedDistancesCache.Add(target, funcToDist);
}
else
{
var funcToDist = visitedDistancesCache[target];
if (!funcToDist.ContainsKey(costFunctionID))
{
funcToDist.Add(costFunctionID, visitedDistances);
}
else
{
funcToDist[costFunctionID] = visitedDistances;
}
}
if (!lastLocationCache.ContainsKey(target))
{
lastLocationCache.Add(target, target.transform.position);
}
else
{
lastLocationCache[target] = target.transform.position;
}
}
//if (counter > 10.0f)
// Debug.Log("timed out");
//Debug.Log("A* done");
List <INavCell> cellPath = new List <INavCell>();
INavCell backTracer = startCells.First(cell => visitedDistances.ContainsKey(cell));
float backTracerDistace = visitedDistances[backTracer];
cellPath.Add(backTracer);
// Debug
//GameObject chosenPath = new GameObject("Chosen path");
while (backTracer != targetCell)
{
int count = cellPath.Count;
List <INavCell> neighbors = backTracer.neighbors;
foreach (INavCell neighbor in neighbors)
{
if (visitedDistances.ContainsKey(neighbor))
{
float weight = computeCost(neighbor, backTracer);
if (Mathf.Abs(backTracerDistace - weight - visitedDistances[neighbor]) < 0.05f)
{
//update stuff about the backtracer
backTracer = neighbor;
backTracerDistace = visitedDistances[neighbor];
//GameObject node = new GameObject();
//node.transform.position = neighbor.center;
//polygonPath.Add(node);
cellPath.Add(neighbor);
// Debug
//GameObject marker = GameObject.CreatePrimitive(PrimitiveType.Cylinder);
//Destroy(marker.GetComponent<Collider>());
//marker.transform.position = neighbor.center;
//marker.transform.parent = chosenPath.transform;
continue;
}
}
}
if (count == cellPath.Count)
{
//if we made it here, something failed
//Debug.Log("dying");
break;
}
}
//Debug.Log("Backtracing done");
//GameObject startNode = new GameObject();
//startNode.transform.position = start.center;
cellPath.Add(targetCell);
//cellPath.Reverse();
//Debug.Log("Backtracing got path length " + cellPath.Count);
List <Vector3> resultPath;
Collider c = source.GetComponentInChildren <Collider>();
if (c != null)
{
//Debug.Log("Got collider " + c.name);
resultPath = SmoothPathByShortCutting(cellPath, LayerMask.GetMask("Obstacle", "Ground"), c.bounds.extents.magnitude);
}
else
{
//Debug.Log(source.name + " does not have a collider.");
resultPath = SmoothPathByShortCutting(cellPath, LayerMask.GetMask("Obstacle", "Ground"));
}
//Debug.Log("Smoothing done");
//resultPath = new List<GameObject>(polygonPath.Select(s =>
//{
// GameObject obj = new GameObject();
// obj.transform.position = s.center;
// return obj;
//}));
return(resultPath);
}
private static float euclideanCost(INavCell source, INavCell target)
{
return(Vector3.Distance(source.center, target.center));
}
public abstract void setProperties(INavCell cell);
