From a4b3759533f4fd939c61f4b871a525968e132670 Mon Sep 17 00:00:00 2001 From: ioes Date: Fri, 16 Oct 2026 00:03:28 +0900 Subject: [PATCH] Avoid false shallow-water barriers from depth roundoff --- src/posim_routing/coastal.py | 5 +++-- src/posim_routing/navigation.py | 8 ++++++-- src/posim_routing/shallow.py | 3 ++- tests/test_shallow.py | 16 ++++++++++++++++ 4 files changed, 27 insertions(+), 5 deletions(-) diff --git a/src/posim_routing/coastal.py b/src/posim_routing/coastal.py index 70d2e6c..a60b98c 100644 --- a/src/posim_routing/coastal.py +++ b/src/posim_routing/coastal.py @@ -17,6 +17,7 @@ from pyproj import Geod from scipy.ndimage import distance_transform_edt, maximum_filter, label from scipy.spatial import cKDTree +from .navigation import DEPTH_EPSILON_M GEO = Geod(ellps="WGS84") COAST_DISCOVERY_M = 80000. @@ -139,7 +140,7 @@ def coastal_corridor(terrain, spec, start, target, timeout=25., guard=None, prog field = TerrainField(terrain["xs"], terrain["ys"], terrain["heights"]) xs, ys, heights = field.xs, field.ys, field.heights finite = np.isfinite(heights) - land = finite & (heights > -MINIMUM_DEPTH_M) + land = finite & (heights > -MINIMUM_DEPTH_M+DEPTH_EPSILON_M) if guard is not None: land |= ~guard.grid(xs, ys) if not land.any(): @@ -159,7 +160,7 @@ def coastal_corridor(terrain, spec, start, target, timeout=25., guard=None, prog size = (2*math.ceil(margin/dy)+1, 2*math.ceil(margin/dx)+1) bounds = maximum_filter(np.where(finite, heights, math.inf), size=size, mode="constant", cval=math.inf) - safe = bounds <= -MINIMUM_DEPTH_M + safe = bounds <= -MINIMUM_DEPTH_M+DEPTH_EPSILON_M if guard is not None: safe &= guard.grid(xs, ys, margin) coast_distance = distance_transform_edt(~land, sampling=(dy, dx)) diff --git a/src/posim_routing/navigation.py b/src/posim_routing/navigation.py index 223b28f..39e875a 100644 --- a/src/posim_routing/navigation.py +++ b/src/posim_routing/navigation.py @@ -9,6 +9,10 @@ import time import numpy as np +# Interpolating a flat two-metre floor can produce -1.9999999999999998. +# Ignore arithmetic roundoff without changing the represented depth policy. +DEPTH_EPSILON_M = 1e-9 + class TerrainField: def __init__(self, xs, ys, heights): @@ -85,7 +89,7 @@ def __init__(self, field, start, boat, step=4., timeout=25., maximum_radius=600. def safe(self, xy, z, radius=None): if math.dist(xy,self.origin)>self.maximum_radius:return False floor=self.field.maximum(*xy,self.radius if radius is None else radius) - return floor is not None and floor<0 and z-floor>=self.clearance + return floor is not None and floor<0 and z-floor>=self.clearance-DEPTH_EPSILON_M def segment(self, a, b, record=False): distance=math.dist(a[:2],b[:2]);count=max(1,math.ceil(distance/1.)) @@ -97,7 +101,7 @@ def segment(self, a, b, record=False): f=k/count;xy=[a[i]+(b[i]-a[i])*f for i in (0,1)] if math.dist(xy,self.origin)>self.maximum_radius:return False floor=self.field.maximum(*xy,radius) - if floor is None or floor>=0 or z-floor=0 or z-floor -MINIMUM_DEPTH_M) + shallow=~np.isfinite(field.heights) | (field.heights > -MINIMUM_DEPTH_M+DEPTH_EPSILON_M) distances=distance_transform_edt(~shallow,sampling=(dy,dx)) return RegularGridInterpolator((field.ys,field.xs),distances, bounds_error=False,fill_value=0.) diff --git a/tests/test_shallow.py b/tests/test_shallow.py index 8d92a5a..5b18dd7 100644 --- a/tests/test_shallow.py +++ b/tests/test_shallow.py @@ -14,6 +14,22 @@ def terrain(): return dict(xs=axis.tolist(),ys=axis.tolist(),heights=np.where(x<0,-1.,-15.).tolist()) class ShallowExit(unittest.TestCase): + def test_roundoff_on_exact_depth_plateau_does_not_create_a_shoal(self): + axis=np.linspace(-1000,1000,41) + for floor,expected in ((np.nextafter(-2.,0.),True),(-2.+1e-6,False)): + raw=TerrainField(axis,axis,np.full((41,41),floor)) + planner=DetourPlanner(CachedTerrain(raw),[0,0,0],True) + planner.radius=200.;planner.clearance=2. + self.assertEqual(planner.safe([0,0],0.),expected) + self.assertEqual(planner.segment([0,0,0],[100,100,0]),expected) + clearance=clearance_field(raw) + self.assertEqual(float(clearance([[0,0]])[0])>0,expected) + def test_roundoff_tolerance_does_not_accept_unknown_depth(self): + axis=np.linspace(-1000,1000,41);heights=np.full((41,41),-2.) + heights[20,20]=np.nan + planner=DetourPlanner(CachedTerrain(TerrainField(axis,axis,heights)),[0,0,0],True) + planner.radius=200.;planner.clearance=2. + self.assertFalse(planner.safe([0,0],0.)) def test_already_clear_position_needs_no_departure(self): self.assertIsNone(exit_corridor(terrain(),SPEC,geo(400,0),geo(1000,2000))) def test_inside_buffer_exits_under_propulsion_with_increasing_separation(self):