feat: 3D constraint solver for assembly (coincident/concentric/distance)
This commit is contained in:
茂之钳
@@ -0,0 +1,76 @@
|
||||
#pragma once
|
||||
/**
|
||||
* @file constraint_solver.h
|
||||
* @brief 3D constraint solver for assembly
|
||||
*
|
||||
* Iterative relaxation solver for spatial constraints between assembly nodes.
|
||||
* Supports coincident, concentric, distance, angle, parallel, perpendicular,
|
||||
* and tangent constraints over 3D bounding box proxies.
|
||||
*
|
||||
* @ingroup brep
|
||||
*/
|
||||
#include "vde/brep/assembly.h"
|
||||
#include "vde/core/point.h"
|
||||
#include <vector>
|
||||
#include <functional>
|
||||
|
||||
namespace vde::brep {
|
||||
|
||||
/// Constraint type for 3D assembly solving
|
||||
enum class Constraint3D {
|
||||
Coincident, ///< Face-face alignment (coplanar, opposite normals)
|
||||
Concentric, ///< Axis-axis alignment
|
||||
Distance, ///< Fixed distance between faces
|
||||
Angle, ///< Fixed angle between faces
|
||||
Parallel, ///< Face normals parallel
|
||||
Perpendicular, ///< Face normals perpendicular
|
||||
Tangent ///< Face tangent contact
|
||||
};
|
||||
|
||||
/// A single constraint between two assembly nodes
|
||||
struct ConstraintEntry {
|
||||
int node_a; ///< Index into a flattened node list (or 0-based child index)
|
||||
int node_b; ///< Index of the node that moves to satisfy constraint
|
||||
Constraint3D type;
|
||||
double value = 0.0; ///< Distance or angle value (radians for Angle)
|
||||
};
|
||||
|
||||
/// Solve a system of 3D constraints on an assembly using iterative relaxation.
|
||||
///
|
||||
/// Repeatedly adjusts transforms for each constraint in round-robin order
|
||||
/// until all constraints are satisfied within tolerance or max_iterations
|
||||
/// is exhausted.
|
||||
///
|
||||
/// @param assembly Target assembly (root children are the constrained nodes)
|
||||
/// @param constraints Ordered list of constraints to satisfy
|
||||
/// @param max_iterations Maximum number of relaxation passes (default 100)
|
||||
/// @param tolerance Convergence tolerance (default 1e-6)
|
||||
/// @return true if all constraints satisfied within tolerance
|
||||
[[nodiscard]] bool solve_constraints(
|
||||
Assembly& assembly,
|
||||
const std::vector<ConstraintEntry>& constraints,
|
||||
int max_iterations = 100,
|
||||
double tolerance = 1e-6);
|
||||
|
||||
/// Apply a single coincident (mate) constraint: align the closest opposing
|
||||
/// faces of node_a and node_b so they are coplanar.
|
||||
///
|
||||
/// @return The correction transform that was applied to node_b
|
||||
[[nodiscard]] core::Transform3D apply_coincident(
|
||||
const AssemblyNode& node_a, AssemblyNode& node_b);
|
||||
|
||||
/// Apply a single concentric constraint: align the bounding-box-estimated
|
||||
/// cylinder axes of node_a and node_b.
|
||||
///
|
||||
/// @return The correction transform that was applied to node_b
|
||||
[[nodiscard]] core::Transform3D apply_concentric(
|
||||
const AssemblyNode& node_a, AssemblyNode& node_b);
|
||||
|
||||
/// Apply a distance constraint: set the closest-approach gap between
|
||||
/// the bounding boxes of node_a and node_b to @p distance.
|
||||
///
|
||||
/// @return The correction transform that was applied to node_b
|
||||
[[nodiscard]] core::Transform3D apply_distance(
|
||||
const AssemblyNode& node_a, AssemblyNode& node_b, double distance);
|
||||
|
||||
} // namespace vde::brep
|
||||
@@ -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
|
||||
|
||||
@@ -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
|
||||
@@ -14,3 +14,4 @@ add_vde_test(test_trimmed_surface)
|
||||
add_vde_test(test_brep_heal)
|
||||
add_vde_test(test_brep_drawing)
|
||||
add_vde_test(test_assembly_instance)
|
||||
add_vde_test(test_constraint_solver)
|
||||
|
||||
@@ -0,0 +1,397 @@
|
||||
#include <gtest/gtest.h>
|
||||
#include "vde/brep/constraint_solver.h"
|
||||
#include "vde/brep/modeling.h"
|
||||
#include "vde/core/transform.h"
|
||||
#include "vde/core/aabb.h"
|
||||
#include <cmath>
|
||||
#include <vector>
|
||||
|
||||
using namespace vde::brep;
|
||||
using namespace vde::core;
|
||||
|
||||
// ════════════════════════════════════════════════════════════════
|
||||
// Helpers
|
||||
// ════════════════════════════════════════════════════════════════
|
||||
|
||||
namespace {
|
||||
|
||||
/// World-space AABB of a node (transforming local model bounds).
|
||||
AABB3D world_bounds(const AssemblyNode* node) {
|
||||
AABB3D bb_world;
|
||||
if (!node->model.has_value()) return bb_world;
|
||||
AABB3D bb_local = node->model->bounds();
|
||||
Point3D corners[8] = {
|
||||
bb_local.min(),
|
||||
Point3D(bb_local.max().x(), bb_local.min().y(), bb_local.min().z()),
|
||||
Point3D(bb_local.max().x(), bb_local.max().y(), bb_local.min().z()),
|
||||
Point3D(bb_local.min().x(), bb_local.max().y(), bb_local.min().z()),
|
||||
Point3D(bb_local.min().x(), bb_local.min().y(), bb_local.max().z()),
|
||||
Point3D(bb_local.max().x(), bb_local.min().y(), bb_local.max().z()),
|
||||
bb_local.max(),
|
||||
Point3D(bb_local.min().x(), bb_local.max().y(), bb_local.max().z()),
|
||||
};
|
||||
for (const auto& c : corners) {
|
||||
bb_world.expand(node->local_transform * c);
|
||||
}
|
||||
return bb_world;
|
||||
}
|
||||
|
||||
} // anonymous namespace
|
||||
|
||||
// ════════════════════════════════════════════════════════════════
|
||||
// apply_coincident
|
||||
// ════════════════════════════════════════════════════════════════
|
||||
|
||||
TEST(ConstraintSolverTest, CoincidentAlignsFacePlanes) {
|
||||
Assembly assy("test_coincident");
|
||||
auto* box_a = assy.root.add_part("box_a", make_box(2, 2, 2));
|
||||
auto* box_b = assy.root.add_part("box_b", make_box(2, 2, 2),
|
||||
translate(0, 0, 10));
|
||||
|
||||
apply_coincident(*box_a, *box_b);
|
||||
|
||||
AABB3D bb_a = world_bounds(box_a);
|
||||
AABB3D bb_b = world_bounds(box_b);
|
||||
|
||||
// After coincident, box_b bottom should sit on box_a top (gap ~0)
|
||||
double gap = bb_b.min().z() - bb_a.max().z();
|
||||
EXPECT_NEAR(gap, 0.0, 1e-4);
|
||||
}
|
||||
|
||||
TEST(ConstraintSolverTest, CoincidentPreservesLateralAlignment) {
|
||||
Assembly assy("test_coincident2");
|
||||
auto* box_a = assy.root.add_part("box_a", make_box(4, 4, 2));
|
||||
auto* box_b = assy.root.add_part("box_b", make_box(2, 2, 4),
|
||||
translate(0, 0, 8));
|
||||
|
||||
apply_coincident(*box_a, *box_b);
|
||||
|
||||
AABB3D bb_a = world_bounds(box_a);
|
||||
AABB3D bb_b = world_bounds(box_b);
|
||||
|
||||
// Centers should align in X and Y
|
||||
EXPECT_NEAR(bb_b.center().x(), bb_a.center().x(), 1e-4);
|
||||
EXPECT_NEAR(bb_b.center().y(), bb_a.center().y(), 1e-4);
|
||||
EXPECT_NEAR(bb_b.min().z(), bb_a.max().z(), 1e-4);
|
||||
}
|
||||
|
||||
TEST(ConstraintSolverTest, CoincidentNoModelReturnsIdentity) {
|
||||
Assembly assy("test_no_model");
|
||||
auto* node_a = assy.root.add_subassembly("empty_a");
|
||||
auto* node_b = assy.root.add_subassembly("empty_b");
|
||||
|
||||
auto T = apply_coincident(*node_a, *node_b);
|
||||
EXPECT_TRUE(T.isApprox(Transform3D::Identity(), 1e-12));
|
||||
}
|
||||
|
||||
// ════════════════════════════════════════════════════════════════
|
||||
// apply_concentric
|
||||
// ════════════════════════════════════════════════════════════════
|
||||
|
||||
TEST(ConstraintSolverTest, ConcentricAlignsAxes) {
|
||||
Assembly assy("test_concentric");
|
||||
auto* cyl_a = assy.root.add_part("cyl_a", make_cylinder(1.0, 6.0));
|
||||
auto* cyl_b = assy.root.add_part("cyl_b", make_cylinder(0.5, 4.0),
|
||||
translate(5, 5, 0));
|
||||
|
||||
apply_concentric(*cyl_a, *cyl_b);
|
||||
|
||||
AABB3D bb_a = world_bounds(cyl_a);
|
||||
AABB3D bb_b = world_bounds(cyl_b);
|
||||
|
||||
// Centers should align in X and Y after concentric constraint
|
||||
EXPECT_NEAR(bb_b.center().x(), bb_a.center().x(), 1e-4);
|
||||
EXPECT_NEAR(bb_b.center().y(), bb_a.center().y(), 1e-4);
|
||||
}
|
||||
|
||||
TEST(ConstraintSolverTest, ConcentricPreservesZPosition) {
|
||||
Assembly assy("test_concentric_z");
|
||||
auto* cyl_a = assy.root.add_part("cyl_a", make_cylinder(1.0, 6.0));
|
||||
auto* cyl_b = assy.root.add_part("cyl_b", make_cylinder(0.5, 4.0),
|
||||
translate(5, 5, 3.0));
|
||||
|
||||
double z_before = world_bounds(cyl_b).center().z();
|
||||
apply_concentric(*cyl_a, *cyl_b);
|
||||
double z_after = world_bounds(cyl_b).center().z();
|
||||
|
||||
// Z should be preserved (concentric only affects radial directions)
|
||||
EXPECT_NEAR(z_after, z_before, 1e-4);
|
||||
}
|
||||
|
||||
TEST(ConstraintSolverTest, ConcentricDifferentOrientationCylinders) {
|
||||
// TODO: This test requires rotated input shapes; for now we test
|
||||
// the axis-invariant case.
|
||||
Assembly assy("test_concentric_orient");
|
||||
auto* cyl_a = assy.root.add_part("cyl_a", make_cylinder(1.0, 6.0));
|
||||
auto* cyl_b = assy.root.add_part("cyl_b", make_cylinder(0.5, 4.0),
|
||||
translate(3, 0, 0));
|
||||
|
||||
apply_concentric(*cyl_a, *cyl_b);
|
||||
|
||||
AABB3D bb_a = world_bounds(cyl_a);
|
||||
AABB3D bb_b = world_bounds(cyl_b);
|
||||
|
||||
EXPECT_NEAR(bb_b.center().x(), bb_a.center().x(), 1e-4);
|
||||
EXPECT_NEAR(bb_b.center().y(), bb_a.center().y(), 1e-4);
|
||||
}
|
||||
|
||||
// ════════════════════════════════════════════════════════════════
|
||||
// apply_distance
|
||||
// ════════════════════════════════════════════════════════════════
|
||||
|
||||
TEST(ConstraintSolverTest, DistanceProducesCorrectGap) {
|
||||
Assembly assy("test_distance");
|
||||
auto* box_a = assy.root.add_part("box_a", make_box(2, 2, 2));
|
||||
auto* box_b = assy.root.add_part("box_b", make_box(2, 2, 2),
|
||||
translate(0, 0, 10));
|
||||
|
||||
apply_distance(*box_a, *box_b, 5.0);
|
||||
|
||||
AABB3D bb_a = world_bounds(box_a);
|
||||
AABB3D bb_b = world_bounds(box_b);
|
||||
|
||||
double gap = bb_b.min().z() - bb_a.max().z();
|
||||
EXPECT_NEAR(gap, 5.0, 1e-4);
|
||||
}
|
||||
|
||||
TEST(ConstraintSolverTest, DistanceZeroIsCoincident) {
|
||||
Assembly assy("test_distance_zero");
|
||||
auto* box_a = assy.root.add_part("box_a", make_box(2, 2, 2));
|
||||
auto* box_b = assy.root.add_part("box_b", make_box(2, 2, 2),
|
||||
translate(0, 0, 8));
|
||||
|
||||
apply_distance(*box_a, *box_b, 0.0);
|
||||
|
||||
AABB3D bb_a = world_bounds(box_a);
|
||||
AABB3D bb_b = world_bounds(box_b);
|
||||
|
||||
double gap = bb_b.min().z() - bb_a.max().z();
|
||||
EXPECT_NEAR(gap, 0.0, 1e-4);
|
||||
}
|
||||
|
||||
TEST(ConstraintSolverTest, DistanceNegativePushesAway) {
|
||||
// Negative distance means overlap
|
||||
Assembly assy("test_distance_neg");
|
||||
auto* box_a = assy.root.add_part("box_a", make_box(2, 2, 2));
|
||||
auto* box_b = assy.root.add_part("box_b", make_box(2, 2, 2),
|
||||
translate(0, 0, 10));
|
||||
|
||||
apply_distance(*box_a, *box_b, -1.0);
|
||||
|
||||
AABB3D bb_a = world_bounds(box_a);
|
||||
AABB3D bb_b = world_bounds(box_b);
|
||||
|
||||
double gap = bb_b.min().z() - bb_a.max().z();
|
||||
EXPECT_NEAR(gap, -1.0, 1e-4);
|
||||
}
|
||||
|
||||
// ════════════════════════════════════════════════════════════════
|
||||
// solve_constraints
|
||||
// ════════════════════════════════════════════════════════════════
|
||||
|
||||
TEST(ConstraintSolverTest, SolveSingleConstraint) {
|
||||
Assembly assy("test_solve_single");
|
||||
assy.root.add_part("box_a", make_box(2, 2, 2));
|
||||
assy.root.add_part("box_b", make_box(2, 2, 2), translate(5, 3, 10));
|
||||
|
||||
std::vector<ConstraintEntry> constraints = {
|
||||
{0, 1, Constraint3D::Coincident}
|
||||
};
|
||||
|
||||
EXPECT_TRUE(solve_constraints(assy, constraints, 50, 1e-4));
|
||||
|
||||
auto* box_a = assy.root.children[0].get();
|
||||
auto* box_b = assy.root.children[1].get();
|
||||
AABB3D bb_a = world_bounds(box_a);
|
||||
AABB3D bb_b = world_bounds(box_b);
|
||||
double gap = bb_b.min().z() - bb_a.max().z();
|
||||
EXPECT_NEAR(gap, 0.0, 1e-3);
|
||||
}
|
||||
|
||||
TEST(ConstraintSolverTest, SolveMultipleConstraints) {
|
||||
Assembly assy("test_solve_multi");
|
||||
assy.root.add_part("box_a", make_box(4, 4, 2));
|
||||
assy.root.add_part("cyl_a", make_cylinder(1.0, 6.0), translate(5, 0, 0));
|
||||
|
||||
std::vector<ConstraintEntry> constraints = {
|
||||
{0, 1, Constraint3D::Coincident}, // faces coplanar
|
||||
{0, 1, Constraint3D::Concentric} // axes aligned
|
||||
};
|
||||
|
||||
EXPECT_TRUE(solve_constraints(assy, constraints, 100, 1e-3));
|
||||
|
||||
auto* box_a = assy.root.children[0].get();
|
||||
auto* cyl_a = assy.root.children[1].get();
|
||||
AABB3D bb_a = world_bounds(box_a);
|
||||
AABB3D bb_b = world_bounds(cyl_a);
|
||||
|
||||
// Centers aligned in X and Y (concentric)
|
||||
EXPECT_NEAR(bb_b.center().x(), bb_a.center().x(), 1e-3);
|
||||
EXPECT_NEAR(bb_b.center().y(), bb_a.center().y(), 1e-3);
|
||||
}
|
||||
|
||||
TEST(ConstraintSolverTest, SolveWithDistance) {
|
||||
Assembly assy("test_solve_dist");
|
||||
assy.root.add_part("box_a", make_box(2, 2, 2));
|
||||
assy.root.add_part("box_b", make_box(2, 2, 2), translate(0, 0, 10));
|
||||
|
||||
std::vector<ConstraintEntry> constraints = {
|
||||
{0, 1, Constraint3D::Distance, 3.0}
|
||||
};
|
||||
|
||||
EXPECT_TRUE(solve_constraints(assy, constraints, 50, 1e-4));
|
||||
|
||||
auto* box_a = assy.root.children[0].get();
|
||||
auto* box_b = assy.root.children[1].get();
|
||||
AABB3D bb_a = world_bounds(box_a);
|
||||
AABB3D bb_b = world_bounds(box_b);
|
||||
double gap = bb_b.min().z() - bb_a.max().z();
|
||||
EXPECT_NEAR(gap, 3.0, 1e-3);
|
||||
}
|
||||
|
||||
TEST(ConstraintSolverTest, SolveEmptyConstraintsReturnsTrue) {
|
||||
Assembly assy("test_solve_empty");
|
||||
assy.root.add_part("box_a", make_box(2, 2, 2));
|
||||
|
||||
std::vector<ConstraintEntry> constraints;
|
||||
EXPECT_TRUE(solve_constraints(assy, constraints));
|
||||
}
|
||||
|
||||
TEST(ConstraintSolverTest, SolveConvergenceCheck) {
|
||||
Assembly assy("test_solve_conv");
|
||||
assy.root.add_part("box_a", make_box(2, 2, 2));
|
||||
assy.root.add_part("box_b", make_box(2, 2, 2), translate(10, 10, 10));
|
||||
|
||||
std::vector<ConstraintEntry> constraints = {
|
||||
{0, 1, Constraint3D::Coincident}
|
||||
};
|
||||
|
||||
// Should converge well within 100 iterations
|
||||
bool converged = solve_constraints(assy, constraints, 100, 1e-6);
|
||||
EXPECT_TRUE(converged);
|
||||
}
|
||||
|
||||
TEST(ConstraintSolverTest, SolveConflictingConstraintsHandledGracefully) {
|
||||
// Conflicting: box_b must be both coincident and at distance 10 from box_a
|
||||
Assembly assy("test_conflict");
|
||||
assy.root.add_part("box_a", make_box(2, 2, 2));
|
||||
assy.root.add_part("box_b", make_box(2, 2, 2), translate(0, 0, 10));
|
||||
|
||||
std::vector<ConstraintEntry> constraints = {
|
||||
{0, 1, Constraint3D::Coincident},
|
||||
{0, 1, Constraint3D::Distance, 10.0}
|
||||
};
|
||||
|
||||
// Should NOT crash — just returns false (didn't converge)
|
||||
bool converged = solve_constraints(assy, constraints, 50, 1e-6);
|
||||
// Conflicting constraints won't converge; this is expected
|
||||
EXPECT_FALSE(converged);
|
||||
}
|
||||
|
||||
// ════════════════════════════════════════════════════════════════
|
||||
// Constraint3D type coverage — Parallel, Perpendicular, Angle
|
||||
// ════════════════════════════════════════════════════════════════
|
||||
|
||||
TEST(ConstraintSolverTest, ParallelConstraintAppliesTransform) {
|
||||
Assembly assy("test_parallel");
|
||||
assy.root.add_part("box_a", make_box(2, 2, 8)); // tall in Z
|
||||
assy.root.add_part("box_b", make_box(2, 2, 8), // tall in Z
|
||||
translate(0, 5, 0));
|
||||
|
||||
std::vector<ConstraintEntry> constraints = {
|
||||
{0, 1, Constraint3D::Parallel}
|
||||
};
|
||||
|
||||
EXPECT_TRUE(solve_constraints(assy, constraints, 50, 1e-6));
|
||||
// Parallel constraint should not crash; it applies rotation to align axes
|
||||
SUCCEED();
|
||||
}
|
||||
|
||||
TEST(ConstraintSolverTest, PerpendicularConstraintAppliesTransform) {
|
||||
Assembly assy("test_perp");
|
||||
assy.root.add_part("box_a", make_box(2, 2, 8)); // tall in Z
|
||||
assy.root.add_part("box_b", make_box(2, 2, 8),
|
||||
translate(0, 5, 0));
|
||||
|
||||
std::vector<ConstraintEntry> constraints = {
|
||||
{0, 1, Constraint3D::Perpendicular}
|
||||
};
|
||||
|
||||
EXPECT_TRUE(solve_constraints(assy, constraints, 50, 1e-6));
|
||||
|
||||
// Verify transform is non-identity
|
||||
auto* box_b = assy.root.children[1].get();
|
||||
EXPECT_FALSE(box_b->local_transform.isApprox(Transform3D::Identity(), 1e-3));
|
||||
}
|
||||
|
||||
TEST(ConstraintSolverTest, AngleConstraintAppliesRotation) {
|
||||
Assembly assy("test_angle");
|
||||
assy.root.add_part("box_a", make_box(2, 2, 2));
|
||||
assy.root.add_part("box_b", make_box(2, 2, 8));
|
||||
|
||||
std::vector<ConstraintEntry> constraints = {
|
||||
{0, 1, Constraint3D::Angle, M_PI / 2.0}
|
||||
};
|
||||
|
||||
EXPECT_TRUE(solve_constraints(assy, constraints, 50, 1e-6));
|
||||
|
||||
auto* box_b = assy.root.children[1].get();
|
||||
EXPECT_FALSE(box_b->local_transform.isApprox(Transform3D::Identity(), 1e-6));
|
||||
}
|
||||
|
||||
TEST(ConstraintSolverTest, TangentConstraintApplies) {
|
||||
Assembly assy("test_tangent");
|
||||
assy.root.add_part("box_a", make_box(2, 2, 2));
|
||||
assy.root.add_part("box_b", make_box(2, 2, 2),
|
||||
translate(0, 0, 10));
|
||||
|
||||
std::vector<ConstraintEntry> constraints = {
|
||||
{0, 1, Constraint3D::Tangent}
|
||||
};
|
||||
|
||||
EXPECT_TRUE(solve_constraints(assy, constraints, 50, 1e-4));
|
||||
|
||||
AABB3D bb_a = world_bounds(assy.root.children[0].get());
|
||||
AABB3D bb_b = world_bounds(assy.root.children[1].get());
|
||||
double gap = bb_b.min().z() - bb_a.max().z();
|
||||
EXPECT_NEAR(gap, 0.0, 1e-3);
|
||||
}
|
||||
|
||||
// ════════════════════════════════════════════════════════════════
|
||||
// Three-node assembly scenario
|
||||
// ════════════════════════════════════════════════════════════════
|
||||
|
||||
TEST(ConstraintSolverTest, ThreeNodeSolve) {
|
||||
Assembly assy("test_three_node");
|
||||
assy.root.add_part("base", make_box(4, 4, 1)); // node 0
|
||||
assy.root.add_part("mid", make_box(2, 2, 2),
|
||||
translate(1, 1, 10)); // node 1
|
||||
assy.root.add_part("top", make_box(1, 1, 3),
|
||||
translate(1, 1, 15)); // node 2
|
||||
|
||||
std::vector<ConstraintEntry> constraints = {
|
||||
{0, 1, Constraint3D::Coincident}, // mid sits on base
|
||||
{1, 2, Constraint3D::Coincident}, // top sits on mid
|
||||
{0, 1, Constraint3D::Concentric}, // centers aligned
|
||||
{1, 2, Constraint3D::Concentric} // centers aligned
|
||||
};
|
||||
|
||||
EXPECT_TRUE(solve_constraints(assy, constraints, 200, 1e-3));
|
||||
|
||||
// All three should now be stacked and centered
|
||||
AABB3D bb0 = world_bounds(assy.root.children[0].get());
|
||||
AABB3D bb1 = world_bounds(assy.root.children[1].get());
|
||||
AABB3D bb2 = world_bounds(assy.root.children[2].get());
|
||||
|
||||
// Centers aligned in X and Y
|
||||
EXPECT_NEAR(bb1.center().x(), bb0.center().x(), 1e-3);
|
||||
EXPECT_NEAR(bb1.center().y(), bb0.center().y(), 1e-3);
|
||||
EXPECT_NEAR(bb2.center().x(), bb0.center().x(), 1e-3);
|
||||
EXPECT_NEAR(bb2.center().y(), bb0.center().y(), 1e-3);
|
||||
|
||||
// Stacked: bb1 sits on bb0, bb2 sits on bb1
|
||||
double gap_01 = bb1.min().z() - bb0.max().z();
|
||||
double gap_12 = bb2.min().z() - bb1.max().z();
|
||||
EXPECT_NEAR(gap_01, 0.0, 1e-3);
|
||||
EXPECT_NEAR(gap_12, 0.0, 1e-3);
|
||||
}
|
||||
Reference in New Issue
Block a user