1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
92
93
94
95
96
97
98
99
100
101
102
103
104
105
106
107
108
109
110
111
112
113
114
115
116
117
118
119
120
121
122
123
124
125
126
127
128
129
130
131
132
133
134
135
136
137
138
139
140
141
142
143
144
145
146
147
148
149
150
151
152
153
154
155
156
157
158
159
160
161
162
163
164
165
166
167
168
169
170
171
172
173
174
175
176
177
178
179
180
181
182
183
184
185
186
187
188
189
190
191
192
193
194
195
196
197
198
199
200
201
202
203
204
205
206
207
208
209
210
211
212
213
214
215
216
217
218
219
220
221
222
223
224
225
226
227
228
229
230
231
232
233
234
235
236
237
238
239
240
241
242
243
244
245
246
247
248
249
250
251
252
253
254
255
256
257
258
259
260
261
262
263
264
265
266
267
268
269
270
271
272
273
274
275
276
277
278
279
280
281
282
283
284
285
286
287
288
289
290
291
292
293
294
295
296
297
298
299
300
301
302
303
304
305
306
307
308
309
310
311
312
313
314
315
316
317
318
319
320
321
322
323
324
325
326
327
328
329
330
331
332
333
334
335
336
337
338
339
340
341
342
343
344
345
346
347
348
349
350
351
352
353
354
355
356
357
358
359
360
361
362
363
364
365
366
367
368
369
370
371
372
373
374
375
376
377
378
379
380
381
382
383
384
385
386
387
388
389
390
391
392
393
394
395
396
397
398
399
400
401
402
403
404
405
406
407
408
409
410
411
412
413
414
415
416
417
418
419
420
421
422
423
424
425
426
427
428
429
430
431
432
433
434
435
436
437
438
439
440
441
442
443
444
445
446
447
448
449
450
451
452
453
454
455
456
457
458
459
460
461
462
463
464
465
466
467
468
469
470
471
472
473
474
475
476
477
478
479
480
481
482
483
484
485
486
487
488
489
490
491
492
493
494
495
496
497
498
499
500
501
502
503
504
505
506
507
508
509
510
511
512
513
514
515
516
517
518
519
520
521
522
523
524
525
526
527
528
529
530
531
532
533
534
535
536
537
538
539
540
541
542
543
544
545
546
547
548
549
550
551
552
553
554
555
556
557
558
559
560
561
562
563
564
565
566
567
568
569
570
571
572
573
574
575
576
577
578
579
580
581
582
583
584
585
586
587
588
589
590
591
592
593
594
595
596
597
598
599
600
601
602
603
604
605
606
607
608
609
610
611
612
613
614
615
616
617
618
619
620
621
622
623
624
625
626
627
628
629
630
631
632
633
634
use crate::common::PCoords;
use crate::common::consensus::{ConsensusModel, Magsac};
use crate::common::svd_basis::SvdBasis;
use crate::geom3::UnitVec3;
use crate::geom3::line3::Line3;
use crate::{Iso3, Point3, Result, SurfacePoint3, Vector3};
use std::ops;
/// A plane in 3D space, defined by a unit normal and a signed offset from the origin.
///
/// This is one of `engeom`'s 3D geometric primitives.
#[derive(Debug, Clone)]
pub struct Plane3 {
pub normal: UnitVec3,
pub d: f64,
}
impl Plane3 {
/// Creates a plane with normal along the x-axis and offset 0.0
pub fn yz() -> Self {
Self::new(Vector3::x_axis(), 0.0)
}
/// Creates a plane with normal along the y-axis and offset 0.0
pub fn xz() -> Self {
Self::new(Vector3::y_axis(), 0.0)
}
/// Creates a plane with normal along the z-axis and offset 0.0
pub fn xy() -> Self {
Self::new(Vector3::z_axis(), 0.0)
}
/// Create a new plane from a unit vector and an offset from the origin. The unit vector
/// components ux, uy, and uz are the equivalent to a, b, and c in the traditional a, b, c, d
/// representation of a plane.
///
/// # Arguments
///
/// * `normal`: The plane normal
/// * `d`: The distance between the plane and the origin in the direction of the plane normal
///
/// returns: Plane3
///
/// # Examples
///
/// ```
///
/// ```
pub fn new(normal: UnitVec3, d: f64) -> Self {
Self { normal, d }
}
/// Fit a plane to a set of points using singular value decomposition, resulting in a
/// least-squares fitting. Optional weights may be provided in a slice of `f64` with the same
/// number of elements as `points`, where the weight `i` corresponds with the point `i`.
///
/// # Arguments
///
/// * `points`: a slice of coordinates to fit the plane to
/// * `weights`: if `Some`, this must be a slice of floating points the same length as `points`,
/// with the weight value to multiply each point residual by.
///
/// returns: Result<Plane3, Box<dyn Error, Global>>
///
/// # Examples
///
/// ```
///
/// ```
pub fn from_fit(points: &[impl PCoords<3>], weights: Option<&[f64]>) -> Result<Self> {
let basis = SvdBasis::from_points(points, weights)
.ok_or("Failed to fit plane with singular value decomposition")?;
Ok(Plane3::from_point_normal(&basis.center, &basis.smallest()))
}
/// Fit a plane to a set of points using MAGSAC++ robust consensus estimation.
///
/// Unlike an ordinary least-squares fit ([`Plane3::from_fit`]), this rejects gross outliers by
/// taking an upper bound on the inlier noise (`sigma_max`) rather than a hard inlier/outlier
/// threshold, and refines each candidate with noise-marginalized iteratively reweighted least
/// squares. It is substantially less sensitive to `sigma_max` than RANSAC is to its threshold,
/// as long as `sigma_max` is not chosen smaller than the actual noise.
///
/// The resulting plane passes through the centroid of the inlier set with a unit normal. The
/// direction of the normal is not meaningful (it may point to either side of the plane).
///
/// # Arguments
///
/// * `points`: the points to fit the plane to
/// * `sigma_max`: the upper bound on the expected inlier noise, in the same units as the points
/// * `options`: an optional [`Magsac`] configuration to override the iteration count, refinement
/// steps, confidence, or RNG seed. Its `sigma_max` field is overridden by the `sigma_max`
/// argument.
///
/// returns: Result<Plane3, Box<dyn Error, Global>>
pub fn from_consensus(
points: &[Point3],
sigma_max: f64,
options: Option<Magsac>,
) -> Result<Self> {
let mut magsac = options.unwrap_or_else(|| Magsac::new(sigma_max));
magsac.sigma_max = sigma_max;
let fit = magsac.fit::<3, Plane3>(points)?;
Ok(fit.model)
}
/// Create a Plane3 from three points, with the normal following the right-hand rule from
/// `p1` to `p2` to `p3`. Returns an error if the points are collinear (or coincident).
///
/// # Arguments
///
/// * `p1`: the first point
/// * `p2`: the second point
/// * `p3`: the third point
///
/// returns: Result<Plane3>
pub fn from_3_points(p1: &Point3, p2: &Point3, p3: &Point3) -> Result<Self> {
let cross = (p2 - p1).cross(&(p3 - p1));
let normal = UnitVec3::try_new(cross, 1e-10).ok_or("Points are collinear")?;
Ok(Self::from_point_normal(p1, &normal))
}
/// Create a Plane3 from a point on the plane and a unit normal direction.
///
/// # Arguments
///
/// * `point`: a point lying on the plane
/// * `normal`: the unit normal of the plane
///
/// returns: Plane3
pub fn from_point_normal(point: &Point3, normal: &UnitVec3) -> Self {
let d = normal.dot(&point.coords);
Self::new(*normal, d)
}
/// Create a Plane3 from a `SurfacePoint3`, using its point and normal directly.
///
/// # Arguments
///
/// * `surface_point`: the surface point to create the plane from
///
/// returns: Plane3
pub fn from_surface_point(surface_point: &SurfacePoint3) -> Self {
Self::from_point_normal(&surface_point.point, &surface_point.normal)
}
/// Returns a new plane in the same position as this one, but with the normal direction
/// reversed, without modifying the original.
pub fn normal_reversed(&self) -> Self {
Self::new(-self.normal, -self.d)
}
/// Measure and return the signed distance from the plane to a point in 3D space. The sign of
/// the distance indicates whether the point is above or below the plane according to the
/// plane's normal vector.
///
/// # Arguments
///
/// * `point`:
///
/// returns: f64
///
/// # Examples
///
/// ```
///
/// ```
pub fn signed_distance_to_point(&self, point: &impl PCoords<3>) -> f64 {
self.normal.dot(&point.coords()) - self.d
}
/// Returns true if the point lies in the positive half-space defined by the plane's normal and
/// offset from the origin. This will return true if the signed distance to the point is >= 0.0
///
/// # Arguments
///
/// * `point`: the point to check against the plane
///
/// returns: bool
pub fn point_is_positive(&self, point: &impl PCoords<3>) -> bool {
self.signed_distance_to_point(point) >= 0.0
}
/// Measure and return the distance from the plane to a point in 3D space. The distance is
/// always positive and indicates the shortest distance from the point to the plane. If you
/// need to know whether the point is above or below the plane, use `signed_distance_to_point`.
///
/// # Arguments
///
/// * `point`:
///
/// returns: f64
///
/// # Examples
///
/// ```
///
/// ```
pub fn distance_to_point(&self, point: &Point3) -> f64 {
self.signed_distance_to_point(point).abs()
}
/// Project a point onto the plane, returning a point in 3D space which lies on the plane. This
/// is also the closest point on the plane to the input point.
///
/// # Arguments
///
/// * `point`:
///
/// returns: OPoint<f64, Const<3>>
///
/// # Examples
///
/// ```
///
/// ```
pub fn project_point(&self, point: &Point3) -> Point3 {
point - self.normal.into_inner() * self.signed_distance_to_point(point)
}
/// Projects a vector onto the plane by removing the component along the plane's normal.
pub fn project_vector(&self, v: &Vector3) -> Vector3 {
v - self.normal.into_inner() * self.normal.dot(v)
}
/// Intersects this plane with another plane, returning the line of intersection, or `None` if
/// the planes are parallel (or coincident).
///
/// The line's direction is `self.normal × other.normal` (not normalized). The origin is the
/// point on the intersection line closest to the world origin.
pub fn intersect_plane(&self, other: &Plane3) -> Option<Line3> {
let direction = self.normal.cross(&other.normal);
let denom = direction.norm_squared();
if denom < 1e-20 {
return None;
}
// Point closest to origin on the intersection line via Lagrange multipliers:
// p = λ1*n1 + λ2*n2, solving n1·p = d1, n2·p = d2
let k = self.normal.dot(&other.normal);
let denom_lm = 1.0 - k * k;
let l1 = (self.d - k * other.d) / denom_lm;
let l2 = (other.d - k * self.d) / denom_lm;
let origin = Point3::from(self.normal.into_inner() * l1 + other.normal.into_inner() * l2);
Some(Line3::new(origin, direction))
}
pub fn intersect_distance(&self, sp: &SurfacePoint3) -> Option<f64> {
let p0 = Point3::from(self.normal.into_inner() * self.d);
let denom = self.normal.dot(&sp.normal);
if denom <= 1e-6 {
None
} else {
Some((p0 - sp.point).dot(&self.normal) / denom)
}
}
/// Create a new plane parallel to this one, displaced along its normal direction by the given
/// distance. A positive value moves in the normal direction; negative moves opposite.
///
/// # Arguments
///
/// * `shift`: The distance to shift the plane along its normal vector.
///
/// returns: Plane3
///
/// # Examples
///
/// ```
/// use engeom::geom3::{Plane3, Point3, Vector3};
/// use approx::assert_relative_eq;
/// let plane = Plane3::new(Vector3::x_axis(), -5.0);
/// let moved = plane.offset_by(2.0);
///
/// assert_relative_eq!(moved.signed_distance_to_point(&Point3::origin()), 3.0, epsilon = 1e-6);
/// ```
pub fn offset_by(&self, shift: f64) -> Self {
Self::new(self.normal, self.d + shift)
}
/// Returns a new plane transformed by the given isometry, without modifying the original.
pub fn transformed_by(&self, iso: &Iso3) -> Self {
let pos = self.normal.into_inner() * self.d;
let repr = SurfacePoint3::new(pos.into(), self.normal);
let new_repr = repr.transformed_by(iso);
Self::from_surface_point(&new_repr)
}
}
impl ConsensusModel<3> for Plane3 {
type Point = Point3;
const SAMPLE_SIZE: usize = 3;
fn from_sample(sample: &[Point3]) -> Option<Self> {
// `from_3_points` already rejects collinear (and coincident) samples.
Plane3::from_3_points(&sample[0], &sample[1], &sample[2]).ok()
}
fn residual(&self, point: &Point3) -> f64 {
// Signed distance keeps the residual smooth through the plane for the least-squares
// refinement; only its magnitude is used for scoring.
self.signed_distance_to_point(point)
}
fn refine_weighted(points: &[Point3], weights: &[f64], _initial: &Self) -> Option<Self> {
// The MAGSAC++ refinement step is a single weighted least-squares fit, which for a plane is
// exactly the weighted SVD fit provided by `from_fit`.
Plane3::from_fit(points, Some(weights)).ok()
}
}
impl ops::Mul<Plane3> for Iso3 {
type Output = Plane3;
fn mul(self, rhs: Plane3) -> Plane3 {
rhs.transformed_by(&self)
}
}
impl ops::Mul<&Plane3> for Iso3 {
type Output = Plane3;
fn mul(self, rhs: &Plane3) -> Plane3 {
rhs.transformed_by(&self)
}
}
impl ops::Mul<Plane3> for &Iso3 {
type Output = Plane3;
fn mul(self, rhs: Plane3) -> Plane3 {
rhs.transformed_by(self)
}
}
impl ops::Mul<&Plane3> for &Iso3 {
type Output = Plane3;
fn mul(self, rhs: &Plane3) -> Plane3 {
rhs.transformed_by(self)
}
}
#[cfg(test)]
mod tests {
use super::*;
use crate::common::consensus::Magsac;
use crate::common::random_geometry::RandomGeometry3;
use approx::assert_relative_eq;
/// Build two orthonormal in-plane basis vectors spanning `plane`.
fn in_plane_basis(plane: &Plane3) -> (Vector3, Vector3) {
let n = plane.normal.into_inner();
let reference = if n.z.abs() < 0.9 {
Vector3::z()
} else {
Vector3::x()
};
let u = reference.cross(&n).normalize();
let v = n.cross(&u);
(u, v)
}
/// Generate `n` points scattered across `plane` with isotropic Gaussian noise `sigma`.
fn plane_noise(
rg: &mut RandomGeometry3,
plane: &Plane3,
n: usize,
span: f64,
sigma: f64,
) -> Vec<Point3> {
let origin = Point3::from(plane.normal.into_inner() * plane.d);
let (u, v) = in_plane_basis(plane);
(0..n)
.map(|_| {
origin
+ u * rg.f64_sym(span)
+ v * rg.f64_sym(span)
+ rg.gaussian_vector::<3>(sigma)
})
.collect()
}
#[test]
fn normal_reversed_negates_normal_and_distance() {
let plane = Plane3::new(Vector3::z_axis(), 2.0);
let reversed = plane.normal_reversed();
assert_relative_eq!(reversed.normal.into_inner(), -plane.normal.into_inner());
assert_relative_eq!(reversed.d, -plane.d);
// The plane occupies the same position in space: signed distance is negated everywhere.
let mut rg = RandomGeometry3::new();
for _ in 0..100 {
let p = rg.point(10.0);
assert_relative_eq!(
reversed.signed_distance_to_point(&p),
-plane.signed_distance_to_point(&p),
epsilon = 1e-12
);
}
}
#[test]
fn intersect_xy_xz_gives_x_axis() {
// xy-plane (z=0) ∩ xz-plane (y=0) should be the X axis
let line = Plane3::xy().intersect_plane(&Plane3::xz()).unwrap();
// direction must be parallel to X
let dir = line.direction.normalize();
assert_relative_eq!(dir.x.abs(), 1.0, epsilon = 1e-12);
assert_relative_eq!(dir.y, 0.0, epsilon = 1e-12);
assert_relative_eq!(dir.z, 0.0, epsilon = 1e-12);
// origin must lie on both planes
assert_relative_eq!(line.origin.y, 0.0, epsilon = 1e-12);
assert_relative_eq!(line.origin.z, 0.0, epsilon = 1e-12);
}
#[test]
fn intersect_parallel_planes_returns_none() {
let p1 = Plane3::new(Vector3::z_axis(), 1.0);
let p2 = Plane3::new(Vector3::z_axis(), 3.0);
assert!(p1.intersect_plane(&p2).is_none());
}
#[test]
fn intersect_same_plane_returns_none() {
let p = Plane3::xy();
assert!(p.intersect_plane(&p).is_none());
}
#[test]
fn stress_intersection_line_lies_on_both_planes() {
let mut rg = RandomGeometry3::new();
for _ in 0..500 {
let iso1 = rg.iso3(10.0);
let iso2 = rg.iso3(10.0);
let p1 = Plane3::xy().transformed_by(&iso1);
let p2 = Plane3::xy().transformed_by(&iso2);
if let Some(line) = p1.intersect_plane(&p2) {
for t in [-5.0, -1.0, 0.0, 1.0, 5.0] {
let pt = line.at(t);
assert_relative_eq!(
p1.signed_distance_to_point(&pt).abs(),
0.0,
epsilon = 1e-8
);
assert_relative_eq!(
p2.signed_distance_to_point(&pt).abs(),
0.0,
epsilon = 1e-8
);
}
}
}
}
// -------------------------------------------------------------------------
// from_fit tests
// -------------------------------------------------------------------------
#[test]
fn from_fit_recovers_flat_plane() {
let points = [
Point3::new(0.0, 0.0, 5.0),
Point3::new(1.0, 0.0, 5.0),
Point3::new(0.0, 1.0, 5.0),
Point3::new(1.0, 1.0, 5.0),
];
let plane = Plane3::from_fit(&points, None).unwrap();
assert_relative_eq!(plane.normal.into_inner().z.abs(), 1.0, epsilon = 1e-10);
for p in &points {
assert_relative_eq!(plane.distance_to_point(p), 0.0, epsilon = 1e-10);
}
}
#[test]
fn from_fit_uniform_weights_match_unweighted() {
let points = [
Point3::new(0.0, 0.0, 5.0),
Point3::new(1.0, 0.3, 5.2),
Point3::new(0.2, 1.0, 4.8),
Point3::new(1.0, 1.0, 5.1),
];
let unweighted = Plane3::from_fit(&points, None).unwrap();
let weights = [1.0; 4];
let weighted = Plane3::from_fit(&points, Some(&weights)).unwrap();
assert_relative_eq!(
weighted.normal.into_inner(),
unweighted.normal.into_inner(),
epsilon = 1e-10
);
assert_relative_eq!(weighted.d, unweighted.d, epsilon = 1e-10);
}
#[test]
fn from_fit_heavily_weighted_point_pulls_plane_toward_it() {
// A mostly-flat cluster at z=0 plus one outlier point; in the limit of a very large
// weight on the outlier, the least-squares fit is forced to pass through it almost
// exactly.
let points = [
Point3::new(0.0, 0.0, 0.0),
Point3::new(1.0, 0.0, 0.0),
Point3::new(0.0, 1.0, 0.0),
Point3::new(1.0, 1.0, 0.0),
Point3::new(0.3, 0.7, 0.0),
Point3::new(0.8, 0.2, 0.0),
Point3::new(3.0, 3.0, 2.0),
];
let outlier = points[6];
let unweighted = Plane3::from_fit(&points, None).unwrap();
let mut weights = [1.0; 7];
weights[6] = 1000.0;
let weighted = Plane3::from_fit(&points, Some(&weights)).unwrap();
assert!(weighted.distance_to_point(&outlier) < unweighted.distance_to_point(&outlier));
assert_relative_eq!(weighted.distance_to_point(&outlier), 0.0, epsilon = 1e-2);
}
#[test]
fn from_fit_empty_points_is_error() {
let points: [Point3; 0] = [];
assert!(Plane3::from_fit(&points, None).is_err());
}
#[test]
fn from_fit_insufficient_points_is_error() {
// Two points can't uniquely determine a plane orientation; the SVD degenerates.
let points = [Point3::new(0.0, 0.0, 0.0), Point3::new(1.0, 0.0, 0.0)];
assert!(Plane3::from_fit(&points, None).is_err());
}
#[test]
fn from_fit_coincident_points_is_error() {
let points = [Point3::new(1.0, 1.0, 1.0), Point3::new(1.0, 1.0, 1.0)];
assert!(Plane3::from_fit(&points, None).is_err());
}
#[test]
fn stress_from_fit_recovers_known_plane() {
let mut rg = RandomGeometry3::new();
for _ in 0..200 {
let normal = rg.unit_vec();
let point_on_plane = rg.point(10.0);
let true_plane = Plane3::from_point_normal(&point_on_plane, &normal);
// Build two orthonormal in-plane basis vectors to generate points that lie exactly
// on the plane.
let n = normal.into_inner();
let reference = if n.z.abs() < 0.9 {
Vector3::z()
} else {
Vector3::x()
};
let u = reference.cross(&n).normalize();
let v = n.cross(&u);
let points: Vec<Point3> = (0..12)
.map(|_| point_on_plane + u * rg.f64(-5.0, 5.0) + v * rg.f64(-5.0, 5.0))
.collect();
let fit = Plane3::from_fit(&points, None).unwrap();
for p in &points {
assert_relative_eq!(fit.distance_to_point(p), 0.0, epsilon = 1e-8);
}
assert_relative_eq!(
fit.normal.dot(&true_plane.normal).abs(),
1.0,
epsilon = 1e-8
);
}
}
// -------------------------------------------------------------------------
// from_consensus tests
// -------------------------------------------------------------------------
#[test]
fn from_consensus_convenience_recovers_plane() {
// A clean set of coplanar points; the convenience method should recover the plane.
let mut rg = RandomGeometry3::from_seed(7);
let true_plane = Plane3::from_point_normal(&Point3::new(1.0, -2.0, 3.0), &rg.unit_vec());
let points = plane_noise(&mut rg, &true_plane, 80, 5.0, 0.0);
let plane = Plane3::from_consensus(&points, 0.01, None).unwrap();
for p in &points {
assert!(plane.distance_to_point(p) < 1e-6);
}
assert_relative_eq!(
plane.normal.dot(&true_plane.normal).abs(),
1.0,
epsilon = 1e-6
);
}
#[test]
fn from_consensus_rejects_outliers() {
let mut rg = RandomGeometry3::from_seed(101);
// Inliers lie on the z = 2 plane with a small amount of noise.
let true_plane = Plane3::new(Vector3::z_axis(), 2.0);
let inliers = plane_noise(&mut rg, &true_plane, 200, 10.0, 0.01);
let mut points = inliers.clone();
// A dense cluster of gross outliers well off the plane.
let center = Point3::new(-4.0, 5.0, 9.0);
for _ in 0..60 {
points.push(center + rg.gaussian_vector::<3>(1.0));
}
let magsac = Magsac {
sigma_max: 0.02,
max_iterations: Some(400),
refinement_steps: 4,
confidence: 0.99,
seed: Some(42),
};
let fit = magsac.fit::<3, Plane3>(&points).unwrap();
// Every inlier should lie very close to the recovered plane.
for i in &inliers {
assert!(fit.model.distance_to_point(i) < 0.01 * 6.0);
}
// The recovered normal should be parallel (or anti-parallel) to the true plane's normal.
assert_relative_eq!(
fit.model.normal.dot(&true_plane.normal).abs(),
1.0,
epsilon = 1e-2
);
// No outlier should be classified as an inlier.
assert!(fit.inliers.iter().all(|&i| i < inliers.len()));
}
}