mirror of
https://bitbucket.org/Ioncannon/project-meteor-server.git
synced 2025-04-21 12:17:46 +00:00
313 lines
11 KiB
C#
313 lines
11 KiB
C#
using System;
|
|
using System.Collections.Generic;
|
|
using System.Linq;
|
|
using System.Text;
|
|
using System.Threading.Tasks;
|
|
using System.IO;
|
|
using SharpNav;
|
|
using SharpNav.Pathfinding;
|
|
using SharpNav.Crowds;
|
|
using SharpNav.IO;
|
|
|
|
namespace FFXIVClassic_Map_Server.utils
|
|
{
|
|
public class Vector3
|
|
{
|
|
public float X;
|
|
public float Y;
|
|
public float Z;
|
|
public static Vector3 Zero = new Vector3();
|
|
|
|
public Vector3(float x, float y, float z)
|
|
{
|
|
X = x;
|
|
Y = y;
|
|
Z = z;
|
|
}
|
|
|
|
public Vector3()
|
|
{
|
|
X = 0.0f;
|
|
Y = 0.0f;
|
|
Z = 0.0f;
|
|
}
|
|
|
|
public Vector3(SharpNav.Geometry.Vector3 vec)
|
|
{
|
|
X = vec.X;
|
|
Y = vec.Y;
|
|
Z = vec.Z;
|
|
}
|
|
|
|
public static Vector3 operator +(Vector3 lhs, Vector3 rhs)
|
|
{
|
|
Vector3 newVec = new Vector3(lhs.X, lhs.Y, lhs.Z);
|
|
newVec.X += rhs.X;
|
|
newVec.Y += rhs.Y;
|
|
newVec.Z += rhs.Z;
|
|
return newVec;
|
|
}
|
|
|
|
public static Vector3 operator -(Vector3 lhs, Vector3 rhs)
|
|
{
|
|
return new Vector3(lhs.X - rhs.X, lhs.Y - rhs.Y, lhs.Z - rhs.Z);
|
|
}
|
|
|
|
public static Vector3 operator *(Vector3 lhs, Vector3 rhs)
|
|
{
|
|
return new Vector3(lhs.X * rhs.X, lhs.Y * rhs.Y, lhs.Z * rhs.Z);
|
|
}
|
|
|
|
public static Vector3 operator *(float scalar, Vector3 rhs)
|
|
{
|
|
return new Vector3(scalar * rhs.X, scalar * rhs.Y, scalar * rhs.Z);
|
|
}
|
|
|
|
public static Vector3 operator /(Vector3 lhs, Vector3 rhs)
|
|
{
|
|
return new Vector3(lhs.X - rhs.X, lhs.Y - rhs.Y, lhs.Z - rhs.Z);
|
|
}
|
|
|
|
public float Length()
|
|
{
|
|
return (float)Math.Sqrt(this.LengthSquared());
|
|
}
|
|
|
|
public float LengthSquared()
|
|
{
|
|
return (this.X * this.X) + (this.Y * this.Y) + (this.Z * this.Z);
|
|
}
|
|
|
|
public static float Dot(Vector3 lhs, Vector3 rhs)
|
|
{
|
|
return (lhs.X * rhs.X) + (lhs.Y * rhs.Y) + (lhs.Z * rhs.Z);
|
|
}
|
|
}
|
|
|
|
class NavmeshUtils
|
|
{
|
|
|
|
// navmesh
|
|
public static bool CanSee(float x1, float y1, float z1, float x2, float y2, float z2)
|
|
{
|
|
|
|
return false;
|
|
}
|
|
|
|
public static Detour.dtNavMesh LoadNavmesh(string path)
|
|
{
|
|
var bytes = File.ReadAllBytes(path);
|
|
var start = 0;
|
|
|
|
var navmesh = Detour.NavMeshSetBuild.FromBytes(bytes, ref start);
|
|
return navmesh;
|
|
}
|
|
|
|
public static SharpNav.TiledNavMesh LoadNavmesh(TiledNavMesh navmesh, string filePath)
|
|
{
|
|
var serialiser = new SharpNav.IO.Json.NavMeshJsonSerializer();
|
|
return serialiser.Deserialize(filePath);
|
|
//return navmesh = new SharpNav.IO.Json.NavMeshJsonSerializer().Deserialize(filePath);
|
|
}
|
|
|
|
public static List<Vector3> GetPath(Detour.dtNavMesh navmesh, Vector3 start, Vector3 end)
|
|
{
|
|
var path = new Vector3[] { };
|
|
|
|
|
|
return path.ToList();
|
|
}
|
|
|
|
#region sharpnav stuff
|
|
// Copyright (c) 2013-2016 Robert Rouhani <robert.rouhani@gmail.com> and other contributors (see CONTRIBUTORS file).
|
|
// Licensed under the MIT License - https://raw.github.com/Robmaister/SharpNav/master/LICENSE
|
|
|
|
public static List<Vector3> GetPath(FFXIVClassic_Map_Server.actors.area.Zone zone, Vector3 startVec, Vector3 endVec, float stepSize = 0.70f, int pathSize = 2048)
|
|
{
|
|
var navMesh = zone.tiledNavMesh;
|
|
var navMeshQuery = zone.navMeshQuery;
|
|
|
|
if (navMesh == null || (startVec.X == endVec.X && startVec.Y == endVec.Y && startVec.Z == endVec.Z))
|
|
{
|
|
return null;
|
|
}
|
|
|
|
float distanceSquared = FFXIVClassic.Common.Utils.DistanceSquared(startVec.X, startVec.Y, startVec.Z, endVec.X, endVec.Y, endVec.Z);
|
|
|
|
// no point pathing if in range
|
|
if (distanceSquared < 4 && Math.Abs(startVec.Y - endVec.Y) < 1.1f)
|
|
{
|
|
return null;
|
|
}
|
|
|
|
var smoothPath = new List<Vector3>(pathSize) { };
|
|
|
|
NavQueryFilter filter = new NavQueryFilter();
|
|
|
|
NavPoint startPt, endPt;
|
|
RaycastHit hit;
|
|
PathCorridor corridor = new PathCorridor();
|
|
|
|
try
|
|
{
|
|
SharpNav.Geometry.Vector3 c = new SharpNav.Geometry.Vector3(startVec.X, startVec.Y, startVec.Z);
|
|
SharpNav.Geometry.Vector3 ep = new SharpNav.Geometry.Vector3(endVec.X, endVec.Y, endVec.Z);
|
|
|
|
SharpNav.Geometry.Vector3 e = new SharpNav.Geometry.Vector3(5, 5, 5);
|
|
navMeshQuery.FindNearestPoly(ref c, ref e, out startPt);
|
|
navMeshQuery.FindNearestPoly(ref ep, ref e, out endPt);
|
|
|
|
//calculate the overall path, which contains an array of polygon references
|
|
int MAX_POLYS = 256;
|
|
var path = new SharpNav.Pathfinding.Path();
|
|
|
|
navMeshQuery.FindPath(ref startPt, ref endPt, filter, path);
|
|
|
|
//find a smooth path over the mesh surface
|
|
int npolys = path.Count;
|
|
SharpNav.Geometry.Vector3 iterPos = new SharpNav.Geometry.Vector3();
|
|
SharpNav.Geometry.Vector3 targetPos = new SharpNav.Geometry.Vector3();
|
|
navMeshQuery.ClosestPointOnPoly(startPt.Polygon, startPt.Position, ref iterPos);
|
|
navMeshQuery.ClosestPointOnPoly(path[npolys - 1], endPt.Position, ref targetPos);
|
|
|
|
smoothPath.Add(new Vector3(iterPos));
|
|
|
|
//float STEP_SIZE = 0.70f;
|
|
float SLOP = 0.15f;
|
|
while (npolys > 0 && smoothPath.Count < smoothPath.Capacity)
|
|
{
|
|
//find location to steer towards
|
|
SharpNav.Geometry.Vector3 steerPos = new SharpNav.Geometry.Vector3();
|
|
StraightPathFlags steerPosFlag = 0;
|
|
NavPolyId steerPosRef = NavPolyId.Null;
|
|
|
|
if (!GetSteerTarget(navMeshQuery, iterPos, targetPos, SLOP, path, ref steerPos, ref steerPosFlag, ref steerPosRef))
|
|
break;
|
|
|
|
bool endOfPath = (steerPosFlag & StraightPathFlags.End) != 0 ? true : false;
|
|
bool offMeshConnection = (steerPosFlag & StraightPathFlags.OffMeshConnection) != 0 ? true : false;
|
|
|
|
//find movement delta
|
|
SharpNav.Geometry.Vector3 delta = steerPos - iterPos;
|
|
float len = (float)Math.Sqrt(SharpNav.Geometry.Vector3.Dot(delta, delta));
|
|
|
|
//if steer target is at end of path or off-mesh link
|
|
//don't move past location
|
|
if ((endOfPath || offMeshConnection) && len < stepSize)
|
|
len = 1;
|
|
else
|
|
len = stepSize / len;
|
|
|
|
SharpNav.Geometry.Vector3 moveTgt = new SharpNav.Geometry.Vector3();
|
|
VMad(ref moveTgt, iterPos, delta, len);
|
|
|
|
//move
|
|
SharpNav.Geometry.Vector3 result = new SharpNav.Geometry.Vector3();
|
|
List<NavPolyId> visited = new List<NavPolyId>(pathSize);
|
|
NavPoint startPoint = new NavPoint(path[0], iterPos);
|
|
navMeshQuery.MoveAlongSurface(ref startPoint, ref moveTgt, out result, visited);
|
|
path.FixupCorridor(visited);
|
|
npolys = path.Count;
|
|
float h = 0;
|
|
navMeshQuery.GetPolyHeight(path[0], result, ref h);
|
|
result.Y = h;
|
|
iterPos = result;
|
|
|
|
//handle end of path when close enough
|
|
if (endOfPath && InRange(iterPos, steerPos, SLOP, 1000.0f))
|
|
{
|
|
//reached end of path
|
|
iterPos = targetPos;
|
|
if (smoothPath.Count < smoothPath.Capacity)
|
|
{
|
|
smoothPath.Add(new Vector3(iterPos));
|
|
}
|
|
break;
|
|
}
|
|
|
|
//store results
|
|
if (smoothPath.Count < smoothPath.Capacity)
|
|
{
|
|
smoothPath.Add(new Vector3(iterPos));
|
|
}
|
|
}
|
|
}
|
|
catch(Exception e)
|
|
{
|
|
Program.Log.Error(e.Message);
|
|
Program.Log.Error("Start pos {0} {1} {2} end pos {3} {4} {5}", startVec.X, startVec.Y, startVec.Z, endVec.X, endVec.Y, endVec.Z);
|
|
// todo: probably log this
|
|
return new List<Vector3>() { };
|
|
}
|
|
return smoothPath;
|
|
}
|
|
|
|
/// <summary>
|
|
/// Scaled vector addition
|
|
/// </summary>
|
|
/// <param name="dest">Result</param>
|
|
/// <param name="v1">Vector 1</param>
|
|
/// <param name="v2">Vector 2</param>
|
|
/// <param name="s">Scalar</param>
|
|
private static void VMad(ref SharpNav.Geometry.Vector3 dest, SharpNav.Geometry.Vector3 v1, SharpNav.Geometry.Vector3 v2, float s)
|
|
{
|
|
dest.X = v1.X + v2.X * s;
|
|
dest.Y = v1.Y + v2.Y * s;
|
|
dest.Z = v1.Z + v2.Z * s;
|
|
}
|
|
|
|
private static bool GetSteerTarget(NavMeshQuery navMeshQuery, SharpNav.Geometry.Vector3 startPos, SharpNav.Geometry.Vector3 endPos, float minTargetDist, SharpNav.Pathfinding.Path path,
|
|
ref SharpNav.Geometry.Vector3 steerPos, ref StraightPathFlags steerPosFlag, ref NavPolyId steerPosRef)
|
|
{
|
|
StraightPath steerPath = new StraightPath();
|
|
navMeshQuery.FindStraightPath(startPos, endPos, path, steerPath, 0);
|
|
int nsteerPath = steerPath.Count;
|
|
if (nsteerPath == 0)
|
|
return false;
|
|
|
|
//find vertex far enough to steer to
|
|
int ns = 0;
|
|
while (ns < nsteerPath)
|
|
{
|
|
if ((steerPath[ns].Flags & StraightPathFlags.OffMeshConnection) != 0 ||
|
|
!InRange(steerPath[ns].Point.Position, startPos, minTargetDist, 1000.0f))
|
|
break;
|
|
|
|
ns++;
|
|
}
|
|
|
|
//failed to find good point to steer to
|
|
if (ns >= nsteerPath)
|
|
return false;
|
|
|
|
steerPos = steerPath[ns].Point.Position;
|
|
steerPos.Y = startPos.Y;
|
|
steerPosFlag = steerPath[ns].Flags;
|
|
if (steerPosFlag == StraightPathFlags.None && ns == (nsteerPath - 1))
|
|
steerPosFlag = StraightPathFlags.End; // otherwise seeks path infinitely!!!
|
|
steerPosRef = steerPath[ns].Point.Polygon;
|
|
|
|
return true;
|
|
}
|
|
|
|
private static bool InRange(SharpNav.Geometry.Vector3 v1, SharpNav.Geometry.Vector3 v2, float r, float h)
|
|
{
|
|
float dx = v2.X - v1.X;
|
|
float dy = v2.Y - v1.Y;
|
|
float dz = v2.Z - v1.Z;
|
|
return (dx * dx + dz * dz) < (r * r) && Math.Abs(dy) < h;
|
|
}
|
|
#endregion
|
|
|
|
public static Vector3 GamePosToNavmeshPos(float x, float y, float z)
|
|
{
|
|
return new Vector3(x, -z, y);
|
|
}
|
|
|
|
public static Vector3 NavmeshPosToGamePos(float x, float y, float z)
|
|
{
|
|
return new Vector3(x, z, -y);
|
|
}
|
|
|
|
}
|
|
}
|