/// <summary>
/// Compares the supplied point and determines whether or not it is inside the Actual Bounds
/// of the Polygon.
/// </summary>
/// <remarks>The calculation formula was converted from the C version available at
/// http://www.ecse.rpi.edu/Homepages/wrf/Research/Short_Notes/pnpoly.html
/// </remarks>
/// <param name="pt">The Vector to compare.</param>
/// <returns>True if the Vector is within the Actual Bounds, False if it is not.</returns>
public bool Contains(SVector3 pt)
{
bool isIn = false;
if (IsInBounds(pt))
{
int i, j = 0;
// The following code is converted from a C version found at
// http://www.ecse.rpi.edu/Homepages/wrf/Research/Short_Notes/pnpoly.html
for (i = 0, j = NumberOfPoints - 1; i < NumberOfPoints; j = i++)
{
if (
(
((m_Geo[i].Y <= pt.Y) && (pt.Y < m_Geo[j].Y)) || ((m_Geo[j].Y <= pt.Y) && (pt.Y < m_Geo[i].Y))
) &&
(pt.X < (m_Geo[j].X - m_Geo[i].X) * (pt.Y - m_Geo[i].Y) / (m_Geo[j].Y - m_Geo[i].Y) + m_Geo[i].X)
)
{
isIn = !isIn;
}
}
}
return(isIn);
}
/// <summary>
/// Add 2 vectors and create a new one.
/// </summary>
/// <param name="vector1">First vector</param>
/// <param name="vector2">Second vector</param>
/// <returns>New vector that is the sum of the 2 vectors</returns>
public static SVector3 Add(SVector3 vector1, SVector3 vector2)
{
if ((vector1 == null) || (vector2 == null))
{
return(null);
}
return(new SVector3(vector1.X + vector2.X, vector1.Y + vector2.Y, vector1.Z + vector2.Z));
}
/// <summary>
/// Return a new vector with negative values.
/// </summary>
/// <param name="v">Original vector</param>
/// <returns>New vector that is the inversion of the original vector</returns>
public static SVector3 Neg(SVector3 vector)
{
if (vector == null)
{
return(null);
}
return(new SVector3(-vector.X, -vector.Y, -vector.Z));
}
/// <summary>
/// Substract 2 vectors and create a new one.
/// </summary>
/// <param name="vector1">First vector</param>
/// <param name="vector2">Second vector</param>
/// <returns>New vector that is the difference of the 2 vectors</returns>
public static SVector3 Subtract(SVector3 vector1, SVector3 vector2)
{
if ((vector1 == null) || (vector2 == null))
{
return(null);
}
return(new SVector3(vector1.X - vector2.X, vector1.Y - vector2.Y, vector1.Z - vector2.Z));
}
/// <summary>
/// Calculate the sum of 2 vectors.
/// </summary>
/// <param name="vector1">First vector</param>
/// <param name="vector2">Second vector</param>
/// <returns>New vector that is the sum of the 2 vectors</returns>
public static SVector3 operator +(SVector3 vector1, SVector3 vector2)
{
if ((vector1 == null) || (vector2 == null))
{
return(null);
}
return(SVector3.Add(vector1, vector2));
}
/// <summary>
/// Multiply a vector with a scalar
/// </summary>
/// <param name="vector">Vector to be multiplied</param>
/// <param name="val">Scalar to multiply vector</param>
/// <returns>New vector that is the multiplication of the vector with the scalar</returns>
public static SVector3 Multiply(SVector3 vector, double val)
{
if (vector == null)
{
return(null);
}
return(new SVector3(vector.X * val, vector.Y * val, vector.Z * val));
}
public static SVector3 MoveVector2DTowards(SVector3 a, SVector3 b, double distance)
{
var vector = new SVector3(b.X - a.X, b.Y - a.Y, 0);
var length = Math.Sqrt(vector.X * vector.X + vector.Y * vector.Y);
var unitVector = new SVector3(vector.X / length, vector.Y / length, 0);
return(new SVector3(a.X + unitVector.X * distance, a.Y + unitVector.Y * distance, 0));
}
/// <summary>
/// Calculate the substraction of 2 vectors
/// </summary>
/// <param name="vector1">First vector</param>
/// <param name="vector2">Second vector</param>
/// <returns>New vector that is the difference of the 2 vectors</returns>
public static SVector3 operator -(SVector3 vector1, SVector3 vector2)
{
if ((vector1 == null) || (vector2 == null))
{
return(null);
}
return(SVector3.Subtract(vector1, vector2));
}
/// <summary>
/// Calculate the negative (inverted) vector
/// </summary>
/// <param name="v">Original vector</param>
/// <returns>New vector that is the invertion of the original vector</returns>
public static SVector3 operator -(SVector3 vector)
{
if (vector == null)
{
return(null);
}
return(SVector3.Neg(vector));
}
/// <summary>
/// Calculate the multiplication of a vector with a scalar
/// </summary>
/// <param name="val">Scalar to multiply vecto</param>
/// <param name="vector">Vector to be multiplied</param>
/// <returns>New vector that is the multiplication of the vector and the scalar</returns>
public static SVector3 operator *(double val, SVector3 vector)
{
if (vector == null)
{
return(null);
}
return(SVector3.Multiply(vector, val));
}
public static double GetDistance(SVector3 a, SVector3 b)
{
double dx = a.X - b.X;
double dy = a.Y - b.Y;
double dz = a.Z - b.Z;
double distance = (double)Math.Sqrt(dx * dx + dy * dy + dz * dz);
return(distance);
}
public static SVector3 GetVector(byte[] dat, Pointer p)
{
SVector3 pos = new SVector3();
pos.X = GetSingle(dat, p);
pos.Y = GetSingle(dat, p);
pos.Z = GetSingle(dat, p);
return(pos);
}
/// <summary>
/// Returns a map chunk (loads it from datastore if not currently in memory), given it's coordinates within the chunk grid
/// </summary>
/// <param name="chunkCoord"></param>
/// <returns></returns>
public MapChunk GetChunkAtChunkCoord(SVector3 chunkCoord)
{
if (chunkCoord.X > m_Chunks.GetUpperBound(0) || chunkCoord.Y > m_Chunks.GetUpperBound(1) || chunkCoord.Z > m_Chunks.GetUpperBound(2))
{
return(null);
}
MapChunk chunk = m_Chunks[(int)chunkCoord.X, (int)chunkCoord.Y, (int)chunkCoord.Z];
return(chunk);
}
public override bool Equals(object obj)
{
SVector3 vector = obj as SVector3;
if (vector != null)
{
return((m_Xcoord.Equals(vector.X)) &&
(m_Ycoord.Equals(vector.Y)) &&
(m_Zcoord.Equals(vector.Z)));
}
return(false);
}
/// <summary>
/// Returns a chunk's coordinate address within the chunk grid, given a location on the world map
/// </summary>
/// <param name="mapLoc"></param>
/// <returns></returns>
public SVector3 GetChunkCoordFromMapLoc(SVector3 mapLoc)
{
if (!IsLocationWithinMap(mapLoc))
{
return(null);
}
double chunkX = (double)Math.Floor(mapLoc.X / m_ChunkSizeX);
double chunkY = (double)Math.Floor(mapLoc.Y / m_ChunkSizeY);
double chunkZ = (double)Math.Floor(mapLoc.Z / m_ChunkSizeZ);
return(new SVector3(chunkX, chunkY, chunkZ));
}
/// <summary>
/// Returns a reference to a map chunk (and all contained GameObjects), given a location
/// </summary>
/// <param name="loc"></param>
/// <returns></returns>
public MapChunk GetChunkAtMapLoc(SVector3 mapLoc)
{
SVector3 chunkCoord = GetChunkCoordFromMapLoc(mapLoc);
if (chunkCoord == null)
{
return(null);
}
MapChunk chunk = GetChunkAtChunkCoord(chunkCoord);
if (chunk == null)
{
throw new Exception("Error loading map chunk.");
}
return(chunk);
}
public Region GetRegionAtLoc(SVector3 v)
{
Region r = null;
Dictionary <Guid, IGameObject> .ValueCollection.Enumerator enu = m_Regions.Objects;
while (enu.MoveNext())
{
Region reg = enu.Current as Region;
if (!reg.IsInBounds(v))
{
continue;
}
if (reg.Contains(v))
{
return(reg);
}
}
return(r);
}
public void InitMap()
{
SVector3 originTileCenter = OriginTileCenter;
for (int row = 0; row <= m_XWidth; row++)
{
for (int col = 0; col <= m_YHeight; col++)
{
TileType s = new TileType();
s.Column = col;
s.Row = row;
s.WorldLocation = new SVector3();
// Set world coordinate
s.WorldLocation.X = originTileCenter.X + (s.Column - 1) * m_WorldUnitsTileWidth;
s.WorldLocation.Z = originTileCenter.Z + (s.Row - 1) * m_WorldUnitsTileWidth;
s.WorldLocation.Y = 0;
Tiles[row, col] = s;
}
}
}
/// <summary>
/// Determines if a given location is valid, given the size of the map.
/// </summary>
/// <param name="loc"></param>
/// <returns></returns>
public bool IsLocationWithinMap(SVector3 loc)
{
return (loc.X <= m_DimensionX && loc.Y <= m_DimensionY && loc.Z <= m_DimensionZ);
}
/// <summary>
/// Initiate vector with same values as given Vector
/// </summary>
/// <param name="v">Vector to copy coordinations</param>
public SVector3(SVector3 vector)
{
m_Xcoord = vector.X;
m_Ycoord = vector.Y;
m_Zcoord = vector.Z;
}
public MobileState()
{
Position = new SVector3();
Rotation = new SVector3();
Velocity = new SVector3();
}
/// <summary>
/// Compares the supplied point and determines whether or not it is inside the Rectangular Bounds
/// of the Polygon.
/// </summary>
/// <param name="pt">The Vector to compare.</param>
/// <returns>True if the Vector is within the Rectangular Bounds, False if it is not.</returns>
public bool IsInBounds(SVector3 pt)
{
return AABounds.Contains(pt);
}
public MobileState()
{
Position = new SVector3();
Rotation = new SVector3();
Velocity = new SVector3();
}
/// <summary>
/// Returns a reference to a map chunk (and all contained GameObjects), given a location
/// </summary>
/// <param name="loc"></param>
/// <returns></returns>
public MapChunk GetChunkAtMapLoc(SVector3 mapLoc)
{
SVector3 chunkCoord = GetChunkCoordFromMapLoc(mapLoc);
if (chunkCoord == null)
{
return null;
}
MapChunk chunk = GetChunkAtChunkCoord(chunkCoord);
if (chunk == null)
{
throw new Exception("Error loading map chunk.");
}
return chunk;
}
protected void ReadObjectDataFromTable(Dictionary<string, byte[]> data)
{
if (data.ContainsKey("dimX"))
{
m_DimensionX = BitConverter.ToDouble((byte[])data["dimX"], 0);
}
if (data.ContainsKey("dimY"))
{
m_DimensionY = BitConverter.ToDouble((byte[])data["dimY"], 0);
}
if (data.ContainsKey("dimZ"))
{
m_DimensionZ = BitConverter.ToDouble((byte[])data["dimZ"], 0);
}
if (data.ContainsKey("chSizeX"))
{
m_ChunkSizeX = BitConverter.ToDouble((byte[])data["chSizeX"], 0);
}
if (data.ContainsKey("chSizeY"))
{
m_ChunkSizeY = BitConverter.ToDouble((byte[])data["chSizeY"], 0);
}
if (data.ContainsKey("chSizeZ"))
{
m_ChunkSizeZ = BitConverter.ToDouble((byte[])data["chSizeZ"], 0);
}
if (data.ContainsKey("scale"))
{
m_Scale = BitConverter.ToDouble((byte[])data["scale"], 0);
}
if (data.ContainsKey("mapName"))
{
byte[] dat = (byte[])data["mapName"];
m_Name = Encoding.UTF8.GetString(dat, 0, dat.Length);
}
if (data.ContainsKey("bg"))
{
byte[] dat = (byte[])data["bg"];
m_MapBackground = Encoding.UTF8.GetString(dat, 0, dat.Length);
}
// load region data
if (data.ContainsKey("regions"))
{
System.IO.MemoryStream memStream = new System.IO.MemoryStream((byte[])data["regions"]);
System.IO.BinaryReader read = new System.IO.BinaryReader(memStream);
int numRegions = read.ReadInt32(); // number of regions
for (int i = 0; i < numRegions; i++)
{
Region r = new Region();
// read region name
int nameLen = read.ReadInt32(); // read name length
byte[] datName = read.ReadBytes(nameLen);
r.ObjectName = Encoding.UTF8.GetString(datName, 0, datName.Length);
// read region internal name
int iNameLen = read.ReadInt32();
byte[] datIName = read.ReadBytes(iNameLen);
r.InternalName = Encoding.UTF8.GetString(datIName, 0, datIName.Length);
// read location
r.WorldLocation = new SVector3(read.ReadDouble(), read.ReadDouble(), read.ReadDouble());
// Read path geo
int numPoints = read.ReadInt32();
List<SVector3> pGeo = new List<SVector3>();
for (int v = 0; v < numPoints; v++)
{
SVector3 vPoint = new SVector3(read.ReadDouble(), read.ReadDouble(), read.ReadDouble());
pGeo.Add(vPoint);
}
r.Geo = pGeo;
// region ID
int UIDLen = read.ReadInt32(); // read title length
byte[] datUid = read.ReadBytes(UIDLen);
string uid = Encoding.UTF8.GetString(datUid, 0, datUid.Length);
r.UID = new Guid(uid);
// title
int titleLen = read.ReadInt32(); // read title length
byte[] datTitle = read.ReadBytes(titleLen);
r.ObjectName = Encoding.UTF8.GetString(datTitle, 0, datTitle.Length);
// description
int descLen = read.ReadInt32(); // read desc length
byte[] datDesc = read.ReadBytes(descLen);
r.Description = Encoding.UTF8.GetString(datDesc, 0, datDesc.Length);
// content id
int cIDLen = read.ReadInt32(); // content id length
byte[] datCont = read.ReadBytes(cIDLen);
r.ContentTag = Encoding.UTF8.GetString(datCont, 0, datCont.Length);
m_Regions.AddObject(r);
}
}
}
public bool Contains(SVector3 pt)
{
return
(pt.X > X && pt.X < X + Width &&
pt.Y > Y && pt.Y < Y + Height);
}
/// <summary>
/// Initiate vector with same values as given Vector
/// </summary>
/// <param name="v">Vector to copy coordinations</param>
public SVector3(SVector3 vector)
{
m_Xcoord = vector.X;
m_Ycoord = vector.Y;
m_Zcoord = vector.Z;
}
/// <summary>
/// Determines if a given location is valid, given the size of the map.
/// </summary>
/// <param name="loc"></param>
/// <returns></returns>
public bool IsLocationWithinMap(SVector3 loc)
{
return(loc.X <= m_DimensionX && loc.Y <= m_DimensionY && loc.Z <= m_DimensionZ);
}
public Region GetRegionAtLoc(SVector3 v)
{
Region r = null;
Dictionary<Guid, IGameObject>.ValueCollection.Enumerator enu = m_Regions.Objects;
while(enu.MoveNext())
{
Region reg = enu.Current as Region;
if (!reg.IsInBounds(v))
{
continue;
}
if (reg.Contains(v))
{
return reg;
}
}
return r;
}
public static SVector3 GetVector(byte[] dat, Pointer p)
{
SVector3 pos = new SVector3();
pos.X = GetSingle(dat, p);
pos.Y = GetSingle(dat, p);
pos.Z = GetSingle(dat, p);
return pos;
}
/// <summary>
/// Returns a chunk's coordinate address within the chunk grid, given a location on the world map
/// </summary>
/// <param name="mapLoc"></param>
/// <returns></returns>
public SVector3 GetChunkCoordFromMapLoc(SVector3 mapLoc)
{
if (!IsLocationWithinMap(mapLoc))
{
return null;
}
double chunkX = (double)Math.Floor(mapLoc.X / m_ChunkSizeX);
double chunkY = (double)Math.Floor(mapLoc.Y / m_ChunkSizeY);
double chunkZ = (double)Math.Floor(mapLoc.Z / m_ChunkSizeZ);
return new SVector3(chunkX, chunkY, chunkZ);
}
public static SVector3 MoveVector2DTowards(SVector3 a, SVector3 b, double distance)
{
var vector = new SVector3(b.X - a.X, b.Y - a.Y, 0);
var length = Math.Sqrt(vector.X * vector.X + vector.Y * vector.Y);
var unitVector = new SVector3(vector.X / length, vector.Y / length, 0);
return new SVector3(a.X + unitVector.X * distance, a.Y + unitVector.Y * distance, 0);
}
/// <summary>
/// Returns a map chunk (loads it from datastore if not currently in memory), given it's coordinates within the chunk grid
/// </summary>
/// <param name="chunkCoord"></param>
/// <returns></returns>
public MapChunk GetChunkAtChunkCoord(SVector3 chunkCoord)
{
if (chunkCoord.X > m_Chunks.GetUpperBound(0) || chunkCoord.Y > m_Chunks.GetUpperBound(1) || chunkCoord.Z > m_Chunks.GetUpperBound(2))
{
return null;
}
MapChunk chunk = m_Chunks[(int)chunkCoord.X, (int)chunkCoord.Y, (int)chunkCoord.Z];
return chunk;
}
/// <summary>
/// Multiply a vector with a scalar
/// </summary>
/// <param name="vector">Vector to be multiplied</param>
/// <param name="val">Scalar to multiply vector</param>
/// <returns>New vector that is the multiplication of the vector with the scalar</returns>
public static SVector3 Multiply(SVector3 vector, double val)
{
if (vector == null)
return null;
return new SVector3(vector.X * val, vector.Y * val, vector.Z * val);
}
public Region()
: base()
{
WorldLocation = new SVector3();
}
/// <summary>
/// Return a new vector with negative values.
/// </summary>
/// <param name="v">Original vector</param>
/// <returns>New vector that is the inversion of the original vector</returns>
public static SVector3 Neg(SVector3 vector)
{
if (vector == null)
return null;
return new SVector3(-vector.X, -vector.Y, -vector.Z);
}
/// <summary>
/// Compares the supplied point and determines whether or not it is inside the Actual Bounds
/// of the Polygon.
/// </summary>
/// <remarks>The calculation formula was converted from the C version available at
/// http://www.ecse.rpi.edu/Homepages/wrf/Research/Short_Notes/pnpoly.html
/// </remarks>
/// <param name="pt">The Vector to compare.</param>
/// <returns>True if the Vector is within the Actual Bounds, False if it is not.</returns>
public bool Contains(SVector3 pt)
{
bool isIn = false;
if (IsInBounds(pt))
{
int i, j = 0;
// The following code is converted from a C version found at
// http://www.ecse.rpi.edu/Homepages/wrf/Research/Short_Notes/pnpoly.html
for (i = 0, j = NumberOfPoints - 1; i < NumberOfPoints; j = i++)
{
if (
(
((m_Geo[i].Y <= pt.Y) && (pt.Y < m_Geo[j].Y)) || ((m_Geo[j].Y <= pt.Y) && (pt.Y < m_Geo[i].Y))
) &&
(pt.X < (m_Geo[j].X - m_Geo[i].X) * (pt.Y - m_Geo[i].Y) / (m_Geo[j].Y - m_Geo[i].Y) + m_Geo[i].X)
)
{
isIn = !isIn;
}
}
}
return isIn;
}
/// <summary>
/// Substract 2 vectors and create a new one.
/// </summary>
/// <param name="vector1">First vector</param>
/// <param name="vector2">Second vector</param>
/// <returns>New vector that is the difference of the 2 vectors</returns>
public static SVector3 Subtract(SVector3 vector1, SVector3 vector2)
{
if ((vector1 == null) || (vector2 == null))
return null;
return new SVector3(vector1.X - vector2.X, vector1.Y - vector2.Y, vector1.Z - vector2.Z);
}
public static void AddVector(ref byte[] dat, Pointer curPointer, SVector3 vec)
{
AddSingle(ref dat, curPointer, (float)vec.X);
AddSingle(ref dat, curPointer, (float)vec.Y);
AddSingle(ref dat, curPointer, (float)vec.Z);
}
protected void ReadObjectDataFromTable(Dictionary <string, byte[]> data)
{
if (data.ContainsKey("dimX"))
{
m_DimensionX = BitConverter.ToDouble((byte[])data["dimX"], 0);
}
if (data.ContainsKey("dimY"))
{
m_DimensionY = BitConverter.ToDouble((byte[])data["dimY"], 0);
}
if (data.ContainsKey("dimZ"))
{
m_DimensionZ = BitConverter.ToDouble((byte[])data["dimZ"], 0);
}
if (data.ContainsKey("chSizeX"))
{
m_ChunkSizeX = BitConverter.ToDouble((byte[])data["chSizeX"], 0);
}
if (data.ContainsKey("chSizeY"))
{
m_ChunkSizeY = BitConverter.ToDouble((byte[])data["chSizeY"], 0);
}
if (data.ContainsKey("chSizeZ"))
{
m_ChunkSizeZ = BitConverter.ToDouble((byte[])data["chSizeZ"], 0);
}
if (data.ContainsKey("scale"))
{
m_Scale = BitConverter.ToDouble((byte[])data["scale"], 0);
}
if (data.ContainsKey("mapName"))
{
byte[] dat = (byte[])data["mapName"];
m_Name = Encoding.UTF8.GetString(dat, 0, dat.Length);
}
if (data.ContainsKey("bg"))
{
byte[] dat = (byte[])data["bg"];
m_MapBackground = Encoding.UTF8.GetString(dat, 0, dat.Length);
}
// load region data
if (data.ContainsKey("regions"))
{
System.IO.MemoryStream memStream = new System.IO.MemoryStream((byte[])data["regions"]);
System.IO.BinaryReader read = new System.IO.BinaryReader(memStream);
int numRegions = read.ReadInt32(); // number of regions
for (int i = 0; i < numRegions; i++)
{
Region r = new Region();
// read region name
int nameLen = read.ReadInt32(); // read name length
byte[] datName = read.ReadBytes(nameLen);
r.ObjectName = Encoding.UTF8.GetString(datName, 0, datName.Length);
// read region internal name
int iNameLen = read.ReadInt32();
byte[] datIName = read.ReadBytes(iNameLen);
r.InternalName = Encoding.UTF8.GetString(datIName, 0, datIName.Length);
// read location
r.WorldLocation = new SVector3(read.ReadDouble(), read.ReadDouble(), read.ReadDouble());
// Read path geo
int numPoints = read.ReadInt32();
List <SVector3> pGeo = new List <SVector3>();
for (int v = 0; v < numPoints; v++)
{
SVector3 vPoint = new SVector3(read.ReadDouble(), read.ReadDouble(), read.ReadDouble());
pGeo.Add(vPoint);
}
r.Geo = pGeo;
// region ID
int UIDLen = read.ReadInt32(); // read title length
byte[] datUid = read.ReadBytes(UIDLen);
string uid = Encoding.UTF8.GetString(datUid, 0, datUid.Length);
r.UID = new Guid(uid);
// title
int titleLen = read.ReadInt32(); // read title length
byte[] datTitle = read.ReadBytes(titleLen);
r.ObjectName = Encoding.UTF8.GetString(datTitle, 0, datTitle.Length);
// description
int descLen = read.ReadInt32(); // read desc length
byte[] datDesc = read.ReadBytes(descLen);
r.Description = Encoding.UTF8.GetString(datDesc, 0, datDesc.Length);
// content id
int cIDLen = read.ReadInt32(); // content id length
byte[] datCont = read.ReadBytes(cIDLen);
r.ContentTag = Encoding.UTF8.GetString(datCont, 0, datCont.Length);
m_Regions.AddObject(r);
}
}
}
//private List<Int64> elapsed = new List<long>();
/// <summary>
/// Returns null, if no path is found. Start- and End-Node are included in returned path. The user context
/// is passed to IsWalkable().
/// </summary>
public List <TPathNode> Search(SVector3 inStartNode, SVector3 inEndNode, TUserContext inUserContext)
{
PathNode startNode = m_SearchSpace[(int)inStartNode.X, (int)inStartNode.Y];
PathNode endNode = m_SearchSpace[(int)inEndNode.X, (int)inEndNode.Y];
//System.Diagnostics.Stopwatch watch = new System.Diagnostics.Stopwatch();
//watch.Start();
if (startNode == endNode)
{
return(new List <TPathNode>(new TPathNode[] { startNode.UserContext }));
}
PathNode[] neighborNodes = new PathNode[8];
m_ClosedSet.Clear();
m_OpenSet.Clear();
m_RuntimeGrid.Clear();
m_OrderedOpenSet.Clear();
for (int x = 0; x < Width; x++)
{
for (int y = 0; y < Height; y++)
{
m_CameFrom[x, y] = null;
}
}
startNode.G = 0;
startNode.H = Heuristic(startNode, endNode);
startNode.F = startNode.H;
m_OpenSet.Add(startNode);
m_OrderedOpenSet.Push(startNode);
m_RuntimeGrid.Add(startNode);
int nodes = 0;
while (!m_OpenSet.IsEmpty)
{
PathNode x = m_OrderedOpenSet.Pop();
if (x == endNode)
{
// watch.Stop();
//elapsed.Add(watch.ElapsedMilliseconds);
List <TPathNode> result = ReconstructPath(m_CameFrom, m_CameFrom[endNode.X, endNode.Y]);
result.Add(endNode.UserContext);
return(result);
}
m_OpenSet.Remove(x);
m_ClosedSet.Add(x);
StoreNeighborNodes(x, neighborNodes);
for (int i = 0; i < neighborNodes.Length; i++)
{
PathNode y = neighborNodes[i];
Boolean tentative_is_better;
if (y == null)
{
continue;
}
if (!y.UserContext.IsWalkable(inUserContext))
{
continue;
}
if (m_ClosedSet.Contains(y))
{
continue;
}
nodes++;
Double tentative_g_score = m_RuntimeGrid[x].G + NeighborDistance(x, y);
Boolean wasAdded = false;
if (!m_OpenSet.Contains(y))
{
m_OpenSet.Add(y);
tentative_is_better = true;
wasAdded = true;
}
else if (tentative_g_score < m_RuntimeGrid[y].G)
{
tentative_is_better = true;
}
else
{
tentative_is_better = false;
}
if (tentative_is_better)
{
m_CameFrom[y.X, y.Y] = x;
if (!m_RuntimeGrid.Contains(y))
{
m_RuntimeGrid.Add(y);
}
m_RuntimeGrid[y].G = tentative_g_score;
m_RuntimeGrid[y].H = Heuristic(y, endNode);
m_RuntimeGrid[y].F = m_RuntimeGrid[y].G + m_RuntimeGrid[y].H;
if (wasAdded)
{
m_OrderedOpenSet.Push(y);
}
else
{
m_OrderedOpenSet.Update(y);
}
}
}
}
return(null);
}
/// <summary>
/// Add 2 vectors and create a new one.
/// </summary>
/// <param name="vector1">First vector</param>
/// <param name="vector2">Second vector</param>
/// <returns>New vector that is the sum of the 2 vectors</returns>
public static SVector3 Add(SVector3 vector1, SVector3 vector2)
{
if ((vector1 == null) || (vector2 == null))
return null;
return new SVector3(vector1.X + vector2.X, vector1.Y + vector2.Y, vector1.Z + vector2.Z);
}
/// <summary>
/// Compares the supplied point and determines whether or not it is inside the Rectangular Bounds
/// of the Polygon.
/// </summary>
/// <param name="pt">The Vector to compare.</param>
/// <returns>True if the Vector is within the Rectangular Bounds, False if it is not.</returns>
public bool IsInBounds(SVector3 pt)
{
return(AABounds.Contains(pt));
}
public static double GetDistance(SVector3 a, SVector3 b)
{
double dx = a.X - b.X;
double dy = a.Y - b.Y;
double dz = a.Z - b.Z;
double distance = (double)Math.Sqrt(dx * dx + dy * dy + dz * dz);
return distance;
}
public Region()
: base()
{
WorldLocation = new SVector3();
}
public static void AddVector(ref byte[] dat, Pointer curPointer, SVector3 vec)
{
AddSingle(ref dat, curPointer, (float)vec.X);
AddSingle(ref dat, curPointer, (float)vec.Y);
AddSingle(ref dat, curPointer, (float)vec.Z);
}