/// <summary>
///Check if first angle is start point
/// </summary>
/// <param name="start"></param>
/// <param name="parentNode"></param>
/// <returns></returns>
public static bool IsAngleStartPoint(Location start, PathFinderNode parentNode)
{
if (start.X == parentNode.ANX &&
start.Y == parentNode.ANY &&
start.Z == parentNode.ANZ)
{
return(true);
}
return(false);
}
public static bool IfMinLength(byte length, PathFinderNode parentNode, int newNodeX, int newNodeY, int newNodeZ)
{
if ((newNodeY - parentNode.ANY) != 0 && (newNodeX - parentNode.ANX) != 0)
{
if (Math.Abs(parentNode.Y - parentNode.ANY) == 0 && Math.Abs(parentNode.X - parentNode.ANX) < length)
{
return(false);
}
if (Math.Abs(parentNode.X - parentNode.ANX) == 0 && Math.Abs(parentNode.Y - parentNode.ANY) < length)
{
return(false);
}
WriteNodeAN(parentNode);
}
else if ((newNodeY - parentNode.ANY) != 0 && (newNodeZ - parentNode.ANZ) != 0)
{
if (Math.Abs(parentNode.Y - parentNode.ANY) == 0 && Math.Abs(parentNode.Z - parentNode.ANZ) < length)
{
return(false);
}
if (Math.Abs(parentNode.Z - parentNode.ANZ) == 0 && Math.Abs(parentNode.Y - parentNode.ANY) < length)
{
return(false);
}
WriteNodeAN(parentNode);
}
else if ((newNodeX - parentNode.ANX) != 0 && (newNodeZ - parentNode.ANZ) != 0)
{
if (Math.Abs(parentNode.Z - parentNode.ANZ) == 0 && Math.Abs(parentNode.X - parentNode.ANX) < length)
{
return(false);
}
if (Math.Abs(parentNode.X - parentNode.ANX) == 0 && Math.Abs(parentNode.Z - parentNode.ANZ) < length)
{
return(false);
}
WriteNodeAN(parentNode);
}
else
{
newNodeANX = parentNode.ANX;
newNodeANY = parentNode.ANY;
newNodeANZ = parentNode.ANZ;
}
return(true);
}
public static int GetNewG(int newNodeX, int newNodeY, int newNodeZ, PathFinderNode parentNode, Location start, int NodeG)
{
if (PointChecker.IsAngleStartPoint(start, parentNode))
{
return(NodeG);
}
//Check if direction was changed
int k = 0;
if ((newNodeX - parentNode.PX) != 0)
{
k++;
}
if ((newNodeY - parentNode.PY) != 0)
{
k++;
}
if ((newNodeZ - parentNode.PZ) != 0)
{
k++;
}
if (k >= 2)
{
NodeG += 5;
//Check distance from parent node to angle
//int maxPointsToAngle = 8;
//int dist = Math.Abs(parentNode.X - parentNode.ANX) +
// Math.Abs(parentNode.Y - parentNode.ANY) +
// Math.Abs(parentNode.Z - parentNode.ANZ);
//if (dist < maxPointsToAngle)
//{
// //Hard punish
// NodeG += 30;
//}
//else
//{
// NodeG += 5;
//}
}
return(NodeG);
}
public List <PathFinderNode> FindPath(Location start, Location end, sbyte[,] direction)
{
PathFinderNode parentNode;
bool found = false;
int gridX = mGrid.GetUpperBound(0) + 1;
int gridY = mGrid.GetUpperBound(1) + 1;
int gridZ = mGrid.GetUpperBound(2) + 1;
mStop = false;
mStopped = false;
mOpen.Clear();
mClose.Clear();
parentNode.G = 0;
parentNode.H = mHEstimate;
parentNode.F = parentNode.G + parentNode.H;
parentNode.X = start.X;
parentNode.Y = start.Y;
parentNode.Z = start.Z;
parentNode.PX = parentNode.X;
parentNode.PY = parentNode.Y;
parentNode.PZ = parentNode.Z;
parentNode.Id = 0;
parentNode.ANX = start.X;
parentNode.ANY = start.Y;
parentNode.ANZ = start.Z;
//Clearance zone creation
CLZChecker cLZChecker = null;
if (mCLZdist != 0)
{
//Clearance zone creation
RefCLZCreator refCLZCreator = new RefCLZCreator(mCLZdist);
mClearanceZone = refCLZCreator.Create();
cLZChecker = new CLZChecker(mGrid, mClearanceZone);
}
mOpen.Push(parentNode);
while (mOpen.Count > 0 && !mStop)
{
parentNode = mOpen.Pop();
if (parentNode.X == end.X && parentNode.Y == end.Y && parentNode.Z == end.Z)
{
mClose.Add(parentNode);
found = true;
break;
}
if (mClose.Count > mSearchLimit)
{
mStopped = true;
return(null);
}
if (mPunishChangeDirection)
{
mHoriz = (parentNode.X - parentNode.PX);
}
//Lets calculate each successors
for (int i = 0; i <= (mDiagonals ? 8 : direction.GetUpperBound(0)); i++)
{
PathFinderNode newNode;
newNode.X = parentNode.X + direction[i, 0];
newNode.Y = parentNode.Y + direction[i, 1];
newNode.Z = parentNode.Z + direction[i, 2];
newNode.Id = 0;
if (newNode.X < 0 || newNode.Y < 0 || newNode.Z < 0 || newNode.X >= gridX || newNode.Y >= gridY || newNode.Z >= gridZ)
{
continue;
}
int newG;
if (mHeavyDiagonals && i > 3)
{
newG = parentNode.G + (int)(mGrid[newNode.X, newNode.Y, newNode.Z] * 2.41);
}
else
{
newG = parentNode.G + mGrid[newNode.X, newNode.Y, newNode.Z];
}
//check point by obstacles and clerance zone
if (cLZChecker != null)
{
if (!cLZChecker.CheckNode(newNode.X, newNode.Y, newNode.Z, gridX, gridY, gridZ))
{
continue;
}
}
else
{
if (mGrid[newNode.X, newNode.Y, newNode.Z] == 1)
{
continue;
}
}
if (mCompactPath)
{
int cost = 2;
if ((start.X == end.X) && (newNode.X - start.X) != 0)
{
if ((newNode.Y - parentNode.Y) != 0)
{
newG += cost;
}
}
if (start.Y == end.Y && (newNode.Y - start.Y) != 0)
{
if ((newNode.X - parentNode.X) != 0)
{
newG += cost;
}
}
if (start.Z == end.Z && (newNode.Z - start.Z) != 0)
{
if ((newNode.X - parentNode.X) != 0 || (newNode.Y - parentNode.Y) != 0)
{
newG += cost;
}
}
}
if (mPunishChangeDirection)
{
newG += PunishChangeDirectionChecker.GetNewG(newNode.X, newNode.Y, newNode.Z,
parentNode, start, newG);
}
int foundInOpenIndex = -1;
for (int j = 0; j < mOpen.Count; j++)
{
if (mOpen[j].X == newNode.X && mOpen[j].Y == newNode.Y && mOpen[j].Z == newNode.Z)
{
foundInOpenIndex = j;
break;
}
}
if (foundInOpenIndex != -1 && mOpen[foundInOpenIndex].G <= newG)
{
continue;
}
int foundInCloseIndex = -1;
for (int j = 0; j < mClose.Count; j++)
{
if (mClose[j].X == newNode.X && mClose[j].Y == newNode.Y && mClose[j].Z == newNode.Z)
{
foundInCloseIndex = j;
break;
}
}
if (foundInCloseIndex != -1 && (mReopenCloseNodes || mClose[foundInCloseIndex].G <= newG))
{
continue;
}
newNode.PX = parentNode.X;
newNode.PY = parentNode.Y;
newNode.PZ = parentNode.Z;
newNode.G = newG;
newNode.ANX = 0;
newNode.ANY = 0;
newNode.ANZ = 0;
if (mANGlength != 0)
{
if (!NodesCheker.IfMinLength(mANGlength, parentNode, newNode.X, newNode.Y, newNode.Z))
{
continue;
}
else
{
newNode.ANX = NodesCheker.newNodeANX;
newNode.ANY = NodesCheker.newNodeANY;
newNode.ANZ = NodesCheker.newNodeANZ;
}
}
switch (mFormula)
{
default:
case HeuristicFormula.Manhattan:
newNode.H = mHEstimate * (Math.Abs(newNode.X - end.X) + Math.Abs(newNode.Y - end.Y) + Math.Abs(newNode.Z - end.Z));
break;
case HeuristicFormula.MaxDXDY:
newNode.H = mHEstimate * (Math.Max(Math.Abs(newNode.X - end.X), Math.Abs(newNode.Y - end.Y)));
break;
case HeuristicFormula.DiagonalShortCut:
int h_diagonal = Math.Min(Math.Abs(newNode.X - end.X), Math.Abs(newNode.Y - end.Y));
int h_straight = (Math.Abs(newNode.X - end.X) + Math.Abs(newNode.Y - end.Y));
newNode.H = (mHEstimate * 2) * h_diagonal + mHEstimate * (h_straight - 2 * h_diagonal);
break;
case HeuristicFormula.Euclidean:
newNode.H = (int)(mHEstimate * Math.Sqrt(Math.Pow((newNode.X - end.X), 2) + Math.Pow((newNode.Y - end.Y), 2)));
break;
case HeuristicFormula.EuclideanNoSQR:
newNode.H = (int)(mHEstimate * (Math.Pow((newNode.X - end.X), 2) + Math.Pow((newNode.Y - end.Y), 2)));
break;
case HeuristicFormula.Custom1:
Point dxy = new Point(Math.Abs(end.X - newNode.X), Math.Abs(end.Y - newNode.Y));
int Orthogonal = Math.Abs(dxy.X - dxy.Y);
int Diagonal = Math.Abs(((dxy.X + dxy.Y) - Orthogonal) / 2);
newNode.H = mHEstimate * (Diagonal + Orthogonal + dxy.X + dxy.Y);
break;
}
if (mTieBreaker)
{
int dx1 = parentNode.X - end.X;
int dy1 = parentNode.Y - end.Y;
int dz1 = parentNode.Z - end.Z;
int dx2 = start.X - end.X;
int dy2 = start.Y - end.Y;
int dz2 = start.Z - end.Z;
int cross = Math.Abs(dx1 * dy2 - dx2 * dy1); //fix z here
newNode.H = (int)(newNode.H + cross * 0.001);
}
newNode.F = newNode.G + newNode.H;
mOpen.Push(newNode);
}
mClose.Add(parentNode);
}
if (found)
{
PathFinderNode fNode = mClose[mClose.Count - 1];
for (int i = mClose.Count - 1; i >= 0; i--)
{
if (fNode.PX == mClose[i].X && fNode.PY == mClose[i].Y && fNode.PZ == mClose[i].Z || i == mClose.Count - 1)
{
fNode = mClose[i];
}
else
{
mClose.RemoveAt(i);
}
}
mStopped = true;
return(mClose);
}
mStopped = true;
return(null);
}
private static void WriteNodeAN(PathFinderNode parentNode)
{
newNodeANX = parentNode.X;
newNodeANY = parentNode.Y;
newNodeANZ = parentNode.Z;
}
public List <PathFinderNode> FindPath(Location start, Location end)
{
PathFinderNode parentNode;
bool found = false;
int gridX = mGrid.GetUpperBound(0) + 1;
int gridY = mGrid.GetUpperBound(1) + 1;
int gridZ = mGrid.GetUpperBound(2) + 1;
mStop = false;
mStopped = false;
mOpen.Clear();
mClose.Clear();
parentNode.G = 0;
parentNode.H = mHEstimate;
parentNode.F = parentNode.G + parentNode.H;
parentNode.X = start.X;
parentNode.Y = start.Y;
parentNode.Z = start.Z;
parentNode.PX = parentNode.X;
parentNode.PY = parentNode.Y;
parentNode.PZ = parentNode.Z;
parentNode.PZ = parentNode.Z;
parentNode.Id = 1;
mOpen.Push(parentNode);
while (mOpen.Count > 0 && !mStop)
{
parentNode = mOpen.Pop();
if (parentNode.X == end.X && parentNode.Y == end.Y && parentNode.Z == end.Z)
{
mClose.Add(parentNode);
found = true;
break;
}
if (mClose.Count > mSearchLimit)
{
mStopped = true;
return(null);
}
if (mPunishChangeDirection)
{
mHoriz = (parentNode.X - parentNode.PX);
}
Neighbor neighb = new Neighbor(Graph.Matrix[parentNode.X, parentNode.Y, parentNode.Z], Graph);
List <Node> neighbors = neighb.GetNeighbors();
//Lets calculate each successors
foreach (Node neighbor in neighbors)
{
PathFinderNode newNode;
newNode.X = neighbor.Location.X;
newNode.Y = neighbor.Location.Y;
newNode.Z = neighbor.Location.Z;
newNode.Id = Graph.Matrix[newNode.X, newNode.Y, newNode.Z];
if (newNode.X < 0 || newNode.Y < 0 || newNode.Z < 0 || newNode.X >= gridX || newNode.Y >= gridY || newNode.Z >= gridZ)
{
continue;
}
int newG;
//if (mHeavyDiagonals && i > 3)
// newG = parentNode.G + (int)(mGrid[newNode.X, newNode.Y, newNode.Z] * 2.41);
//else
//newG = parentNode.G + mGrid[newNode.X, newNode.Y, newNode.Z];
newG = parentNode.G + neighbor.LengthToBase;
//check obstacles
if (newG == 1)
{
continue;
}
if (mPunishChangeDirection)
{
if ((newNode.X - parentNode.X) != 0)
{
if (mHoriz == 0)
{
newG += 20;
}
}
if ((newNode.Y - parentNode.Y) != 0)
{
if (mHoriz != 0)
{
newG += 20;
}
}
}
int foundInOpenIndex = -1;
for (int j = 0; j < mOpen.Count; j++)
{
if (mOpen[j].X == newNode.X && mOpen[j].Y == newNode.Y && mOpen[j].Z == newNode.Z)
{
foundInOpenIndex = j;
break;
}
}
if (foundInOpenIndex != -1 && mOpen[foundInOpenIndex].G <= newG)
{
continue;
}
int foundInCloseIndex = -1;
for (int j = 0; j < mClose.Count; j++)
{
if (mClose[j].X == newNode.X && mClose[j].Y == newNode.Y && mClose[j].Z == newNode.Z)
{
foundInCloseIndex = j;
break;
}
}
if (foundInCloseIndex != -1 && (mReopenCloseNodes || mClose[foundInCloseIndex].G <= newG))
{
continue;
}
newNode.PX = parentNode.X;
newNode.PY = parentNode.Y;
newNode.PZ = parentNode.Z;
newNode.G = newG;
switch (mFormula)
{
default:
case HeuristicFormula.Manhattan:
newNode.H = mHEstimate * (Math.Abs(newNode.X - end.X) + Math.Abs(newNode.Y - end.Y) + Math.Abs(newNode.Z - end.Z));
break;
case HeuristicFormula.MaxDXDY:
newNode.H = mHEstimate * (Math.Max(Math.Abs(newNode.X - end.X), Math.Abs(newNode.Y - end.Y)));
break;
case HeuristicFormula.DiagonalShortCut:
int h_diagonal = Math.Min(Math.Abs(newNode.X - end.X), Math.Abs(newNode.Y - end.Y));
int h_straight = (Math.Abs(newNode.X - end.X) + Math.Abs(newNode.Y - end.Y));
newNode.H = (mHEstimate * 2) * h_diagonal + mHEstimate * (h_straight - 2 * h_diagonal);
break;
case HeuristicFormula.Euclidean:
newNode.H = (int)(mHEstimate * Math.Sqrt(Math.Pow((newNode.X - end.X), 2) + Math.Pow((newNode.Y - end.Y), 2)));
break;
case HeuristicFormula.EuclideanNoSQR:
newNode.H = (int)(mHEstimate * (Math.Pow((newNode.X - end.X), 2) + Math.Pow((newNode.Y - end.Y), 2)));
break;
case HeuristicFormula.Custom1:
Point dxy = new Point(Math.Abs(end.X - newNode.X), Math.Abs(end.Y - newNode.Y));
int Orthogonal = Math.Abs(dxy.X - dxy.Y);
int Diagonal = Math.Abs(((dxy.X + dxy.Y) - Orthogonal) / 2);
newNode.H = mHEstimate * (Diagonal + Orthogonal + dxy.X + dxy.Y);
break;
}
if (mTieBreaker)
{
int dx1 = parentNode.X - end.X;
int dy1 = parentNode.Y - end.Y;
int dz1 = parentNode.Z - end.Z;
int dx2 = start.X - end.X;
int dy2 = start.Y - end.Y;
int dz2 = start.Z - end.Z;
int cross = Math.Abs(dx1 * dy2 - dx2 * dy1); //fix z here
newNode.H = (int)(newNode.H + cross * 0.001);
}
newNode.F = newNode.G + newNode.H;
mOpen.Push(newNode);
}
mClose.Add(parentNode);
}
if (found)
{
PathFinderNode fNode = mClose[mClose.Count - 1];
for (int i = mClose.Count - 1; i >= 0; i--)
{
if (fNode.PX == mClose[i].X && fNode.PY == mClose[i].Y && fNode.PZ == mClose[i].Z || i == mClose.Count - 1)
{
fNode = mClose[i];
}
else
{
mClose.RemoveAt(i);
}
}
mStopped = true;
return(mClose);
}
mStopped = true;
return(null);
}
