public List <PathNode> FindPath(Vector3 startPos, Vector3 endPos #if UNITY_EDITOR && Draw_Calc_Node , ref Dictionary <int, PathNode> calcNodeDic #endif ) { lock (this) { #if Diagnostic var sw = new System.Diagnostics.Stopwatch(); sw.Start(); #endif #if UNITY_EDITOR && Draw_Calc_Node calcNodeDic.Clear(); PathNode calcNode; calcNode.F = 0; calcNode.G = 0; calcNode.PX = 0; calcNode.PY = 0; #endif mFound = false; mStop = false; mStopped = false; mCloseNodeCounter = 0; mOpenNodeValue += 2; mCloseNodeValue += 2; mOpen.clear(); Point start = mModel.getXZ(startPos); Point end = mModel.getXZ(endPos); mLocation = (start.y << mGridXBitWide) + start.x; mEndLocation = (end.y << mGridXBitWide) + end.x; mCalcGrid[mLocation].G = 0; mCalcGrid[mLocation].F = Math.Abs(start.x - end.x) + Math.Abs(start.y - end.y); mCalcGrid[mLocation].PX = (ushort)start.x; mCalcGrid[mLocation].PY = (ushort)start.y; mCalcGrid[mLocation].Status = mOpenNodeValue; mOpen.push(mLocation); ushort locationX = 0, locationY = 0, newLocationX = 0, newLocationY = 0; int newLocation = 0, horiz = 0; float newG = 0, heuristic = 0; #if Diagnostic sw.Stop(); Debug.Log(sw.ElapsedTicks / 10000.0f); sw.Reset(); sw.Start(); #endif while (mOpen.count > 0 && !mStop) { mLocation = mOpen.pop(); PathNodeFast curNode = mCalcGrid[mLocation]; if (curNode.Status == mCloseNodeValue) { continue; } locationX = (ushort)(mLocation & mGridXMinus); locationY = (ushort)(mLocation >> mGridXBitWide); if (mLocation == mEndLocation) { mCalcGrid[mLocation].Status = mCloseNodeValue; mFound = true; break; } //search time limit? //punish change direction? if (punishChangeDirection) { horiz = locationX - mCalcGrid[mLocation].PX; } for (int i = 0; i < 8; i++) { newLocationX = (ushort)(locationX + mDirection[i, 0]); newLocationY = (ushort)(locationY + mDirection[i, 1]); newLocation = (newLocationY << mGridXBitWide) + newLocationX; if (newLocationX >= mGridX || newLocationY >= mGridY) { continue; } //Debug.Log("newLocationX: " + newLocationX + " newLocationY: " + newLocationY); if (mModel.getTargetGrid(newLocationX, newLocationY) == NodeState.Block) { continue; } newG = curNode.G + 1; if (mHeavyDiagonals && i > 3) { newG += 0.41f; } if (punishChangeDirection) { if ((newLocationX - locationX) != 0) { if (0 == horiz) { newG += Math.Abs(newLocationX - end.x) + Math.Abs(newLocationY - end.y); } } if (0 != (newLocationY - locationY)) { if (0 != horiz) { newG += Math.Abs(newLocationX - end.x) + Math.Abs(newLocationY - end.y); } } } //if(newLocation >= mCalcGrid.Length) //{ // Debug.Log("newLocation: " + newLocation); //} PathNodeFast newNode = mCalcGrid[newLocation]; byte newLocalStatus = newNode.Status; if (newLocalStatus == mOpenNodeValue || newLocalStatus == mCloseNodeValue) { if (newNode.G <= newG) { continue; } } mCalcGrid[newLocation].PX = locationX; mCalcGrid[newLocation].PY = locationY; mCalcGrid[newLocation].G = newG; switch (formula) { case HeuristicFormula.Manhattan: heuristic = Math.Abs(newLocationX - end.x) + Math.Abs(newLocationY - end.y); break; case HeuristicFormula.MaxDXDY: heuristic = Math.Max(Math.Abs(newLocationX - end.x), Math.Abs(newLocationY - end.y)); break; case HeuristicFormula.DiagonalShortCut: { int xDist = Math.Abs(newLocationX - end.x); int yDist = Math.Abs(newLocationY - end.y); if (xDist > yDist) { heuristic = 1.4f * yDist + (xDist - yDist); } else { heuristic = 1.4f * xDist + (yDist - xDist); } } break; case HeuristicFormula.Euclidean: heuristic = Mathf.Sqrt(Mathf.Pow(Math.Abs(newLocationX - end.x), 2) + Mathf.Pow(Math.Abs(newLocationY - end.y), 2)); break; case HeuristicFormula.EuclideanWithG: heuristic = Mathf.Sqrt(Mathf.Pow(Math.Abs(newLocationX - end.x), 2) + Mathf.Pow(Math.Abs(newLocationY - end.y), 2)); newG *= newG; break; case HeuristicFormula.EuclideanNoSQR: heuristic = Mathf.Pow(Math.Abs(newLocationX - end.x), 2) + Mathf.Pow(Math.Abs(newLocationY - end.y), 2); break; case HeuristicFormula.Custom1: { int xDist = Math.Abs(newLocationX - end.x); int yDist = Math.Abs(newLocationY - end.y); int Orthogonal = Math.Abs(xDist - yDist); int Diagonal = Math.Abs(((xDist + yDist) - Orthogonal) / 2); heuristic = Diagonal + Orthogonal + xDist + yDist; } break; } if (tieBreaker) { int dx1 = locationX - end.x; int dy1 = locationY - end.y; int dx2 = start.x - end.x; int dy2 = start.y - end.y; int cross = Math.Abs(dx1 * dy2 - dx2 * dy1); heuristic += cross * 0.001f; } mCalcGrid[newLocation].F = newG + heuristic; #if UNITY_EDITOR && Draw_Calc_Node if (!calcNodeDic.ContainsKey(newLocation)) { calcNode.X = newLocationX; calcNode.Y = newLocationY; calcNodeDic[newLocation] = calcNode; } #endif mOpen.push(newLocation); mCalcGrid[newLocation].Status = mOpenNodeValue; } mCloseNodeCounter++; mCalcGrid[mLocation].Status = mCloseNodeValue; } #if Diagnostic sw.Stop(); Debug.Log(sw.ElapsedTicks / 10000.0f); #endif if (mFound) { #if Diagnostic sw.Reset(); sw.Start(); #endif mClose.Clear(); int posX = end.x; int posY = end.y; PathNodeFast fNodeTmp; PathNode fNode; do { fNodeTmp = mCalcGrid[(posY << mGridXBitWide) + posX]; fNode.F = fNodeTmp.F; fNode.G = fNodeTmp.G; fNode.PX = fNodeTmp.PX; fNode.PY = fNodeTmp.PY; fNode.X = posX; fNode.Y = posY; posX = fNode.PX; posY = fNode.PY; if (fNode.X != fNode.PX || fNode.Y != fNode.PY) { mClose.Add(fNode); } else { break; } }while(true); mClose.Add(fNode); #if Diagnostic sw.Stop(); Debug.Log(sw.ElapsedTicks / 10000.0f); #endif mStopped = true; return(mClose); } mStopped = true; return(null); } }