茂之钳
6bc9db663e
M3.1 — 大装配 LOD + 约束求解器 (Agent #0): - assembly_lod.h/.cpp: 3-level LOD (Full/Simplified/BBox), view-distance auto-switch - constraint_solver.h/.cpp: ConstraintGraph, DOF analysis, Newton-Raphson solver - 7 constraint types, over-constraint detection, incremental solve - 59 tests (22 LOD + 37 constraint) M3.2 — 完整 CAM 策略 (Agent #1): - cam_strategies.h/.cpp: roughing(Z-layer), finishing(parallel/spiral), drilling(G81/G83/G84) - Tool/ToolLibrary, PostProcessor (Fanuc/Siemens/Heidenhain) - material_removal_simulation with volume stats - 27 tests, 26/27 passing M3.3 — 直接建模 (Agent #2): - direct_modeling.h/.cpp: tweak_face, move_face, replace_face, push_pull, offset_face - Auto neighbor-face extension for watertightness - DirectModelingResult with diagnostics - 23 tests with validate() verification 12 files, ~5400 lines, 109 tests
395 lines
14 KiB
C++
395 lines
14 KiB
C++
#pragma once
|
||
/**
|
||
* @file constraint_solver.h
|
||
* @brief 3D 装配约束求解器 — 约束图 + Newton-Raphson + DOF 分析
|
||
*
|
||
* 为 3D 装配提供完整的约束求解系统：
|
||
* - ConstraintGraph: 约束图（节点=零件, 边=约束）
|
||
* - Newton-Raphson 迭代求解
|
||
* - DOF 分析（每个零件 6 DOF）
|
||
* - 过度约束检测（冗余约束识别 + 报告）
|
||
* - 联动求解（增量更新）
|
||
*
|
||
* @ingroup brep
|
||
*/
|
||
|
||
#include "vde/brep/assembly.h"
|
||
#include "vde/core/point.h"
|
||
#include "vde/core/transform.h"
|
||
#include "vde/core/aabb.h"
|
||
#include <vector>
|
||
#include <string>
|
||
#include <memory>
|
||
#include <unordered_map>
|
||
#include <unordered_set>
|
||
#include <functional>
|
||
#include <optional>
|
||
|
||
namespace vde::brep {
|
||
|
||
// ═══════════════════════════════════════════════════════════
|
||
// ConstraintType
|
||
// ═══════════════════════════════════════════════════════════
|
||
|
||
/// 3D 约束类型
|
||
enum class ConstraintType {
|
||
Coincident, ///< 面-面贴合（共面，法向相反）
|
||
Concentric, ///< 轴-轴对齐（同轴）
|
||
Tangent, ///< 面相切接触
|
||
Distance, ///< 固定面间距
|
||
Angle, ///< 固定面夹角（弧度）
|
||
Parallel, ///< 面法线平行
|
||
Perpendicular ///< 面法线垂直
|
||
};
|
||
|
||
/// 约束类型转字符串
|
||
[[nodiscard]] inline const char* constraint_type_name(ConstraintType t) {
|
||
switch (t) {
|
||
case ConstraintType::Coincident: return "Coincident";
|
||
case ConstraintType::Concentric: return "Concentric";
|
||
case ConstraintType::Tangent: return "Tangent";
|
||
case ConstraintType::Distance: return "Distance";
|
||
case ConstraintType::Angle: return "Angle";
|
||
case ConstraintType::Parallel: return "Parallel";
|
||
case ConstraintType::Perpendicular: return "Perpendicular";
|
||
}
|
||
return "Unknown";
|
||
}
|
||
|
||
/// 每个约束消除的自由度数（估算）
|
||
[[nodiscard]] inline int constraint_dof_elimination(ConstraintType t) {
|
||
switch (t) {
|
||
case ConstraintType::Coincident: return 3; // 1 平移 + 2 旋转
|
||
case ConstraintType::Concentric: return 4; // 2 平移 + 2 旋转
|
||
case ConstraintType::Tangent: return 1; // 1 平移
|
||
case ConstraintType::Distance: return 1; // 1 平移
|
||
case ConstraintType::Angle: return 1; // 1 旋转
|
||
case ConstraintType::Parallel: return 2; // 2 旋转
|
||
case ConstraintType::Perpendicular: return 2; // 2 旋转
|
||
}
|
||
return 0;
|
||
}
|
||
|
||
// ═══════════════════════════════════════════════════════════
|
||
// Constraint — 单个约束边
|
||
// ═══════════════════════════════════════════════════════════
|
||
|
||
/// 单个约束描述
|
||
struct Constraint {
|
||
int node_a; ///< 约束的零件 A 索引
|
||
int node_b; ///< 约束的零件 B 索引
|
||
ConstraintType type; ///< 约束类型
|
||
double value = 0.0; ///< 约束值(Distance=distance, Angle=radians)
|
||
double weight = 1.0; ///< 权重（用于过约束系统的加权最小二乘）
|
||
std::string name; ///< 约束名称（可选）
|
||
|
||
Constraint() = default;
|
||
Constraint(int a, int b, ConstraintType t, double v = 0.0, double w = 1.0)
|
||
: node_a(a), node_b(b), type(t), value(v), weight(w) {}
|
||
};
|
||
|
||
// ═══════════════════════════════════════════════════════════
|
||
// DOFInfo — 每个节点的自由度信息
|
||
// ═══════════════════════════════════════════════════════════
|
||
|
||
/// 单个零件的自由度分析结果
|
||
struct DOFInfo {
|
||
int total_dof = 6; ///< 起始 DOF（空间刚体：6）
|
||
int eliminated_dof = 0; ///< 已消除的 DOF
|
||
int remaining_dof = 6; ///< 剩余 DOF
|
||
bool is_fixed = false; ///< 是否完全约束
|
||
std::vector<int> constraining; ///< 约束此零件的约束索引列表
|
||
};
|
||
|
||
// ═══════════════════════════════════════════════════════════
|
||
// ConstraintGraph
|
||
// ═══════════════════════════════════════════════════════════
|
||
|
||
/**
|
||
* @brief 3D 约束图
|
||
*
|
||
* 节点 = 装配零件（AssemblyNode），边 = 约束。
|
||
* 提供图遍历、DOF 分析、过度约束检测。
|
||
*
|
||
* @ingroup brep
|
||
*/
|
||
class ConstraintGraph {
|
||
public:
|
||
/// 添加零件节点
|
||
/// @param name 零件名称
|
||
/// @param node 零件节点指针
|
||
/// @return 节点索引
|
||
int add_node(const std::string& name, AssemblyNode* node);
|
||
|
||
/// 添加约束边
|
||
/// @param c 约束描述
|
||
/// @return 约束索引
|
||
int add_constraint(const Constraint& c);
|
||
|
||
/// 删除约束
|
||
/// @param index 约束索引
|
||
void remove_constraint(int index);
|
||
|
||
/// 获取节点数
|
||
[[nodiscard]] int node_count() const { return static_cast<int>(nodes_.size()); }
|
||
|
||
/// 获取约束数
|
||
[[nodiscard]] int constraint_count() const { return static_cast<int>(constraints_.size()); }
|
||
|
||
/// 获取节点名
|
||
[[nodiscard]] const std::string& node_name(int idx) const;
|
||
|
||
/// 获取节点指针
|
||
[[nodiscard]] AssemblyNode* node_ptr(int idx) const;
|
||
|
||
/// 获取约束
|
||
[[nodiscard]] const Constraint& constraint(int idx) const;
|
||
|
||
/// 获取可修改约束引用（用于增量更新）
|
||
[[nodiscard]] Constraint& constraint_mut(int idx);
|
||
|
||
/// 获取某节点的所有相邻约束
|
||
[[nodiscard]] std::vector<int> node_constraints(int node_idx) const;
|
||
|
||
/// 获取所有节点索引
|
||
[[nodiscard]] const std::vector<int>& node_indices() const { return node_ids_; }
|
||
|
||
// ── DOF 分析 ──
|
||
|
||
/**
|
||
* @brief 执行 DOF 分析
|
||
*
|
||
* 对每个零件计算自由度状态：
|
||
* - 初始 6 DOF（3 平移 + 3 旋转）
|
||
* - 每个约束消除若干 DOF
|
||
* - 返回每个节点的 DOFInfo
|
||
*
|
||
* @return 节点索引 → DOF 信息
|
||
*/
|
||
[[nodiscard]] std::vector<DOFInfo> analyze_dof() const;
|
||
|
||
/**
|
||
* @brief 计算单个节点的剩余 DOF
|
||
* @param node_idx 节点索引
|
||
* @return 剩余自由度数
|
||
*/
|
||
[[nodiscard]] int remaining_dof(int node_idx) const;
|
||
|
||
/**
|
||
* @brief 检查图是否完全约束（所有节点 fixed）
|
||
* @return true 如果所有节点 DOF=0
|
||
*/
|
||
[[nodiscard]] bool is_fully_constrained() const;
|
||
|
||
// ── 过度约束检测 ──
|
||
|
||
/**
|
||
* @brief 过度约束检测结果
|
||
*/
|
||
struct OverConstraintInfo {
|
||
int node_index; ///< 哪个节点
|
||
int eliminated_dof; ///< 已消除 DOF
|
||
bool over_constrained; ///< 是否过度约束（eliminated > 6）
|
||
std::vector<int> redundant; ///< 冗余约束索引列表
|
||
std::string message; ///< 人类可读的消息
|
||
};
|
||
|
||
/**
|
||
* @brief 检测过度约束
|
||
*
|
||
* 对每个节点检查：已消除 DOF > 6 即为过度约束。
|
||
* 识别并报告冗余约束。
|
||
*
|
||
* @return 过度约束节点列表
|
||
*/
|
||
[[nodiscard]] std::vector<OverConstraintInfo> detect_over_constraints() const;
|
||
|
||
// ── 图结构查询 ──
|
||
|
||
/// 获取所有约束
|
||
[[nodiscard]] const std::vector<Constraint>& constraints() const { return constraints_; }
|
||
|
||
/// 检查两个节点之间是否已存在约束
|
||
[[nodiscard]] bool has_constraint_between(int a, int b) const;
|
||
|
||
/// 获取两个节点之间的所有约束
|
||
[[nodiscard]] std::vector<int> constraints_between(int a, int b) const;
|
||
|
||
/// 清空图
|
||
void clear();
|
||
|
||
private:
|
||
struct NodeInfo {
|
||
std::string name;
|
||
AssemblyNode* ptr = nullptr;
|
||
};
|
||
|
||
std::vector<NodeInfo> nodes_;
|
||
std::vector<int> node_ids_; // 压缩的索引映射
|
||
std::vector<Constraint> constraints_;
|
||
std::vector<bool> constraint_active_; // 约束是否激活（未删除）
|
||
|
||
/// 邻接表：node_idx → [constraint_idx...]
|
||
std::vector<std::vector<int>> adjacency_;
|
||
};
|
||
|
||
// ═══════════════════════════════════════════════════════════
|
||
// Newton-Raphson 求解器
|
||
// ═══════════════════════════════════════════════════════════
|
||
|
||
/**
|
||
* @brief Newton-Raphson 求解器配置
|
||
*/
|
||
struct NRSolverConfig {
|
||
int max_iterations = 100; ///< 最大迭代次数
|
||
double tolerance = 1e-6; ///< 收敛容差
|
||
double step_size = 0.5; ///< 步长衰减因子（0~1，防震荡）
|
||
double damping = 0.5; ///< 阻尼因子
|
||
bool check_over_constrained = true; ///< 求解前检测过度约束
|
||
};
|
||
|
||
/**
|
||
* @brief 求解器状态
|
||
*/
|
||
struct SolveResult {
|
||
bool converged = false; ///< 是否收敛
|
||
int iterations = 0; ///< 实际迭代次数
|
||
double final_error = 0.0; ///< 最终残差 L2 范数
|
||
std::vector<double> error_history; ///< 每步误差历史
|
||
std::string message; ///< 人类可读的消息
|
||
};
|
||
|
||
/**
|
||
* @brief Newton-Raphson 约束求解器
|
||
*
|
||
* 将约束表示为方程系统 f(x)=0，使用 Newton-Raphson 迭代求解。
|
||
*
|
||
* 状态向量 x = [t1_x, t1_y, t1_z, r1_x, r1_y, r1_z, t2_..., ...]
|
||
* 每个零件有 6 个参数：3 平移 + 3 旋转（轴-角表示的一部分，用增量旋转）。
|
||
*
|
||
* 每一步：
|
||
* 1. 评估约束函数 f(x) → 残差向量
|
||
* 2. 数值计算 Jacobian J = ∂f/∂x
|
||
* 3. 求解 J·Δx = -f(x)
|
||
* 4. 更新 x ← x + α·Δx（α 为步长）
|
||
*
|
||
* @ingroup brep
|
||
*/
|
||
class NewtonRaphsonSolver {
|
||
public:
|
||
/**
|
||
* @brief 求解约束图
|
||
*
|
||
* @param graph 约束图
|
||
* @param assembly 装配体（将被修改以满足约束）
|
||
* @param config 求解器配置
|
||
* @return SolveResult 求解结果
|
||
*/
|
||
[[nodiscard]] SolveResult solve(
|
||
ConstraintGraph& graph,
|
||
Assembly& assembly,
|
||
const NRSolverConfig& config = NRSolverConfig{});
|
||
|
||
/**
|
||
* @brief 增量求解：修改单个约束值后更新
|
||
*
|
||
* 在已有求解结果基础上，仅修改一个约束值，
|
||
* 从当前状态出发重新求解（热启动）。
|
||
*
|
||
* @param graph 约束图
|
||
* @param assembly 装配体
|
||
* @param constraint_idx 被修改的约束索引
|
||
* @param new_value 新的约束值
|
||
* @param config 求解器配置
|
||
* @return SolveResult
|
||
*/
|
||
[[nodiscard]] SolveResult incremental_solve(
|
||
ConstraintGraph& graph,
|
||
Assembly& assembly,
|
||
int constraint_idx,
|
||
double new_value,
|
||
const NRSolverConfig& config = NRSolverConfig{});
|
||
|
||
/// 获取最后一次求解的状态向量
|
||
[[nodiscard]] const std::vector<double>& last_state() const { return state_; }
|
||
|
||
private:
|
||
int n_parts_ = 0; ///< 零件数
|
||
std::vector<double> state_; ///< 状态向量 [tx,ty,tz,rx,ry,rz]*n
|
||
std::vector<double> last_error_; ///< 最近误差向量
|
||
|
||
/// 从 assembly 读取变换到状态向量
|
||
void read_state(const ConstraintGraph& graph, const Assembly& assembly);
|
||
|
||
/// 将状态向量写回 assembly
|
||
void write_state(const ConstraintGraph& graph, Assembly& assembly) const;
|
||
|
||
/// 评估所有约束函数，填充残差向量 f
|
||
/// @return 残差的 L2 范数
|
||
double evaluate_constraints(
|
||
const ConstraintGraph& graph,
|
||
const Assembly& assembly,
|
||
std::vector<double>& residual) const;
|
||
|
||
/// 数值计算 Jacobian（中心差分）
|
||
void compute_jacobian(
|
||
const ConstraintGraph& graph,
|
||
Assembly& assembly,
|
||
const std::vector<double>& residual,
|
||
Eigen::MatrixXd& J);
|
||
|
||
/// 单个约束的评估函数
|
||
double evaluate_single_constraint(
|
||
const Constraint& c,
|
||
const AssemblyNode& na,
|
||
const AssemblyNode& nb) const;
|
||
|
||
/// 求解线性最小二乘问题 J·Δx = -f
|
||
Eigen::VectorXd solve_linear_step(
|
||
const Eigen::MatrixXd& J,
|
||
const Eigen::VectorXd& f) const;
|
||
};
|
||
|
||
// ═══════════════════════════════════════════════════════════
|
||
// 向后兼容 API（保留旧接口）
|
||
// ═══════════════════════════════════════════════════════════
|
||
|
||
/// @deprecated 使用 ConstraintType 代替
|
||
using Constraint3D = ConstraintType;
|
||
|
||
/// @deprecated 使用 Constraint 代替
|
||
struct ConstraintEntry : public Constraint {
|
||
using Constraint::Constraint;
|
||
};
|
||
|
||
/**
|
||
* @brief 简化版求解器（向后兼容）
|
||
*
|
||
* 使用迭代松弛法求解约束系统。
|
||
*
|
||
* @param assembly 目标装配体
|
||
* @param constraints 约束列表（node_a, node_b 为 root.children 中的直接索引）
|
||
* @param max_iterations 最大迭代次数
|
||
* @param tolerance 收敛容差
|
||
* @return true 如果所有约束在容差内满足
|
||
*/
|
||
[[nodiscard]] bool solve_constraints(
|
||
Assembly& assembly,
|
||
const std::vector<ConstraintEntry>& constraints,
|
||
int max_iterations = 100,
|
||
double tolerance = 1e-6);
|
||
|
||
/// 应用贴合约束
|
||
[[nodiscard]] core::Transform3D apply_coincident(
|
||
const AssemblyNode& node_a, AssemblyNode& node_b);
|
||
|
||
/// 应用同轴约束
|
||
[[nodiscard]] core::Transform3D apply_concentric(
|
||
const AssemblyNode& node_a, AssemblyNode& node_b);
|
||
|
||
/// 应用距离约束
|
||
[[nodiscard]] core::Transform3D apply_distance(
|
||
const AssemblyNode& node_a, AssemblyNode& node_b, double distance);
|
||
|
||
} // namespace vde::brep
|