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 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 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>(); }