fix(assembly): add datum plane logging + fix cross-product singularity #317

Merged
forbes merged 1 commits from fix/distance-datum-plane-classification into main 2026-02-22 22:05:50 +00:00
5 changed files with 157 additions and 10 deletions

View File

@@ -1115,10 +1115,19 @@ KCSolve::SolveContext AssemblyObject::buildSolveContext(
break;
default:
FC_WARN("Assembly : Distance joint '" << joint->getFullName()
<< "' — unhandled DistanceType "
<< distanceTypeName(distType)
<< ", falling back to Planar");
kind = KCSolve::BaseJointKind::Planar;
params.push_back(distance);
break;
}
FC_LOG("Assembly : Distance joint '" << joint->getFullName()
<< "' — DistanceType=" << distanceTypeName(distType)
<< ", kind=" << static_cast<int>(kind)
<< ", distance=" << distance);
break;
}
default:

View File

@@ -54,10 +54,56 @@
namespace PartApp = Part;
FC_LOG_LEVEL_INIT("Assembly", true, true, true)
// ======================================= Utils ======================================
namespace Assembly
{
const char* distanceTypeName(DistanceType dt)
{
switch (dt) {
case DistanceType::PointPoint: return "PointPoint";
case DistanceType::LineLine: return "LineLine";
case DistanceType::LineCircle: return "LineCircle";
case DistanceType::CircleCircle: return "CircleCircle";
case DistanceType::PlanePlane: return "PlanePlane";
case DistanceType::PlaneCylinder: return "PlaneCylinder";
case DistanceType::PlaneSphere: return "PlaneSphere";
case DistanceType::PlaneCone: return "PlaneCone";
case DistanceType::PlaneTorus: return "PlaneTorus";
case DistanceType::CylinderCylinder: return "CylinderCylinder";
case DistanceType::CylinderSphere: return "CylinderSphere";
case DistanceType::CylinderCone: return "CylinderCone";
case DistanceType::CylinderTorus: return "CylinderTorus";
case DistanceType::ConeCone: return "ConeCone";
case DistanceType::ConeTorus: return "ConeTorus";
case DistanceType::ConeSphere: return "ConeSphere";
case DistanceType::TorusTorus: return "TorusTorus";
case DistanceType::TorusSphere: return "TorusSphere";
case DistanceType::SphereSphere: return "SphereSphere";
case DistanceType::PointPlane: return "PointPlane";
case DistanceType::PointCylinder: return "PointCylinder";
case DistanceType::PointSphere: return "PointSphere";
case DistanceType::PointCone: return "PointCone";
case DistanceType::PointTorus: return "PointTorus";
case DistanceType::LinePlane: return "LinePlane";
case DistanceType::LineCylinder: return "LineCylinder";
case DistanceType::LineSphere: return "LineSphere";
case DistanceType::LineCone: return "LineCone";
case DistanceType::LineTorus: return "LineTorus";
case DistanceType::CurvePlane: return "CurvePlane";
case DistanceType::CurveCylinder: return "CurveCylinder";
case DistanceType::CurveSphere: return "CurveSphere";
case DistanceType::CurveCone: return "CurveCone";
case DistanceType::CurveTorus: return "CurveTorus";
case DistanceType::PointLine: return "PointLine";
case DistanceType::PointCurve: return "PointCurve";
case DistanceType::Other: return "Other";
}
return "Unknown";
}
void swapJCS(const App::DocumentObject* joint)
{
if (!joint) {
@@ -173,6 +219,8 @@ DistanceType getDistanceType(App::DocumentObject* joint)
if (datum1 || datum2) {
if (datum1 && datum2) {
FC_LOG("Assembly : getDistanceType('" << joint->getFullName()
<< "') — datum+datum → PlanePlane");
return DistanceType::PlanePlane;
}
@@ -193,17 +241,22 @@ DistanceType getDistanceType(App::DocumentObject* joint)
if (datum1) {
swapJCS(joint); // move datum from Ref1 → Ref2
}
if (otherType == "Vertex") {
return DistanceType::PointPlane;
}
return DistanceType::LinePlane;
DistanceType result = (otherType == "Vertex")
? DistanceType::PointPlane : DistanceType::LinePlane;
FC_LOG("Assembly : getDistanceType('" << joint->getFullName()
<< "') — datum+" << otherType << ""
<< distanceTypeName(result)
<< (datum1 ? " (swapped)" : ""));
return result;
}
// Face + datum or unknown + datum → PlanePlane
// Datum on Reference1 for consistency with Face+Face path.
if (!datum1) {
swapJCS(joint); // move datum from Ref2 → Ref1
}
// Face + datum or unknown + datum → PlanePlane.
// No swap needed: PlanarConstraint is symmetric (uses both
// z_i and z_j), and preserving the original Reference order
// keeps the initial Placement values consistent so the solver
// stays in the correct orientation branch.
FC_LOG("Assembly : getDistanceType('" << joint->getFullName()
<< "') — datum+" << otherType << " → PlanePlane");
return DistanceType::PlanePlane;
}

View File

@@ -148,6 +148,7 @@ AssemblyExport double getFaceRadius(const App::DocumentObject* obj, const std::s
AssemblyExport double getEdgeRadius(const App::DocumentObject* obj, const std::string& elName);
AssemblyExport DistanceType getDistanceType(App::DocumentObject* joint);
AssemblyExport const char* distanceTypeName(DistanceType dt);
AssemblyExport JointGroup* getJointGroup(const App::Part* part);
AssemblyExport std::vector<App::DocumentObject*> getAssemblyComponents(const AssemblyObject* assembly);

View File

@@ -191,6 +191,90 @@ class TestAssemblyOriginPlanes(unittest.TestCase):
result = self.assembly.solve()
self.assertEqual(result, 0, "Solve should succeed with origin plane joint")
# ── Distance joint to datum plane tests ────────────────────────
def test_distance_vertex_to_datum_plane_solves(self):
"""Distance(0) joint: vertex → datum plane solves and pins position."""
origin = self._get_origin()
xy = origin.getXY() # Top (Z normal)
xz = origin.getXZ() # Front (Y normal)
yz = origin.getYZ() # Right (X normal)
box = self._make_box(50, 50, 50)
# 3 Distance joints, each vertex→datum, distance=0.
# This should pin the box's Vertex1 (corner at local 0,0,0) to the
# origin, giving 3 PointInPlane constraints (1 residual each = 3 total).
for plane in [xy, xz, yz]:
joint = self._make_joint(
5, # Distance
[box, ["Vertex1", "Vertex1"]],
[origin, [plane.Name + ".", plane.Name + "."]],
)
joint.Distance = 0.0
result = self.assembly.solve()
self.assertEqual(
result, 0, "Solve should succeed for vertex→datum Distance joints"
)
# The box's Vertex1 (at local 0,0,0) should be at the origin.
v = box.Placement.Base
self.assertAlmostEqual(v.x, 0.0, places=2, msg="X should be pinned to 0")
self.assertAlmostEqual(v.y, 0.0, places=2, msg="Y should be pinned to 0")
self.assertAlmostEqual(v.z, 0.0, places=2, msg="Z should be pinned to 0")
def test_distance_vertex_to_datum_plane_preserves_orientation(self):
"""Distance(0) vertex→datum should not constrain orientation."""
origin = self._get_origin()
xy = origin.getXY()
xz = origin.getXZ()
yz = origin.getYZ()
# Start box with a known rotation (45° about Z).
rot = App.Rotation(App.Vector(0, 0, 1), 45)
box = self._make_box(50, 50, 50)
box.Placement = App.Placement(App.Vector(50, 50, 50), rot)
for plane in [xy, xz, yz]:
joint = self._make_joint(
5,
[box, ["Vertex1", "Vertex1"]],
[origin, [plane.Name + ".", plane.Name + "."]],
)
joint.Distance = 0.0
self.assembly.solve()
# 3 PointInPlane constraints pin position (3 DOF) but leave
# orientation free (3 DOF). The solver should keep the original
# orientation since it's the lowest-energy solution from the
# initial placement.
dof = self.assembly.getLastDoF()
self.assertEqual(
dof, 3, "3 PointInPlane constraints should leave 3 DOF (orientation)"
)
def test_distance_face_to_datum_plane_solves(self):
"""Distance(0) joint: face → datum plane solves (PlanePlane/Planar)."""
origin = self._get_origin()
xy = origin.getXY()
box = self._make_box(0, 0, 50)
# Face1 is the -Z face of a Part::Box.
joint = self._make_joint(
5,
[box, ["Face1", "Vertex1"]],
[origin, [xy.Name + ".", xy.Name + "."]],
)
joint.Distance = 0.0
result = self.assembly.solve()
self.assertEqual(
result, 0, "Solve should succeed for face→datum Distance joint"
)
# ── Round-trip test ──────────────────────────────────────────────
def test_save_load_preserves_labels(self):