// (src,target]までRayCastする
public override void RaycastCell(Vector2 src, Vector2 target, AstarCell.Type ignore, System.Func <int, int, bool> act)
{
if (this.UsingUnityRaycast)
{
var hits = Physics.SphereCastAll(new Vector3(src.x, this.TileSize * 0.6f, src.y),
this.RayCastHalfExtents, // this.TileSize * 0.5f,
new Vector3(target.x - src.x, 0.0f, target.y - src.y),
(target - src).magnitude);
foreach (var h in hits)
{
if (this.DisallowTags.Contains(h.transform.tag))
{
continue;
}
var pos = hits[0].transform.position;
int index = this.cellIndex(new Vector2(pos.y, pos.z));
AstarCell cell = null;
if (index >= 0 && index < this.cellMapBody.Count())
{
cell = this.cellMapBody[index];
}
///act(cell);
return;
}
///act(this.CellMap(target));
}
else
{
base.RaycastCell(src, target, ignore, act);
}
}
public void PathFind(AstarCell startCell,
AstarCell goalCell,
System.Action <AstarCell> makeRelation,
System.Action <List <Vector2> > onGoal,
bool initOnly = false)
{
if (this.Busy)
{
throw new System.InvalidOperationException();
}
this.Busy = true;
this.startCell = startCell;
this.goalCell = goalCell;
this.MakeRelation = makeRelation;
this.onGoal = onGoal;
this.pathCount = 0;
this.finished = false;
this.goalCandidate.Clear();
ScanAround(this.startCell);
if (!initOnly)
{
while (!this.finished)
{
pathFindProcess();
}
}
}
private void drawCell(AstarCell cell)
{
float x = cell.Position.x;
float y = cell.Position.y;
float t = this.TileSize * 0.4f;
Color[] coltbl = { Color.green,// empty
Color.blue, Color.yellow, Color.white, Color.gray, Color.black, Color.red, new Color(0.1f, 0.1f, 0.1f, 0.1f),
Color.red };
var color = coltbl[(int)cell.CellType];
if (cell.CellType != Tsl.Math.Pathfinder.AstarCell.Type.Removed)
{
Debug.DrawLine(new Vector3(x - t, 0.1f, y - t), new Vector3(x + t, 0.1f, y - t), color, 1.0f, false);
Debug.DrawLine(new Vector3(x + t, 0.1f, y - t), new Vector3(x + t, 0.1f, y + t), color, 1.0f, false);
Debug.DrawLine(new Vector3(x + t, 0.1f, y + t), new Vector3(x - t, 0.1f, y + t), color, 1.0f, false);
Debug.DrawLine(new Vector3(x - t, 0.1f, y + t), new Vector3(x - t, 0.1f, y - t), color, 1.0f, false);
foreach (var r in cell.Related)
{
var p = r.cell.Position;
Debug.DrawLine(new Vector3(x, 0.1f, y), new Vector3(p.x, 0.1f, p.y), new Color(0.0f, 1.0f, 1.0f, 0.2f));
}
}
}
// 上下左右斜めのグリッドに対してリンクを作成する
public override void MakeRelation(AstarCell parent)
{
parent.Related.Clear();
if (parent.CellType == AstarCell.Type.Block)
{
return;
}
int x = (int)parent.Position.x;
int y = (int)parent.Position.y;
for (int dx = -1; dx < 2; ++dx)
{
for (int dy = -1; dy < 2; ++dy)
{
if (dx == 0 && dy == 0)
{
continue;
}
float nx = x + dx * this.TileSize;
float ny = y + dy * this.TileSize;
Vector2 n = new Vector2(nx, ny);
if (!this.MapRect.Contains(n))
{
continue;
}
var cell = this.CellMap(n);
if (cell != null && cell.CellType != AstarCell.Type.Block)
{
parent.AddRelated(this.CellMap(n), parent.Heuristic(cell));
}
}
}
parent.RelationBuilt = true;
}
// intで評価されるmapRectのグリッドセルで初期化
public void MapInit(Rect mapRect, float tilesize = 0.0f)
{
if (tilesize != 0.0f)
{
this.TileSize = tilesize;
}
this.MapRect = mapRect;
// グリッドのサイズを計算しておく
this.GridWidth = (int)(this.MapRect.width / this.TileSize + 0.5f);
this.GridHeight = (int)(this.MapRect.height / this.TileSize + 0.5f);
// グリッドを埋め尽くすセルの配列
this.cellMapBody = new AstarCell[this.GridHeight * this.GridWidth];
// グリッドをセルの実体で埋め尽くす
for (int iy = 0; iy < this.GridHeight; ++iy)
{
float y = mapRect.y + iy * this.TileSize;
for (int ix = 0; ix < this.GridWidth; ++ix)
{
float x = mapRect.x + ix * this.TileSize;
var cell = new AstarCell();
cell.Position = new Vector2(x, y);
cellMapBody[cellIndex(cell.Position)] = cell;
}
}
}
// 動的なセルの削除
public override void RemoveCell(AstarCell cell)
{
if (cell != null)
{
setCellType(cell.Position, AstarCell.Type.Removed);
base.RemoveCell(cell);
}
}
// 接続情報を生成する
public override void MakeRelation(AstarCell cell)
{
if (this.logic.cells == null)
{
Debug.LogError("this.logic.cells is null");
throw new System.InvalidOperationException();
}
// if (!this.logic.cells.Contains(cell)) this.logic.cells.Add(cell);
setGridRelatedSearchRaycast(cell, true);
cell.RelationBuilt = true;
}
public void PathFind(Vector2 start,
Vector2 goal,
System.Action <List <Vector2> > onEnd = null,
ExecuteMode mode = ExecuteMode.ASync)
{
this.executeMode = mode;
System.Action <List <Vector2> > onFinish = r =>
{
RemoveCell(this.startCell);
RemoveCell(this.goalCell);
if (this.DebugHaltMode && r == null)
{
this.DebugHalt = true;
}
else
{
onEnd(r);
}
};
if (mode != ExecuteMode.StepNext)
{ // 初回のPathFind
this.goalCell = AddCellImmediate(goal, AstarCell.Type.Goal);
this.startCell = AddCellImmediate(start, AstarCell.Type.Start);
if (this.startCell == null || this.goalCell == null)
{
Debug.LogWarning("start or gold is in block");
RemoveCell(this.startCell);
RemoveCell(this.goalCell);
Reset(false);
onEnd(null);
return;
}
this.logic.PathFind(startCell, goalCell, this.MakeRelation, onFinish, mode != ExecuteMode.Sync);
return;
}
switch (mode)
{
case ExecuteMode.Sync:
break;
case ExecuteMode.ASync:
pathFindProcessCoroutine();
break;
case ExecuteMode.StepFirst:
case ExecuteMode.StepNext:
this.logic.pathFindProcess();
break;
default:
throw new System.InvalidOperationException();
}
}
// 動的なセルの削除
public override void RemoveCell(AstarCell cell)
{
foreach (var target in cell.Related)
{ // taget はcellからの接続先
var found = target.cell.Related.Find(c => c.cell == cell);
if (found.cell == cell)
{
target.cell.Related.Remove(found);
}
}
this.logic.cells.Remove(cell);
}
private void drawCellCorrect(AstarCell cell)
{
if (cell.CellType == AstarCell.Type.Correct || cell.CellType == AstarCell.Type.Start || cell.CellType == AstarCell.Type.Goal)
{
foreach (var r in cell.Related)
{
if (r.cell.CellType == Tsl.Math.Pathfinder.AstarCell.Type.Correct)
{
var p = r.cell.Position;
Debug.DrawLine(new Vector3(cell.Position.x, 0.1f, cell.Position.y), new Vector3(p.x, 0.1f, p.y), Color.red, 1.0f, false);
}
}
}
}
public void Reset(bool allReset)
{
if (allReset)
{
this.CellType = Type.Removed;
}
else
{
if (this.CellType != Type.Removed && this.CellType != Type.Block)
{
this.CellType = Type.Empty;
}
}
this.Score = 0.0f;
this.Cost = 0.0f;
this.Hint = 0.0f;
this.Parent = null;
}
public override AstarCell AddCellImmediate(Vector2 pos, AstarCell.Type type)
{
if (this.GridMode)
{
return(base.AddCellImmediate(pos, type));
}
else
{
AstarCell cell = null;
if (type == AstarCell.Type.Start || type == AstarCell.Type.Goal)
{ // スタートかゴールがブロックの場合はエラー
if (CellMap(pos).CellType == AstarCell.Type.Block)
{
return(null);
}
}
else
{ // StartとGoal以外はセルを探す
cell = CellMap(pos);
}
if (cell == null)
{
cell = new AstarCell();
cell.CellType = type;
cell.Position = pos;
setCellType(cell);
MakeRelation(cell);
this.logic.cells.Add(cell);
}
else
{
cell.CellType = type;
setCellType(cell);
MakeRelation(cell);
if (!this.logic.cells.Contains(cell))
{
this.logic.cells.Add(cell);
}
}
return(cell);
}
}
private void pathfindFinished(AstarCell cell)
{
var pathList = new List <Vector2>();
pathList.Add(this.goalCell.Position);
var parent = cell;
while (parent.Parent != null)
{
pathList.Add(parent.Position);
parent.CellType = AstarCell.Type.Correct;
parent = parent.Parent;
}
pathList.Add(this.startCell.Position);
pathList.Reverse();
this.finished = true;
this.onGoal(pathList);
//var links = this.cells.Sum(c => c.Related.Count);
//Debug.Log(string.Format("{0} cells {1} links", this.cells.Count, links));
this.Busy = false;
}
// intで評価されるmapRectのグリッドセルで初期化
public void MapInit(int columns, int rows)
{
// グリッドのサイズを計算しておく
this.GridColumns = columns;
this.GridRows = rows;
// グリッドを埋め尽くすセルの配列
this.cellMapBody = new AstarCell[(this.GridColumns + 1) / 2, this.GridRows];
// グリッドをセルの実体で埋め尽くす
for (int iy = 0; iy < this.GridRows; ++iy)
{
for (int ix = 0; ix < (this.GridColumns + 1) / 2; ++ix)
{
var cell = new AstarCell();
float x = ix + ((iy & 1) == 0 ? 0.0f : cos60);
float y = iy * sin60;
cell.Position = new Vector2(x, y);
cellMapBody[ix, iy] = cell;
}
}
}
public override void MakeRelation(AstarCell cell)
{
if (this.logic.cells == null)
{
Debug.LogError("this.logic.cells is null");
throw new System.InvalidOperationException();
}
cell.Related.Clear();
for (int iy = -1; iy <= 1; ++iy)
{
float y = cell.Position.y + iy * sin60;
for (int ix = -1; ix <= 1; ix += 2)
{
float x;
if (iy == 0)
{
x = cell.Position.x + ix;
}
else
{
x = cell.Position.x + ix * cos60;
}
var c = FindCell(new Vector2(x, y));
if (c != cell && c != null)
{
if (!cell.Contains(c))
{
cell.AddRelated(c, c.MoveCost);
}
if (!c.Contains(cell))
{
c.AddRelated(cell, cell.MoveCost);
}
}
}
}
cell.RelationBuilt = true;
}
private void ScanAround(AstarCell parent)
{
// 接続情報を作成する
if (!parent.RelationBuilt)
{
MakeRelation(parent);
}
foreach (var related in parent.Related)
{
if (related.cell.CellType == AstarCell.Type.Goal)
{ // !! GOAL!
var goalcost = parent.Cost + related.cost;
related.cell.Cost = goalcost;
related.cell.Score = goalcost;
if (this.goalCandidate.ContainsKey(goalcost))
{
this.goalCandidate[goalcost] = parent;
}
else
{
this.goalCandidate.Add(goalcost, parent);
}
}
float cost = parent.Cost + related.cost;
float hint = this.goalCell.Heuristic(related.cell);
float score = cost + hint;
if (related.cell.CellType == AstarCell.Type.Empty || related.cell.Score > score)
{
related.cell.Cost = cost;
related.cell.Hint = hint;
related.cell.Score = score;
related.cell.Parent = parent;
related.cell.CellType = AstarCell.Type.Open;
}
}
}
public bool Contains(AstarCell cell)
{
return(this.Related.Any(r => r.cell == cell));
}
public void AddRelated(AstarCell cell, float cost)
{
this.Related.Add(new RelatedData {
cell = cell, cost = cost
});
}
public float Heuristic3D(AstarCell cell)
{
return(((cell as AstarCell3D).Position3D - this.Position3D).magnitude);
}
private void setCellType(AstarCell cell)
{
setCellType(cell.Position, cell.CellType);
}
// 到達可能なノードを全ノードからレイキャストして調べる
public void setGridRelatedSearchRaycast(AstarCell parent, bool newCell = false)
{
foreach (var cell in this.logic.cells.Where(c => c.IsValidCell()))
{
if (cell == parent)
{
continue;
}
var fromCell = cell.Find(parent);
if (fromCell.cell != null)
{ // 相手から自分が見えている場合
if (!parent.Contains(cell))
{
parent.AddRelated(cell, fromCell.cost);
}
continue;
}
// raycast
float cost = 0;
var prevPos = parent.Position;
if (!this.GridMode)
{ // 中間グリッドを飛ばして最短距離で結ぶ場合
RaycastCell(parent.Position, cell.Position, AstarCell.Type.Removed,
(ix, iy) =>
{
var cellType = this.cellType[ix, iy];
if (cellType != AstarCell.Type.Removed)
{ // 何かあった
if (cellType != AstarCell.Type.Block)
{ // ブロックもしくは圏外ではない場合
var rcell = this.Cell(ix, iy);
if ((rcell.Position - cell.Position).sqrMagnitude < 0.1f)
{ // 見つかった!
if (!parent.Contains(cell))
{
cost = (cell.Position - parent.Position).magnitude;
parent.AddRelated(cell, cost);
if (newCell && !cell.Contains(parent))
{
cell.AddRelated(parent, cost);
}
}
}
}
return(true);
}
else
{
return(false);
}
});
}
else
{ // グリッドモード
RaycastCell(parent.Position, cell.Position, AstarCell.Type.NoIgnore,
(ix, iy) =>
{
var rcell = this.Cell(ix, iy);
// グリッドモードではコストを計算する
var nowpos = rcell.Position;
cost += (nowpos - prevPos).magnitude;
prevPos = nowpos;
if (rcell.CellType != AstarCell.Type.Removed)
{ // 何かあった
if (rcell.CellType != AstarCell.Type.Block)
{ // ブロックもしくは圏外ではない場合
if (rcell == cell)
{ // 見つかった!
if (!parent.Contains(cell))
{
if (!this.GridMode)
{
cost = (cell.Position - parent.Position).magnitude;
}
parent.AddRelated(cell, cost);
if (newCell && !cell.Contains(parent))
{
cell.AddRelated(parent, cost);
}
}
}
}
return(true);
}
else
{
return(false);
}
});
}
}
}
public RelatedData Find(AstarCell cell)
{
return(this.Related.Find(r => r.cell == cell));
}
// 動的なセルの削除 public abstract void RemoveCell(AstarCell cell);
// セル間の接続情報の生成 public abstract void MakeRelation(AstarCell cell);
public float Heuristic(AstarCell cell)
{
return((cell.Position - this.Position).magnitude);
}
// 動的なセルの削除
public override void RemoveCell(AstarCell cell)
{
cell.CellType = AstarCell.Type.Empty;
}
