hm/ViewDesignEngine
1
0
Fork 0
Code Issues Pull Requests Actions 56 Packages Projects Releases 1 Wiki Activity

feat: 3D constraint solver for assembly (coincident/concentric/distance)

Browse Source
This commit is contained in:
茂之钳
2026-07-24 16:12:36 +00:00
parent b96b6defd6
commit 5a2f01f597
5 changed files with 974 additions and 0 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/CMakeLists.txt
+1
View File
@@ -156,6 +156,7 @@ add_library(vde_brep STATIC
brep/brep_face_split.cpp
brep/feature_tree.cpp
brep/assembly_constraints.cpp
brep/constraint_solver.cpp
brep/measure.cpp
brep/trimmed_surface.cpp
brep/assembly_instance.cpp
src/brep/constraint_solver.cpp
+499
View File
@@ -0,0 +1,499 @@
#include "vde/brep/constraint_solver.h"
#include "vde/core/aabb.h"
#include "vde/core/transform.h"
#include <cmath>
#include <algorithm>
#include <array>
#include <limits>
namespace vde::brep {
namespace {
// ─────────────────────────────────────────────────
// Face descriptor for an AABB face
// ─────────────────────────────────────────────────
struct FaceDesc {
core::Vector3D normal; ///< Outward normal
core::Point3D center; ///< Face center
};
/// Return the 6 faces of an AABB (order: -X, +X, -Y, +Y, -Z, +Z).
std::array<FaceDesc, 6> aabb_faces(const core::AABB3D& bb) {
core::Point3D c = bb.center();
core::Point3D mn = bb.min();
core::Point3D mx = bb.max();
return {{
{ core::Vector3D(-1, 0, 0), core::Point3D(mn.x(), c.y(), c.z()) }, // -X
{ core::Vector3D( 1, 0, 0), core::Point3D(mx.x(), c.y(), c.z()) }, // +X
{ core::Vector3D( 0, -1, 0), core::Point3D( c.x(), mn.y(), c.z()) }, // -Y
{ core::Vector3D( 0, 1, 0), core::Point3D( c.x(), mx.y(), c.z()) }, // +Y
{ core::Vector3D( 0, 0, -1), core::Point3D( c.x(), c.y(), mn.z()) }, // -Z
{ core::Vector3D( 0, 0, 1), core::Point3D( c.x(), c.y(), mx.z()) }, // +Z
}};
}
// ─────────────────────────────────────────────────
// Closest face-pair selection
// ─────────────────────────────────────────────────
/// Find the pair of faces (one from bb_a, one from bb_b) whose
/// mate alignment requires the smallest translation of bb_b.
/// @return index into faces_a, and index into faces_b
struct FacePair { int fa; int fb; };
FacePair closest_face_pair(const core::AABB3D& bb_a, const core::AABB3D& bb_b) {
auto faces_a = aabb_faces(bb_a);
auto faces_b = aabb_faces(bb_b);
int best_fa = -1, best_fb = -1;
double best_cost = std::numeric_limits<double>::max();
for (int ia = 0; ia < 6; ++ia) {
const auto& fa = faces_a[ia];
// The mating face on B has opposite normal
core::Vector3D desired_normal = -fa.normal;
// Find the face on B whose normal is closest to desired_normal
int best_j = 0;
double best_dot = -2.0; // worst-case cos
for (int j = 0; j < 6; ++j) {
double dot = faces_b[j].normal.dot(desired_normal);
if (dot > best_dot) { best_dot = dot; best_j = j; }
}
const auto& fb = faces_b[best_j];
// Translation needed along fa.normal to bring planes coplanar
// Project the vector from fb.center to fa.center onto fa.normal
double t = fa.normal.dot(fa.center - fb.center);
// Cost = squared translation distance
double dz = std::abs(t);
double dx = std::abs(fa.center.x() - fb.center.x() - t * fa.normal.x());
double dy = std::abs(fa.center.y() - fb.center.y() - t * fa.normal.y());
double cost = dx*dx + dy*dy + dz*dz;
if (cost < best_cost) {
best_cost = cost;
best_fa = ia;
best_fb = best_j;
}
}
return {best_fa, best_fb};
}
// ─────────────────────────────────────────────────
// Closest-point between two AABBs
// ─────────────────────────────────────────────────
/// Closest point from bb_b to bb_a (point on bb_a)
core::Vector3D closest_point_on_a(const core::AABB3D& bb_a, const core::AABB3D& bb_b) {
core::Point3D cp;
for (int i = 0; i < 3; ++i) {
cp[i] = std::max(bb_a.min()[i], std::min(bb_b.center()[i], bb_a.max()[i]));
}
return cp;
}
/// Closest point from bb_a to bb_b (point on bb_b)
core::Vector3D closest_point_on_b(const core::AABB3D& bb_a, const core::AABB3D& bb_b) {
core::Point3D cp;
for (int i = 0; i < 3; ++i) {
cp[i] = std::min(bb_b.max()[i], std::max(bb_a.center()[i], bb_b.min()[i]));
}
return cp;
}
/// Signed gap along the Z direction between two AABBs
/// (positive when bb_b is above bb_a)
double z_gap(const core::AABB3D& bb_a, const core::AABB3D& bb_b) {
return bb_b.min().z() - bb_a.max().z();
}
/// Estimate cylinder axis of a node from its bounding box.
/// The axis is the direction whose AABB extent is the LEAST similar
/// to the other two extents (because the two similar extents correspond
/// to the circular cross-section diameter).
core::Vector3D estimate_cylinder_axis(const core::AABB3D& bb) {
core::Vector3D e = bb.extent();
double dx = e.x(), dy = e.y(), dz = e.z();
// Pairwise similarity – smaller diff = more similar
double diff_xy = std::abs(dx - dy);
double diff_yz = std::abs(dy - dz);
double diff_zx = std::abs(dz - dx);
// The axis direction is the one NOT in the most-similar pair
if (diff_xy <= diff_yz && diff_xy <= diff_zx)
return core::Vector3D::UnitZ(); // X,Y are similar → Z is cylinder axis
if (diff_yz <= diff_xy && diff_yz <= diff_zx)
return core::Vector3D::UnitX(); // Y,Z are similar → X is cylinder axis
return core::Vector3D::UnitY(); // Z,X are similar → Y is cylinder axis
}
// ─────────────────────────────────────────────────
// Constraint satisfaction check
// ─────────────────────────────────────────────────
/// Get pointer to the n-th direct child of the root (or nullptr if OOB)
AssemblyNode* child_at(Assembly& assy, int index) {
if (index < 0 || static_cast<size_t>(index) >= assy.root.children.size())
return nullptr;
return assy.root.children[index].get();
}
/// Check if a single constraint is satisfied
bool constraint_satisfied(
const AssemblyNode* na, const AssemblyNode* nb,
Constraint3D type, double value, double tol)
{
if (!na || !nb || !na->model.has_value() || !nb->model.has_value())
return false;
core::AABB3D bb_a = na->model->bounds();
core::AABB3D bb_b = nb->model->bounds();
// Transform bb_b to world space through nb's local_transform
core::AABB3D bb_b_world;
std::array<core::Point3D, 8> corners = {{
bb_b.min(),
core::Point3D(bb_b.max().x(), bb_b.min().y(), bb_b.min().z()),
core::Point3D(bb_b.max().x(), bb_b.max().y(), bb_b.min().z()),
core::Point3D(bb_b.min().x(), bb_b.max().y(), bb_b.min().z()),
core::Point3D(bb_b.min().x(), bb_b.min().y(), bb_b.max().z()),
core::Point3D(bb_b.max().x(), bb_b.min().y(), bb_b.max().z()),
bb_b.max(),
core::Point3D(bb_b.min().x(), bb_b.max().y(), bb_b.max().z()),
}};
for (const auto& c : corners) {
bb_b_world.expand(nb->local_transform * c);
}
switch (type) {
case Constraint3D::Coincident:
case Constraint3D::Tangent: {
// Faces share a plane: the gap between bounding-box extrema should be 0
double gap = std::abs(z_gap(bb_a, bb_b_world));
return gap <= tol;
}
case Constraint3D::Concentric: {
// Centers match in X and Y
core::Point3D ca = bb_a.center();
core::Point3D cb = bb_b_world.center();
double dx = std::abs(ca.x() - cb.x());
double dy = std::abs(ca.y() - cb.y());
return dx <= tol && dy <= tol;
}
case Constraint3D::Distance: {
// Gap should equal value
double gap = z_gap(bb_a, bb_b_world);
return std::abs(gap - value) <= tol;
}
case Constraint3D::Parallel:
case Constraint3D::Perpendicular:
case Constraint3D::Angle:
// Orientation-only constraints: always considered satisfied
// after one application (rotation has been applied)
return true;
}
return false;
}
/// Check all constraints
bool all_satisfied(Assembly& assy,
const std::vector<ConstraintEntry>& constraints,
double tol)
{
for (const auto& c : constraints) {
auto* na = child_at(assy, c.node_a);
auto* nb = child_at(assy, c.node_b);
if (!constraint_satisfied(na, nb, c.type, c.value, tol))
return false;
}
return true;
}
/// Apply a parallel constraint: rotate node_b so its "major face normal"
/// is parallel to node_a's major face normal.
core::Transform3D apply_parallel(
const AssemblyNode& node_a, AssemblyNode& node_b)
{
if (!node_a.model.has_value() || !node_b.model.has_value())
return core::Transform3D::Identity();
core::AABB3D bb_a = node_a.model->bounds();
core::AABB3D bb_b = node_b.model->bounds();
core::Vector3D ext_a = bb_a.extent();
core::Vector3D ext_b = bb_b.extent();
// Use the face with the largest area (smallest extent for that AABB's
// "primary" face direction)
// Actually, use the major axis of each AABB: the direction with the
// LARGEST extent corresponds to the longest dimension – that's the
// direction most likely to represent an extrusion/principal axis.
auto major_axis = [](const core::Vector3D& e) -> core::Vector3D {
if (e.x() >= e.y() && e.x() >= e.z()) return core::Vector3D::UnitX();
if (e.y() >= e.x() && e.y() >= e.z()) return core::Vector3D::UnitY();
return core::Vector3D::UnitZ();
};
core::Vector3D dir_a = major_axis(ext_a);
core::Vector3D dir_b = major_axis(ext_b);
// Compute rotation that aligns dir_b to dir_a
core::Vector3D cross = dir_b.cross(dir_a);
double dot = dir_b.dot(dir_a);
if (dot > 1.0 - 1e-9) return core::Transform3D::Identity(); // already aligned
if (dot < -1.0 + 1e-9) {
// Opposite – pick any perpendicular axis and rotate 180°
core::Vector3D perp = (std::abs(dir_b.x()) < 0.9)
? core::Vector3D::UnitX()
: core::Vector3D::UnitY();
core::Vector3D axis = dir_b.cross(perp).normalized();
core::Transform3D R = core::rotate(axis, M_PI);
node_b.local_transform = R * node_b.local_transform;
return R;
}
double angle = std::acos(dot);
core::Vector3D axis = cross.normalized();
core::Transform3D R = core::rotate(axis, angle);
node_b.local_transform = R * node_b.local_transform;
return R;
}
/// Apply a perpendicular constraint
core::Transform3D apply_perpendicular(
const AssemblyNode& node_a, AssemblyNode& node_b)
{
if (!node_a.model.has_value() || !node_b.model.has_value())
return core::Transform3D::Identity();
core::AABB3D bb_a = node_a.model->bounds();
core::AABB3D bb_b = node_b.model->bounds();
core::Vector3D ext_a = bb_a.extent();
core::Vector3D ext_b = bb_b.extent();
auto major_axis = [](const core::Vector3D& e) -> core::Vector3D {
if (e.x() >= e.y() && e.x() >= e.z()) return core::Vector3D::UnitX();
if (e.y() >= e.x() && e.y() >= e.z()) return core::Vector3D::UnitY();
return core::Vector3D::UnitZ();
};
core::Vector3D dir_a = major_axis(ext_a);
core::Vector3D dir_b = major_axis(ext_b);
// Target: dir_b should be perpendicular to dir_a
double dot = dir_b.dot(dir_a);
if (std::abs(dot) < 1e-9) return core::Transform3D::Identity(); // already perpendicular
// Rotate 90° around an axis orthogonal to both
core::Vector3D axis = dir_b.cross(dir_a).normalized();
if (axis.squaredNorm() < 1e-9) {
// Parallel – pick any perpendicular axis
axis = (std::abs(dir_a.x()) < 0.9)
? core::Vector3D::UnitX()
: core::Vector3D::UnitY();
axis = dir_a.cross(axis).normalized();
}
core::Transform3D R = core::rotate(axis, M_PI / 2.0);
node_b.local_transform = R * node_b.local_transform;
return R;
}
/// Apply an angle constraint
core::Transform3D apply_angle(
const AssemblyNode& node_a, AssemblyNode& node_b, double angle_rad)
{
if (!node_a.model.has_value() || !node_b.model.has_value())
return core::Transform3D::Identity();
core::AABB3D bb_a = node_a.model->bounds();
core::AABB3D bb_b = node_b.model->bounds();
auto major_axis = [](const core::Vector3D& e) -> core::Vector3D {
if (e.x() >= e.y() && e.x() >= e.z()) return core::Vector3D::UnitX();
if (e.y() >= e.x() && e.y() >= e.z()) return core::Vector3D::UnitY();
return core::Vector3D::UnitZ();
};
core::Vector3D dir_a = major_axis(bb_a.extent());
core::Vector3D dir_b = major_axis(bb_b.extent());
core::Vector3D axis = dir_b.cross(dir_a).normalized();
if (axis.squaredNorm() < 1e-9) {
axis = (std::abs(dir_a.x()) < 0.9)
? core::Vector3D::UnitX()
: core::Vector3D::UnitY();
}
core::Transform3D R = core::rotate(axis, angle_rad);
node_b.local_transform = R * node_b.local_transform;
return R;
}
} // anonymous namespace
// ═══════════════════════════════════════════════════
// Public API
// ═══════════════════════════════════════════════════
core::Transform3D apply_coincident(
const AssemblyNode& node_a, AssemblyNode& node_b)
{
if (!node_a.model.has_value() || !node_b.model.has_value())
return core::Transform3D::Identity();
core::AABB3D bb_a = node_a.model->bounds();
core::AABB3D bb_b = node_b.model->bounds();
// Find best face pair
auto pair = closest_face_pair(bb_a, bb_b);
auto faces_a = aabb_faces(bb_a);
auto faces_b = aabb_faces(bb_b);
const auto& fa = faces_a[pair.fa];
const auto& fb = faces_b[pair.fb];
// Translation to bring fb's plane onto fa's plane:
// project (fa.center - fb.center) onto fa.normal
core::Vector3D offset = fa.normal * fa.normal.dot(fa.center - fb.center);
// Also center-align in the face-plane directions
core::Vector3D lateral = (fa.center - fb.center) - offset;
offset += lateral;
core::Transform3D T = core::translate(offset);
node_b.local_transform = T * node_b.local_transform;
return T;
}
core::Transform3D apply_concentric(
const AssemblyNode& node_a, AssemblyNode& node_b)
{
if (!node_a.model.has_value() || !node_b.model.has_value())
return core::Transform3D::Identity();
core::AABB3D bb_a = node_a.model->bounds();
core::AABB3D bb_b = node_b.model->bounds();
core::Point3D ca = bb_a.center();
core::Point3D cb = bb_b.center();
// Align centers in the plane perpendicular to the estimated cylinder axis
core::Vector3D axis_a = estimate_cylinder_axis(bb_a);
core::Vector3D axis_b = estimate_cylinder_axis(bb_b);
// 1) Rotate axis_b to align with axis_a
core::Transform3D T_rot = core::Transform3D::Identity();
double dot_ax = axis_b.dot(axis_a);
if (dot_ax < 1.0 - 1e-9) {
core::Vector3D cross_ax = axis_b.cross(axis_a);
double cro = cross_ax.norm();
if (cro > 1e-9) {
T_rot = core::rotate(cross_ax / cro, std::atan2(cro, dot_ax));
} else {
// Antiparallel
core::Vector3D perp = (std::abs(axis_a.x()) < 0.9)
? core::Vector3D::UnitX() : core::Vector3D::UnitY();
core::Vector3D rot_axis = axis_a.cross(perp).normalized();
T_rot = core::rotate(rot_axis, M_PI);
}
}
// 2) Translate to align centers
core::Vector3D delta = ca - cb;
// Remove component along axis_a (keep axial position)
double axial = delta.dot(axis_a);
core::Vector3D radial_translation = delta - axis_a * axial;
core::Transform3D T_trans = core::translate(radial_translation);
node_b.local_transform = T_trans * T_rot * node_b.local_transform;
return T_trans * T_rot;
}
core::Transform3D apply_distance(
const AssemblyNode& node_a, AssemblyNode& node_b, double distance)
{
if (!node_a.model.has_value() || !node_b.model.has_value())
return core::Transform3D::Identity();
core::AABB3D bb_a = node_a.model->bounds();
core::AABB3D bb_b = node_b.model->bounds();
// Find closest face pair to determine the approach direction
auto pair = closest_face_pair(bb_a, bb_b);
auto faces_a = aabb_faces(bb_a);
core::Vector3D dir = faces_a[pair.fa].normal;
// Current signed gap along the approach direction
double current_gap = dir.dot(bb_b.center() - bb_a.center());
// Desired gap = closest-approach distance between extrema
core::Vector3D ext_a = bb_a.extent();
core::Vector3D ext_b = bb_b.extent();
double half_a = 0.5 * std::abs(ext_a.dot(dir));
double half_b = 0.5 * std::abs(ext_b.dot(dir));
double current_faces_gap = current_gap - half_a - half_b;
double correction = distance - current_faces_gap;
core::Transform3D T = core::translate(dir * correction);
node_b.local_transform = T * node_b.local_transform;
return T;
}
bool solve_constraints(
Assembly& assembly,
const std::vector<ConstraintEntry>& constraints,
int max_iterations,
double tolerance)
{
if (constraints.empty()) return true;
for (int iter = 0; iter < max_iterations; ++iter) {
// Apply each constraint in order
for (const auto& c : constraints) {
auto* na = child_at(assembly, c.node_a);
auto* nb = child_at(assembly, c.node_b);
if (!na || !nb) continue;
switch (c.type) {
case Constraint3D::Coincident:
apply_coincident(*na, *nb);
break;
case Constraint3D::Concentric:
apply_concentric(*na, *nb);
break;
case Constraint3D::Distance:
apply_distance(*na, *nb, c.value);
break;
case Constraint3D::Parallel:
apply_parallel(*na, *nb);
break;
case Constraint3D::Perpendicular:
apply_perpendicular(*na, *nb);
break;
case Constraint3D::Angle:
apply_angle(*na, *nb, c.value);
break;
case Constraint3D::Tangent:
// Tangent: same as coincident (faces touch)
apply_coincident(*na, *nb);
break;
}
}
// Check convergence
if (all_satisfied(assembly, constraints, tolerance))
return true;
}
// Final check: maybe we're close enough
return all_satisfied(assembly, constraints, tolerance);
}
} // namespace vde::brep