From 47e6c144610eeaae3719ed631829ecf8dabe6e3f Mon Sep 17 00:00:00 2001 From: forbes Date: Thu, 19 Feb 2026 15:55:57 -0600 Subject: [PATCH 1/5] feat(solver): define IKCSolver C++ API types and interface (#292) Add the pluggable solver API as header-only files under src/Mod/Assembly/Solver/. This is Phase 1a of the pluggable solver system (INTER_SOLVER.md). New files: - Types.h: BaseJointKind enum (24 decomposed constraint types), Transform, Part, Constraint, SolveContext, SolveResult, and supporting types. Uses standalone types (no FreeCAD dependency) for future server worker compatibility. - IKCSolver.h: Abstract solver interface with solve(), drag protocol (pre_drag/drag_step/post_drag), kinematic simulation (run_kinematic/num_frames/update_for_frame), and diagnostics. Only solve(), name(), and supported_joints() are pure virtual; all other methods have default implementations. - SolverRegistry.h: Thread-safe singleton registry for solver backends with factory-based registration and default solver selection. - CMakeLists.txt: INTERFACE library target (header-only for now). Modified: - Assembly/CMakeLists.txt: add_subdirectory(Solver) - Assembly/App/CMakeLists.txt: link KCSolve INTERFACE target --- src/Mod/Assembly/App/CMakeLists.txt | 1 + src/Mod/Assembly/CMakeLists.txt | 1 + src/Mod/Assembly/Solver/CMakeLists.txt | 12 + src/Mod/Assembly/Solver/IKCSolver.h | 181 ++++++++++++++ src/Mod/Assembly/Solver/SolverRegistry.h | 164 +++++++++++++ src/Mod/Assembly/Solver/Types.h | 286 +++++++++++++++++++++++ 6 files changed, 645 insertions(+) create mode 100644 src/Mod/Assembly/Solver/CMakeLists.txt create mode 100644 src/Mod/Assembly/Solver/IKCSolver.h create mode 100644 src/Mod/Assembly/Solver/SolverRegistry.h create mode 100644 src/Mod/Assembly/Solver/Types.h diff --git a/src/Mod/Assembly/App/CMakeLists.txt b/src/Mod/Assembly/App/CMakeLists.txt index 87053222f0..96919de54e 100644 --- a/src/Mod/Assembly/App/CMakeLists.txt +++ b/src/Mod/Assembly/App/CMakeLists.txt @@ -6,6 +6,7 @@ set(Assembly_LIBS Spreadsheet FreeCADApp OndselSolver + KCSolve ) generate_from_py(AssemblyObject) diff --git a/src/Mod/Assembly/CMakeLists.txt b/src/Mod/Assembly/CMakeLists.txt index f79b42cb35..1c51a9a13f 100644 --- a/src/Mod/Assembly/CMakeLists.txt +++ b/src/Mod/Assembly/CMakeLists.txt @@ -11,6 +11,7 @@ else () endif () endif () +add_subdirectory(Solver) add_subdirectory(App) if(BUILD_GUI) diff --git a/src/Mod/Assembly/Solver/CMakeLists.txt b/src/Mod/Assembly/Solver/CMakeLists.txt new file mode 100644 index 0000000000..51717be77e --- /dev/null +++ b/src/Mod/Assembly/Solver/CMakeLists.txt @@ -0,0 +1,12 @@ +# SPDX-License-Identifier: LGPL-2.1-or-later + +# Phase 1a: header-only INTERFACE library. +# Phase 1b will convert to SHARED when .cpp files are added. + +add_library(KCSolve INTERFACE) + +target_include_directories(KCSolve + INTERFACE + ${CMAKE_SOURCE_DIR}/src + ${CMAKE_BINARY_DIR}/src +) diff --git a/src/Mod/Assembly/Solver/IKCSolver.h b/src/Mod/Assembly/Solver/IKCSolver.h new file mode 100644 index 0000000000..cd9ac78a9b --- /dev/null +++ b/src/Mod/Assembly/Solver/IKCSolver.h @@ -0,0 +1,181 @@ +// 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_IKCSOLVER_H +#define KCSOLVE_IKCSOLVER_H + +#include +#include +#include + +#include "Types.h" + +namespace KCSolve +{ + +/// Abstract interface for a pluggable assembly constraint solver. +/// +/// Solver backends implement this interface. The Assembly module calls +/// through it via the SolverRegistry. A minimal solver only needs to +/// implement solve(), name(), and supported_joints() — all other methods +/// have default implementations that either delegate to solve() or +/// return sensible defaults. +/// +/// Method mapping to current AssemblyObject operations: +/// +/// solve() <-> AssemblyObject::solve() +/// pre_drag() <-> AssemblyObject::preDrag() +/// drag_step() <-> AssemblyObject::doDragStep() +/// post_drag() <-> AssemblyObject::postDrag() +/// run_kinematic() <-> AssemblyObject::generateSimulation() +/// num_frames() <-> AssemblyObject::numberOfFrames() +/// update_for_frame() <-> AssemblyObject::updateForFrame() +/// diagnose() <-> AssemblyObject::updateSolveStatus() + +class IKCSolver +{ +public: + virtual ~IKCSolver() = default; + + /// Human-readable solver name (e.g. "OndselSolver (Lagrangian)"). + virtual std::string name() const = 0; + + /// Return the set of BaseJointKind values this solver supports. + /// The registry uses this for capability-based solver selection. + virtual std::vector supported_joints() const = 0; + + // ── Static solve ─────────────────────────────────────────────── + + /// Solve the assembly for static equilibrium. + /// @param ctx Complete description of parts, constraints, and options. + /// @return Result with updated placements and diagnostics. + virtual SolveResult solve(const SolveContext& ctx) = 0; + + /// Incrementally update an already-solved assembly after parameter + /// changes (e.g. joint angle/distance changed during joint creation). + /// Default: delegates to solve(). + virtual SolveResult update(const SolveContext& ctx) + { + return solve(ctx); + } + + // ── Interactive drag ─────────────────────────────────────────── + // + // Three-phase protocol for interactive part dragging: + // 1. pre_drag() — solve initial state, prepare for dragging + // 2. drag_step() — called on each mouse move with updated positions + // 3. post_drag() — finalize and release internal solver state + // + // Solvers can maintain internal state across the drag session for + // better interactive performance. This addresses a known weakness + // in the current direct-OndselSolver integration. + + /// Prepare for an interactive drag session. + /// @param ctx Assembly state before dragging begins. + /// @param drag_parts IDs of parts being dragged. + /// @return Initial solve result. + virtual SolveResult pre_drag(const SolveContext& ctx, + const std::vector& /*drag_parts*/) + { + return solve(ctx); + } + + /// Perform one incremental drag step. + /// @param drag_placements Current placements of the dragged parts + /// (part ID + new transform). + /// @return Updated placements for all affected parts. + virtual SolveResult drag_step( + const std::vector& /*drag_placements*/) + { + return SolveResult {SolveStatus::Success}; + } + + /// End an interactive drag session and finalize state. + virtual void post_drag() + { + } + + // ── Kinematic simulation ─────────────────────────────────────── + + /// Run a kinematic simulation over the time range in ctx.simulation. + /// After this call, num_frames() returns the frame count and + /// update_for_frame(i) retrieves individual frame placements. + /// Default: delegates to solve() (ignoring simulation params). + virtual SolveResult run_kinematic(const SolveContext& /*ctx*/) + { + return SolveResult {SolveStatus::Failed}; + } + + /// Number of simulation frames available after run_kinematic(). + virtual std::size_t num_frames() const + { + return 0; + } + + /// Retrieve part placements for simulation frame at index. + /// @pre index < num_frames() + virtual SolveResult update_for_frame(std::size_t /*index*/) + { + return SolveResult {SolveStatus::Failed}; + } + + // ── Diagnostics ──────────────────────────────────────────────── + + /// Analyze the assembly for redundant, conflicting, or malformed + /// constraints. May require a prior solve() call for some solvers. + virtual std::vector diagnose(const SolveContext& /*ctx*/) + { + return {}; + } + + // ── Capability queries ───────────────────────────────────────── + + /// Whether this solver produces deterministic results given + /// identical input. + virtual bool is_deterministic() const + { + return true; + } + + /// 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 + /// unbundled parts and optimizes internally. + virtual bool supports_bundle_fixed() const + { + return false; + } + +protected: + IKCSolver() = default; + + // Non-copyable, non-movable (polymorphic base class) + IKCSolver(const IKCSolver&) = delete; + IKCSolver& operator=(const IKCSolver&) = delete; + IKCSolver(IKCSolver&&) = delete; + IKCSolver& operator=(IKCSolver&&) = delete; +}; + +} // namespace KCSolve + +#endif // KCSOLVE_IKCSOLVER_H diff --git a/src/Mod/Assembly/Solver/SolverRegistry.h b/src/Mod/Assembly/Solver/SolverRegistry.h new file mode 100644 index 0000000000..ea0dee13cc --- /dev/null +++ b/src/Mod/Assembly/Solver/SolverRegistry.h @@ -0,0 +1,164 @@ +// 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_SOLVERREGISTRY_H +#define KCSOLVE_SOLVERREGISTRY_H + +#include +#include +#include +#include +#include +#include + +#include "IKCSolver.h" + +namespace KCSolve +{ + +/// Factory function that creates a solver instance. +using CreateSolverFn = std::function()>; + +/// Singleton registry for pluggable solver backends. +/// +/// Solver plugins register themselves at module load time via +/// register_solver(). The Assembly module retrieves solvers via get(). +/// +/// Thread safety: all public methods are internally synchronized. +/// +/// Usage: +/// // Registration (at module init): +/// KCSolve::SolverRegistry::instance().register_solver( +/// "ondsel", []() { return std::make_unique(); }); +/// +/// // Retrieval: +/// auto solver = KCSolve::SolverRegistry::instance().get(); // default +/// auto solver = KCSolve::SolverRegistry::instance().get("ondsel"); + +class SolverRegistry +{ +public: + /// Access the singleton instance. + static SolverRegistry& instance() + { + static SolverRegistry reg; + return reg; + } + + /// Register a solver backend. + /// @param name Unique solver name (e.g. "ondsel"). + /// @param factory Factory function that creates solver instances. + /// @return true if registration succeeded, false if name taken. + bool register_solver(const std::string& name, CreateSolverFn factory) + { + std::lock_guard lock(mutex_); + auto [it, inserted] = factories_.emplace(name, std::move(factory)); + if (inserted && default_name_.empty()) { + default_name_ = name; // first registered becomes default + } + return inserted; + } + + /// Create an instance of the named solver. + /// @param name Solver name. If empty, uses the default solver. + /// @return Solver instance, or nullptr if not found. + std::unique_ptr get(const std::string& name = {}) const + { + std::lock_guard lock(mutex_); + const std::string& key = name.empty() ? default_name_ : name; + if (key.empty()) { + return nullptr; + } + auto it = factories_.find(key); + if (it == factories_.end()) { + return nullptr; + } + return it->second(); + } + + /// Return the names of all registered solvers. + std::vector available() const + { + std::lock_guard lock(mutex_); + std::vector names; + names.reserve(factories_.size()); + for (const auto& [name, _] : factories_) { + names.push_back(name); + } + return names; + } + + /// Query which BaseJointKind values a named solver supports. + /// Creates a temporary instance to call supported_joints(). + std::vector joints_for(const std::string& name) const + { + auto solver = get(name); + if (!solver) { + return {}; + } + return solver->supported_joints(); + } + + /// Set the default solver name. + /// @return true if the name is registered, false otherwise. + bool set_default(const std::string& name) + { + std::lock_guard lock(mutex_); + if (factories_.find(name) == factories_.end()) { + return false; + } + default_name_ = name; + return true; + } + + /// Get the default solver name. + std::string get_default() const + { + std::lock_guard lock(mutex_); + return default_name_; + } + + /// Scan a directory for solver plugins (Phase 1b). + /// Currently a no-op placeholder. Will dlopen/LoadLibrary shared + /// objects that export kcsolve_create() / kcsolve_api_version(). + void scan(const std::string& /*directory*/) + { + } + +private: + SolverRegistry() = default; + ~SolverRegistry() = default; + + SolverRegistry(const SolverRegistry&) = delete; + SolverRegistry& operator=(const SolverRegistry&) = delete; + SolverRegistry(SolverRegistry&&) = delete; + SolverRegistry& operator=(SolverRegistry&&) = delete; + + mutable std::mutex mutex_; + std::unordered_map factories_; + std::string default_name_; +}; + +} // namespace KCSolve + +#endif // KCSOLVE_SOLVERREGISTRY_H diff --git a/src/Mod/Assembly/Solver/Types.h b/src/Mod/Assembly/Solver/Types.h new file mode 100644 index 0000000000..6fdcca6fb3 --- /dev/null +++ b/src/Mod/Assembly/Solver/Types.h @@ -0,0 +1,286 @@ +// 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_TYPES_H +#define KCSOLVE_TYPES_H + +#include +#include +#include +#include +#include +#include + +namespace KCSolve +{ + +// ── Transform ────────────────────────────────────────────────────── +// +// Rigid-body transform: position (x, y, z) + unit quaternion (w, x, y, z). +// Semantically equivalent to Base::Placement but free of FreeCAD dependencies +// so that KCSolve headers remain standalone (for future server worker use). +// +// Quaternion convention: (w, x, y, z) — mathematical standard. +// Note: Base::Rotation(q0,q1,q2,q3) uses (x, y, z, w) ordering. +// The adapter layer handles this swap. + +struct Transform +{ + std::array position {0.0, 0.0, 0.0}; + std::array quaternion {1.0, 0.0, 0.0, 0.0}; // w, x, y, z + + static Transform identity() + { + return {}; + } +}; + +// ── BaseJointKind ────────────────────────────────────────────────── +// +// Decomposed primitive constraint types. Uses SOLIDWORKS-inspired vocabulary +// from the INTER_SOLVER.md spec rather than OndselSolver internal names. +// +// The existing Assembly::JointType (13 values) and Assembly::DistanceType +// (35+ values) map to these via the adapter layer. In particular, the +// "Distance" JointType is decomposed based on geometry classification +// (see makeMbdJointDistance in AssemblyObject.cpp). + +enum class BaseJointKind : std::uint8_t +{ + // Point constraints (decomposed from JointType::Distance) + Coincident, // PointOnPoint, d=0 — 3 DOF removed + PointOnLine, // Point constrained to a line — 2 DOF removed + PointInPlane, // Point constrained to a plane — 1 DOF removed + + // Axis/surface constraints (decomposed from JointType::Distance) + Concentric, // Coaxial (line-line, circle-circle, cyl-cyl) — 4 DOF removed + Tangent, // Face-on-face tangency — 1 DOF removed + Planar, // Coplanar faces — 3 DOF removed + LineInPlane, // Line constrained to a plane — 2 DOF removed + + // Axis orientation constraints (direct from JointType) + Parallel, // Parallel axes — 2 DOF removed + Perpendicular, // 90-degree axes — 1 DOF removed + Angle, // Arbitrary axis angle — 1 DOF removed + + // Standard kinematic joints (direct 1:1 from JointType) + Fixed, // Rigid weld — 6 DOF removed + Revolute, // Hinge — 5 DOF removed + Cylindrical, // Rotation + sliding on axis — 4 DOF removed + Slider, // Linear translation — 5 DOF removed + Ball, // Spherical — 3 DOF removed + Screw, // Helical (rotation + coupled translation) — 5 DOF removed + Universal, // U-joint / Cardan — 4 DOF removed (future) + + // Mechanical element constraints + Gear, // Gear pair or belt (sign determines direction) + RackPinion, // Rack-and-pinion + Cam, // Cam-follower (future) + Slot, // Slot constraint (future) + + // Distance variants with non-zero offset + DistancePointPoint, // Point-to-point with offset — 2 DOF removed + DistanceCylSph, // Cylinder-sphere distance — varies + + Custom, // Solver-specific extension point +}; + +// ── Part ─────────────────────────────────────────────────────────── + +struct Part +{ + std::string id; + Transform placement; + double mass {1.0}; + bool grounded {false}; +}; + +// ── Constraint ───────────────────────────────────────────────────── +// +// A constraint between two parts. Built from a FreeCAD JointObject by +// the adapter layer (classifying geometry into the specific BaseJointKind). + +struct Constraint +{ + std::string id; // FreeCAD document object name (e.g. "Joint001") + + std::string part_i; // solver-side part ID for first reference + Transform marker_i; // coordinate system on part_i + + std::string part_j; // solver-side part ID for second reference + Transform marker_j; // coordinate system on part_j + + BaseJointKind type {}; + + // Scalar parameters (interpretation depends on type): + // Angle: params[0] = angle in radians + // RackPinion: params[0] = pitch radius + // Screw: params[0] = pitch + // Gear: params[0] = radiusI, params[1] = radiusJ (negative for belt) + // DistancePointPoint: params[0] = distance + // DistanceCylSph: params[0] = distance + // Planar: params[0] = offset + // Concentric: params[0] = distance + // PointInPlane: params[0] = offset + // LineInPlane: params[0] = offset + std::vector params; + + // Joint limits (length or angle bounds) + struct Limit + { + enum class Kind : std::uint8_t + { + TranslationMin, + TranslationMax, + RotationMin, + RotationMax, + }; + + Kind kind {}; + double value {0.0}; + double tolerance {1.0e-9}; + }; + std::vector limits; + + bool activated {true}; +}; + +// ── MotionDef ────────────────────────────────────────────────────── +// +// A motion driver for kinematic simulation. + +struct MotionDef +{ + enum class Kind : std::uint8_t + { + Rotational, + Translational, + General, + }; + + Kind kind {}; + std::string joint_id; // which constraint this drives + std::string marker_i; + std::string marker_j; + + // Motion law expressions (function of time 't'). + // For General: both are set. Otherwise only the relevant one. + std::string rotation_expr; + std::string translation_expr; +}; + +// ── SimulationParams ─────────────────────────────────────────────── +// +// Parameters for kinematic simulation (run_kinematic). +// Maps to create_mbdSimulationParameters() in AssemblyObject.cpp. + +struct SimulationParams +{ + double t_start {0.0}; + double t_end {1.0}; + double h_out {0.01}; // output time step + double h_min {1.0e-9}; + double h_max {1.0}; + double error_tol {1.0e-6}; +}; + +// ── SolveContext ─────────────────────────────────────────────────── +// +// Complete input to a solve operation. Built by the adapter layer +// from FreeCAD document objects. + +struct SolveContext +{ + std::vector parts; + std::vector constraints; + std::vector motions; + + // Present when running kinematic simulation via run_kinematic(). + std::optional simulation; + + // Hint: bundle parts connected by Fixed joints into single rigid bodies. + // When true and the solver does not support_bundle_fixed(), the adapter + // layer pre-bundles before passing to the solver. + bool bundle_fixed {false}; +}; + +// ── SolveStatus ──────────────────────────────────────────────────── +// +// Matches the return codes from AssemblyObject::solve(). + +enum class SolveStatus : std::int8_t +{ + Success = 0, + Failed = -1, + InvalidFlip = -2, // orientation flipped past threshold + NoGroundedParts = -6, // no grounded parts in assembly +}; + +// ── ConstraintDiagnostic ─────────────────────────────────────────── +// +// Per-constraint diagnostic information from updateSolveStatus(). + +struct ConstraintDiagnostic +{ + enum class Kind : std::uint8_t + { + Redundant, + Conflicting, + PartiallyRedundant, + Malformed, + }; + + std::string constraint_id; // FreeCAD object name + Kind kind {}; + std::string detail; // human-readable description +}; + +// ── SolveResult ──────────────────────────────────────────────────── +// +// Output of a solve operation. + +struct SolveResult +{ + SolveStatus status {SolveStatus::Success}; + + // Updated placements for each part (only parts that moved). + struct PartResult + { + std::string id; + Transform placement; + }; + std::vector placements; + + // Degrees of freedom remaining (-1 = unknown). + int dof {-1}; + + // Constraint diagnostics (redundant, conflicting, etc.). + std::vector diagnostics; + + // For kinematic simulation: number of computed frames. + std::size_t num_frames {0}; +}; + +} // namespace KCSolve + +#endif // KCSOLVE_TYPES_H -- 2.49.1 From 76b91c65974a3769ecd7b14d43f2b392ce94bb4e Mon Sep 17 00:00:00 2001 From: forbes Date: Thu, 19 Feb 2026 16:07:37 -0600 Subject: [PATCH 2/5] feat(solver): implement SolverRegistry with plugin loading (#293) Phase 1b of the pluggable solver system. Converts KCSolve from a header-only INTERFACE target to a SHARED library and implements the SolverRegistry with dynamic plugin discovery. Changes: - Add KCSolveGlobal.h export macro header (KCSolveExport) - Move SolverRegistry method bodies from header to SolverRegistry.cpp - Implement scan() with dlopen/LoadLibrary plugin loading - Add scan_default_paths() for KCSOLVE_PLUGIN_PATH + system paths - Plugin entry points: kcsolve_api_version() + kcsolve_create() - API version checking (major version compatibility) - Convert CMakeLists.txt from INTERFACE to SHARED library - Link FreeCADBase (PRIVATE) for Console logging - Link dl on POSIX for dynamic loading - Fix -Wmissing-field-initializers warnings in IKCSolver.h defaults The registry discovers plugins by scanning directories for shared libraries that export the kcsolve C entry points. Plugins are validated for API version compatibility before registration. Manual registration via register_solver() remains available for built-in solvers (e.g. OndselAdapter in Phase 1c). --- src/Mod/Assembly/Solver/CMakeLists.txt | 35 ++- src/Mod/Assembly/Solver/IKCSolver.h | 6 +- src/Mod/Assembly/Solver/KCSolveGlobal.h | 37 +++ src/Mod/Assembly/Solver/SolverRegistry.cpp | 346 +++++++++++++++++++++ src/Mod/Assembly/Solver/SolverRegistry.h | 96 ++---- 5 files changed, 445 insertions(+), 75 deletions(-) create mode 100644 src/Mod/Assembly/Solver/KCSolveGlobal.h create mode 100644 src/Mod/Assembly/Solver/SolverRegistry.cpp diff --git a/src/Mod/Assembly/Solver/CMakeLists.txt b/src/Mod/Assembly/Solver/CMakeLists.txt index 51717be77e..b3d78d5681 100644 --- a/src/Mod/Assembly/Solver/CMakeLists.txt +++ b/src/Mod/Assembly/Solver/CMakeLists.txt @@ -1,12 +1,39 @@ # SPDX-License-Identifier: LGPL-2.1-or-later -# Phase 1a: header-only INTERFACE library. -# Phase 1b will convert to SHARED when .cpp files are added. +set(KCSolve_SRCS + KCSolveGlobal.h + Types.h + IKCSolver.h + SolverRegistry.h + SolverRegistry.cpp +) -add_library(KCSolve INTERFACE) +add_library(KCSolve SHARED ${KCSolve_SRCS}) target_include_directories(KCSolve - INTERFACE + PUBLIC ${CMAKE_SOURCE_DIR}/src ${CMAKE_BINARY_DIR}/src ) + +target_compile_definitions(KCSolve + PRIVATE + CMAKE_INSTALL_PREFIX="${CMAKE_INSTALL_PREFIX}" +) + +target_link_libraries(KCSolve + PRIVATE + FreeCADBase +) + +# Platform-specific dynamic loading library +if(NOT WIN32) + target_link_libraries(KCSolve PRIVATE ${CMAKE_DL_LIBS}) +endif() + +if(FREECAD_WARN_ERROR) + target_compile_warn_error(KCSolve) +endif() + +SET_BIN_DIR(KCSolve KCSolve /Mod/Assembly) +INSTALL(TARGETS KCSolve DESTINATION ${CMAKE_INSTALL_LIBDIR}) diff --git a/src/Mod/Assembly/Solver/IKCSolver.h b/src/Mod/Assembly/Solver/IKCSolver.h index cd9ac78a9b..e81d3c6dd8 100644 --- a/src/Mod/Assembly/Solver/IKCSolver.h +++ b/src/Mod/Assembly/Solver/IKCSolver.h @@ -107,7 +107,7 @@ public: virtual SolveResult drag_step( const std::vector& /*drag_placements*/) { - return SolveResult {SolveStatus::Success}; + return SolveResult {SolveStatus::Success, {}, -1, {}, 0}; } /// End an interactive drag session and finalize state. @@ -123,7 +123,7 @@ public: /// Default: delegates to solve() (ignoring simulation params). virtual SolveResult run_kinematic(const SolveContext& /*ctx*/) { - return SolveResult {SolveStatus::Failed}; + return SolveResult {SolveStatus::Failed, {}, -1, {}, 0}; } /// Number of simulation frames available after run_kinematic(). @@ -136,7 +136,7 @@ public: /// @pre index < num_frames() virtual SolveResult update_for_frame(std::size_t /*index*/) { - return SolveResult {SolveStatus::Failed}; + return SolveResult {SolveStatus::Failed, {}, -1, {}, 0}; } // ── Diagnostics ──────────────────────────────────────────────── diff --git a/src/Mod/Assembly/Solver/KCSolveGlobal.h b/src/Mod/Assembly/Solver/KCSolveGlobal.h new file mode 100644 index 0000000000..4985ef6934 --- /dev/null +++ b/src/Mod/Assembly/Solver/KCSolveGlobal.h @@ -0,0 +1,37 @@ +// 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 + +#ifndef KCSOLVE_GLOBAL_H +#define KCSOLVE_GLOBAL_H + +#ifndef KCSolveExport +# ifdef KCSolve_EXPORTS +# define KCSolveExport FREECAD_DECL_EXPORT +# else +# define KCSolveExport FREECAD_DECL_IMPORT +# endif +#endif + +#endif // KCSOLVE_GLOBAL_H diff --git a/src/Mod/Assembly/Solver/SolverRegistry.cpp b/src/Mod/Assembly/Solver/SolverRegistry.cpp new file mode 100644 index 0000000000..a3544e939a --- /dev/null +++ b/src/Mod/Assembly/Solver/SolverRegistry.cpp @@ -0,0 +1,346 @@ +// 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 "SolverRegistry.h" + +#include + +#include +#include +#include +#include + +#ifdef _WIN32 +# define WIN32_LEAN_AND_MEAN +# include +#else +# include +#endif + +namespace fs = std::filesystem; + +namespace +{ + +// Platform extension for shared libraries. +#ifdef _WIN32 +constexpr const char* PLUGIN_EXT = ".dll"; +constexpr char PATH_SEP = ';'; +#elif defined(__APPLE__) +constexpr const char* PLUGIN_EXT = ".dylib"; +constexpr char PATH_SEP = ':'; +#else +constexpr const char* PLUGIN_EXT = ".so"; +constexpr char PATH_SEP = ':'; +#endif + +// Dynamic library loading wrappers. +void* open_library(const char* path) +{ +#ifdef _WIN32 + return static_cast(LoadLibraryA(path)); +#else + return dlopen(path, RTLD_LAZY); +#endif +} + +void* get_symbol(void* handle, const char* symbol) +{ +#ifdef _WIN32 + return reinterpret_cast( + GetProcAddress(static_cast(handle), symbol)); +#else + return dlsym(handle, symbol); +#endif +} + +std::string load_error() +{ +#ifdef _WIN32 + DWORD err = GetLastError(); + char* msg = nullptr; + FormatMessageA(FORMAT_MESSAGE_ALLOCATE_BUFFER | FORMAT_MESSAGE_FROM_SYSTEM, + nullptr, err, 0, reinterpret_cast(&msg), 0, nullptr); + std::string result = msg ? msg : "unknown error"; + LocalFree(msg); + return result; +#else + const char* err = dlerror(); + return err ? err : "unknown error"; +#endif +} + +/// Parse major version from a version string like "1.0" or "2.1.3". +/// Returns -1 on failure. +int parse_major_version(const char* version_str) +{ + if (!version_str) { + return -1; + } + char* end = nullptr; + long major = std::strtol(version_str, &end, 10); + if (end == version_str || major < 0) { + return -1; + } + return static_cast(major); +} + +} // anonymous namespace + + +namespace KCSolve +{ + +// Plugin C entry point types. +using ApiVersionFn = const char* (*)(); +using CreateFn = IKCSolver* (*)(); + + +// ── Singleton ────────────────────────────────────────────────────── + +SolverRegistry& SolverRegistry::instance() +{ + static SolverRegistry reg; + return reg; +} + +SolverRegistry::SolverRegistry() = default; + +SolverRegistry::~SolverRegistry() +{ + for (void* handle : handles_) { + close_handle(handle); + } +} + +void SolverRegistry::close_handle(void* handle) +{ + if (!handle) { + return; + } +#ifdef _WIN32 + FreeLibrary(static_cast(handle)); +#else + dlclose(handle); +#endif +} + + +// ── Registration ─────────────────────────────────────────────────── + +bool SolverRegistry::register_solver(const std::string& name, CreateSolverFn factory) +{ + std::lock_guard lock(mutex_); + auto [it, inserted] = factories_.emplace(name, std::move(factory)); + if (!inserted) { + Base::Console().warning("KCSolve: solver '%s' already registered, skipping\n", + name.c_str()); + return false; + } + if (default_name_.empty()) { + default_name_ = name; + } + Base::Console().log("KCSolve: registered solver '%s'\n", name.c_str()); + return true; +} + + +// ── Lookup ───────────────────────────────────────────────────────── + +std::unique_ptr SolverRegistry::get(const std::string& name) const +{ + std::lock_guard lock(mutex_); + const std::string& key = name.empty() ? default_name_ : name; + if (key.empty()) { + return nullptr; + } + auto it = factories_.find(key); + if (it == factories_.end()) { + return nullptr; + } + return it->second(); +} + +std::vector SolverRegistry::available() const +{ + std::lock_guard lock(mutex_); + std::vector names; + names.reserve(factories_.size()); + for (const auto& [name, _] : factories_) { + names.push_back(name); + } + return names; +} + +std::vector SolverRegistry::joints_for(const std::string& name) const +{ + auto solver = get(name); + if (!solver) { + return {}; + } + return solver->supported_joints(); +} + +bool SolverRegistry::set_default(const std::string& name) +{ + std::lock_guard lock(mutex_); + if (factories_.find(name) == factories_.end()) { + return false; + } + default_name_ = name; + return true; +} + +std::string SolverRegistry::get_default() const +{ + std::lock_guard lock(mutex_); + return default_name_; +} + + +// ── Plugin scanning ──────────────────────────────────────────────── + +void SolverRegistry::scan(const std::string& directory) +{ + std::error_code ec; + if (!fs::is_directory(directory, ec)) { + // Non-existent directories are not an error — just skip. + return; + } + + Base::Console().log("KCSolve: scanning '%s' for plugins\n", directory.c_str()); + + for (const auto& entry : fs::directory_iterator(directory, ec)) { + if (ec) { + Base::Console().warning("KCSolve: error iterating '%s': %s\n", + directory.c_str(), ec.message().c_str()); + break; + } + + if (!entry.is_regular_file(ec)) { + continue; + } + + const auto& path = entry.path(); + if (path.extension() != PLUGIN_EXT) { + continue; + } + + const std::string path_str = path.string(); + + // Load the shared library. + void* handle = open_library(path_str.c_str()); + if (!handle) { + Base::Console().warning("KCSolve: failed to load '%s': %s\n", + path_str.c_str(), load_error().c_str()); + continue; + } + + // Check API version. + auto version_fn = reinterpret_cast( + get_symbol(handle, "kcsolve_api_version")); + if (!version_fn) { + // Not a KCSolve plugin — silently skip. + close_handle(handle); + continue; + } + + const char* version_str = version_fn(); + int major = parse_major_version(version_str); + if (major != API_VERSION_MAJOR) { + Base::Console().warning( + "KCSolve: plugin '%s' has incompatible API version '%s' " + "(expected major %d)\n", + path_str.c_str(), + version_str ? version_str : "(null)", + API_VERSION_MAJOR); + close_handle(handle); + continue; + } + + // Get the factory symbol. + auto create_fn = reinterpret_cast( + get_symbol(handle, "kcsolve_create")); + if (!create_fn) { + Base::Console().warning( + "KCSolve: plugin '%s' missing kcsolve_create() symbol\n", + path_str.c_str()); + close_handle(handle); + continue; + } + + // Create a temporary instance to get the solver name. + std::unique_ptr probe(create_fn()); + if (!probe) { + Base::Console().warning( + "KCSolve: plugin '%s' kcsolve_create() returned null\n", + path_str.c_str()); + close_handle(handle); + continue; + } + + std::string solver_name = probe->name(); + probe.reset(); + + // Wrap the C function pointer in a factory lambda. + CreateSolverFn factory = [create_fn]() -> std::unique_ptr { + return std::unique_ptr(create_fn()); + }; + + if (register_solver(solver_name, std::move(factory))) { + handles_.push_back(handle); + Base::Console().log("KCSolve: loaded plugin '%s' from '%s'\n", + solver_name.c_str(), path_str.c_str()); + } + else { + // Duplicate name — close the handle. + close_handle(handle); + } + } +} + +void SolverRegistry::scan_default_paths() +{ + // 1. KCSOLVE_PLUGIN_PATH environment variable. + const char* env_path = std::getenv("KCSOLVE_PLUGIN_PATH"); + if (env_path && env_path[0] != '\0') { + std::istringstream stream(env_path); + std::string dir; + while (std::getline(stream, dir, PATH_SEP)) { + if (!dir.empty()) { + scan(dir); + } + } + } + + // 2. System install path: /lib/kcsolve/ + // Derive from the executable location or use a compile-time path. + // For now, use a path relative to the FreeCAD lib directory. + std::error_code ec; + fs::path system_dir = fs::path(CMAKE_INSTALL_PREFIX) / "lib" / "kcsolve"; + if (fs::is_directory(system_dir, ec)) { + scan(system_dir.string()); + } +} + +} // namespace KCSolve diff --git a/src/Mod/Assembly/Solver/SolverRegistry.h b/src/Mod/Assembly/Solver/SolverRegistry.h index ea0dee13cc..056274dc33 100644 --- a/src/Mod/Assembly/Solver/SolverRegistry.h +++ b/src/Mod/Assembly/Solver/SolverRegistry.h @@ -32,6 +32,7 @@ #include #include "IKCSolver.h" +#include "KCSolveGlobal.h" namespace KCSolve { @@ -39,6 +40,9 @@ namespace KCSolve /// Factory function that creates a solver instance. using CreateSolverFn = std::function()>; +/// Current KCSolve API major version. Plugins must match this to load. +constexpr int API_VERSION_MAJOR = 1; + /// Singleton registry for pluggable solver backends. /// /// Solver plugins register themselves at module load time via @@ -55,108 +59,64 @@ using CreateSolverFn = std::function()>; /// auto solver = KCSolve::SolverRegistry::instance().get(); // default /// auto solver = KCSolve::SolverRegistry::instance().get("ondsel"); -class SolverRegistry +class KCSolveExport SolverRegistry { public: /// Access the singleton instance. - static SolverRegistry& instance() - { - static SolverRegistry reg; - return reg; - } + static SolverRegistry& instance(); + + ~SolverRegistry(); /// Register a solver backend. /// @param name Unique solver name (e.g. "ondsel"). /// @param factory Factory function that creates solver instances. /// @return true if registration succeeded, false if name taken. - bool register_solver(const std::string& name, CreateSolverFn factory) - { - std::lock_guard lock(mutex_); - auto [it, inserted] = factories_.emplace(name, std::move(factory)); - if (inserted && default_name_.empty()) { - default_name_ = name; // first registered becomes default - } - return inserted; - } + bool register_solver(const std::string& name, CreateSolverFn factory); /// Create an instance of the named solver. /// @param name Solver name. If empty, uses the default solver. /// @return Solver instance, or nullptr if not found. - std::unique_ptr get(const std::string& name = {}) const - { - std::lock_guard lock(mutex_); - const std::string& key = name.empty() ? default_name_ : name; - if (key.empty()) { - return nullptr; - } - auto it = factories_.find(key); - if (it == factories_.end()) { - return nullptr; - } - return it->second(); - } + std::unique_ptr get(const std::string& name = {}) const; /// Return the names of all registered solvers. - std::vector available() const - { - std::lock_guard lock(mutex_); - std::vector names; - names.reserve(factories_.size()); - for (const auto& [name, _] : factories_) { - names.push_back(name); - } - return names; - } + std::vector available() const; /// Query which BaseJointKind values a named solver supports. /// Creates a temporary instance to call supported_joints(). - std::vector joints_for(const std::string& name) const - { - auto solver = get(name); - if (!solver) { - return {}; - } - return solver->supported_joints(); - } + std::vector joints_for(const std::string& name) const; /// Set the default solver name. /// @return true if the name is registered, false otherwise. - bool set_default(const std::string& name) - { - std::lock_guard lock(mutex_); - if (factories_.find(name) == factories_.end()) { - return false; - } - default_name_ = name; - return true; - } + bool set_default(const std::string& name); /// Get the default solver name. - std::string get_default() const - { - std::lock_guard lock(mutex_); - return default_name_; - } + std::string get_default() const; - /// Scan a directory for solver plugins (Phase 1b). - /// Currently a no-op placeholder. Will dlopen/LoadLibrary shared - /// objects that export kcsolve_create() / kcsolve_api_version(). - void scan(const std::string& /*directory*/) - { - } + /// Scan a directory for solver plugin shared libraries. + /// Each plugin must export kcsolve_api_version() and kcsolve_create(). + /// Non-existent or empty directories are handled gracefully. + void scan(const std::string& directory); + + /// Scan all default plugin discovery paths: + /// 1. KCSOLVE_PLUGIN_PATH env var (colon-separated, semicolon on Windows) + /// 2. /lib/kcsolve/ + void scan_default_paths(); private: - SolverRegistry() = default; - ~SolverRegistry() = default; + SolverRegistry(); SolverRegistry(const SolverRegistry&) = delete; SolverRegistry& operator=(const SolverRegistry&) = delete; SolverRegistry(SolverRegistry&&) = delete; SolverRegistry& operator=(SolverRegistry&&) = delete; + /// Close a single plugin handle (platform-specific). + static void close_handle(void* handle); + mutable std::mutex mutex_; std::unordered_map factories_; std::string default_name_; + std::vector handles_; // loaded plugin library handles }; } // namespace KCSolve -- 2.49.1 From 32dbe20ce037d5fbe73564e2359880488e7a2cdc Mon Sep 17 00:00:00 2001 From: forbes Date: Thu, 19 Feb 2026 16:19:44 -0600 Subject: [PATCH 3/5] 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 -- 2.49.1 From 5c33aacecbbb48b480570d28f43174ae16b32e4e Mon Sep 17 00:00:00 2001 From: forbes Date: Thu, 19 Feb 2026 16:43:52 -0600 Subject: [PATCH 4/5] feat(solver): refactor AssemblyObject to use IKCSolver interface (#295) Rewire AssemblyObject to call through KCSolve::IKCSolver instead of directly manipulating OndselSolver ASMT types. Key changes: - Remove all 30+ #include from AssemblyObject.cpp - Remove MbDPartData, objectPartMap, mbdAssembly members - Add solver_ (IKCSolver), lastResult_ (SolveResult), partIdToObjs_ maps - New buildSolveContext() builds SolveContext from FreeCAD document objects with JointType/DistanceType -> BaseJointKind decomposition - New computeMarkerTransform() replaces handleOneSideOfJoint() - New computeRackPinionMarkers() replaces getRackPinionMarkers() - Rewrite solve/preDrag/doDragStep/postDrag/generateSimulation to call through IKCSolver interface - Rewrite setNewPlacements/validateNewPlacements to use SolveResult - Rewrite updateSolveStatus to use ConstraintDiagnostic - Add export_native() to IKCSolver for ASMT export support - Register OndselAdapter at Assembly module init in AppAssembly.cpp - Remove OndselSolver from Assembly_LIBS (linked transitively via KCSolve) Assembly module now has zero OndselSolver includes. All solver coupling is confined to KCSolve/OndselAdapter.cpp. --- src/Mod/Assembly/App/AppAssembly.cpp | 6 + src/Mod/Assembly/App/AssemblyObject.cpp | 2080 ++++++++++----------- src/Mod/Assembly/App/AssemblyObject.h | 104 +- src/Mod/Assembly/App/CMakeLists.txt | 1 - src/Mod/Assembly/Solver/IKCSolver.h | 6 + src/Mod/Assembly/Solver/OndselAdapter.cpp | 9 + src/Mod/Assembly/Solver/OndselAdapter.h | 1 + 7 files changed, 1092 insertions(+), 1115 deletions(-) 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. -- 2.49.1 From 934cdf57673e26bbee34ceebd26e648869f4798b Mon Sep 17 00:00:00 2001 From: forbes Date: Thu, 19 Feb 2026 16:56:11 -0600 Subject: [PATCH 5/5] test(assembly): regression tests for KCSolve solver refactor (#296) MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit Phase 1e: Add C++ gtest and Python unittest coverage for the pluggable solver system introduced in Phases 1a-1d. C++ tests (KCSolve_tests_run): - SolverRegistryTest (8 tests): register/get, duplicates, defaults, available list, joints_for capability queries - OndselAdapterTest (10 tests): identity checks, supported/unsupported joints, Fixed/Revolute solve round-trips, no-grounded-parts handling, exception safety, drag protocol (pre_drag/drag_step/post_drag), redundant constraint diagnostics Python tests (TestSolverIntegration): - Full-stack solve via AssemblyObject → IKCSolver → OndselAdapter - Fixed joint placement matching, revolute joint success - No-ground error code (-6), redundancy detection (-2) - ASMT export produces non-empty file - Deterministic solve stability (solve twice → same result) --- .../AssemblyTests/TestSolverIntegration.py | 216 +++++++++++++++ src/Mod/Assembly/CMakeLists.txt | 1 + src/Mod/Assembly/TestAssemblyWorkbench.py | 6 +- tests/CMakeLists.txt | 1 + tests/src/Mod/Assembly/CMakeLists.txt | 1 + tests/src/Mod/Assembly/Solver/CMakeLists.txt | 13 + .../src/Mod/Assembly/Solver/OndselAdapter.cpp | 251 ++++++++++++++++++ .../Mod/Assembly/Solver/SolverRegistry.cpp | 131 +++++++++ 8 files changed, 617 insertions(+), 3 deletions(-) create mode 100644 src/Mod/Assembly/AssemblyTests/TestSolverIntegration.py create mode 100644 tests/src/Mod/Assembly/Solver/CMakeLists.txt create mode 100644 tests/src/Mod/Assembly/Solver/OndselAdapter.cpp create mode 100644 tests/src/Mod/Assembly/Solver/SolverRegistry.cpp diff --git a/src/Mod/Assembly/AssemblyTests/TestSolverIntegration.py b/src/Mod/Assembly/AssemblyTests/TestSolverIntegration.py new file mode 100644 index 0000000000..7062876e74 --- /dev/null +++ b/src/Mod/Assembly/AssemblyTests/TestSolverIntegration.py @@ -0,0 +1,216 @@ +# 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 * +# . * +# * +# ***************************************************************************/ + +""" +Solver integration tests for Phase 1e (KCSolve refactor). + +These tests verify that the AssemblyObject → IKCSolver → OndselAdapter pipeline +produces correct results via the full FreeCAD stack. They complement the C++ +unit tests in tests/src/Mod/Assembly/Solver/. +""" + +import os +import tempfile +import unittest + +import FreeCAD as App +import JointObject + + +class TestSolverIntegration(unittest.TestCase): + """Full-stack solver regression tests exercising AssemblyObject.solve().""" + + def setUp(self): + doc_name = self.__class__.__name__ + if App.ActiveDocument: + if App.ActiveDocument.Name != doc_name: + App.newDocument(doc_name) + else: + App.newDocument(doc_name) + App.setActiveDocument(doc_name) + self.doc = App.ActiveDocument + + self.assembly = self.doc.addObject("Assembly::AssemblyObject", "Assembly") + self.jointgroup = self.assembly.newObject("Assembly::JointGroup", "Joints") + + def tearDown(self): + App.closeDocument(self.doc.Name) + + # ── Helpers ───────────────────────────────────────────────────── + + def _make_box(self, x=0, y=0, z=0, size=10): + """Create a Part::Box inside the assembly with a given offset.""" + box = self.assembly.newObject("Part::Box", "Box") + box.Length = size + box.Width = size + box.Height = size + box.Placement = App.Placement(App.Vector(x, y, z), App.Rotation()) + return box + + def _ground(self, obj): + """Create a grounded joint for the given object.""" + gnd = self.jointgroup.newObject("App::FeaturePython", "GroundedJoint") + JointObject.GroundedJoint(gnd, obj) + return gnd + + def _make_joint(self, joint_type, ref1, ref2): + """Create a joint of the given type connecting two (obj, subelements) pairs. + + joint_type: integer JointType enum value (0=Fixed, 1=Revolute, etc.) + ref1, ref2: tuples of (obj, [sub_element, sub_element]) + """ + joint = self.jointgroup.newObject("App::FeaturePython", "Joint") + JointObject.Joint(joint, joint_type) + + refs = [ + [ref1[0], ref1[1]], + [ref2[0], ref2[1]], + ] + joint.Proxy.setJointConnectors(joint, refs) + return joint + + # ── Tests ─────────────────────────────────────────────────────── + + def test_solve_fixed_joint(self): + """Two boxes + grounded + fixed joint → placements match.""" + box1 = self._make_box(10, 20, 30) + box2 = self._make_box(40, 50, 60) + self._ground(box2) + + # Fixed joint (type 0) connecting Face6+Vertex7 on each box. + self._make_joint( + 0, + [box2, ["Face6", "Vertex7"]], + [box1, ["Face6", "Vertex7"]], + ) + + # After setJointConnectors, solve() was already called internally. + # Verify that box1 moved to match box2. + self.assertTrue( + box1.Placement.isSame(box2.Placement, 1e-6), + "Fixed joint: box1 should match box2 placement", + ) + + def test_solve_revolute_joint(self): + """Two boxes + grounded + revolute joint → solve succeeds (return 0).""" + box1 = self._make_box(0, 0, 0) + box2 = self._make_box(100, 0, 0) + self._ground(box1) + + # Revolute joint (type 1) connecting Face6+Vertex7. + self._make_joint( + 1, + [box1, ["Face6", "Vertex7"]], + [box2, ["Face6", "Vertex7"]], + ) + + result = self.assembly.solve() + self.assertEqual(result, 0, "Revolute joint solve should succeed") + + def test_solve_returns_code_for_no_ground(self): + """Assembly with no grounded parts → solve returns -6.""" + box1 = self._make_box(0, 0, 0) + box2 = self._make_box(50, 0, 0) + + # Fixed joint but no ground. + joint = self.jointgroup.newObject("App::FeaturePython", "Joint") + JointObject.Joint(joint, 0) + refs = [ + [box1, ["Face6", "Vertex7"]], + [box2, ["Face6", "Vertex7"]], + ] + joint.Proxy.setJointConnectors(joint, refs) + + result = self.assembly.solve() + self.assertEqual(result, -6, "No grounded parts should return -6") + + def test_solve_returns_redundancy(self): + """Over-constrained assembly → solve returns -2 (redundant).""" + box1 = self._make_box(0, 0, 0) + box2 = self._make_box(50, 0, 0) + self._ground(box1) + + # Two fixed joints between the same faces → redundant. + self._make_joint( + 0, + [box1, ["Face6", "Vertex7"]], + [box2, ["Face6", "Vertex7"]], + ) + self._make_joint( + 0, + [box1, ["Face5", "Vertex5"]], + [box2, ["Face5", "Vertex5"]], + ) + + result = self.assembly.solve() + self.assertEqual(result, -2, "Redundant constraints should return -2") + + def test_export_asmt(self): + """exportAsASMT() produces a non-empty file.""" + box1 = self._make_box(0, 0, 0) + box2 = self._make_box(50, 0, 0) + self._ground(box1) + + self._make_joint( + 0, + [box1, ["Face6", "Vertex7"]], + [box2, ["Face6", "Vertex7"]], + ) + + self.assembly.solve() + + with tempfile.NamedTemporaryFile(suffix=".asmt", delete=False) as f: + path = f.name + + try: + self.assembly.exportAsASMT(path) + self.assertTrue(os.path.exists(path), "ASMT file should exist") + self.assertGreater( + os.path.getsize(path), 0, "ASMT file should be non-empty" + ) + finally: + if os.path.exists(path): + os.unlink(path) + + def test_solve_multiple_times_stable(self): + """Solving the same assembly twice produces identical placements.""" + box1 = self._make_box(10, 20, 30) + box2 = self._make_box(40, 50, 60) + self._ground(box2) + + self._make_joint( + 0, + [box2, ["Face6", "Vertex7"]], + [box1, ["Face6", "Vertex7"]], + ) + + self.assembly.solve() + plc_first = App.Placement(box1.Placement) + + self.assembly.solve() + plc_second = box1.Placement + + self.assertTrue( + plc_first.isSame(plc_second, 1e-6), + "Deterministic solver should produce identical results", + ) diff --git a/src/Mod/Assembly/CMakeLists.txt b/src/Mod/Assembly/CMakeLists.txt index 1c51a9a13f..0505b216fa 100644 --- a/src/Mod/Assembly/CMakeLists.txt +++ b/src/Mod/Assembly/CMakeLists.txt @@ -57,6 +57,7 @@ SET(AssemblyTests_SRCS AssemblyTests/__init__.py AssemblyTests/TestCore.py AssemblyTests/TestCommandInsertLink.py + AssemblyTests/TestSolverIntegration.py AssemblyTests/mocks/__init__.py AssemblyTests/mocks/MockGui.py ) diff --git a/src/Mod/Assembly/TestAssemblyWorkbench.py b/src/Mod/Assembly/TestAssemblyWorkbench.py index 911a298a6a..387f4e4e14 100644 --- a/src/Mod/Assembly/TestAssemblyWorkbench.py +++ b/src/Mod/Assembly/TestAssemblyWorkbench.py @@ -22,11 +22,11 @@ # **************************************************************************/ import TestApp - -from AssemblyTests.TestCore import TestCore from AssemblyTests.TestCommandInsertLink import TestCommandInsertLink - +from AssemblyTests.TestCore import TestCore +from AssemblyTests.TestSolverIntegration import TestSolverIntegration # Use the modules so that code checkers don't complain (flake8) True if TestCore else False True if TestCommandInsertLink else False +True if TestSolverIntegration else False diff --git a/tests/CMakeLists.txt b/tests/CMakeLists.txt index da69d6f09c..6c9e3a42ea 100644 --- a/tests/CMakeLists.txt +++ b/tests/CMakeLists.txt @@ -95,6 +95,7 @@ if(BUILD_GUI) endif() if(BUILD_ASSEMBLY) list (APPEND TestExecutables Assembly_tests_run) + list (APPEND TestExecutables KCSolve_tests_run) endif(BUILD_ASSEMBLY) if(BUILD_MATERIAL) list (APPEND TestExecutables Material_tests_run) diff --git a/tests/src/Mod/Assembly/CMakeLists.txt b/tests/src/Mod/Assembly/CMakeLists.txt index 1d3bb763b3..787d743e2d 100644 --- a/tests/src/Mod/Assembly/CMakeLists.txt +++ b/tests/src/Mod/Assembly/CMakeLists.txt @@ -1,6 +1,7 @@ # SPDX-License-Identifier: LGPL-2.1-or-later add_subdirectory(App) +add_subdirectory(Solver) if (NOT FREECAD_USE_EXTERNAL_ONDSELSOLVER) target_include_directories(Assembly_tests_run PUBLIC diff --git a/tests/src/Mod/Assembly/Solver/CMakeLists.txt b/tests/src/Mod/Assembly/Solver/CMakeLists.txt new file mode 100644 index 0000000000..7160170070 --- /dev/null +++ b/tests/src/Mod/Assembly/Solver/CMakeLists.txt @@ -0,0 +1,13 @@ +# SPDX-License-Identifier: LGPL-2.1-or-later + +add_executable(KCSolve_tests_run + SolverRegistry.cpp + OndselAdapter.cpp +) + +target_link_libraries(KCSolve_tests_run + gtest_main + ${Google_Tests_LIBS} + KCSolve + FreeCADApp +) diff --git a/tests/src/Mod/Assembly/Solver/OndselAdapter.cpp b/tests/src/Mod/Assembly/Solver/OndselAdapter.cpp new file mode 100644 index 0000000000..be475bd808 --- /dev/null +++ b/tests/src/Mod/Assembly/Solver/OndselAdapter.cpp @@ -0,0 +1,251 @@ +// SPDX-License-Identifier: LGPL-2.1-or-later + +#include + +#include + +#include +#include +#include +#include +#include + +#include +#include + +using namespace KCSolve; + +// ── Fixture ──────────────────────────────────────────────────────── + +class OndselAdapterTest : public ::testing::Test +{ +protected: + static void SetUpTestSuite() + { + tests::initApplication(); + } + + void SetUp() override + { + adapter_ = std::make_unique(); + } + + /// Build a minimal two-part context with a single constraint. + static SolveContext twoPartContext(BaseJointKind jointKind, + bool groundFirst = true) + { + SolveContext ctx; + + Part p1; + p1.id = "Part1"; + p1.placement = Transform::identity(); + p1.grounded = groundFirst; + ctx.parts.push_back(p1); + + Part p2; + p2.id = "Part2"; + p2.placement = Transform::identity(); + p2.placement.position = {100.0, 0.0, 0.0}; + p2.grounded = false; + ctx.parts.push_back(p2); + + Constraint c; + c.id = "Joint1"; + c.part_i = "Part1"; + c.marker_i = Transform::identity(); + c.part_j = "Part2"; + c.marker_j = Transform::identity(); + c.type = jointKind; + ctx.constraints.push_back(c); + + return ctx; + } + + std::unique_ptr adapter_; +}; + + +// ── Identity / capability tests ──────────────────────────────────── + +TEST_F(OndselAdapterTest, Name) // NOLINT +{ + auto n = adapter_->name(); + EXPECT_FALSE(n.empty()); + EXPECT_NE(n.find("Ondsel"), std::string::npos); +} + +TEST_F(OndselAdapterTest, SupportedJoints) // NOLINT +{ + auto joints = adapter_->supported_joints(); + EXPECT_FALSE(joints.empty()); + + // Must include core kinematic joints. + EXPECT_NE(std::find(joints.begin(), joints.end(), BaseJointKind::Fixed), joints.end()); + EXPECT_NE(std::find(joints.begin(), joints.end(), BaseJointKind::Revolute), joints.end()); + EXPECT_NE(std::find(joints.begin(), joints.end(), BaseJointKind::Cylindrical), joints.end()); + EXPECT_NE(std::find(joints.begin(), joints.end(), BaseJointKind::Ball), joints.end()); + + // Must exclude unsupported types. + EXPECT_EQ(std::find(joints.begin(), joints.end(), BaseJointKind::Universal), joints.end()); + EXPECT_EQ(std::find(joints.begin(), joints.end(), BaseJointKind::Cam), joints.end()); + EXPECT_EQ(std::find(joints.begin(), joints.end(), BaseJointKind::Slot), joints.end()); +} + +TEST_F(OndselAdapterTest, IsDeterministic) // NOLINT +{ + EXPECT_TRUE(adapter_->is_deterministic()); +} + +TEST_F(OndselAdapterTest, SupportsBundleFixed) // NOLINT +{ + EXPECT_FALSE(adapter_->supports_bundle_fixed()); +} + + +// ── Solve round-trips ────────────────────────────────────────────── + +TEST_F(OndselAdapterTest, SolveFixedJoint) // NOLINT +{ + auto ctx = twoPartContext(BaseJointKind::Fixed); + auto result = adapter_->solve(ctx); + + EXPECT_EQ(result.status, SolveStatus::Success); + EXPECT_FALSE(result.placements.empty()); + + // Both parts should end up at the same position (fixed joint). + const auto* pr1 = &result.placements[0]; + const auto* pr2 = &result.placements[1]; + if (pr1->id == "Part2") { + std::swap(pr1, pr2); + } + + // Part1 is grounded — should remain at origin. + EXPECT_NEAR(pr1->placement.position[0], 0.0, 1e-3); + EXPECT_NEAR(pr1->placement.position[1], 0.0, 1e-3); + EXPECT_NEAR(pr1->placement.position[2], 0.0, 1e-3); + + // Part2 should be pulled to Part1's position by the fixed joint + // (markers are both identity, so the parts are welded at the same point). + EXPECT_NEAR(pr2->placement.position[0], 0.0, 1e-3); + EXPECT_NEAR(pr2->placement.position[1], 0.0, 1e-3); + EXPECT_NEAR(pr2->placement.position[2], 0.0, 1e-3); +} + +TEST_F(OndselAdapterTest, SolveRevoluteJoint) // NOLINT +{ + auto ctx = twoPartContext(BaseJointKind::Revolute); + auto result = adapter_->solve(ctx); + + EXPECT_EQ(result.status, SolveStatus::Success); + EXPECT_FALSE(result.placements.empty()); +} + +TEST_F(OndselAdapterTest, SolveNoGroundedParts) // NOLINT +{ + // OndselAdapter itself doesn't require grounded parts — that check + // lives in AssemblyObject. The solver should still attempt to solve. + auto ctx = twoPartContext(BaseJointKind::Fixed, /*groundFirst=*/false); + auto result = adapter_->solve(ctx); + + // May succeed or fail depending on OndselSolver's behavior, but must not crash. + EXPECT_TRUE(result.status == SolveStatus::Success + || result.status == SolveStatus::Failed); +} + +TEST_F(OndselAdapterTest, SolveCatchesException) // NOLINT +{ + // Malformed context: constraint references non-existent parts. + SolveContext ctx; + + Part p; + p.id = "LonePart"; + p.placement = Transform::identity(); + p.grounded = true; + ctx.parts.push_back(p); + + Constraint c; + c.id = "BadJoint"; + c.part_i = "DoesNotExist"; + c.marker_i = Transform::identity(); + c.part_j = "AlsoDoesNotExist"; + c.marker_j = Transform::identity(); + c.type = BaseJointKind::Fixed; + ctx.constraints.push_back(c); + + // Should not crash — returns Failed or succeeds with warnings. + auto result = adapter_->solve(ctx); + SUCCEED(); // If we get here without crashing, the test passes. +} + + +// ── Drag protocol ────────────────────────────────────────────────── + +TEST_F(OndselAdapterTest, DragProtocol) // NOLINT +{ + auto ctx = twoPartContext(BaseJointKind::Revolute); + + auto preResult = adapter_->pre_drag(ctx, {"Part2"}); + EXPECT_EQ(preResult.status, SolveStatus::Success); + + // Move Part2 slightly. + SolveResult::PartResult dragPlc; + dragPlc.id = "Part2"; + dragPlc.placement = Transform::identity(); + dragPlc.placement.position = {10.0, 5.0, 0.0}; + + auto stepResult = adapter_->drag_step({dragPlc}); + // drag_step may fail if the solver can't converge — that's OK. + EXPECT_TRUE(stepResult.status == SolveStatus::Success + || stepResult.status == SolveStatus::Failed); + + // post_drag must not crash. + adapter_->post_drag(); + SUCCEED(); +} + + +// ── Diagnostics ──────────────────────────────────────────────────── + +TEST_F(OndselAdapterTest, DiagnoseRedundant) // NOLINT +{ + // Over-constrained: two fixed joints between the same two parts. + SolveContext ctx; + + Part p1; + p1.id = "PartA"; + p1.placement = Transform::identity(); + p1.grounded = true; + ctx.parts.push_back(p1); + + Part p2; + p2.id = "PartB"; + p2.placement = Transform::identity(); + p2.placement.position = {50.0, 0.0, 0.0}; + p2.grounded = false; + ctx.parts.push_back(p2); + + Constraint c1; + c1.id = "FixedJoint1"; + c1.part_i = "PartA"; + c1.marker_i = Transform::identity(); + c1.part_j = "PartB"; + c1.marker_j = Transform::identity(); + c1.type = BaseJointKind::Fixed; + ctx.constraints.push_back(c1); + + Constraint c2; + c2.id = "FixedJoint2"; + c2.part_i = "PartA"; + c2.marker_i = Transform::identity(); + c2.part_j = "PartB"; + c2.marker_j = Transform::identity(); + c2.type = BaseJointKind::Fixed; + ctx.constraints.push_back(c2); + + auto diags = adapter_->diagnose(ctx); + // With two identical fixed joints, one must be redundant. + bool hasRedundant = std::any_of(diags.begin(), diags.end(), [](const auto& d) { + return d.kind == ConstraintDiagnostic::Kind::Redundant; + }); + EXPECT_TRUE(hasRedundant); +} diff --git a/tests/src/Mod/Assembly/Solver/SolverRegistry.cpp b/tests/src/Mod/Assembly/Solver/SolverRegistry.cpp new file mode 100644 index 0000000000..5c29027620 --- /dev/null +++ b/tests/src/Mod/Assembly/Solver/SolverRegistry.cpp @@ -0,0 +1,131 @@ +// SPDX-License-Identifier: LGPL-2.1-or-later + +#include + +#include +#include +#include + +#include + +using namespace KCSolve; + +// ── Minimal mock solver for registry tests ───────────────────────── + +namespace +{ + +class MockSolver : public IKCSolver +{ +public: + std::string name() const override + { + return "MockSolver"; + } + + std::vector supported_joints() const override + { + return {BaseJointKind::Fixed, BaseJointKind::Revolute}; + } + + SolveResult solve(const SolveContext& /*ctx*/) override + { + return SolveResult {SolveStatus::Success, {}, 0, {}, 0}; + } +}; + +} // namespace + + +// ── Tests ────────────────────────────────────────────────────────── +// +// SolverRegistry is a singleton — tests use unique names to avoid +// interference across test cases. + +TEST(SolverRegistryTest, GetUnknownReturnsNull) // NOLINT +{ + auto solver = SolverRegistry::instance().get("nonexistent_solver_xyz"); + EXPECT_EQ(solver, nullptr); +} + +TEST(SolverRegistryTest, RegisterAndGet) // NOLINT +{ + auto& reg = SolverRegistry::instance(); + + bool ok = reg.register_solver("test_reg_get", + []() { return std::make_unique(); }); + EXPECT_TRUE(ok); + + auto solver = reg.get("test_reg_get"); + ASSERT_NE(solver, nullptr); + EXPECT_EQ(solver->name(), "MockSolver"); +} + +TEST(SolverRegistryTest, DuplicateRegistrationFails) // NOLINT +{ + auto& reg = SolverRegistry::instance(); + + bool first = reg.register_solver("test_dup", + []() { return std::make_unique(); }); + EXPECT_TRUE(first); + + bool second = reg.register_solver("test_dup", + []() { return std::make_unique(); }); + EXPECT_FALSE(second); +} + +TEST(SolverRegistryTest, AvailableListsSolvers) // NOLINT +{ + auto& reg = SolverRegistry::instance(); + + reg.register_solver("test_avail_1", + []() { return std::make_unique(); }); + reg.register_solver("test_avail_2", + []() { return std::make_unique(); }); + + auto names = reg.available(); + EXPECT_NE(std::find(names.begin(), names.end(), "test_avail_1"), names.end()); + EXPECT_NE(std::find(names.begin(), names.end(), "test_avail_2"), names.end()); +} + +TEST(SolverRegistryTest, SetDefaultAndGet) // NOLINT +{ + auto& reg = SolverRegistry::instance(); + + reg.register_solver("test_default", + []() { return std::make_unique(); }); + + bool ok = reg.set_default("test_default"); + EXPECT_TRUE(ok); + + // get() with no arg should return the default. + auto solver = reg.get(); + ASSERT_NE(solver, nullptr); + EXPECT_EQ(solver->name(), "MockSolver"); +} + +TEST(SolverRegistryTest, SetDefaultUnknownFails) // NOLINT +{ + auto& reg = SolverRegistry::instance(); + bool ok = reg.set_default("totally_unknown_solver"); + EXPECT_FALSE(ok); +} + +TEST(SolverRegistryTest, JointsForReturnsCapabilities) // NOLINT +{ + auto& reg = SolverRegistry::instance(); + + reg.register_solver("test_joints", + []() { return std::make_unique(); }); + + auto joints = reg.joints_for("test_joints"); + EXPECT_EQ(joints.size(), 2u); + EXPECT_NE(std::find(joints.begin(), joints.end(), BaseJointKind::Fixed), joints.end()); + EXPECT_NE(std::find(joints.begin(), joints.end(), BaseJointKind::Revolute), joints.end()); +} + +TEST(SolverRegistryTest, JointsForUnknownReturnsEmpty) // NOLINT +{ + auto joints = SolverRegistry::instance().joints_for("totally_unknown_solver_2"); + EXPECT_TRUE(joints.empty()); +} -- 2.49.1