//-------------------------------------------------------------------------
void _NewStep(ViAstarStep step)
{
for (Int32 idx = 0; idx < step.RoundSteps.Count; ++idx)
{
ViAstarRoundStep roundStep = step.RoundSteps[idx];
ViDebuger.AssertError(roundStep.node);
ViAstarStep childStep = roundStep.node;
if (childStep.IsClose)
{
continue;
}
float newG = roundStep.cost + step.G;
if (childStep.IsOpen)
{
if (childStep.G > newG)
{
childStep.G = newG;
childStep.Key = childStep.G + childStep.H;
childStep.Parent = step;
}
}
else
{
childStep.G = newG;
childStep.H = ViAstarStep.Distance(childStep, _destStep);
childStep.Key = childStep.G + childStep.H;
childStep.Parent = step;
_AddToOpen(childStep);
}
}
}
//-------------------------------------------------------------------------
public static float Distance(ViAstarStep from, ViAstarStep to)
{
float deltaX = from.Pos.x - to.Pos.x;
float deltaY = from.Pos.y - to.Pos.y;
return((float)Math.Sqrt(deltaX * deltaX + deltaY * deltaY));
}
//-------------------------------------------------------------------------
void _Fresh(Int32 x, Int32 y)
{
ViAstarStep step = _steps[x + y * _sizeX];
if (step.Cost > 0.0f)
{
return;
}
for (Int32 yIdx = y - 1; yIdx < y + 2; ++yIdx)
{
for (Int32 xIdx = x - 1; xIdx < x + 2; ++xIdx)
{
if (xIdx == x && yIdx == y)
{
continue;
}
ViAstarStep roundStep = _steps[xIdx + yIdx * _sizeX];
if (roundStep.Cost == 0.0f)
{
ViAstarRoundStep node = new ViAstarRoundStep();
node.node = roundStep;
//
float deltaX = x - xIdx;
float deltaY = y - yIdx;
node.cost = (float)Math.Sqrt(deltaX * deltaX + deltaY * deltaY);
step.RoundSteps.Add(node);
}
else
{
roundStep.Cost = 1.0f;
}
}
}
}
//-------------------------------------------------------------------------
public void MakeRoute(List <ViAstarStep> steps)
{
int dir = 1 + (1 << 2);
ViAstarStep next = _bestStep;
if (next == null)
{
return;
}
while (true)
{
ViAstarStep fromStep = next;
next = next.Parent;
if (next != null)
{
ViAstarStep toStep = next;
int deltaX = toStep.Pos.x - fromStep.Pos.x + 1;
int deltaY = (toStep.Pos.y - fromStep.Pos.y + 1) << 2;
int delta = deltaY + deltaX;
if (delta != dir)
{
steps.Insert(0, fromStep);
dir = delta;
}
}
else
{
steps.Insert(0, fromStep);
break;
}
}
}
//-------------------------------------------------------------------------
public void ClipRoute(List <ViAstarStep> complex, List <ViVector3> simple)
{
if (complex.Count <= 2)
{
for (Int32 idx = 0; idx < complex.Count; ++idx)
{
ViAstarStep pre = complex[idx];
simple.Add(new ViVector3(pre.Pos.x, pre.Pos.y, 0));
}
return;
}
ViAstarStep from = complex[0];
simple.Add(new ViVector3(from.Pos.x, from.Pos.y, 0));
for (Int32 idx = 1; idx < complex.Count; ++idx)
{
ViAstarStep pre = complex[idx - 1];
ViAstarStep current = complex[idx];
if (Pick(from.Pos.x, from.Pos.y, current.Pos.x, current.Pos.y))
{
simple.Add(new ViVector3(pre.Pos.x, pre.Pos.y, 0));
from = pre;
}
}
ViAstarStep back = complex[complex.Count - 1];
simple.Add(new ViVector3(back.Pos.x, back.Pos.y, 0));
}
//-------------------------------------------------------------------------
public bool Search(Int32 srcX, Int32 srcY, Int32 destX, Int32 destY, UInt32 maxStep)
{
_route.Clear();
//ViDebuger.Note("Search((" + srcX + ", " + srcY + "), (" + destX + ", " + destY + "))");
if ((srcX < _sizeX && srcY < _sizeY) == false || (destX < _sizeX && destY < _sizeY) == false)
{
return(false);
}
GetRound(srcX, srcY, 0.0f, 100, ref srcX, ref srcY);
if (srcX == destX && srcY == destY)
{
_route.Add(new ViVector3(srcX, srcY, 0));
_route.Add(new ViVector3(destX, destY, 0));
return(true);
}
if (!Pick(srcX, srcY, destX, destY))
{
_route.Add(new ViVector3(srcX, srcY, 0));
_route.Add(new ViVector3(destX, destY, 0));
return(true);
}
ViAstarStep srcStep = _steps[srcX + srcY * _sizeX];
ViAstarStep destStep = _steps[destX + destY * _sizeX];
_astar0.DestChecker.Dest = destStep;
_astar0.MaxStepCnt = maxStep;
bool reachDest = _astar0.Search(srcStep, destStep);
_stepPool.Clear();
_astar0.MakeRoute(_stepPool);
_astar0.Reset();
ClipRoute(_stepPool, _route);
return(reachDest);
}
//-------------------------------------------------------------------------
public bool Search(ViAstarStep src, ViAstarStep dest)
{
ViDebuger.AssertWarning(src.Cost == 0.0f);
src.G = 0;
src.H = ViAstarStep.Distance(src, dest);
src.Key = src.G + src.H;
//
_startStep = src;
_bestStep = src;
_startStep.Clear();
_destStep = dest;
//
_stepCnt = 0;
//
_AddToOpen(src);
//
while (_openHeap.Size > 0)
{
_currentStep = _openHeap.Pop();
ViDebuger.AssertError(_currentStep);
if (DestChecker.IsDest(_currentStep))
{
_bestStep = _currentStep;
return(true);
}
if (_stepCnt > _maxStepCnt)
{
return(false);
}
if (_currentStep.H < _bestStep.H)
{
_bestStep = _currentStep;
}
++_stepCnt;
ViDebuger.AssertError(_currentStep.IsOpen);
_NewStep(_currentStep);
_currentStep.AttachNode.Detach();
_AddToClose(_currentStep);
}
return(false);
}
//-------------------------------------------------------------------------
public bool Search(Int32 srcX, Int32 srcY, Int32 destX, Int32 destY, float fDiff, UInt32 maxStep)
{
//ViDebuger.Note("Search((" + srcX + ", " + srcY + "), (" + destX + ", " + destY + "))");
if ((srcX < _sizeX && srcY < _sizeY) == false || (destX < _sizeX && destY < _sizeY) == false)
{
return(false);
}
GetRound(srcX, srcY, 0.0f, 100, ref srcX, ref srcY);
ViAstarStep srcStep = _steps[srcX + srcY * _sizeX];
ViAstarStep destStep = _steps[destX + destY * _sizeX];
_astar1.DestChecker.Dest = destStep;
_astar1.DestChecker.Diff = fDiff;
_astar1.MaxStepCnt = maxStep;
bool reachDest = _astar1.Search(srcStep, destStep);
_route.Clear();
_stepPool.Clear();
_astar1.MakeRoute(_stepPool);
_astar1.Reset();
ClipRoute(_stepPool, _route);
return(reachDest);
}
//-------------------------------------------------------------------------
public void BlockStart(Int32 sizeX, Int32 sizeY)
{
Int32 cnt = sizeX * sizeY;
if (cnt == 0)
{
return;
}
_steps = new ViAstarStep[cnt];
_sizeX = sizeX;
_sizeY = sizeY;
for (Int32 yIdx = 0; yIdx < _sizeY; ++yIdx)
{
for (Int32 xIdx = 0; xIdx < _sizeX; ++xIdx)
{
ViAstarStep step = new ViAstarStep();
step.Cost = 0;
step.Pos.x = xIdx;
step.Pos.y = yIdx;
_steps[xIdx + yIdx * _sizeX] = step;
}
}
}
//-------------------------------------------------------------------------
public virtual bool IsDest(ViAstarStep kStep)
{
return(true);
}
//-------------------------------------------------------------------------
public void Clear()
{
G = 0;
_parent = null;
State = ViAstarStepState.NONE;
}
//-------------------------------------------------------------------------
void _AddToClose(ViAstarStep step)
{
step.State = ViAstarStepState.CLOSE;
_closeList.PushBack(step.AttachNode);
}
//-------------------------------------------------------------------------
void _AddToOpen(ViAstarStep step)
{
_openList.PushBack(step.AttachNode);
step.State = ViAstarStepState.OPEN;
_openHeap.Push(step);
}
//-------------------------------------------------------------------------
public override bool IsDest(ViAstarStep step)
{
return(Object.ReferenceEquals(step, Dest));
}
//-------------------------------------------------------------------------
public override bool IsDest(ViAstarStep step)
{
return(ViAstarStep.Distance(step, Dest) < Diff);
}
