#include "dc_qef.h"
#include <cmath>
#include <cstring>
// ---------------------------------------------------------------------------
// Helpers
// ---------------------------------------------------------------------------
static inline float dot3(const float a[3], const float b[3]) {
return a[0]*b[0] + a[1]*b[1] + a[2]*b[2];
}
// Multiply symmetric 3x3 (stored as upper-triangle, 6 floats) by vector.
static inline void sym3_mul(const float m[6], const float v[3], float out[3]) {
// [ m0 m1 m2 ] [ v0 ]
// [ m1 m3 m4 ] * [ v1 ]
// [ m2 m4 m5 ] [ v2 ]
out[0] = m[0]*v[0] + m[1]*v[1] + m[2]*v[2];
out[1] = m[1]*v[0] + m[3]*v[1] + m[4]*v[2];
out[2] = m[2]*v[0] + m[4]*v[1] + m[5]*v[2];
}
// ---------------------------------------------------------------------------
// Jacobi eigenvalue decomposition for 3x3 symmetric matrices.
//
// Decomposes S = V diag(eigenvalues) V^T.
// V is orthogonal, eigenvalues are sorted descending.
// ---------------------------------------------------------------------------
static void jacobi_rotation(float S[6], float V[9], int p, int q) {
// S is symmetric upper-triangle: 0=xx 1=xy 2=xz 3=yy 4=yz 5=zz
// Map (row,col) with row<=col to index in upper triangle.
// (0,0)=0 (0,1)=1 (0,2)=2 (1,1)=3 (1,2)=4 (2,2)=5
auto idx = [](int r, int c) -> int {
if (r > c) { int t = r; r = c; c = t; }
// r<=c guaranteed
if (r == 0) return c; // (0,0)=0, (0,1)=1, (0,2)=2
if (r == 1) return c + 2; // (1,1)=3, (1,2)=4
return 5; // (2,2)=5
};
float s_pq = S[idx(p, q)];
if (std::fabs(s_pq) < 1e-12f) return;
float s_pp = S[idx(p, p)];
float s_qq = S[idx(q, q)];
float tau = (s_qq - s_pp) / (2.0f * s_pq);
float t;
if (tau >= 0.0f)
t = 1.0f / (tau + std::sqrt(1.0f + tau * tau));
else
t = -1.0f / (-tau + std::sqrt(1.0f + tau * tau));
float c = 1.0f / std::sqrt(1.0f + t * t);
float s = t * c;
// Update S. We need to rebuild affected elements.
// Make a full symmetric copy, rotate, pack back.
float M[3][3];
M[0][0] = S[0]; M[0][1] = S[1]; M[0][2] = S[2];
M[1][0] = S[1]; M[1][1] = S[3]; M[1][2] = S[4];
M[2][0] = S[2]; M[2][1] = S[4]; M[2][2] = S[5];
// Givens rotation: columns p,q of M and rows p,q of M.
// Apply on right: M' = M * G where G rotates columns p,q.
for (int i = 0; i < 3; i++) {
float a = M[i][p], b = M[i][q];
M[i][p] = c * a - s * b;
M[i][q] = s * a + c * b;
}
// Apply on left: M'' = G^T * M'
for (int j = 0; j < 3; j++) {
float a = M[p][j], b = M[q][j];
M[p][j] = c * a - s * b;
M[q][j] = s * a + c * b;
}
// Pack back upper triangle.
S[0] = M[0][0]; S[1] = M[0][1]; S[2] = M[0][2];
S[3] = M[1][1]; S[4] = M[1][2]; S[5] = M[2][2];
// Accumulate into V (columns are eigenvectors).
for (int i = 0; i < 3; i++) {
float a = V[i * 3 + p], b = V[i * 3 + q];
V[i * 3 + p] = c * a - s * b;
V[i * 3 + q] = s * a + c * b;
}
}
// Decompose symmetric S into eigenvalues (evals[3]) and eigenvectors
// (columns of evecs[9], row-major 3x3).
static void sym3_eigen(const float S_in[6], float evals[3], float evecs[9]) {
float S[6];
std::memcpy(S, S_in, sizeof(S));
// V = identity
std::memset(evecs, 0, 9 * sizeof(float));
evecs[0] = evecs[4] = evecs[8] = 1.0f;
// Jacobi sweeps — 3x3 converges very fast.
for (int iter = 0; iter < 20; iter++) {
// Off-diagonal magnitude.
float off = S[1]*S[1] + S[2]*S[2] + S[4]*S[4];
if (off < 1e-20f) break;
jacobi_rotation(S, evecs, 0, 1);
jacobi_rotation(S, evecs, 0, 2);
jacobi_rotation(S, evecs, 1, 2);
}
evals[0] = S[0];
evals[1] = S[3];
evals[2] = S[5];
}
// ---------------------------------------------------------------------------
// QEFData implementation
// ---------------------------------------------------------------------------
void QEFData::clear() {
for (int i = 0; i < 6; i++) ata[i] = 0.0f;
for (int i = 0; i < 3; i++) atb[i] = 0.0f;
btb = 0.0f;
for (int i = 0; i < 3; i++) masspoint[i] = 0.0f;
point_count = 0;
}
void QEFData::add(const float point[3], const float normal[3]) {
// ATA += outer(n, n) (upper triangle)
ata[0] += normal[0] * normal[0];
ata[1] += normal[0] * normal[1];
ata[2] += normal[0] * normal[2];
ata[3] += normal[1] * normal[1];
ata[4] += normal[1] * normal[2];
ata[5] += normal[2] * normal[2];
// d = dot(normal, point)
float d = dot3(normal, point);
// ATB += d * normal
atb[0] += d * normal[0];
atb[1] += d * normal[1];
atb[2] += d * normal[2];
// BTB += d*d
btb += d * d;
// Update running average of masspoint.
float n = (float)(point_count + 1);
masspoint[0] = masspoint[0] * (point_count / n) + point[0] / n;
masspoint[1] = masspoint[1] * (point_count / n) + point[1] / n;
masspoint[2] = masspoint[2] * (point_count / n) + point[2] / n;
point_count++;
}
void QEFData::accumulate(const QEFData& other) {
for (int i = 0; i < 6; i++) ata[i] += other.ata[i];
for (int i = 0; i < 3; i++) atb[i] += other.atb[i];
btb += other.btb;
// Combine masspoints as weighted average.
int total = point_count + other.point_count;
if (total > 0) {
for (int i = 0; i < 3; i++) {
masspoint[i] = (masspoint[i] * point_count +
other.masspoint[i] * other.point_count) / (float)total;
}
}
point_count = total;
}
void QEFData::solve(float result[3]) const {
// Eigendecompose ATA = V diag(evals) V^T
float evals[3], V[9]; // V row-major: V[row*3+col]
sym3_eigen(ata, evals, V);
// Find largest eigenvalue for threshold.
float max_eval = 0.0f;
for (int i = 0; i < 3; i++) {
float a = std::fabs(evals[i]);
if (a > max_eval) max_eval = a;
}
float threshold = std::fmax(max_eval * 1e-4f, 1e-7f);
// Pseudoinverse solve: x = V diag(1/evals_i) V^T atb
// For degenerate directions, use masspoint to supply missing info.
//
// x = masspoint + sum_i [ (v_i . (atb - ATA*masspoint)) / evals_i ] * v_i
// (only for eigenvalues above threshold)
// Compute r = atb - ATA * masspoint
float ata_mp[3];
sym3_mul(ata, masspoint, ata_mp);
float r[3];
for (int i = 0; i < 3; i++) r[i] = atb[i] - ata_mp[i];
result[0] = masspoint[0];
result[1] = masspoint[1];
result[2] = masspoint[2];
for (int i = 0; i < 3; i++) {
if (std::fabs(evals[i]) < threshold) continue;
// Eigenvector i is column i of V, stored row-major: V[row*3+i]
float vi[3] = { V[0*3+i], V[1*3+i], V[2*3+i] };
float proj = dot3(vi, r) / evals[i];
result[0] += proj * vi[0];
result[1] += proj * vi[1];
result[2] += proj * vi[2];
}
}
void QEFData::solve_biased(float result[3], const float center[3], float bias_weight) const {
// Add Tikhonov regularization: minimize
// E(x) = sum (n_i . (x - p_i))^2 + bias_weight * ||x - center||^2
//
// This adds bias_weight * I to ATA and bias_weight * center to ATb.
// The masspoint is unchanged (it still reflects the actual crossings).
float biased_ata[6];
float biased_atb[3];
for (int i = 0; i < 6; i++) biased_ata[i] = ata[i];
for (int i = 0; i < 3; i++) biased_atb[i] = atb[i];
// Add bias_weight * I to diagonal of ATA
biased_ata[0] += bias_weight; // xx
biased_ata[3] += bias_weight; // yy
biased_ata[5] += bias_weight; // zz
// Add bias_weight * center to ATb
biased_atb[0] += bias_weight * center[0];
biased_atb[1] += bias_weight * center[1];
biased_atb[2] += bias_weight * center[2];
// Eigendecompose biased ATA
float evals[3], V[9];
sym3_eigen(biased_ata, evals, V);
float max_eval = 0.0f;
for (int i = 0; i < 3; i++) {
float a = std::fabs(evals[i]);
if (a > max_eval) max_eval = a;
}
float threshold = std::fmax(max_eval * 1e-4f, 1e-7f);
// Solve relative to masspoint
float ata_mp[3];
sym3_mul(biased_ata, masspoint, ata_mp);
float r[3];
for (int i = 0; i < 3; i++) r[i] = biased_atb[i] - ata_mp[i];
result[0] = masspoint[0];
result[1] = masspoint[1];
result[2] = masspoint[2];
for (int i = 0; i < 3; i++) {
if (std::fabs(evals[i]) < threshold) continue;
float vi[3] = { V[0*3+i], V[1*3+i], V[2*3+i] };
float proj = dot3(vi, r) / evals[i];
result[0] += proj * vi[0];
result[1] += proj * vi[1];
result[2] += proj * vi[2];
}
}
float QEFData::error(const float point[3]) const {
// E(x) = x^T ATA x - 2 x^T ATB + BTB
float ata_x[3];
sym3_mul(ata, point, ata_x);
return dot3(point, ata_x) - 2.0f * dot3(point, atb) + btb;
}