juli/OrcaSlicer
1
1
Fork 0
mirror of https://github.com/OrcaSlicer/OrcaSlicer.git synced 2026-10-15 03:41:28 +00:00
Code Issues Packages Projects Releases Wiki Activity
Files
f33a9bf9992960c2cf9a2413bc6249c4ebcf1fee
OrcaSlicer/src/libslic3r/CAD/SketchSolver.cpp
T

686 lines
36 KiB
C++
Raw Blame History

#include "libslic3r/CAD/SketchSolver.hpp"
#include "libslic3r/CAD/SketchEngine.hpp"
#include <math.h>
#include <slvs.h>
#include <algorithm>
#include <cmath>
#include <cstring>
#include <functional>
#include <map>
#include <unordered_map>
#include <vector>
#include <utility>
#include "libslic3r/Point.hpp"
namespace Slic3r {
using CT = SketchConstraintType;
using Role = SketchPointRole;
namespace {
constexpr Slvs_hGroup G_FIXED = 1; // workplane / reference: held constant
constexpr Slvs_hGroup G_SK = 2; // sketch geometry: the group we solve
// Per-entity slvs handles. p0/p1/center are point2d entity handles; prim is the
// line/arc/circle entity; rparam is the circle radius param.
struct Slots {
Slvs_hEntity prim{0}, p0{0}, p1{0}, center{0};
Slvs_hParam rparam{0};
std::vector<Slvs_hEntity> pts; // BSpline control points (point2d handles)
};
struct Build {
std::vector<Slvs_Param> params;
std::vector<Slvs_Entity> ents;
std::vector<Slvs_Constraint> cons;
Slvs_hParam ph{0};
Slvs_hEntity eh{0};
Slvs_hConstraint ch{0};
Slvs_hEntity wp{0}, normal{0};
Slvs_hParam P(Slvs_hGroup g, double v) { params.push_back(Slvs_MakeParam(++ph, g, v)); return ph; }
Slvs_hEntity E(Slvs_Entity e) { ents.push_back(e); return e.h; }
Slvs_hEntity pt2d(Slvs_hGroup g, double u, double v)
{ return E(Slvs_MakePoint2d(++eh, g, wp, P(g, u), P(g, v))); }
// Generic constraint (entityC unused by Slvs_MakeConstraint — set it manually below).
// `other` / `other2` pick the END point (point[2]) of entity A / B instead of its start
// (point[1]) for the constraints that read an arc's endpoint (the tangencies).
void C(int type, double val, Slvs_hEntity ptA, Slvs_hEntity ptB,
Slvs_hEntity eA, Slvs_hEntity eB, Slvs_hEntity eC = 0, int other = 0, int other2 = 0)
{
Slvs_Constraint c = Slvs_MakeConstraint(++ch, G_SK, type, wp, val, ptA, ptB, eA, eB);
c.entityC = eC;
c.other = other;
c.other2 = other2;
cons.push_back(c);
}
};
inline int role_idx(Role r) { return int(r); }
} // namespace
static SketchSolveResult solve_system(std::vector<SketchEntity>& entities,
const std::vector<SketchEntityConstraintDef>& constraints,
int dragged_ei, Role dragged_role)
{
SketchSolveResult out;
if (constraints.empty()) { out.ok = true; out.dof = -1; return out; }
Build b;
// ---- Fixed 2D XY workplane (origin at 0,0,0; identity normal) -------------------
Slvs_hEntity origin = b.E(Slvs_MakePoint3d(++b.eh, G_FIXED,
b.P(G_FIXED, 0.0), b.P(G_FIXED, 0.0), b.P(G_FIXED, 0.0)));
double qw, qx, qy, qz;
Slvs_MakeQuaternion(1, 0, 0, 0, 1, 0, &qw, &qx, &qy, &qz);
b.normal = b.E(Slvs_MakeNormal3d(++b.eh, G_FIXED,
b.P(G_FIXED, qw), b.P(G_FIXED, qx), b.P(G_FIXED, qy), b.P(G_FIXED, qz)));
b.wp = b.E(Slvs_MakeWorkplane(++b.eh, G_FIXED, origin, b.normal));
// Unit direction references for the axis-projected distance constraints. Both live in
// G_FIXED, so they are held constant and add no DOF to the system.
// libslvs defines a LINE_SEGMENT's direction as point[0] - point[1] (entity.cpp
// VectorGetExprs), so the unit vector's head is listed first to yield +X / +Y.
const Slvs_hEntity dir_x [[maybe_unused]] = b.E(Slvs_MakeLineSegment(++b.eh, G_FIXED, b.wp,
b.pt2d(G_FIXED, 1.0, 0.0), b.pt2d(G_FIXED, 0.0, 0.0)));
const Slvs_hEntity dir_y [[maybe_unused]] = b.E(Slvs_MakeLineSegment(++b.eh, G_FIXED, b.wp,
b.pt2d(G_FIXED, 0.0, 1.0), b.pt2d(G_FIXED, 0.0, 0.0)));
// Implicit sketch references (origin, X axis, Y axis), addressable by the negative
// sentinels in SketchEngine.hpp. G_FIXED: held constant, zero added DOF. The axis lines
// are built head-first so their direction reads +X / +Y, matching dir_x / dir_y.
const Slvs_hEntity ref_origin_pt = b.pt2d(G_FIXED, 0.0, 0.0);
const Slvs_hEntity ref_axis_x = b.E(Slvs_MakeLineSegment(++b.eh, G_FIXED, b.wp,
b.pt2d(G_FIXED, 1.0, 0.0), ref_origin_pt));
const Slvs_hEntity ref_axis_y = b.E(Slvs_MakeLineSegment(++b.eh, G_FIXED, b.wp,
b.pt2d(G_FIXED, 0.0, 1.0), ref_origin_pt));
// ---- Entities -------------------------------------------------------------------
std::vector<Slots> slot(entities.size());
for (size_t i = 0; i < entities.size(); ++i) {
const SketchEntity& e = entities[i];
Slots s;
switch (e.type) {
case SketchEntity::Type::Line:
s.p0 = b.pt2d(G_SK, e.p0.x(), e.p0.y());
s.p1 = b.pt2d(G_SK, e.p1.x(), e.p1.y());
s.prim = b.E(Slvs_MakeLineSegment(++b.eh, G_SK, b.wp, s.p0, s.p1));
break;
case SketchEntity::Type::Point:
s.p0 = b.pt2d(G_SK, e.p0.x(), e.p0.y());
break;
case SketchEntity::Type::Circle: {
s.center = b.pt2d(G_SK, e.center.x(), e.center.y());
s.p0 = s.center; // p0 mirrors centre for circles
// A zero-radius circle is degenerate everywhere else (the wire builder, offset and
// inference all reject it). It used to be seeded with radius 1 here and written
// back, so any unrelated solve silently turned it into a 1 mm circle. It gets no
// primitive: its centre still solves, anything needing the rim is skipped.
if (e.radius > 1e-9) {
s.rparam = b.P(G_SK, e.radius);
Slvs_hEntity dist = b.E(Slvs_MakeDistance(++b.eh, G_SK, b.wp, s.rparam));
s.prim = b.E(Slvs_MakeCircle(++b.eh, G_SK, b.wp, s.center, b.normal, dist));
}
break;
}
case SketchEntity::Type::Arc:
s.center = b.pt2d(G_SK, e.center.x(), e.center.y());
s.p0 = b.pt2d(G_SK, e.p0.x(), e.p0.y()); // start
s.p1 = b.pt2d(G_SK, e.p1.x(), e.p1.y()); // end
s.prim = b.E(Slvs_MakeArcOfCircle(++b.eh, G_SK, b.wp, b.normal, s.center, s.p0, s.p1));
break;
// libslvs has no conic entity (scope note): register the ellipse's defining
// points only (center + arc endpoints) so center/endpoint constraints solve;
// the a/b/phi shape params pass through unsolved.
case SketchEntity::Type::Ellipse:
s.center = b.pt2d(G_SK, e.center.x(), e.center.y());
s.p0 = s.center; // p0 mirrors centre (circle convention)
break;
case SketchEntity::Type::EllipseArc:
s.center = b.pt2d(G_SK, e.center.x(), e.center.y());
s.p0 = b.pt2d(G_SK, e.p0.x(), e.p0.y()); // start
s.p1 = b.pt2d(G_SK, e.p1.x(), e.p1.y()); // end
break;
// No native slvs curve for an arbitrary-degree spline: register the control
// poles as point2d so endpoints (and any pole-targeted constraint) solve. The
// OCCT curve is rebuilt from the solved poles. p0/p1 mirror first/last pole so
// Coincident at the spline ends closes loops just like a Line.
case SketchEntity::Type::BSpline:
s.pts.reserve(e.ctrl.size());
for (const Vec2d& cp : e.ctrl)
s.pts.push_back(b.pt2d(G_SK, cp.x(), cp.y()));
if (!s.pts.empty()) { s.p0 = s.pts.front(); s.p1 = s.pts.back(); }
break;
}
slot[i] = s;
}
auto valid = [&](int ei) { return ei >= 0 && ei < int(entities.size()); };
auto ptOf = [&](int ei, Role r) -> Slvs_hEntity {
if (ei == kSketchRefOrigin) return ref_origin_pt;
if (ei == kSketchRefAxisX || ei == kSketchRefAxisY) return ref_origin_pt; // axes pass through it
if (!valid(ei)) return 0;
const Slots& s = slot[ei];
switch (r) {
case Role::P0: return s.p0;
case Role::P1: return s.p1;
case Role::Center: return s.center ? s.center : s.p0;
}
return 0;
};
auto primOf = [&](int ei) -> Slvs_hEntity {
if (ei == kSketchRefAxisX) return ref_axis_x;
if (ei == kSketchRefAxisY) return ref_axis_y;
return valid(ei) ? slot[ei].prim : 0; // origin has no prim: it is a point
};
auto coordOf = [&](int ei, Role r) -> Vec2d {
if (is_sketch_ref(ei)) return Vec2d(0, 0); // all three pass through the origin
if (!valid(ei)) return Vec2d(0, 0);
const SketchEntity& e = entities[ei];
switch (r) { case Role::P0: return e.p0; case Role::P1: return e.p1; case Role::Center: return e.center; }
return e.p0;
};
// Which side of line `li` the point (ei, r) is on, in slvs' sign convention for the
// in-workplane PT_LINE_DISTANCE: with d = point[0] - point[1] and a = point[0], the signed
// distance has the sign of d x (p - a) (pinned by the tests "a point below the line stays
// below it" and "tangent line to circle, line below it"). +1 when on the line.
auto side_of = [&](int ei, Role r, int li) -> double {
Vec2d a, bb;
if (li == kSketchRefAxisX) { a = Vec2d(1, 0); bb = Vec2d(0, 0); }
else if (li == kSketchRefAxisY) { a = Vec2d(0, 1); bb = Vec2d(0, 0); }
else if (valid(li)) { a = entities[li].p0; bb = entities[li].p1; }
else return 1.0;
const Vec2d d = a - bb, ap = coordOf(ei, r) - a;
const double cr = d.x() * ap.y() - d.y() * ap.x();
return cr < 0.0 ? -1.0 : 1.0;
};
// A fixed reference point at (x,y) — used to pin coordinates (Fix / LockX / LockY).
auto fixedRef = [&](double x, double y) -> Slvs_hEntity { return b.pt2d(G_FIXED, x, y); };
// ---- Constraints ----------------------------------------------------------------
for (const auto& c : constraints) {
// Robustness: never feed libslvs a null handle. A constraint that references an
// entity which produced no solver primitive (Point/Ellipse/EllipseArc/BSpline get
// no `prim`) or no point for the requested role would make Slvs FindById abort the
// whole process. Skip such a constraint instead of crashing.
bool ref_ok = true;
switch (c.type) {
case CT::Coincident: case CT::Horizontal: case CT::Vertical: case CT::Distance:
ref_ok = ptOf(c.ea, c.ra) && ptOf(c.eb, c.rb); break;
case CT::DistanceX:
case CT::DistanceY:
ref_ok = ptOf(c.ea, c.ra) && ptOf(c.eb, c.rb); break;
case CT::Concentric:
ref_ok = ptOf(c.ea, Role::Center) && ptOf(c.eb, Role::Center); break;
case CT::Fix: case CT::LockX: case CT::LockY:
ref_ok = ptOf(c.ea, c.ra) != 0; break;
case CT::EqualLength: case CT::Parallel: case CT::Perpendicular:
case CT::Angle: case CT::Tangent:
ref_ok = primOf(c.ea) && primOf(c.eb); break;
case CT::Radius: case CT::Diameter:
ref_ok = primOf(c.ea) != 0; break;
case CT::Midpoint:
ref_ok = ptOf(c.ea, c.ra) && primOf(c.eb); break;
case CT::Symmetric:
ref_ok = ptOf(c.ea, c.ra) && ptOf(c.eb, c.rb) && primOf(c.ec); break;
case CT::SymmetricAboutY: case CT::SymmetricAboutX:
ref_ok = ptOf(c.ea, c.ra) && ptOf(c.eb, c.rb); break;
case CT::PointOnLine: case CT::PointOnObject:
ref_ok = ptOf(c.ea, c.ra) && primOf(c.eb); break;
case CT::EqualRadius:
case CT::Collinear:
ref_ok = primOf(c.ea) && primOf(c.eb); break;
}
if (!ref_ok) { out.skipped.push_back(int(&c - constraints.data())); continue; }
switch (c.type) {
case CT::Coincident:
b.C(SLVS_C_POINTS_COINCIDENT, 0, ptOf(c.ea, c.ra), ptOf(c.eb, c.rb), 0, 0);
break;
case CT::Concentric:
b.C(SLVS_C_POINTS_COINCIDENT, 0, ptOf(c.ea, Role::Center), ptOf(c.eb, Role::Center), 0, 0);
break;
case CT::Horizontal:
b.C(SLVS_C_HORIZONTAL, 0, ptOf(c.ea, c.ra), ptOf(c.eb, c.rb), 0, 0);
break;
case CT::Vertical:
b.C(SLVS_C_VERTICAL, 0, ptOf(c.ea, c.ra), ptOf(c.eb, c.rb), 0, 0);
break;
case CT::Distance:
b.C(SLVS_C_PT_PT_DISTANCE, c.value, ptOf(c.ea, c.ra), ptOf(c.eb, c.rb), 0, 0);
break;
case CT::DistanceX:
// Distance between the two points measured along X only: project the vector
// between them onto the fixed unit X direction.
b.C(SLVS_C_PROJ_PT_DISTANCE, c.value, ptOf(c.ea, c.ra), ptOf(c.eb, c.rb), dir_x, 0);
break;
case CT::DistanceY:
b.C(SLVS_C_PROJ_PT_DISTANCE, c.value, ptOf(c.ea, c.ra), ptOf(c.eb, c.rb), dir_y, 0);
break;
case CT::Fix: {
const Vec2d p = coordOf(c.ea, c.ra);
b.C(SLVS_C_POINTS_COINCIDENT, 0, ptOf(c.ea, c.ra), fixedRef(p.x(), p.y()), 0, 0);
break;
}
case CT::LockX: {
const Vec2d p = coordOf(c.ea, c.ra);
b.C(SLVS_C_VERTICAL, 0, ptOf(c.ea, c.ra), fixedRef(c.value, p.y()), 0, 0);
break;
}
case CT::LockY: {
const Vec2d p = coordOf(c.ea, c.ra);
b.C(SLVS_C_HORIZONTAL, 0, ptOf(c.ea, c.ra), fixedRef(p.x(), c.value), 0, 0);
break;
}
case CT::EqualLength:
b.C(SLVS_C_EQUAL_LENGTH_LINES, 0, 0, 0, primOf(c.ea), primOf(c.eb));
break;
case CT::Parallel:
b.C(SLVS_C_PARALLEL, 0, 0, 0, primOf(c.ea), primOf(c.eb));
break;
case CT::Perpendicular:
b.C(SLVS_C_PERPENDICULAR, 0, 0, 0, primOf(c.ea), primOf(c.eb));
break;
case CT::Midpoint:
b.C(SLVS_C_AT_MIDPOINT, 0, ptOf(c.ea, c.ra), 0, primOf(c.eb), 0);
break;
case CT::Symmetric:
// ptA, ptB symmetric about the axis line (ec).
b.C(SLVS_C_SYMMETRIC_LINE, 0, ptOf(c.ea, c.ra), ptOf(c.eb, c.rb), primOf(c.ec), 0);
break;
case CT::SymmetricAboutY:
b.C(SLVS_C_SYMMETRIC_LINE, 0, ptOf(c.ea, c.ra), ptOf(c.eb, c.rb), primOf(kSketchRefAxisY), 0);
break;
case CT::SymmetricAboutX:
b.C(SLVS_C_SYMMETRIC_LINE, 0, ptOf(c.ea, c.ra), ptOf(c.eb, c.rb), primOf(kSketchRefAxisX), 0);
break;
case CT::Angle:
// model stores radians; slvs angle is in degrees.
b.C(SLVS_C_ANGLE, c.value * 180.0 / M_PI, 0, 0, primOf(c.ea), primOf(c.eb));
break;
case CT::Radius:
b.C(SLVS_C_DIAMETER, 2.0 * c.value, 0, 0, primOf(c.ea), 0);
break;
case CT::Diameter:
b.C(SLVS_C_DIAMETER, c.value, 0, 0, primOf(c.ea), 0);
break;
case CT::Tangent: {
const bool aCurve = valid(c.ea) && entities[c.ea].type != SketchEntity::Type::Line;
const bool bCurve = valid(c.eb) && entities[c.eb].type != SketchEntity::Type::Line;
const bool aCircle = aCurve && entities[c.ea].type == SketchEntity::Type::Circle;
const bool bCircle = bCurve && entities[c.eb].type == SketchEntity::Type::Circle;
if (aCurve && bCurve && (aCircle || bCircle)) {
// CURVE_CURVE_TANGENT reads each curve's ENDPOINT, which a full circle does not
// have: slvs asserts and takes the process down (the same trap the circle-line
// case below documents). Two round curves are tangent exactly when their
// centres are r1 + r2 apart (outside each other) or |r1 - r2| apart (one inside
// the other); keep whichever the sketch is closer to now. Radii are captured as
// constants, with the same caveat as the circle-line case.
const SketchEntity& A = entities[c.ea];
const SketchEntity& B = entities[c.eb];
const double d = (A.center - B.center).norm();
const double ext = A.radius + B.radius;
const double in = std::abs(A.radius - B.radius);
b.C(SLVS_C_PT_PT_DISTANCE, std::abs(d - ext) <= std::abs(d - in) ? ext : in,
ptOf(c.ea, Role::Center), ptOf(c.eb, Role::Center), 0, 0);
} else if (aCurve && bCurve) {
// Tangent where the two arcs MEET: pick, for each arc, the endpoint closest to
// the other arc's nearest endpoint. Always binding the start point (the old
// behaviour) made a tangency at an arc's end act on its start instead.
const SketchEntity& A = entities[c.ea];
const SketchEntity& B = entities[c.eb];
int oa = 0, ob = 0; double best = 1e300;
for (int i = 0; i < 2; ++i)
for (int j = 0; j < 2; ++j) {
const double dd = ((i ? A.p1 : A.p0) - (j ? B.p1 : B.p0)).squaredNorm();
if (dd < best) { best = dd; oa = i; ob = j; }
}
b.C(SLVS_C_CURVE_CURVE_TANGENT, 0, 0, 0, primOf(c.ea), primOf(c.eb), 0, oa, ob);
} else {
const int ci = aCurve ? c.ea : c.eb; // the curve
const int li = aCurve ? c.eb : c.ea; // the line
if (valid(ci) && entities[ci].type == SketchEntity::Type::Circle) {
// A FULL circle cannot use SLVS_C_ARC_LINE_TANGENT. That constraint reads
// arc->point[1] / point[2] — the arc's endpoints (see constrainteq.cpp,
// Type::ARC_LINE_TANGENT) — and a circle entity only has point[0], its
// centre. The zero handles send FindById into "Cannot find handle", which
// ABORTS the process rather than failing the solve, taking every later test
// with it. It is also the wrong equation for a circle: it only makes the
// line perpendicular to the radius AT AN ENDPOINT that does not exist.
//
// For a circle, tangency is exactly "the centre sits one radius away from
// the line", which slvs expresses directly.
//
// ponytail: the radius is captured here rather than tied as a variable —
// the C API takes a constant distance and offers no way to reference the
// circle's radius parameter. Exact whenever the radius is fixed or simply
// not being changed by another constraint in the same solve; if some other
// constraint drives the radius, re-solving restores tangency. Tying them
// would need an auxiliary point constrained onto both circle and line.
b.C(SLVS_C_PT_LINE_DISTANCE, side_of(ci, Role::Center, li) * entities[ci].radius,
ptOf(ci, Role::Center), 0, primOf(li), 0);
} else {
// The arc endpoint that touches the line is the one the tangency is at. The
// constraint used to bind the START point always, so a fillet — its start on
// one leg, its end on the other — forced both legs parallel.
int other = 0;
if (valid(ci) && valid(li)) {
const SketchEntity& A = entities[ci];
const SketchEntity& L = entities[li];
auto seg_dist = [&](const Vec2d& p) {
const Vec2d d = L.p1 - L.p0;
const double t = d.squaredNorm() > 1e-18
? std::clamp((p - L.p0).dot(d) / d.squaredNorm(), 0.0, 1.0) : 0.0;
return (L.p0 + t * d - p).norm();
};
other = seg_dist(A.p1) < seg_dist(A.p0) ? 1 : 0;
}
b.C(SLVS_C_ARC_LINE_TANGENT, 0, 0, 0, primOf(ci), primOf(li), 0, other);
}
}
break;
}
case CT::PointOnLine:
// slvs' in-workplane point-line distance is SIGNED. A negative stored value is an
// explicit side; a positive one (what the UI stores) keeps the side the point is on
// now. Passing |value| forced every point to the positive side, flipping any that
// sat on the other one across the line.
if (std::abs(c.value) < 1e-9)
b.C(SLVS_C_PT_ON_LINE, 0, ptOf(c.ea, c.ra), 0, primOf(c.eb), 0);
else
b.C(SLVS_C_PT_LINE_DISTANCE,
c.value < 0.0 ? c.value : side_of(c.ea, c.ra, c.eb) * c.value,
ptOf(c.ea, c.ra), 0, primOf(c.eb), 0);
break;
case CT::PointOnObject:
// Point (ea,ra) lies on entity edge eb: a circle or arc rim -> PT_ON_CIRCLE (slvs
// takes both), otherwise the segment line -> PT_ON_LINE. An arc used to fall to
// PT_ON_LINE, which is not an equation about an arc at all.
if (valid(c.eb) && (entities[c.eb].type == SketchEntity::Type::Circle ||
entities[c.eb].type == SketchEntity::Type::Arc))
b.C(SLVS_C_PT_ON_CIRCLE, 0, ptOf(c.ea, c.ra), 0, primOf(c.eb), 0);
else
b.C(SLVS_C_PT_ON_LINE, 0, ptOf(c.ea, c.ra), 0, primOf(c.eb), 0);
break;
case CT::EqualRadius:
b.C(SLVS_C_EQUAL_RADIUS, 0, 0, 0, primOf(c.ea), primOf(c.eb));
break;
case CT::Collinear:
// libslvs has no collinear code. Two lines are collinear iff they are parallel
// AND a point of one lies on the other's infinite line — emit both.
b.C(SLVS_C_PARALLEL, 0, 0, 0, primOf(c.ea), primOf(c.eb));
// Point-on-infinite-line via PT_LINE_DISTANCE=0 rather than PT_ON_LINE: the
// latter creates an internal `valP` param that this port's Slvs_Solve leaves at
// 0 in the working set (ModifyToSatisfy only updates SK.param), so an already
// collinear pair drifts. PT_LINE_DISTANCE=0 is the same condition with no extra
// parameter, so an already-satisfied solve is a clean no-op.
b.C(SLVS_C_PT_LINE_DISTANCE, 0, ptOf(c.eb, Role::P0), 0, primOf(c.ea), 0);
break;
}
}
// ---- Solve ----------------------------------------------------------------------
Slvs_System sys;
std::memset(&sys, 0, sizeof(sys));
sys.param = b.params.data(); sys.params = int(b.params.size());
sys.entity = b.ents.data(); sys.entities = int(b.ents.size());
sys.constraint = b.cons.data(); sys.constraints = int(b.cons.size());
std::vector<Slvs_hConstraint> failed(b.cons.size() + 1, 0);
sys.failed = failed.data();
sys.faileds = int(failed.size());
sys.calculateFaileds = 1;
// Drag pin: feed the dragged point's two params into sys.dragged[] so the solver
// favours keeping that point at the cursor and re-solves the rest around it.
if (dragged_ei >= 0) {
const Slvs_hEntity h = ptOf(dragged_ei, dragged_role);
for (const Slvs_Entity& en : b.ents)
if (en.h == h) { sys.dragged[0] = en.param[0]; sys.dragged[1] = en.param[1]; break; }
}
Slvs_Solve(&sys, G_SK);
out.result = sys.result;
out.dof = sys.dof;
out.ok = (sys.result == SLVS_RESULT_OKAY);
// Map solved param handles -> values, then read points back.
std::unordered_map<Slvs_hParam, double> pv;
pv.reserve(sys.params * 2);
for (int i = 0; i < sys.params; ++i) pv[sys.param[i].h] = sys.param[i].val;
std::unordered_map<Slvs_hEntity, const Slvs_Entity*> byH;
byH.reserve(sys.entities * 2);
for (int i = 0; i < sys.entities; ++i) byH[sys.entity[i].h] = &sys.entity[i];
auto coord = [&](Slvs_hEntity h) -> Vec2d {
auto it = byH.find(h);
if (it == byH.end()) return Vec2d(0, 0);
return Vec2d(pv[it->second->param[0]], pv[it->second->param[1]]);
};
// Map failed constraint handles back to indices into `constraints`.
if (!out.ok && sys.faileds > 0) {
std::unordered_map<Slvs_hConstraint, int> chToIdx;
// constraint handles were assigned in order starting after the fixed group; the
// i-th sketch constraint in b.cons has handle = its position. Rebuild by scanning.
for (size_t k = 0; k < b.cons.size(); ++k) chToIdx[b.cons[k].h] = int(k);
for (int i = 0; i < sys.faileds; ++i) {
auto it = chToIdx.find(failed[i]);
if (it != chToIdx.end() && it->second < int(constraints.size()))
out.bad.push_back(it->second);
}
}
// ---- Read solved geometry back --------------------------------------------------
// ONLY on success. A failed solve leaves libslvs' params holding its last Newton
// iterate — geometry that satisfies nothing and is usually wildly deformed. Writing
// that back made every rejected attempt destructive: the caller rolls the constraints
// back, but the sketch it rolls back to is already wreckage, so the next attempt starts
// from the corpse. The fillet degrade ladder hit this on every corner — rung 1 (a
// tangent on each leg) is legitimately over-constrained against the legs' own H/V, and
// its wreckage then failed rungs 2 and 3, which solve cleanly on their own. The arc
// ended up with no constraints at all and the solver snapped the corner shut. pl5.
if (!out.ok) return out;
for (size_t i = 0; i < entities.size(); ++i) {
SketchEntity& e = entities[i];
const Slots& s = slot[i];
if (s.p0) e.p0 = coord(s.p0);
if (s.p1) e.p1 = coord(s.p1);
if (s.center) e.center = coord(s.center);
if (e.type == SketchEntity::Type::BSpline) {
for (size_t k = 0; k < s.pts.size() && k < e.ctrl.size(); ++k)
e.ctrl[k] = coord(s.pts[k]);
if (!e.ctrl.empty()) { e.p0 = e.ctrl.front(); e.p1 = e.ctrl.back(); }
} else if (e.type == SketchEntity::Type::Circle) {
if (s.rparam) { auto it = pv.find(s.rparam); if (it != pv.end()) e.radius = it->second; }
e.p0 = e.center;
} else if (e.type == SketchEntity::Type::Arc && s.center) {
// Reflow arc angles from solved centre + endpoints, preserving sweep sign.
const double old_sweep = e.end_angle - e.start_angle;
const double ns = std::atan2(e.p0.y() - e.center.y(), e.p0.x() - e.center.x());
const double ne = std::atan2(e.p1.y() - e.center.y(), e.p1.x() - e.center.x());
double sweep = ne - ns;
const double TWO_PI = 2.0 * M_PI;
while (sweep <= -TWO_PI) sweep += TWO_PI;
while (sweep >= TWO_PI) sweep -= TWO_PI;
if (old_sweep >= 0.0 && sweep < 0.0) sweep += TWO_PI;
if (old_sweep < 0.0 && sweep > 0.0) sweep -= TWO_PI;
e.start_angle = ns;
e.end_angle = ns + sweep;
e.radius = 0.5 * ((e.p0 - e.center).norm() + (e.p1 - e.center).norm());
} else if (e.type == SketchEntity::Type::EllipseArc && s.center && e.radius > 1e-9 && e.rminor > 1e-9) {
// The solver moves the centre and the two ends as free points (it has no conic), so
// after a solve they need not agree with the stored angles, and the wire builder then
// finds the arc's ends away from its vertices and drops the whole sketch. Re-derive
// the parametric angles from the solved ends and put the ends back ON the ellipse.
const double cr = std::cos(e.rotation), sr = std::sin(e.rotation);
auto param = [&](const Vec2d& p) {
const Vec2d d = p - e.center;
const double x = d.x() * cr + d.y() * sr, y = -d.x() * sr + d.y() * cr;
return std::atan2(y / e.rminor, x / e.radius);
};
auto at = [&](double t) {
const double x = e.radius * std::cos(t), y = e.rminor * std::sin(t);
return Vec2d(e.center.x() + x * cr - y * sr, e.center.y() + x * sr + y * cr);
};
const double old_sweep = e.end_angle - e.start_angle;
double t0 = param(e.p0), t1 = param(e.p1);
if (old_sweep >= 0.0) { while (t1 <= t0) t1 += 2.0 * M_PI; }
else { while (t1 >= t0) t1 -= 2.0 * M_PI; }
e.start_angle = t0;
e.end_angle = t1;
e.p0 = at(t0);
e.p1 = at(t1);
}
}
return out;
}
// libslvs carries a COMPILE-TIME ceiling: solvespace.h declares `enum { MAX_UNKNOWNS = 1024 }`
// and sizes the System's param and equation arrays with it. solve_system() hands the solver every
// entity in the sketch, constrained or not, at 2 params per point — so a sketch of about 480 lines
// is the last one that fits, and the very next one comes back TOO_MANY_UNKNOWNS.
//
// What that did, before this: DesignSketchTool::try_add_constraints rolls the whole batch back
// when the solve fails, so the auto-constraint pass over a large sketch dropped EVERY constraint
// it had just inferred. Measured on the rig — 480 lines: 960 constraints, dof 480. 520 lines:
// 0 constraints, dof unknown. Nothing was said, and from there on no dimension and no constraint
// could ever be applied to that sketch, because each attempt re-solved the same oversized system
// and was rejected in turn. A typed length simply did nothing.
//
// Constraints only couple entities that SHARE a point, so a sketch is naturally a set of
// independent systems — a plate with 300 cut-outs is 301 little problems, not one big one.
// Solving them separately keeps every one of them far under the ceiling AND is faster, since the
// solver's work is superlinear in system size.
//
// The whole system is still tried FIRST, and this runs only on TOO_MANY_UNKNOWNS, so every sketch
// that fits today keeps its exact current behaviour, including its reported degrees of freedom.
// A genuinely over-constrained sketch still fails: the conflict lives inside one component and
// that component still rejects it.
// Degrees of freedom an entity has on its own, as the whole-system solve counts them.
static int natural_dof(const SketchEntity& e)
{
switch (e.type) {
case SketchEntity::Type::Line: return 4;
case SketchEntity::Type::Point: return 2;
case SketchEntity::Type::Circle: return e.radius > 1e-9 ? 3 : 2;
case SketchEntity::Type::Arc: return 5; // centre + two ends, ends equidistant
case SketchEntity::Type::Ellipse: return 2; // only the centre is a solver point
case SketchEntity::Type::EllipseArc: return 6; // centre + two ends
case SketchEntity::Type::BSpline: return 2 * int(e.ctrl.size());
}
return 0;
}
static SketchSolveResult solve_partitioned(std::vector<SketchEntity>& entities,
const std::vector<SketchEntityConstraintDef>& constraints,
int dragged_ei, Role dragged_role)
{
const int n = int(entities.size());
std::vector<int> parent(n);
for (int i = 0; i < n; ++i) parent[i] = i;
std::function<int(int)> find = [&](int a) {
while (parent[a] != a) { parent[a] = parent[parent[a]]; a = parent[a]; }
return a;
};
auto unite = [&](int a, int b) {
if (a < 0 || b < 0 || a >= n || b >= n) return;
a = find(a); b = find(b);
if (a != b) parent[a] = b;
};
for (const auto& c : constraints) { unite(c.ea, c.eb); unite(c.ea, c.ec); }
// Group the constraints by the component they belong to: that of the first ENTITY they
// reference. ea can be a sketch reference (origin / axis, negative) while eb is the entity,
// and such a constraint was dropped outright.
std::map<int, std::vector<int>> groups;
std::vector<bool> constrained(n, false);
for (size_t i = 0; i < constraints.size(); ++i) {
const SketchEntityConstraintDef& c = constraints[i];
const int a = (c.ea >= 0 && c.ea < n) ? c.ea : (c.eb >= 0 && c.eb < n) ? c.eb
: (c.ec >= 0 && c.ec < n) ? c.ec : -1;
if (a < 0) continue;
groups[find(a)].push_back(int(i));
for (int e : { c.ea, c.eb, c.ec }) if (e >= 0 && e < n) constrained[e] = true;
}
SketchSolveResult out;
out.ok = true;
out.dof = 0;
// Solve into COPIES and commit only if every component succeeded. The contract callers rely
// on is all-or-nothing — try_add_constraints rolls the batch back and expects the geometry it
// rolls back to be untouched — and partial writes would break it.
std::vector<std::pair<std::vector<int>, std::vector<SketchEntity>>> solved;
for (const auto& [root, cidx] : groups) {
std::vector<int> ents; // global indices, in order
std::map<int, int> local; // global -> local
auto take = [&](int e) {
if (e < 0 || e >= n || local.count(e)) return;
local[e] = int(ents.size());
ents.push_back(e);
};
for (int ci : cidx) { take(constraints[ci].ea); take(constraints[ci].eb); take(constraints[ci].ec); }
std::vector<SketchEntity> sub;
sub.reserve(ents.size());
for (int e : ents) sub.push_back(entities[e]);
std::vector<SketchEntityConstraintDef> subc;
subc.reserve(cidx.size());
for (int ci : cidx) {
SketchEntityConstraintDef d = constraints[ci];
// Sketch references (origin, axes: negative sentinels) are global and pass through.
auto map1 = [&](int& e) { if (e >= 0) e = local.count(e) ? local[e] : -1; };
map1(d.ea); map1(d.eb); map1(d.ec);
subc.push_back(d);
}
const int sub_drag = (dragged_ei >= 0 && local.count(dragged_ei)) ? local[dragged_ei] : -1;
SketchSolveResult r = solve_system(sub, subc, sub_drag, dragged_role);
if (!r.ok) {
out.ok = false;
out.result = r.result;
for (int bi : r.bad)
if (bi >= 0 && bi < int(cidx.size())) out.bad.push_back(cidx[bi]);
}
if (r.dof > 0) out.dof += r.dof;
for (int si : r.skipped)
if (si >= 0 && si < int(cidx.size())) out.skipped.push_back(cidx[si]);
solved.emplace_back(std::move(ents), std::move(sub));
}
// Entities no constraint touches are in no group but still have their freedoms; the
// whole-system solve counts them, so this path must too or the two report different dof.
for (int i = 0; i < n; ++i)
if (!constrained[i]) out.dof += natural_dof(entities[i]);
if (!out.ok) return out;
for (auto& [ents, sub] : solved)
for (size_t k = 0; k < ents.size(); ++k) entities[ents[k]] = sub[k];
return out;
}
static SketchSolveResult solve_impl(std::vector<SketchEntity>& entities,
const std::vector<SketchEntityConstraintDef>& constraints,
int dragged_ei, Role dragged_role)
{
SketchSolveResult out = solve_system(entities, constraints, dragged_ei, dragged_role);
if (out.ok || out.result != SLVS_RESULT_TOO_MANY_UNKNOWNS) return out;
return solve_partitioned(entities, constraints, dragged_ei, dragged_role);
}
SketchSolveResult sketch_solve(std::vector<SketchEntity>& entities,
const std::vector<SketchEntityConstraintDef>& constraints)
{
return solve_impl(entities, constraints, -1, Role::P0);
}
SketchSolveResult sketch_solve_drag(std::vector<SketchEntity>& entities,
const std::vector<SketchEntityConstraintDef>& constraints,
int dragged_ei, SketchPointRole dragged_role)
{
return solve_impl(entities, constraints, dragged_ei, dragged_role);
}
} // namespace Slic3r
Reference in New Issue View Git Blame Copy Permalink