public void Update(float dt)
{
if (!m_Destination.IsZero())
{
m_Navigator.Destination = new destination(m_Destination);
}
m_LastGotoPos = m_Navigator.GoToPosition.pos;
if (m_LastGotoPos.IsZero())
{
return;
}
Vec3 wanted_dir = Vec3.ZERO;
float dist = 0;
if (!Paused && !SimulateStuck && !m_LastGotoPos.Equals(m_Navigator.CurrentPos))
{
wanted_dir = m_LastGotoPos - m_Navigator.CurrentPos;
dist = wanted_dir.Length();
wanted_dir.Normalize();
}
m_Navigator.IsStandingOnPurpose = m_Navigator.ThreatAhead > 0;
//turn
m_Direction = Vec3.RotateTowards(m_Direction, wanted_dir, m_AngularSpeed * dt);
//move
if (m_Navigator.ThreatAhead == 0)
{
m_Navigator.CurrentPos = m_Navigator.CurrentPos + m_Direction * Math.Min(m_Speed * dt, dist);
}
}
private void PostProcessPath(ref List <Vec3> path, float merge_distance, float shift_nodes_distance)
{
if (merge_distance > 0)
{
for (int i = 0; i < path.Count - 1; ++i)
{
int start_count = path.Count;
Vec3 merge_point = path[i];
while (i < path.Count - 1 && path[i].Distance2D(path[i + 1]) < merge_distance)
{
merge_point = merge_point + path[i + 1];
path.RemoveAt(i + 1);
}
if (path.Count != start_count)
{
path[i] = merge_point / (float)(start_count - path.Count + 1);
}
}
}
// shift points to increase movement accuracy
if (shift_nodes_distance > 0)
{
for (int i = path.Count - 2; i > 0; --i)
{
Vec3 dir_to_next = path[i] - path[i - 1];
dir_to_next.Normalize();
path[i] += dir_to_next * shift_nodes_distance;
}
}
}
public void Update(float dt)
{
if (m_GotoPosUpdateTimer.ElapsedMilliseconds > GOTO_POS_UPDATE_INTERVAL)
{
m_LastGotoPos = m_Navigator.GoToPosition.pos;
m_GotoPosUpdateTimer.Restart();
}
if (!m_Destination.IsZero())
{
m_Navigator.Destination = new destination(m_Destination);
}
if (m_LastGotoPos.IsZero())
{
return;
}
Vec3 dir = Vec3.ZERO;
float dist = 0;
if (!Paused && !SimulateStuck && !m_LastGotoPos.Equals(m_Navigator.CurrentPos))
{
dir = m_LastGotoPos - m_Navigator.CurrentPos;
dist = dir.Length();
dir.Normalize();
}
m_Navigator.IsStandingOnPurpose = m_Navigator.IsThreatAhead;
if (!m_Navigator.IsThreatAhead)
{
m_Navigator.CurrentPos = m_Navigator.CurrentPos + dir * Math.Min(m_Speed * dt, dist);
}
}
public Vec3 GetRandomPosInRing(Vec3 ring_center, float min_radius, float max_radius)
{
var potential_grid_cells = GetCellsOverlappedByCircle(m_GridCells, ring_center, max_radius);
List <Cell> cells = new List <Cell>();
foreach (GridCell grid_cell in potential_grid_cells)
{
cells.AddRange(grid_cell.Cells.FindAll(x => x.Overlaps(ring_center, max_radius) && !x.AABB.Inside(ring_center, min_radius)));
}
Cell random_cell = cells[Rng.Next(cells.Count)];
Vec3 random_pos = random_cell.AABB.GetRandomPos();
float dist = random_pos.Distance(ring_center);
if (dist > min_radius && dist <= max_radius)
{
return(random_pos);
}
//align position to ring
Vec3 dir_to_random_pos = random_pos - ring_center;
dir_to_random_pos.Normalize();
random_pos = ring_center + dir_to_random_pos * (min_radius + (float)Rng.NextDouble() * (max_radius - min_radius));
return(random_cell.AABB.Align(random_pos));
}
public bool RayTrace(Vec3 from, Vec3 to, MovementFlag flags, out Vec3 intersection)
{
using (new ReadLock(DataLock))
{
intersection = Vec3.Empty;
Vec3 ray_dir = to - from;
ray_dir.Normalize();
foreach (Cell c in m_AllCells)
{
if (!c.HasFlags(flags))
{
continue;
}
Vec3 new_intersection = null;
if (c.AABB.RayTest(from, ray_dir, ref new_intersection) && from.DistanceSqr(new_intersection) < from.DistanceSqr(intersection))
{
intersection = new_intersection;
}
}
return(!intersection.IsEmpty);
}
}
public Plane(Vec3 p1, Vec3 normal)
{
normal.Normalize();
A = normal.X;
B = normal.Y;
C = normal.Z;
D = -A * p1.X - B * p1.Y - C * p1.Z;
}
public Plane(Vec3 p1, Vec3 p2, Vec3 p3)
{
Vec3 v1 = p2 - p1;
Vec3 v2 = p3 - p1;
Vec3 plane_norm = v1.Cross(v2);
plane_norm.Normalize();
A = plane_norm.X;
B = plane_norm.Y;
C = plane_norm.Z;
D = -A * p1.X - B * p1.Y - C * p1.Z;
}
private void SmoothenPath(ref List <path_pos> path, MovementFlag flags, int skip_first_count = 0)
{
int ray_start_index = skip_first_count;
Vec3 intersection = null;
while (ray_start_index + 2 < path.Count)
{
path_pos ray_start_data = path[ray_start_index];
path_pos intermediate_data = path[ray_start_index + 1];
path_pos ray_end_data = path[ray_start_index + 2];
// try remove middle point completely
if (m_Navmesh.RayCast2D(ray_start_data.pos, ray_end_data.pos, flags, ref intersection, false))
{
path.RemoveAt(ray_start_index + 1);
}
else
{
++ray_start_index;
}
}
// perform second smoothing pass after getting rid of all unnecessary points to get better performance and results
// this pass is basically "moving" from point to point and checking at which step (in-between points) I can already see the next point
if (PathSmoothingPrecision > 0)
{
ray_start_index = skip_first_count;
while (ray_start_index + 2 < path.Count)
{
path_pos ray_start_data = path[ray_start_index];
path_pos intermediate_data = path[ray_start_index + 1];
path_pos ray_end_data = path[ray_start_index + 2];
Vec3 dir = intermediate_data.pos - ray_start_data.pos;
float length = dir.Normalize();
int steps = (int)(length / PathSmoothingPrecision);
for (int i = 1; i < steps; ++i) // checking 0 is unnecessary since this is the current path
{
if (m_Navmesh.RayCast2D(ray_start_data.pos + dir * (float)i * PathSmoothingPrecision, ray_end_data.pos, flags, ref intersection, false))
{
path[ray_start_index + 1] = new path_pos(ray_start_data.pos + dir * (float)i * PathSmoothingPrecision, intermediate_data.cell);
break;
}
}
++ray_start_index;
}
}
}
private bool InternalSegmentTest(Vec3 start, Vec3 end, ref Vec3 result, int num_dim)
{
Vec3 ray_dir = end - start;
float length = num_dim > 2 ? ray_dir.Normalize() : ray_dir.Normalize2D();
float length_2 = length * length;
if (InternalRayTest(start, ray_dir, ref result, num_dim))
{
if ((num_dim > 2 ? result.DistanceSqr(start) : result.Distance2DSqr(start)) <= length_2)
{
return(true);
}
result = end;
return(false);
}
return(false);
}
public void Update(float dt)
{
if (!m_Destination.IsZero())
{
m_Navigator.Destination = new destination(m_Destination);
}
m_LastGotoPos = m_Navigator.GoToPosition.pos;
var newPos = m_Navigator.CurrentPos;
if (!m_LastGotoPos.IsZero())
{
Vec3 wanted_dir = Vec3.ZERO;
float dist = 0;
wanted_dir = m_LastGotoPos - m_Navigator.CurrentPos;
dist = wanted_dir.Length();
wanted_dir.Normalize();
//turn
if (m_AngularSpeed > 0)
{
m_Direction = Vec3.RotateTowards(m_Direction, wanted_dir, m_AngularSpeed * dt);
}
else
{
m_Direction = wanted_dir;
}
//move
if (m_Navigator.ThreatAhead.Item2 == 0 && !Paused && !SimulateStuck)
{
newPos += m_Direction * Math.Min(m_Speed * dt, dist);
}
}
m_Navigator.CurrentPos = newPos;
m_Navigator.IsStandingOnPurpose = m_Navigator.ThreatAhead.Item2 > 0;
}
// Aquires DataLock (read); returns true when there is no obstacle
private bool RayCast(Vec3 from, Cell from_cell, Vec3 to, MovementFlag flags, ref Vec3 intersection, bool test_2d, bool ignore_movement_cost, ref HashSet <Cell> ignored_cells)
{
using (new ReadLock(DataLock))
{
if (to.IsEmpty)
{
intersection = new Vec3(from);
return(false);
}
if (from_cell == null && !GetCellContaining(from, out from_cell, flags, false, false, -1, test_2d, 2, ignored_cells))
{
intersection = new Vec3(from);
return(false);
}
if (test_2d ? from_cell.Contains2D(to) : from_cell.Contains(to, 2))
{
intersection = new Vec3(to);
return(true);
}
ignored_cells.Add(from_cell);
Vec3 ray_dir = to - from;
if (test_2d)
{
ray_dir.Normalize2D();
}
else
{
ray_dir.Normalize();
}
Vec3 ray_origin = new Vec3(from);
// check if intersection in
foreach (Cell.Neighbour neighbour in from_cell.Neighbours)
{
if (neighbour.cell.Disabled || (neighbour.connection_flags & flags) != flags || (!ignore_movement_cost && neighbour.cell.MovementCostMult > from_cell.MovementCostMult))
{
continue;
}
Cell neighbour_cell = neighbour.cell;
if (ignored_cells.Contains(neighbour_cell))
{
continue;
}
bool ray_test_result = test_2d ? neighbour_cell.AABB.RayTest2D(ray_origin, ray_dir, ref intersection) :
neighbour_cell.AABB.RayTest(ray_origin, ray_dir, ref intersection);
if (ray_test_result)
{
AABB shared_aabb = test_2d ? from_cell.AABB.Intersect2D(neighbour_cell.AABB, true) :
from_cell.AABB.Intersect(neighbour_cell.AABB, true);
// ray intersects on connection plane
if (shared_aabb != null)
{
bool accepted = test_2d ? shared_aabb.Contains2D(intersection) :
shared_aabb.Contains(intersection);
if (accepted && RayCast(intersection, neighbour_cell, to, flags, ref intersection, test_2d, ignore_movement_cost, ref ignored_cells))
{
return(true);
}
}
}
}
return(false);
}
}
