private void InitRuntimeInstances()
{
runtimeWaypoints = new List <RuntimeWaypoint>();
runtimeConnections = new List <RuntimeWaypointsConnection>();
var waypointsMapping = new Dictionary <Waypoint, RuntimeWaypoint>();
foreach (var wp in waypoints)
{
var newRuntimeWaypoint = new RuntimeWaypoint(wp.transform.position);
waypointsMapping.Add(wp, newRuntimeWaypoint);
runtimeWaypoints.Add(newRuntimeWaypoint);
}
foreach (var conn in connections)
{
runtimeConnections.Add(
new RuntimeWaypointsConnection(
waypointsMapping[conn.wp01],
waypointsMapping[conn.wp02],
conn.cost,
conn.enabled,
conn.name
)
);
Destroy(conn.gameObject);
}
foreach (var wp in waypoints)
{
Destroy(wp.gameObject);
}
connections = null;
waypoints = null;
}
//public RuntimeWaypoint FindNearestWaypoint(Vector3 position)
//{
// if (runtimeWaypoints == null || runtimeWaypoints.Count == 0)
// {
// return null;
// }
// if (runtimeWaypoints.Count == 1)
// {
// return runtimeWaypoints[0];
// }
// var nearestWaypoint = runtimeWaypoints[0];
// var nearestSqrMagnitude = (nearestWaypoint.position - position).sqrMagnitude;
// for (var i = 1; i < runtimeWaypoints.Count; i++)
// {
// var currWaypoint = runtimeWaypoints[i];
// var currSqrMagnitude = (currWaypoint.position - position).sqrMagnitude;
// if (currSqrMagnitude < nearestSqrMagnitude)
// {
// nearestSqrMagnitude = currSqrMagnitude;
// nearestWaypoint = currWaypoint;
// }
// }
// return nearestWaypoint;
//}
public List <WaypointNeighbor> GetNeighborWaypoints(RuntimeWaypoint waypoint)
{
if (neighborsIndex.ContainsKey(waypoint))
{
return(neighborsIndex[waypoint]);
}
return(new List <WaypointNeighbor>());
}
public PathFindingResult(
bool success,
RuntimeWaypoint[] path = null,
Dictionary<RuntimeWaypoint, RuntimeWaypoint> cameFrom = null,
string msg = null
)
{
this.success = success;
this.path = path;
this.cameFrom = cameFrom;
this.msg = msg;
}
public RuntimeWaypointsConnection(
RuntimeWaypoint waypoint1,
RuntimeWaypoint waypoint2,
float cost,
bool enabled,
string name)
{
this.Waypoint1 = waypoint1;
this.Waypoint2 = waypoint2;
this.cost = cost;
this.enabled = enabled;
this.name = name;
}
public RuntimeWaypointsConnection(
RuntimeWaypoint waypoint1,
RuntimeWaypoint waypoint2,
float cost,
bool enabled,
string name)
{
this.Waypoint1 = waypoint1;
this.Waypoint2 = waypoint2;
this.cost = cost;
this.enabled = enabled;
this.name = name;
}
public void Enqueue(RuntimeWaypoint wp, float priority)
{
var newNode = new Node(wp, priority);
if (topNode == null)
{
topNode = newNode;
}
else
{
Node previousNode = null;
Node currentNode = topNode;
var currentNodeIsTop = true;
var inserted = false;
while (currentNode != null)
{
if (currentNode.priority > priority)
{
newNode.next = currentNode;
if (currentNodeIsTop)
{
topNode = newNode;
}
else
{
previousNode.next = newNode;
}
inserted = true;
break;
}
previousNode = currentNode;
currentNode = currentNode.next;
currentNodeIsTop = false;
}
if (!inserted)
{
previousNode.next = newNode;
}
}
}
public void Enqueue(RuntimeWaypoint wp, float priority)
{
var newNode = new Node(wp, priority);
if (topNode == null)
{
topNode = newNode;
}
else
{
Node previousNode = null;
Node currentNode = topNode;
var currentNodeIsTop = true;
var inserted = false;
while (currentNode != null)
{
if (currentNode.priority > priority)
{
newNode.next = currentNode;
if (currentNodeIsTop)
{
topNode = newNode;
}
else
{
previousNode.next = newNode;
}
inserted = true;
break;
}
previousNode = currentNode;
currentNode = currentNode.next;
currentNodeIsTop = false;
}
if (!inserted)
{
previousNode.next = newNode;
}
}
}
protected override void OnDrawGizmos()
{
base.OnDrawGizmos();
if (debugProcessing && currentPath != null && currentPath.cameFrom != null && currentPath.success)
{
Gizmos.color = debugProcessingColor;
foreach (var item in currentPath.cameFrom)
{
Gizmos.DrawWireSphere(item.Key.position, .1f);
if (item.Value != null)
{
Gizmos.DrawWireSphere(item.Value.position, .1f);
Gizmos.DrawLine(item.Key.position, item.Value.position);
}
}
}
if (debugPath && currentPath != null && currentPath.success)
{
Gizmos.color = debugPathColor;
Waypoint lastWp = null;
foreach (var wp in currentPath.path)
{
Gizmos.DrawWireSphere(wp.position, .1f);
if (lastWp != null)
{
Gizmos.DrawLine(wp.position, lastWp.position);
}
lastWp = wp;
}
}
}
public Node(RuntimeWaypoint wp, float priority)
{
this.priority = priority;
this.waypoint = wp;
}
public Node(RuntimeWaypoint wp, float priority)
{
this.priority = priority;
this.waypoint = wp;
}
public PathFindingResult FindPath(RuntimeWaypoint origin, RuntimeWaypoint destination)
{
var frontier = new PriorityQueue();
frontier.Enqueue(destination, 0);
var cameFrom = new Dictionary <RuntimeWaypoint, RuntimeWaypoint>();
cameFrom.Add(destination, null);
var costSoFar = new Dictionary <RuntimeWaypoint, float>();
costSoFar.Add(destination, 0);
var frontierLoopCount = 0;
while (frontier.IsNotEmpty)
{
if (++frontierLoopCount > 1000)
{
return(new PathFindingResult(false, msg: "Loops overflow."));
}
var current = frontier.Dequeue();
if (current.Equals(origin))
{
break;
}
foreach (var neighbor in GetNeighborWaypoints(current))
{
if (!neighbor.connection.enabled)
{
continue;
}
var newCost = costSoFar[current] + neighbor.connection.cost;
if (!costSoFar.ContainsKey(neighbor.waypoint) || newCost < costSoFar[neighbor.waypoint])
{
if (costSoFar.ContainsKey(neighbor.waypoint))
{
costSoFar[neighbor.waypoint] = newCost;
}
else
{
costSoFar.Add(neighbor.waypoint, newCost);
}
var priority =
Math.Abs(origin.position.x - neighbor.waypoint.position.x) +
Math.Abs(origin.position.y - neighbor.waypoint.position.y) +
Math.Abs(origin.position.z - neighbor.waypoint.position.z) +
newCost;
frontier.Enqueue(neighbor.waypoint, priority);
if (cameFrom.ContainsKey(neighbor.waypoint))
{
cameFrom[neighbor.waypoint] = current;
}
else
{
cameFrom.Add(neighbor.waypoint, current);
}
}
}
}
var currentStep = origin;
var path = new List <RuntimeWaypoint>();
path.Add(currentStep);
var loopCount = 0;
while (!currentStep.Equals(destination))
{
if (++loopCount > runtimeConnections.Count)
{
return(new PathFindingResult(false, msg: "More loops than connections. Path finding aborted."));
}
if (!cameFrom.ContainsKey(currentStep))
{
return(new PathFindingResult(false, msg: "Impossible path."));
}
currentStep = cameFrom[currentStep];
path.Add(currentStep);
}
return(new PathFindingResult(true, path.ToArray(), cameFrom));
}
public WaypointNeighbor(RuntimeWaypoint waypoint, RuntimeWaypointsConnection connection)
{
this.waypoint = waypoint;
this.connection = connection;
}
private void InitRuntimeInstances()
{
runtimeWaypoints = new List<RuntimeWaypoint>();
runtimeConnections = new List<RuntimeWaypointsConnection>();
var waypointsMapping = new Dictionary<Waypoint, RuntimeWaypoint>();
foreach (var wp in waypoints)
{
var newRuntimeWaypoint = new RuntimeWaypoint(wp.transform.position);
waypointsMapping.Add(wp, newRuntimeWaypoint);
runtimeWaypoints.Add(newRuntimeWaypoint);
}
foreach (var conn in connections)
{
runtimeConnections.Add(
new RuntimeWaypointsConnection(
waypointsMapping[conn.wp01],
waypointsMapping[conn.wp02],
conn.cost,
conn.enabled,
conn.name
)
);
Destroy(conn.gameObject);
}
foreach (var wp in waypoints)
{
Destroy(wp.gameObject);
}
connections = null;
waypoints = null;
}
public PathFindingResult FindPath(RuntimeWaypoint origin, RuntimeWaypoint destination)
{
var frontier = new PriorityQueue();
frontier.Enqueue(destination, 0);
var cameFrom = new Dictionary<RuntimeWaypoint, RuntimeWaypoint>();
cameFrom.Add(destination, null);
var costSoFar = new Dictionary<RuntimeWaypoint, float>();
costSoFar.Add(destination, 0);
var frontierLoopCount = 0;
while (frontier.IsNotEmpty)
{
if (++frontierLoopCount > 1000)
{
return new PathFindingResult(false, msg: "Loops overflow.");
}
var current = frontier.Dequeue();
if (current.Equals(origin))
{
break;
}
foreach (var neighbor in GetNeighborWaypoints(current))
{
if (!neighbor.connection.enabled) continue;
var newCost = costSoFar[current] + neighbor.connection.cost;
if (!costSoFar.ContainsKey(neighbor.waypoint) || newCost < costSoFar[neighbor.waypoint])
{
if (costSoFar.ContainsKey(neighbor.waypoint))
{
costSoFar[neighbor.waypoint] = newCost;
}
else
{
costSoFar.Add(neighbor.waypoint, newCost);
}
var priority =
Math.Abs(origin.position.x - neighbor.waypoint.position.x) +
Math.Abs(origin.position.y - neighbor.waypoint.position.y) +
Math.Abs(origin.position.z - neighbor.waypoint.position.z) +
newCost;
frontier.Enqueue(neighbor.waypoint, priority);
if (cameFrom.ContainsKey(neighbor.waypoint))
{
cameFrom[neighbor.waypoint] = current;
}
else
{
cameFrom.Add(neighbor.waypoint, current);
}
}
}
}
var currentStep = origin;
var path = new List<RuntimeWaypoint>();
path.Add(currentStep);
var loopCount = 0;
while (!currentStep.Equals(destination))
{
if (++loopCount > runtimeConnections.Count)
{
return new PathFindingResult(false, msg: "More loops than connections. Path finding aborted.");
}
if (!cameFrom.ContainsKey(currentStep))
{
return new PathFindingResult(false, msg: "Impossible path.");
}
currentStep = cameFrom[currentStep];
path.Add(currentStep);
}
return new PathFindingResult(true, path.ToArray(), cameFrom);
}
//public RuntimeWaypoint FindNearestWaypoint(Vector3 position)
//{
// if (runtimeWaypoints == null || runtimeWaypoints.Count == 0)
// {
// return null;
// }
// if (runtimeWaypoints.Count == 1)
// {
// return runtimeWaypoints[0];
// }
// var nearestWaypoint = runtimeWaypoints[0];
// var nearestSqrMagnitude = (nearestWaypoint.position - position).sqrMagnitude;
// for (var i = 1; i < runtimeWaypoints.Count; i++)
// {
// var currWaypoint = runtimeWaypoints[i];
// var currSqrMagnitude = (currWaypoint.position - position).sqrMagnitude;
// if (currSqrMagnitude < nearestSqrMagnitude)
// {
// nearestSqrMagnitude = currSqrMagnitude;
// nearestWaypoint = currWaypoint;
// }
// }
// return nearestWaypoint;
//}
public List<WaypointNeighbor> GetNeighborWaypoints(RuntimeWaypoint waypoint)
{
if (neighborsIndex.ContainsKey(waypoint))
{
return neighborsIndex[waypoint];
}
return new List<WaypointNeighbor>();
}
public WaypointNeighbor(RuntimeWaypoint waypoint, RuntimeWaypointsConnection connection)
{
this.waypoint = waypoint;
this.connection = connection;
}
