1use nalgebra::{Matrix2, Vector2};
12use std::f64::consts::FRAC_PI_2;
13
14#[derive(Debug, Clone, PartialEq)]
22pub struct RectangleData {
23 pub a: [f64; 4],
25 pub b: [f64; 4],
27 pub c: [f64; 4],
29}
30
31impl RectangleData {
32 fn new() -> Self {
33 Self {
34 a: [0.0; 4],
35 b: [0.0; 4],
36 c: [0.0; 4],
37 }
38 }
39
40 pub fn corners(&self) -> [(f64, f64); 4] {
44 let cross = |i: usize, j: usize| -> (f64, f64) {
45 let det = self.a[i] * self.b[j] - self.a[j] * self.b[i];
46 let x = (self.b[i] * -self.c[j] - self.b[j] * -self.c[i]) / det;
47 let y = (self.a[j] * -self.c[i] - self.a[i] * -self.c[j]) / det;
48 (x, y)
49 };
50 [cross(0, 1), cross(1, 2), cross(2, 3), cross(3, 0)]
51 }
52}
53
54#[derive(Debug, Clone, Copy, PartialEq, Eq)]
60pub enum Criteria {
61 Area,
63 Closeness,
65 Variance,
67}
68
69#[derive(Debug, Clone)]
75pub struct LShapeFitting {
76 pub criteria: Criteria,
78 pub min_dist_of_closeness_criteria: f64,
80 pub d_theta: f64,
82 pub r0: f64,
84 pub rd: f64,
86}
87
88impl Default for LShapeFitting {
89 fn default() -> Self {
90 Self {
91 criteria: Criteria::Variance,
92 min_dist_of_closeness_criteria: 0.01,
93 d_theta: 1.0_f64.to_radians(),
94 r0: 3.0,
95 rd: 0.001,
96 }
97 }
98}
99
100impl LShapeFitting {
101 pub fn new() -> Self {
103 Self::default()
104 }
105
106 pub fn fitting(&self, ox: &[f64], oy: &[f64]) -> Vec<(RectangleData, Vec<usize>)> {
113 let segments = self.adaptive_range_segmentation(ox, oy);
114 segments
115 .into_iter()
116 .map(|ids| {
117 let cx: Vec<f64> = ids.iter().map(|&i| ox[i]).collect();
118 let cy: Vec<f64> = ids.iter().map(|&i| oy[i]).collect();
119 let rect = self.rectangle_search(&cx, &cy);
120 (rect, ids)
121 })
122 .collect()
123 }
124
125 pub fn rectangle_search(&self, x: &[f64], y: &[f64]) -> RectangleData {
127 assert_eq!(x.len(), y.len());
128 assert!(!x.is_empty(), "need at least one point");
129
130 let n = x.len();
131
132 let mut best_cost = f64::NEG_INFINITY;
133 let mut best_theta: f64 = 0.0;
134
135 let mut theta = 0.0_f64;
136 while theta < FRAC_PI_2 - self.d_theta {
137 let rot = rot_mat_2d(theta);
138
139 let mut c1 = Vec::with_capacity(n);
140 let mut c2 = Vec::with_capacity(n);
141 for i in 0..n {
142 let p = rot * Vector2::new(x[i], y[i]);
143 c1.push(p[0]);
144 c2.push(p[1]);
145 }
146
147 let cost = match self.criteria {
148 Criteria::Area => calc_area_criterion(&c1, &c2),
149 Criteria::Closeness => self.calc_closeness_criterion(&c1, &c2),
150 Criteria::Variance => calc_variance_criterion(&c1, &c2),
151 };
152
153 if cost > best_cost {
154 best_cost = cost;
155 best_theta = theta;
156 }
157
158 theta += self.d_theta;
159 }
160
161 let sin_s = best_theta.sin();
162 let cos_s = best_theta.cos();
163
164 let c1_s: Vec<f64> = x
165 .iter()
166 .zip(y.iter())
167 .map(|(xi, yi)| cos_s * xi + sin_s * yi)
168 .collect();
169 let c2_s: Vec<f64> = x
170 .iter()
171 .zip(y.iter())
172 .map(|(xi, yi)| -sin_s * xi + cos_s * yi)
173 .collect();
174
175 let c1_min = c1_s.iter().cloned().fold(f64::INFINITY, f64::min);
176 let c1_max = c1_s.iter().cloned().fold(f64::NEG_INFINITY, f64::max);
177 let c2_min = c2_s.iter().cloned().fold(f64::INFINITY, f64::min);
178 let c2_max = c2_s.iter().cloned().fold(f64::NEG_INFINITY, f64::max);
179
180 let mut rect = RectangleData::new();
181 rect.a = [cos_s, -sin_s, cos_s, -sin_s];
182 rect.b = [sin_s, cos_s, sin_s, cos_s];
183 rect.c = [c1_min, c2_min, c1_max, c2_max];
184
185 rect
186 }
187
188 fn adaptive_range_segmentation(&self, ox: &[f64], oy: &[f64]) -> Vec<Vec<usize>> {
190 let n = ox.len();
191 let mut segments: Vec<Vec<usize>> = Vec::new();
192
193 let mut sets: Vec<Vec<usize>> = Vec::with_capacity(n);
195 for i in 0..n {
196 let r = self.r0 + self.rd * (ox[i].powi(2) + oy[i].powi(2)).sqrt();
197 let mut cluster = Vec::new();
198 for j in 0..n {
199 let d = ((ox[i] - ox[j]).powi(2) + (oy[i] - oy[j]).powi(2)).sqrt();
200 if d <= r {
201 cluster.push(j);
202 }
203 }
204 sets.push(cluster);
205 }
206
207 let mut merged: Vec<Vec<bool>> = sets
209 .iter()
210 .map(|s| {
211 let mut bv = vec![false; n];
212 for &idx in s {
213 bv[idx] = true;
214 }
215 bv
216 })
217 .collect();
218
219 loop {
220 let mut changed = false;
221 let len = merged.len();
222 let mut i = 0;
223 while i < len {
224 let mut j = i + 1;
225 while j < merged.len() {
226 let overlap = merged[i]
227 .iter()
228 .zip(merged[j].iter())
229 .any(|(a, b)| *a && *b);
230 if overlap {
231 let other = merged.remove(j);
232 for (k, value) in other.iter().enumerate().take(n) {
233 merged[i][k] = merged[i][k] || *value;
234 }
235 changed = true;
236 } else {
237 j += 1;
238 }
239 }
240 i += 1;
241 }
242 if !changed {
243 break;
244 }
245 }
246
247 for bv in merged {
248 let ids: Vec<usize> = bv
249 .iter()
250 .enumerate()
251 .filter_map(|(i, &v)| if v { Some(i) } else { None })
252 .collect();
253 if !ids.is_empty() {
254 segments.push(ids);
255 }
256 }
257
258 segments
259 }
260
261 fn calc_closeness_criterion(&self, c1: &[f64], c2: &[f64]) -> f64 {
262 let (c1_max, c1_min, c2_max, c2_min) = find_min_max(c1, c2);
263 c1.iter()
264 .zip(c2.iter())
265 .map(|(v1, v2)| {
266 let d1 = (c1_max - v1).min(v1 - c1_min);
267 let d2 = (c2_max - v2).min(v2 - c2_min);
268 1.0 / d1.min(d2).max(self.min_dist_of_closeness_criteria)
269 })
270 .sum()
271 }
272}
273
274fn rot_mat_2d(angle: f64) -> Matrix2<f64> {
279 let (s, c) = angle.sin_cos();
280 Matrix2::new(c, s, -s, c)
281}
282
283fn find_min_max(c1: &[f64], c2: &[f64]) -> (f64, f64, f64, f64) {
284 let c1_max = c1.iter().cloned().fold(f64::NEG_INFINITY, f64::max);
285 let c1_min = c1.iter().cloned().fold(f64::INFINITY, f64::min);
286 let c2_max = c2.iter().cloned().fold(f64::NEG_INFINITY, f64::max);
287 let c2_min = c2.iter().cloned().fold(f64::INFINITY, f64::min);
288 (c1_max, c1_min, c2_max, c2_min)
289}
290
291fn calc_area_criterion(c1: &[f64], c2: &[f64]) -> f64 {
292 let (c1_max, c1_min, c2_max, c2_min) = find_min_max(c1, c2);
293 -((c1_max - c1_min) * (c2_max - c2_min))
294}
295
296fn calc_variance_criterion(c1: &[f64], c2: &[f64]) -> f64 {
297 let (c1_max, c1_min, c2_max, c2_min) = find_min_max(c1, c2);
298
299 let mut e1: Vec<f64> = Vec::new();
300 let mut e2: Vec<f64> = Vec::new();
301
302 for (v1, v2) in c1.iter().zip(c2.iter()) {
303 let d1 = (c1_max - v1).min(v1 - c1_min);
304 let d2 = (c2_max - v2).min(v2 - c2_min);
305 if d1 < d2 {
306 e1.push(d1);
307 } else {
308 e2.push(d2);
309 }
310 }
311
312 let var = |vals: &[f64]| -> f64 {
313 if vals.is_empty() {
314 return 0.0;
315 }
316 let mean = vals.iter().sum::<f64>() / vals.len() as f64;
317 vals.iter().map(|v| (v - mean).powi(2)).sum::<f64>() / vals.len() as f64
318 };
319
320 let v1 = if e1.is_empty() { 0.0 } else { -var(&e1) };
321 let v2 = if e2.is_empty() { 0.0 } else { -var(&e2) };
322 v1 + v2
323}
324
325#[cfg(test)]
326mod tests {
327 use super::*;
328 use std::f64::consts::PI;
329
330 fn rect_points(cx: f64, cy: f64, w: f64, h: f64, n_per_side: usize) -> (Vec<f64>, Vec<f64>) {
332 let mut xs = Vec::new();
333 let mut ys = Vec::new();
334 let half_w = w / 2.0;
335 let half_h = h / 2.0;
336 for i in 0..n_per_side {
337 let t = i as f64 / n_per_side as f64;
338 xs.push(cx - half_w + w * t);
340 ys.push(cy - half_h);
341 xs.push(cx - half_w + w * t);
343 ys.push(cy + half_h);
344 xs.push(cx - half_w);
346 ys.push(cy - half_h + h * t);
347 xs.push(cx + half_w);
349 ys.push(cy - half_h + h * t);
350 }
351 (xs, ys)
352 }
353
354 #[test]
355 fn test_rectangle_search_axis_aligned() {
356 let (x, y) = rect_points(0.0, 0.0, 4.0, 2.0, 20);
357 let fitter = LShapeFitting::new();
358 let rect = fitter.rectangle_search(&x, &y);
359
360 let corners = rect.corners();
361 let mut xs: Vec<f64> = corners.iter().map(|c| c.0).collect();
362 let mut ys: Vec<f64> = corners.iter().map(|c| c.1).collect();
363 xs.sort_by(|a, b| a.partial_cmp(b).unwrap());
364 ys.sort_by(|a, b| a.partial_cmp(b).unwrap());
365
366 let width = xs[3] - xs[0];
367 let height = ys[3] - ys[0];
368 let (w, h) = if width > height {
369 (width, height)
370 } else {
371 (height, width)
372 };
373
374 assert!((w - 4.0).abs() < 0.2, "width mismatch: {}", w);
375 assert!((h - 2.0).abs() < 0.2, "height mismatch: {}", h);
376 }
377
378 #[test]
379 fn test_rectangle_search_rotated() {
380 let angle = PI / 6.0; let (s, c) = angle.sin_cos();
382 let (base_x, base_y) = rect_points(0.0, 0.0, 6.0, 3.0, 20);
383 let x: Vec<f64> = base_x
384 .iter()
385 .zip(base_y.iter())
386 .map(|(bx, by)| c * bx - s * by)
387 .collect();
388 let y: Vec<f64> = base_x
389 .iter()
390 .zip(base_y.iter())
391 .map(|(bx, by)| s * bx + c * by)
392 .collect();
393
394 let fitter = LShapeFitting::new();
395 let rect = fitter.rectangle_search(&x, &y);
396
397 let corners = rect.corners();
398 let dist =
400 |a: (f64, f64), b: (f64, f64)| ((a.0 - b.0).powi(2) + (a.1 - b.1).powi(2)).sqrt();
401 let mut sides = [
402 dist(corners[0], corners[1]),
403 dist(corners[1], corners[2]),
404 dist(corners[2], corners[3]),
405 dist(corners[3], corners[0]),
406 ];
407 sides.sort_by(|a, b| a.partial_cmp(b).unwrap());
408
409 assert!((sides[0] - 3.0).abs() < 0.3, "short side: {}", sides[0]);
410 assert!((sides[1] - 3.0).abs() < 0.3, "short side: {}", sides[1]);
411 assert!((sides[2] - 6.0).abs() < 0.3, "long side: {}", sides[2]);
412 assert!((sides[3] - 6.0).abs() < 0.3, "long side: {}", sides[3]);
413 }
414
415 #[test]
416 fn test_fitting_with_segmentation() {
417 let (mut x, mut y) = rect_points(0.0, 0.0, 2.0, 1.0, 10);
419 let (x2, y2) = rect_points(20.0, 20.0, 3.0, 1.5, 10);
420 x.extend(x2);
421 y.extend(y2);
422
423 let fitter = LShapeFitting {
424 r0: 3.0,
425 rd: 0.001,
426 ..LShapeFitting::default()
427 };
428 let results = fitter.fitting(&x, &y);
429 assert_eq!(results.len(), 2, "should detect two clusters");
430 }
431
432 #[test]
433 fn test_criteria_area() {
434 let (x, y) = rect_points(0.0, 0.0, 4.0, 2.0, 15);
435 let fitter = LShapeFitting {
436 criteria: Criteria::Area,
437 ..LShapeFitting::default()
438 };
439 let rect = fitter.rectangle_search(&x, &y);
440 let corners = rect.corners();
441 for (cx, cy) in &corners {
443 assert!(cx.is_finite() && cy.is_finite());
444 }
445 }
446
447 #[test]
448 fn test_criteria_closeness() {
449 let (x, y) = rect_points(0.0, 0.0, 4.0, 2.0, 15);
450 let fitter = LShapeFitting {
451 criteria: Criteria::Closeness,
452 ..LShapeFitting::default()
453 };
454 let rect = fitter.rectangle_search(&x, &y);
455 let corners = rect.corners();
456 for (cx, cy) in &corners {
457 assert!(cx.is_finite() && cy.is_finite());
458 }
459 }
460}