From 5615914c7eaff58a4fe7bf04d8e964f22faac2f4 Mon Sep 17 00:00:00 2001 From: Pim Nelissen Date: Wed, 25 Feb 2026 14:20:11 +0100 Subject: [PATCH] update projection utlity functions --- src/pg_rad/utils/positional.py | 50 ---------- src/pg_rad/utils/projection.py | 164 +++++++++++++++++++++++++++++++++ 2 files changed, 164 insertions(+), 50 deletions(-) delete mode 100644 src/pg_rad/utils/positional.py create mode 100644 src/pg_rad/utils/projection.py diff --git a/src/pg_rad/utils/positional.py b/src/pg_rad/utils/positional.py deleted file mode 100644 index e8b2ddd..0000000 --- a/src/pg_rad/utils/positional.py +++ /dev/null @@ -1,50 +0,0 @@ -from typing import List - -import numpy as np - -from pg_rad.exceptions.exceptions import OutOfBoundsError - - -def rel_to_abs_source_position( - x_list: List, - y_list: List, - path_z: int | float, - along_path: int | float, - side: str, - dist_from_path: int | float, - z_rel: int | float = 0 -): - path = np.column_stack((x_list, y_list)) - segments = np.diff(path, axis=0) - segment_lengths = np.hypot(segments[:, 0], segments[:, 1]) - - arc_length = np.cumsum(segment_lengths) - arc_length = np.insert(arc_length, 0, 0) - - # find index of the segment where the source should be placed - s_i = np.searchsorted(arc_length, along_path, side='right') - 1 - if not 0 <= s_i < len(segments): - raise OutOfBoundsError( - "along_path must be less than the length of the path!" - ) - - # fraction of segment length where to place the point source - segment_fraction = ( - (along_path - arc_length[s_i]) - / segment_lengths[s_i] - ) - - # tangential vector at point where point source to be placed - point_on_path = path[s_i] + segment_fraction * segments[s_i] - tangent_vec = segments[s_i] / segment_lengths[s_i] - - if side not in {"left", "right"}: - raise ValueError("side must be 'left' or 'right'") - - orientation = 1 if side == "left" else -1 - perp_vec = orientation * np.array([-tangent_vec[1], tangent_vec[0]]) - - x_abs, y_abs = point_on_path + dist_from_path * perp_vec - z_abs = path_z + z_rel - - return x_abs, y_abs, z_abs diff --git a/src/pg_rad/utils/projection.py b/src/pg_rad/utils/projection.py new file mode 100644 index 0000000..c796097 --- /dev/null +++ b/src/pg_rad/utils/projection.py @@ -0,0 +1,164 @@ +from typing import List, Tuple + +import numpy as np + +from pg_rad.exceptions.exceptions import OutOfBoundsError + + +def rel_to_abs_source_position( + x_list: List, + y_list: List, + path_z: int | float, + along_path: int | float, + side: str, + dist_from_path: int | float, + z_rel: int | float = 0 +): + path = np.column_stack((x_list, y_list)) + segments = np.diff(path, axis=0) + segment_lengths = np.hypot(segments[:, 0], segments[:, 1]) + + arc_length = np.cumsum(segment_lengths) + arc_length = np.insert(arc_length, 0, 0) + + # find index of the segment where the source should be placed + s_i = np.searchsorted(arc_length, along_path, side='right') - 1 + if not 0 <= s_i < len(segments): + raise OutOfBoundsError( + "along_path must be less than the length of the path!" + ) + + # fraction of segment length where to place the point source + segment_fraction = ( + (along_path - arc_length[s_i]) + / segment_lengths[s_i] + ) + + # tangential vector at point where point source to be placed + point_on_path = path[s_i] + segment_fraction * segments[s_i] + tangent_vec = segments[s_i] / segment_lengths[s_i] + + if side not in {"left", "right"}: + raise ValueError("side must be 'left' or 'right'") + + orientation = 1 if side == "left" else -1 + perp_vec = orientation * np.array([-tangent_vec[1], tangent_vec[0]]) + + x_abs, y_abs = point_on_path + dist_from_path * perp_vec + z_abs = path_z + z_rel + + return x_abs, y_abs, z_abs + + +def minimal_distance_to_path( + x_list: List[float], + y_list: List[float], + path_z: float, + pos: Tuple[float, float, float] +) -> float: + """ + Compute minimal Euclidean distance from pos (x,y,z) + to interpolated polyline path at height path_z. + """ + x, y, z = pos + + path = np.column_stack((x_list, y_list)) + point_xy = np.array([x, y]) + + segments = np.diff(path, axis=0) + segment_lengths = np.hypot(segments[:, 0], segments[:, 1]) + + min_dist = np.inf + + for p0, seg, seg_len in zip(path[:-1], segments, segment_lengths): + if seg_len == 0: + continue + + t = np.dot(point_xy - p0, seg) / (seg_len ** 2) + t = np.clip(t, 0.0, 1.0) + + projection_xy = p0 + t * seg + + dx, dy = point_xy - projection_xy + dz = z - path_z + + dist = np.sqrt(dx**2 + dy**2 + dz**2) + + if dist < min_dist: + min_dist = dist + + return min_dist + + +def abs_to_rel_source_position( + x_list: List, + y_list: List, + path_z: int | float, + pos: Tuple[float, float, float], +) -> Tuple[float, float, str, float]: + """ + Convert absolute (x,y,z) position into: + along_path, + dist_from_path, + side ("left" or "right"), + z_rel + """ + + x, y, z = pos + + path = np.column_stack((x_list, y_list)) + point = np.array([x, y]) + + segments = np.diff(path, axis=0) + segment_lengths = np.hypot(segments[:, 0], segments[:, 1]) + + arc_length = np.cumsum(segment_lengths) + arc_length = np.insert(arc_length, 0, 0) + + min_dist = np.inf + best_projection = None + best_s_i = None + best_fraction = None + best_signed_dist = None + + for i, (p0, seg, seg_len) in enumerate(zip(path[:-1], segments, segment_lengths)): + if seg_len == 0: + continue + + # project point onto infinite line + t = np.dot(point - p0, seg) / (seg_len ** 2) + + # clamp to segment + t_clamped = np.clip(t, 0.0, 1.0) + + projection = p0 + t_clamped * seg + diff_vec = point - projection + dist = np.linalg.norm(diff_vec) + + if dist < min_dist: + min_dist = dist + best_projection = projection + best_s_i = i + best_fraction = t_clamped + + # tangent and perpendicular + tangent_vec = seg / seg_len + perp_vec = np.array([-tangent_vec[1], tangent_vec[0]]) + + signed_dist = np.dot(diff_vec, perp_vec) + best_signed_dist = signed_dist + + if best_projection is None: + raise ValueError("Could not project point onto path.") + + # Compute along_path + along_path = arc_length[best_s_i] + best_fraction * segment_lengths[best_s_i] + + # Side and distance + side = "left" if best_signed_dist > 0 else "right" + dist_from_path = abs(best_signed_dist) + + # z relative + z_rel = z - path_z + + return along_path, dist_from_path, side, z_rel \ No newline at end of file