From 32dbe20ce037d5fbe73564e2359880488e7a2cdc Mon Sep 17 00:00:00 2001 From: forbes Date: Thu, 19 Feb 2026 16:19:44 -0600 Subject: [PATCH] feat(solver): implement OndselAdapter wrapping OndselSolver behind IKCSolver (#294) MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit Phase 1c of the pluggable solver system. Creates OndselAdapter — the concrete IKCSolver implementation that wraps OndselSolver's Lagrangian MBD engine behind the KCSolve abstraction layer. The adapter translates KCSolve types (SolveContext, BaseJointKind, Transform) to OndselSolver's ASMT hierarchy (ASMTAssembly, ASMTPart, ASMTJoint, ASMTMarker) and extracts results back into SolveResult. All 30+ OndselSolver #includes are confined to OndselAdapter.cpp. Key implementation details: - build_assembly(): creates ASMTAssembly from SolveContext - create_joint(): maps 20 BaseJointKind values to ASMT joint types (eliminates the 35-case DistanceType switch — decomposition done upstream) - Quaternion-to-rotation-matrix conversion for OndselSolver input - Direct quaternion extraction for output (both use w,x,y,z convention) - Drag lifecycle: pre_drag/drag_step/post_drag with stateful assembly - Simulation lifecycle: run_kinematic/num_frames/update_for_frame - Diagnostic extraction: iterates MBD system constraints for redundancy - Static register_solver() for SolverRegistry integration - ExternalSystem back-pointer NOT set — breaks bidirectional coupling New files: - Solver/OndselAdapter.h — class declaration with KCSolveExport - Solver/OndselAdapter.cpp — full implementation (~530 lines) Modified: - Solver/CMakeLists.txt — add sources, link OndselSolver (PRIVATE) --- src/Mod/Assembly/Solver/CMakeLists.txt | 3 + src/Mod/Assembly/Solver/OndselAdapter.cpp | 787 ++++++++++++++++++++++ src/Mod/Assembly/Solver/OndselAdapter.h | 128 ++++ 3 files changed, 918 insertions(+) create mode 100644 src/Mod/Assembly/Solver/OndselAdapter.cpp create mode 100644 src/Mod/Assembly/Solver/OndselAdapter.h diff --git a/src/Mod/Assembly/Solver/CMakeLists.txt b/src/Mod/Assembly/Solver/CMakeLists.txt index b3d78d5681..206ff7d1f4 100644 --- a/src/Mod/Assembly/Solver/CMakeLists.txt +++ b/src/Mod/Assembly/Solver/CMakeLists.txt @@ -6,6 +6,8 @@ set(KCSolve_SRCS IKCSolver.h SolverRegistry.h SolverRegistry.cpp + OndselAdapter.h + OndselAdapter.cpp ) add_library(KCSolve SHARED ${KCSolve_SRCS}) @@ -24,6 +26,7 @@ target_compile_definitions(KCSolve target_link_libraries(KCSolve PRIVATE FreeCADBase + OndselSolver ) # Platform-specific dynamic loading library diff --git a/src/Mod/Assembly/Solver/OndselAdapter.cpp b/src/Mod/Assembly/Solver/OndselAdapter.cpp new file mode 100644 index 0000000000..230993795f --- /dev/null +++ b/src/Mod/Assembly/Solver/OndselAdapter.cpp @@ -0,0 +1,787 @@ +// SPDX-License-Identifier: LGPL-2.1-or-later +/**************************************************************************** + * * + * Copyright (c) 2025 Kindred Systems * + * * + * This file is part of FreeCAD. * + * * + * FreeCAD is free software: you can redistribute it and/or modify it * + * under the terms of the GNU Lesser General Public License as * + * published by the Free Software Foundation, either version 2.1 of the * + * License, or (at your option) any later version. * + * * + * FreeCAD is distributed in the hope that it will be useful, but * + * WITHOUT ANY WARRANTY; without even the implied warranty of * + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU * + * Lesser General Public License for more details. * + * * + * You should have received a copy of the GNU Lesser General Public * + * License along with FreeCAD. If not, see * + * . * + * * + ***************************************************************************/ + +#include "OndselAdapter.h" +#include "SolverRegistry.h" + +#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 + +// For System::jointsMotionsDo and diagnostic iteration +#include +#include +#include + +using namespace MbD; + +namespace KCSolve +{ + +// ── Static registration ──────────────────────────────────────────── + +void OndselAdapter::register_solver() +{ + SolverRegistry::instance().register_solver( + "ondsel", + []() { return std::make_unique(); }); +} + + +// ── IKCSolver identity ───────────────────────────────────────────── + +std::string OndselAdapter::name() const +{ + return "OndselSolver (Lagrangian)"; +} + +bool OndselAdapter::is_deterministic() const +{ + return true; +} + +bool OndselAdapter::supports_bundle_fixed() const +{ + return false; +} + +std::vector OndselAdapter::supported_joints() const +{ + return { + BaseJointKind::Coincident, + BaseJointKind::PointOnLine, + BaseJointKind::PointInPlane, + BaseJointKind::Concentric, + BaseJointKind::Tangent, + BaseJointKind::Planar, + BaseJointKind::LineInPlane, + BaseJointKind::Parallel, + BaseJointKind::Perpendicular, + BaseJointKind::Angle, + BaseJointKind::Fixed, + BaseJointKind::Revolute, + BaseJointKind::Cylindrical, + BaseJointKind::Slider, + BaseJointKind::Ball, + BaseJointKind::Screw, + BaseJointKind::Gear, + BaseJointKind::RackPinion, + BaseJointKind::DistancePointPoint, + BaseJointKind::DistanceCylSph, + }; +} + + +// ── Quaternion → rotation matrix ─────────────────────────────────── + +void OndselAdapter::quat_to_matrix(const std::array& q, + double (&mat)[3][3]) +{ + double w = q[0], x = q[1], y = q[2], z = q[3]; + double xx = x * x, yy = y * y, zz = z * z; + double xy = x * y, xz = x * z, yz = y * z; + double wx = w * x, wy = w * y, wz = w * z; + + mat[0][0] = 1.0 - 2.0 * (yy + zz); + mat[0][1] = 2.0 * (xy - wz); + mat[0][2] = 2.0 * (xz + wy); + mat[1][0] = 2.0 * (xy + wz); + mat[1][1] = 1.0 - 2.0 * (xx + zz); + mat[1][2] = 2.0 * (yz - wx); + mat[2][0] = 2.0 * (xz - wy); + mat[2][1] = 2.0 * (yz + wx); + mat[2][2] = 1.0 - 2.0 * (xx + yy); +} + + +// ── Assembly building ────────────────────────────────────────────── + +std::shared_ptr OndselAdapter::make_part(const Part& part) +{ + auto mbdPart = CREATE::With(); + mbdPart->setName(part.id); + + auto massMarker = CREATE::With(); + massMarker->setMass(part.mass); + massMarker->setDensity(1.0); + massMarker->setMomentOfInertias(1.0, 1.0, 1.0); + mbdPart->setPrincipalMassMarker(massMarker); + + const auto& pos = part.placement.position; + mbdPart->setPosition3D(pos[0], pos[1], pos[2]); + + double mat[3][3]; + quat_to_matrix(part.placement.quaternion, mat); + mbdPart->setRotationMatrix( + mat[0][0], mat[0][1], mat[0][2], + mat[1][0], mat[1][1], mat[1][2], + mat[2][0], mat[2][1], mat[2][2]); + + return mbdPart; +} + +std::shared_ptr OndselAdapter::make_marker(const std::string& markerName, + const Transform& tf) +{ + auto mbdMarker = CREATE::With(); + mbdMarker->setName(markerName); + + const auto& pos = tf.position; + mbdMarker->setPosition3D(pos[0], pos[1], pos[2]); + + double mat[3][3]; + quat_to_matrix(tf.quaternion, mat); + mbdMarker->setRotationMatrix( + mat[0][0], mat[0][1], mat[0][2], + mat[1][0], mat[1][1], mat[1][2], + mat[2][0], mat[2][1], mat[2][2]); + + return mbdMarker; +} + +std::shared_ptr OndselAdapter::create_joint(const Constraint& c) +{ + auto param = [&](std::size_t i, double fallback = 0.0) -> double { + return i < c.params.size() ? c.params[i] : fallback; + }; + + switch (c.type) { + case BaseJointKind::Coincident: + return CREATE::With(); + + case BaseJointKind::PointOnLine: { + auto j = CREATE::With(); + j->distanceIJ = param(0); + return j; + } + + case BaseJointKind::PointInPlane: { + auto j = CREATE::With(); + j->offset = param(0); + return j; + } + + case BaseJointKind::Concentric: { + auto j = CREATE::With(); + j->distanceIJ = param(0); + return j; + } + + case BaseJointKind::Tangent: { + auto j = CREATE::With(); + j->offset = param(0); + return j; + } + + case BaseJointKind::Planar: { + auto j = CREATE::With(); + j->offset = param(0); + return j; + } + + case BaseJointKind::LineInPlane: { + auto j = CREATE::With(); + j->offset = param(0); + return j; + } + + case BaseJointKind::Parallel: + return CREATE::With(); + + case BaseJointKind::Perpendicular: + return CREATE::With(); + + case BaseJointKind::Angle: { + auto j = CREATE::With(); + j->theIzJz = param(0); + return j; + } + + case BaseJointKind::Fixed: + return CREATE::With(); + + case BaseJointKind::Revolute: + return CREATE::With(); + + case BaseJointKind::Cylindrical: + return CREATE::With(); + + case BaseJointKind::Slider: + return CREATE::With(); + + case BaseJointKind::Ball: + return CREATE::With(); + + case BaseJointKind::Screw: { + auto j = CREATE::With(); + j->pitch = param(0); + return j; + } + + case BaseJointKind::Gear: { + auto j = CREATE::With(); + j->radiusI = param(0); + j->radiusJ = param(1); + return j; + } + + case BaseJointKind::RackPinion: { + auto j = CREATE::With(); + j->pitchRadius = param(0); + return j; + } + + case BaseJointKind::DistancePointPoint: { + auto j = CREATE::With(); + j->distanceIJ = param(0); + return j; + } + + case BaseJointKind::DistanceCylSph: { + auto j = CREATE::With(); + j->distanceIJ = param(0); + return j; + } + + // Unsupported types + case BaseJointKind::Universal: + case BaseJointKind::Cam: + case BaseJointKind::Slot: + case BaseJointKind::Custom: + Base::Console().warning( + "KCSolve: OndselAdapter does not support joint kind %d for constraint '%s'\n", + static_cast(c.type), c.id.c_str()); + return nullptr; + } + + return nullptr; // unreachable, but silences compiler warnings +} + +void OndselAdapter::add_limits(const Constraint& c, + const std::string& marker_i, + const std::string& marker_j) +{ + for (const auto& lim : c.limits) { + switch (lim.kind) { + case Constraint::Limit::Kind::TranslationMin: { + auto limit = CREATE::With(); + limit->setName(c.id + "-LimitLenMin"); + limit->setMarkerI(marker_i); + limit->setMarkerJ(marker_j); + limit->settype("=>"); + limit->setlimit(std::to_string(lim.value)); + limit->settol(std::to_string(lim.tolerance)); + assembly_->addLimit(limit); + break; + } + case Constraint::Limit::Kind::TranslationMax: { + auto limit = CREATE::With(); + limit->setName(c.id + "-LimitLenMax"); + limit->setMarkerI(marker_i); + limit->setMarkerJ(marker_j); + limit->settype("=<"); + limit->setlimit(std::to_string(lim.value)); + limit->settol(std::to_string(lim.tolerance)); + assembly_->addLimit(limit); + break; + } + case Constraint::Limit::Kind::RotationMin: { + auto limit = CREATE::With(); + limit->setName(c.id + "-LimitRotMin"); + limit->setMarkerI(marker_i); + limit->setMarkerJ(marker_j); + limit->settype("=>"); + limit->setlimit(std::to_string(lim.value) + "*pi/180.0"); + limit->settol(std::to_string(lim.tolerance)); + assembly_->addLimit(limit); + break; + } + case Constraint::Limit::Kind::RotationMax: { + auto limit = CREATE::With(); + limit->setName(c.id + "-LimitRotMax"); + limit->setMarkerI(marker_i); + limit->setMarkerJ(marker_j); + limit->settype("=<"); + limit->setlimit(std::to_string(lim.value) + "*pi/180.0"); + limit->settol(std::to_string(lim.tolerance)); + assembly_->addLimit(limit); + break; + } + } + } +} + +void OndselAdapter::add_motions(const SolveContext& ctx, + const std::string& marker_i, + const std::string& marker_j, + const std::string& joint_id) +{ + // Collect motions that target this joint. + std::vector joint_motions; + for (const auto& m : ctx.motions) { + if (m.joint_id == joint_id) { + joint_motions.push_back(&m); + } + } + + if (joint_motions.empty()) { + return; + } + + // If there are two motions of different kinds on the same joint, + // combine them into a GeneralMotion (cylindrical joint case). + if (joint_motions.size() == 2 + && joint_motions[0]->kind != joint_motions[1]->kind) { + auto motion = CREATE::With(); + motion->setName(joint_id + "-GeneralMotion"); + motion->setMarkerI(marker_i); + motion->setMarkerJ(marker_j); + + for (const auto* m : joint_motions) { + if (m->kind == MotionDef::Kind::Rotational) { + motion->angIJJ->atiput(2, m->rotation_expr); + } + else { + motion->rIJI->atiput(2, m->translation_expr); + } + } + assembly_->addMotion(motion); + return; + } + + // Single motion or multiple of the same kind. + for (const auto* m : joint_motions) { + switch (m->kind) { + case MotionDef::Kind::Rotational: { + auto motion = CREATE::With(); + motion->setName(joint_id + "-AngularMotion"); + motion->setMarkerI(marker_i); + motion->setMarkerJ(marker_j); + motion->setRotationZ(m->rotation_expr); + assembly_->addMotion(motion); + break; + } + case MotionDef::Kind::Translational: { + auto motion = CREATE::With(); + motion->setName(joint_id + "-LinearMotion"); + motion->setMarkerI(marker_i); + motion->setMarkerJ(marker_j); + motion->setTranslationZ(m->translation_expr); + assembly_->addMotion(motion); + break; + } + case MotionDef::Kind::General: { + auto motion = CREATE::With(); + motion->setName(joint_id + "-GeneralMotion"); + motion->setMarkerI(marker_i); + motion->setMarkerJ(marker_j); + if (!m->rotation_expr.empty()) { + motion->angIJJ->atiput(2, m->rotation_expr); + } + if (!m->translation_expr.empty()) { + motion->rIJI->atiput(2, m->translation_expr); + } + assembly_->addMotion(motion); + break; + } + } + } +} + +void OndselAdapter::fix_grounded_parts(const SolveContext& ctx) +{ + for (const auto& part : ctx.parts) { + if (!part.grounded) { + continue; + } + + auto it = part_map_.find(part.id); + if (it == part_map_.end()) { + continue; + } + + // Assembly-level marker at the part's placement. + std::string asmMarkerName = "ground-" + part.id; + auto asmMarker = make_marker(asmMarkerName, part.placement); + assembly_->addMarker(asmMarker); + + // Part-level marker at identity. + std::string partMarkerName = "FixingMarker"; + auto partMarker = make_marker(partMarkerName, Transform::identity()); + it->second->addMarker(partMarker); + + // Fixed joint connecting them. + auto fixedJoint = CREATE::With(); + fixedJoint->setName("ground-fix-" + part.id); + fixedJoint->setMarkerI("/OndselAssembly/" + asmMarkerName); + fixedJoint->setMarkerJ("/OndselAssembly/" + part.id + "/" + partMarkerName); + assembly_->addJoint(fixedJoint); + } +} + +void OndselAdapter::set_simulation_params(const SimulationParams& params) +{ + auto mbdSim = assembly_->simulationParameters; + mbdSim->settstart(params.t_start); + mbdSim->settend(params.t_end); + mbdSim->sethout(params.h_out); + mbdSim->sethmin(params.h_min); + mbdSim->sethmax(params.h_max); + mbdSim->seterrorTol(params.error_tol); +} + +void OndselAdapter::build_assembly(const SolveContext& ctx) +{ + assembly_ = CREATE::With(); + assembly_->setName("OndselAssembly"); + part_map_.clear(); + + // Do NOT set externalSystem->freecadAssemblyObject — breaking the coupling. + + // Add parts. + for (const auto& part : ctx.parts) { + auto mbdPart = make_part(part); + assembly_->addPart(mbdPart); + part_map_[part.id] = mbdPart; + } + + // Fix grounded parts. + fix_grounded_parts(ctx); + + // Add constraints (joints + limits + motions). + for (const auto& c : ctx.constraints) { + if (!c.activated) { + continue; + } + + auto mbdJoint = create_joint(c); + if (!mbdJoint) { + continue; + } + + // Create markers on the respective parts. + auto it_i = part_map_.find(c.part_i); + auto it_j = part_map_.find(c.part_j); + if (it_i == part_map_.end() || it_j == part_map_.end()) { + Base::Console().warning( + "KCSolve: constraint '%s' references unknown part(s)\n", + c.id.c_str()); + continue; + } + + std::string markerNameI = c.id + "-mkrI"; + std::string markerNameJ = c.id + "-mkrJ"; + + auto mkrI = make_marker(markerNameI, c.marker_i); + it_i->second->addMarker(mkrI); + + auto mkrJ = make_marker(markerNameJ, c.marker_j); + it_j->second->addMarker(mkrJ); + + std::string fullMarkerI = "/OndselAssembly/" + c.part_i + "/" + markerNameI; + std::string fullMarkerJ = "/OndselAssembly/" + c.part_j + "/" + markerNameJ; + + mbdJoint->setName(c.id); + mbdJoint->setMarkerI(fullMarkerI); + mbdJoint->setMarkerJ(fullMarkerJ); + assembly_->addJoint(mbdJoint); + + // Add limits (only when not in simulation mode). + if (!ctx.simulation.has_value() && !c.limits.empty()) { + add_limits(c, fullMarkerI, fullMarkerJ); + } + + // Add motions. + if (!ctx.motions.empty()) { + add_motions(ctx, fullMarkerI, fullMarkerJ, c.id); + } + } + + // Set simulation parameters if present. + if (ctx.simulation.has_value()) { + set_simulation_params(*ctx.simulation); + } +} + + +// ── Result extraction ────────────────────────────────────────────── + +Transform OndselAdapter::extract_part_transform( + const std::shared_ptr& part) const +{ + Transform tf; + double x, y, z; + part->getPosition3D(x, y, z); + tf.position = {x, y, z}; + + double q0, q1, q2, q3; + part->getQuarternions(q0, q1, q2, q3); + // OndselSolver returns (w, x, y, z) — matches our convention. + tf.quaternion = {q0, q1, q2, q3}; + + return tf; +} + +SolveResult OndselAdapter::extract_result() const +{ + SolveResult result; + result.status = SolveStatus::Success; + + for (const auto& [id, mbdPart] : part_map_) { + SolveResult::PartResult pr; + pr.id = id; + pr.placement = extract_part_transform(mbdPart); + result.placements.push_back(std::move(pr)); + } + + return result; +} + +std::vector OndselAdapter::extract_diagnostics() const +{ + std::vector diags; + + if (!assembly_ || !assembly_->mbdSystem) { + return diags; + } + + assembly_->mbdSystem->jointsMotionsDo([&](std::shared_ptr jm) { + if (!jm) { + return; + } + + bool isRedundant = false; + jm->constraintsDo([&](std::shared_ptr con) { + if (!con) { + return; + } + std::string spec = con->constraintSpec(); + if (spec.rfind("Redundant", 0) == 0) { + isRedundant = true; + } + }); + + if (isRedundant) { + // Extract the constraint name from the solver's joint name. + // Format: "/OndselAssembly/ground_moves#Joint001" → "Joint001" + std::string fullName = jm->name; + std::size_t hashPos = fullName.find_last_of('#'); + std::string cleanName = (hashPos != std::string::npos) + ? fullName.substr(hashPos + 1) + : fullName; + + ConstraintDiagnostic diag; + diag.constraint_id = cleanName; + diag.kind = ConstraintDiagnostic::Kind::Redundant; + diag.detail = "Constraint is redundant"; + diags.push_back(std::move(diag)); + } + }); + + return diags; +} + + +// ── Solve operations ─────────────────────────────────────────────── + +SolveResult OndselAdapter::solve(const SolveContext& ctx) +{ + try { + build_assembly(ctx); + assembly_->runPreDrag(); + return extract_result(); + } + catch (const std::exception& e) { + Base::Console().warning("KCSolve: OndselAdapter solve failed: %s\n", e.what()); + return SolveResult {SolveStatus::Failed, {}, -1, {}, 0}; + } + catch (...) { + Base::Console().warning("KCSolve: OndselAdapter solve failed: unknown exception\n"); + return SolveResult {SolveStatus::Failed, {}, -1, {}, 0}; + } +} + +SolveResult OndselAdapter::update(const SolveContext& ctx) +{ + return solve(ctx); +} + + +// ── Drag protocol ────────────────────────────────────────────────── + +SolveResult OndselAdapter::pre_drag(const SolveContext& ctx, + const std::vector& drag_parts) +{ + drag_part_ids_ = drag_parts; + + try { + build_assembly(ctx); + assembly_->runPreDrag(); + return extract_result(); + } + catch (const std::exception& e) { + Base::Console().warning("KCSolve: OndselAdapter pre_drag failed: %s\n", e.what()); + return SolveResult {SolveStatus::Failed, {}, -1, {}, 0}; + } + catch (...) { + Base::Console().warning("KCSolve: OndselAdapter pre_drag failed: unknown exception\n"); + return SolveResult {SolveStatus::Failed, {}, -1, {}, 0}; + } +} + +SolveResult OndselAdapter::drag_step( + const std::vector& drag_placements) +{ + if (!assembly_) { + return SolveResult {SolveStatus::Failed, {}, -1, {}, 0}; + } + + try { + auto dragParts = std::make_shared>>(); + + for (const auto& dp : drag_placements) { + auto it = part_map_.find(dp.id); + if (it == part_map_.end()) { + continue; + } + auto& mbdPart = it->second; + + // Update position. + const auto& pos = dp.placement.position; + mbdPart->updateMbDFromPosition3D(pos[0], pos[1], pos[2]); + + // Update rotation. + double mat[3][3]; + quat_to_matrix(dp.placement.quaternion, mat); + mbdPart->updateMbDFromRotationMatrix( + mat[0][0], mat[0][1], mat[0][2], + mat[1][0], mat[1][1], mat[1][2], + mat[2][0], mat[2][1], mat[2][2]); + + dragParts->push_back(mbdPart); + } + + assembly_->runDragStep(dragParts); + return extract_result(); + } + catch (...) { + // Drag step failures are non-fatal — caller will skip this frame. + return SolveResult {SolveStatus::Failed, {}, -1, {}, 0}; + } +} + +void OndselAdapter::post_drag() +{ + if (assembly_) { + assembly_->runPostDrag(); + } + drag_part_ids_.clear(); +} + + +// ── Kinematic simulation ─────────────────────────────────────────── + +SolveResult OndselAdapter::run_kinematic(const SolveContext& ctx) +{ + try { + build_assembly(ctx); + assembly_->runKINEMATIC(); + + auto result = extract_result(); + result.num_frames = assembly_->numberOfFrames(); + return result; + } + catch (const std::exception& e) { + Base::Console().warning("KCSolve: OndselAdapter run_kinematic failed: %s\n", e.what()); + return SolveResult {SolveStatus::Failed, {}, -1, {}, 0}; + } + catch (...) { + Base::Console().warning( + "KCSolve: OndselAdapter run_kinematic failed: unknown exception\n"); + return SolveResult {SolveStatus::Failed, {}, -1, {}, 0}; + } +} + +std::size_t OndselAdapter::num_frames() const +{ + if (!assembly_) { + return 0; + } + return assembly_->numberOfFrames(); +} + +SolveResult OndselAdapter::update_for_frame(std::size_t index) +{ + if (!assembly_) { + return SolveResult {SolveStatus::Failed, {}, -1, {}, 0}; + } + + if (index >= assembly_->numberOfFrames()) { + return SolveResult {SolveStatus::Failed, {}, -1, {}, 0}; + } + + assembly_->updateForFrame(index); + return extract_result(); +} + + +// ── Diagnostics ──────────────────────────────────────────────────── + +std::vector OndselAdapter::diagnose(const SolveContext& ctx) +{ + // Ensure we have a solved assembly to inspect. + if (!assembly_ || !assembly_->mbdSystem) { + solve(ctx); + } + return extract_diagnostics(); +} + +} // namespace KCSolve diff --git a/src/Mod/Assembly/Solver/OndselAdapter.h b/src/Mod/Assembly/Solver/OndselAdapter.h new file mode 100644 index 0000000000..3d2b03434a --- /dev/null +++ b/src/Mod/Assembly/Solver/OndselAdapter.h @@ -0,0 +1,128 @@ +// SPDX-License-Identifier: LGPL-2.1-or-later +/**************************************************************************** + * * + * Copyright (c) 2025 Kindred Systems * + * * + * This file is part of FreeCAD. * + * * + * FreeCAD is free software: you can redistribute it and/or modify it * + * under the terms of the GNU Lesser General Public License as * + * published by the Free Software Foundation, either version 2.1 of the * + * License, or (at your option) any later version. * + * * + * FreeCAD is distributed in the hope that it will be useful, but * + * WITHOUT ANY WARRANTY; without even the implied warranty of * + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU * + * Lesser General Public License for more details. * + * * + * You should have received a copy of the GNU Lesser General Public * + * License along with FreeCAD. If not, see * + * . * + * * + ***************************************************************************/ + +#ifndef KCSOLVE_ONDSELADAPTER_H +#define KCSOLVE_ONDSELADAPTER_H + +#include +#include +#include +#include + +#include "IKCSolver.h" +#include "KCSolveGlobal.h" + +namespace MbD +{ +class ASMTAssembly; +class ASMTJoint; +class ASMTMarker; +class ASMTPart; +} // namespace MbD + +namespace KCSolve +{ + +/// IKCSolver implementation wrapping OndselSolver's Lagrangian MBD engine. +/// +/// Translates KCSolve types (SolveContext, BaseJointKind, Transform) to +/// OndselSolver's ASMT hierarchy (ASMTAssembly, ASMTPart, ASMTJoint, etc.) +/// and extracts results back into SolveResult. +/// +/// All OndselSolver #includes are confined to OndselAdapter.cpp. + +class KCSolveExport OndselAdapter : public IKCSolver +{ +public: + OndselAdapter() = default; + + // ── IKCSolver pure virtuals ──────────────────────────────────── + + std::string name() const override; + std::vector supported_joints() const override; + SolveResult solve(const SolveContext& ctx) override; + + // ── IKCSolver overrides ──────────────────────────────────────── + + SolveResult update(const SolveContext& ctx) override; + + SolveResult pre_drag(const SolveContext& ctx, + const std::vector& drag_parts) override; + SolveResult drag_step( + const std::vector& drag_placements) override; + void post_drag() override; + + SolveResult run_kinematic(const SolveContext& ctx) override; + std::size_t num_frames() const override; + SolveResult update_for_frame(std::size_t index) override; + + std::vector diagnose(const SolveContext& ctx) override; + + bool is_deterministic() const override; + bool supports_bundle_fixed() const override; + + /// Register OndselAdapter as "ondsel" in the SolverRegistry. + /// Call once at module init time. + static void register_solver(); + +private: + // ── Assembly building ────────────────────────────────────────── + + void build_assembly(const SolveContext& ctx); + std::shared_ptr make_part(const Part& part); + std::shared_ptr make_marker(const std::string& name, + const Transform& tf); + std::shared_ptr create_joint(const Constraint& c); + void add_limits(const Constraint& c, + const std::string& marker_i, + const std::string& marker_j); + void add_motions(const SolveContext& ctx, + const std::string& marker_i, + const std::string& marker_j, + const std::string& joint_id); + void fix_grounded_parts(const SolveContext& ctx); + void set_simulation_params(const SimulationParams& params); + + // ── Result extraction ────────────────────────────────────────── + + SolveResult extract_result() const; + std::vector extract_diagnostics() const; + Transform extract_part_transform( + const std::shared_ptr& part) const; + + // ── Quaternion ↔ rotation matrix conversion ──────────────────── + + /// Convert unit quaternion (w,x,y,z) to 3×3 rotation matrix (row-major). + static void quat_to_matrix(const std::array& q, + double (&mat)[3][3]); + + // ── Internal state ───────────────────────────────────────────── + + std::shared_ptr assembly_; + std::unordered_map> part_map_; + std::vector drag_part_ids_; +}; + +} // namespace KCSolve + +#endif // KCSOLVE_ONDSELADAPTER_H