public static Vec3 ProjectPosOnPath(Vec3 pos, List <Vec3> path, out int projection_segment_start_idx, out float scalar_projection)
{
Vec3 projection = path[0];
projection_segment_start_idx = 0;
scalar_projection = 0;
float dist = pos.Distance2D(projection);
// find closest pos projection on path
for (int i = 0; i < path.Count - 1; ++i)
{
var p = Vec3.ProjectPointOnSegment(path[i], path[i + 1], pos, out var sp, true);
var d = pos.Distance2D(p);
if (d < dist)
{
projection = p;
dist = d;
projection_segment_start_idx = i;
scalar_projection = sp;
}
}
return(projection);
}
public static void ProjectAndMovePosOnPath(Vec3 pos, List <Vec3> path, float move_distance, out Vec3 projection, out Vec3 moved_projection, out int projection_segment_start_idx, out float scalar_projection)
{
projection = ProjectPosOnPath(pos, path, out projection_segment_start_idx, out scalar_projection);
float dist = pos.Distance2D(projection);
moved_projection = projection;
float move_dist_remaining = move_distance - dist; // if we are far away from path, we care mostly about getting close to it first
int current_segment_start_idx = projection_segment_start_idx;
while (move_dist_remaining > 0 && current_segment_start_idx < path.Count - 1)
{
float dist_to_segment_end = moved_projection.Distance2D(path[current_segment_start_idx + 1]);
if (dist_to_segment_end > move_dist_remaining)
{
var move_dir = path[current_segment_start_idx + 1] - path[current_segment_start_idx];
move_dir.Normalize2D();
moved_projection += move_dir * move_dist_remaining;
break;
}
moved_projection = path[current_segment_start_idx + 1];
move_dist_remaining -= dist_to_segment_end;
++current_segment_start_idx;
}
}
public bool Overlaps2D(Vec3 circle_center, float radius, bool tangential_ok = false)
{
Vec3 center_aligned = Align(circle_center);
float dist = circle_center.Distance2D(center_aligned);
return(tangential_ok ? dist < radius : dist <= radius);
}
private void UpdatePathProgression(Vec3 current_pos)
{
bool any_node_reached = false;
Vec3 reached_pos = Vec3.Empty;
// check and removed reached nodes from path
using (new ReadLock(PathLock, true))
{
while (m_Path.Count > 0)
{
float precision = DefaultPrecision;
if (m_PrecisionOverride > 0)
{
precision = m_PrecisionOverride;
}
else if (m_Path.Count == 1)
{
precision = Precision;
}
if (current_pos.Distance2D(m_Path[0]) > precision)
{
break;
}
reached_pos = m_Path[0];
using (new WriteLock(PathLock))
m_Path.RemoveAt(0);
any_node_reached = true;
ResetAntiStuckPrecition(current_pos);
}
}
Vec3 destination = Destination;
// update destination arrived
if (any_node_reached && !destination.IsEmpty)
{
if (IsDestinationReached(DestType.All))
{
using (new WriteLock(InputLock))
{
if (m_DestinationType != DestType.BackTrack &&
(m_DestinationsHistory.Count == 0 || (!m_DestinationsHistory[m_DestinationsHistory.Count - 1].Equals(reached_pos) &&
m_DestinationsHistory[m_DestinationsHistory.Count - 1].Distance(reached_pos) > MIN_DEST_DIST_TO_ADD_TO_HISTORY)))
{
m_DestinationsHistory.Add(reached_pos);
}
}
NotifyOnDestinationReached(m_DestinationType, destination);
ResetAntiStuckPathing(current_pos);
//m_Navmesh.Log("[Nav] Dest " + m_Destination + " [" + m_DestinationType + "] reached!");
}
if (IsDestinationReached(DestType.Waypoint))
{
using (new WriteLock(InputLock))
m_Waypoints.RemoveAt(0);
if (m_Waypoints.Count == 0)
{
ClearDestination(DestType.Waypoint);
}
}
if (IsDestinationReached(DestType.User))
{
ClearDestination(DestType.User);
}
if (IsDestinationReached(DestType.BackTrack))
{
--m_HistoryDestId;
if (!BackTrackEnabled)
{
ClearDestination(DestType.BackTrack);
}
}
}
}
private void UpdatePath()
{
if (!m_Navmesh.IsNavDataAvailable)
{
return;
}
Vec3 destination = Vec3.Empty;
DestType dest_type = DestType.None;
// make sure destination and its type are in sync
using (new ReadLock(InputLock))
{
destination = m_Destination;
dest_type = m_DestinationType;
}
Vec3 current_pos = CurrentPos;
if (current_pos.IsEmpty || destination.IsEmpty)
{
return;
}
List <Vec3> new_path = new List <Vec3>();
FindPath(current_pos, destination, MovementFlags, ref new_path, PATH_NODES_MERGE_DISTANCE, true, false, m_PathRandomCoeffOverride > 0 ? m_PathRandomCoeffOverride : PathRandomCoeff, m_PathBounce, PathNodesShiftDist);
// verify whenever some point of path was not already passed during its calculation (this may take place when path calculations took long time)
// this is done by finding first path segment current position can be casted on and removing all points preceding this segment including segment origin
current_pos = CurrentPos;
while (new_path.Count > 1)
{
Vec3 segment = new_path[1] - new_path[0];
Vec3 segment_origin_to_current_pos = current_pos - new_path[0];
float segment_len = segment.Length2D();
float projection_len = segment.Dot2D(segment_origin_to_current_pos) / segment_len;
// current position is already 'after' segment origin so remove it from path
if (projection_len > 0)
{
float distance_from_segment = -1;
// additionally verify if current pos is close enough to segment
if (projection_len > segment_len)
{
distance_from_segment = current_pos.Distance2D(new_path[1]);
}
else
{
distance_from_segment = current_pos.Distance2D(segment.Normalized2D() * projection_len);
}
if (distance_from_segment < DefaultPrecision)
{
new_path.RemoveAt(0);
}
else
{
break;
}
}
else
{
break;
}
}
using (new WriteLock(PathLock))
{
// reset override when first destination from path changed
if (m_Path.Count == 0 || (new_path.Count > 0 && !m_Path[0].Equals(new_path[0])))
{
ResetAntiStuckPrecition(current_pos);
}
m_Path = new_path;
m_PathDestination = destination;
m_PathDestType = dest_type;
}
}
