3 hours later
This commit is contained in:
Simon Holmberg
Executable
+62
@@ -0,0 +1,62 @@
|
||||
<?xml version="1.0" encoding="utf-8"?>
|
||||
<Project ToolsVersion="4.0" DefaultTargets="Build" xmlns="http://schemas.microsoft.com/developer/msbuild/2003">
|
||||
<PropertyGroup>
|
||||
<Configuration Condition=" '$(Configuration)' == '' ">Debug</Configuration>
|
||||
<Platform Condition=" '$(Platform)' == '' ">x86</Platform>
|
||||
<ProductVersion>8.0.30703</ProductVersion>
|
||||
<SchemaVersion>2.0</SchemaVersion>
|
||||
<ProjectGuid>{CD3F949D-54AA-4D38-99DB-92905A375D84}</ProjectGuid>
|
||||
<OutputType>Library</OutputType>
|
||||
<AppDesignerFolder>Properties</AppDesignerFolder>
|
||||
<RootNamespace>AStar</RootNamespace>
|
||||
<AssemblyName>AStar</AssemblyName>
|
||||
<TargetFrameworkVersion>v4.0</TargetFrameworkVersion>
|
||||
<TargetFrameworkProfile>
|
||||
</TargetFrameworkProfile>
|
||||
<FileAlignment>512</FileAlignment>
|
||||
</PropertyGroup>
|
||||
<PropertyGroup Condition=" '$(Configuration)|$(Platform)' == 'Debug|x86' ">
|
||||
<PlatformTarget>x86</PlatformTarget>
|
||||
<DebugSymbols>true</DebugSymbols>
|
||||
<DebugType>full</DebugType>
|
||||
<Optimize>false</Optimize>
|
||||
<OutputPath>bin\Debug\</OutputPath>
|
||||
<DefineConstants>DEBUG;TRACE</DefineConstants>
|
||||
<ErrorReport>prompt</ErrorReport>
|
||||
<WarningLevel>4</WarningLevel>
|
||||
<AllowUnsafeBlocks>true</AllowUnsafeBlocks>
|
||||
</PropertyGroup>
|
||||
<PropertyGroup Condition=" '$(Configuration)|$(Platform)' == 'Release|x86' ">
|
||||
<PlatformTarget>x86</PlatformTarget>
|
||||
<DebugType>pdbonly</DebugType>
|
||||
<Optimize>true</Optimize>
|
||||
<OutputPath>bin\Release\</OutputPath>
|
||||
<DefineConstants>TRACE</DefineConstants>
|
||||
<ErrorReport>prompt</ErrorReport>
|
||||
<WarningLevel>4</WarningLevel>
|
||||
<AllowUnsafeBlocks>true</AllowUnsafeBlocks>
|
||||
</PropertyGroup>
|
||||
<PropertyGroup>
|
||||
<StartupObject />
|
||||
</PropertyGroup>
|
||||
<ItemGroup>
|
||||
<Compile Include="AuthorAttribute.cs" />
|
||||
<Compile Include="HighResolutionTime.cs" />
|
||||
<Compile Include="IPathFinder.cs" />
|
||||
<Compile Include="PathFinder.cs" />
|
||||
<Compile Include="PathFinderFast.cs" />
|
||||
<Compile Include="PriorityQueueB.cs" />
|
||||
</ItemGroup>
|
||||
<ItemGroup>
|
||||
<Reference Include="System" />
|
||||
<Reference Include="System.Drawing" />
|
||||
</ItemGroup>
|
||||
<Import Project="$(MSBuildToolsPath)\Microsoft.CSharp.targets" />
|
||||
<!-- To modify your build process, add your task inside one of the targets below and uncomment it.
|
||||
Other similar extension points exist, see Microsoft.Common.targets.
|
||||
<Target Name="BeforeBuild">
|
||||
</Target>
|
||||
<Target Name="AfterBuild">
|
||||
</Target>
|
||||
-->
|
||||
</Project>
|
||||
Executable
+24
@@ -0,0 +1,24 @@
|
||||
//
|
||||
// THIS CODE AND INFORMATION IS PROVIDED "AS IS" WITHOUT WARRANTY OF ANY
|
||||
// KIND, EITHER EXPRESSED OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE
|
||||
// IMPLIED WARRANTIES OF MERCHANTABILITY AND/OR FITNESS FOR A PARTICULAR
|
||||
// PURPOSE. IT CAN BE DISTRIBUTED FREE OF CHARGE AS LONG AS THIS HEADER
|
||||
// REMAINS UNCHANGED.
|
||||
//
|
||||
// Email: gustavo_franco@hotmail.com
|
||||
//
|
||||
// Copyright (C) 2006 Franco, Gustavo
|
||||
//
|
||||
using System;
|
||||
|
||||
namespace AStar
|
||||
{
|
||||
internal class AuthorAttribute : Attribute
|
||||
{
|
||||
#region Constructors
|
||||
public AuthorAttribute(string authorName)
|
||||
{
|
||||
}
|
||||
#endregion
|
||||
}
|
||||
}
|
||||
Executable
+45
@@ -0,0 +1,45 @@
|
||||
using System;
|
||||
using System.Collections.Generic;
|
||||
using System.Text;
|
||||
|
||||
namespace AStar
|
||||
{
|
||||
public static class HighResolutionTime
|
||||
{
|
||||
#region Win32APIs
|
||||
[System.Runtime.InteropServices.DllImport("Kernel32.dll")]
|
||||
private static extern bool QueryPerformanceCounter(out long perfcount);
|
||||
|
||||
[System.Runtime.InteropServices.DllImport("Kernel32.dll")]
|
||||
private static extern bool QueryPerformanceFrequency(out long freq);
|
||||
#endregion
|
||||
|
||||
#region Variables Declaration
|
||||
private static long mStartCounter;
|
||||
private static long mFrequency;
|
||||
#endregion
|
||||
|
||||
#region Constuctors
|
||||
static HighResolutionTime()
|
||||
{
|
||||
QueryPerformanceFrequency(out mFrequency);
|
||||
}
|
||||
#endregion
|
||||
|
||||
#region Methods
|
||||
public static void Start()
|
||||
{
|
||||
QueryPerformanceCounter(out mStartCounter);
|
||||
}
|
||||
|
||||
public static double GetTime()
|
||||
{
|
||||
long endCounter;
|
||||
QueryPerformanceCounter(out endCounter);
|
||||
long elapsed = endCounter - mStartCounter;
|
||||
return (double) elapsed / mFrequency;
|
||||
}
|
||||
#endregion
|
||||
}
|
||||
}
|
||||
|
||||
Executable
+89
@@ -0,0 +1,89 @@
|
||||
using System;
|
||||
using System.Collections.Generic;
|
||||
using System.Text;
|
||||
using AStar;
|
||||
using System.Drawing;
|
||||
|
||||
namespace AStar
|
||||
{
|
||||
[Author("Franco, Gustavo")]
|
||||
interface IPathFinder
|
||||
{
|
||||
#region Events
|
||||
event PathFinderDebugHandler PathFinderDebug;
|
||||
#endregion
|
||||
|
||||
#region Properties
|
||||
bool Stopped
|
||||
{
|
||||
get;
|
||||
}
|
||||
|
||||
HeuristicFormula Formula
|
||||
{
|
||||
get;
|
||||
set;
|
||||
}
|
||||
|
||||
bool Diagonals
|
||||
{
|
||||
get;
|
||||
set;
|
||||
}
|
||||
|
||||
bool HeavyDiagonals
|
||||
{
|
||||
get;
|
||||
set;
|
||||
}
|
||||
|
||||
int HeuristicEstimate
|
||||
{
|
||||
get;
|
||||
set;
|
||||
}
|
||||
|
||||
bool PunishChangeDirection
|
||||
{
|
||||
get;
|
||||
set;
|
||||
}
|
||||
|
||||
bool TieBreaker
|
||||
{
|
||||
get;
|
||||
set;
|
||||
}
|
||||
|
||||
int SearchLimit
|
||||
{
|
||||
get;
|
||||
set;
|
||||
}
|
||||
|
||||
double CompletedTime
|
||||
{
|
||||
get;
|
||||
set;
|
||||
}
|
||||
|
||||
bool DebugProgress
|
||||
{
|
||||
get;
|
||||
set;
|
||||
}
|
||||
|
||||
bool DebugFoundPath
|
||||
{
|
||||
get;
|
||||
set;
|
||||
}
|
||||
#endregion
|
||||
|
||||
#region Methods
|
||||
void FindPathStop();
|
||||
List<PathFinderNode> FindPath(Point start, Point end);
|
||||
#endregion
|
||||
|
||||
}
|
||||
}
|
||||
Executable
+404
@@ -0,0 +1,404 @@
|
||||
//
|
||||
// THIS CODE AND INFORMATION IS PROVIDED "AS IS" WITHOUT WARRANTY OF ANY
|
||||
// KIND, EITHER EXPRESSED OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE
|
||||
// IMPLIED WARRANTIES OF MERCHANTABILITY AND/OR FITNESS FOR A PARTICULAR
|
||||
// PURPOSE. IT CAN BE DISTRIBUTED FREE OF CHARGE AS LONG AS THIS HEADER
|
||||
// REMAINS UNCHANGED.
|
||||
//
|
||||
// Email: gustavo_franco@hotmail.com
|
||||
//
|
||||
// Copyright (C) 2006 Franco, Gustavo
|
||||
//
|
||||
//#define DEBUGON
|
||||
|
||||
using System;
|
||||
using System.Drawing;
|
||||
using System.Collections.Generic;
|
||||
|
||||
namespace AStar
|
||||
{
|
||||
#region Structs
|
||||
[Author("Franco, Gustavo")]
|
||||
public struct PathFinderNode
|
||||
{
|
||||
#region Variables Declaration
|
||||
public int F;
|
||||
public int G;
|
||||
public int H; // f = gone + heuristic
|
||||
public int X;
|
||||
public int Y;
|
||||
public int PX; // Parent
|
||||
public int PY;
|
||||
#endregion
|
||||
}
|
||||
#endregion
|
||||
|
||||
#region Enum
|
||||
[Author("Franco, Gustavo")]
|
||||
public enum PathFinderNodeType
|
||||
{
|
||||
Start = 1,
|
||||
End = 2,
|
||||
Open = 4,
|
||||
Close = 8,
|
||||
Current = 16,
|
||||
Path = 32
|
||||
}
|
||||
|
||||
public enum HeuristicFormula
|
||||
{
|
||||
Manhattan = 1,
|
||||
MaxDXDY = 2,
|
||||
DiagonalShortCut = 3,
|
||||
Euclidean = 4,
|
||||
EuclideanNoSQR = 5,
|
||||
Custom1 = 6
|
||||
}
|
||||
#endregion
|
||||
|
||||
#region Delegates
|
||||
public delegate void PathFinderDebugHandler(int fromX, int fromY, int x, int y, PathFinderNodeType type, int totalCost, int cost);
|
||||
#endregion
|
||||
|
||||
[Author("Franco, Gustavo")]
|
||||
public class PathFinder : IPathFinder
|
||||
{
|
||||
[System.Runtime.InteropServices.DllImport("KERNEL32.DLL", EntryPoint="RtlZeroMemory")]
|
||||
public unsafe static extern bool ZeroMemory(byte* destination, int length);
|
||||
|
||||
#region Events
|
||||
public event PathFinderDebugHandler PathFinderDebug;
|
||||
#endregion
|
||||
|
||||
#region Variables Declaration
|
||||
private byte[,] mGrid = null;
|
||||
private PriorityQueueB<PathFinderNode> mOpen = new PriorityQueueB<PathFinderNode>(new ComparePFNode());
|
||||
private List<PathFinderNode> mClose = new List<PathFinderNode>();
|
||||
private bool mStop = false;
|
||||
private bool mStopped = true;
|
||||
private int mHoriz = 0;
|
||||
private HeuristicFormula mFormula = HeuristicFormula.Manhattan;
|
||||
private bool mDiagonals = true;
|
||||
private int mHEstimate = 2;
|
||||
private bool mPunishChangeDirection = false;
|
||||
private bool mTieBreaker = false;
|
||||
private bool mHeavyDiagonals = false;
|
||||
private int mSearchLimit = 2000;
|
||||
private double mCompletedTime = 0;
|
||||
private bool mDebugProgress = false;
|
||||
private bool mDebugFoundPath = false;
|
||||
#endregion
|
||||
|
||||
#region Constructors
|
||||
public PathFinder(byte[,] grid)
|
||||
{
|
||||
if (grid == null)
|
||||
throw new Exception("Grid cannot be null");
|
||||
|
||||
mGrid = grid;
|
||||
}
|
||||
#endregion
|
||||
|
||||
#region Properties
|
||||
public bool Stopped
|
||||
{
|
||||
get { return mStopped; }
|
||||
}
|
||||
|
||||
public HeuristicFormula Formula
|
||||
{
|
||||
get { return mFormula; }
|
||||
set { mFormula = value; }
|
||||
}
|
||||
|
||||
public bool Diagonals
|
||||
{
|
||||
get { return mDiagonals; }
|
||||
set { mDiagonals = value; }
|
||||
}
|
||||
|
||||
public bool HeavyDiagonals
|
||||
{
|
||||
get { return mHeavyDiagonals; }
|
||||
set { mHeavyDiagonals = value; }
|
||||
}
|
||||
|
||||
public int HeuristicEstimate
|
||||
{
|
||||
get { return mHEstimate; }
|
||||
set { mHEstimate = value; }
|
||||
}
|
||||
|
||||
public bool PunishChangeDirection
|
||||
{
|
||||
get { return mPunishChangeDirection; }
|
||||
set { mPunishChangeDirection = value; }
|
||||
}
|
||||
|
||||
public bool TieBreaker
|
||||
{
|
||||
get { return mTieBreaker; }
|
||||
set { mTieBreaker = value; }
|
||||
}
|
||||
|
||||
public int SearchLimit
|
||||
{
|
||||
get { return mSearchLimit; }
|
||||
set { mSearchLimit = value; }
|
||||
}
|
||||
|
||||
public double CompletedTime
|
||||
{
|
||||
get { return mCompletedTime; }
|
||||
set { mCompletedTime = value; }
|
||||
}
|
||||
|
||||
public bool DebugProgress
|
||||
{
|
||||
get { return mDebugProgress; }
|
||||
set { mDebugProgress = value; }
|
||||
}
|
||||
|
||||
public bool DebugFoundPath
|
||||
{
|
||||
get { return mDebugFoundPath; }
|
||||
set { mDebugFoundPath = value; }
|
||||
}
|
||||
#endregion
|
||||
|
||||
#region Methods
|
||||
public void FindPathStop()
|
||||
{
|
||||
mStop = true;
|
||||
}
|
||||
|
||||
public List<PathFinderNode> FindPath(Point start, Point end)
|
||||
{
|
||||
HighResolutionTime.Start();
|
||||
|
||||
PathFinderNode parentNode;
|
||||
bool found = false;
|
||||
int gridX = mGrid.GetUpperBound(0);
|
||||
int gridY = mGrid.GetUpperBound(1);
|
||||
|
||||
mStop = false;
|
||||
mStopped = false;
|
||||
mOpen.Clear();
|
||||
mClose.Clear();
|
||||
|
||||
#if DEBUGON
|
||||
if (mDebugProgress && PathFinderDebug != null)
|
||||
PathFinderDebug(0, 0, start.X, start.Y, PathFinderNodeType.Start, -1, -1);
|
||||
if (mDebugProgress && PathFinderDebug != null)
|
||||
PathFinderDebug(0, 0, end.X, end.Y, PathFinderNodeType.End, -1, -1);
|
||||
#endif
|
||||
|
||||
sbyte[,] direction;
|
||||
if (mDiagonals)
|
||||
direction = new sbyte[8,2]{ {0,-1} , {1,0}, {0,1}, {-1,0}, {1,-1}, {1,1}, {-1,1}, {-1,-1}};
|
||||
else
|
||||
direction = new sbyte[4,2]{ {0,-1} , {1,0}, {0,1}, {-1,0}};
|
||||
|
||||
parentNode.G = 0;
|
||||
parentNode.H = mHEstimate;
|
||||
parentNode.F = parentNode.G + parentNode.H;
|
||||
parentNode.X = start.X;
|
||||
parentNode.Y = start.Y;
|
||||
parentNode.PX = parentNode.X;
|
||||
parentNode.PY = parentNode.Y;
|
||||
mOpen.Push(parentNode);
|
||||
while(mOpen.Count > 0 && !mStop)
|
||||
{
|
||||
parentNode = mOpen.Pop();
|
||||
|
||||
#if DEBUGON
|
||||
if (mDebugProgress && PathFinderDebug != null)
|
||||
PathFinderDebug(0, 0, parentNode.X, parentNode.Y, PathFinderNodeType.Current, -1, -1);
|
||||
#endif
|
||||
|
||||
if (parentNode.X == end.X && parentNode.Y == end.Y)
|
||||
{
|
||||
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 : 4); i++)
|
||||
{
|
||||
PathFinderNode newNode;
|
||||
newNode.X = parentNode.X + direction[i,0];
|
||||
newNode.Y = parentNode.Y + direction[i,1];
|
||||
|
||||
if (newNode.X < 0 || newNode.Y < 0 || newNode.X >= gridX || newNode.Y >= gridY)
|
||||
continue;
|
||||
|
||||
int newG;
|
||||
if (mHeavyDiagonals && i>3)
|
||||
newG = parentNode.G + (int) (mGrid[newNode.X, newNode.Y] * 2.41);
|
||||
else
|
||||
newG = parentNode.G + mGrid[newNode.X, newNode.Y];
|
||||
|
||||
|
||||
if (newG == parentNode.G)
|
||||
{
|
||||
//Unbrekeable
|
||||
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)
|
||||
{
|
||||
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)
|
||||
{
|
||||
foundInCloseIndex = j;
|
||||
break;
|
||||
}
|
||||
}
|
||||
if (foundInCloseIndex != -1 && mClose[foundInCloseIndex].G <= newG)
|
||||
continue;
|
||||
|
||||
newNode.PX = parentNode.X;
|
||||
newNode.PY = parentNode.Y;
|
||||
newNode.G = newG;
|
||||
|
||||
switch(mFormula)
|
||||
{
|
||||
default:
|
||||
case HeuristicFormula.Manhattan:
|
||||
newNode.H = mHEstimate * (Math.Abs(newNode.X - end.X) + Math.Abs(newNode.Y - end.Y));
|
||||
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 dx2 = start.X - end.X;
|
||||
int dy2 = start.Y - end.Y;
|
||||
int cross = Math.Abs(dx1 * dy2 - dx2 * dy1);
|
||||
newNode.H = (int) (newNode.H + cross * 0.001);
|
||||
}
|
||||
newNode.F = newNode.G + newNode.H;
|
||||
|
||||
#if DEBUGON
|
||||
if (mDebugProgress && PathFinderDebug != null)
|
||||
PathFinderDebug(parentNode.X, parentNode.Y, newNode.X, newNode.Y, PathFinderNodeType.Open, newNode.F, newNode.G);
|
||||
#endif
|
||||
|
||||
|
||||
//It is faster if we leave the open node in the priority queue
|
||||
//When it is removed, all nodes around will be closed, it will be ignored automatically
|
||||
//if (foundInOpenIndex != -1)
|
||||
// mOpen.RemoveAt(foundInOpenIndex);
|
||||
|
||||
//if (foundInOpenIndex == -1)
|
||||
mOpen.Push(newNode);
|
||||
}
|
||||
|
||||
mClose.Add(parentNode);
|
||||
|
||||
#if DEBUGON
|
||||
if (mDebugProgress && PathFinderDebug != null)
|
||||
PathFinderDebug(0, 0, parentNode.X, parentNode.Y, PathFinderNodeType.Close, parentNode.F, parentNode.G);
|
||||
#endif
|
||||
}
|
||||
|
||||
mCompletedTime = HighResolutionTime.GetTime();
|
||||
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 || i == mClose.Count - 1)
|
||||
{
|
||||
#if DEBUGON
|
||||
if (mDebugFoundPath && PathFinderDebug != null)
|
||||
PathFinderDebug(fNode.X, fNode.Y, mClose[i].X, mClose[i].Y, PathFinderNodeType.Path, mClose[i].F, mClose[i].G);
|
||||
#endif
|
||||
fNode = mClose[i];
|
||||
}
|
||||
else
|
||||
mClose.RemoveAt(i);
|
||||
}
|
||||
mStopped = true;
|
||||
return mClose;
|
||||
}
|
||||
mStopped = true;
|
||||
return null;
|
||||
}
|
||||
#endregion
|
||||
|
||||
#region Inner Classes
|
||||
[Author("Franco, Gustavo")]
|
||||
internal class ComparePFNode : IComparer<PathFinderNode>
|
||||
{
|
||||
#region IComparer Members
|
||||
public int Compare(PathFinderNode x, PathFinderNode y)
|
||||
{
|
||||
if (x.F > y.F)
|
||||
return 1;
|
||||
else if (x.F < y.F)
|
||||
return -1;
|
||||
return 0;
|
||||
}
|
||||
#endregion
|
||||
}
|
||||
#endregion
|
||||
}
|
||||
}
|
||||
Executable
+460
@@ -0,0 +1,460 @@
|
||||
//
|
||||
// THIS CODE AND INFORMATION IS PROVIDED "AS IS" WITHOUT WARRANTY OF ANY
|
||||
// KIND, EITHER EXPRESSED OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE
|
||||
// IMPLIED WARRANTIES OF MERCHANTABILITY AND/OR FITNESS FOR A PARTICULAR
|
||||
// PURPOSE. IT CAN BE DISTRIBUTED FREE OF CHARGE AS LONG AS THIS HEADER
|
||||
// REMAINS UNCHANGED.
|
||||
//
|
||||
// Email: gustavo_franco@hotmail.com
|
||||
//
|
||||
// Copyright (C) 2006 Franco, Gustavo
|
||||
//
|
||||
#define DEBUGON
|
||||
|
||||
using System;
|
||||
using System.Text;
|
||||
using System.Drawing;
|
||||
using System.Threading;
|
||||
using System.Collections;
|
||||
|
||||
using System.Drawing.Drawing2D;
|
||||
using System.Collections.Generic;
|
||||
using System.Runtime.InteropServices;
|
||||
using AStar;
|
||||
|
||||
namespace AStar
|
||||
{
|
||||
[Author("Franco, Gustavo")]
|
||||
public class PathFinderFast : IPathFinder
|
||||
{
|
||||
#region Structs
|
||||
[Author("Franco, Gustavo")]
|
||||
[StructLayout(LayoutKind.Sequential, Pack=1)]
|
||||
internal struct PathFinderNodeFast
|
||||
{
|
||||
#region Variables Declaration
|
||||
public int F; // f = gone + heuristic
|
||||
public int G;
|
||||
public ushort PX; // Parent
|
||||
public ushort PY;
|
||||
public byte Status;
|
||||
#endregion
|
||||
}
|
||||
#endregion
|
||||
|
||||
#region Win32APIs
|
||||
[System.Runtime.InteropServices.DllImport("KERNEL32.DLL", EntryPoint="RtlZeroMemory")]
|
||||
public unsafe static extern bool ZeroMemory(byte* destination, int length);
|
||||
#endregion
|
||||
|
||||
#region Events
|
||||
public event PathFinderDebugHandler PathFinderDebug;
|
||||
#endregion
|
||||
|
||||
#region Variables Declaration
|
||||
// Heap variables are initializated to default, but I like to do it anyway
|
||||
private byte[,] mGrid = null;
|
||||
private PriorityQueueB<int> mOpen = null;
|
||||
private List<PathFinderNode> mClose = new List<PathFinderNode>();
|
||||
private bool mStop = false;
|
||||
private bool mStopped = true;
|
||||
private int mHoriz = 0;
|
||||
private HeuristicFormula mFormula = HeuristicFormula.Manhattan;
|
||||
private bool mDiagonals = true;
|
||||
private int mHEstimate = 2;
|
||||
private bool mPunishChangeDirection = false;
|
||||
private bool mTieBreaker = false;
|
||||
private bool mHeavyDiagonals = false;
|
||||
private int mSearchLimit = 2000;
|
||||
private double mCompletedTime = 0;
|
||||
private bool mDebugProgress = false;
|
||||
private bool mDebugFoundPath = false;
|
||||
private PathFinderNodeFast[] mCalcGrid = null;
|
||||
private byte mOpenNodeValue = 1;
|
||||
private byte mCloseNodeValue = 2;
|
||||
|
||||
//Promoted local variables to member variables to avoid recreation between calls
|
||||
private int mH = 0;
|
||||
private int mLocation = 0;
|
||||
private int mNewLocation = 0;
|
||||
private ushort mLocationX = 0;
|
||||
private ushort mLocationY = 0;
|
||||
private ushort mNewLocationX = 0;
|
||||
private ushort mNewLocationY = 0;
|
||||
private int mCloseNodeCounter = 0;
|
||||
private ushort mGridX = 0;
|
||||
private ushort mGridY = 0;
|
||||
private ushort mGridXMinus1 = 0;
|
||||
private ushort mGridYLog2 = 0;
|
||||
private bool mFound = false;
|
||||
private sbyte[,] mDirection = new sbyte[8,2]{{0,-1} , {1,0}, {0,1}, {-1,0}, {1,-1}, {1,1}, {-1,1}, {-1,-1}};
|
||||
private int mEndLocation = 0;
|
||||
private int mNewG = 0;
|
||||
#endregion
|
||||
|
||||
#region Constructors
|
||||
public PathFinderFast(byte[,] grid)
|
||||
{
|
||||
if (grid == null)
|
||||
throw new Exception("Grid cannot be null");
|
||||
|
||||
mGrid = grid;
|
||||
mGridX = (ushort) (mGrid.GetUpperBound(0) + 1);
|
||||
mGridY = (ushort) (mGrid.GetUpperBound(1) + 1);
|
||||
mGridXMinus1 = (ushort) (mGridX - 1);
|
||||
mGridYLog2 = (ushort) Math.Log(mGridY, 2);
|
||||
|
||||
// This should be done at the constructor, for now we leave it here.
|
||||
if (Math.Log(mGridX, 2) != (int) Math.Log(mGridX, 2) ||
|
||||
Math.Log(mGridY, 2) != (int) Math.Log(mGridY, 2))
|
||||
throw new Exception("Invalid Grid, size in X and Y must be power of 2");
|
||||
|
||||
if (mCalcGrid == null || mCalcGrid.Length != (mGridX * mGridY))
|
||||
mCalcGrid = new PathFinderNodeFast[mGridX * mGridY];
|
||||
|
||||
mOpen = new PriorityQueueB<int>(new ComparePFNodeMatrix(mCalcGrid));
|
||||
}
|
||||
#endregion
|
||||
|
||||
#region Properties
|
||||
public bool Stopped
|
||||
{
|
||||
get { return mStopped; }
|
||||
}
|
||||
|
||||
public HeuristicFormula Formula
|
||||
{
|
||||
get { return mFormula; }
|
||||
set { mFormula = value; }
|
||||
}
|
||||
|
||||
public bool Diagonals
|
||||
{
|
||||
get { return mDiagonals; }
|
||||
set
|
||||
{
|
||||
mDiagonals = value;
|
||||
if (mDiagonals)
|
||||
mDirection = new sbyte[8,2]{{0,-1} , {1,0}, {0,1}, {-1,0}, {1,-1}, {1,1}, {-1,1}, {-1,-1}};
|
||||
else
|
||||
mDirection = new sbyte[4,2]{{0,-1} , {1,0}, {0,1}, {-1,0}};
|
||||
}
|
||||
}
|
||||
|
||||
public bool HeavyDiagonals
|
||||
{
|
||||
get { return mHeavyDiagonals; }
|
||||
set { mHeavyDiagonals = value; }
|
||||
}
|
||||
|
||||
public int HeuristicEstimate
|
||||
{
|
||||
get { return mHEstimate; }
|
||||
set { mHEstimate = value; }
|
||||
}
|
||||
|
||||
public bool PunishChangeDirection
|
||||
{
|
||||
get { return mPunishChangeDirection; }
|
||||
set { mPunishChangeDirection = value; }
|
||||
}
|
||||
|
||||
public bool TieBreaker
|
||||
{
|
||||
get { return mTieBreaker; }
|
||||
set { mTieBreaker = value; }
|
||||
}
|
||||
|
||||
public int SearchLimit
|
||||
{
|
||||
get { return mSearchLimit; }
|
||||
set { mSearchLimit = value; }
|
||||
}
|
||||
|
||||
public double CompletedTime
|
||||
{
|
||||
get { return mCompletedTime; }
|
||||
set { mCompletedTime = value; }
|
||||
}
|
||||
|
||||
public bool DebugProgress
|
||||
{
|
||||
get { return mDebugProgress; }
|
||||
set { mDebugProgress = value; }
|
||||
}
|
||||
|
||||
public bool DebugFoundPath
|
||||
{
|
||||
get { return mDebugFoundPath; }
|
||||
set { mDebugFoundPath = value; }
|
||||
}
|
||||
#endregion
|
||||
|
||||
#region Methods
|
||||
public void FindPathStop()
|
||||
{
|
||||
mStop = true;
|
||||
}
|
||||
|
||||
public List<PathFinderNode> FindPath(Point start, Point end)
|
||||
{
|
||||
lock(this)
|
||||
{
|
||||
HighResolutionTime.Start();
|
||||
|
||||
// Is faster if we don't clear the matrix, just assign different values for open and close and ignore the rest
|
||||
// I could have user Array.Clear() but using unsafe code is faster, no much but it is.
|
||||
//fixed (PathFinderNodeFast* pGrid = tmpGrid)
|
||||
// ZeroMemory((byte*) pGrid, sizeof(PathFinderNodeFast) * 1000000);
|
||||
|
||||
mFound = false;
|
||||
mStop = false;
|
||||
mStopped = false;
|
||||
mCloseNodeCounter = 0;
|
||||
mOpenNodeValue += 2;
|
||||
mCloseNodeValue += 2;
|
||||
mOpen.Clear();
|
||||
mClose.Clear();
|
||||
|
||||
#if DEBUGON
|
||||
if (mDebugProgress && PathFinderDebug != null)
|
||||
PathFinderDebug(0, 0, start.X, start.Y, PathFinderNodeType.Start, -1, -1);
|
||||
if (mDebugProgress && PathFinderDebug != null)
|
||||
PathFinderDebug(0, 0, end.X, end.Y, PathFinderNodeType.End, -1, -1);
|
||||
#endif
|
||||
|
||||
mLocation = (start.Y << mGridYLog2) + start.X;
|
||||
mEndLocation = (end.Y << mGridYLog2) + end.X;
|
||||
mCalcGrid[mLocation].G = 0;
|
||||
mCalcGrid[mLocation].F = mHEstimate;
|
||||
mCalcGrid[mLocation].PX = (ushort) start.X;
|
||||
mCalcGrid[mLocation].PY = (ushort) start.Y;
|
||||
mCalcGrid[mLocation].Status = mOpenNodeValue;
|
||||
|
||||
mOpen.Push(mLocation);
|
||||
while(mOpen.Count > 0 && !mStop)
|
||||
{
|
||||
mLocation = mOpen.Pop();
|
||||
|
||||
//Is it in closed list? means this node was already processed
|
||||
if (mCalcGrid[mLocation].Status == mCloseNodeValue)
|
||||
continue;
|
||||
|
||||
mLocationX = (ushort) (mLocation & mGridXMinus1);
|
||||
mLocationY = (ushort) (mLocation >> mGridYLog2);
|
||||
|
||||
#if DEBUGON
|
||||
if (mDebugProgress && PathFinderDebug != null)
|
||||
PathFinderDebug(0, 0, mLocation & mGridXMinus1, mLocation >> mGridYLog2, PathFinderNodeType.Current, -1, -1);
|
||||
#endif
|
||||
|
||||
if (mLocation == mEndLocation)
|
||||
{
|
||||
mCalcGrid[mLocation].Status = mCloseNodeValue;
|
||||
mFound = true;
|
||||
break;
|
||||
}
|
||||
|
||||
if (mCloseNodeCounter > mSearchLimit)
|
||||
{
|
||||
mStopped = true;
|
||||
mCompletedTime = HighResolutionTime.GetTime();
|
||||
return null;
|
||||
}
|
||||
|
||||
if (mPunishChangeDirection)
|
||||
mHoriz = (mLocationX - mCalcGrid[mLocation].PX);
|
||||
|
||||
//Lets calculate each successors
|
||||
for (int i=0; i<(mDiagonals ? 8 : 4); i++)
|
||||
{
|
||||
mNewLocationX = (ushort) (mLocationX + mDirection[i,0]);
|
||||
mNewLocationY = (ushort) (mLocationY + mDirection[i,1]);
|
||||
mNewLocation = (mNewLocationY << mGridYLog2) + mNewLocationX;
|
||||
|
||||
if (mNewLocationX >= mGridX || mNewLocationY >= mGridY)
|
||||
continue;
|
||||
|
||||
// Unbreakeable?
|
||||
if (mGrid[mNewLocationX, mNewLocationY] == 0)
|
||||
continue;
|
||||
|
||||
if (mHeavyDiagonals && i>3)
|
||||
mNewG = mCalcGrid[mLocation].G + (int) (mGrid[mNewLocationX, mNewLocationY] * 2.41);
|
||||
else
|
||||
mNewG = mCalcGrid[mLocation].G + mGrid[mNewLocationX, mNewLocationY];
|
||||
|
||||
if (mPunishChangeDirection)
|
||||
{
|
||||
if ((mNewLocationX - mLocationX) != 0)
|
||||
{
|
||||
if (mHoriz == 0)
|
||||
mNewG += Math.Abs(mNewLocationX - end.X) + Math.Abs(mNewLocationY - end.Y);
|
||||
}
|
||||
if ((mNewLocationY - mLocationY) != 0)
|
||||
{
|
||||
if (mHoriz != 0)
|
||||
mNewG += Math.Abs(mNewLocationX - end.X) + Math.Abs(mNewLocationY - end.Y);
|
||||
}
|
||||
}
|
||||
|
||||
//Is it open or closed?
|
||||
if (mCalcGrid[mNewLocation].Status == mOpenNodeValue || mCalcGrid[mNewLocation].Status == mCloseNodeValue)
|
||||
{
|
||||
// The current node has less code than the previous? then skip this node
|
||||
if (mCalcGrid[mNewLocation].G <= mNewG)
|
||||
continue;
|
||||
}
|
||||
|
||||
mCalcGrid[mNewLocation].PX = mLocationX;
|
||||
mCalcGrid[mNewLocation].PY = mLocationY;
|
||||
mCalcGrid[mNewLocation].G = mNewG;
|
||||
|
||||
switch(mFormula)
|
||||
{
|
||||
default:
|
||||
case HeuristicFormula.Manhattan:
|
||||
mH = mHEstimate * (Math.Abs(mNewLocationX - end.X) + Math.Abs(mNewLocationY - end.Y));
|
||||
break;
|
||||
case HeuristicFormula.MaxDXDY:
|
||||
mH = mHEstimate * (Math.Max(Math.Abs(mNewLocationX - end.X), Math.Abs(mNewLocationY - end.Y)));
|
||||
break;
|
||||
case HeuristicFormula.DiagonalShortCut:
|
||||
int h_diagonal = Math.Min(Math.Abs(mNewLocationX - end.X), Math.Abs(mNewLocationY - end.Y));
|
||||
int h_straight = (Math.Abs(mNewLocationX - end.X) + Math.Abs(mNewLocationY - end.Y));
|
||||
mH = (mHEstimate * 2) * h_diagonal + mHEstimate * (h_straight - 2 * h_diagonal);
|
||||
break;
|
||||
case HeuristicFormula.Euclidean:
|
||||
mH = (int) (mHEstimate * Math.Sqrt(Math.Pow((mNewLocationY - end.X) , 2) + Math.Pow((mNewLocationY - end.Y), 2)));
|
||||
break;
|
||||
case HeuristicFormula.EuclideanNoSQR:
|
||||
mH = (int) (mHEstimate * (Math.Pow((mNewLocationX - end.X) , 2) + Math.Pow((mNewLocationY - end.Y), 2)));
|
||||
break;
|
||||
case HeuristicFormula.Custom1:
|
||||
Point dxy = new Point(Math.Abs(end.X - mNewLocationX), Math.Abs(end.Y - mNewLocationY));
|
||||
int Orthogonal = Math.Abs(dxy.X - dxy.Y);
|
||||
int Diagonal = Math.Abs(((dxy.X + dxy.Y) - Orthogonal) / 2);
|
||||
mH = mHEstimate * (Diagonal + Orthogonal + dxy.X + dxy.Y);
|
||||
break;
|
||||
}
|
||||
if (mTieBreaker)
|
||||
{
|
||||
int dx1 = mLocationX - end.X;
|
||||
int dy1 = mLocationY - end.Y;
|
||||
int dx2 = start.X - end.X;
|
||||
int dy2 = start.Y - end.Y;
|
||||
int cross = Math.Abs(dx1 * dy2 - dx2 * dy1);
|
||||
mH = (int) (mH + cross * 0.001);
|
||||
}
|
||||
mCalcGrid[mNewLocation].F = mNewG + mH;
|
||||
|
||||
#if DEBUGON
|
||||
if (mDebugProgress && PathFinderDebug != null)
|
||||
PathFinderDebug(mLocationX, mLocationY, mNewLocationX, mNewLocationY, PathFinderNodeType.Open, mCalcGrid[mNewLocation].F, mCalcGrid[mNewLocation].G);
|
||||
#endif
|
||||
|
||||
//It is faster if we leave the open node in the priority queue
|
||||
//When it is removed, it will be already closed, it will be ignored automatically
|
||||
//if (tmpGrid[newLocation].Status == 1)
|
||||
//{
|
||||
// //int removeX = newLocation & gridXMinus1;
|
||||
// //int removeY = newLocation >> gridYLog2;
|
||||
// mOpen.RemoveLocation(newLocation);
|
||||
//}
|
||||
|
||||
//if (tmpGrid[newLocation].Status != 1)
|
||||
//{
|
||||
mOpen.Push(mNewLocation);
|
||||
//}
|
||||
mCalcGrid[mNewLocation].Status = mOpenNodeValue;
|
||||
}
|
||||
|
||||
mCloseNodeCounter++;
|
||||
mCalcGrid[mLocation].Status = mCloseNodeValue;
|
||||
|
||||
#if DEBUGON
|
||||
if (mDebugProgress && PathFinderDebug != null)
|
||||
PathFinderDebug(0, 0, mLocationX, mLocationY, PathFinderNodeType.Close, mCalcGrid[mLocation].F, mCalcGrid[mLocation].G);
|
||||
#endif
|
||||
}
|
||||
|
||||
mCompletedTime = HighResolutionTime.GetTime();
|
||||
if (mFound)
|
||||
{
|
||||
mClose.Clear();
|
||||
int posX = end.X;
|
||||
int posY = end.Y;
|
||||
|
||||
PathFinderNodeFast fNodeTmp = mCalcGrid[(end.Y << mGridYLog2) + end.X];
|
||||
PathFinderNode fNode;
|
||||
fNode.F = fNodeTmp.F;
|
||||
fNode.G = fNodeTmp.G;
|
||||
fNode.H = 0;
|
||||
fNode.PX = fNodeTmp.PX;
|
||||
fNode.PY = fNodeTmp.PY;
|
||||
fNode.X = end.X;
|
||||
fNode.Y = end.Y;
|
||||
|
||||
while(fNode.X != fNode.PX || fNode.Y != fNode.PY)
|
||||
{
|
||||
mClose.Add(fNode);
|
||||
#if DEBUGON
|
||||
if (mDebugFoundPath && PathFinderDebug != null)
|
||||
PathFinderDebug(fNode.PX, fNode.PY, fNode.X, fNode.Y, PathFinderNodeType.Path, fNode.F, fNode.G);
|
||||
#endif
|
||||
posX = fNode.PX;
|
||||
posY = fNode.PY;
|
||||
fNodeTmp = mCalcGrid[(posY << mGridYLog2) + posX];
|
||||
fNode.F = fNodeTmp.F;
|
||||
fNode.G = fNodeTmp.G;
|
||||
fNode.H = 0;
|
||||
fNode.PX = fNodeTmp.PX;
|
||||
fNode.PY = fNodeTmp.PY;
|
||||
fNode.X = posX;
|
||||
fNode.Y = posY;
|
||||
}
|
||||
|
||||
mClose.Add(fNode);
|
||||
#if DEBUGON
|
||||
if (mDebugFoundPath && PathFinderDebug != null)
|
||||
PathFinderDebug(fNode.PX, fNode.PY, fNode.X, fNode.Y, PathFinderNodeType.Path, fNode.F, fNode.G);
|
||||
#endif
|
||||
|
||||
mStopped = true;
|
||||
return mClose;
|
||||
}
|
||||
mStopped = true;
|
||||
return null;
|
||||
}
|
||||
}
|
||||
#endregion
|
||||
|
||||
#region Inner Classes
|
||||
[Author("Franco, Gustavo")]
|
||||
internal class ComparePFNodeMatrix : IComparer<int>
|
||||
{
|
||||
#region Variables Declaration
|
||||
PathFinderNodeFast[] mMatrix;
|
||||
#endregion
|
||||
|
||||
#region Constructors
|
||||
public ComparePFNodeMatrix(PathFinderNodeFast[] matrix)
|
||||
{
|
||||
mMatrix = matrix;
|
||||
}
|
||||
#endregion
|
||||
|
||||
#region IComparer Members
|
||||
public int Compare(int a, int b)
|
||||
{
|
||||
if (mMatrix[a].F > mMatrix[b].F)
|
||||
return 1;
|
||||
else if (mMatrix[a].F < mMatrix[b].F)
|
||||
return -1;
|
||||
return 0;
|
||||
}
|
||||
#endregion
|
||||
}
|
||||
#endregion
|
||||
}
|
||||
}
|
||||
Executable
+213
@@ -0,0 +1,213 @@
|
||||
//
|
||||
// THIS CODE AND INFORMATION IS PROVIDED "AS IS" WITHOUT WARRANTY OF ANY
|
||||
// KIND, EITHER EXPRESSED OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE
|
||||
// IMPLIED WARRANTIES OF MERCHANTABILITY AND/OR FITNESS FOR A PARTICULAR
|
||||
// PURPOSE. IT CAN BE DISTRIBUTED FREE OF CHARGE AS LONG AS THIS HEADER
|
||||
// REMAINS UNCHANGED.
|
||||
//
|
||||
// Email: gustavo_franco@hotmail.com
|
||||
//
|
||||
// Copyright (C) 2006 Franco, Gustavo
|
||||
//
|
||||
using System;
|
||||
using System.Collections;
|
||||
using System.Collections.Generic;
|
||||
using System.Diagnostics;
|
||||
|
||||
namespace AStar
|
||||
{
|
||||
#region Interfaces
|
||||
[Author("Franco, Gustavo")]
|
||||
public interface IPriorityQueue<T>
|
||||
{
|
||||
#region Methods
|
||||
int Push(T item);
|
||||
T Pop();
|
||||
T Peek();
|
||||
void Update(int i);
|
||||
#endregion
|
||||
}
|
||||
#endregion
|
||||
|
||||
[Author("Franco, Gustavo")]
|
||||
public class PriorityQueueB<T> : IPriorityQueue<T>
|
||||
{
|
||||
#region Variables Declaration
|
||||
protected List<T> InnerList = new List<T>();
|
||||
protected IComparer<T> mComparer;
|
||||
#endregion
|
||||
|
||||
#region Contructors
|
||||
public PriorityQueueB()
|
||||
{
|
||||
mComparer = Comparer<T>.Default;
|
||||
}
|
||||
|
||||
public PriorityQueueB(IComparer<T> comparer)
|
||||
{
|
||||
mComparer = comparer;
|
||||
}
|
||||
|
||||
public PriorityQueueB(IComparer<T> comparer, int capacity)
|
||||
{
|
||||
mComparer = comparer;
|
||||
InnerList.Capacity = capacity;
|
||||
}
|
||||
#endregion
|
||||
|
||||
#region Methods
|
||||
protected void SwitchElements(int i, int j)
|
||||
{
|
||||
T h = InnerList[i];
|
||||
InnerList[i] = InnerList[j];
|
||||
InnerList[j] = h;
|
||||
}
|
||||
|
||||
protected virtual int OnCompare(int i, int j)
|
||||
{
|
||||
return mComparer.Compare(InnerList[i],InnerList[j]);
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// Push an object onto the PQ
|
||||
/// </summary>
|
||||
/// <param name="O">The new object</param>
|
||||
/// <returns>The index in the list where the object is _now_. This will change when objects are taken from or put onto the PQ.</returns>
|
||||
public int Push(T item)
|
||||
{
|
||||
int p = InnerList.Count,p2;
|
||||
InnerList.Add(item); // E[p] = O
|
||||
do
|
||||
{
|
||||
if(p==0)
|
||||
break;
|
||||
p2 = (p-1)/2;
|
||||
if(OnCompare(p,p2)<0)
|
||||
{
|
||||
SwitchElements(p,p2);
|
||||
p = p2;
|
||||
}
|
||||
else
|
||||
break;
|
||||
}while(true);
|
||||
return p;
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// Get the smallest object and remove it.
|
||||
/// </summary>
|
||||
/// <returns>The smallest object</returns>
|
||||
public T Pop()
|
||||
{
|
||||
T result = InnerList[0];
|
||||
int p = 0,p1,p2,pn;
|
||||
InnerList[0] = InnerList[InnerList.Count-1];
|
||||
InnerList.RemoveAt(InnerList.Count-1);
|
||||
do
|
||||
{
|
||||
pn = p;
|
||||
p1 = 2*p+1;
|
||||
p2 = 2*p+2;
|
||||
if(InnerList.Count>p1 && OnCompare(p,p1)>0) // links kleiner
|
||||
p = p1;
|
||||
if(InnerList.Count>p2 && OnCompare(p,p2)>0) // rechts noch kleiner
|
||||
p = p2;
|
||||
|
||||
if(p==pn)
|
||||
break;
|
||||
SwitchElements(p,pn);
|
||||
}while(true);
|
||||
|
||||
return result;
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// Notify the PQ that the object at position i has changed
|
||||
/// and the PQ needs to restore order.
|
||||
/// Since you dont have access to any indexes (except by using the
|
||||
/// explicit IList.this) you should not call this function without knowing exactly
|
||||
/// what you do.
|
||||
/// </summary>
|
||||
/// <param name="i">The index of the changed object.</param>
|
||||
public void Update(int i)
|
||||
{
|
||||
int p = i,pn;
|
||||
int p1,p2;
|
||||
do // aufsteigen
|
||||
{
|
||||
if(p==0)
|
||||
break;
|
||||
p2 = (p-1)/2;
|
||||
if(OnCompare(p,p2)<0)
|
||||
{
|
||||
SwitchElements(p,p2);
|
||||
p = p2;
|
||||
}
|
||||
else
|
||||
break;
|
||||
}while(true);
|
||||
if(p<i)
|
||||
return;
|
||||
do // absteigen
|
||||
{
|
||||
pn = p;
|
||||
p1 = 2*p+1;
|
||||
p2 = 2*p+2;
|
||||
if(InnerList.Count>p1 && OnCompare(p,p1)>0) // links kleiner
|
||||
p = p1;
|
||||
if(InnerList.Count>p2 && OnCompare(p,p2)>0) // rechts noch kleiner
|
||||
p = p2;
|
||||
|
||||
if(p==pn)
|
||||
break;
|
||||
SwitchElements(p,pn);
|
||||
}while(true);
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// Get the smallest object without removing it.
|
||||
/// </summary>
|
||||
/// <returns>The smallest object</returns>
|
||||
public T Peek()
|
||||
{
|
||||
if(InnerList.Count>0)
|
||||
return InnerList[0];
|
||||
return default(T);
|
||||
}
|
||||
|
||||
public void Clear()
|
||||
{
|
||||
InnerList.Clear();
|
||||
}
|
||||
|
||||
public int Count
|
||||
{
|
||||
get{ return InnerList.Count; }
|
||||
}
|
||||
|
||||
public void RemoveLocation(T item)
|
||||
{
|
||||
int index = -1;
|
||||
for(int i=0; i<InnerList.Count; i++)
|
||||
{
|
||||
|
||||
if (mComparer.Compare(InnerList[i], item) == 0)
|
||||
index = i;
|
||||
}
|
||||
|
||||
if (index != -1)
|
||||
InnerList.RemoveAt(index);
|
||||
}
|
||||
|
||||
public T this[int index]
|
||||
{
|
||||
get { return InnerList[index]; }
|
||||
set
|
||||
{
|
||||
InnerList[index] = value;
|
||||
Update(index);
|
||||
}
|
||||
}
|
||||
#endregion
|
||||
}
|
||||
}
|
||||
Reference in New Issue
Block a user