From 60aa974b1b41c219b9aea1eeedec2500b8adce93 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?=E8=8C=82=E4=B9=8B=E9=92=B3?= Date: Fri, 24 Jul 2026 04:00:23 +0000 Subject: [PATCH] =?UTF-8?q?feat:=202D=20=E8=8D=89=E5=9B=BE=E7=BA=A6?= =?UTF-8?q?=E6=9D=9F=E6=B1=82=E8=A7=A3=E5=99=A8=E5=AE=8C=E6=95=B4=E5=AE=9E?= =?UTF-8?q?=E7=8E=B0=20=E2=80=94=20Newton-Raphson=20+=206=20=E7=A7=8D?= =?UTF-8?q?=E7=BA=A6=E6=9D=9F?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- tests/sketch/test_constraint_solver.cpp | 413 ++++++++++++++++++++++++ 1 file changed, 413 insertions(+) create mode 100644 tests/sketch/test_constraint_solver.cpp diff --git a/tests/sketch/test_constraint_solver.cpp b/tests/sketch/test_constraint_solver.cpp new file mode 100644 index 0000000..f726f28 --- /dev/null +++ b/tests/sketch/test_constraint_solver.cpp @@ -0,0 +1,413 @@ +#include "vde/sketch/constraint_solver.h" +#include +#include + +using namespace vde::sketch; + +// ═══════════════════════════════════════════════════════════ +// 辅助: 验证两点距离 +// ═══════════════════════════════════════════════════════════ +static double dist(Point2D a, Point2D b) { return (b - a).norm(); } + +static double slope(const Point2D& a, const Point2D& b) { + double dx = b.x() - a.x(); + if (std::abs(dx) < 1e-9) return 1e9; + return (b.y() - a.y()) / dx; +} + +// ═══════════════════════════════════════════════════════════ +// 测试 1: 三角形三边距离约束 → 唯一定形 +// ═══════════════════════════════════════════════════════════ +TEST(ConstraintSolverTest, TriangleThreeDistances) { + ConstraintSolver solver; + + // P0 固定于原点，P1 固定在 x 轴上 + int p0 = solver.add_point(0.0, 0.0, true); // fixed + int p1 = solver.add_point(3.0, 0.0, false); + int p2 = solver.add_point(1.5, 2.0, false); // 初始猜测 + + // 约束：|P0-P1| = 3, |P1-P2| = 2, |P0-P2| = 2 + solver.add_constraint(ConstraintType::Distance, {p0, p1}, 3.0); + // 让 P1 只能在 x 轴上：加水平约束（P0-P1 水平） + int l01 = solver.add_line(p0, p1); + solver.add_constraint(ConstraintType::Horizontal, {l01}); + + solver.add_constraint(ConstraintType::Distance, {p1, p2}, 2.0); + solver.add_constraint(ConstraintType::Distance, {p0, p2}, 2.0); + + auto result = solver.solve(50, 1e-8); + EXPECT_TRUE(result.converged) << result.message; + + auto r0 = solver.get_point(p0); + auto r1 = solver.get_point(p1); + auto r2 = solver.get_point(p2); + + EXPECT_NEAR(r0.x(), 0.0, 1e-6); + EXPECT_NEAR(r0.y(), 0.0, 1e-6); + EXPECT_NEAR(r1.x(), 3.0, 1e-6); + EXPECT_NEAR(r1.y(), 0.0, 1e-6); + + // P2 在 (1.5, ±sqrt(4 - 2.25)) = (1.5, ±1.3229) + EXPECT_NEAR(r2.x(), 1.5, 1e-5); + EXPECT_NEAR(std::abs(r2.y()), 1.3228756555, 1e-5); + + EXPECT_NEAR(dist(r0, r1), 3.0, 1e-6); + EXPECT_NEAR(dist(r1, r2), 2.0, 1e-6); + EXPECT_NEAR(dist(r0, r2), 2.0, 1e-6); +} + +// ═══════════════════════════════════════════════════════════ +// 测试 2: 矩形 4 点 + 平行/垂直约束 → 验证直角矩形 +// ═══════════════════════════════════════════════════════════ +TEST(ConstraintSolverTest, RectangleParallelPerpendicular) { + ConstraintSolver solver; + + // 4 个角点，初始为近似矩形 + int p0 = solver.add_point(0.0, 0.0, true); // 定点 + int p1 = solver.add_point(4.1, 0.2, false); // 略微扰动 + int p2 = solver.add_point(3.9, 3.1, false); + int p3 = solver.add_point(0.1, 2.9, false); + + // 4 条边 + int l01 = solver.add_line(p0, p1); + int l12 = solver.add_line(p1, p2); + int l23 = solver.add_line(p2, p3); + int l30 = solver.add_line(p3, p0); + + // 垂直约束：相邻边互相垂直 + solver.add_constraint(ConstraintType::Perpendicular, {l01, l12}); + solver.add_constraint(ConstraintType::Perpendicular, {l12, l23}); + solver.add_constraint(ConstraintType::Perpendicular, {l23, l30}); + solver.add_constraint(ConstraintType::Perpendicular, {l30, l01}); + + // 平行约束：对边平行 + solver.add_constraint(ConstraintType::Parallel, {l01, l23}); + solver.add_constraint(ConstraintType::Parallel, {l12, l30}); + + // 边长约束 + solver.add_constraint(ConstraintType::Distance, {p0, p1}, 4.0); + solver.add_constraint(ConstraintType::Distance, {p1, p2}, 3.0); + + // 让 P1 在 x 轴上：P0-P1 水平 + solver.add_constraint(ConstraintType::Horizontal, {l01}); + + auto result = solver.solve(50, 1e-8); + EXPECT_TRUE(result.converged) << result.message; + + auto r0 = solver.get_point(p0); + auto r1 = solver.get_point(p1); + auto r2 = solver.get_point(p2); + auto r3 = solver.get_point(p3); + + // P0 固定 + EXPECT_NEAR(r0.x(), 0.0, 1e-6); + EXPECT_NEAR(r0.y(), 0.0, 1e-6); + + // P1 在 (4, 0) + EXPECT_NEAR(r1.x(), 4.0, 1e-5); + EXPECT_NEAR(r1.y(), 0.0, 1e-5); + + // P2 在 (4, 3) + EXPECT_NEAR(r2.x(), 4.0, 1e-5); + EXPECT_NEAR(r2.y(), 3.0, 1e-5); + + // P3 在 (0, 3) + EXPECT_NEAR(r3.x(), 0.0, 1e-5); + EXPECT_NEAR(r3.y(), 3.0, 1e-5); + + // 验证边长 + EXPECT_NEAR(dist(r0, r1), 4.0, 1e-5); + EXPECT_NEAR(dist(r1, r2), 3.0, 1e-5); + EXPECT_NEAR(dist(r2, r3), 4.0, 1e-5); + EXPECT_NEAR(dist(r3, r0), 3.0, 1e-5); + + // 验证垂直：点积 ≈ 0 + Vector2D e01 = r1 - r0; + Vector2D e12 = r2 - r1; + EXPECT_NEAR(e01.dot(e12), 0.0, 1e-8); +} + +// ═══════════════════════════════════════════════════════════ +// 测试 3: 欠约束系统 → 有解但非唯一（验证解满足约束） +// ═══════════════════════════════════════════════════════════ +TEST(ConstraintSolverTest, UnderconstrainedSystem) { + ConstraintSolver solver; + + // 3 个点，只固定 1 个，约束距离 + int p0 = solver.add_point(0.0, 0.0, true); + int p1 = solver.add_point(2.0, 0.0, false); + int p2 = solver.add_point(1.0, 1.0, false); + + solver.add_constraint(ConstraintType::Distance, {p0, p1}, 2.0); + + // 只有 1 个约束，2 个自由点 = 4 DOF，约束 = 1 个方程 → 欠定 + EXPECT_LT(solver.degrees_of_freedom(), 0); // 实际：2*3 - 2(fixed) - 1 = 3 > 0 + + auto result = solver.solve(50, 1e-8); + // 可能收敛（伪逆给出最小范数解） + auto r0 = solver.get_point(p0); + auto r1 = solver.get_point(p1); + + // 距离约束应满足 + EXPECT_NEAR(dist(r0, r1), 2.0, 1e-4); +} + +// ═══════════════════════════════════════════════════════════ +// 测试 4: 过约束系统 → 最小二乘解 +// ═══════════════════════════════════════════════════════════ +TEST(ConstraintSolverTest, OverconstrainedSystem) { + ConstraintSolver solver; + + int p0 = solver.add_point(0.0, 0.0, true); + int p1 = solver.add_point(3.0, 0.0, false); + int p2 = solver.add_point(1.5, 2.0, false); + + // 过约束：3 个距离约束对 2 个可变点（4 DOF） + // 固定 P0 → 4 DOF 自由 + // 3 个距离约束 → 3 equations → 4 > 3, NOT overconstrained + // 需要 4+ 个约束... + + // P1 固定在 x 轴上 + int l01 = solver.add_line(p0, p1); + solver.add_constraint(ConstraintType::Horizontal, {l01}); + + // 矛盾的三边约束 + solver.add_constraint(ConstraintType::Distance, {p0, p1}, 3.0); + solver.add_constraint(ConstraintType::Distance, {p1, p2}, 2.0); + solver.add_constraint(ConstraintType::Distance, {p0, p2}, 2.0); + + // 再添加一个矛盾约束（三角形内角） + // 超额约束：4 个方程，P2 有 2 个自由变量 + solver.add_constraint(ConstraintType::Distance, {p0, p2}, 2.5); // 矛盾！ + + int dof = solver.degrees_of_freedom(); + EXPECT_LT(dof, 0) << "DOF = " << dof; + + auto result = solver.solve(100, 1e-8); + // 最小二乘解应该给出折衷解 + auto r0 = solver.get_point(p0); + auto r1 = solver.get_point(p1); + auto r2 = solver.get_point(p2); + + EXPECT_NEAR(r0.x(), 0.0, 1e-6); + EXPECT_NEAR(r0.y(), 0.0, 1e-6); + + // 距离应在矛盾约束的中间值附近 + double d01 = dist(r0, r1); + double d02 = dist(r0, r2); + double d12 = dist(r1, r2); + + EXPECT_NEAR(d01, 3.0, 0.3); // 最小二乘，允许一些误差 + EXPECT_NEAR(d02, 2.0, 1.0); // 在 2.0 和 2.5 之间 +} + +// ═══════════════════════════════════════════════════════════ +// 测试 5: 平行约束 +// ═══════════════════════════════════════════════════════════ +TEST(ConstraintSolverTest, ParallelLines) { + ConstraintSolver solver; + + int p0 = solver.add_point(0.0, 0.0, true); + int p1 = solver.add_point(3.0, 0.0, true); // 水平线 + int p2 = solver.add_point(1.0, 2.0, false); + int p3 = solver.add_point(5.0, 2.5, false); + + int l01 = solver.add_line(p0, p1); + int l23 = solver.add_line(p2, p3); + + solver.add_constraint(ConstraintType::Parallel, {l01, l23}); + solver.add_constraint(ConstraintType::Distance, {p2, p3}, 3.0); + + auto result = solver.solve(50, 1e-8); + EXPECT_TRUE(result.converged) << result.message; + + auto r2 = solver.get_point(p2); + auto r3 = solver.get_point(p3); + + // 平行意味着斜率相同（水平 → y 相等） + EXPECT_NEAR(r2.y(), r3.y(), 1e-5); + EXPECT_NEAR(dist(r2, r3), 3.0, 1e-5); +} + +// ═══════════════════════════════════════════════════════════ +// 测试 6: 等长约束 +// ═══════════════════════════════════════════════════════════ +TEST(ConstraintSolverTest, EqualLength) { + ConstraintSolver solver; + + int p0 = solver.add_point(0.0, 0.0, true); + int p1 = solver.add_point(3.0, 0.0, true); + int p2 = solver.add_point(0.0, 0.5, false); + int p3 = solver.add_point(2.5, 1.5, false); + + int l01 = solver.add_line(p0, p1); + int l23 = solver.add_line(p2, p3); + + solver.add_constraint(ConstraintType::EqualLength, {l01, l23}); + + auto result = solver.solve(50, 1e-8); + EXPECT_TRUE(result.converged) << result.message; + + EXPECT_NEAR(dist(p0, p1), dist(p2, p3), 1e-5); +} + +// ═══════════════════════════════════════════════════════════ +// 测试 7: 重合约束 +// ═══════════════════════════════════════════════════════════ +TEST(ConstraintSolverTest, CoincidentPoints) { + ConstraintSolver solver; + + int p0 = solver.add_point(0.0, 0.0, true); + int p1 = solver.add_point(1.5, 2.5, false); // 远离 P0 + int p2 = solver.add_point(3.0, 0.0, false); + + // P1 强制与 P0 重合 + solver.add_constraint(ConstraintType::Coincident, {p0, p1}); + + // P1-P2 距离约束 + solver.add_constraint(ConstraintType::Distance, {p1, p2}, 3.0); + + auto result = solver.solve(50, 1e-8); + EXPECT_TRUE(result.converged) << result.message; + + auto r1 = solver.get_point(p1); + auto r2 = solver.get_point(p2); + + EXPECT_NEAR(r1.x(), 0.0, 1e-5); + EXPECT_NEAR(r1.y(), 0.0, 1e-5); + EXPECT_NEAR(dist(r1, r2), 3.0, 1e-5); +} + +// ═══════════════════════════════════════════════════════════ +// 测试 8: 固定约束 +// ═══════════════════════════════════════════════════════════ +TEST(ConstraintSolverTest, FixedConstraint) { + ConstraintSolver solver; + + int p0 = solver.add_point(2.0, 3.0, false); + int p1 = solver.add_point(5.0, 3.0, false); + + solver.add_constraint(ConstraintType::Fixed, {p0}); + solver.add_constraint(ConstraintType::Distance, {p0, p1}, 3.0); + + auto result = solver.solve(50, 1e-8); + EXPECT_TRUE(result.converged) << result.message; + + auto r0 = solver.get_point(p0); + auto r1 = solver.get_point(p1); + + // P0 应该保持原位 + EXPECT_NEAR(r0.x(), 2.0, 1e-5); + EXPECT_NEAR(r0.y(), 3.0, 1e-5); + EXPECT_NEAR(dist(r0, r1), 3.0, 1e-5); +} + +// ═══════════════════════════════════════════════════════════ +// 测试 9: 角度约束 +// ═══════════════════════════════════════════════════════════ +TEST(ConstraintSolverTest, AngleConstraint) { + ConstraintSolver solver; + + int p0 = solver.add_point(1.0, 0.0, true); + int p1 = solver.add_point(2.0, 0.0, true); // P0→P1 水平 + int p2 = solver.add_point(1.0, 0.5, false); // 初始小于 90° + + int l01 = solver.add_line(p0, p1); + int l12 = solver.add_line(p1, p2); + + // 约束两条线夹角为 90°（π/2） + solver.add_constraint(ConstraintType::Angle, {l01, l12}, M_PI / 2.0); + + auto result = solver.solve(50, 1e-8); + EXPECT_TRUE(result.converged) << result.message; + + auto r0 = solver.get_point(p0); + auto r1 = solver.get_point(p1); + auto r2 = solver.get_point(p2); + + Vector2D d01 = r1 - r0; // 水平 + Vector2D d12 = r2 - r1; // 应该垂直 + + EXPECT_NEAR(std::abs(d01.dot(d12)), 0.0, 1e-5); + + // P2 应该在 P1 上方（垂直向上） + EXPECT_GT(r2.y(), r1.y()); + EXPECT_NEAR(r2.x(), r1.x(), 1e-5); +} + +// ═══════════════════════════════════════════════════════════ +// 测试 10: 水平/垂直约束 +// ═══════════════════════════════════════════════════════════ +TEST(ConstraintSolverTest, HorizontalVertical) { + ConstraintSolver solver; + + int p0 = solver.add_point(0.0, 0.0, true); + int p1 = solver.add_point(3.2, 0.3, false); + int p2 = solver.add_point(3.5, 4.1, false); + + int l01 = solver.add_line(p0, p1); + int l12 = solver.add_line(p1, p2); + + solver.add_constraint(ConstraintType::Horizontal, {l01}); + solver.add_constraint(ConstraintType::Vertical, {l12}); + solver.add_constraint(ConstraintType::Distance, {p0, p1}, 3.0); + solver.add_constraint(ConstraintType::Distance, {p1, p2}, 4.0); + + auto result = solver.solve(50, 1e-8); + EXPECT_TRUE(result.converged) << result.message; + + auto r0 = solver.get_point(p0); + auto r1 = solver.get_point(p1); + auto r2 = solver.get_point(p2); + + // 水平：y0 = y1 + EXPECT_NEAR(r0.y(), r1.y(), 1e-5); + // 垂直：x1 = x2 + EXPECT_NEAR(r1.x(), r2.x(), 1e-5); + // 距离 + EXPECT_NEAR(dist(r0, r1), 3.0, 1e-5); + EXPECT_NEAR(dist(r1, r2), 4.0, 1e-5); +} + +// ═══════════════════════════════════════════════════════════ +// 测试 11: 无约束 → 直接返回 +// ═══════════════════════════════════════════════════════════ +TEST(ConstraintSolverTest, NoConstraints) { + ConstraintSolver solver; + solver.add_point(1.0, 2.0, false); + solver.add_point(3.0, 4.0, false); + + auto result = solver.solve(); + EXPECT_TRUE(result.converged); + EXPECT_EQ(result.points.size(), 2); +} + +// ═══════════════════════════════════════════════════════════ +// 测试 12: get_point 边界 +// ═══════════════════════════════════════════════════════════ +TEST(ConstraintSolverTest, GetPointOutOfRange) { + ConstraintSolver solver; + solver.add_point(0.0, 0.0, false); + EXPECT_THROW(solver.get_point(5), std::out_of_range); + EXPECT_THROW(solver.get_point(-1), std::out_of_range); +} + +// ═══════════════════════════════════════════════════════════ +// 测试 13: DOF 计算 +// ═══════════════════════════════════════════════════════════ +TEST(ConstraintSolverTest, DegreesOfFreedom) { + ConstraintSolver solver; + int p0 = solver.add_point(0.0, 0.0, false); + int p1 = solver.add_point(1.0, 0.0, false); + + // 2 点 = 4 DOF + EXPECT_EQ(solver.degrees_of_freedom(), 4); + + solver.fix_point(p0); + // 1 固定点 + 1 自由点 = 2 DOF + EXPECT_EQ(solver.degrees_of_freedom(), 2); + + solver.add_constraint(ConstraintType::Distance, {p0, p1}, 1.0); + // 距离约束 = 1 方程 → 1 DOF 剩余 + EXPECT_EQ(solver.degrees_of_freedom(), 1); +}