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; } }