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
use super::cell::CellVertex;
/// Solver for a quadratic error function to position a vertex within a cell
#[derive(Copy, Clone, Debug, Default)]
pub struct QuadraticErrorSolver {
/// A^T A term
ata: nalgebra::Matrix3<f32>,
/// A^T B term
atb: nalgebra::Vector3<f32>,
/// B^T B term
btb: f32,
/// Mass point of intersections is stored as XYZ / W, so that summing works
mass_point: nalgebra::Vector4<f32>,
}
impl std::ops::AddAssign for QuadraticErrorSolver {
fn add_assign(&mut self, rhs: Self) {
self.ata += rhs.ata;
self.atb += rhs.atb;
self.btb += rhs.btb;
self.mass_point += rhs.mass_point;
}
}
impl QuadraticErrorSolver {
pub fn new() -> Self {
Self {
ata: nalgebra::Matrix3::zeros(),
atb: nalgebra::Vector3::zeros(),
btb: 0.0,
mass_point: nalgebra::Vector4::zeros(),
}
}
#[cfg(test)]
pub fn mass_point(&self) -> nalgebra::Vector4<f32> {
self.mass_point
}
/// Adds a new intersection to the QEF
///
/// `pos` is the position of the intersection and is accumulated in the mass
/// point. `grad` is the gradient at the surface, and is normalized in this
/// function.
pub fn add_intersection(
&mut self,
pos: nalgebra::Vector3<f32>,
grad: nalgebra::Vector4<f32>,
) {
// TODO: correct for non-zero distance value in grad.w
self.mass_point += nalgebra::Vector4::new(pos.x, pos.y, pos.z, 1.0);
let norm = grad.xyz().normalize();
self.ata += norm * norm.transpose();
self.atb += norm * norm.dot(&pos);
self.btb += norm.dot(&pos).powi(2);
}
/// Solve the given QEF, minimizing towards the mass point
///
/// Returns a vertex localized within the given cell, and adjusts the solver
/// to increase the likelihood that the vertex is bounded in the cell.
///
/// Also returns the QEF error as the second item in the tuple
pub fn solve(&self) -> (CellVertex<3>, f32) {
// This gets a little tricky; see
// https://www.mattkeeter.com/projects/qef for a walkthrough of QEF math
// and references to primary sources.
let center = self.mass_point.xyz() / self.mass_point.w;
let atb = self.atb - self.ata * center;
let svd = nalgebra::linalg::SVD::new(self.ata, true, true);
// nalgebra doesn't always actually order singular values (?!?)
// https://github.com/dimforge/nalgebra/issues/1215
let mut singular_values =
svd.singular_values.data.0[0].map(ordered_float::OrderedFloat);
singular_values.sort();
singular_values.reverse();
let singular_values = singular_values.map(|o| o.0);
// Skip any eigenvalues that are small relative to the maximum
// eigenvalue. This is very much a tuned value (alas!). If the value
// is too small, then we incorrectly pick high-rank solutions, which may
// shoot vertices out of their cells in near-planar situations. If the
// value is too large, then we incorrectly pick low-rank solutions,
// which makes us less likely to snap to sharp features.
//
// For example, our cone test needs to use a rank-3 solver for
// eigenvalues of [1.5633028, 1.430821, 0.0058764853] (a dynamic range
// of 2e3); while the bear model needs to use a rank-2 solver for
// eigenvalues of [2.87, 0.13, 5.64e-7] (a dynamic range of 10^7). We
// pick 10^3 here somewhat arbitrarily to be within that range.
const EIGENVALUE_CUTOFF_RELATIVE: f32 = 1e-3;
let cutoff = singular_values[0].abs() * EIGENVALUE_CUTOFF_RELATIVE;
// Intuition about `rank`:
// 0 => all eigenvalues are invalid (?!), use the center point
// 1 => the first eigenvalue is valid, this must be planar
// 2 => the first two eigenvalues are valid, this is a planar or an edge
// 3 => all eigenvalues are valid, this is a planar, edge, or corner
let rank = (0..3)
.find(|i| singular_values[*i].abs() < cutoff)
.unwrap_or(3);
let epsilon = singular_values.get(rank).cloned().unwrap_or(0.0);
let sol = svd.solve(&atb, epsilon);
let pos = sol.map(|c| c + center).unwrap_or(center);
// We'll clamp the error to a small > 0 value for ease of comparison
let err = ((pos.transpose() * self.ata * pos
- 2.0 * pos.transpose() * self.atb)[0]
+ self.btb)
.max(1e-6);
(CellVertex { pos }, err)
}
}
#[cfg(test)]
mod test {
use super::*;
use nalgebra::{Vector3, Vector4};
#[test]
fn qef_rank2() {
let mut q = QuadraticErrorSolver::new();
q.add_intersection(
Vector3::new(-0.5, -0.75, -0.75),
Vector4::new(0.24, 0.12, 0.0, 0.0),
);
q.add_intersection(
Vector3::new(-0.75, -1.0, -0.6),
Vector4::new(0.0, 0.0, 0.31, 0.0),
);
q.add_intersection(
Vector3::new(-0.50, -1.0, -0.6),
Vector4::new(0.0, 0.0, 0.31, 0.0),
);
let (_out, err) = q.solve();
assert_eq!(err, 1e-6);
}
#[test]
fn qef_near_planar() {
let mut q = QuadraticErrorSolver::new();
q.add_intersection(
Vector3::new(-0.5, -0.25, 0.4999981),
Vector4::new(-0.66666776, -0.33333388, 0.66666526, -1.2516975e-6),
);
q.add_intersection(
Vector3::new(-0.5, -0.25, 0.50),
Vector4::new(-0.6666667, -0.33333334, 0.6666667, 0.0),
);
q.add_intersection(
Vector3::new(-0.5, -0.25, 0.50),
Vector4::new(-0.6666667, -0.33333334, 0.6666667, 0.0),
);
let (out, err) = q.solve();
assert_eq!(err, 1e-6);
let expected = Vector3::new(-0.5, -0.25, 0.5);
assert!(
(out.pos - expected).norm() < 1e-3,
"expected {expected:?}, got {:?}",
out.pos
);
}
}