1use nalgebra::DMatrix;
9
10const INF: f64 = 1e20;
11
12fn dt_1d(d: &mut [f64]) {
16 let n = d.len();
17 if n == 0 {
18 return;
19 }
20 let mut v = vec![0usize; n + 1];
21 let mut z = vec![0.0f64; n + 1];
22 let mut k: usize = 0;
23 v[0] = 0;
24 z[0] = -INF;
25 z[1] = INF;
26
27 for q in 1..n {
28 let vk = v[k];
29 let mut s = ((d[q] + (q * q) as f64) - (d[vk] + (vk * vk) as f64))
30 / (2.0 * q as f64 - 2.0 * vk as f64);
31 while s <= z[k] {
32 k -= 1;
33 let vk2 = v[k];
34 s = ((d[q] + (q * q) as f64) - (d[vk2] + (vk2 * vk2) as f64))
35 / (2.0 * q as f64 - 2.0 * vk2 as f64);
36 }
37 k += 1;
38 v[k] = q;
39 z[k] = s;
40 if k + 1 < z.len() {
41 z[k + 1] = INF;
42 }
43 }
44
45 k = 0;
46 for q in 0..n {
47 while k + 1 < z.len() && z[k + 1] < q as f64 {
48 k += 1;
49 }
50 let dx = q as f64 - v[k] as f64;
51 d[q] = dx * dx + d[v[k]];
52 }
53}
54
55pub fn compute_udf(obstacles: &DMatrix<bool>) -> DMatrix<f64> {
64 let nrows = obstacles.nrows();
65 let ncols = obstacles.ncols();
66
67 let mut edt = DMatrix::from_fn(
69 nrows,
70 ncols,
71 |r, c| {
72 if obstacles[(r, c)] {
73 0.0
74 } else {
75 INF
76 }
77 },
78 );
79
80 for r in 0..nrows {
82 let mut row: Vec<f64> = (0..ncols).map(|c| edt[(r, c)]).collect();
83 dt_1d(&mut row);
84 for c in 0..ncols {
85 edt[(r, c)] = row[c];
86 }
87 }
88
89 for c in 0..ncols {
91 let mut col: Vec<f64> = (0..nrows).map(|r| edt[(r, c)]).collect();
92 dt_1d(&mut col);
93 for r in 0..nrows {
94 edt[(r, c)] = col[r];
95 }
96 }
97
98 edt.map(|v| v.sqrt())
100}
101
102pub fn compute_sdf(obstacles: &DMatrix<bool>) -> DMatrix<f64> {
114 let udf_obstacle = compute_udf(obstacles);
115 let inverted = obstacles.map(|v| !v);
116 let udf_free = compute_udf(&inverted);
117 udf_obstacle - udf_free
118}
119
120#[cfg(test)]
121mod tests {
122 use super::*;
123 use nalgebra::DMatrix;
124
125 fn make_obstacles() -> DMatrix<bool> {
126 #[rustfmt::skip]
128 let data: Vec<bool> = vec![
129 true, false, false, false, false,
130 false, true, true, true, false,
131 false, true, true, true, false,
132 false, false, true, true, false,
133 false, false, true, false, false,
134 ];
135 DMatrix::from_row_slice(5, 5, &data)
136 }
137
138 #[test]
139 fn test_udf_obstacle_cells_are_zero() {
140 let obs = make_obstacles();
141 let udf = compute_udf(&obs);
142
143 for r in 0..obs.nrows() {
145 for c in 0..obs.ncols() {
146 if obs[(r, c)] {
147 assert!(
148 udf[(r, c)].abs() < 1e-10,
149 "Obstacle cell ({}, {}) should be 0, got {}",
150 r,
151 c,
152 udf[(r, c)]
153 );
154 }
155 }
156 }
157 }
158
159 #[test]
160 fn test_udf_non_negative() {
161 let obs = make_obstacles();
162 let udf = compute_udf(&obs);
163
164 for r in 0..udf.nrows() {
165 for c in 0..udf.ncols() {
166 assert!(
167 udf[(r, c)] >= 0.0,
168 "UDF should be non-negative at ({}, {}), got {}",
169 r,
170 c,
171 udf[(r, c)]
172 );
173 }
174 }
175 }
176
177 #[test]
178 fn test_udf_free_cells_positive() {
179 let obs = make_obstacles();
180 let udf = compute_udf(&obs);
181
182 for r in 0..obs.nrows() {
183 for c in 0..obs.ncols() {
184 if !obs[(r, c)] {
185 assert!(
186 udf[(r, c)] > 0.0,
187 "Free cell ({}, {}) should have positive distance",
188 r,
189 c
190 );
191 }
192 }
193 }
194 }
195
196 #[test]
197 fn test_sdf_sign_convention() {
198 let obs = make_obstacles();
199 let sdf = compute_sdf(&obs);
200
201 for r in 0..obs.nrows() {
203 for c in 0..obs.ncols() {
204 if obs[(r, c)] {
205 assert!(
206 sdf[(r, c)] <= 0.0,
207 "Obstacle cell ({}, {}) should have non-positive SDF, got {}",
208 r,
209 c,
210 sdf[(r, c)]
211 );
212 }
213 }
214 }
215 }
216
217 #[test]
218 fn test_udf_known_distance() {
219 #[rustfmt::skip]
221 let data = vec![
222 true, false, false,
223 false, false, false,
224 false, false, false,
225 ];
226 let obs = DMatrix::from_row_slice(3, 3, &data);
227 let udf = compute_udf(&obs);
228
229 assert!((udf[(0, 0)] - 0.0).abs() < 1e-10);
230 assert!((udf[(0, 1)] - 1.0).abs() < 1e-10);
231 assert!((udf[(1, 0)] - 1.0).abs() < 1e-10);
232 assert!((udf[(1, 1)] - std::f64::consts::SQRT_2).abs() < 1e-10);
234 }
235
236 #[test]
237 fn test_all_obstacles() {
238 let obs = DMatrix::from_element(4, 4, true);
239 let udf = compute_udf(&obs);
240 for r in 0..4 {
241 for c in 0..4 {
242 assert!((udf[(r, c)] - 0.0).abs() < 1e-10);
243 }
244 }
245 }
246
247 #[test]
248 fn test_dt_1d_basic() {
249 let mut d = vec![0.0, INF, INF, INF];
250 dt_1d(&mut d);
251 assert!((d[0] - 0.0).abs() < 1e-10);
252 assert!((d[1] - 1.0).abs() < 1e-10);
253 assert!((d[2] - 4.0).abs() < 1e-10);
254 assert!((d[3] - 9.0).abs() < 1e-10);
255 }
256}