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