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