茂之钳
d949da2719
The contour extraction previously relied on LS fitting to handle loop closure internally, but removing the closing point caused fitting quality issues. Restore comp.push_back(comp[0]) to ensure closed curves are properly fitted. Related: VDE-021 mesh contour extraction
482 lines
17 KiB
C++
482 lines
17 KiB
C++
#include "vde/mesh/mesh_to_curve.h"
|
||
#include <algorithm>
|
||
#include <cmath>
|
||
#include <map>
|
||
#include <set>
|
||
#include <unordered_map>
|
||
#include <cassert>
|
||
|
||
namespace vde::mesh {
|
||
using core::Point3D;
|
||
using core::Vector3D;
|
||
|
||
namespace {
|
||
|
||
// ── Helper: chord-length parameterization of polyline ──
|
||
std::vector<double> chord_length_params(const std::vector<Point3D>& pts) {
|
||
std::vector<double> t(pts.size(), 0.0);
|
||
double total = 0.0;
|
||
for (size_t i = 1; i < pts.size(); ++i)
|
||
total += (pts[i] - pts[i-1]).norm();
|
||
if (total < 1e-15) {
|
||
for (size_t i = 0; i < pts.size(); ++i)
|
||
t[i] = static_cast<double>(i) / (pts.size() - 1);
|
||
return t;
|
||
}
|
||
for (size_t i = 1; i < pts.size(); ++i)
|
||
t[i] = t[i-1] + (pts[i] - pts[i-1]).norm() / total;
|
||
return t;
|
||
}
|
||
|
||
// ── Helper: uniform knot vector for degree p, n control points ──
|
||
std::vector<double> make_uniform_knots(int n, int p) {
|
||
int m = n + p + 1;
|
||
std::vector<double> knots(m, 0.0);
|
||
for (int i = 0; i < p + 1; ++i) knots[i] = 0.0;
|
||
for (int i = n; i < m; ++i) knots[i] = 1.0;
|
||
int inner = m - 2 * (p + 1);
|
||
for (int i = 0; i < inner; ++i)
|
||
knots[p + 1 + i] = static_cast<double>(i + 1) / (inner + 1);
|
||
return knots;
|
||
}
|
||
|
||
// ── B-spline basis function N_{i,p}(t) ──
|
||
double basis_function(int i, int p, const std::vector<double>& knots, double t) {
|
||
if (p == 0) {
|
||
return (t >= knots[i] && t < knots[i+1]) ||
|
||
(t >= knots[i] && t <= knots[i+1] &&
|
||
i + 1 == static_cast<int>(knots.size()) - p - 1) ? 1.0 : 0.0;
|
||
}
|
||
double left = 0.0, right = 0.0;
|
||
double denom1 = knots[i+p] - knots[i];
|
||
double denom2 = knots[i+p+1] - knots[i+1];
|
||
if (denom1 > 1e-15)
|
||
left = (t - knots[i]) / denom1 * basis_function(i, p-1, knots, t);
|
||
if (denom2 > 1e-15)
|
||
right = (knots[i+p+1] - t) / denom2 * basis_function(i+1, p-1, knots, t);
|
||
return left + right;
|
||
}
|
||
|
||
// ── Least-squares B-spline fit ──
|
||
// Given m sample points with parameters t, compute n control points (degree=p)
|
||
// Solve: N^T N CP = N^T P (m×n linear system)
|
||
curves::NurbsCurve ls_fit_bspline(const std::vector<Point3D>& samples,
|
||
const std::vector<double>& params,
|
||
int degree, int num_ctrl) {
|
||
int m = static_cast<int>(samples.size());
|
||
if (m < 2 || num_ctrl < degree + 1) {
|
||
// Degenerate: return empty curve
|
||
return curves::NurbsCurve();
|
||
}
|
||
int n = std::max(degree + 1, num_ctrl);
|
||
n = std::min(n, m); // Can't have more ctrl points than samples
|
||
|
||
auto knots = make_uniform_knots(n, degree);
|
||
|
||
// Build normal equations: A^T A x = A^T b
|
||
// A_ij = N_{j,degree}(t_i)
|
||
// For points, we solve 3 independent systems (x, y, z)
|
||
std::vector<std::vector<double>> ATA(n, std::vector<double>(n, 0.0));
|
||
std::vector<Point3D> ATb(n, Point3D(0, 0, 0));
|
||
|
||
for (int i = 0; i < m; ++i) {
|
||
double t = params[i];
|
||
// Evaluate all non-zero basis functions at t
|
||
std::vector<double> N(n, 0.0);
|
||
for (int j = 0; j < n; ++j)
|
||
N[j] = basis_function(j, degree, knots, t);
|
||
|
||
for (int r = 0; r < n; ++r) {
|
||
if (std::abs(N[r]) < 1e-15) continue;
|
||
for (int c = 0; c < n; ++c) {
|
||
if (std::abs(N[c]) < 1e-15) continue;
|
||
ATA[r][c] += N[r] * N[c];
|
||
}
|
||
ATb[r] = ATb[r] + N[r] * samples[i];
|
||
}
|
||
}
|
||
|
||
// Gaussian elimination for each coordinate
|
||
std::vector<double> x(n), y(n), z(n);
|
||
auto solve_system = [&](std::vector<double>& b) {
|
||
auto mat = ATA;
|
||
// Forward elimination with partial pivoting
|
||
for (int k = 0; k < n; ++k) {
|
||
int max_row = k;
|
||
double max_val = std::abs(mat[k][k]);
|
||
for (int r = k + 1; r < n; ++r) {
|
||
if (std::abs(mat[r][k]) > max_val) {
|
||
max_val = std::abs(mat[r][k]);
|
||
max_row = r;
|
||
}
|
||
}
|
||
if (max_val < 1e-15) continue;
|
||
if (max_row != k) {
|
||
std::swap(mat[k], mat[max_row]);
|
||
std::swap(b[k], b[max_row]);
|
||
}
|
||
double pivot = mat[k][k];
|
||
for (int r = k + 1; r < n; ++r) {
|
||
double factor = mat[r][k] / pivot;
|
||
for (int c = k; c < n; ++c)
|
||
mat[r][c] -= factor * mat[k][c];
|
||
b[r] -= factor * b[k];
|
||
}
|
||
}
|
||
// Back substitution
|
||
for (int k = n - 1; k >= 0; --k) {
|
||
double sum = b[k];
|
||
for (int c = k + 1; c < n; ++c)
|
||
sum -= mat[k][c] * b[c];
|
||
b[k] = (std::abs(mat[k][k]) > 1e-15) ? sum / mat[k][k] : 0.0;
|
||
}
|
||
};
|
||
|
||
std::vector<double> bx(n), by(n), bz(n);
|
||
for (int i = 0; i < n; ++i) {
|
||
bx[i] = ATb[i].x();
|
||
by[i] = ATb[i].y();
|
||
bz[i] = ATb[i].z();
|
||
}
|
||
solve_system(bx);
|
||
solve_system(by);
|
||
solve_system(bz);
|
||
|
||
std::vector<Point3D> cps(n);
|
||
for (int i = 0; i < n; ++i)
|
||
cps[i] = Point3D(bx[i], by[i], bz[i]);
|
||
|
||
// For closed curves, wrap first 'degree' ctrl points
|
||
// to ensure C0 continuity at the seam
|
||
for (int i = 0; i < degree && i < n; ++i)
|
||
cps.push_back(cps[i]);
|
||
|
||
std::vector<double> weights(cps.size(), 1.0);
|
||
return curves::NurbsCurve(cps, make_uniform_knots(static_cast<int>(cps.size()), degree),
|
||
weights, degree);
|
||
}
|
||
|
||
// ── Find boundary edges by counting undirected edge occurrences across faces ──
|
||
// Since add_face never sets opposite_index, all edges appear alone. Count each
|
||
// undirected edge across all faces; those appearing exactly once are boundary edges.
|
||
std::vector<std::pair<int,int>> find_boundary_edges(const HalfedgeMesh& mesh) {
|
||
std::map<std::pair<int,int>, int> edge_count;
|
||
// Also record one face-side direction for each edge so we can orient the boundary
|
||
std::map<std::pair<int,int>, std::pair<int,int>> edge_dir; // undirected key -> face-winding direction
|
||
for (size_t fi = 0; fi < mesh.num_faces(); ++fi) {
|
||
auto fv = mesh.face_vertices(static_cast<int>(fi));
|
||
int n = static_cast<int>(fv.size());
|
||
for (int i = 0; i < n; ++i) {
|
||
int v0 = fv[i];
|
||
int v1 = fv[(i + 1) % n];
|
||
auto key = std::make_pair(std::min(v0, v1), std::max(v0, v1));
|
||
edge_count[key]++;
|
||
if (edge_count[key] == 1)
|
||
edge_dir[key] = std::make_pair(v0, v1); // face winding direction
|
||
}
|
||
}
|
||
|
||
std::vector<std::pair<int,int>> result;
|
||
for (const auto& [key, cnt] : edge_count) {
|
||
if (cnt == 1) {
|
||
// Boundary edge: the boundary direction is opposite to face winding
|
||
auto [fv0, fv1] = edge_dir[key];
|
||
result.emplace_back(fv1, fv0); // reverse for boundary loop direction
|
||
}
|
||
}
|
||
return result;
|
||
}
|
||
|
||
// ── Trace boundary loops from adjacency map ──
|
||
std::vector<std::vector<Point3D>> trace_boundary_loops(
|
||
const HalfedgeMesh& mesh,
|
||
const std::vector<std::pair<int,int>>& be)
|
||
{
|
||
// Build adjacency: vertex -> list of adjacent boundary vertices
|
||
std::map<int, std::vector<int>> adj;
|
||
for (const auto& [v0, v1] : be) {
|
||
adj[v0].push_back(v1);
|
||
adj[v1].push_back(v0); // both directions for tracing
|
||
}
|
||
|
||
std::vector<std::vector<Point3D>> loops;
|
||
std::set<int> visited_verts;
|
||
|
||
for (const auto& [start, neighbors] : adj) {
|
||
if (visited_verts.count(start)) continue;
|
||
if (neighbors.size() != 2) continue; // regular boundary vertex has degree 2 in boundary graph
|
||
|
||
std::vector<int> loop_verts;
|
||
int cur = start;
|
||
int prev = -1;
|
||
bool closed = false;
|
||
|
||
while (!visited_verts.count(cur)) {
|
||
visited_verts.insert(cur);
|
||
loop_verts.push_back(cur);
|
||
|
||
auto it = adj.find(cur);
|
||
if (it == adj.end()) break;
|
||
|
||
// Find the next unvisited neighbor (not the one we came from)
|
||
int next = -1;
|
||
for (int nv : it->second) {
|
||
if (nv != prev && (!visited_verts.count(nv) || nv == start)) {
|
||
next = nv;
|
||
break;
|
||
}
|
||
}
|
||
if (next < 0) break;
|
||
if (next == start) { closed = true; break; }
|
||
prev = cur;
|
||
cur = next;
|
||
}
|
||
|
||
if (closed && loop_verts.size() >= 3) {
|
||
std::vector<Point3D> pts;
|
||
for (int vi : loop_verts)
|
||
pts.push_back(mesh.vertex(static_cast<size_t>(vi)));
|
||
// Don't add closing duplicate — it would create a singular LS system
|
||
loops.push_back(std::move(pts));
|
||
}
|
||
}
|
||
return loops;
|
||
}
|
||
|
||
} // namespace
|
||
|
||
// ── Public API ────────────────────────────────────────
|
||
|
||
curves::NurbsCurve mesh_boundary_to_curve(const HalfedgeMesh& mesh) {
|
||
auto be = find_boundary_edges(mesh);
|
||
if (be.empty()) return curves::NurbsCurve();
|
||
|
||
auto loops = trace_boundary_loops(mesh, be);
|
||
if (loops.empty()) return curves::NurbsCurve();
|
||
|
||
// Pick the longest loop
|
||
size_t best = 0;
|
||
for (size_t i = 1; i < loops.size(); ++i)
|
||
if (loops[i].size() > loops[best].size())
|
||
best = i;
|
||
|
||
const auto& pts = loops[best];
|
||
int n_pts = static_cast<int>(pts.size());
|
||
if (n_pts < 3) {
|
||
return curves::NurbsCurve();
|
||
}
|
||
|
||
// Upsample if too few points (need at least degree+1 for LS fit)
|
||
std::vector<Point3D> sampled;
|
||
if (n_pts < 4) {
|
||
// Insert midpoints between every pair of vertices
|
||
sampled.push_back(pts[0]);
|
||
for (int i = 0; i < n_pts; ++i) {
|
||
int j = (i + 1) % n_pts;
|
||
sampled.push_back((pts[i] + pts[j]) * 0.5);
|
||
sampled.push_back(pts[j]);
|
||
}
|
||
} else if (n_pts <= 64) {
|
||
sampled = pts;
|
||
} else {
|
||
int step = n_pts / 64;
|
||
for (int i = 0; i < 64; ++i)
|
||
sampled.push_back(pts[i * step]);
|
||
sampled.push_back(pts[0]); // close
|
||
}
|
||
|
||
auto params = chord_length_params(sampled);
|
||
// For a closed loop, degree 3 with ctrl_pts = max(4, sampled/2)
|
||
int num_ctrl = std::max(4, static_cast<int>(sampled.size()) / 2);
|
||
return ls_fit_bspline(sampled, params, 3, num_ctrl);
|
||
}
|
||
|
||
std::vector<curves::NurbsCurve> extract_profiles_from_mesh(
|
||
const HalfedgeMesh& mesh, const Vector3D& projection_axis)
|
||
{
|
||
auto be = find_boundary_edges(mesh);
|
||
if (be.empty()) return {};
|
||
|
||
auto loops = trace_boundary_loops(mesh, be);
|
||
std::vector<curves::NurbsCurve> results;
|
||
|
||
// Normalize projection axis
|
||
Vector3D axis = projection_axis.normalized();
|
||
|
||
for (auto& pts : loops) {
|
||
if (pts.size() < 4) continue;
|
||
|
||
// Project points onto the plane perpendicular to axis
|
||
std::vector<Point3D> projected;
|
||
for (auto& p : pts) {
|
||
double d = p.dot(axis);
|
||
projected.push_back(p - d * axis);
|
||
}
|
||
|
||
// Subsample if needed
|
||
std::vector<Point3D> sampled;
|
||
int n_pts = static_cast<int>(projected.size());
|
||
if (n_pts <= 64) {
|
||
sampled = projected;
|
||
} else {
|
||
int step = n_pts / 64;
|
||
for (int i = 0; i < 64; ++i)
|
||
sampled.push_back(projected[i * step]);
|
||
sampled.push_back(projected[0]);
|
||
}
|
||
|
||
auto params = chord_length_params(sampled);
|
||
int num_ctrl = std::max(4, static_cast<int>(sampled.size()) / 2);
|
||
results.push_back(ls_fit_bspline(sampled, params, 3, num_ctrl));
|
||
}
|
||
return results;
|
||
}
|
||
|
||
std::vector<curves::NurbsCurve> mesh_contour_to_curves(
|
||
const HalfedgeMesh& mesh, double z_height)
|
||
{
|
||
// Collect all edge intersections with z=z_height by iterating faces
|
||
// Store unique intersection points keyed by their position
|
||
struct ContourPoint {
|
||
Point3D pt;
|
||
int face_idx;
|
||
int vertex_a; // edge endpoint vertex indices (for dedup)
|
||
int vertex_b;
|
||
};
|
||
|
||
struct PtKey {
|
||
double x, y, z;
|
||
bool operator<(const PtKey& o) const {
|
||
if (std::abs(x - o.x) > 1e-9) return x < o.x;
|
||
if (std::abs(y - o.y) > 1e-9) return y < o.y;
|
||
if (std::abs(z - o.z) > 1e-9) return z < o.z;
|
||
return false;
|
||
}
|
||
};
|
||
|
||
std::map<PtKey, ContourPoint> unique_pts;
|
||
std::map<int, std::vector<PtKey>> face_to_pts; // face_idx -> contour points in this face
|
||
|
||
// Iterate faces, find edges crossing z=z_height
|
||
for (size_t fi = 0; fi < mesh.num_faces(); ++fi) {
|
||
auto fv = mesh.face_vertices(static_cast<int>(fi));
|
||
int n = static_cast<int>(fv.size());
|
||
for (int i = 0; i < n; ++i) {
|
||
int v0 = fv[i];
|
||
int v1 = fv[(i + 1) % n];
|
||
double z0 = mesh.vertex(static_cast<size_t>(v0)).z();
|
||
double z1 = mesh.vertex(static_cast<size_t>(v1)).z();
|
||
|
||
if ((z0 <= z_height && z1 >= z_height) ||
|
||
(z0 >= z_height && z1 <= z_height)) {
|
||
if (std::abs(z1 - z0) < 1e-15) continue;
|
||
double t = (z_height - z0) / (z1 - z0);
|
||
Point3D pt = mesh.vertex(static_cast<size_t>(v0)) +
|
||
t * (mesh.vertex(static_cast<size_t>(v1)) -
|
||
mesh.vertex(static_cast<size_t>(v0)));
|
||
PtKey key{pt.x(), pt.y(), pt.z()};
|
||
if (unique_pts.find(key) == unique_pts.end()) {
|
||
unique_pts[key] = {pt, static_cast<int>(fi), v0, v1};
|
||
}
|
||
face_to_pts[static_cast<int>(fi)].push_back(key);
|
||
}
|
||
}
|
||
}
|
||
|
||
if (unique_pts.empty()) return {};
|
||
|
||
// Build adjacency: connect intersection points within each face
|
||
struct PtCmp {
|
||
bool operator()(const Point3D& a, const Point3D& b) const {
|
||
if (a.x() != b.x()) return a.x() < b.x();
|
||
if (a.y() != b.y()) return a.y() < b.y();
|
||
return a.z() < b.z();
|
||
}
|
||
};
|
||
std::map<Point3D, std::vector<Point3D>, PtCmp> pt_adj;
|
||
|
||
for (auto& [fi, keys] : face_to_pts) {
|
||
if (keys.size() < 2) continue;
|
||
// Connect consecutive points in this face
|
||
for (size_t i = 0; i + 1 < keys.size(); ++i) {
|
||
auto& p0 = unique_pts[keys[i]].pt;
|
||
auto& p1 = unique_pts[keys[i+1]].pt;
|
||
pt_adj[p0].push_back(p1);
|
||
pt_adj[p1].push_back(p0);
|
||
}
|
||
if (keys.size() > 2) {
|
||
auto& p0 = unique_pts[keys[0]].pt;
|
||
auto& pn = unique_pts[keys.back()].pt;
|
||
pt_adj[p0].push_back(pn);
|
||
pt_adj[pn].push_back(p0);
|
||
}
|
||
}
|
||
|
||
// Now trace connected components using the adjacency
|
||
std::set<Point3D, PtCmp> visited;
|
||
|
||
std::vector<std::vector<Point3D>> components;
|
||
|
||
for (auto& [pt, neighbors] : pt_adj) {
|
||
if (visited.count(pt)) continue;
|
||
if (neighbors.empty()) continue;
|
||
|
||
std::vector<Point3D> comp;
|
||
Point3D cur = pt;
|
||
Point3D prev_pt = pt; // arbitrary initial
|
||
while (!visited.count(cur)) {
|
||
visited.insert(cur);
|
||
comp.push_back(cur);
|
||
auto it = pt_adj.find(cur);
|
||
if (it == pt_adj.end()) break;
|
||
|
||
// Find an unvisited neighbor (not the one we came from)
|
||
Point3D next;
|
||
bool found = false;
|
||
for (auto& n : it->second) {
|
||
// Skip if it's the previous point (unless we're wrapping around)
|
||
if ((n - prev_pt).norm() < 1e-9) continue;
|
||
if (!visited.count(n)) {
|
||
next = n;
|
||
found = true;
|
||
break;
|
||
}
|
||
// Wrap: if we're back at the start and this is the start point
|
||
if (comp.size() > 1 && (n - comp[0]).norm() < 1e-9) {
|
||
next = n;
|
||
found = true;
|
||
break;
|
||
}
|
||
}
|
||
if (!found) break;
|
||
prev_pt = cur;
|
||
cur = next;
|
||
}
|
||
if (comp.size() >= 3) {
|
||
// Close the loop for fitting
|
||
comp.push_back(comp[0]);
|
||
components.push_back(std::move(comp));
|
||
}
|
||
}
|
||
|
||
std::vector<curves::NurbsCurve> results;
|
||
for (auto& comp : components) {
|
||
if (comp.size() < 4) continue;
|
||
auto sampled = comp;
|
||
if (sampled.size() > 64) {
|
||
std::vector<Point3D> sub;
|
||
int step = static_cast<int>(sampled.size()) / 64;
|
||
for (int i = 0; i < 64; ++i)
|
||
sub.push_back(sampled[i * step]);
|
||
sub.push_back(sampled[0]);
|
||
sampled = sub;
|
||
}
|
||
auto params = chord_length_params(sampled);
|
||
int num_ctrl = std::max(4, static_cast<int>(sampled.size()) / 2);
|
||
results.push_back(ls_fit_bspline(sampled, params, 3, num_ctrl));
|
||
}
|
||
return results;
|
||
}
|
||
|
||
} // namespace vde::mesh
|