private static float[] PositionToArray(TundraVector3 vec)
{
float[] arr = new float[3];
arr[0] = vec.X;
arr[1] = vec.Y + YOffset;
arr[2] = vec.Z;
return(arr);
}
public void Deserialize(DeserializeEvent e)
{
ID = e.Reader.ReadUInt32();
TargetID = e.Reader.ReadUInt32();
Name = e.Reader.ReadString(Encoding.Unicode);
State = (EntityState)e.Reader.ReadByte();
Position = e.Reader.ReadSerializable <TundraVector3>();
Health = e.Reader.ReadInt32();
}
private void HandleNavigateTo(Message message)
{
var data = message.Deserialize <NavigateToPacket>();
if (data != null)
{
navComponent.SetDestination(TundraVector3.Create(data.Destination.X, data.Destination.Y, data.Destination.Z));
}
}
public void LoadLevel(string levelName)
{
NavMeshQuery = NavMeshSerializer.CreateMeshQuery(NavMeshSerializer.Deserialize("Levels/" + levelName + ".nav"));
//The YOffset requires the world to have a valid NavMesh-Position at (0,0,0)
Pathfinder.YOffset = Pathfinder.GetClosestPointOnNavMesh(NavMeshQuery, TundraVector3.Create(0f, 0f, 0f)).Y;
AddEntity(new Monster {
Name = "Monster", Position = TundraVector3.Zero
});
}
public void SetDestination(TundraVector3 destination)
{
CurrentPath = Pathfinder.ComputeSmoothPath(NavMeshQuery, Position, destination);
if (CurrentPath.Points.Count > 0)
{
_currentPathIndex = 0;
_currentDestination = CurrentPath.Points[0];
Destination = CurrentPath.Points[CurrentPath.PointsCount - 1];
}
else
{
CurrentPath = null;
}
}
public static TundraVector3 GetClosestPointOnNavMesh(NavMeshQuery navQuery, TundraVector3 pos)
{
float[] extents = new float[3];
for (int i = 0; i < 3; ++i)
{
extents[i] = 10.0f;
}
Detour.dtQueryFilter filter = new Detour.dtQueryFilter();
dtPolyRef startRef = 0;
float[] res = new float[3];
navQuery.findNearestPoly(PositionToArray(pos), extents, filter, ref startRef, ref res);
return(ArrayToPosition(res));
}
public void Integrate(float deltaTime)
{
var maxDistance = Speed * deltaTime;
if (TundraVector3.Distance(Position, _currentDestination) <= StoppingDistance)
{
if (_currentPathIndex < CurrentPath.PointsCount - 1)
{
_currentDestination = CurrentPath.Points[++_currentPathIndex];
Position = TundraVector3.MoveTowards(Position, _currentDestination, maxDistance);
}
else
{
CurrentPath = null;
}
}
else
{
Position = TundraVector3.MoveTowards(Position, _currentDestination, maxDistance);
}
}
public static SmoothPath ComputeSmoothPath(NavMeshQuery navQuery, TundraVector3 start, TundraVector3 end)
{
var startWorldPos = PositionToArray(start);
var endWorldPos = PositionToArray(end);
SmoothPath smoothPath = new SmoothPath();
if (navQuery == null)
{
return(smoothPath);
}
float[] extents = new float[3];
for (int i = 0; i < 3; ++i)
{
extents[i] = 10.0f;
}
uint startRef = 0;
uint endRef = 0;
float[] startPt = new float[3];
float[] endPt = new float[3];
Detour.dtQueryFilter filter = new Detour.dtQueryFilter();
navQuery.findNearestPoly(startWorldPos, extents, filter, ref startRef, ref startPt);
navQuery.findNearestPoly(endWorldPos, extents, filter, ref endRef, ref endPt);
const int maxPath = SmoothPath.MAX_POLYS;
uint[] path = new uint[maxPath];
int pathCount = -1;
navQuery.findPath(startRef, endRef, startPt, endPt, filter, path, ref pathCount, maxPath);
smoothPath.PointsCount = 0;
if (pathCount > 0)
{
// Iterate over the path to find smooth path on the detail mesh surface.
uint[] polys = new uint[SmoothPath.MAX_POLYS];
for (int i = 0; i < pathCount; ++i)
{
polys[i] = path[i];
}
int npolys = pathCount;
float[] iterPos = new float[3];
float[] targetPos = new float[3];
bool posOverPoly_dummy = false;
navQuery.closestPointOnPoly(startRef, startPt, iterPos, ref posOverPoly_dummy);
navQuery.closestPointOnPoly(polys[npolys - 1], endPt, targetPos, ref posOverPoly_dummy);
const float STEP_SIZE = 0.5f;
const float SLOP = 0.01f;
smoothPath.PointsCount = 0;
smoothPath.Points.Add(ArrayToPosition(iterPos));
//Detour.dtVcopy(smoothPath.Points, smoothPath.PointsCount * 3, iterPos, 0);
smoothPath.PointsCount++;
// Move towards target a small advancement at a time until target reached or
// when ran out of memory to store the path.
while (npolys != 0 && smoothPath.PointsCount < SmoothPath.MAX_SMOOTH)
{
// Find location to steer towards.
float[] steerPos = new float[3];
byte steerPosFlag = 0;
uint steerPosRef = 0;
if (!getSteerTarget(navQuery, iterPos, targetPos, SLOP,
polys, npolys, steerPos, ref steerPosFlag, ref steerPosRef))
{
break;
}
bool endOfPath = (steerPosFlag & (byte)Detour.dtStraightPathFlags.DT_STRAIGHTPATH_END) != 0 ? true : false;
bool offMeshConnection = (steerPosFlag & (byte)Detour.dtStraightPathFlags.DT_STRAIGHTPATH_OFFMESH_CONNECTION) != 0 ? true : false;
// Find movement delta.
float[] delta = new float[3];//, len;
float len = .0f;
Detour.dtVsub(delta, steerPos, iterPos);
len = MathF.Sqrt(Detour.dtVdot(delta, delta));
// If the steer target is end of path or off-mesh link, do not move past the location.
if ((endOfPath || offMeshConnection) && len < STEP_SIZE)
{
len = 1;
}
else
{
len = STEP_SIZE / len;
}
float[] moveTgt = new float[3];
Detour.dtVmad(moveTgt, iterPos, delta, len);
// Move
float[] result = new float[3];
uint[] visited = new uint[16];
int nvisited = 0;
navQuery.moveAlongSurface(polys[0], iterPos, moveTgt, filter,
result, visited, ref nvisited, 16);
npolys = fixupCorridor(polys, npolys, SmoothPath.MAX_POLYS, visited, nvisited);
npolys = fixupShortcuts(polys, npolys, navQuery);
float h = 0;
var getHeightStatus = navQuery.getPolyHeight(polys[0], result, ref h);
result[1] = h;
if ((getHeightStatus & Detour.DT_FAILURE) != 0)
{
Debug.WriteLine("Failed to getPolyHeight " + polys[0] + " pos " + result[0] + " " + result[1] + " " + result[2] + " h " + h);
}
Detour.dtVcopy(iterPos, result);
// Handle end of path and off-mesh links when close enough.
if (endOfPath && inRange(iterPos, 0, steerPos, 0, SLOP, 1.0f))
{
// Reached end of path.
Detour.dtVcopy(iterPos, targetPos);
if (smoothPath.PointsCount < SmoothPath.MAX_SMOOTH)
{
smoothPath.Points.Add(ArrayToPosition(iterPos));
//Detour.dtVcopy(smoothPath.Points, smoothPath.PointsCount * 3, iterPos, 0);
smoothPath.PointsCount++;
}
break;
}
else if (offMeshConnection && inRange(iterPos, 0, steerPos, 0, SLOP, 1.0f))
{
// Reached off-mesh connection.
float[] startPos = new float[3];//, endPos[3];
float[] endPos = new float[3];
// Advance the path up to and over the off-mesh connection.
uint prevRef = 0, polyRef = polys[0];
int npos = 0;
while (npos < npolys && polyRef != steerPosRef)
{
prevRef = polyRef;
polyRef = polys[npos];
npos++;
}
for (int i = npos; i < npolys; ++i)
{
polys[i - npos] = polys[i];
}
npolys -= npos;
// Handle the connection.
var status = navQuery.getAttachedNavMesh().getOffMeshConnectionPolyEndPoints(prevRef, polyRef, startPos, endPos);
if (Detour.dtStatusSucceed(status))
{
if (smoothPath.PointsCount < SmoothPath.MAX_SMOOTH)
{
smoothPath.Points.Add(ArrayToPosition(startPos));
//Detour.dtVcopy(smoothPath.Points, smoothPath.PointsCount * 3, startPos, 0);
smoothPath.PointsCount++;
// Hack to make the dotted path not visible during off-mesh connection.
if ((smoothPath.PointsCount & 1) != 0)
{
smoothPath.Points.Add(ArrayToPosition(startPos));
//Detour.dtVcopy(smoothPath.Points, smoothPath.PointsCount * 3, startPos, 0);
smoothPath.PointsCount++;
}
}
// Move position at the other side of the off-mesh link.
Detour.dtVcopy(iterPos, endPos);
float eh = 0.0f;
navQuery.getPolyHeight(polys[0], iterPos, ref eh);
iterPos[1] = eh;
}
}
// Store results.
if (smoothPath.PointsCount < SmoothPath.MAX_SMOOTH)
{
smoothPath.Points.Add(ArrayToPosition(iterPos));
//Detour.dtVcopy(smoothPath.Points, smoothPath.PointsCount * 3, iterPos, 0);
smoothPath.PointsCount++;
}
}
}
return(smoothPath);
}
private static TundraVector3 ArrayToPosition(float[] pos, int start = 0)
{
return(TundraVector3.Create(pos[start], pos[start + 1] - YOffset, pos[start + 2]));
}
public void Deserialize(DeserializeEvent e)
{
Destination = e.Reader.ReadSerializable <TundraVector3>();
}
