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
|
use nalgebra::SMatrix;
use super::{distance_field::DistanceField, Point, Scalar, Vector, N};
type ProjectionMatrix = SMatrix<Scalar, N, N>;
pub struct Subspace<const DIM: usize> {
point: Point,
vectors: [Vector; DIM],
projection_matrix: ProjectionMatrix,
}
pub type Line = Subspace<1>;
pub type Plane = Subspace<2>;
impl<const DIM: usize> Subspace<DIM> {
pub fn new(point: Point, mut vectors: [Vector; DIM]) -> Self {
for vector in &mut vectors {
vector.normalize_mut();
}
Self {
point,
vectors,
projection_matrix: {
let v = SMatrix::<Scalar, N, DIM>::from_columns(&vectors);
let transpose = v.transpose();
let inverse = (transpose * v).try_inverse().unwrap();
v * inverse * transpose
}
}
}
pub fn project_point(&self, point: Point) -> Point {
self.projection_matrix * (point - self.point.coords) + self.point.coords
}
}
impl<const DIM: usize> DistanceField for Subspace<DIM> {
fn distance(&self, point: Point) -> Scalar {
let projected = self.project_point(point);
(projected - point).norm()
}
}
#[cfg(test)]
mod tests {
use crate::algebra::{subspace::Line, Point, Vector, distance_field::DistanceField};
#[test]
fn test_projection_onto_line() {
let line = Line::new(Point::new(1.0, 0.0, 0.0), [Vector::new(3.0, 1.0, 0.0)]);
let point = Point::new(3.0, 4.0, 0.0);
let matrix = line.projection_matrix;
{
let diff = matrix * matrix - matrix;
assert!(diff.norm() < 0.001, "Projection matrix squared should be equal to itself: {}, {}", matrix, matrix* matrix);
}
{
let diff = matrix - matrix.transpose();
assert!(diff.norm() < 0.001, "Projection matrix transpose should be equal to itself: {}, {}", matrix, matrix.transpose());
}
{
let projected = line.project_point(point);
let diff = projected - Point::new(4.0, 1.0, 0.0);
assert!(diff.norm() < 0.001, "Point projected incorrectly");
}
}
#[test]
fn test_distance() {
let origin = Point::new(0.0, 0.0, 0.0);
let line = Line::new(origin, [Vector::x()]);
let dq = 0.001;
let dx = line.distance(origin + Vector::x() * dq);
let dy = line.distance(origin + Vector::y() * dq);
let dz = line.distance(origin + Vector::z() * dq);
assert_eq!(dx, 0.0);
assert_eq!(dy, dz);
}
}
|
