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