茂之钳
252 lines
7.7 KiB
C++
252 lines
7.7 KiB
C++
#pragma once
|
|
#include "vde/core/point.h"
|
|
#include <vector>
|
|
#include <string>
|
|
#include <unordered_map>
|
|
|
|
namespace vde::sketch {
|
|
using core::Point2D;
|
|
using core::Vector2D;
|
|
|
|
/**
|
|
* @brief 草图点元素
|
|
* @ingroup sketch
|
|
*/
|
|
struct SketchPoint {
|
|
int id; ///< 唯一标识
|
|
Point2D pos; ///< 2D 坐标
|
|
bool fixed = false; ///< 是否固定（排除自由度）
|
|
};
|
|
|
|
/**
|
|
* @brief 草图线段元素
|
|
* @ingroup sketch
|
|
*/
|
|
struct SketchLine {
|
|
int id; ///< 唯一标识
|
|
int p0, p1; ///< 起止点 ID
|
|
};
|
|
|
|
/**
|
|
* @brief 草图圆元素
|
|
* @ingroup sketch
|
|
*/
|
|
struct SketchCircle {
|
|
int id; ///< 唯一标识
|
|
int center; ///< 圆心点 ID
|
|
double radius; ///< 半径
|
|
};
|
|
|
|
/**
|
|
* @brief 约束类型枚举
|
|
*
|
|
* 每个约束类型对应一个残差方程，方程数见注释：
|
|
* - 距离类约束：1 个方程
|
|
* - 位置/重合类：2 个方程（x + y）
|
|
*
|
|
* @ingroup sketch
|
|
*/
|
|
enum class ConstraintType {
|
|
Horizontal, ///< 水平线: y1 - y0 = 0 [1 eq]
|
|
Vertical, ///< 垂直线: x1 - x0 = 0 [1 eq]
|
|
Parallel, ///< 两线平行: cross(d0, d1) = 0 [1 eq]
|
|
Perpendicular, ///< 两线垂直: dot(d0, d1) = 0 [1 eq]
|
|
Coincident, ///< 两点重合: |p1-p0| = 0 [2 eqs: Δx=0, Δy=0]
|
|
EqualLength, ///< 等长度: |d0| - |d1| = 0 [1 eq]
|
|
Distance, ///< 距离约束: |p1-p0| - d = 0 [1 eq]
|
|
Radius, ///< 半径约束 [1 eq]
|
|
Tangent, ///< 相切 [1 eq]
|
|
Concentric, ///< 同心 [2 eqs]
|
|
FixedPoint, ///< 固定点（硬约束，排除变量）[0 eq — 系统预处理]
|
|
Fixed, ///< 固定点位置: x-x0=0, y-y0=0 [2 eqs]
|
|
Angle, ///< 角度约束: atan2(cross(d0,d1), dot(d0,d1)) - θ = 0 [1 eq]
|
|
};
|
|
|
|
/**
|
|
* @brief 约束实例
|
|
* @ingroup sketch
|
|
*/
|
|
struct Constraint {
|
|
ConstraintType type; ///< 约束类型
|
|
std::vector<int> elements; ///< 引用的元素索引（点/线/圆 ID）
|
|
double value = 0.0; ///< 距离/角度值
|
|
};
|
|
|
|
/**
|
|
* @brief 求解器返回结果
|
|
* @ingroup sketch
|
|
*/
|
|
struct SolverResult {
|
|
bool converged = false; ///< 是否收敛
|
|
int iterations = 0; ///< 实际迭代次数
|
|
double residual = 0.0; ///< 最终残差（均方根）
|
|
std::vector<Point2D> points; ///< 求解后的点坐标
|
|
std::string message; ///< 诊断信息（成功或失败原因）
|
|
};
|
|
|
|
/**
|
|
* @brief 二维几何约束求解器
|
|
*
|
|
* 基于 Gauss-Newton 非线性最小二乘的 2D 草图约束求解器。
|
|
* 支持点、线段、圆元素的水平/垂直/平行/垂直/相切/距离/角度等约束。
|
|
*
|
|
* 求解流程：
|
|
* 1. 构建变量向量（非固定点的 x,y 坐标）
|
|
* 2. 对每个约束方程计算残差向量 r(x)
|
|
* 3. 数值 Jacobian J = ∂r/∂x（中心差分）
|
|
* 4. Gauss-Newton 步：Δx = -(J^T J)^{-1} J^T r
|
|
* 5. x ← x + Δx，重复直到收敛或 max_iter
|
|
*
|
|
* 自由度分析：
|
|
* DOF = 2·N_free_points - Σ(约束方程数)
|
|
* DOF > 0 → 欠约束（解不唯一）
|
|
* DOF = 0 → 恰定（应有唯一解）
|
|
* DOF < 0 → 过约束（最小二乘意义下求解）
|
|
*
|
|
* @code{.cpp}
|
|
* ConstraintSolver solver;
|
|
* int p0 = solver.add_point(0, 0); // 原点
|
|
* int p1 = solver.add_point(100, 0);
|
|
* int p2 = solver.add_point(100, 50);
|
|
* solver.add_line(p0, p1); // 底边
|
|
* solver.fix_point(p0); // 原点固定
|
|
* solver.add_constraint(ConstraintType::Horizontal, {p0, p1});
|
|
* solver.add_constraint(ConstraintType::Distance, {p0, p2}, 100.0);
|
|
*
|
|
* auto result = solver.solve();
|
|
* if (result.converged) {
|
|
* auto p = result.points;
|
|
* }
|
|
* @endcode
|
|
*
|
|
* @ingroup sketch
|
|
*/
|
|
class ConstraintSolver {
|
|
public:
|
|
/**
|
|
* @brief 添加自由点
|
|
* @param x X 坐标
|
|
* @param y Y 坐标
|
|
* @param fixed 是否固定（true 则排除变量）
|
|
* @return 点 ID
|
|
*/
|
|
int add_point(double x, double y, bool fixed = false);
|
|
|
|
/**
|
|
* @brief 添加线段
|
|
* @param p0 起点 ID
|
|
* @param p1 终点 ID
|
|
* @return 线段 ID
|
|
*/
|
|
int add_line(int p0, int p1);
|
|
|
|
/**
|
|
* @brief 添加圆
|
|
* @param center 圆心点 ID
|
|
* @param radius 半径
|
|
* @return 圆 ID
|
|
*/
|
|
int add_circle(int center, double radius);
|
|
|
|
/**
|
|
* @brief 添加约束
|
|
* @param type 约束类型
|
|
* @param elements 约束涉及的元素 ID 列表
|
|
* @param val 约束值（距离、角度等），默认 0
|
|
*
|
|
* @note 不同类型 elements 的语义：
|
|
* Horizontal/Vertical: {p0, p1}
|
|
* Parallel/Perpendicular: {l0_p0, l0_p1, l1_p0, l1_p1}
|
|
* Distance: {p0, p1}, val = 距离
|
|
* Angle: {l0_p0, l0_p1, l1_p0, l1_p1}, val = 角度（度）
|
|
* Radius: {circle_id}, val = 半径
|
|
* Tangent: {line_p0, line_p1, circle_id}
|
|
* Coincident: {p0, p1}
|
|
*/
|
|
void add_constraint(ConstraintType type, const std::vector<int>& elements, double val = 0.0);
|
|
|
|
/**
|
|
* @brief 标记点为固定（硬约束）
|
|
* @param pid 点 ID
|
|
* @note 固定点不参与变量优化，从自由度中排除
|
|
*/
|
|
void fix_point(int pid) {
|
|
if (pid >= 0 && pid < static_cast<int>(points_.size()))
|
|
points_[pid].fixed = true;
|
|
}
|
|
|
|
/**
|
|
* @brief 执行约束求解
|
|
* @param max_iter 最大 Gauss-Newton 迭代次数，默认 100
|
|
* @param tol 收敛容差（梯度范数 < tol 时停止），默认 1e-8
|
|
* @return SolverResult 求解结果
|
|
*
|
|
* @note 收敛判断：||J^T r|| < tol
|
|
* @note 欠约束时（DOF > 0）解不唯一；过约束时（DOF < 0）为最小二乘解
|
|
*/
|
|
[[nodiscard]] SolverResult solve(int max_iter = 100, double tol = 1e-8);
|
|
|
|
/**
|
|
* @brief 计算自由度
|
|
* @return DOF = 2·N_free - Σ(约束方程数)
|
|
*
|
|
* @note DOF < 0 时系统过约束，求解器仍然工作但解非精确满足所有约束
|
|
*/
|
|
[[nodiscard]] int degrees_of_freedom() const;
|
|
|
|
/**
|
|
* @brief 当前点集访问
|
|
* @return SketchPoint 数组只读引用
|
|
*/
|
|
[[nodiscard]] const std::vector<SketchPoint>& points() const { return points_; }
|
|
|
|
/**
|
|
* @brief 查询点坐标
|
|
* @param id 点 ID
|
|
* @return Point2D 当前坐标
|
|
*/
|
|
[[nodiscard]] Point2D get_point(int id) const;
|
|
|
|
private:
|
|
std::vector<SketchPoint> points_; ///< 点集
|
|
std::vector<SketchLine> lines_; ///< 线段集
|
|
std::vector<SketchCircle> circles_; ///< 圆集
|
|
std::vector<Constraint> constraints_; ///< 约束集
|
|
|
|
/**
|
|
* @brief 将求解器变量向量同步回 points_（用于 Jacobian 数值计算）
|
|
* @param vars 变量数组（长度为 2·N_free）
|
|
* @param nv 变量数
|
|
*/
|
|
void sync_from_vars(const double* vars, int nv);
|
|
|
|
/**
|
|
* @brief 评估单个约束的残差值
|
|
* @param c 约束
|
|
* @return 残差（方程 → 0 表示约束满足）
|
|
*/
|
|
double eval_constraint(const Constraint& c) const;
|
|
|
|
/**
|
|
* @brief 计算单个约束的方程数
|
|
* @param c 约束
|
|
* @return Coincident/Fixed → 2，其余 → 1
|
|
*/
|
|
int count_equations(const Constraint& c) const;
|
|
|
|
/**
|
|
* @brief 填充 Jacobian 矩阵中对应约束 ci 的行
|
|
* @param c 约束
|
|
* @param ci 约束索引
|
|
* @param row_map 方程行映射
|
|
* @param J_data Jacobian 矩阵数据（行优先）
|
|
* @param nv 变量数
|
|
* @param stride 行跨度
|
|
*/
|
|
void fill_jacobian_rows(const Constraint& c, int ci,
|
|
const std::vector<int>& row_map,
|
|
double* J_data, int nv, int stride) const;
|
|
};
|
|
|
|
} // namespace vde::sketch
|