#include "dc_qef.h"
#include <cstdio>
#include <cmath>
#include <cstdlib>
// ---------------------------------------------------------------------------
// Minimal test harness
// ---------------------------------------------------------------------------
static int tests_run = 0;
static int tests_passed = 0;
#define ASSERT_NEAR(val, expected, tol, msg) do { \
float _v = (val), _e = (expected), _t = (tol); \
if (std::fabs(_v - _e) > _t) { \
std::printf(" FAIL: %s got %.6f expected %.6f (tol %.6f)\n", \
msg, _v, _e, _t); \
return false; \
} \
} while(0)
static void report(const char* name, bool pass) {
tests_run++;
if (pass) { tests_passed++; std::printf("[PASS] %s\n", name); }
else { std::printf("[FAIL] %s\n", name); }
}
// ---------------------------------------------------------------------------
// Tests
// ---------------------------------------------------------------------------
static bool test_flat_plane() {
// Four intersection points on the plane y=5, all with normal (0,1,0).
// The QEF should place the vertex at the masspoint with y=5.
QEFData q;
q.clear();
float n[3] = {0, 1, 0};
float pts[4][3] = {{0,5,0}, {1,5,0}, {0,5,1}, {1,5,1}};
for (int i = 0; i < 4; i++) q.add(pts[i], n);
float res[3];
q.solve(res);
// The masspoint x,z = (0.5, 0.5). y must be 5.
ASSERT_NEAR(res[1], 5.0f, 1e-4f, "y coordinate");
ASSERT_NEAR(res[0], 0.5f, 1e-4f, "x coordinate (masspoint avg)");
ASSERT_NEAR(res[2], 0.5f, 1e-4f, "z coordinate (masspoint avg)");
// Error at the solution should be ~0.
float err = q.error(res);
ASSERT_NEAR(err, 0.0f, 1e-4f, "error at solution");
return true;
}
static bool test_sharp_corner() {
// Three perpendicular planes meeting at (2, 3, 4).
// Plane 1: normal (1,0,0), point on plane = (2, 0, 0)
// Plane 2: normal (0,1,0), point on plane = (0, 3, 0)
// Plane 3: normal (0,0,1), point on plane = (0, 0, 4)
QEFData q;
q.clear();
float n0[3] = {1,0,0}, p0[3] = {2,0,0};
float n1[3] = {0,1,0}, p1[3] = {0,3,0};
float n2[3] = {0,0,1}, p2[3] = {0,0,4};
q.add(p0, n0);
q.add(p1, n1);
q.add(p2, n2);
float res[3];
q.solve(res);
ASSERT_NEAR(res[0], 2.0f, 1e-4f, "x");
ASSERT_NEAR(res[1], 3.0f, 1e-4f, "y");
ASSERT_NEAR(res[2], 4.0f, 1e-4f, "z");
float err = q.error(res);
ASSERT_NEAR(err, 0.0f, 1e-4f, "error at corner");
return true;
}
static bool test_edge() {
// Two perpendicular planes meeting along the line x=3, z=7 (y free).
// Plane 1: normal (1,0,0), point (3, 0, 0)
// Plane 2: normal (0,0,1), point (0, 0, 7)
// With masspoint y avg the solver should pick a point on the line.
QEFData q;
q.clear();
float n0[3] = {1,0,0}, p0[3] = {3,1,0};
float n1[3] = {0,0,1}, p1[3] = {0,5,7};
q.add(p0, n0);
q.add(p1, n1);
float res[3];
q.solve(res);
ASSERT_NEAR(res[0], 3.0f, 1e-4f, "x on edge");
ASSERT_NEAR(res[2], 7.0f, 1e-4f, "z on edge");
// y should be the masspoint y = (1+5)/2 = 3
ASSERT_NEAR(res[1], 3.0f, 1e-4f, "y from masspoint");
float err = q.error(res);
ASSERT_NEAR(err, 0.0f, 1e-4f, "error on edge");
return true;
}
static bool test_degenerate() {
// All normals parallel (rank 1). Three points at different positions
// but all normals = (0,1,0). The solver should constrain y and use
// masspoint for x, z.
QEFData q;
q.clear();
float n[3] = {0,1,0};
float p0[3] = {1, 10, 2};
float p1[3] = {3, 10, 4};
float p2[3] = {5, 10, 6};
q.add(p0, n);
q.add(p1, n);
q.add(p2, n);
float res[3];
q.solve(res);
// y must be 10.
ASSERT_NEAR(res[1], 10.0f, 1e-4f, "y constrained");
// x, z = masspoint = (3, 4)
ASSERT_NEAR(res[0], 3.0f, 1e-4f, "x from masspoint");
ASSERT_NEAR(res[2], 4.0f, 1e-4f, "z from masspoint");
return true;
}
static bool test_accumulation() {
// Build one QEF with all four planes, and another by accumulating two
// separate QEFs. Results must match.
float planes[][6] = {
// point(3), normal(3)
{2, 0, 0, 1, 0, 0},
{0, 3, 0, 0, 1, 0},
{0, 0, 4, 0, 0, 1},
{2, 3, 4, 0.577350f, 0.577350f, 0.577350f}, // diagonal
};
// Single QEF with all planes.
QEFData qAll;
qAll.clear();
for (int i = 0; i < 4; i++) qAll.add(planes[i], planes[i]+3);
// Two halves accumulated.
QEFData qA, qB;
qA.clear(); qB.clear();
for (int i = 0; i < 2; i++) qA.add(planes[i], planes[i]+3);
for (int i = 2; i < 4; i++) qB.add(planes[i], planes[i]+3);
qA.accumulate(qB);
// Internal state must match.
for (int i = 0; i < 6; i++)
ASSERT_NEAR(qA.ata[i], qAll.ata[i], 1e-5f, "ata match");
for (int i = 0; i < 3; i++)
ASSERT_NEAR(qA.atb[i], qAll.atb[i], 1e-5f, "atb match");
ASSERT_NEAR(qA.btb, qAll.btb, 1e-5f, "btb match");
for (int i = 0; i < 3; i++)
ASSERT_NEAR(qA.masspoint[i], qAll.masspoint[i], 1e-5f, "masspoint match");
// Solve results must match.
float resAll[3], resAcc[3];
qAll.solve(resAll);
qA.solve(resAcc);
ASSERT_NEAR(resAcc[0], resAll[0], 1e-4f, "solve x match");
ASSERT_NEAR(resAcc[1], resAll[1], 1e-4f, "solve y match");
ASSERT_NEAR(resAcc[2], resAll[2], 1e-4f, "solve z match");
return true;
}
static bool test_error_computation() {
// Build a QEF from a single plane: y=5, normal (0,1,0), point (0,5,0).
// E(x) = (ny*(y - py))^2 = (y - 5)^2
QEFData q;
q.clear();
float n[3] = {0,1,0}, p[3] = {0,5,0};
q.add(p, n);
// At the plane: error = 0.
float on[3] = {10, 5, 20};
ASSERT_NEAR(q.error(on), 0.0f, 1e-5f, "on-plane error");
// At y=7: error = (7-5)^2 = 4.
float off[3] = {10, 7, 20};
ASSERT_NEAR(q.error(off), 4.0f, 1e-5f, "off-plane error");
// At y=0: error = (0-5)^2 = 25.
float far[3] = {0, 0, 0};
ASSERT_NEAR(q.error(far), 25.0f, 1e-5f, "far error");
return true;
}
static bool test_overconstrained() {
// More planes than dimensions — 6 planes roughly converging near origin.
// Just verify it doesn't crash and produces something reasonable.
QEFData q;
q.clear();
float data[][6] = {
{ 1,0,0, 1,0,0},
{-1,0,0, -1,0,0},
{0, 1,0, 0,1,0},
{0,-1,0, 0,-1,0},
{0,0, 1, 0,0,1},
{0,0,-1, 0,0,-1},
};
for (int i = 0; i < 6; i++) q.add(data[i], data[i]+3);
float res[3];
q.solve(res);
// All planes say "distance 1 from origin in their normal direction",
// but with opposing normals: n.p = +1 and n.p = -1 for each axis.
// Average plane distance = 0 per axis, so solution ~ origin.
ASSERT_NEAR(res[0], 0.0f, 1e-3f, "overconst x");
ASSERT_NEAR(res[1], 0.0f, 1e-3f, "overconst y");
ASSERT_NEAR(res[2], 0.0f, 1e-3f, "overconst z");
return true;
}
// ---------------------------------------------------------------------------
// Main
// ---------------------------------------------------------------------------
int main() {
std::printf("=== QEF Solver Tests ===\n\n");
report("flat_plane", test_flat_plane());
report("sharp_corner", test_sharp_corner());
report("edge", test_edge());
report("degenerate", test_degenerate());
report("accumulation", test_accumulation());
report("error_computation", test_error_computation());
report("overconstrained", test_overconstrained());
std::printf("\n%d / %d tests passed.\n", tests_passed, tests_run);
return (tests_passed == tests_run) ? 0 : 1;
}