diff --git a/src/Mod/Assembly/App/AppAssembly.cpp b/src/Mod/Assembly/App/AppAssembly.cpp index 118e18f029..b378c23f98 100644 --- a/src/Mod/Assembly/App/AppAssembly.cpp +++ b/src/Mod/Assembly/App/AppAssembly.cpp @@ -26,6 +26,8 @@ #include #include +#include + #include "AssemblyObject.h" #include "AssemblyLink.h" #include "BomObject.h" @@ -54,6 +56,10 @@ PyMOD_INIT_FUNC(AssemblyApp) } PyObject* mod = Assembly::initModule(); + + // Register the built-in OndselSolver adapter with the solver registry. + KCSolve::OndselAdapter::register_solver(); + Base::Console().log("Loading Assembly module... done\n"); diff --git a/src/Mod/Assembly/App/AssemblyObject.cpp b/src/Mod/Assembly/App/AssemblyObject.cpp index ca480c88a5..89400b1f3c 100644 --- a/src/Mod/Assembly/App/AssemblyObject.cpp +++ b/src/Mod/Assembly/App/AssemblyObject.cpp @@ -23,6 +23,7 @@ #include #include +#include #include #include @@ -43,39 +44,8 @@ #include #include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include +#include +#include #include "AssemblyLink.h" #include "AssemblyObject.h" @@ -87,19 +57,42 @@ FC_LOG_LEVEL_INIT("Assembly", true, true, true) using namespace Assembly; -using namespace MbD; - namespace PartApp = Part; +// ── Transform conversion helpers ─────────────────────────────────── + +static KCSolve::Transform placementToTransform(const Base::Placement& plc) +{ + KCSolve::Transform tf; + Base::Vector3d pos = plc.getPosition(); + tf.position = {pos.x, pos.y, pos.z}; + + // Base::Rotation(q0,q1,q2,q3) = (x,y,z,w) + // KCSolve::Transform quaternion = (w,x,y,z) + double q0, q1, q2, q3; + plc.getRotation().getValue(q0, q1, q2, q3); + tf.quaternion = {q3, q0, q1, q2}; + + return tf; +} + +static Base::Placement transformToPlacement(const KCSolve::Transform& tf) +{ + Base::Vector3d pos(tf.position[0], tf.position[1], tf.position[2]); + // KCSolve (w,x,y,z) → Base::Rotation(x,y,z,w) + Base::Rotation rot(tf.quaternion[1], tf.quaternion[2], tf.quaternion[3], tf.quaternion[0]); + return Base::Placement(pos, rot); +} + + // ================================ Assembly Object ============================ PROPERTY_SOURCE(Assembly::AssemblyObject, App::Part) AssemblyObject::AssemblyObject() - : mbdAssembly(std::make_shared()) - , bundleFixed(false) + : bundleFixed(false) , lastDoF(0) , lastHasConflict(false) , lastHasRedundancies(false) @@ -107,8 +100,6 @@ AssemblyObject::AssemblyObject() , lastHasMalformedConstraints(false) , lastSolverStatus(0) { - mbdAssembly->externalSystem->freecadAssemblyObject = this; - lastDoF = numberOfComponents() * 6; signalSolverUpdate(); } @@ -150,32 +141,47 @@ void AssemblyObject::onChanged(const App::Property* prop) App::Part::onChanged(prop); } + +// ── Solver integration ───────────────────────────────────────────── + +KCSolve::IKCSolver* AssemblyObject::getOrCreateSolver() +{ + if (!solver_) { + solver_ = KCSolve::SolverRegistry::instance().get("ondsel"); + } + return solver_.get(); +} + int AssemblyObject::solve(bool enableRedo, bool updateJCS) { ensureIdentityPlacements(); - mbdAssembly = makeMbdAssembly(); - objectPartMap.clear(); - motions.clear(); + auto* solver = getOrCreateSolver(); + if (!solver) { + FC_ERR("No solver available"); + lastSolverStatus = -1; + return -1; + } - auto groundedObjs = fixGroundedParts(); + partIdToObjs_.clear(); + objToPartId_.clear(); + + auto groundedObjs = getGroundedParts(); if (groundedObjs.empty()) { - // If no part fixed we can't solve. return -6; } std::vector joints = getJoints(updateJCS); - removeUnconnectedJoints(joints, groundedObjs); - jointParts(joints); + KCSolve::SolveContext ctx = buildSolveContext(joints); // Always save placements to enable orientation flip detection savePlacementsForUndo(); try { - mbdAssembly->runPreDrag(); - lastSolverStatus = 0; + lastResult_ = solver->solve(ctx); + lastSolverStatus = static_cast(lastResult_.status); } catch (const std::exception& e) { FC_ERR("Solve failed: " << e.what()); @@ -190,6 +196,11 @@ int AssemblyObject::solve(bool enableRedo, bool updateJCS) return -1; } + if (lastResult_.status == KCSolve::SolveStatus::Failed) { + updateSolveStatus(); + return -1; + } + // Validate that the solve didn't cause any parts to flip orientation if (!validateNewPlacements()) { // Restore previous placements - the solve found an invalid configuration @@ -220,106 +231,96 @@ void AssemblyObject::updateSolveStatus() //+1 because there's a grounded joint to origin lastDoF = (1 + numberOfComponents()) * 6; - if (!mbdAssembly || !mbdAssembly->mbdSystem) { + if (!solver_ || lastResult_.placements.empty()) { solve(); } - if (!mbdAssembly || !mbdAssembly->mbdSystem) { + if (!solver_) { return; } - // Helper lambda to clean up the joint name from the solver - auto cleanJointName = [](const std::string& rawName) -> std::string { - // rawName is like : /OndselAssembly/ground_moves#Joint001 - size_t hashPos = rawName.find_last_of('#'); - if (hashPos != std::string::npos) { - // Return the substring after the '#' - return rawName.substr(hashPos + 1); - } - return rawName; - }; - - - // Iterate through all joints and motions in the MBD system - mbdAssembly->mbdSystem->jointsMotionsDo([&](std::shared_ptr jm) { - if (!jm) { - return; - } - // Base::Console().warning("jm->name %s\n", jm->name); - bool isJointRedundant = false; - - jm->constraintsDo([&](std::shared_ptr con) { - if (!con) { - return; - } - - std::string spec = con->constraintSpec(); - // A constraint is redundant if its spec starts with "Redundant" - if (spec.rfind("Redundant", 0) == 0) { - isJointRedundant = true; - } - // Base::Console().warning(" - %s\n", spec); - --lastDoF; - }); - - const std::string fullName = cleanJointName(jm->name); - App::DocumentObject* docObj = getDocument()->getObject(fullName.c_str()); - - // We only care about objects that are actual joints in the FreeCAD document. - // This effectively filters out the grounding joints, which are named after parts. - if (!docObj || !docObj->getPropertyByName("Reference1")) { - return; - } - - if (isJointRedundant) { - // Check if this joint is already in the list to avoid duplicates - std::string objName = docObj->getNameInDocument(); - if (std::find(lastRedundantJoints.begin(), lastRedundantJoints.end(), objName) - == lastRedundantJoints.end()) { - lastRedundantJoints.push_back(objName); - } - } - }); - - // Update the summary boolean flag - if (!lastRedundantJoints.empty()) { - lastHasRedundancies = true; + // Use DOF from the solver result if available + if (lastResult_.dof >= 0) { + lastDoF = lastResult_.dof; } + // Process diagnostics from the solver result + for (const auto& diag : lastResult_.diagnostics) { + // Filter to only actual FreeCAD joint objects (not grounding joints) + App::DocumentObject* docObj = getDocument()->getObject(diag.constraint_id.c_str()); + if (!docObj || !docObj->getPropertyByName("Reference1")) { + continue; + } + + std::string objName = docObj->getNameInDocument(); + + switch (diag.kind) { + case KCSolve::ConstraintDiagnostic::Kind::Redundant: + if (std::find(lastRedundantJoints.begin(), lastRedundantJoints.end(), objName) + == lastRedundantJoints.end()) { + lastRedundantJoints.push_back(objName); + } + break; + case KCSolve::ConstraintDiagnostic::Kind::Conflicting: + if (std::find(lastConflictingJoints.begin(), lastConflictingJoints.end(), objName) + == lastConflictingJoints.end()) { + lastConflictingJoints.push_back(objName); + } + break; + case KCSolve::ConstraintDiagnostic::Kind::PartiallyRedundant: + if (std::find(lastPartialRedundantJoints.begin(), lastPartialRedundantJoints.end(), objName) + == lastPartialRedundantJoints.end()) { + lastPartialRedundantJoints.push_back(objName); + } + break; + case KCSolve::ConstraintDiagnostic::Kind::Malformed: + if (std::find(lastMalformedJoints.begin(), lastMalformedJoints.end(), objName) + == lastMalformedJoints.end()) { + lastMalformedJoints.push_back(objName); + } + break; + } + } + + lastHasRedundancies = !lastRedundantJoints.empty(); + lastHasConflict = !lastConflictingJoints.empty(); + lastHasPartialRedundancies = !lastPartialRedundantJoints.empty(); + lastHasMalformedConstraints = !lastMalformedJoints.empty(); + signalSolverUpdate(); } int AssemblyObject::generateSimulation(App::DocumentObject* sim) { - mbdAssembly = makeMbdAssembly(); - objectPartMap.clear(); + auto* solver = getOrCreateSolver(); + if (!solver) { + return -1; + } - motions = getMotionsFromSimulation(sim); + partIdToObjs_.clear(); + objToPartId_.clear(); - auto groundedObjs = fixGroundedParts(); + auto groundedObjs = getGroundedParts(); if (groundedObjs.empty()) { - // If no part fixed we can't solve. return -6; } std::vector joints = getJoints(); - removeUnconnectedJoints(joints, groundedObjs); - jointParts(joints); - - create_mbdSimulationParameters(sim); + KCSolve::SolveContext ctx = buildSolveContext(joints, true, sim); try { - mbdAssembly->runKINEMATIC(); + lastResult_ = solver->run_kinematic(ctx); } catch (...) { Base::Console().error("Generation of simulation failed\n"); - motions.clear(); return -1; } - motions.clear(); + if (lastResult_.status == KCSolve::SolveStatus::Failed) { + return -1; + } return 0; } @@ -340,16 +341,16 @@ std::vector AssemblyObject::getMotionsFromSimulation(App:: int Assembly::AssemblyObject::updateForFrame(size_t index, bool updateJCS) { - if (!mbdAssembly) { + if (!solver_) { return -1; } - auto nfrms = mbdAssembly->numberOfFrames(); + auto nfrms = solver_->num_frames(); if (index >= nfrms) { return -1; } - mbdAssembly->updateForFrame(index); + lastResult_ = solver_->update_for_frame(index); setNewPlacements(); auto jointDocs = getJoints(updateJCS); redrawJointPlacements(jointDocs); @@ -358,13 +359,32 @@ int Assembly::AssemblyObject::updateForFrame(size_t index, bool updateJCS) size_t Assembly::AssemblyObject::numberOfFrames() { - return mbdAssembly->numberOfFrames(); + return solver_ ? solver_->num_frames() : 0; } void AssemblyObject::preDrag(std::vector dragParts) { bundleFixed = true; - solve(); + + auto* solver = getOrCreateSolver(); + if (!solver) { + bundleFixed = false; + return; + } + + partIdToObjs_.clear(); + objToPartId_.clear(); + + auto groundedObjs = getGroundedParts(); + if (groundedObjs.empty()) { + bundleFixed = false; + return; + } + + std::vector joints = getJoints(); + removeUnconnectedJoints(joints, groundedObjs); + + KCSolve::SolveContext ctx = buildSolveContext(joints); bundleFixed = false; draggedParts.clear(); @@ -380,60 +400,68 @@ void AssemblyObject::preDrag(std::vector dragParts) } // Some objects have been bundled, we don't want to add these to dragged parts - Base::Placement plc; - for (auto& pair : objectPartMap) { - App::DocumentObject* parti = pair.first; - if (parti != part) { - continue; - } - plc = pair.second.offsetPlc; + auto it = objToPartId_.find(part); + if (it == objToPartId_.end()) { + continue; } - if (!plc.isIdentity()) { - // If not identity, then it's a bundled object. Some bundled objects may - // have identity placement if they have the same position as the main object of - // the bundle. But they're not going to be a problem. + + // Check if this is a bundled (non-primary) object + const auto& mappings = partIdToObjs_[it->second]; + bool isBundled = false; + for (const auto& m : mappings) { + if (m.obj == part && !m.offset.isIdentity()) { + isBundled = true; + break; + } + } + if (isBundled) { continue; } draggedParts.push_back(part); } + + // Build drag part IDs for the solver + std::vector dragPartIds; + for (auto* part : draggedParts) { + auto idIt = objToPartId_.find(part); + if (idIt != objToPartId_.end()) { + dragPartIds.push_back(idIt->second); + } + } + + savePlacementsForUndo(); + + try { + lastResult_ = solver->pre_drag(ctx, dragPartIds); + setNewPlacements(); + } + catch (...) { + // If pre_drag fails, we still need to be in a valid state + } } void AssemblyObject::doDragStep() { try { - std::vector> dragMbdParts; + std::vector dragPlacements; for (auto& part : draggedParts) { if (!part) { continue; } - auto mbdPart = getMbDPart(part); - dragMbdParts.push_back(mbdPart); + auto idIt = objToPartId_.find(part); + if (idIt == objToPartId_.end()) { + continue; + } - // Update the MBD part's position Base::Placement plc = getPlacementFromProp(part, "Placement"); - Base::Vector3d pos = plc.getPosition(); - mbdPart->updateMbDFromPosition3D( - std::make_shared>(ListD {pos.x, pos.y, pos.z}) - ); - - // Update the MBD part's rotation - Base::Rotation rot = plc.getRotation(); - Base::Matrix4D mat; - rot.getValue(mat); - Base::Vector3d r0 = mat.getRow(0); - Base::Vector3d r1 = mat.getRow(1); - Base::Vector3d r2 = mat.getRow(2); - mbdPart->updateMbDFromRotationMatrix(r0.x, r0.y, r0.z, r1.x, r1.y, r1.z, r2.x, r2.y, r2.z); + dragPlacements.push_back({idIt->second, placementToTransform(plc)}); } - // Timing mbdAssembly->runDragStep() - auto dragPartsVec = std::make_shared>>(dragMbdParts); - mbdAssembly->runDragStep(dragPartsVec); + lastResult_ = solver_->drag_step(dragPlacements); - // Timing the validation and placement setting if (validateNewPlacements()) { setNewPlacements(); @@ -451,23 +479,6 @@ void AssemblyObject::doDragStep() } } -Base::Placement AssemblyObject::getMbdPlacement(std::shared_ptr mbdPart) -{ - if (!mbdPart) { - return Base::Placement(); - } - - double x, y, z; - mbdPart->getPosition3D(x, y, z); - Base::Vector3d pos = Base::Vector3d(x, y, z); - - double q0, q1, q2, q3; - mbdPart->getQuarternions(q3, q0, q1, q2); - Base::Rotation rot = Base::Rotation(q0, q1, q2, q3); - - return Base::Placement(pos, rot); -} - bool AssemblyObject::validateNewPlacements() { // First we check if a grounded object has moved. It can happen that they flip. @@ -479,12 +490,26 @@ bool AssemblyObject::validateNewPlacements() if (propPlacement) { Base::Placement oldPlc = propPlacement->getValue(); - auto it = objectPartMap.find(obj); - if (it != objectPartMap.end()) { - std::shared_ptr mbdPart = it->second.part; - Base::Placement newPlacement = getMbdPlacement(mbdPart); - if (!it->second.offsetPlc.isIdentity()) { - newPlacement = newPlacement * it->second.offsetPlc; + auto idIt = objToPartId_.find(obj); + if (idIt == objToPartId_.end()) { + continue; + } + + // Find the new placement from lastResult_ + for (const auto& pr : lastResult_.placements) { + if (pr.id != idIt->second) { + continue; + } + + Base::Placement newPlacement = transformToPlacement(pr.placement); + + // Apply bundle offset if present + const auto& mappings = partIdToObjs_[pr.id]; + for (const auto& m : mappings) { + if (m.obj == obj && !m.offset.isIdentity()) { + newPlacement = newPlacement * m.offset; + break; + } } if (!oldPlc.isSame(newPlacement, Precision::Confusion())) { @@ -494,57 +519,66 @@ bool AssemblyObject::validateNewPlacements() ); return false; } + break; } } } // Check if any part has flipped orientation (rotation > 90 degrees from original) - // This prevents joints from "breaking" when the solver finds an alternate configuration for (const auto& savedPair : previousPositions) { App::DocumentObject* obj = savedPair.first; if (!obj) { continue; } - auto it = objectPartMap.find(obj); - if (it == objectPartMap.end()) { + auto idIt = objToPartId_.find(obj); + if (idIt == objToPartId_.end()) { continue; } - std::shared_ptr mbdPart = it->second.part; - if (!mbdPart) { - continue; - } + // Find the new placement from lastResult_ + for (const auto& pr : lastResult_.placements) { + if (pr.id != idIt->second) { + continue; + } - Base::Placement newPlacement = getMbdPlacement(mbdPart); - if (!it->second.offsetPlc.isIdentity()) { - newPlacement = newPlacement * it->second.offsetPlc; - } + Base::Placement newPlacement = transformToPlacement(pr.placement); - const Base::Placement& oldPlc = savedPair.second; + // Apply bundle offset if present + const auto& mappings = partIdToObjs_[pr.id]; + for (const auto& m : mappings) { + if (m.obj == obj && !m.offset.isIdentity()) { + newPlacement = newPlacement * m.offset; + break; + } + } - // Calculate the rotation difference between old and new orientations - Base::Rotation oldRot = oldPlc.getRotation(); - Base::Rotation newRot = newPlacement.getRotation(); + const Base::Placement& oldPlc = savedPair.second; - // Get the relative rotation: how much did the part rotate? - Base::Rotation relativeRot = newRot * oldRot.inverse(); + // Calculate the rotation difference between old and new orientations + Base::Rotation oldRot = oldPlc.getRotation(); + Base::Rotation newRot = newPlacement.getRotation(); - // Get the angle of this rotation - Base::Vector3d axis; - double angle; - relativeRot.getRawValue(axis, angle); + // Get the relative rotation: how much did the part rotate? + Base::Rotation relativeRot = newRot * oldRot.inverse(); - // If the part rotated more than 90 degrees, consider it a flip - // Use 91 degrees to allow for small numerical errors - constexpr double maxAngle = 91.0 * M_PI / 180.0; - if (std::abs(angle) > maxAngle) { - Base::Console().warning( - "Assembly : Ignoring bad solve, part (%s) flipped orientation (%.1f degrees).\n", - obj->getFullLabel(), - std::abs(angle) * 180.0 / M_PI - ); - return false; + // Get the angle of this rotation + Base::Vector3d axis; + double angle; + relativeRot.getRawValue(axis, angle); + + // If the part rotated more than 90 degrees, consider it a flip + // Use 91 degrees to allow for small numerical errors + constexpr double maxAngle = 91.0 * M_PI / 180.0; + if (std::abs(angle) > maxAngle) { + Base::Console().warning( + "Assembly : Ignoring bad solve, part (%s) flipped orientation (%.1f degrees).\n", + obj->getFullLabel(), + std::abs(angle) * 180.0 / M_PI + ); + return false; + } + break; } } @@ -553,7 +587,9 @@ bool AssemblyObject::validateNewPlacements() void AssemblyObject::postDrag() { - mbdAssembly->runPostDrag(); // Do this after last drag + if (solver_) { + solver_->post_drag(); + } purgeTouched(); } @@ -561,23 +597,20 @@ void AssemblyObject::savePlacementsForUndo() { previousPositions.clear(); - for (auto& pair : objectPartMap) { - App::DocumentObject* obj = pair.first; - if (!obj) { - continue; + for (const auto& [partId, mappings] : partIdToObjs_) { + for (const auto& mapping : mappings) { + App::DocumentObject* obj = mapping.obj; + if (!obj) { + continue; + } + + auto* propPlc = dynamic_cast(obj->getPropertyByName("Placement")); + if (!propPlc) { + continue; + } + + previousPositions.push_back({obj, propPlc->getValue()}); } - - std::pair savePair; - savePair.first = obj; - - // Check if the object has a "Placement" property - auto* propPlc = dynamic_cast(obj->getPropertyByName("Placement")); - if (!propPlc) { - continue; - } - savePair.second = propPlc->getValue(); - - previousPositions.push_back(savePair); } } @@ -616,43 +649,61 @@ void AssemblyObject::clearUndo() void AssemblyObject::exportAsASMT(std::string fileName) { - mbdAssembly = makeMbdAssembly(); - objectPartMap.clear(); - fixGroundedParts(); + auto* solver = getOrCreateSolver(); + if (!solver) { + return; + } + partIdToObjs_.clear(); + objToPartId_.clear(); + + auto groundedObjs = getGroundedParts(); std::vector joints = getJoints(); + removeUnconnectedJoints(joints, groundedObjs); - jointParts(joints); + KCSolve::SolveContext ctx = buildSolveContext(joints); - mbdAssembly->outputFile(fileName); + try { + solver->solve(ctx); + } + catch (...) { + // Build anyway for export + } + + solver->export_native(fileName); } void AssemblyObject::setNewPlacements() { - for (auto& pair : objectPartMap) { - App::DocumentObject* obj = pair.first; - std::shared_ptr mbdPart = pair.second.part; - - if (!obj || !mbdPart) { + for (const auto& pr : lastResult_.placements) { + auto it = partIdToObjs_.find(pr.id); + if (it == partIdToObjs_.end()) { continue; } - // Check if the object has a "Placement" property - auto* propPlacement = dynamic_cast( - obj->getPropertyByName("Placement") - ); - if (!propPlacement) { - continue; - } + Base::Placement basePlc = transformToPlacement(pr.placement); + for (const auto& mapping : it->second) { + App::DocumentObject* obj = mapping.obj; + if (!obj) { + continue; + } - Base::Placement newPlacement = getMbdPlacement(mbdPart); - if (!pair.second.offsetPlc.isIdentity()) { - newPlacement = newPlacement * pair.second.offsetPlc; - } - if (!propPlacement->getValue().isSame(newPlacement)) { - propPlacement->setValue(newPlacement); - obj->purgeTouched(); + auto* propPlacement = dynamic_cast( + obj->getPropertyByName("Placement") + ); + if (!propPlacement) { + continue; + } + + Base::Placement newPlacement = basePlc; + if (!mapping.offset.isIdentity()) { + newPlacement = basePlc * mapping.offset; + } + if (!propPlacement->getValue().isSame(newPlacement)) { + propPlacement->setValue(newPlacement); + obj->purgeTouched(); + } } } } @@ -698,20 +749,726 @@ void AssemblyObject::redrawJointPlacement(App::DocumentObject* joint) } } -std::shared_ptr AssemblyObject::makeMbdAssembly() + +// ── SolveContext building ────────────────────────────────────────── + +std::string AssemblyObject::registerPart(App::DocumentObject* obj) { - auto assembly = CREATE::With(); - assembly->externalSystem->freecadAssemblyObject = this; - assembly->setName("OndselAssembly"); + // Check if already registered + auto it = objToPartId_.find(obj); + if (it != objToPartId_.end()) { + return it->second; + } - ParameterGrp::handle hPgr = App::GetApplication().GetParameterGroupByPath( - "User parameter:BaseApp/Preferences/Mod/Assembly" - ); + std::string partId = obj->getFullName(); + Base::Placement plc = getPlacementFromProp(obj, "Placement"); - assembly->setDebug(hPgr->GetBool("LogSolverDebug", false)); - return assembly; + objToPartId_[obj] = partId; + partIdToObjs_[partId].push_back({obj, Base::Placement()}); + + // When bundling fixed joints, recursively discover connected parts + if (bundleFixed) { + auto addConnectedFixedParts = [&](App::DocumentObject* currentPart, auto& self) -> void { + std::vector joints = getJointsOfPart(currentPart); + for (auto* joint : joints) { + JointType jointType = getJointType(joint); + if (jointType == JointType::Fixed) { + App::DocumentObject* part1 = getMovingPartFromRef(joint, "Reference1"); + App::DocumentObject* part2 = getMovingPartFromRef(joint, "Reference2"); + App::DocumentObject* partToAdd = currentPart == part1 ? part2 : part1; + + if (objToPartId_.find(partToAdd) != objToPartId_.end()) { + continue; // already registered + } + + Base::Placement plci = getPlacementFromProp(partToAdd, "Placement"); + Base::Placement offset = plc.inverse() * plci; + + objToPartId_[partToAdd] = partId; + partIdToObjs_[partId].push_back({partToAdd, offset}); + + self(partToAdd, self); + } + } + }; + + addConnectedFixedParts(obj, addConnectedFixedParts); + } + + return partId; } +KCSolve::SolveContext AssemblyObject::buildSolveContext( + const std::vector& joints, + bool forSimulation, + App::DocumentObject* sim +) +{ + KCSolve::SolveContext ctx; + ctx.bundle_fixed = bundleFixed; + + // ── Parts: register grounded parts ───────────────────────────── + + auto groundedObjs = getGroundedParts(); + for (auto* obj : groundedObjs) { + if (!obj) { + continue; + } + + std::string partId = registerPart(obj); + Base::Placement plc = getPlacementFromProp(obj, "Placement"); + + KCSolve::Part part; + part.id = partId; + part.placement = placementToTransform(plc); + part.mass = getObjMass(obj); + part.grounded = true; + ctx.parts.push_back(std::move(part)); + } + + // ── Constraints: process each joint ──────────────────────────── + + // Collect motions for simulation + std::vector motionObjs; + if (forSimulation && sim) { + motionObjs = getMotionsFromSimulation(sim); + } + + for (auto* joint : joints) { + if (!joint) { + continue; + } + + JointType jointType = getJointType(joint); + + // When bundling fixed joints, skip Fixed type (parts are already bundled) + if (bundleFixed && jointType == JointType::Fixed) { + continue; + } + + // Determine BaseJointKind and params + KCSolve::BaseJointKind kind; + std::vector params; + + switch (jointType) { + case JointType::Fixed: + kind = KCSolve::BaseJointKind::Fixed; + break; + case JointType::Revolute: + kind = KCSolve::BaseJointKind::Revolute; + break; + case JointType::Cylindrical: + kind = KCSolve::BaseJointKind::Cylindrical; + break; + case JointType::Slider: + kind = KCSolve::BaseJointKind::Slider; + break; + case JointType::Ball: + kind = KCSolve::BaseJointKind::Ball; + break; + case JointType::Parallel: + kind = KCSolve::BaseJointKind::Parallel; + break; + case JointType::Perpendicular: + kind = KCSolve::BaseJointKind::Perpendicular; + break; + case JointType::Angle: { + double angle = fabs(Base::toRadians(getJointAngle(joint))); + if (fmod(angle, 2 * std::numbers::pi) < Precision::Confusion()) { + kind = KCSolve::BaseJointKind::Parallel; + } + else { + kind = KCSolve::BaseJointKind::Angle; + params.push_back(angle); + } + break; + } + case JointType::RackPinion: { + kind = KCSolve::BaseJointKind::RackPinion; + params.push_back(getJointDistance(joint)); + break; + } + case JointType::Screw: { + int slidingIndex = slidingPartIndex(joint); + if (slidingIndex == 0) { + continue; // invalid — needs a slider + } + if (slidingIndex != 1) { + swapJCS(joint); + } + kind = KCSolve::BaseJointKind::Screw; + params.push_back(getJointDistance(joint)); + break; + } + case JointType::Gears: { + kind = KCSolve::BaseJointKind::Gear; + params.push_back(getJointDistance(joint)); + params.push_back(getJointDistance2(joint)); + break; + } + case JointType::Belt: { + kind = KCSolve::BaseJointKind::Gear; + params.push_back(getJointDistance(joint)); + params.push_back(-getJointDistance2(joint)); + break; + } + case JointType::Distance: { + // Decompose based on geometry classification + DistanceType distType = getDistanceType(joint); + std::string elt1 = getElementFromProp(joint, "Reference1"); + std::string elt2 = getElementFromProp(joint, "Reference2"); + auto* obj1 = getLinkedObjFromRef(joint, "Reference1"); + auto* obj2 = getLinkedObjFromRef(joint, "Reference2"); + double distance = getJointDistance(joint); + + switch (distType) { + case DistanceType::PointPoint: + if (distance < Precision::Confusion()) { + kind = KCSolve::BaseJointKind::Coincident; + } + else { + kind = KCSolve::BaseJointKind::DistancePointPoint; + params.push_back(distance); + } + break; + + case DistanceType::LineLine: + kind = KCSolve::BaseJointKind::Concentric; + params.push_back(distance); + break; + case DistanceType::LineCircle: + kind = KCSolve::BaseJointKind::Concentric; + params.push_back(distance + getEdgeRadius(obj2, elt2)); + break; + case DistanceType::CircleCircle: + kind = KCSolve::BaseJointKind::Concentric; + params.push_back(distance + getEdgeRadius(obj1, elt1) + getEdgeRadius(obj2, elt2)); + break; + + case DistanceType::PlanePlane: + kind = KCSolve::BaseJointKind::Planar; + params.push_back(distance); + break; + case DistanceType::PlaneCylinder: + kind = KCSolve::BaseJointKind::LineInPlane; + params.push_back(distance + getFaceRadius(obj2, elt2)); + break; + case DistanceType::PlaneSphere: + kind = KCSolve::BaseJointKind::PointInPlane; + params.push_back(distance + getFaceRadius(obj2, elt2)); + break; + case DistanceType::PlaneTorus: + kind = KCSolve::BaseJointKind::Planar; + params.push_back(distance); + break; + + case DistanceType::CylinderCylinder: + kind = KCSolve::BaseJointKind::Concentric; + params.push_back(distance + getFaceRadius(obj1, elt1) + getFaceRadius(obj2, elt2)); + break; + case DistanceType::CylinderSphere: + kind = KCSolve::BaseJointKind::DistanceCylSph; + params.push_back(distance + getFaceRadius(obj1, elt1) + getFaceRadius(obj2, elt2)); + break; + case DistanceType::CylinderTorus: + kind = KCSolve::BaseJointKind::Concentric; + params.push_back(distance + getFaceRadius(obj1, elt1) + getFaceRadius(obj2, elt2)); + break; + + case DistanceType::TorusTorus: + kind = KCSolve::BaseJointKind::Planar; + params.push_back(distance); + break; + case DistanceType::TorusSphere: + kind = KCSolve::BaseJointKind::DistanceCylSph; + params.push_back(distance + getFaceRadius(obj1, elt1) + getFaceRadius(obj2, elt2)); + break; + case DistanceType::SphereSphere: + kind = KCSolve::BaseJointKind::DistancePointPoint; + params.push_back(distance + getFaceRadius(obj1, elt1) + getFaceRadius(obj2, elt2)); + break; + + case DistanceType::PointPlane: + kind = KCSolve::BaseJointKind::PointInPlane; + params.push_back(distance); + break; + case DistanceType::PointCylinder: + kind = KCSolve::BaseJointKind::DistanceCylSph; + params.push_back(distance + getFaceRadius(obj1, elt1)); + break; + case DistanceType::PointSphere: + kind = KCSolve::BaseJointKind::DistancePointPoint; + params.push_back(distance + getFaceRadius(obj1, elt1)); + break; + + case DistanceType::LinePlane: + kind = KCSolve::BaseJointKind::LineInPlane; + params.push_back(distance); + break; + + case DistanceType::PointLine: + kind = KCSolve::BaseJointKind::DistanceCylSph; + params.push_back(distance); + break; + case DistanceType::PointCurve: + kind = KCSolve::BaseJointKind::PointInPlane; + params.push_back(distance); + break; + + default: + kind = KCSolve::BaseJointKind::Planar; + params.push_back(distance); + break; + } + break; + } + default: + continue; + } + + // Validate the joint (skip self-referential bundled parts) + if (!isJointValid(joint)) { + continue; + } + + // Compute marker transforms + std::string partIdI, partIdJ; + KCSolve::Transform markerI, markerJ; + + if (jointType == JointType::RackPinion) { + auto rp = computeRackPinionMarkers(joint); + partIdI = rp.partIdI; + markerI = rp.markerI; + partIdJ = rp.partIdJ; + markerJ = rp.markerJ; + + if (partIdI.empty() || partIdJ.empty()) { + continue; + } + } + else { + // Resolve part IDs from joint references + App::DocumentObject* part1 = getMovingPartFromRef(joint, "Reference1"); + App::DocumentObject* part2 = getMovingPartFromRef(joint, "Reference2"); + if (!part1 || !part2) { + continue; + } + + // Ensure both parts are registered + partIdI = registerPart(part1); + partIdJ = registerPart(part2); + + markerI = computeMarkerTransform(joint, "Reference1", "Placement1"); + markerJ = computeMarkerTransform(joint, "Reference2", "Placement2"); + } + + // Build the constraint + KCSolve::Constraint c; + c.id = joint->getNameInDocument(); + c.part_i = partIdI; + c.marker_i = markerI; + c.part_j = partIdJ; + c.marker_j = markerJ; + c.type = kind; + c.params = std::move(params); + + // Add limits (only if not in simulation mode — motions may clash) + if (motionObjs.empty()) { + if (jointType == JointType::Slider || jointType == JointType::Cylindrical) { + auto* pLenMin = dynamic_cast(joint->getPropertyByName("LengthMin")); + auto* pLenMax = dynamic_cast(joint->getPropertyByName("LengthMax")); + auto* pMinEnabled = dynamic_cast( + joint->getPropertyByName("EnableLengthMin") + ); + auto* pMaxEnabled = dynamic_cast( + joint->getPropertyByName("EnableLengthMax") + ); + + if (pLenMin && pLenMax && pMinEnabled && pMaxEnabled) { + bool minEnabled = pMinEnabled->getValue(); + bool maxEnabled = pMaxEnabled->getValue(); + double minLength = pLenMin->getValue(); + double maxLength = pLenMax->getValue(); + + if ((minLength > maxLength) && minEnabled && maxEnabled) { + pLenMin->setValue(maxLength); + pLenMax->setValue(minLength); + minLength = maxLength; + maxLength = pLenMax->getValue(); + + pMinEnabled->setValue(maxEnabled); + pMaxEnabled->setValue(minEnabled); + minEnabled = maxEnabled; + maxEnabled = pMaxEnabled->getValue(); + } + + if (minEnabled) { + c.limits.push_back({ + KCSolve::Constraint::Limit::Kind::TranslationMin, + minLength, + 1.0e-9 + }); + } + if (maxEnabled) { + c.limits.push_back({ + KCSolve::Constraint::Limit::Kind::TranslationMax, + maxLength, + 1.0e-9 + }); + } + } + } + if (jointType == JointType::Revolute || jointType == JointType::Cylindrical) { + auto* pRotMin = dynamic_cast(joint->getPropertyByName("AngleMin")); + auto* pRotMax = dynamic_cast(joint->getPropertyByName("AngleMax")); + auto* pMinEnabled = dynamic_cast( + joint->getPropertyByName("EnableAngleMin") + ); + auto* pMaxEnabled = dynamic_cast( + joint->getPropertyByName("EnableAngleMax") + ); + + if (pRotMin && pRotMax && pMinEnabled && pMaxEnabled) { + bool minEnabled = pMinEnabled->getValue(); + bool maxEnabled = pMaxEnabled->getValue(); + double minAngle = pRotMin->getValue(); + double maxAngle = pRotMax->getValue(); + + if ((minAngle > maxAngle) && minEnabled && maxEnabled) { + pRotMin->setValue(maxAngle); + pRotMax->setValue(minAngle); + minAngle = maxAngle; + maxAngle = pRotMax->getValue(); + + pMinEnabled->setValue(maxEnabled); + pMaxEnabled->setValue(minEnabled); + minEnabled = maxEnabled; + maxEnabled = pMaxEnabled->getValue(); + } + + if (minEnabled) { + c.limits.push_back({ + KCSolve::Constraint::Limit::Kind::RotationMin, + minAngle, + 1.0e-9 + }); + } + if (maxEnabled) { + c.limits.push_back({ + KCSolve::Constraint::Limit::Kind::RotationMax, + maxAngle, + 1.0e-9 + }); + } + } + } + } + + ctx.constraints.push_back(std::move(c)); + + // Add motions for simulation + if (forSimulation) { + std::vector done; + for (auto* motion : motionObjs) { + if (std::ranges::find(done, motion) != done.end()) { + continue; + } + + auto* pJoint = dynamic_cast(motion->getPropertyByName("Joint")); + if (!pJoint) { + continue; + } + App::DocumentObject* motionJoint = pJoint->getValue(); + if (joint != motionJoint) { + continue; + } + + auto* pType = dynamic_cast(motion->getPropertyByName("MotionType")); + auto* pFormula = dynamic_cast(motion->getPropertyByName("Formula")); + if (!pType || !pFormula) { + continue; + } + std::string formula = pFormula->getValue(); + if (formula.empty()) { + continue; + } + std::string motionType = pType->getValueAsString(); + + // Check for paired motion (cylindrical joints can have both angular + linear) + for (auto* motion2 : motionObjs) { + auto* pJoint2 = dynamic_cast(motion2->getPropertyByName("Joint")); + if (!pJoint2) { + continue; + } + App::DocumentObject* motionJoint2 = pJoint2->getValue(); + if (joint != motionJoint2 || motion2 == motion) { + continue; + } + + auto* pType2 = dynamic_cast( + motion2->getPropertyByName("MotionType") + ); + auto* pFormula2 = dynamic_cast(motion2->getPropertyByName("Formula")); + if (!pType2 || !pFormula2) { + continue; + } + std::string formula2 = pFormula2->getValue(); + if (formula2.empty()) { + continue; + } + std::string motionType2 = pType2->getValueAsString(); + if (motionType2 == motionType) { + continue; + } + + // Two different motion types on same joint → General motion + KCSolve::MotionDef md; + md.kind = KCSolve::MotionDef::Kind::General; + md.joint_id = joint->getNameInDocument(); + md.marker_i = ""; // Adapter resolves from joint_id + md.marker_j = ""; + md.rotation_expr = motionType == "Angular" ? formula : formula2; + md.translation_expr = motionType == "Angular" ? formula2 : formula; + ctx.motions.push_back(std::move(md)); + + done.push_back(motion2); + } + + // Single motion + KCSolve::MotionDef md; + md.joint_id = joint->getNameInDocument(); + md.marker_i = ""; + md.marker_j = ""; + if (motionType == "Angular") { + md.kind = KCSolve::MotionDef::Kind::Rotational; + md.rotation_expr = formula; + } + else { + md.kind = KCSolve::MotionDef::Kind::Translational; + md.translation_expr = formula; + } + ctx.motions.push_back(std::move(md)); + } + } + } + + // ── Parts: ensure all referenced parts are in the context ────── + + // Some parts may have been registered during constraint processing + // but not yet added to ctx.parts + for (const auto& [partId, mappings] : partIdToObjs_) { + bool alreadyInCtx = false; + for (const auto& p : ctx.parts) { + if (p.id == partId) { + alreadyInCtx = true; + break; + } + } + if (!alreadyInCtx) { + App::DocumentObject* primaryObj = mappings[0].obj; + Base::Placement plc = getPlacementFromProp(primaryObj, "Placement"); + + KCSolve::Part part; + part.id = partId; + part.placement = placementToTransform(plc); + part.mass = getObjMass(primaryObj); + part.grounded = false; + ctx.parts.push_back(std::move(part)); + } + } + + // ── Simulation parameters ────────────────────────────────────── + + if (forSimulation && sim) { + auto valueOf = [](App::DocumentObject* docObj, const char* propName) { + auto* prop = dynamic_cast(docObj->getPropertyByName(propName)); + if (!prop) { + return 0.0; + } + return prop->getValue(); + }; + + KCSolve::SimulationParams sp; + sp.t_start = valueOf(sim, "aTimeStart"); + sp.t_end = valueOf(sim, "bTimeEnd"); + sp.h_out = valueOf(sim, "cTimeStepOutput"); + sp.h_min = 1.0e-9; + sp.h_max = 1.0; + sp.error_tol = valueOf(sim, "fGlobalErrorTolerance"); + ctx.simulation = sp; + } + + return ctx; +} + + +// ── Marker transform computation ─────────────────────────────────── + +KCSolve::Transform AssemblyObject::computeMarkerTransform( + App::DocumentObject* joint, + const char* propRefName, + const char* propPlcName +) +{ + App::DocumentObject* part = getMovingPartFromRef(joint, propRefName); + App::DocumentObject* obj = getObjFromJointRef(joint, propRefName); + + if (!part || !obj) { + Base::Console() + .warning("The property %s of Joint %s is bad.\n", propRefName, joint->getFullName()); + return KCSolve::Transform::identity(); + } + + Base::Placement plc = getPlacementFromProp(joint, propPlcName); + // Now we have plc which is the JCS placement, but its relative to the Object, not to the + // containing Part. + + if (obj->getNameInDocument() != part->getNameInDocument()) { + auto* ref = dynamic_cast(joint->getPropertyByName(propRefName)); + if (!ref) { + return KCSolve::Transform::identity(); + } + + Base::Placement obj_global_plc = getGlobalPlacement(obj, ref); + plc = obj_global_plc * plc; + + Base::Placement part_global_plc = getGlobalPlacement(part, ref); + plc = part_global_plc.inverse() * plc; + } + + // Apply bundle offset if present + auto idIt = objToPartId_.find(part); + if (idIt != objToPartId_.end()) { + const auto& mappings = partIdToObjs_[idIt->second]; + for (const auto& m : mappings) { + if (m.obj == part && !m.offset.isIdentity()) { + plc = m.offset * plc; + break; + } + } + } + + return placementToTransform(plc); +} + +AssemblyObject::RackPinionResult AssemblyObject::computeRackPinionMarkers(App::DocumentObject* joint) +{ + RackPinionResult result; + + // ASMT rack pinion joint must get the rack as I and pinion as J. + int slidingIndex = slidingPartIndex(joint); + if (slidingIndex == 0) { + return result; + } + + if (slidingIndex != 1) { + swapJCS(joint); // make sure that rack is first. + } + + App::DocumentObject* part1 = getMovingPartFromRef(joint, "Reference1"); + App::DocumentObject* obj1 = getObjFromJointRef(joint, "Reference1"); + Base::Placement plc1 = getPlacementFromProp(joint, "Placement1"); + + App::DocumentObject* obj2 = getObjFromJointRef(joint, "Reference2"); + Base::Placement plc2 = getPlacementFromProp(joint, "Placement2"); + + if (!part1 || !obj1) { + Base::Console().warning("Reference1 of Joint %s is bad.\n", joint->getFullName()); + return result; + } + + // Ensure parts are registered + result.partIdI = registerPart(part1); + + // For the pinion — use standard marker computation + result.markerJ = computeMarkerTransform(joint, "Reference2", "Placement2"); + + App::DocumentObject* part2 = getMovingPartFromRef(joint, "Reference2"); + if (part2) { + result.partIdJ = registerPart(part2); + } + + // For the rack — need to adjust placement so X axis aligns with slider axis + auto* ref1 = dynamic_cast(joint->getPropertyByName("Reference1")); + auto* ref2 = dynamic_cast(joint->getPropertyByName("Reference2")); + if (!ref1 || !ref2) { + return result; + } + + // Make the pinion plc relative to the rack placement + Base::Placement pinion_global_plc = getGlobalPlacement(obj2, ref2); + plc2 = pinion_global_plc * plc2; + Base::Placement rack_global_plc = getGlobalPlacement(obj1, ref1); + plc2 = rack_global_plc.inverse() * plc2; + + // The rot of the rack placement should be the same as the pinion, but with X axis along the + // slider axis. + Base::Rotation rot = plc2.getRotation(); + Base::Vector3d currentZAxis = rot.multVec(Base::Vector3d(0, 0, 1)); + Base::Vector3d currentXAxis = rot.multVec(Base::Vector3d(1, 0, 0)); + Base::Vector3d targetXAxis = plc1.getRotation().multVec(Base::Vector3d(0, 0, 1)); + + double yawAdjustment = currentXAxis.GetAngle(targetXAxis); + + Base::Vector3d crossProd = currentXAxis.Cross(targetXAxis); + if (currentZAxis * crossProd < 0) { + yawAdjustment = -yawAdjustment; + } + + Base::Rotation yawRotation(currentZAxis, yawAdjustment); + Base::Rotation adjustedRotation = rot * yawRotation; + plc1.setRotation(adjustedRotation); + + // Transform to part-relative coordinates (same as handleOneSideOfJoint end logic) + if (obj1->getNameInDocument() != part1->getNameInDocument()) { + plc1 = rack_global_plc * plc1; + Base::Placement part_global_plc = getGlobalPlacement(part1, ref1); + plc1 = part_global_plc.inverse() * plc1; + } + + // Apply bundle offset if present + auto idIt = objToPartId_.find(part1); + if (idIt != objToPartId_.end()) { + const auto& mappings = partIdToObjs_[idIt->second]; + for (const auto& m : mappings) { + if (m.obj == part1 && !m.offset.isIdentity()) { + plc1 = m.offset * plc1; + break; + } + } + } + + result.markerI = placementToTransform(plc1); + + return result; +} + +bool AssemblyObject::isJointValid(App::DocumentObject* joint) +{ + // When dragging, parts connected by fixed joints are bundled. + // A joint that references two parts in the same bundle is self-referential and must be skipped. + App::DocumentObject* part1 = getMovingPartFromRef(joint, "Reference1"); + App::DocumentObject* part2 = getMovingPartFromRef(joint, "Reference2"); + if (!part1 || !part2) { + return false; + } + + auto it1 = objToPartId_.find(part1); + auto it2 = objToPartId_.find(part2); + if (it1 != objToPartId_.end() && it2 != objToPartId_.end() && it1->second == it2->second) { + Base::Console().warning( + "Assembly: Ignoring joint (%s) because its parts are connected by a fixed " + "joint bundle. This joint is a conflicting or redundant constraint.\n", + joint->getFullLabel() + ); + return false; + } + return true; +} + + +// ── Joint / Part graph helpers (unchanged) ───────────────────────── + App::DocumentObject* AssemblyObject::getJointOfPartConnectingToGround( App::DocumentObject* part, std::string& name, @@ -952,50 +1709,6 @@ std::unordered_set AssemblyObject::getGroundedParts() return groundedSet; } -std::unordered_set AssemblyObject::fixGroundedParts() -{ - auto groundedParts = getGroundedParts(); - - for (auto obj : groundedParts) { - if (!obj) { - continue; - } - - Base::Placement plc = getPlacementFromProp(obj, "Placement"); - std::string str = obj->getFullName(); - fixGroundedPart(obj, plc, str); - } - return groundedParts; -} - -void AssemblyObject::fixGroundedPart(App::DocumentObject* obj, Base::Placement& plc, std::string& name) -{ - if (!obj) { - return; - } - - std::string markerName1 = "marker-" + obj->getFullName(); - auto mbdMarker1 = makeMbdMarker(markerName1, plc); - mbdAssembly->addMarker(mbdMarker1); - - std::shared_ptr mbdPart = getMbDPart(obj); - - std::string markerName2 = "FixingMarker"; - Base::Placement basePlc = Base::Placement(); - auto mbdMarker2 = makeMbdMarker(markerName2, basePlc); - mbdPart->addMarker(mbdMarker2); - - markerName1 = "/OndselAssembly/" + mbdMarker1->name; - markerName2 = "/OndselAssembly/" + mbdPart->name + "/" + mbdMarker2->name; - - auto mbdJoint = CREATE::With(); - mbdJoint->setName(name); - mbdJoint->setMarkerI(markerName1); - mbdJoint->setMarkerJ(markerName2); - - mbdAssembly->addJoint(mbdJoint); -} - bool AssemblyObject::isJointConnectingPartToGround(App::DocumentObject* joint, const char* propname) { if (!joint || !isJointTypeConnecting(joint)) { @@ -1213,641 +1926,6 @@ bool AssemblyObject::isPartConnected(App::DocumentObject* obj) return false; } -void AssemblyObject::jointParts(std::vector joints) -{ - for (auto* joint : joints) { - if (!joint) { - continue; - } - - std::vector> mbdJoints = makeMbdJoint(joint); - for (auto& mbdJoint : mbdJoints) { - mbdAssembly->addJoint(mbdJoint); - } - } -} - -void Assembly::AssemblyObject::create_mbdSimulationParameters(App::DocumentObject* sim) -{ - auto mbdSim = mbdAssembly->simulationParameters; - if (!sim) { - return; - } - auto valueOf = [](DocumentObject* docObj, const char* propName) { - auto* prop = dynamic_cast(docObj->getPropertyByName(propName)); - if (!prop) { - return 0.0; - } - return prop->getValue(); - }; - mbdSim->settstart(valueOf(sim, "aTimeStart")); - mbdSim->settend(valueOf(sim, "bTimeEnd")); - mbdSim->sethout(valueOf(sim, "cTimeStepOutput")); - mbdSim->sethmin(1.0e-9); - mbdSim->sethmax(1.0); - mbdSim->seterrorTol(valueOf(sim, "fGlobalErrorTolerance")); -} - -std::shared_ptr AssemblyObject::makeMbdJointOfType(App::DocumentObject* joint, JointType type) -{ - switch (type) { - case JointType::Fixed: - if (bundleFixed) { - return nullptr; - } - return CREATE::With(); - - case JointType::Revolute: - return CREATE::With(); - - case JointType::Cylindrical: - return CREATE::With(); - - case JointType::Slider: - return CREATE::With(); - - case JointType::Ball: - return CREATE::With(); - - case JointType::Distance: - return makeMbdJointDistance(joint); - - case JointType::Parallel: - return CREATE::With(); - - case JointType::Perpendicular: - return CREATE::With(); - - case JointType::Angle: { - double angle = fabs(Base::toRadians(getJointAngle(joint))); - if (fmod(angle, 2 * std::numbers::pi) < Precision::Confusion()) { - return CREATE::With(); - } - auto mbdJoint = CREATE::With(); - mbdJoint->theIzJz = angle; - return mbdJoint; - } - - case JointType::RackPinion: { - auto mbdJoint = CREATE::With(); - mbdJoint->pitchRadius = getJointDistance(joint); - return mbdJoint; - } - - case JointType::Screw: { - int slidingIndex = slidingPartIndex(joint); - if (slidingIndex == 0) { // invalid this joint needs a slider - return nullptr; - } - - if (slidingIndex != 1) { - swapJCS(joint); // make sure that sliding is first. - } - - auto mbdJoint = CREATE::With(); - mbdJoint->pitch = getJointDistance(joint); - return mbdJoint; - } - - case JointType::Gears: { - auto mbdJoint = CREATE::With(); - mbdJoint->radiusI = getJointDistance(joint); - mbdJoint->radiusJ = getJointDistance2(joint); - return mbdJoint; - } - - case JointType::Belt: { - auto mbdJoint = CREATE::With(); - mbdJoint->radiusI = getJointDistance(joint); - mbdJoint->radiusJ = -getJointDistance2(joint); - return mbdJoint; - } - - default: - return nullptr; - } -} - -std::shared_ptr AssemblyObject::makeMbdJointDistance(App::DocumentObject* joint) -{ - DistanceType type = getDistanceType(joint); - - std::string elt1 = getElementFromProp(joint, "Reference1"); - std::string elt2 = getElementFromProp(joint, "Reference2"); - auto* obj1 = getLinkedObjFromRef(joint, "Reference1"); - auto* obj2 = getLinkedObjFromRef(joint, "Reference2"); - - switch (type) { - case DistanceType::PointPoint: { - // Point to point distance, or ball joint if distance=0. - double distance = getJointDistance(joint); - if (distance < Precision::Confusion()) { - return CREATE::With(); - } - auto mbdJoint = CREATE::With(); - mbdJoint->distanceIJ = distance; - return mbdJoint; - } - - // Edge - edge cases - case DistanceType::LineLine: { - auto mbdJoint = CREATE::With(); - mbdJoint->distanceIJ = getJointDistance(joint); - return mbdJoint; - } - - case DistanceType::LineCircle: { - auto mbdJoint = CREATE::With(); - mbdJoint->distanceIJ = getJointDistance(joint) + getEdgeRadius(obj2, elt2); - return mbdJoint; - } - - case DistanceType::CircleCircle: { - auto mbdJoint = CREATE::With(); - mbdJoint->distanceIJ = getJointDistance(joint) + getEdgeRadius(obj1, elt1) - + getEdgeRadius(obj2, elt2); - return mbdJoint; - } - - // Face - Face cases - case DistanceType::PlanePlane: { - auto mbdJoint = CREATE::With(); - mbdJoint->offset = getJointDistance(joint); - return mbdJoint; - } - - case DistanceType::PlaneCylinder: { - auto mbdJoint = CREATE::With(); - mbdJoint->offset = getJointDistance(joint) + getFaceRadius(obj2, elt2); - return mbdJoint; - } - - case DistanceType::PlaneSphere: { - auto mbdJoint = CREATE::With(); - mbdJoint->offset = getJointDistance(joint) + getFaceRadius(obj2, elt2); - return mbdJoint; - } - - case DistanceType::PlaneTorus: { - auto mbdJoint = CREATE::With(); - mbdJoint->offset = getJointDistance(joint); - return mbdJoint; - } - - case DistanceType::CylinderCylinder: { - auto mbdJoint = CREATE::With(); - mbdJoint->distanceIJ = getJointDistance(joint) + getFaceRadius(obj1, elt1) - + getFaceRadius(obj2, elt2); - return mbdJoint; - } - - case DistanceType::CylinderSphere: { - auto mbdJoint = CREATE::With(); - mbdJoint->distanceIJ = getJointDistance(joint) + getFaceRadius(obj1, elt1) - + getFaceRadius(obj2, elt2); - return mbdJoint; - } - - case DistanceType::CylinderTorus: { - auto mbdJoint = CREATE::With(); - mbdJoint->distanceIJ = getJointDistance(joint) + getFaceRadius(obj1, elt1) - + getFaceRadius(obj2, elt2); - return mbdJoint; - } - - case DistanceType::TorusTorus: { - auto mbdJoint = CREATE::With(); - mbdJoint->offset = getJointDistance(joint); - return mbdJoint; - } - - case DistanceType::TorusSphere: { - auto mbdJoint = CREATE::With(); - mbdJoint->distanceIJ = getJointDistance(joint) + getFaceRadius(obj1, elt1) - + getFaceRadius(obj2, elt2); - return mbdJoint; - } - - case DistanceType::SphereSphere: { - auto mbdJoint = CREATE::With(); - mbdJoint->distanceIJ = getJointDistance(joint) + getFaceRadius(obj1, elt1) - + getFaceRadius(obj2, elt2); - return mbdJoint; - } - - // Point - Face cases - case DistanceType::PointPlane: { - auto mbdJoint = CREATE::With(); - mbdJoint->offset = getJointDistance(joint); - return mbdJoint; - } - - case DistanceType::PointCylinder: { - auto mbdJoint = CREATE::With(); - mbdJoint->distanceIJ = getJointDistance(joint) + getFaceRadius(obj1, elt1); - return mbdJoint; - } - - case DistanceType::PointSphere: { - auto mbdJoint = CREATE::With(); - mbdJoint->distanceIJ = getJointDistance(joint) + getFaceRadius(obj1, elt1); - return mbdJoint; - } - - // Edge - Face cases - case DistanceType::LinePlane: { - auto mbdJoint = CREATE::With(); - mbdJoint->offset = getJointDistance(joint); - return mbdJoint; - } - - // Point - Edge cases - case DistanceType::PointLine: { - auto mbdJoint = CREATE::With(); - mbdJoint->distanceIJ = getJointDistance(joint); - return mbdJoint; - } - - case DistanceType::PointCurve: { - // For other curves we do a point in plane-of-the-curve. - // Maybe it would be best tangent / distance to the conic? - // For arcs and circles we could use ASMTRevSphJoint. But is it better than - // pointInPlane? - auto mbdJoint = CREATE::With(); - mbdJoint->offset = getJointDistance(joint); - return mbdJoint; - } - - default: { - // by default we make a planar joint. - auto mbdJoint = CREATE::With(); - mbdJoint->offset = getJointDistance(joint); - return mbdJoint; - } - } -} - -std::vector> AssemblyObject::makeMbdJoint(App::DocumentObject* joint) -{ - if (!joint) { - return {}; - } - - JointType jointType = getJointType(joint); - - std::shared_ptr mbdJoint = makeMbdJointOfType(joint, jointType); - if (!mbdJoint || !isMbDJointValid(joint)) { - return {}; - } - - std::string fullMarkerNameI, fullMarkerNameJ; - if (jointType == JointType::RackPinion) { - getRackPinionMarkers(joint, fullMarkerNameI, fullMarkerNameJ); - } - else { - fullMarkerNameI = handleOneSideOfJoint(joint, "Reference1", "Placement1"); - fullMarkerNameJ = handleOneSideOfJoint(joint, "Reference2", "Placement2"); - } - if (fullMarkerNameI == "" || fullMarkerNameJ == "") { - return {}; - } - - mbdJoint->setName(joint->getFullName()); - mbdJoint->setMarkerI(fullMarkerNameI); - mbdJoint->setMarkerJ(fullMarkerNameJ); - - // Add limits if needed. We do not add if this is a simulation or their might clash. - if (motions.empty()) { - if (jointType == JointType::Slider || jointType == JointType::Cylindrical) { - auto* pLenMin = dynamic_cast(joint->getPropertyByName("LengthMin")); - auto* pLenMax = dynamic_cast(joint->getPropertyByName("LengthMax")); - auto* pMinEnabled = dynamic_cast( - joint->getPropertyByName("EnableLengthMin") - ); - auto* pMaxEnabled = dynamic_cast( - joint->getPropertyByName("EnableLengthMax") - ); - - if (pLenMin && pLenMax && pMinEnabled && pMaxEnabled) { // Make sure properties do exist - // Swap the values if necessary. - bool minEnabled = pMinEnabled->getValue(); - bool maxEnabled = pMaxEnabled->getValue(); - double minLength = pLenMin->getValue(); - double maxLength = pLenMax->getValue(); - - if ((minLength > maxLength) && minEnabled && maxEnabled) { - pLenMin->setValue(maxLength); - pLenMax->setValue(minLength); - minLength = maxLength; - maxLength = pLenMax->getValue(); - - pMinEnabled->setValue(maxEnabled); - pMaxEnabled->setValue(minEnabled); - minEnabled = maxEnabled; - maxEnabled = pMaxEnabled->getValue(); - } - - if (minEnabled) { - auto limit = ASMTTranslationLimit::With(); - limit->setName(joint->getFullName() + "-LimitLenMin"); - limit->setMarkerI(fullMarkerNameI); - limit->setMarkerJ(fullMarkerNameJ); - limit->settype("=>"); - limit->setlimit(std::to_string(minLength)); - limit->settol("1.0e-9"); - mbdAssembly->addLimit(limit); - } - - if (maxEnabled) { - auto limit2 = ASMTTranslationLimit::With(); - limit2->setName(joint->getFullName() + "-LimitLenMax"); - limit2->setMarkerI(fullMarkerNameI); - limit2->setMarkerJ(fullMarkerNameJ); - limit2->settype("=<"); - limit2->setlimit(std::to_string(maxLength)); - limit2->settol("1.0e-9"); - mbdAssembly->addLimit(limit2); - } - } - } - if (jointType == JointType::Revolute || jointType == JointType::Cylindrical) { - auto* pRotMin = dynamic_cast(joint->getPropertyByName("AngleMin")); - auto* pRotMax = dynamic_cast(joint->getPropertyByName("AngleMax")); - auto* pMinEnabled = dynamic_cast( - joint->getPropertyByName("EnableAngleMin") - ); - auto* pMaxEnabled = dynamic_cast( - joint->getPropertyByName("EnableAngleMax") - ); - - if (pRotMin && pRotMax && pMinEnabled && pMaxEnabled) { // Make sure properties do exist - // Swap the values if necessary. - bool minEnabled = pMinEnabled->getValue(); - bool maxEnabled = pMaxEnabled->getValue(); - double minAngle = pRotMin->getValue(); - double maxAngle = pRotMax->getValue(); - if ((minAngle > maxAngle) && minEnabled && maxEnabled) { - pRotMin->setValue(maxAngle); - pRotMax->setValue(minAngle); - minAngle = maxAngle; - maxAngle = pRotMax->getValue(); - - pMinEnabled->setValue(maxEnabled); - pMaxEnabled->setValue(minEnabled); - minEnabled = maxEnabled; - maxEnabled = pMaxEnabled->getValue(); - } - - if (minEnabled) { - auto limit = ASMTRotationLimit::With(); - limit->setName(joint->getFullName() + "-LimitRotMin"); - limit->setMarkerI(fullMarkerNameI); - limit->setMarkerJ(fullMarkerNameJ); - limit->settype("=>"); - limit->setlimit(std::to_string(minAngle) + "*pi/180.0"); - limit->settol("1.0e-9"); - mbdAssembly->addLimit(limit); - } - - if (maxEnabled) { - auto limit2 = ASMTRotationLimit::With(); - limit2->setName(joint->getFullName() + "-LimitRotMax"); - limit2->setMarkerI(fullMarkerNameI); - limit2->setMarkerJ(fullMarkerNameJ); - limit2->settype("=<"); - limit2->setlimit(std::to_string(maxAngle) + "*pi/180.0"); - limit2->settol("1.0e-9"); - mbdAssembly->addLimit(limit2); - } - } - } - } - std::vector done; - // Add motions if needed - for (auto* motion : motions) { - if (std::ranges::find(done, motion) != done.end()) { - continue; // don't process twice (can happen in case of cylindrical) - } - - auto* pJoint = dynamic_cast(motion->getPropertyByName("Joint")); - if (!pJoint) { - continue; - } - App::DocumentObject* motionJoint = pJoint->getValue(); - if (joint != motionJoint) { - continue; - } - - auto* pType = dynamic_cast(motion->getPropertyByName("MotionType")); - auto* pFormula = dynamic_cast(motion->getPropertyByName("Formula")); - if (!pType || !pFormula) { - continue; - } - std::string formula = pFormula->getValue(); - if (formula == "") { - continue; - } - std::string motionType = pType->getValueAsString(); - - // check if there is a second motion as cylindrical can have both, - // in which case the solver needs a general motion. - for (auto* motion2 : motions) { - pJoint = dynamic_cast(motion2->getPropertyByName("Joint")); - if (!pJoint) { - continue; - } - motionJoint = pJoint->getValue(); - if (joint != motionJoint || motion2 == motion) { - continue; - } - - auto* pType2 = dynamic_cast( - motion2->getPropertyByName("MotionType") - ); - auto* pFormula2 = dynamic_cast(motion2->getPropertyByName("Formula")); - if (!pType2 || !pFormula2) { - continue; - } - std::string formula2 = pFormula2->getValue(); - if (formula2 == "") { - continue; - } - std::string motionType2 = pType2->getValueAsString(); - if (motionType2 == motionType) { - continue; // only if both motions are different. ie one angular and one linear. - } - - auto ASMTmotion = CREATE::With(); - ASMTmotion->setName(joint->getFullName() + "-ScrewMotion"); - ASMTmotion->setMarkerI(fullMarkerNameI); - ASMTmotion->setMarkerJ(fullMarkerNameJ); - ASMTmotion->rIJI->atiput(2, motionType == "Angular" ? formula2 : formula); - ASMTmotion->angIJJ->atiput(2, motionType == "Angular" ? formula : formula2); - mbdAssembly->addMotion(ASMTmotion); - - done.push_back(motion2); - } - - if (motionType == "Angular") { - auto ASMTmotion = CREATE::With(); - ASMTmotion->setName(joint->getFullName() + "-AngularMotion"); - ASMTmotion->setMarkerI(fullMarkerNameI); - ASMTmotion->setMarkerJ(fullMarkerNameJ); - ASMTmotion->setRotationZ(formula); - mbdAssembly->addMotion(ASMTmotion); - } - else if (motionType == "Linear") { - auto ASMTmotion = CREATE::With(); - ASMTmotion->setName(joint->getFullName() + "-LinearMotion"); - ASMTmotion->setMarkerI(fullMarkerNameI); - ASMTmotion->setMarkerJ(fullMarkerNameJ); - ASMTmotion->setTranslationZ(formula); - mbdAssembly->addMotion(ASMTmotion); - } - } - - return {mbdJoint}; -} - -std::string AssemblyObject::handleOneSideOfJoint( - App::DocumentObject* joint, - const char* propRefName, - const char* propPlcName -) -{ - App::DocumentObject* part = getMovingPartFromRef(joint, propRefName); - App::DocumentObject* obj = getObjFromJointRef(joint, propRefName); - - if (!part || !obj) { - Base::Console() - .warning("The property %s of Joint %s is bad.\n", propRefName, joint->getFullName()); - return ""; - } - - MbDPartData data = getMbDData(part); - std::shared_ptr mbdPart = data.part; - Base::Placement plc = getPlacementFromProp(joint, propPlcName); - // Now we have plc which is the JCS placement, but its relative to the Object, not to the - // containing Part. - - if (obj->getNameInDocument() != part->getNameInDocument()) { - - auto* ref = dynamic_cast(joint->getPropertyByName(propRefName)); - if (!ref) { - return ""; - } - - Base::Placement obj_global_plc = getGlobalPlacement(obj, ref); - plc = obj_global_plc * plc; - - Base::Placement part_global_plc = getGlobalPlacement(part, ref); - plc = part_global_plc.inverse() * plc; - } - // check if we need to add an offset in case of bundled parts. - if (!data.offsetPlc.isIdentity()) { - plc = data.offsetPlc * plc; - } - - std::string markerName = joint->getFullName(); - auto mbdMarker = makeMbdMarker(markerName, plc); - mbdPart->addMarker(mbdMarker); - - return "/OndselAssembly/" + mbdPart->name + "/" + markerName; -} - -void AssemblyObject::getRackPinionMarkers( - App::DocumentObject* joint, - std::string& markerNameI, - std::string& markerNameJ -) -{ - // ASMT rack pinion joint must get the rack as I and pinion as J. - // - rack marker has to have Z axis parallel to pinion Z axis. - // - rack marker has to have X axis parallel to the sliding axis. - // The user will have selected the sliding marker so we need to transform it. - // And we need to detect which marker is the rack. - - int slidingIndex = slidingPartIndex(joint); - if (slidingIndex == 0) { - return; - } - - if (slidingIndex != 1) { - swapJCS(joint); // make sure that rack is first. - } - - App::DocumentObject* part1 = getMovingPartFromRef(joint, "Reference1"); - App::DocumentObject* obj1 = getObjFromJointRef(joint, "Reference1"); - Base::Placement plc1 = getPlacementFromProp(joint, "Placement1"); - - App::DocumentObject* obj2 = getObjFromJointRef(joint, "Reference2"); - Base::Placement plc2 = getPlacementFromProp(joint, "Placement2"); - - if (!part1 || !obj1) { - Base::Console().warning("Reference1 of Joint %s is bad.\n", joint->getFullName()); - return; - } - - // For the pinion nothing special needed : - markerNameJ = handleOneSideOfJoint(joint, "Reference2", "Placement2"); - - // For the rack we need to change the placement : - // make the pinion plc relative to the rack placement. - auto* ref1 = dynamic_cast(joint->getPropertyByName("Reference1")); - auto* ref2 = dynamic_cast(joint->getPropertyByName("Reference2")); - if (!ref1 || !ref2) { - return; - } - Base::Placement pinion_global_plc = getGlobalPlacement(obj2, ref2); - plc2 = pinion_global_plc * plc2; - Base::Placement rack_global_plc = getGlobalPlacement(obj1, ref1); - plc2 = rack_global_plc.inverse() * plc2; - - // The rot of the rack placement should be the same as the pinion, but with X axis along the - // slider axis. - Base::Rotation rot = plc2.getRotation(); - // the yaw of rot has to be the same as plc1 - Base::Vector3d currentZAxis = rot.multVec(Base::Vector3d(0, 0, 1)); - Base::Vector3d currentXAxis = rot.multVec(Base::Vector3d(1, 0, 0)); - Base::Vector3d targetXAxis = plc1.getRotation().multVec(Base::Vector3d(0, 0, 1)); - - // Calculate the angle between the current X axis and the target X axis - double yawAdjustment = currentXAxis.GetAngle(targetXAxis); - - // Determine the direction of the yaw adjustment using cross product - Base::Vector3d crossProd = currentXAxis.Cross(targetXAxis); - if (currentZAxis * crossProd < 0) { // If cross product is in opposite direction to Z axis - yawAdjustment = -yawAdjustment; - } - - // Create a yaw rotation around the Z axis - Base::Rotation yawRotation(currentZAxis, yawAdjustment); - - // Combine the initial rotation with the yaw adjustment - Base::Rotation adjustedRotation = rot * yawRotation; - plc1.setRotation(adjustedRotation); - - // Then end of processing similar to handleOneSideOfJoint : - MbDPartData data1 = getMbDData(part1); - std::shared_ptr mbdPart = data1.part; - if (obj1->getNameInDocument() != part1->getNameInDocument()) { - plc1 = rack_global_plc * plc1; - - Base::Placement part_global_plc = getGlobalPlacement(part1, ref1); - plc1 = part_global_plc.inverse() * plc1; - } - // check if we need to add an offset in case of bundled parts. - if (!data1.offsetPlc.isIdentity()) { - plc1 = data1.offsetPlc * plc1; - } - - std::string markerName = joint->getFullName(); - auto mbdMarker = makeMbdMarker(markerName, plc1); - mbdPart->addMarker(mbdMarker); - - markerNameI = "/OndselAssembly/" + mbdPart->name + "/" + markerName; -} - int AssemblyObject::slidingPartIndex(App::DocumentObject* joint) { App::DocumentObject* part1 = getMovingPartFromRef(joint, "Reference1"); @@ -1879,8 +1957,6 @@ int AssemblyObject::slidingPartIndex(App::DocumentObject* joint) } if (found != 0) { - // check the placements plcjt and (jcs1 or jcs2 depending on found value) Z axis are - // colinear ie if their pitch and roll are the same. double y1, p1, r1, y2, p2, r2; plcjt.getRotation().getYawPitchRoll(y1, p1, r1); plci.getRotation().getYawPitchRoll(y2, p2, r2); @@ -1893,130 +1969,6 @@ int AssemblyObject::slidingPartIndex(App::DocumentObject* joint) return slidingFound; } -bool AssemblyObject::isMbDJointValid(App::DocumentObject* joint) -{ - // When dragging a part, we are bundling fixed parts together. - // This may lead to a conflicting joint that is self referencing a MbD part. - // The solver crash when fed such a bad joint. So we make sure it does not happen. - App::DocumentObject* part1 = getMovingPartFromRef(joint, "Reference1"); - App::DocumentObject* part2 = getMovingPartFromRef(joint, "Reference2"); - if (!part1 || !part2) { - return false; - } - - // If this joint is self-referential it must be ignored. - if (getMbDPart(part1) == getMbDPart(part2)) { - Base::Console().warning( - "Assembly: Ignoring joint (%s) because its parts are connected by a fixed " - "joint bundle. This joint is a conflicting or redundant constraint.\n", - joint->getFullLabel() - ); - return false; - } - return true; -} - -AssemblyObject::MbDPartData AssemblyObject::getMbDData(App::DocumentObject* part) -{ - auto it = objectPartMap.find(part); - if (it != objectPartMap.end()) { - // part has been associated with an ASMTPart before - return it->second; - } - - // part has not been associated with an ASMTPart before - std::string str = part->getFullName(); - Base::Placement plc = getPlacementFromProp(part, "Placement"); - std::shared_ptr mbdPart = makeMbdPart(str, plc); - mbdAssembly->addPart(mbdPart); - MbDPartData data = {mbdPart, Base::Placement()}; - objectPartMap[part] = data; // Store the association - - // Associate other objects connected with fixed joints - if (bundleFixed) { - auto addConnectedFixedParts = [&](App::DocumentObject* currentPart, auto& self) -> void { - std::vector joints = getJointsOfPart(currentPart); - for (auto* joint : joints) { - JointType jointType = getJointType(joint); - if (jointType == JointType::Fixed) { - App::DocumentObject* part1 = getMovingPartFromRef(joint, "Reference1"); - App::DocumentObject* part2 = getMovingPartFromRef(joint, "Reference2"); - App::DocumentObject* partToAdd = currentPart == part1 ? part2 : part1; - - if (objectPartMap.find(partToAdd) != objectPartMap.end()) { - // already added - continue; - } - - Base::Placement plci = getPlacementFromProp(partToAdd, "Placement"); - MbDPartData partData = {mbdPart, plc.inverse() * plci}; - objectPartMap[partToAdd] = partData; // Store the association - - // Recursively call for partToAdd - self(partToAdd, self); - } - } - }; - - addConnectedFixedParts(part, addConnectedFixedParts); - } - return data; -} - -std::shared_ptr AssemblyObject::getMbDPart(App::DocumentObject* part) -{ - if (!part) { - return nullptr; - } - return getMbDData(part).part; -} - -std::shared_ptr AssemblyObject::makeMbdPart(std::string& name, Base::Placement plc, double mass) -{ - auto mbdPart = CREATE::With(); - mbdPart->setName(name); - - auto massMarker = CREATE::With(); - massMarker->setMass(mass); - massMarker->setDensity(1.0); - massMarker->setMomentOfInertias(1.0, 1.0, 1.0); - mbdPart->setPrincipalMassMarker(massMarker); - - Base::Vector3d pos = plc.getPosition(); - mbdPart->setPosition3D(pos.x, pos.y, pos.z); - - // TODO : replace with quaternion to simplify - Base::Rotation rot = plc.getRotation(); - Base::Matrix4D mat; - rot.getValue(mat); - Base::Vector3d r0 = mat.getRow(0); - Base::Vector3d r1 = mat.getRow(1); - Base::Vector3d r2 = mat.getRow(2); - mbdPart->setRotationMatrix(r0.x, r0.y, r0.z, r1.x, r1.y, r1.z, r2.x, r2.y, r2.z); - - return mbdPart; -} - -std::shared_ptr AssemblyObject::makeMbdMarker(std::string& name, Base::Placement& plc) -{ - auto mbdMarker = CREATE::With(); - mbdMarker->setName(name); - - Base::Vector3d pos = plc.getPosition(); - mbdMarker->setPosition3D(pos.x, pos.y, pos.z); - - // TODO : replace with quaternion to simplify - Base::Rotation rot = plc.getRotation(); - Base::Matrix4D mat; - rot.getValue(mat); - Base::Vector3d r0 = mat.getRow(0); - Base::Vector3d r1 = mat.getRow(1); - Base::Vector3d r2 = mat.getRow(2); - mbdMarker->setRotationMatrix(r0.x, r0.y, r0.z, r1.x, r1.y, r1.z, r2.x, r2.y, r2.z); - - return mbdMarker; -} - std::vector AssemblyObject::getDownstreamParts( App::DocumentObject* part, App::DocumentObject* joint diff --git a/src/Mod/Assembly/App/AssemblyObject.h b/src/Mod/Assembly/App/AssemblyObject.h index 74bc8e64d5..cffe0d2483 100644 --- a/src/Mod/Assembly/App/AssemblyObject.h +++ b/src/Mod/Assembly/App/AssemblyObject.h @@ -25,24 +25,21 @@ #ifndef ASSEMBLY_AssemblyObject_H #define ASSEMBLY_AssemblyObject_H +#include + #include #include +#include #include #include #include -#include - -namespace MbD +namespace KCSolve { -class ASMTPart; -class ASMTAssembly; -class ASMTJoint; -class ASMTMarker; -class ASMTPart; -} // namespace MbD +class IKCSolver; +} // namespace KCSolve namespace App { @@ -105,7 +102,6 @@ public: void exportAsASMT(std::string fileName); - Base::Placement getMbdPlacement(std::shared_ptr mbdPart); bool validateNewPlacements(); void setNewPlacements(); static void redrawJointPlacements(std::vector joints); @@ -114,42 +110,8 @@ public: // This makes sure that LinkGroups or sub-assemblies have identity placements. void ensureIdentityPlacements(); - // Ondsel Solver interface - std::shared_ptr makeMbdAssembly(); - void create_mbdSimulationParameters(App::DocumentObject* sim); - std::shared_ptr makeMbdPart( - std::string& name, - Base::Placement plc = Base::Placement(), - double mass = 1.0 - ); - std::shared_ptr getMbDPart(App::DocumentObject* obj); - // To help the solver, during dragging, we are bundling parts connected by a fixed joint. - // So several assembly components are bundled in a single ASMTPart. - // So we need to store the plc of each bundled object relative to the bundle origin (first obj - // of objectPartMap). - struct MbDPartData - { - std::shared_ptr part; - Base::Placement offsetPlc; // This is the offset within the bundled parts - }; - MbDPartData getMbDData(App::DocumentObject* part); - std::shared_ptr makeMbdMarker(std::string& name, Base::Placement& plc); - std::vector> makeMbdJoint(App::DocumentObject* joint); - std::shared_ptr makeMbdJointOfType(App::DocumentObject* joint, JointType jointType); - std::shared_ptr makeMbdJointDistance(App::DocumentObject* joint); - std::string handleOneSideOfJoint( - App::DocumentObject* joint, - const char* propRefName, - const char* propPlcName - ); - void getRackPinionMarkers( - App::DocumentObject* joint, - std::string& markerNameI, - std::string& markerNameJ - ); int slidingPartIndex(App::DocumentObject* joint); - void jointParts(std::vector joints); JointGroup* getJointGroup() const; ViewGroup* getExplodedViewGroup() const; template @@ -169,8 +131,6 @@ public: const std::vector& excludeJoints = {} ); std::unordered_set getGroundedParts(); - std::unordered_set fixGroundedParts(); - void fixGroundedPart(App::DocumentObject* obj, Base::Placement& plc, std::string& jointName); bool isJointConnectingPartToGround(App::DocumentObject* joint, const char* partPropName); bool isJointTypeConnecting(App::DocumentObject* joint); @@ -210,7 +170,7 @@ public: std::vector getMotionsFromSimulation(App::DocumentObject* sim); - bool isMbDJointValid(App::DocumentObject* joint); + bool isJointValid(App::DocumentObject* joint); bool isEmpty() const; int numberOfComponents() const; @@ -259,12 +219,56 @@ public: fastsignals::signal signalSolverUpdate; private: - std::shared_ptr mbdAssembly; + // ── Solver integration ───────────────────────────────────────── + + KCSolve::IKCSolver* getOrCreateSolver(); + + KCSolve::SolveContext buildSolveContext( + const std::vector& joints, + bool forSimulation = false, + App::DocumentObject* sim = nullptr + ); + + KCSolve::Transform computeMarkerTransform( + App::DocumentObject* joint, + const char* propRefName, + const char* propPlcName + ); + + struct RackPinionResult + { + std::string partIdI; + KCSolve::Transform markerI; + std::string partIdJ; + KCSolve::Transform markerJ; + }; + RackPinionResult computeRackPinionMarkers(App::DocumentObject* joint); + + // ── Part ↔ solver ID mapping ─────────────────────────────────── + + // Maps a solver part ID to the FreeCAD objects it represents. + // Multiple objects map to one ID when parts are bundled by Fixed joints. + struct PartMapping + { + App::DocumentObject* obj; + Base::Placement offset; // identity for primary, non-identity for bundled + }; + std::unordered_map> partIdToObjs_; + std::unordered_map objToPartId_; + + // Register a part (and recursively its fixed-joint bundle when bundleFixed is set). + // Returns the solver part ID. + std::string registerPart(App::DocumentObject* obj); + + // ── Solver state ─────────────────────────────────────────────── + + std::unique_ptr solver_; + KCSolve::SolveResult lastResult_; + + // ── Existing state (unchanged) ───────────────────────────────── - std::unordered_map objectPartMap; std::vector> objMasses; std::vector draggedParts; - std::vector motions; std::vector> previousPositions; diff --git a/src/Mod/Assembly/App/CMakeLists.txt b/src/Mod/Assembly/App/CMakeLists.txt index 96919de54e..0bff518aec 100644 --- a/src/Mod/Assembly/App/CMakeLists.txt +++ b/src/Mod/Assembly/App/CMakeLists.txt @@ -5,7 +5,6 @@ set(Assembly_LIBS PartDesign Spreadsheet FreeCADApp - OndselSolver KCSolve ) diff --git a/src/Mod/Assembly/Solver/IKCSolver.h b/src/Mod/Assembly/Solver/IKCSolver.h index e81d3c6dd8..47abc5af01 100644 --- a/src/Mod/Assembly/Solver/IKCSolver.h +++ b/src/Mod/Assembly/Solver/IKCSolver.h @@ -157,6 +157,12 @@ public: return true; } + /// Export solver-native debug/diagnostic file (e.g. ASMT for OndselSolver). + /// Default: no-op. Requires a prior solve() or run_kinematic() call. + virtual void export_native(const std::string& /*path*/) + { + } + /// Whether this solver handles fixed-joint part bundling internally. /// When false, the caller bundles parts connected by Fixed joints /// before building the SolveContext. When true, the solver receives diff --git a/src/Mod/Assembly/Solver/OndselAdapter.cpp b/src/Mod/Assembly/Solver/OndselAdapter.cpp index 230993795f..a2c58db8f0 100644 --- a/src/Mod/Assembly/Solver/OndselAdapter.cpp +++ b/src/Mod/Assembly/Solver/OndselAdapter.cpp @@ -784,4 +784,13 @@ std::vector OndselAdapter::diagnose(const SolveContext& ct return extract_diagnostics(); } +// ── Native export ────────────────────────────────────────────────── + +void OndselAdapter::export_native(const std::string& path) +{ + if (assembly_) { + assembly_->outputFile(path); + } +} + } // namespace KCSolve diff --git a/src/Mod/Assembly/Solver/OndselAdapter.h b/src/Mod/Assembly/Solver/OndselAdapter.h index 3d2b03434a..ba26093345 100644 --- a/src/Mod/Assembly/Solver/OndselAdapter.h +++ b/src/Mod/Assembly/Solver/OndselAdapter.h @@ -80,6 +80,7 @@ public: bool is_deterministic() const override; bool supports_bundle_fixed() const override; + void export_native(const std::string& path) override; /// Register OndselAdapter as "ondsel" in the SolverRegistry. /// Call once at module init time.