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