IEnumerator GetPathCoro(Coordinate start, Coordinate end) { List <Coordinate> visited = new List <Coordinate>(); if (Target) { Priority_Queue.SimplePriorityQueue <Coordinate> queue = new Priority_Queue.SimplePriorityQueue <Coordinate>(); if (start == null) { start = prev_start_coord; } if (start != null) { start.parent = null; } if (end == null) { end = prev_end_coord; } if (start == null || end == null) { yield break; } float tstart = Time.realtimeSinceStartup; prev_start_coord = start; prev_end_coord = end; queue.Enqueue(start, GetCoordinateDistFromTarget(start)); while (Path == null) { start = queue.Dequeue(); if (start == end) { /* Debug.Log(Time.realtimeSinceStartup - tstart + " seconds : " + * queue.Count + " end routes considered : " + * start.GetNumParents() + " parents."); */ Path = start; yield break; } if (Time.realtimeSinceStartup > tstart + .01f || index != go_index) { yield return(new WaitForEndOfFrame()); tstart = Time.realtimeSinceStartup; } int safe_layer = ptr.gameObject.layer; foreach (Coordinate coord in start.GetChildren()) { coord.traverse_cost = GetCoordinateDistFromTarget(coord); if (!visited.Contains(coord)) { coord.parent = start; queue.Enqueue(coord, coord.GetTotalCost(safe_layer, can_dodge)); visited.Add(coord); } else if (queue.Contains(coord) && coord.GetTotalCost(safe_layer, can_dodge, 0, start) < queue.GetPriority(coord)) { coord.parent = start; queue.UpdatePriority(coord, coord.GetTotalCost(safe_layer, can_dodge)); } } if (queue.Count != 0) { start = queue.First; } else { break; } } } yield return(null); }
List <Vector3> FindPathMonteCarlo() { /* * Please configure the random generation of configurations before calling this function (or you can use FindPath instead). * ALGO: * We draw a point: * - If it connects two connected components together, we keep it * - If it is not reachable from any previous configuration, we keep it * - Otherwise we ignore it * - We can also accept a point that do not respect any of the conditions after a certain number of consecutive rejections * We update the connected components (union find) and eventually the dico for memoisation * We stop when the initial config and the final one are in the same connected component * We retrieve the configurations of this connected component and we search a path with it (we can use a dico for memoisation) */ Dictionary <Link, float> dico = new Dictionary <Link, float>(); UnionFind <Vector3> components = new UnionFind <Vector3>(); List <Vector3> pts = new List <Vector3>(); pts.Add(ConfigInfos.initialConf); components.MakeSet(pts[0]); pts.Add(ConfigInfos.finalConf); components.MakeSet(pts[1]); if (phy.moveAllowed(false, pts[0], pts[1])) { dico.Add(new Link(pts[0], pts[1]), distanceBetweenConf(pts[0], pts[1])); Debug.DrawLine(CarController.spatialCoordOfConfiguration(pts[0]), CarController.spatialCoordOfConfiguration(pts[1]), Color.red, 5f); components.UnionValues(pts[0], pts[1]); } int i = 0; int cons_rejections = 0; Dictionary <Vector3, bool> tmp_dico = new Dictionary <Vector3, bool>(); while (components.Find(pts[0]).value != components.Find(pts[1]).value || i < minPointsMonteCarlo) { if (i >= maxPointsMonteCarlo) { return(null); } Vector3 pt = DrawConfiguration(); if (!phy.configurationInCollision(pt)) { bool reachable = false; HashSet <Vector3> components_linked = new HashSet <Vector3>(); foreach (Vector3 v in pts) { if (components_linked.Contains(components.Find(v).value)) { continue; } bool linked = phy.moveAllowed(false, v, pt); tmp_dico[v] = linked; if (linked) { reachable = true; components_linked.Add(components.Find(v).value); } } if (!reachable || components_linked.Count >= 2 || cons_rejections >= maxConsecutiveRejections) { cons_rejections = 0; foreach (Vector3 v in pts) { bool linked = false; try { linked = tmp_dico[v]; } catch { linked = phy.moveAllowed(false, v, pt); } if (linked) { Debug.DrawLine(CarController.spatialCoordOfConfiguration(v), CarController.spatialCoordOfConfiguration(pt), Color.red, 5f); dico[new Link(v, pt)] = distanceBetweenConf(v, pt); } } pts.Add(pt); components.MakeSet(pt); foreach (Vector3 v in components_linked) { components.UnionValues(pt, v); } } else { cons_rejections++; } tmp_dico.Clear(); i++; } } // Searching a path with Dijkstra in the connected component of the initial config... Vector3 component = components.Find(ConfigInfos.initialConf).value; pts.RemoveAll((v => components.Find(v).value != component)); Priority_Queue.SimplePriorityQueue <Vector3> vertices = new Priority_Queue.SimplePriorityQueue <Vector3>(); foreach (Vector3 pt in pts) { if (pt == ConfigInfos.initialConf) { vertices.Enqueue(pt, 0); } else { vertices.Enqueue(pt, Mathf.Infinity); } } Dictionary <Vector3, Vector3> paths = new Dictionary <Vector3, Vector3>(); float current_w = vertices.GetPriority(vertices.First); Vector3 current = vertices.Dequeue(); while (current != ConfigInfos.finalConf) { foreach (Vector3 v in vertices) { float w = GetMemoisedValue(dico, v, current); float new_w = w + current_w; if (new_w < vertices.GetPriority(v)) { vertices.UpdatePriority(v, new_w); paths[v] = current; } } current_w = vertices.GetPriority(vertices.First); current = vertices.Dequeue(); } List <Vector3> result = new List <Vector3>(); result.Add(ConfigInfos.finalConf); current = ConfigInfos.finalConf; while (current != ConfigInfos.initialConf) { current = paths[current]; result.Add(current); } result.Reverse(); return(result); }