fix: nearest_point_on_segment returns negative/beyond-length distances

[andrewwhitecdw]

This PR addresses the following issue in exts/omni.ext.mobility_gen/omni/ext/mobility_gen/utils/path_utils.py: nearest_point_on_segment returns negative/beyond-length distances.

Changes

• exts/omni.ext.mobility_gen/omni/ext/mobility_gen/utils/path_utils.py: nearest_point_on_segment returns negative/beyond-length distances.

Details

Before:

def nearest_point_on_segment(a: np.ndarray, b: np.ndarray, c: np.ndarray):
    a2b = b - a
    a2c = c - a
    a2b_mag = np.sqrt(np.sum(a2b**2))
    a2b_norm = a2b / (a2b_mag + 1e-6)
    dist = np.dot(a2c, a2b_norm)
    if dist < 0:
        return a, dist
    elif dist > a2b_mag:
        return b, dist
    else:
        return a + a2b_norm * dist, dist

After:

def nearest_point_on_segment(a: np.ndarray, b: np.ndarray, c: np.ndarray):
    a2b = b - a
    a2c = c - a
    a2b_mag = np.sqrt(np.sum(a2b**2))
    a2b_norm = a2b / (a2b_mag + 1e-6)
    dist = np.dot(a2c, a2b_norm)
    if dist < 0:
        return a, 0.0
    elif dist > a2b_mag:
        return b, a2b_mag
    else:
        return a + a2b_norm * dist, dist