private Vector2[] findSmoothPath(Vector2 from, Vector2 to) { Func <Vector2, NevPoint> getClosestPoint = (pos) => { var index = indexTileMap.GetClosestTile(pos, -1); Debug.Assert(index != -1); return(points[index]); }; Debug.Assert(edge.Contains(from) && edge.Contains(to)); NevPoint fromPoint = getClosestPoint(from); NevPoint toPoint = getClosestPoint(to); NevPoint[] aStarResult = aStarSearch(fromPoint, toPoint); // smooth path int smoothPathSize = aStarResult.Length + 4; Vector2[] smoothPath = new Vector2[smoothPathSize]; int i = 0; Action <Vector2> addOne = (p) => smoothPath[i++] = p; Action <IEnumerable <NevPoint> > addMultiNevPoints = (ps) => { foreach (var p in ps) { addOne(p.pos); } }; addOne(from); addOne(fromPoint.pos); addMultiNevPoints(aStarResult); addOne(toPoint.pos); addOne(to); return(highSmooth(smoothPath)); }
private NevPoint[] aStarSearch(NevPoint from, NevPoint to) { SimplePriorityQueue <NevPoint> searchQueue = new SimplePriorityQueue <NevPoint>(); Dictionary <NevPoint, NevPoint> last = new Dictionary <NevPoint, NevPoint>(); Dictionary <NevPoint, float> dis = new Dictionary <NevPoint, float>(); Action <NevPoint, NevPoint, float, float> insertPoint = (point, lastPoint, _dis, priority) => { searchQueue.Enqueue(point, priority); last.Add(point, lastPoint); dis.Add(point, _dis); }; insertPoint(from, from, 0, 0); while (searchQueue.Count != 0) { NevPoint point = searchQueue.Dequeue(); if (point == to) { break; } foreach (var dir in Dirs) { if (point.GetAroundFlag(dir)) { NevPoint tar = GetClosestNevPoint(point.pos + dir * step); float nowDis = dis[point] + step; float priority = nowDis + (to.pos - tar.pos).magnitude; bool contain = last.ContainsKey(tar); if (!contain) { insertPoint(tar, point, nowDis, priority); } else if (searchQueue.Contains(tar) && searchQueue.GetPriority(tar) > priority) { searchQueue.UpdatePriority(tar, priority); last[tar] = point; dis[tar] = nowDis; } } } } Action <Action <NevPoint> > doeachPathNode = (a) => { var node = to; while (last[node] != from) { node = last[node]; a(node); } }; int pathSize = 0; doeachPathNode((p) => pathSize++); NevPoint[] result = new NevPoint[pathSize]; //Not contain from and to doeachPathNode((p) => { pathSize--; result[pathSize] = p; }); return(result); }
public void TestSmoothPath() { Func <Vector2> getProperRandomPos = () => { Vector2 pos = Vector2.zero; NevPoint center = null; Action a = () => { pos = getRandomPosInEdge(); center = points[indexTileMap.GetClosestTile(pos, -1)]; }; a(); while (!validSmoothPath(pos, center.pos)) { a(); } return(pos); }; testPath = findSmoothPath(getProperRandomPos(), getProperRandomPos()); }
public void Bake() { originPos = VectorUtils.V32(transform.position); indexTileMap = new TileMapInt(edge, originPos, step, -1); points.Clear(); LinkedList <Vector2> queue = new LinkedList <Vector2>(); queue.AddLast(originPos); Func <Vector2, NevPoint> insertNewPos = (pos) => { var nevPoint = new NevPoint(pos); indexTileMap[pos] = points.Count; points.Add(nevPoint); return(nevPoint); }; insertNewPos(originPos); while (queue.Count != 0) { Vector2 pos = queue.First.Value; queue.RemoveFirst(); var nevPoint = GetClosestNevPoint(pos); foreach (var dir in Dirs) { Vector2 tar = pos + dir * step; if (!Physics2D.Raycast(pos, dir, step, solidLayer) && edge.Contains(tar) && (boxCollision == null || !boxCollision.CheckStaticCollision(solidLayer, tar))) { nevPoint.SetAroundFlag(dir, true); if (indexTileMap[tar] == -1) { insertNewPos(tar); queue.AddLast(tar); } } } } }
public void TestAstarSearch() { Func <NevPoint> getRandomNevPoint = () => { Func <NevPoint> gR = () => GetClosestNevPoint(getRandomPosInEdge()); var random = gR(); while (random == null) { random = gR(); } return(random); }; NevPoint testFrom = getRandomNevPoint(); NevPoint testTo = getRandomNevPoint(); var result = aStarSearch(testFrom, testTo); testPath = new Vector2[result.Length + 2]; testPath[0] = testFrom.pos; testPath[result.Length + 1] = testTo.pos; for (int i = 0; i < result.Length; i++) { testPath[i + 1] = result[i].pos; } return; }