private unsafe static void TcpHandleRandomPointRequest(BinaryWriter writer, byte[] bytes)
{
RandomPointRequest request = Utils.FromBytes <RandomPointRequest>(bytes);
lock (querylock)
{
if (!AmeisenNav.IsMapLoaded(request.MapId))
{
LogQueue.Enqueue(new LogEntry("[MMAPS] ", ConsoleColor.Green, $"Loading Map: {request.MapId}", LogLevel.INFO));
AmeisenNav.LoadMap(request.MapId);
}
}
fixed(float *pointerStart = request.A.ToArray())
{
float[] randomPoint;
lock (querylock)
{
randomPoint = request.MaxRadius > 0f ? AmeisenNav.GetRandomPointAround(request.MapId, pointerStart, request.MaxRadius)
: AmeisenNav.GetRandomPoint(request.MapId);
}
fixed(float *pRandomPoint = randomPoint)
{
TcpSendData(writer, pRandomPoint, sizeof(Vector3));
}
}
}
public void PathfindingTest()
{
int pathSize;
List <Vector3> path = new List <Vector3>();
float[] start = { -8826.562500f, -371.839752f, 71.638428f };
float[] end = { -8918.406250f, -130.297256f, 80.906364f };
Settings settings = new Settings();
AmeisenNav ameisenNav = new AmeisenNav(settings.MmapsFolder);
unsafe
{
fixed(float *pStart = start)
fixed(float *pEnd = end)
{
float *path_raw = ameisenNav.GetPath(0, pStart, pEnd, &pathSize);
for (int i = 0; i < pathSize * 3; i += 3)
{
path.Add(new Vector3(path_raw[i], path_raw[i + 1], path_raw[i + 2]));
}
}
}
Assert.IsTrue(path.Count == 45);
}
private static void PreloadMaps()
{
LogQueue.Enqueue(new LogEntry($"Preloading Maps: ", ConsoleColor.Green, JsonConvert.SerializeObject(Settings.PreloadMaps), LogLevel.DEBUG));
foreach (int i in Settings.PreloadMaps)
{
AmeisenNav.LoadMap(i);
LogQueue.Enqueue(new LogEntry($"Preloaded Map: ", ConsoleColor.Green, i.ToString(), LogLevel.DEBUG));
}
LogQueue.Enqueue(new LogEntry($"Preloaded {Settings.PreloadMaps.Length} Maps", ConsoleColor.Green, string.Empty, LogLevel.DEBUG));
}
public static List <Vector3> GetPath(Vector3 start, Vector3 end, int mapId, PathRequestFlags flags, string clientIp)
{
int pathSize;
List <Vector3> path = new List <Vector3>();
Stopwatch sw = new Stopwatch();
sw.Start();
lock (maplock)
{
if (!AmeisenNav.IsMapLoaded(mapId))
{
AmeisenNav.LoadMap(mapId);
}
unsafe
{
fixed(float *pointerStart = start.ToArray())
fixed(float *pointerEnd = end.ToArray())
{
float *path_raw = AmeisenNav.GetPath(mapId, pointerStart, pointerEnd, &pathSize);
// postprocess the raw path to a list of Vector3
// the raw path looks like this:
// [ x1, y1, z1, x2, y2, z2, ...]
for (int i = 0; i < pathSize * 3; i += 3)
{
path.Add(new Vector3(path_raw[i], path_raw[i + 1], path_raw[i + 2]));
}
}
}
}
if (flags.HasFlag(PathRequestFlags.NaturalSteeringBehavior))
{
path = NaturalSteeringBehavior.Perform(path);
}
if (flags.HasFlag(PathRequestFlags.ChaikinCurve))
{
path = ChaikinCurve.Perform(path);
}
//// bugged atm path = NodeReduction.Perform(path);
sw.Stop();
LogQueue.Enqueue(new LogEntry($"[{clientIp}] ", ConsoleColor.Green, $"Building Path with {path.Count} Nodes took {sw.ElapsedMilliseconds}ms ({sw.ElapsedTicks} ticks)", LogLevel.INFO));
return(path);
}
public static void Main()
{
// buggy need to find a beter way
//// Console.CancelKeyPress += Console_CancelKeyPress;
PrintHeader();
LogQueue = new ConcurrentQueue <LogEntry>();
Settings = LoadConfigFile();
SetupLogging();
UpdateConnectedClientCount();
if (Settings == null)
{
Console.ReadKey();
}
else if (!Directory.Exists(Settings.MmapsFolder))
{
LogQueue.Enqueue(new LogEntry($"MMAP folder missing, edit folder in config.json...", ConsoleColor.Red, string.Empty, LogLevel.ERROR));
Console.ReadKey();
}
else
{
Settings.MmapsFolder = Settings.MmapsFolder.Replace('/', '\\');
AmeisenNav = new AmeisenNav(Settings.MmapsFolder, Settings.MaxPolyPathCount, Settings.MaxPointPathCount);
if (Settings.PreloadMaps.Length > 0)
{
PreloadMaps();
}
TcpListener = new TcpListener(IPAddress.Parse(Settings.IpAddress), Settings.Port);
TcpListener.Start();
LogQueue.Enqueue(new LogEntry($"Listening on {Settings.IpAddress}:{Settings.Port} press Ctrl + C to exit...", ConsoleColor.Green, string.Empty, LogLevel.MASTER));
EnterServerLoop();
// cleanup after server stopped
AmeisenNav.Dispose();
}
}
private unsafe static void TcpHandlePathRequest(BinaryWriter writer, byte[] bytes)
{
PathRequest request = Utils.FromBytes <PathRequest>(bytes);
lock (querylock)
{
if (!AmeisenNav.IsMapLoaded(request.MapId))
{
LogQueue.Enqueue(new LogEntry("[MMAPS] ", ConsoleColor.Green, $"Loading Map: {request.MapId}", LogLevel.INFO));
AmeisenNav.LoadMap(request.MapId);
}
}
Stopwatch sw = Stopwatch.StartNew();
fixed(float *pStartPosition = request.A.ToArray())
fixed(float *pEndPosition = request.B.ToArray())
{
switch (request.MovementType)
{
case MovementType.FindPath:
int pathSize = 0;
float[] movePath = null;
try
{
lock (querylock)
{
movePath = AmeisenNav.GetPath(request.MapId, pStartPosition, pEndPosition, &pathSize);
}
}
catch { }
if (movePath != null && movePath.Length != 0)
{
// we can't apply the chaikin-curve on a path that has just a single node
if (request.Flags.HasFlag(PathRequestFlags.ChaikinCurve) &&
pathSize > 1)
{
movePath = ChaikinCurve.Perform(movePath, Settings.ChaikinIterations);
}
// we can't apply the catmull-rom-spline on a path with less than 4 nodes
if (request.Flags.HasFlag(PathRequestFlags.CatmullRomSpline) &&
pathSize >= 4)
{
movePath = CatmullRomSpline.Perform(movePath, Settings.CatmullRomSplinePoints);
}
pathSize = movePath.Length / 3;
fixed(float *pPath = movePath)
{
TcpSendData(writer, pPath, sizeof(Vector3) * pathSize);
}
}
sw.Stop();
LogQueue.Enqueue(new LogEntry("[FINDPATH] ", ConsoleColor.Green, $"took {sw.ElapsedMilliseconds}ms ({sw.ElapsedTicks} ticks) Nodes: {pathSize}/{Settings.MaxPointPathCount} Flags: {request.Flags}"));
break;
case MovementType.MoveAlongSurface:
float[] surfacePath;
lock (querylock) { surfacePath = AmeisenNav.MoveAlongSurface(request.MapId, pStartPosition, pEndPosition); }
fixed(float *pSurfacePath = surfacePath)
{
TcpSendData(writer, pSurfacePath, sizeof(Vector3));
}
sw.Stop();
// LogQueue.Enqueue(new LogEntry("[MOVEALONGSURFACE] ", ConsoleColor.Green, $"took {sw.ElapsedMilliseconds}ms ({sw.ElapsedTicks} ticks)"));
break;
case MovementType.CastMovementRay:
Vector3 castRayResult;
lock (querylock) { castRayResult = AmeisenNav.CastMovementRay(request.MapId, pStartPosition, pEndPosition) ? request.B : Vector3.Zero; }
TcpSendData(writer, &castRayResult, sizeof(Vector3));
sw.Stop();
LogQueue.Enqueue(new LogEntry("[CASTMOVEMENTRAY] ", ConsoleColor.Green, $"took {sw.ElapsedMilliseconds}ms ({sw.ElapsedTicks} ticks)"));
break;
}
}
}
public static List <Vector3> GetPath(Vector3 start, Vector3 end, float maxRadius, int mapId, MovementType movementType, PathRequestFlags flags, string clientIp)
{
int pathSize;
List <Vector3> path = new List <Vector3>();
Stopwatch sw = new Stopwatch();
sw.Start();
lock (querylock)
{
if (!AmeisenNav.IsMapLoaded(mapId))
{
AmeisenNav.LoadMap(mapId);
}
unsafe
{
fixed(float *pointerStart = start.ToArray())
fixed(float *pointerEnd = end.ToArray())
{
switch (movementType)
{
case MovementType.MoveToPosition:
float[] movePath = AmeisenNav.GetPath(mapId, pointerStart, pointerEnd, &pathSize);
for (int i = 0; i < pathSize * 3; i += 3)
{
path.Add(new Vector3(movePath[i], movePath[i + 1], movePath[i + 2]));
}
if (flags.HasFlag(PathRequestFlags.ChaikinCurve))
{
path = ChaikinCurve.Perform(path);
}
break;
case MovementType.MoveAlongSurface:
float[] surfacePath = AmeisenNav.MoveAlongSurface(mapId, pointerStart, pointerEnd);
path.Add(new Vector3(surfacePath[0], surfacePath[1], surfacePath[2]));
break;
case MovementType.CastMovementRay:
if (AmeisenNav.CastMovementRay(mapId, pointerStart, pointerEnd))
{
// return end if target is in line of sight
path.Add(end);
}
else
{
// return none if target is not in line of sight
path.Clear();
}
break;
case MovementType.GetRandomPoint:
float[] randomPoint = AmeisenNav.GetRandomPoint(mapId);
path.Add(new Vector3(randomPoint[0], randomPoint[1], randomPoint[2]));
break;
case MovementType.GetRandomPointAround:
float[] randomPointAround = AmeisenNav.GetRandomPointAround(mapId, pointerStart, maxRadius);
path.Add(new Vector3(randomPointAround[0], randomPointAround[1], randomPointAround[2]));
break;
}
}
}
}
sw.Stop();
LogQueue.Enqueue(new LogEntry($"[{clientIp}] ", ConsoleColor.Green, $"{movementType} with {path.Count}/{Settings.MaxPointPathCount} Nodes took {sw.ElapsedMilliseconds}ms ({sw.ElapsedTicks} ticks)", LogLevel.INFO));
return(path);
}
private unsafe static void HandlePathRequest(BinaryWriter writer, byte[] bytes)
{
PathRequest request = Utils.FromBytes <PathRequest>(bytes);
lock (querylock)
{
if (!AmeisenNav.IsMapLoaded(request.MapId))
{
LogQueue.Enqueue(new LogEntry("[MMAPS] ", ConsoleColor.Green, $"Loading Map: {request.MapId}", LogLevel.INFO));
AmeisenNav.LoadMap(request.MapId);
}
}
fixed(float *pStartPosition = request.A.ToArray())
fixed(float *pEndPosition = request.B.ToArray())
{
switch (request.MovementType)
{
case MovementType.FindPath:
int pathSize = 0;
float[] movePath;
Stopwatch swPath = Stopwatch.StartNew();
try
{
lock (querylock) { movePath = AmeisenNav.GetPath(request.MapId, pStartPosition, pEndPosition, &pathSize); }
}
catch { movePath = null; }
List <Vector3> path = new List <Vector3>(movePath != null ? movePath.Length * 3 : 1);
if (movePath == null || movePath.Length == 0)
{
path.Add(Vector3.Zero);
fixed(Vector3 *pPath = path.ToArray())
SendData(writer, pPath, sizeof(Vector3));
return;
}
for (int i = 0; i < pathSize * 3; i += 3)
{
path.Add(new Vector3(movePath[i], movePath[i + 1], movePath[i + 2]));
}
if (request.Flags.HasFlag(PathRequestFlags.ChaikinCurve))
{
if (path.Count > 1)
{
path = ChaikinCurve.Perform(path, Settings.ChaikinIterations);
}
else
{
request.Flags &= ~PathRequestFlags.ChaikinCurve;
}
}
if (request.Flags.HasFlag(PathRequestFlags.CatmullRomSpline))
{
if (path.Count >= 4)
{
path = CatmullRomSpline.Perform(path, Settings.CatmullRomSplinePoints);
}
else
{
request.Flags &= ~PathRequestFlags.CatmullRomSpline;
}
}
int size = sizeof(Vector3) * path.Count;
fixed(Vector3 *pPath = path.ToArray())
SendData(writer, pPath, size);
swPath.Stop();
LogQueue.Enqueue(new LogEntry("[FINDPATH] ", ConsoleColor.Green, $"Generating path with {path.Count}/{Settings.MaxPointPathCount} nodes took {swPath.ElapsedMilliseconds}ms ({swPath.ElapsedTicks} ticks) (Flags: {request.Flags})", LogLevel.INFO));
break;
case MovementType.MoveAlongSurface:
Stopwatch swMoveAlongSurface = Stopwatch.StartNew();
float[] surfacePath;
lock (querylock) { surfacePath = AmeisenNav.MoveAlongSurface(request.MapId, pStartPosition, pEndPosition); }
fixed(float *pSurfacePath = surfacePath)
SendData(writer, pSurfacePath, sizeof(Vector3));
swMoveAlongSurface.Stop();
LogQueue.Enqueue(new LogEntry("[MOVEALONGSURFACE] ", ConsoleColor.Green, $"MoveAlongSurface took {swMoveAlongSurface.ElapsedMilliseconds}ms ({swMoveAlongSurface.ElapsedTicks} ticks)", LogLevel.INFO));
break;
case MovementType.CastMovementRay:
Stopwatch swCastMovementRay = Stopwatch.StartNew();
Vector3 castRayResult;
lock (querylock) { castRayResult = AmeisenNav.CastMovementRay(request.MapId, pStartPosition, pEndPosition) ? request.B : Vector3.Zero; }
SendData(writer, &castRayResult, sizeof(Vector3));
swCastMovementRay.Stop();
LogQueue.Enqueue(new LogEntry("[CASTMOVEMENTRAY] ", ConsoleColor.Green, $"CastMovementRay took {swCastMovementRay.ElapsedMilliseconds}ms ({swCastMovementRay.ElapsedTicks} ticks)", LogLevel.INFO));
break;
}
}
}
private static void Main(string[] args)
{
Console.Title = "AmeisenNavigation Server";
Console.WriteLine($"-> AmeisenNavigation Server");
Console.WriteLine($">> Loading config from: {settingsPath}");
Settings settings = new Settings();
if (File.Exists(settingsPath))
{
settings = JsonConvert.DeserializeObject <Settings>(File.ReadAllText(settingsPath));
}
else
{
File.WriteAllText(settingsPath, JsonConvert.SerializeObject(settings));
}
if (!Directory.Exists(settings.mmapsFolder))
{
Console.ForegroundColor = ConsoleColor.Red;
Console.WriteLine(">> MMAP folder missing, edit folder in config.json");
Console.ResetColor();
Console.ReadKey();
}
else
{
Console.WriteLine($">> MMAPS located at: {settings.mmapsFolder}");
AmeisenNav = new AmeisenNav(settings.mmapsFolder);
StopServer = false;
if (settings.preloadMaps.Length > 0)
{
Console.WriteLine($">> Preloading Maps");
foreach (int i in settings.preloadMaps)
{
AmeisenNav.LoadMap(i);
}
Console.WriteLine($">> Preloaded {settings.preloadMaps.Length} Maps");
}
TcpListener = new TcpListener(IPAddress.Parse(settings.ipAddress), settings.port);
TcpListener.Start();
Console.ForegroundColor = ConsoleColor.Green;
Console.WriteLine($">> Server running ({settings.ipAddress}:{settings.port}) press Ctrl + C to exit");
Console.ResetColor();
EnterServerLoop();
AmeisenNav.Dispose();
}
// Debug stuff
/*float[] start = { -8826.562500f, -371.839752f, 71.638428f };
* float[] end = { -8847.150391f, -387.518677f, 72.575912f };
* float[] tileLoc = { -8918.406250f, -130.297256f, 80.906364f };
*
* List<Vector3> Path = GetPath(start, tileLoc);
* string json_path = JsonConvert.SerializeObject(Path);
*
* Console.WriteLine($"Path contains {Path.Count} Nodes");
* Console.WriteLine($"Path json {json_path}");
* Console.ReadKey();*/
}