From c3a0b2491a317654750be6d4446a49bfd8e0c8b2 Mon Sep 17 00:00:00 2001 From: saltyjoss Date: Sat, 16 May 2026 10:28:31 +0100 Subject: [PATCH 001/210] feat: Added Scalar template types for Eigen vectors and matrices --- PxMLib/MathLib/include/core/Types_tpl.h | 22 ++++++++++++++++++++++ 1 file changed, 22 insertions(+) create mode 100644 PxMLib/MathLib/include/core/Types_tpl.h diff --git a/PxMLib/MathLib/include/core/Types_tpl.h b/PxMLib/MathLib/include/core/Types_tpl.h new file mode 100644 index 00000000..cc7d806d --- /dev/null +++ b/PxMLib/MathLib/include/core/Types_tpl.h @@ -0,0 +1,22 @@ +// PxM/MathLib Types_tpl.h +#pragma once + +#include + +template +using Vec3_T = Eigen::Matrix; + +template +using Mat3_T = Eigen::Matrix; + +template +using Vec6_T = Eigen::Matrix; + +template +using Mat6_T = Eigen::Matrix; + +template +using VecX_T = Eigen::Matrix; + +template +using MatX_T = Eigen::Matrix; \ No newline at end of file From bd90252b1c5191af5b7bd21ced7389b0d3d2991d Mon Sep 17 00:00:00 2001 From: saltyjoss Date: Sat, 16 May 2026 10:29:06 +0100 Subject: [PATCH 002/210] refactor: Updated `SpatialMath.h` to use new Scalar templates --- PxMLib/MathLib/include/core/SpatialMath.h | 171 ++++++++++++++-------- PxMLib/MathLib/include/core/Types_tpl.h | 33 +++-- 2 files changed, 127 insertions(+), 77 deletions(-) diff --git a/PxMLib/MathLib/include/core/SpatialMath.h b/PxMLib/MathLib/include/core/SpatialMath.h index cac0c54d..b21aafe6 100644 --- a/PxMLib/MathLib/include/core/SpatialMath.h +++ b/PxMLib/MathLib/include/core/SpatialMath.h @@ -3,105 +3,137 @@ #include "MathLibAPI.h" #include "core/Types.h" +#include "core/Types_tpl.h" namespace mathlib { + using Vec3 = Vec3_T; + using Mat3 = Mat3_T; - using SpatialMat = Eigen::Matrix; + // Template version of spatial vector + template + struct SpatialVec_T { + using ScalarT = Scalar; + using Vec3S = Vec3_T; + using Vec6S = Vec6_T; - struct SpatialVec { - Eigen::Matrix v; + using AngularBlock = decltype(std::declval().template segment<3>(0)); + using ConstAngularBlock = decltype(std::declval().template segment<3>(0)); - SpatialVec() { - v.setZero(); - } + using LinearBlock = decltype(std::declval().template segment<3>(3)); + using ConstLinearBlock = decltype(std::declval().template segment<3>(3)); + + Vec6S v; + + SpatialVec_T() { v.setZero(); } - SpatialVec(const Vec3& angular, const Vec3& linear) { - v.segment<3>(0) = angular; - v.segment<3>(3) = linear; + SpatialVec_T(const Vec3S& angular, const Vec3S& linear) { + v.template segment<3>(0) = angular; + v.template segment<3>(3) = linear; } - Vec3 angular() const { return v.segment<3>(0); } - Vec3 linear() const { return v.segment<3>(3); } + AngularBlock angular() { return v.template segment<3>(0); } + ConstAngularBlock angular() const { return v.template segment<3>(0); } - SpatialVec operator+(const SpatialVec& rhs) const { - SpatialVec out; + LinearBlock linear() { return v.template segment<3>(3); } + ConstLinearBlock linear() const { return v.template segment<3>(3); } + + SpatialVec_T operator+(const SpatialVec_T& rhs) const { + SpatialVec_T out; out.v = this->v + rhs.v; return out; } - SpatialVec operator-(const SpatialVec& rhs) const { - SpatialVec out; + SpatialVec_T operator-(const SpatialVec_T& rhs) const { + SpatialVec_T out; out.v = this->v - rhs.v; return out; } - SpatialVec operator*(double rhs) const { - SpatialVec out; + SpatialVec_T operator*(Scalar rhs) const { + SpatialVec_T out; out.v = this->v * rhs; return out; } - SpatialVec& operator+=(const SpatialVec& rhs) { + SpatialVec_T& operator+=(const SpatialVec_T& rhs) { this->v += rhs.v; return *this; } - SpatialVec operator*=(double rhs) { + SpatialVec_T& operator*=(Scalar rhs) { this->v *= rhs; return *this; } - double dot(const SpatialVec& sv) const { + Scalar dot(const SpatialVec_T& sv) const { return this->v.dot(sv.v); } }; + using SpatialVec = SpatialVec_T; + + // Template version of spatial matrix + template + using SpatialMat_T = Mat6_T; + using SpatialMat = SpatialMat_T; - inline Mat3 skewSymmetric(const Vec3& v) { - Mat3 S; + template + inline Mat3_T skewSymmetric(const Vec3_T& v) { + Mat3_T S; S << - 0, -v.z(), v.y(), - v.z(), 0, -v.x(), - -v.y(), v.x(), 0; + Scalar(0), -v.z(), v.y(), + v.z(), Scalar(0), -v.x(), + -v.y(), v.x(), Scalar(0); return S; } - inline SpatialMat motionCrossMatrix(const SpatialVec& sv) { - SpatialMat X = SpatialMat::Zero(); + template + inline SpatialMat_T motionCrossMatrix(const SpatialVec_T& sv) { + SpatialMat_T X = SpatialMat_T::Zero(); - Vec3 w = sv.angular(); - Vec3 v = sv.linear(); + Vec3_T w = sv.angular(); + Vec3_T v = sv.linear(); - X.block<3, 3>(0, 0) = skewSymmetric(w); - X.block<3, 3>(3, 0) = skewSymmetric(v); - X.block<3, 3>(3, 3) = skewSymmetric(w); + X.template block<3, 3>(0, 0) = skewSymmetric(w); + X.template block<3, 3>(3, 0) = skewSymmetric(v); + X.template block<3, 3>(3, 3) = skewSymmetric(w); return X; } - inline SpatialMat forceCrossMatrix(const SpatialVec& sv) { + template + inline SpatialMat_T forceCrossMatrix(const SpatialVec_T& sv) { return (- motionCrossMatrix(sv).transpose()).eval(); } - inline SpatialMat spatialTransform(const Mat3& R, const Vec3& r) { - SpatialMat X = SpatialMat::Zero(); + template + inline SpatialMat_T spatialTransform( + const Mat3_T& R, + const Vec3_T& r + ) { + SpatialMat_T X = SpatialMat_T::Zero(); - X.block<3, 3>(0, 0) = R; - X.block<3, 3>(3, 0) = -R * skewSymmetric(r); - X.block<3, 3>(3, 3) = R; + X.template block<3, 3>(0, 0) = R; + X.template block<3, 3>(3, 0) = -R * skewSymmetric(r); + X.template block<3, 3>(3, 3) = R; return X; } - inline SpatialMat spatialInertia(const double mass, const Vec3& com, const Mat3& I_com) { - SpatialMat I = SpatialMat::Zero(); + template + inline SpatialMat_T spatialInertia( + const Scalar mass, + const Vec3_T& com, + const Mat3_T& I_com + ) { + SpatialMat_T I = SpatialMat_T::Zero(); - Mat3 c_skew = skewSymmetric(com); + Mat3_T c_skew = skewSymmetric(com); - I.block<3, 3>(0, 0) = I_com + mass * c_skew.transpose() * c_skew; - I.block<3, 3>(0, 3) = mass * c_skew.transpose(); - I.block<3, 3>(3, 0) = mass * c_skew; - I.block<3, 3>(3, 3) = mass * Mat3::Identity(); + I.template block<3, 3>(0, 0) = I_com + mass * c_skew.transpose() * c_skew; + I.template block<3, 3>(0, 3) = mass * c_skew.transpose(); + I.template block<3, 3>(3, 0) = mass * c_skew; + I.template block<3, 3>(3, 3) = mass * Mat3_T::Identity(); return I; } @@ -109,55 +141,64 @@ namespace mathlib { // Operator overloads for spatial vector and matrix operations // SpatialMat * SpatialVec - inline SpatialVec operator*( - const SpatialMat& lhs, - const SpatialVec& rhs + template + inline SpatialVec_T operator*( + const SpatialMat_T& lhs, + const SpatialVec_T& rhs ) { - SpatialVec out; + SpatialVec_T out; out.v = lhs * rhs.v; return out; } // SpatialVec1 * SpatialVec2 (outer product) - inline SpatialMat outer( - const SpatialVec& rhs, - const SpatialVec& lhs + template + inline SpatialMat_T outer( + const SpatialVec_T& rhs, + const SpatialVec_T& lhs ) { return rhs.v * lhs.v.transpose(); } + // SpatialVec^2 (outer product) - inline SpatialMat outer(const SpatialVec& sv) { + template + inline SpatialMat_T outer(const SpatialVec_T& sv) { return sv.v * sv.v.transpose(); } // SpatialVec1 . SpatialVec2 - inline double dot( - const SpatialVec& lhs, - const SpatialVec& rhs + template + inline Scalar dot( + const SpatialVec_T& lhs, + const SpatialVec_T& rhs ) { return lhs.v.dot(rhs.v); } + // SpatialVec .^2 (element-wise square) - inline double dot(const SpatialVec& sv) { + template + inline Scalar dot(const SpatialVec_T& sv) { return sv.v.dot(sv.v); } // SpatialVec x SpatialVec (motion cross product) - inline SpatialVec crossMotion( - const SpatialVec& lhs, - const SpatialVec& rhs + template + inline SpatialVec_T crossMotion( + const SpatialVec_T& lhs, + const SpatialVec_T& rhs ) { - SpatialVec out; + SpatialVec_T out; out.v = motionCrossMatrix(lhs) * rhs.v; return out; } // SpatialVec x SpatialVec (force cross product) - inline SpatialVec crossForce( - const SpatialVec& lhs, - const SpatialVec& rhs + template + inline SpatialVec_T crossForce( + const SpatialVec_T& lhs, + const SpatialVec_T& rhs ) { - SpatialVec out; + SpatialVec_T out; out.v = forceCrossMatrix(lhs) * rhs.v; return out; } diff --git a/PxMLib/MathLib/include/core/Types_tpl.h b/PxMLib/MathLib/include/core/Types_tpl.h index cc7d806d..4d3e8427 100644 --- a/PxMLib/MathLib/include/core/Types_tpl.h +++ b/PxMLib/MathLib/include/core/Types_tpl.h @@ -1,22 +1,31 @@ // PxM/MathLib Types_tpl.h #pragma once +#include "MathLibAPI.h" #include -template -using Vec3_T = Eigen::Matrix; +namespace mathlib { + // Template version of 3D vector + template + using Vec3_T = Eigen::Matrix; -template -using Mat3_T = Eigen::Matrix; + // Template version of 3x3 matrix + template + using Mat3_T = Eigen::Matrix; -template -using Vec6_T = Eigen::Matrix; + // Template version of 6D vector and 6x6 matrix for spatial algebra + template + using Vec6_T = Eigen::Matrix; -template -using Mat6_T = Eigen::Matrix; + // Template version of 6x6 matrix for spatial algebra + template + using Mat6_T = Eigen::Matrix; -template -using VecX_T = Eigen::Matrix; + // Template version of dynamic-size vector and matrix + template + using VecX_T = Eigen::Matrix; -template -using MatX_T = Eigen::Matrix; \ No newline at end of file + // Template version of dynamic-size matrix + template + using MatX_T = Eigen::Matrix; +} \ No newline at end of file From f40dc7aec67e33a4866fdf4defd7dba382840ac3 Mon Sep 17 00:00:00 2001 From: saltyjoss Date: Sat, 16 May 2026 10:49:59 +0100 Subject: [PATCH 003/210] refactor: Moved `SpatialDynamics` to a templated header, with not .cpp --- DSFE_App/DSFE_Core/CMakeLists.txt | 1 - .../include/Robots/SpatialDynamics.h | 336 +++++++++++++++--- .../DSFE_Core/src/Robots/SpatialDynamics.cpp | 304 ---------------- 3 files changed, 282 insertions(+), 359 deletions(-) delete mode 100644 DSFE_App/DSFE_Core/src/Robots/SpatialDynamics.cpp diff --git a/DSFE_App/DSFE_Core/CMakeLists.txt b/DSFE_App/DSFE_Core/CMakeLists.txt index 44714ecb..feac49ce 100644 --- a/DSFE_App/DSFE_Core/CMakeLists.txt +++ b/DSFE_App/DSFE_Core/CMakeLists.txt @@ -20,7 +20,6 @@ set(CORE_SRC src/Robots/RobotSystem.cpp src/Robots/TrajectoryManager.cpp src/Robots/RobotSimSnapshot.cpp - src/Robots/SpatialDynamics.cpp src/Interpreter/Command.cpp src/Interpreter/CommandContext.cpp diff --git a/DSFE_App/DSFE_Core/include/Robots/SpatialDynamics.h b/DSFE_App/DSFE_Core/include/Robots/SpatialDynamics.h index f1ee66af..23c8a24f 100644 --- a/DSFE_App/DSFE_Core/include/Robots/SpatialDynamics.h +++ b/DSFE_App/DSFE_Core/include/Robots/SpatialDynamics.h @@ -7,80 +7,308 @@ #include namespace robots { - class DSFE_API SpatialDynamics { public: + template static void computeSpatialKinematicsAndBias( const SpatialModel& model, - const mathlib::VecX& q, const mathlib::VecX& qd, - std::vector& Xup_out, - std::vector& v_out, - std::vector& c_out - ); + const mathlib::VecX_T& q, + const mathlib::VecX_T& qd, + std::vector>& Xup_out, + std::vector>& v_out, + std::vector>& c_out + ) { + const size_t n = model.joints.size(); + v_out.resize(n); + Xup_out.resize(n); + c_out.resize(n); + + for (size_t i = 0; i < n; ++i) { + const SpatialJoint& j = model.joints[i]; + + // Joint Transform XJ + mathlib::SpatialMat_T XJ = mathlib::SpatialMat_T::Identity(); + + if (j.type == eJointType::REVOLUTE) { + Eigen::AngleAxisd aa(q[i], j.S.angular().normalized()); + mathlib::Mat3_T R = aa.toRotationMatrix(); + XJ = mathlib::spatialTransform(R, mathlib::Vec3_T::Zero()); + } + else if (j.type == eJointType::PRISMATIC) { + mathlib::Vec3_T r = q[i] * j.S.linear().normalized(); + XJ = mathlib::spatialTransform(mathlib::Mat3_T::Identity(), r); + } + + Xup_out[i] = XJ * j.Xtree; // Combined Transform + mathlib::SpatialVec_T vJ = j.S * qd[i]; // Joint Velocity + // Root Link + if (j.parent < 0) { v_out[i] = vJ; } + else { v_out[i] = Xup_out[i] * v_out[j.parent] + vJ; } + + c_out[i] = crossMotion(v_out[i], vJ); // Coriolis Term + } + } + + template static void computeAccelerations_RNEA( const SpatialModel& model, - const mathlib::VecX& qdd, - const std::vector& Xup, - const std::vector& c, - const mathlib::VecX& g, - std::vector& a_out - ); + const mathlib::VecX_T& qdd, + const std::vector>& Xup, + const std::vector>& c, + const mathlib::VecX_T& g, + std::vector>& a_out + ) { + const size_t n = model.joints.size(); + a_out.resize(n); + + SpatialVec_T a0; // base acceleration (gravity) + a0.v << + g.template segment<3>(0), + g.template segment<3>(3); + + for (size_t i = 0; i < n; ++i) { + const SpatialJoint& j = model.joints[i]; + mathlib::SpatialVec_T aJ = j.S * qdd[i]; // Joint Acceleration + + // Root Link + if (j.parent < 0) { + a_out[i] = Xup[i] * a0 + aJ + c[i]; + continue; + } + + a_out[i] = Xup[i] * a_out[j.parent] + aJ + c[i]; + } + } + template static void computeBackwardForces_RNEA( const SpatialModel& model, - const std::vector& Xup, - const std::vector& v, - const std::vector& a, - mathlib::VecX& tau_out - ); + const std::vector>& Xup, + const std::vector>& v, + const std::vector>& a, + mathlib::VecX_T& tau_out + ) { + const size_t n = model.joints.size(); + tau_out.resize(n); - static mathlib::VecX RNEA( + std::vector> f(n); + + // Forward Force Computation + for (size_t i = 0; i < n; ++i) { + const SpatialJoint& j = model.joints[i]; + mathlib::SpatialVec_T I_v = j.inertia * v[i]; + mathlib::SpatialVec_T coriolis = crossForce(v[i], I_v); + f[i].v = j.inertia * a[i].v + coriolis.v; + } + + // Backward Recursion Computation + for (int i = (int)n - 1; i >= 0; --i) { + const SpatialJoint& j = model.joints[i]; + tau_out[i] = j.S.dot(f[i]); + if (j.parent >= 0) { f[j.parent] += Xup[i].transpose() * f[i]; } + } + } + + template + static mathlib::VecX_T RNEA( const SpatialModel& model, - const mathlib::VecX& q, - const mathlib::VecX& qd, - const mathlib::VecX& qdd, - DynamicsScratch& scratch - ); + const mathlib::VecX_T& q, + const mathlib::VecX_T& qd, + const mathlib::VecX_T& qdd, + DynamicsScratch& scratch + ) { + const size_t n = model.joints.size(); + + // TODO Remove these temp scratches AFTER debugging + std::vector> v(n); + std::vector> Xup(n); + std::vector> c(n); + std::vector> a(n); + mathlib::VecX_T tau; + + // Compute spatial velocities and transforms + computeSpatialKinematicsAndBias(model, q, qd, Xup, v, c); + // Compute spatial accelerations + computeAccelerations_RNEA(model, qdd, Xup, c, scratch.g, a); + // Compute inverse dynamics (joint torques) + computeBackwardForces_RNEA(model, Xup, v, a, tau); - static mathlib::MatX CRBA( + return tau; + } + + template + static mathlib::MatX_T CRBA( const SpatialModel& model, - const std::vector& Xup, - DynamicsScratch& scratch - ); + const std::vector>& Xup, + DynamicsScratch& scratch + ) { + const size_t n = model.joints.size(); + scratch.dense.M.setZero(n, n); + std::vector> Ic(n); // spatial inertia for each link + + // Initialise spatial inertia for each link based on the robot model + for (size_t i = 0; i < n; ++i) { Ic[i] = model.joints[i].inertia; } + + // Upward pass: propagate spatial inertia from child links to parent joints + for (int i = (int)n - 1; i >= 0; --i) { + const SpatialJoint& j = model.joints[i]; + if (j.type == eJointType::FIXED) { continue; } + int p = j.parent; + if (p >= 0) { + Ic[p] += Xup[i].transpose() * Ic[i] * Xup[i]; + } + } + + // Downward pass: compute mass matrix contributions for each joint + for (size_t i = 0; i < n; ++i) { + const SpatialJoint& j = model.joints[i]; + if (j.type == eJointType::FIXED) { continue; } + SpatialVec_T F = Ic[i] * j.S; + scratch.dense.M(i, i) = j.S.dot(F); + int jIdx = (int)i; + while (model.joints[jIdx].parent >= 0) { + int p = model.joints[jIdx].parent; + F = Xup[jIdx].transpose() * F; + scratch.dense.M(i, p) = model.joints[p].S.dot(F); + scratch.dense.M(p, i) = scratch.dense.M(i, p); + jIdx = p; + } + } + return scratch.dense.M; // [kg*m^2], mass matrix computed using the Composite Rigid Body Algorithm (CRBA) + } + + template static void computeArticulatedBodies_ABA( const SpatialModel& model, - const std::vector& Xup, - const std::vector& v, - const std::vector& c, - const mathlib::VecX& tau, - std::vector& IA_out, - std::vector& pA_out, - std::vector& Ia_out, - mathlib::VecX& u_out, - mathlib::VecX& d_out, - std::vector& U_out - ); + const std::vector>& Xup, + const std::vector>& v, + const std::vector>& c, + const mathlib::VecX_T& tau, + std::vector>& IA_out, + std::vector>& pA_out, + std::vector>& Ia_out, + mathlib::VecX_T& u_out, + mathlib::VecX_T& d_out, + std::vector>& U_out + ) { + const size_t n = model.joints.size(); + + // Resize scratch buffers + IA_out.resize(n); + pA_out.resize(n); + Ia_out.resize(n); + U_out.resize(n); + u_out.resize(n); + d_out.resize(n); + + // Upward pass: compute articulated body inertias and bias forces + for (int i = (int)n - 1; i >= 0; --i) { + const SpatialJoint& j = model.joints[i]; + + if (j.type == eJointType::FIXED) { + Ia_out[i] = IA_out[i]; + if (j.parent >= 0) { + IA_out[j.parent] += Xup[i].transpose() * Ia_out[i] * Xup[i]; + pA_out[j.parent] += Xup[i].transpose() * pA_out[i]; + } + continue; + } + + U_out[i] = IA_out[i] * j.S; + d_out[i] = dot(j.S, U_out[i]); + if (std::abs(d_out[i]) < 1e-12) { d_out[i] = 1e-12; } // Regularisation to avoid singularities + u_out[i] = tau[i] - dot(j.S, pA_out[i]); + Ia_out[i] = IA_out[i] - outer(U_out[i]) / d_out[i]; + + // pA = pA + Ia * c + U * (u/d) + pA_out[i] += Ia_out[i] * c[i] + U_out[i] * (u_out[i] / d_out[i]); + + if (j.parent >= 0) { + IA_out[j.parent] += Xup[i].transpose() * Ia_out[i] * Xup[i]; + pA_out[j.parent] += Xup[i].transpose() * pA_out[i]; + } + } + } + + template static void computeAccelerations_ABA( const SpatialModel& model, - const std::vector& Xup, - const std::vector& c, - const mathlib::VecX& u_out, - const mathlib::VecX& d_out, - const std::vector& U, - const SpatialVec& a0, - std::vector& a_out, - mathlib::VecX& qdd_out - ); - - static mathlib::VecX ABA( + const std::vector>& Xup, + const std::vector>& c, + const mathlib::VecX_T& u_out, + const mathlib::VecX_T& d_out, + const std::vector>& U, + const SpatialVec_T& a0, + std::vector>& a_out, + mathlib::VecX_T& qdd_out + ) { + const size_t n = model.joints.size(); + a_out.resize(n); + qdd_out.resize(n); + + for (size_t i = 0; i < n; ++i) { + const SpatialJoint& j = model.joints[i]; + + if (j.parent < 0) { a_out[i] = Xup[i] * a0 + c[i]; } + else { a_out[i] = Xup[i] * a_out[j.parent] + c[i]; } + + if (j.type == eJointType::FIXED) { + qdd_out[i] = 0.0; + continue; + } + + qdd_out[i] = (u_out[i] - U[i].dot(a_out[i])) / d_out[i]; + a_out[i] += j.S * qdd_out[i]; + } + } + + template + static mathlib::VecX_T ABA( const SpatialModel& model, - const mathlib::VecX& q, - const mathlib::VecX& qd, - const mathlib::VecX& tau, - DynamicsScratch& scratch - ); + const mathlib::VecX_T& q, + const mathlib::VecX_T& qd, + const mathlib::VecX_T& tau, + DynamicsScratch& scratch + ) { + const size_t n = model.joints.size(); + VecX_T qdd = VecX_T::Zero(n); + + SpatialVec_T a0; // base acceleration (gravity) + a0.v << + scratch.g.template segment<3>(0), + scratch.g.template segment<3>(3); + + computeSpatialKinematicsAndBias( + model, q, qd, + scratch.spatial.Xup, + scratch.spatial.v, + scratch.spatial.c + ); + + for (size_t i = 0; i < n; ++i) { + scratch.spatial.IA[i] = model.joints[i].inertia; // Articulated Body Inertia + scratch.spatial.pA[i] = crossForce(scratch.spatial.v[i], (scratch.spatial.IA[i] * scratch.spatial.v[i])); + } + + // Compute articulated body inertias and bias forces + computeArticulatedBodies_ABA( + model, scratch.spatial.Xup, + scratch.spatial.v, scratch.spatial.c, tau, + scratch.spatial.IA, scratch.spatial.pA, scratch.spatial.Ia, + scratch.spatial.u, scratch.spatial.d, scratch.spatial.U + ); + + // Compute joint accelerations using the articulated body algorithm + computeAccelerations_ABA( + model, scratch.spatial.Xup, scratch.spatial.c, + scratch.spatial.u, scratch.spatial.d, scratch.spatial.U, + a0, scratch.spatial.a,qdd + ); + + return qdd; // [rad/s^2], joint accelerations computed using the Articulated Body Algorithm (ABA) + } }; } \ No newline at end of file diff --git a/DSFE_App/DSFE_Core/src/Robots/SpatialDynamics.cpp b/DSFE_App/DSFE_Core/src/Robots/SpatialDynamics.cpp deleted file mode 100644 index 116a51ee..00000000 --- a/DSFE_App/DSFE_Core/src/Robots/SpatialDynamics.cpp +++ /dev/null @@ -1,304 +0,0 @@ -// DSFE_Core SpatialDynamics.cpp -#include "pch.h" - -#include "Robots/SpatialDynamics.h" - -namespace robots { - // Recursive function to compute spatial velocities using the articulated body algorithm - void SpatialDynamics::computeSpatialKinematicsAndBias( - const SpatialModel& model, - const mathlib::VecX& q, - const mathlib::VecX& qd, - std::vector& Xup_out, - std::vector& v_out, - std::vector& c_out - ) { - const size_t n = model.joints.size(); - v_out.resize(n); - Xup_out.resize(n); - c_out.resize(n); - - for (size_t i = 0; i < n; ++i) { - const SpatialJoint& j = model.joints[i]; - - // Joint Transform XJ - mathlib::SpatialMat XJ = mathlib::SpatialMat::Identity(); - - if (j.type == eJointType::REVOLUTE) { - Eigen::AngleAxisd aa(q[i], j.S.angular().normalized()); - mathlib::Mat3 R = aa.toRotationMatrix(); - XJ = mathlib::spatialTransform(R, mathlib::Vec3::Zero()); - } - else if (j.type == eJointType::PRISMATIC) { - mathlib::Vec3 r = q[i] * j.S.linear().normalized(); - XJ = mathlib::spatialTransform(mathlib::Mat3::Identity(), r); - } - - Xup_out[i] = XJ * j.Xtree; // Combined Transform - mathlib::SpatialVec vJ = j.S * qd[i]; // Joint Velocity - - // Root Link - if (j.parent < 0) { v_out[i] = vJ; } - else { v_out[i] = Xup_out[i] * v_out[j.parent] + vJ; } - - c_out[i] = crossMotion(v_out[i], vJ); // Coriolis Term - } - } - - // Recursive function to compute spatial accelerations using the articulated body algorithm - void SpatialDynamics::computeAccelerations_RNEA( - const SpatialModel& model, - const mathlib::VecX& qdd, - const std::vector& Xup, - const std::vector& c, - const mathlib::VecX& g, - std::vector& a_out - ) { - const size_t n = model.joints.size(); - a_out.resize(n); - - SpatialVec a0; - a0.v << g.segment<3>(0), g.segment<3>(3); // Base acceleration (gravity) - - for (size_t i = 0; i < n; ++i) { - const SpatialJoint& j = model.joints[i]; - mathlib::SpatialVec aJ = j.S * qdd[i]; // Joint Acceleration - - // Root Link - if (j.parent < 0) { - a_out[i] = Xup[i] * a0 + aJ + c[i]; - continue; - } - - a_out[i] = Xup[i] * a_out[j.parent] + aJ + c[i]; - } - } - - // Recursive function to compute inverse dynamics (joint torques) using the articulated body algorithm - void SpatialDynamics::computeBackwardForces_RNEA( - const SpatialModel& model, - const std::vector& Xup, - const std::vector& v, - const std::vector& a, - mathlib::VecX& tau_out - ) { - const size_t n = model.joints.size(); - tau_out.resize(n); - - std::vector f(n); - - // Forward Force Computation - for (size_t i = 0; i < n; ++i) { - const SpatialJoint& j = model.joints[i]; - mathlib::SpatialVec I_v = j.inertia * v[i]; - mathlib::SpatialVec coriolis = crossForce(v[i], I_v); - f[i].v = j.inertia * a[i].v + coriolis.v; - } - - // Backward Recursion Computation - for (int i = (int)n - 1; i >= 0; --i) { - const SpatialJoint& j = model.joints[i]; - tau_out[i] = j.S.dot(f[i]); - if (j.parent >= 0) { f[j.parent] += Xup[i].transpose() * f[i]; } - } - } - - // Main function to compute inverse dynamics (joint torques) given joint states and accelerations using the Recursive Newton-Euler Algorithm (RNEA) - mathlib::VecX SpatialDynamics::RNEA( - const SpatialModel& model, - const mathlib::VecX& q, - const mathlib::VecX& qd, - const mathlib::VecX& qdd, - DynamicsScratch& scratch - ) { - const size_t n = model.joints.size(); - - // TODO Remove these temp scratches AFTER debugging - std::vector v(n); - std::vector Xup(n); - std::vector c(n); - std::vector a(n); - mathlib::VecX tau; - - // Compute spatial velocities and transforms - computeSpatialKinematicsAndBias(model, q, qd, Xup, v, c); - // Compute spatial accelerations - computeAccelerations_RNEA(model, qdd, Xup, c, scratch.g, a); - // Compute inverse dynamics (joint torques) - computeBackwardForces_RNEA(model, Xup, v, a, tau); - - return tau; - } - - // Main function to compute the mass matrix of the robot at a given configuration using the Composite Rigid Body Algorithm (CRBA) - mathlib::MatX SpatialDynamics::CRBA( - const SpatialModel& model, - const std::vector& Xup, - DynamicsScratch& scratch - ) { - const size_t n = model.joints.size(); - scratch.dense.M.setZero(n, n); - std::vector Ic(n); // spatial inertia for each link - - // Initialise spatial inertia for each link based on the robot model - for (size_t i = 0; i < n; ++i) { Ic[i] = model.joints[i].inertia; } - - // Upward pass: propagate spatial inertia from child links to parent joints - for (int i = (int)n - 1; i >= 0; --i) { - const SpatialJoint& j = model.joints[i]; - if (j.type == eJointType::FIXED) { continue; } - int p = j.parent; - if (p >= 0) { - Ic[p] += Xup[i].transpose() * Ic[i] * Xup[i]; - } - } - - // Downward pass: compute mass matrix contributions for each joint - for (size_t i = 0; i < n; ++i) { - const SpatialJoint& j = model.joints[i]; - if (j.type == eJointType::FIXED) { continue; } - SpatialVec F = Ic[i] * j.S; - scratch.dense.M(i, i) = j.S.dot(F); - - int jIdx = (int)i; - while (model.joints[jIdx].parent >= 0) { - int p = model.joints[jIdx].parent; - F = Xup[jIdx].transpose() * F; - scratch.dense.M(i, p) = model.joints[p].S.dot(F); - scratch.dense.M(p, i) = scratch.dense.M(i, p); - jIdx = p; - } - } - return scratch.dense.M; // [kg*m^2], mass matrix computed using the Composite Rigid Body Algorithm (CRBA) - } - - // Recursive function to compute articulated body inertias and bias forces using the Articulated Body Algorithm (ABA) - void SpatialDynamics::computeArticulatedBodies_ABA( - const SpatialModel& model, - const std::vector& Xup, - const std::vector& v, - const std::vector& c, - const mathlib::VecX& tau, - std::vector& IA_out, - std::vector& pA_out, - std::vector& Ia_out, - mathlib::VecX& u_out, - mathlib::VecX& d_out, - std::vector& U_out - ) { - const size_t n = model.joints.size(); - - // Resize scratch buffers - IA_out.resize(n); - pA_out.resize(n); - Ia_out.resize(n); - U_out.resize(n); - u_out.resize(n); - d_out.resize(n); - - // Upward pass: compute articulated body inertias and bias forces - for (int i = (int)n - 1; i >= 0; --i) { - const SpatialJoint& j = model.joints[i]; - - if (j.type == eJointType::FIXED) { - Ia_out[i] = IA_out[i]; - if (j.parent >= 0) { - IA_out[j.parent] += Xup[i].transpose() * Ia_out[i] * Xup[i]; - pA_out[j.parent] += Xup[i].transpose() * pA_out[i]; - } - continue; - } - - U_out[i] = IA_out[i] * j.S; - d_out[i] = dot(j.S, U_out[i]); - if (std::abs(d_out[i]) < 1e-12) { d_out[i] = 1e-12; } // Regularisation to avoid singularities - - u_out[i] = tau[i] - dot(j.S, pA_out[i]); - Ia_out[i] = IA_out[i] - outer(U_out[i]) / d_out[i]; - - // pA = pA + Ia * c + U * (u/d) - pA_out[i] += Ia_out[i] * c[i] + U_out[i] * (u_out[i] / d_out[i]); - - if (j.parent >= 0) { - IA_out[j.parent] += Xup[i].transpose() * Ia_out[i] * Xup[i]; - pA_out[j.parent] += Xup[i].transpose() * pA_out[i]; - } - } - } - - // Recursive function to compute joint accelerations using the Articulated Body Algorithm (ABA) - void SpatialDynamics::computeAccelerations_ABA( - const SpatialModel& model, - const std::vector& Xup, - const std::vector& c, - const mathlib::VecX& u_out, - const mathlib::VecX& d_out, - const std::vector& U, - const SpatialVec& a0, - std::vector& a_out, - mathlib::VecX& qdd_out - ) { - const size_t n = model.joints.size(); - a_out.resize(n); - qdd_out.resize(n); - - for (size_t i = 0; i < n; ++i) { - const SpatialJoint& j = model.joints[i]; - - if (j.parent < 0) { a_out[i] = Xup[i] * a0 + c[i]; } - else { a_out[i] = Xup[i] * a_out[j.parent] + c[i]; } - - if (j.type == eJointType::FIXED) { - qdd_out[i] = 0.0; - continue; - } - - qdd_out[i] = (u_out[i] - U[i].dot(a_out[i])) / d_out[i]; - a_out[i] += j.S * qdd_out[i]; - } - } - - // Main function to compute joint accelerations given joint states and torques using the Articulated Body Algorithm (ABA) - mathlib::VecX SpatialDynamics::ABA( - const SpatialModel& model, - const mathlib::VecX& q, - const mathlib::VecX& qd, - const mathlib::VecX& tau, - DynamicsScratch& scratch - ) { - const size_t n = model.joints.size(); - VecX qdd = VecX::Zero(n); - - SpatialVec a0; // base acceleration (gravity) - a0.v << scratch.g.segment<3>(0), scratch.g.segment<3>(3); - - computeSpatialKinematicsAndBias( - model, q, qd, - scratch.spatial.Xup, - scratch.spatial.v, - scratch.spatial.c - ); - - for (size_t i = 0; i < n; ++i) { - scratch.spatial.IA[i] = model.joints[i].inertia; // Articulated Body Inertia - scratch.spatial.pA[i] = crossForce(scratch.spatial.v[i], (scratch.spatial.IA[i] * scratch.spatial.v[i])); - } - - // Compute articulated body inertias and bias forces - computeArticulatedBodies_ABA( - model, scratch.spatial.Xup, - scratch.spatial.v, scratch.spatial.c, tau, - scratch.spatial.IA, scratch.spatial.pA, scratch.spatial.Ia, - scratch.spatial.u, scratch.spatial.d, scratch.spatial.U - ); - - // Compute joint accelerations using the articulated body algorithm - computeAccelerations_ABA( - model, scratch.spatial.Xup, scratch.spatial.c, - scratch.spatial.u, scratch.spatial.d, scratch.spatial.U, - a0, scratch.spatial.a,qdd - ); - - return qdd; // [rad/s^2], joint accelerations computed using the Articulated Body Algorithm (ABA) - } -} \ No newline at end of file From 424df54e14beb9583e4e3377bc66606091ed5de0 Mon Sep 17 00:00:00 2001 From: saltyjoss Date: Sat, 16 May 2026 10:50:23 +0100 Subject: [PATCH 004/210] refactor: Templarised all structs in `DynamicTypes` --- .../DSFE_Core/include/Robots/DynamicsTypes.h | 50 ++++++++++--------- 1 file changed, 27 insertions(+), 23 deletions(-) diff --git a/DSFE_App/DSFE_Core/include/Robots/DynamicsTypes.h b/DSFE_App/DSFE_Core/include/Robots/DynamicsTypes.h index df225154..a19d0e15 100644 --- a/DSFE_App/DSFE_Core/include/Robots/DynamicsTypes.h +++ b/DSFE_App/DSFE_Core/include/Robots/DynamicsTypes.h @@ -10,13 +10,14 @@ namespace robots { // Scratch buffers for dense dynamics + template struct DenseDynamicsScratch { - mathlib::MatX M; // mass matrix - mathlib::VecX rhs; // right-hand side vector for dynamics equations (Coriolis, gravity, control torques) - mathlib::VecX h; // Coriolis and centrifugal bias vector - mathlib::VecX tau; // control torque vector + mathlib::MatX_T M; // mass matrix + mathlib::VecX_T rhs; // right-hand side vector for dynamics equations (Coriolis, gravity, control torques) + mathlib::VecX_T h; // Coriolis and centrifugal bias vector + mathlib::VecX_T tau; // control torque vector - mathlib::VecX I_eff_controller; // effective inertia vector for controller design (e.g., for inverse dynamics control) + mathlib::VecX_T I_eff_controller; // effective inertia vector for controller design (e.g., for inverse dynamics control) std::vector T_world; std::vector jointWorldPoses; @@ -73,26 +74,27 @@ namespace robots { }; // Scratch buffers for spatial dynamics computations + template struct SpatialDynamicsScratch { - std::vector Xup; // spatial transformation from parent to current link - std::vector IA; // articulated body inertia - std::vector Ia; // articulated body inertia in the link frame + std::vector> Xup; // spatial transformation from parent to current link + std::vector> IA; // articulated body inertia + std::vector> Ia; // articulated body inertia in the link frame - std::vector v; // spatial velocity - std::vector c; // spatial bias acceleration - std::vector a; // spatial acceleration - std::vector pA; // articulated bias force - std::vector U; // articulated body force + std::vector> v; // spatial velocity + std::vector> c; // spatial bias acceleration + std::vector> a; // spatial acceleration + std::vector> pA; // articulated bias force + std::vector> U; // articulated body force - mathlib::VecX u; // joint force contribution - mathlib::VecX d; // joint inertia contribution + mathlib::VecX_T u; // joint force contribution + mathlib::VecX_T d; // joint inertia contribution - std::vector> dXup_dq; // derivative of spatial transformation w.r.t. joint angles + std::vector>> dXup_dq; // derivative of spatial transformation w.r.t. joint angles - std::vector> dv_dq; // derivative of spatial velocity w.r.t. joint angles - std::vector> dv_dqd; // derivative of spatial velocity w.r.t. joint velocities - std::vector> dc_dq; // derivative of spatial bias acceleration w.r.t. joint angles - std::vector> dc_dqd; // derivative of spatial bias acceleration w.r.t. joint velocities + std::vector>> dv_dq; // derivative of spatial velocity w.r.t. joint angles + std::vector>> dv_dqd; // derivative of spatial velocity w.r.t. joint velocities + std::vector>> dc_dq; // derivative of spatial bias acceleration w.r.t. joint angles + std::vector>> dc_dqd; // derivative of spatial bias acceleration w.r.t. joint velocities size_t jointCap = 0; @@ -132,9 +134,10 @@ namespace robots { }; // Output structure for dynamics computations + template struct DynamicsResult { - mathlib::VecX dxdt; - mathlib::VecX qdd; + mathlib::VecX_T dxdt; + mathlib::VecX_T qdd; RobotMetrics metrics; @@ -146,13 +149,14 @@ namespace robots { }; // Central scratch structure that contains all buffers needed for dynamics computations, both dense and spatial + template struct DynamicsScratch { DenseDynamicsScratch dense; SpatialDynamicsScratch spatial; // TODO add kinematics scratch // Gravity scratch buffer - mathlib::VecX g; + mathlib::VecX_T g; // Resizes all scratch buffers using the given number of joints and links void resize(size_t nJoints, size_t nLinks) { From 5ca88a7b197f928acc9915201d276fd0aee6a1ae Mon Sep 17 00:00:00 2001 From: saltyjoss Date: Sat, 16 May 2026 10:59:11 +0100 Subject: [PATCH 005/210] refactor: Templarised `SpatialModel` and `SpatialJoint` with Scalar typename --- .../DSFE_Core/include/Robots/SpatialDynamics.h | 16 ++++++++-------- DSFE_App/DSFE_Core/include/Robots/SpatialModel.h | 14 ++++++++------ 2 files changed, 16 insertions(+), 14 deletions(-) diff --git a/DSFE_App/DSFE_Core/include/Robots/SpatialDynamics.h b/DSFE_App/DSFE_Core/include/Robots/SpatialDynamics.h index 23c8a24f..b4f65deb 100644 --- a/DSFE_App/DSFE_Core/include/Robots/SpatialDynamics.h +++ b/DSFE_App/DSFE_Core/include/Robots/SpatialDynamics.h @@ -11,7 +11,7 @@ namespace robots { public: template static void computeSpatialKinematicsAndBias( - const SpatialModel& model, + const SpatialModel& model, const mathlib::VecX_T& q, const mathlib::VecX_T& qd, std::vector>& Xup_out, @@ -52,7 +52,7 @@ namespace robots { template static void computeAccelerations_RNEA( - const SpatialModel& model, + const SpatialModel& model, const mathlib::VecX_T& qdd, const std::vector>& Xup, const std::vector>& c, @@ -83,7 +83,7 @@ namespace robots { template static void computeBackwardForces_RNEA( - const SpatialModel& model, + const SpatialModel& model, const std::vector>& Xup, const std::vector>& v, const std::vector>& a, @@ -112,7 +112,7 @@ namespace robots { template static mathlib::VecX_T RNEA( - const SpatialModel& model, + const SpatialModel& model, const mathlib::VecX_T& q, const mathlib::VecX_T& qd, const mathlib::VecX_T& qdd, @@ -139,7 +139,7 @@ namespace robots { template static mathlib::MatX_T CRBA( - const SpatialModel& model, + const SpatialModel& model, const std::vector>& Xup, DynamicsScratch& scratch ) { @@ -181,7 +181,7 @@ namespace robots { template static void computeArticulatedBodies_ABA( - const SpatialModel& model, + const SpatialModel& model, const std::vector>& Xup, const std::vector>& v, const std::vector>& c, @@ -235,7 +235,7 @@ namespace robots { template static void computeAccelerations_ABA( - const SpatialModel& model, + const SpatialModel& model, const std::vector>& Xup, const std::vector>& c, const mathlib::VecX_T& u_out, @@ -267,7 +267,7 @@ namespace robots { template static mathlib::VecX_T ABA( - const SpatialModel& model, + const SpatialModel& model, const mathlib::VecX_T& q, const mathlib::VecX_T& qd, const mathlib::VecX_T& tau, diff --git a/DSFE_App/DSFE_Core/include/Robots/SpatialModel.h b/DSFE_App/DSFE_Core/include/Robots/SpatialModel.h index 05af1253..210fe9af 100644 --- a/DSFE_App/DSFE_Core/include/Robots/SpatialModel.h +++ b/DSFE_App/DSFE_Core/include/Robots/SpatialModel.h @@ -9,21 +9,23 @@ namespace robots { // Spatial joint struct - struct DSFE_API SpatialJoint { + template + struct SpatialJoint { int parent = -1; eJointType type = eJointType::FIXED; - mathlib::SpatialMat Xtree; - mathlib::SpatialMat inertia; - mathlib::SpatialVec S; + mathlib::SpatialMat_T Xtree; + mathlib::SpatialMat_T inertia; + mathlib::SpatialVec_T S; std::string name; }; // Spatial model struct - struct DSFE_API SpatialModel { - std::vector joints; + template + struct SpatialModel { + std::vector> joints; std::unordered_map linkNameToIndex; }; } \ No newline at end of file From fa671d7bf1e8ca155994abb04006d0e46454121d Mon Sep 17 00:00:00 2001 From: saltyjoss Date: Sat, 16 May 2026 16:12:13 +0100 Subject: [PATCH 006/210] refactor: Templarised `RobotDynamics`, `SpatialDynamics`, and `RobotKinematics` using the Scalar typename * Each header includes the corresponding .inl file holding the templated methods, thus improving code readability and scalability --- .../DSFE_Core/include/Robots/DynamicsTypes.h | 5 +- .../DSFE_Core/include/Robots/RobotDynamics.h | 116 ++-- .../include/Robots/RobotDynamics.inl | 603 ++++++++++++++++++ .../include/Robots/RobotKinematics.h | 28 +- .../include/Robots/RobotKinematics.inl | 92 +++ .../DSFE_Core/include/Robots/RobotModel.h | 4 +- .../include/Robots/SpatialDynamics.h | 254 +------- .../include/Robots/SpatialDynamics.inl | 306 +++++++++ .../DSFE_Core/src/Robots/RobotDynamics.cpp | 560 ---------------- .../DSFE_Core/src/Robots/RobotKinematics.cpp | 89 --- PxMLib/MathLib/include/core/Types_tpl.h | 6 +- 11 files changed, 1120 insertions(+), 943 deletions(-) create mode 100644 DSFE_App/DSFE_Core/include/Robots/RobotDynamics.inl create mode 100644 DSFE_App/DSFE_Core/include/Robots/RobotKinematics.inl create mode 100644 DSFE_App/DSFE_Core/include/Robots/SpatialDynamics.inl diff --git a/DSFE_App/DSFE_Core/include/Robots/DynamicsTypes.h b/DSFE_App/DSFE_Core/include/Robots/DynamicsTypes.h index a19d0e15..12c6f43f 100644 --- a/DSFE_App/DSFE_Core/include/Robots/DynamicsTypes.h +++ b/DSFE_App/DSFE_Core/include/Robots/DynamicsTypes.h @@ -16,11 +16,10 @@ namespace robots { mathlib::VecX_T rhs; // right-hand side vector for dynamics equations (Coriolis, gravity, control torques) mathlib::VecX_T h; // Coriolis and centrifugal bias vector mathlib::VecX_T tau; // control torque vector - mathlib::VecX_T I_eff_controller; // effective inertia vector for controller design (e.g., for inverse dynamics control) - std::vector T_world; - std::vector jointWorldPoses; + std::vector> T_world; + std::vector> jointWorldPoses; size_t jointCap = 0; size_t linkCap = 0; diff --git a/DSFE_App/DSFE_Core/include/Robots/RobotDynamics.h b/DSFE_App/DSFE_Core/include/Robots/RobotDynamics.h index bfda0f5e..27377f76 100644 --- a/DSFE_App/DSFE_Core/include/Robots/RobotDynamics.h +++ b/DSFE_App/DSFE_Core/include/Robots/RobotDynamics.h @@ -7,6 +7,9 @@ #include "Robots/DynamicsTypes.h" #include "Robots/RobotMetrics.h" +#include "Robots/SpatialDynamics.h" +#include "Robots/SpatialModel.h" + // Forward declarations namespace control { class TrajectoryManager; } namespace integration { class IntegrationService; enum class eIntegrationMethod; } @@ -15,7 +18,6 @@ namespace robots { // Forward declarations class RobotKinematics; struct RobotConstModel; - struct SpatialModel; struct RobotSimSnapshot; struct RobotLink; struct RobotJoint; @@ -28,89 +30,106 @@ namespace robots { RobotDynamics(); // Computes the inertia tensor of a robot link - mathlib::Mat3 computeLinkInertiaTensor(const RobotLink& link) const; + template + mathlib::Mat3_T computeLinkInertiaTensor(const RobotLink& link) const; // Computes the contribution of a single joint and its child link to the effective inertia I_eff of the joint - double computeJointInertiaContribution( + template + Scalar computeJointInertiaContribution( const RobotJoint& joint, const RobotLink& link, - const mathlib::Pose& jointWorldPose, - const mathlib::Pose& linkWorldPose + const mathlib::Pose_T& jointWorldPose, + const mathlib::Pose_T& linkWorldPose ) const; // Computes the full mass matrix M(q) based on the current state and robot configuration + template void computeMassMatrix( const RobotConstModel& robot, - const std::vector& T_world, - const std::vector& jointWorldPoses, - mathlib::MatX& M_out + const std::vector>& T_world, + const std::vector>& jointWorldPoses, + mathlib::MatX_T& M_out ) const; // Computes the Coriolis and centrifugal bias vector h(q, qd) based on the current state and robot configuration - mathlib::VecX computeCoriolisVector( + template + mathlib::VecX_T computeCoriolisVector( const RobotConstModel& robot, - const mathlib::VecX& q, const mathlib::VecX& qd, - const std::vector& T_world, - const mathlib::MatX& M + const mathlib::VecX_T& q, + const mathlib::VecX_T& qd, + const std::vector>& T_world, + const mathlib::MatX_T& M ) const; // Computes the gravity torque for a joint based on the current state and robot configuration - mathlib::VecX computeGravityTorque( + template + mathlib::VecX_T computeGravityTorque( const RobotConstModel& robot, - const std::vector& T_world, - const std::vector& jointWorldPoses + const std::vector>& T_world, + const std::vector>& jointWorldPoses ) const; // Computes the analytical Jacobian matrix J(q) for the robot based on the current state and robot configuration + template void analyticalJacobian( const RobotConstModel& robot, - const mathlib::VecX& x, - mathlib::MatX& J_out, - DenseDynamicsScratch& scratch + const mathlib::VecX_T& x, + mathlib::MatX_T& J_out, + DenseDynamicsScratch& scratch ); // Computes the Coriolis and centrifugal torque for a joint based on the current state and robot configuration - mathlib::VecX derivative_dense( - double t, - const mathlib::VecX& x, + template + mathlib::VecX_T derivative_dense( + Scalar t, + const mathlib::VecX_T& x, const RobotSimSnapshot& snap, - DynamicsScratch& scratch, - DynamicsResult& out + DynamicsScratch& scratch, + DynamicsResult& out ); - mathlib::VecX derivative_spatial( - const robots::SpatialModel& model, - double t, - const mathlib::VecX& x, + template + mathlib::VecX_T derivative_spatial( + const robots::SpatialModel& model, + Scalar t, + const mathlib::VecX_T& x, const RobotSimSnapshot& snap, - DynamicsScratch& scratch, - DynamicsResult& out + DynamicsScratch& scratch, + DynamicsResult& out ); + template void jacobian_spatial( - const robots::SpatialModel& model, - const mathlib::VecX& x, + const robots::SpatialModel& model, + const mathlib::VecX_T& x, const RobotSimSnapshot& snap, - const mathlib::VecX& kp, - const mathlib::VecX& kd, - mathlib::MatX& F_out, - DynamicsScratch& scratch + const mathlib::VecX_T& kp, + const mathlib::VecX_T& kd, + mathlib::MatX_T& F_out, + DynamicsScratch& scratch ); // Computes the derivative of the state vector with control gains based on the current state and robot configurations - mathlib::VecX derivative_with_gains( - double t, - const mathlib::VecX& x, + template + mathlib::VecX_T derivative_with_gains( + Scalar t, + const mathlib::VecX_T& x, const RobotSimSnapshot& snap, - const mathlib::VecX& kp,const mathlib::VecX& kd, - DynamicsScratch& scratch, DynamicsResult& out + const mathlib::VecX_T& kp, + const mathlib::VecX_T& kd, + DynamicsScratch& scratch, + DynamicsResult& out ); // Computes the Jacobian matrix with control gains based on the current state and robot configuration + template void jacobian_with_gains( - const mathlib::VecX& x, const RobotSimSnapshot& snap, - const mathlib::VecX& kp, const mathlib::VecX& kd, mathlib::MatX& F_out, - DenseDynamicsScratch& scratch + const mathlib::VecX_T& x, + const RobotSimSnapshot& snap, + const mathlib::VecX_T& kp, + const mathlib::VecX_T& kd, + mathlib::MatX_T& F_out, + DenseDynamicsScratch& scratch ); // Set the gravity strength for the robot system @@ -125,10 +144,19 @@ namespace robots { // References and pointers std::unique_ptr _kinematics = nullptr; + static bool isControlledJoint(eJointType t) { + return + t == eJointType::REVOLUTE || + t == eJointType::PRISMATIC; + } + double _dt = 1.0 / 180.0; // default timestep for dynamics updates double _gravity{ 0.0 }; bool _baseIsFree = false; double _lastBaseForwardForce{ 0.0 }; }; -} // namespace robots \ No newline at end of file +} // namespace robots + + +#include "Robots/RobotDynamics.inl" \ No newline at end of file diff --git a/DSFE_App/DSFE_Core/include/Robots/RobotDynamics.inl b/DSFE_App/DSFE_Core/include/Robots/RobotDynamics.inl new file mode 100644 index 00000000..e97266f6 --- /dev/null +++ b/DSFE_App/DSFE_Core/include/Robots/RobotDynamics.inl @@ -0,0 +1,603 @@ +// DSFE_Core RobotDynamics.inl +#pragma once + +namespace robots { + // Computes the inertia tensor of a robot link + template + mathlib::Mat3_T RobotDynamics::computeLinkInertiaTensor(const RobotLink& link) const { + const robots::Inertia& I = link.inertial.inertia; + + // Construct the inertia tensor matrix + mathlib::Mat3_T M = mathlib::Mat3_T::Zero(); + M << + I.ixx, I.ixy, I.ixz, + I.ixy, I.iyy, I.iyz, + I.ixz, I.iyz, I.izz; + + return M; // [kg*m^2], (3x3) inertia tensor in link frame + } + + // Computes the contribution of a single joint and its child link to the effective inertia I_eff of the joint + template + Scalar RobotDynamics::computeJointInertiaContribution( + const RobotJoint& joint, + const RobotLink& link, + const mathlib::Pose_T& jointWorldPose, + const mathlib::Pose_T& linkWorldPose + ) const { + const Scalar m = (Scalar)link.inertial.mass; + + // Rotation from link frame to world frame + mathlib::Mat3_T R_joint = jointWorldPose.template block<3, 3>(0, 0); + // Joint axis in world frame + mathlib::Vec3_T axis_world = (R_joint * joint.axis).normalized(); + + // Position of joint in world frame + mathlib::Vec3_T joint_pos_world = jointWorldPose.template block<3, 1>(0, 3); + + // Rotation from link frame to world frame + mathlib::Mat3_T R_link = linkWorldPose.template block<3, 3>(0, 0); + mathlib::Vec3_T link_pos_world = linkWorldPose.template block<3, 1>(0, 3); + mathlib::Vec3_T com_world = R_link * Vec3_T(link.inertial.com_xyz) + link_pos_world; + + // Translational contribution (parallel axis theorem) + mathlib::Vec3_T r = com_world - joint_pos_world; + + // Translational contribution to inertia about the joint axis + Scalar I_trans = m * (axis_world.cross(r)).squaredNorm(); + + // Rotational contribution + mathlib::Mat3_T I_local = computeLinkInertiaTensor(link); + mathlib::Mat3_T I_world = R_link * I_local * R_link.transpose(); + + // Rotational contribution to inertia about the joint axis + Scalar I_rot = axis_world.transpose() * I_world * axis_world; + Scalar I_eff_i = I_trans + I_rot; + + return std::max(I_eff_i, 1e-6); // [kg*m^2], I_eff contribution of this joint + floor to avoid singularities + } + + // Computes the full mass matrix M(q) based on the current state and robot configuration + template + void RobotDynamics::computeMassMatrix( + const RobotConstModel& robot, + const std::vector>& T_world, + const std::vector>& jointWorldPoses, + mathlib::MatX_T& M_out + ) const { + const size_t n = robot.joints.size(); + M_out.resize(n, n); + M_out.setZero(); + + // Compute its contribution to the mass matrix for each link + for (size_t k = 0; k < robot.links.size(); ++k) { + const RobotLink& link = robot.links[k]; + const Scalar m = link.inertial.mass; + + if (m <= Scalar(0)) { continue; } + + const mathlib::Mat3_T R = T_world[k].template block<3, 3>(0, 0); // Rotation from link frame to world frame + const mathlib::Vec3_T p = T_world[k].template block<3, 1>(0, 3); // Center of mass of the link in world frame + const mathlib::Vec3_T com = R * link.inertial.com_xyz + p; // Center of mass in world frame + + mathlib::Mat3_T I_local = computeLinkInertiaTensor(link); // inertia tensor in link frame + mathlib::Mat3_T I_world = R * I_local * R.transpose(); // inertia tensor in world frame + + // Compute Jacobian columns for each joint and accumulate mass matrix contributions + for (size_t i = 0; i < n; ++i) { + const RobotJoint& j_i = robot.joints[i]; + if (j_i.type == eJointType::FIXED) { continue; } + if (!robot.jointAffectsLink(i, k)) { continue; } // skip if joint i does not affect link k + + const mathlib::Pose_T& T_joint_i = jointWorldPoses[i]; // pose of joint i in world frame + + // Rotation from joint i frame to world frame + const mathlib::Mat3_T R_i = T_joint_i.template block<3, 3>(0, 0); // rotation from joint i frame to world frame + const mathlib::Vec3_T p_i = T_joint_i.template block<3, 1>(0, 3); // joint position in world frame + const mathlib::Vec3_T z_i = (R_i * j_i.axis).normalized(); // joint axis in world frame + + mathlib::Vec3_T J_vi = z_i.cross(com - p_i); // linear velocity Jacobian column for joint i + mathlib::Vec3_T J_wi = z_i; // angular velocity Jacobian column for joint i + + // Computes the contribution to the mass matrix from this link for joints i and j + for (size_t j = 0; j < n; ++j) { + const RobotJoint& j_j = robot.joints[j]; + if (j_j.type == eJointType::FIXED) { continue; } + + if (!robot.jointAffectsLink(j, k)) { continue; } // skip if joint j does not affect link k + + const mathlib::Pose_T& T_joint_j = jointWorldPoses[j]; // pose of joint i in world frame + + const mathlib::Mat3_T R_j = T_joint_j.template block<3, 3>(0, 0); + const mathlib::Vec3_T p_j = T_joint_j.template block<3, 1>(0, 3); + const mathlib::Vec3_T z_j = (R_j * j_j.axis).normalized(); + + mathlib::Vec3_T J_vj = z_j.cross(com - p_j); // linear velocity Jacobian column for joint j + mathlib::Vec3_T J_wj = z_j; // angular velocity Jacobian column for joint j + + M_out(i, j) += m * J_vi.dot(J_vj) + J_wi.transpose() * I_world * J_wj; // [kg*m^2] + } + } + } + } + + // Computes the Coriolis and centrifugal bias vector h(q, qd) based on the current state and robot configuration + template + mathlib::VecX_T RobotDynamics::computeCoriolisVector( + const RobotConstModel& robot, + const mathlib::VecX_T& q, + const mathlib::VecX_T& qd, + const std::vector>& T_world, + const mathlib::MatX_T& M + ) const { + const size_t n = robot.joints.size(); + const Scalar eps = 1e-6; // small value to prevent division by zero + mathlib::VecX_T q_eps = q; + + std::vector> dM_dq(n, mathlib::MatX_T::Zero(n, n)); // partial derivatives of M with respect to each joint angle + mathlib::VecX_T x_eps(2 * n); // state vector for kinematics + + std::vector> T_world_eps; // forward kinematics for perturbed configurations + T_world_eps.resize(robot.links.size()); + + std::vector> jointWorldPoses_eps; + jointWorldPoses_eps.resize(n); + + mathlib::MatX_T M_plus(n, n); + + // Finite difference approximation of dM/dq for each joint + for (size_t k = 0; k < n; ++k) { + q_eps = q; // reset to original configuration for each joint perturbation + q_eps[k] += eps; // perturb joint k by a small amount + + // Construct the state vector for the perturbed configuration + for (size_t i = 0; i < n; ++i) { + x_eps[i] = q_eps[i]; + x_eps[n + i] = qd[i]; + } + + // Compute forward kinematics for the perturbed state + _kinematics->computeForwardKinematics_fromState(robot, x_eps, T_world_eps); + jointWorldPoses_eps = _kinematics->calcJointWorldPoses(T_world_eps, robot); + computeMassMatrix(robot, T_world_eps, jointWorldPoses_eps, M_plus); // mass matrix for the perturbed configuration + + dM_dq[k] = (M_plus - M) / eps; // [kg*m^2/rad], partial derivative of mass matrix with + } + + // Compute Coriolis and centrifugal bias vector h using Christoffel symbols of the first kind + VecX_T h = VecX_T::Zero(n); + for (size_t i = 0; i < n; ++i) { + for (size_t j = 0; j < n; ++j) { + for (size_t k = 0; k < n; ++k) { + Scalar C_ijk = 0.5 * (dM_dq[k](i, j) + dM_dq[j](i, k) - dM_dq[i](j, k)) * qd[k]; // Christoffel symbol of the first kind for indices (i, j, k) + h(i) += C_ijk * qd[j] * qd[k]; // contribution to Coriolis and centrifugal bias for joint i from joints j and k + } + } + } + return h; // [Nm], Coriolis and centrifugal bias vector for the robot at configuration q and velocity qd + } + + // Computes the gravity torque for a joint based on the current state and robot configuration + template + mathlib::VecX_T RobotDynamics::computeGravityTorque( + const RobotConstModel& robot, + const std::vector>& T_world, + const std::vector>& jointWorldPoses + ) const { + const size_t n = robot.joints.size(); + mathlib::VecX_T tau_G = mathlib::VecX_T::Zero(n); + Scalar g{ _gravity }; // [m/s^2], gravity acceleration magnitude + + // For each joint, sum the gravity contributions from all links + for (size_t i = 0; i < n; ++i) { + const RobotJoint& j = robot.joints[i]; + if (j.type == eJointType::FIXED) { continue; } + + Scalar tau_g_i = Scalar(0); // [Nm], gravity torque contribution for joint i + + const mathlib::Pose_T& T_joint = jointWorldPoses[i]; // pose of joint i in world frame + + const mathlib::Mat3_T R_i = T_joint.block<3, 3>(0, 0); + const mathlib::Vec3_T p_i = T_joint.block<3, 1>(0, 3); + const mathlib::Vec3_T axis_world = (R_i * robot.joints[i].axis).normalized(); + + // For each link, compute the gravitational force and its torque contribution about joint i + for (size_t k = 0; k < robot.links.size(); ++k) { + const RobotLink& link = robot.links[k]; + const Scalar m = (Scalar)link.inertial.mass; + if (m <= 0.0) { continue; } + + if (!robot.jointAffectsLink(i, k)) { continue; } + + // Link's center of mass in world frame + const mathlib::Mat3_T R_k = T_world[k].template block<3, 3>(0, 0); + const mathlib::Vec3_T com_world = R_k * link.inertial.com_xyz + T_world[k].template block<3, 1>(0, 3); + + // Gravitational force on the link + mathlib::Vec3_T g_world; + g << mathlib::Vec3_T(0.0, 0.0, -g); // [m/s^2], gravity vector in world frame + const mathlib::Vec3_T F_g; + F_g = m * g_world; // [N], gravitational force on the link in world frame + + // Torque contribution from this link's weight about joint i + const mathlib::Vec3_T r = com_world - p_i; // [m] + + // Torque = r × F_g projected onto joint axis + tau_g_i += axis_world.dot(r.cross(F_g)); + } + tau_G[i] = tau_g_i; + } + return tau_G; // [Nm], gravity torques for each joint + } + + // Computes the analytical Jacobian matrix J(q) for the robot based on the current state and robot configuration + template + void RobotDynamics::analyticalJacobian( + const RobotConstModel& robot, + const mathlib::VecX_T& x, + mathlib::MatX_T& J_out, + DenseDynamicsScratch& scratch + ) { + const size_t n = robot.joints.size(); + + Eigen::Map> q_local(x.data(), n); + Eigen::Map> qd_local(x.data() + n, n); + + _kinematics->computeForwardKinematics_fromState(robot, x, scratch.T_world); + scratch.jointWorldPoses = _kinematics->calcJointWorldPoses(scratch.T_world, robot); + computeMassMatrix(robot, scratch.T_world, scratch.jointWorldPoses, scratch.M); + + J_out.setZero(2 * n, 2 * n); // [rad/rad] position part, [rad/s / rad/s] velocity part + J_out.block(0, n, n, n).setIdentity(); + + mathlib::MatX_T dTau_dq = mathlib::MatX::Zero(n, n); + mathlib::MatX_T dTau_dv = mathlib::MatX::Zero(n, n); + + for (size_t i = 0; i < n; ++i) { + const RobotJoint& joint = robot.joints[i]; + if (joint.type == eJointType::FIXED) { continue; } + + const Scalar wn = (Scalar)joint.wn_target; + const Scalar z = (Scalar)joint.zeta_target; + const Scalar eps = (Scalar)1e-6; + const Scalar I_eff = std::max(scratch.M(i, i), eps); + + const Scalar k_p = I_eff * wn * wn; + const Scalar k_d = Scalar(2.0) * z * I_eff * wn; + + const Scalar b = Scalar(0.2); // viscous damping coefficient + const Scalar c = Scalar(0.05); // Coulomb friction coefficient + const Scalar eps_f = (Scalar)1e-2; + + dTau_dq(i, i) = -k_p; + + Scalar qd_i = qd_local[i]; + Scalar tanh_term = std::tanh(qd_i / eps_f); + Scalar stiff_friction_slope = c * (1.0 - tanh_term * tanh_term) / eps_f; + dTau_dv(i, i) = -k_d - b + stiff_friction_slope; + } + + auto solver = scratch.M.ldlt(); + mathlib::MatX_T da_dq = solver.solve(dTau_dq); + mathlib::MatX_T da_dv = solver.solve(dTau_dv); + + J_out.block(n, 0, n, n) = da_dq; + J_out.block(n, n, n, n) = da_dv; + } + + // Computes the Coriolis and centrifugal torque for a joint based on the current state and robot configuration + template + mathlib::VecX_T RobotDynamics::derivative_dense( + Scalar t, + const mathlib::VecX_T& x, + const RobotSimSnapshot& snap, + DynamicsScratch& scratch, + DynamicsResult& out + ) { + const size_t n = snap.model->joints.size(); + mathlib::VecX_T dx(2 * n); + + // Map the input state vector to joint angles and velocities + Eigen::Map> q(x.data(), n); + Eigen::Map> qd(x.data() + n, n); + + _kinematics->computeForwardKinematics_fromState(*snap.model, x, scratch.dense.T_world); + + scratch.dense.jointWorldPoses = _kinematics->calcJointWorldPoses(scratch.dense.T_world, *snap.model); + + computeMassMatrix(*snap.model, scratch.dense.T_world, scratch.dense.jointWorldPoses, scratch.dense.M); // [kg*m^2], full mass matrix for the robot at configuration q + scratch.dense.h = computeCoriolisVector(*snap.model, q, qd, scratch.dense.T_world, scratch.dense.M); // [Nm], full Coriolis and centrifugal torque vector + + scratch.g.setZero(); + if (snap.torqueMode != eTorqueMode::NONE) { + scratch.g = computeGravityTorque(*snap.model, scratch.dense.T_world, scratch.dense.jointWorldPoses); + } + + scratch.dense.tau.setZero(); + for (size_t i = 0; i < n; ++i) { + const RobotJoint& joint = snap.model->joints[i]; + + // Fixed joints + if (joint.type == eJointType::FIXED) { + out.metrics.q[i] = q[i]; + out.metrics.qd[i] = qd[i]; + out.metrics.qdd[i] = 0.0; + continue; + } + + const Scalar wn = (Scalar)joint.wn_target; // [rad/s], natural frequency + const Scalar z = (Scalar)joint.zeta_target; // damping ratio + + const Scalar err = snap.q_ref[i] - q[i]; // [rad], position error + const Scalar err_d = snap.qd_ref[i] - qd[i]; // [rad/s], velocity error + + const Scalar eps = (Scalar)1e-6; + + const Scalar I_eff = std::max(scratch.dense.M(i, i), eps); // [kg*m^2], effective inertia for joint i with floor to prevent singularities + const Scalar k_p = I_eff * wn * wn; // [Nm/rad], proportional gain + const Scalar k_d = Scalar(2.0) * z * I_eff * wn; // [Nm/(rad/s)], derivative gain + + const Scalar b = Scalar(0.2); // viscous damping coefficient + const Scalar c = Scalar(0.05); // Coulomb friction coefficient + const Scalar eps_f = (Scalar)1e-2; + + Scalar tau_i = k_p * err + k_d * err_d + I_eff * snap.qdd_ref[i]; // [Nm], control torque for joint i + tau_i += scratch.g[i]; // Gravity compensation + tau_i += scratch.dense.h[i]; // add Coriolis and centrifugal bias + tau_i -= b * qd[i]; // subtract viscous damping + tau_i -= c * std::tanh(qd[i] / eps_f); // subtract Coulomb friction + + scratch.dense.tau[i] = tau_i; + + // metrics + out.metrics.q[i] = q[i]; + out.metrics.qd[i] = qd[i]; + + out.metrics.err[i] = err; + out.metrics.errd[i] = err_d; + + out.metrics.I_eff[i] = I_eff; + out.metrics.tau[i] = tau_i; + + //_metrics.tau_sat[i] = tau_sat; + //_metrics.sat_flag[i] = saturated; + } + + // Solve Forward Dynamics: M(q) qdd = tau - h(q, qd) - g(q) + scratch.dense.rhs.noalias() = scratch.dense.tau - scratch.dense.h - scratch.g; // [Nm], right-hand side of the dynamics equation M*qdd = tau - h - g + + // Solve for Accelerations + out.qdd = scratch.dense.M.ldlt().solve(scratch.dense.rhs); // [rad/s^2], joint accelerations computed from dynamics + out.metrics.qdd = out.qdd; + + // Fill derivatives + dx.head(n) = qd; + dx.tail(n) = out.qdd; + + return dx; + } + + template + mathlib::VecX_T RobotDynamics::derivative_spatial( + const robots::SpatialModel& model, + Scalar t, + const mathlib::VecX_T& x, + const RobotSimSnapshot& snap, + DynamicsScratch& scratch, + DynamicsResult& out + ) { + const size_t n = model.joints.size(); + mathlib::VecX_T dx(2 * n); + + Eigen::Map> q(x.data(), n); + Eigen::Map> qd(x.data() + n, n); + + SpatialDynamics::computeSpatialKinematicsAndBias( + model, q, qd, + scratch.spatial.Xup, + scratch.spatial.v, + scratch.spatial.c + ); + + mathlib::MatX_T M = SpatialDynamics::CRBA(model, scratch.spatial.Xup, scratch); + // RNEA to compute gravity compensation (q, 0, 0) for gravity, (q, qd, 0) for Coriolis + mathlib::VecX_T tau_g = SpatialDynamics::RNEA(model, q, VecX::Zero(n), VecX::Zero(n), scratch); + + scratch.dense.tau.setZero(); + for (size_t i = 0; i < n; ++i) { + const SpatialJoint& joint = model.joints[i]; + if (!isControlledJoint(joint.type)) { + scratch.dense.tau[i] = 0.0; + continue; + } + + const Scalar wn = Scalar(5.0); + const Scalar z = Scalar(0.7); + + const Scalar err = snap.q_ref[i] - q[i]; + const Scalar err_d = snap.qd_ref[i] - qd[i]; + + const Scalar eps = (Scalar)1e-6; + + const Scalar I_eff = std::max(M(i, i), eps); + const Scalar k_p = I_eff * wn * wn; + const Scalar k_d = Scalar(2.0) * z * I_eff * wn; + + const Scalar b = Scalar(0.2); // viscous damping coefficient + const Scalar c = Scalar(0.05); // Coulomb friction coefficient + const Scalar eps_f = (Scalar)1e-2; + + Scalar tau_i = k_p * err + k_d * err_d + I_eff * snap.qdd_ref[i]; + tau_i -= b * qd[i]; + //tau_i -= 0.05 * std::tanh(qd[i] / 1e-2); + + scratch.dense.tau[i] = tau_i; + + out.metrics.q[i] = q[i]; + out.metrics.qd[i] = qd[i]; + + out.metrics.err[i] = err; + out.metrics.errd[i] = err_d; + + out.metrics.I_eff[i] = I_eff; + out.metrics.tau[i] = tau_i; + } + + out.qdd = SpatialDynamics::ABA(model, q, qd, scratch.dense.tau, scratch); + out.metrics.qdd = out.qdd; + dx.head(n) = qd; + dx.tail(n) = out.qdd; + + return dx; + } + + template + void RobotDynamics::jacobian_spatial( + const robots::SpatialModel& model, + const mathlib::VecX_T& x, + const RobotSimSnapshot& snap, + const mathlib::VecX_T& kp, + const mathlib::VecX_T& kd, + mathlib::MatX_T& F_out, + DynamicsScratch& scratch + ) { + const size_t n = model.joints.size(); + + F_out.setZero(2 * n, 2 * n); + F_out.block(0, n, n, n).setIdentity(); + + Eigen::Map> q(x.data(), n); + Eigen::Map> qd(x.data() + n, n); + + SpatialDynamics::computeSpatialKinematicsAndBias( + model, q, qd, + scratch.spatial.Xup, + scratch.spatial.v, + scratch.spatial.c + ); + + scratch.dense.M = SpatialDynamics::CRBA(model, scratch.spatial.Xup, scratch); + + mathlib::MatX_T dTau_dq = mathlib::MatX_T::Zero(n, n); + mathlib::MatX_T dTau_dv = mathlib::MatX_T::Zero(n, n); + + for (size_t i = 0; i < n; ++i) { + const SpatialJoint& joint = model.joints[i]; + if (!isControlledJoint(joint.type)) { continue; } + dTau_dq(i, i) = -kp[i]; + const Scalar eps_f = 1e-2; + const Scalar b = Scalar(0.2); + const Scalar c = Scalar(0.05); + + Scalar tanh_term = std::tanh(qd[i] / eps_f); + Scalar stiff_friction_slope = -c * (Scalar(1.0) - tanh_term * tanh_term) / eps_f; + dTau_dv(i, i) = -kd[i] - b; + } + + Eigen::LDLT> solver(scratch.dense.M); // compute the Cholesky decomposition of the mass matrix for efficient solving + mathlib::MatX_T dqdd_dtau_q = solver.solve(dTau_dq); // compute the partial derivative of qdd with respect to q + mathlib::MatX_T dqdd_dtau_v = solver.solve(dTau_dv); // compute the partial derivative of qdd with respect to qd + + F_out.block(n, 0, n, n) = dqdd_dtau_q; // fill the Jacobian block for qdd with respect to q + F_out.block(n, n, n, n) = dqdd_dtau_v; // fill the Jacobian block for qdd with respect to qd) + } + + // Computes the derivative of the state vector with control gains based on the current state and robot configurations + template + mathlib::VecX_T RobotDynamics::derivative_with_gains( + Scalar t, + const mathlib::VecX_T& x, + const RobotSimSnapshot& snap, + const mathlib::VecX_T& kp, + const mathlib::VecX_T& kd, + DynamicsScratch& scratch, + DynamicsResult& out + ) { + const size_t n = snap.model->joints.size(); + mathlib::VecX_T dx(2 * n); + + Eigen::Map> q(x.data(), n); + Eigen::Map> qd(x.data() + n, n); + + _kinematics->computeForwardKinematics_fromState(*snap.model, x, scratch.dense.T_world); + scratch.dense.jointWorldPoses = _kinematics->calcJointWorldPoses(scratch.dense.T_world, *snap.model); + + computeMassMatrix(*snap.model, scratch.dense.T_world, scratch.dense.jointWorldPoses, scratch.dense.M); + scratch.dense.h = computeCoriolisVector(*snap.model, q, qd, scratch.dense.T_world, scratch.dense.M); + + scratch.g.setZero(); + if (snap.torqueMode != eTorqueMode::NONE) { + scratch.g = computeGravityTorque(*snap.model, scratch.dense.T_world, scratch.dense.jointWorldPoses); + } + + scratch.dense.tau.setZero(); + for (size_t i = 0; i < n; ++i) { + if (snap.model->joints[i].type == eJointType::FIXED) continue; + + const Scalar eps = (Scalar)1e-6; + const Scalar b = Scalar(0.2); // viscous damping coefficient + const Scalar c = Scalar(0.05); // Coulomb friction coefficient + const Scalar eps_f = (Scalar)1e-2; + + Scalar tau_i = kp[i] * (snap.q_ref[i] - q[i]) + kd[i] * (snap.qd_ref[i] - qd[i]) + std::max(scratch.dense.M(i, i), eps) * snap.qdd_ref[i]; + tau_i += scratch.g[i] + scratch.dense.h[i]; + tau_i -= b * qd[i]; + tau_i -= c * std::tanh(qd[i] / eps_f); + + scratch.dense.tau[i] = tau_i; + } + + scratch.dense.rhs.noalias() = scratch.dense.tau - scratch.dense.h - scratch.g; + out.qdd = scratch.dense.M.ldlt().solve(scratch.dense.rhs); + out.metrics.qdd = out.qdd; + + dx.head(n) = qd; + dx.tail(n) = out.qdd; + return dx; + } + + // Computes the Jacobian matrix with control gains based on the current state and robot configuration + template + void RobotDynamics::jacobian_with_gains( + const mathlib::VecX_T& x, + const RobotSimSnapshot& snap, + const mathlib::VecX_T& kp, + const mathlib::VecX_T& kd, + mathlib::MatX_T& F_out, + DenseDynamicsScratch& scratch + ) { + const size_t n = snap.model->joints.size(); + + F_out.setZero(2 * n, 2 * n); + F_out.block(0, n, n, n).setIdentity(); + + Eigen::Map> q(x.data(), n); + Eigen::Map> qd(x.data() + n, n); + + // Compute a local mass matrix for this exact stage evaluation frame + _kinematics->computeForwardKinematics_fromState(*snap.model, x, scratch.T_world); + scratch.jointWorldPoses = _kinematics->calcJointWorldPoses(scratch.T_world, *snap.model); + computeMassMatrix(*snap.model, scratch.T_world, scratch.jointWorldPoses, scratch.M); + + mathlib::MatX dTau_dq = mathlib::MatX_T::Zero(n, n); + mathlib::MatX dTau_dv = mathlib::MatX_T::Zero(n, n); + + for (size_t i = 0; i < n; ++i) { + if (snap.model->joints[i].type == eJointType::FIXED) continue; + + dTau_dq(i, i) = -kp[i]; + + const Scalar b = Scalar(0.2); // viscous damping coefficient + const Scalar c = Scalar(0.05); // Coulomb friction coefficient + const Scalar eps_f = (Scalar)1e-2; + + Scalar tanh_term = std::tanh(qd[i] / eps_f); + Scalar stiff_friction = -c * (Scalar(1.0) - tanh_term * tanh_term) / eps_f; + dTau_dv(i, i) = -kd[i] - b + stiff_friction; + } + + auto solver = scratch.M.ldlt(); + F_out.block(n, 0, n, n) = solver.solve(dTau_dq); + F_out.block(n, n, n, n) = solver.solve(dTau_dv); + } +} // namespace robots \ No newline at end of file diff --git a/DSFE_App/DSFE_Core/include/Robots/RobotKinematics.h b/DSFE_App/DSFE_Core/include/Robots/RobotKinematics.h index 515aae6f..6a820745 100644 --- a/DSFE_App/DSFE_Core/include/Robots/RobotKinematics.h +++ b/DSFE_App/DSFE_Core/include/Robots/RobotKinematics.h @@ -3,7 +3,12 @@ #include "EngineCore.h" #include "MathLibAPI.h" -#include "core/Types.h" +#include +#include +#include +#include + +#include "EngineLib/LogMacros.h" namespace robots { // Forward declarations @@ -20,25 +25,30 @@ namespace robots { RobotKinematics(); // Computes the forward kinematics for the robot based on the current state and robot configuration + template void computeForwardKinematics_fromState( const RobotConstModel& robot, - const mathlib::VecX& x, - std::vector& T_world_out + const mathlib::VecX_T& x, + std::vector>& T_world_out ) const; // Computes the joint world poses for all joints based on the current state and robot configuration - std::vector calcJointWorldPoses( - const std::vector& T_world, + template + std::vector> calcJointWorldPoses( + const std::vector>& T_world, const RobotConstModel& robot ); // Computes the forward kinematics for a single joint motion based on the joint axis and angle - mathlib::Pose jointMotionTransform( - const mathlib::Vec3& axis_joint, - double q + template + mathlib::Pose_T jointMotionTransform( + const mathlib::Vec3_T& axis_joint, + Scalar q ) const; // Converts roll-pitch-yaw angles (in radians) to a quaternion representation mathlib::Quat rpyRadToQuat(const mathlib::Vec3& rpyRad); }; -} \ No newline at end of file +} + +#include "Robots/RobotKinematics.inl" \ No newline at end of file diff --git a/DSFE_App/DSFE_Core/include/Robots/RobotKinematics.inl b/DSFE_App/DSFE_Core/include/Robots/RobotKinematics.inl new file mode 100644 index 00000000..d67d5f75 --- /dev/null +++ b/DSFE_App/DSFE_Core/include/Robots/RobotKinematics.inl @@ -0,0 +1,92 @@ +// DSFE_Core RobotKinematics.inl +#pragma once + +namespace robots { + // Computes the forward kinematics for the robot based on the current state and robot configuration + template + void RobotKinematics::computeForwardKinematics_fromState( + const RobotConstModel& robot, + const mathlib::VecX_T& x, + std::vector>& T_world_out + ) const { + const auto& joints = robot.joints; + const auto& links = robot.links; + const size_t n = joints.size(); + + T_world_out.resize(robot.links.size()); + + mathlib::Pose_T T = mathlib::Pose_T::Identity(); // world -> base + T_world_out[0] = T; // base link + + if (T_world_out.empty()) { + LOG_ERROR("T_world_out is empty"); + return; + } + + // Compute the transform to the next link using each joint + for (size_t i = 0; i < n; ++i) { + const Scalar& joint = joints[i]; + const Scalar q = x[i]; // joint angle from state vector + + mathlib::Pose_T T_origin = mathlib::Pose_T::Identity(); // transform from parent link to joint frame (fixed) + T_origin.template block<3, 3>(0, 0) = joint.origin_q.toRotationMatrix(); // rotation from parent link frame to joint frame, derived from rpy in JSON + T_origin.template block<3, 1>(0, 3) = joint.origin_xyz; // translation from parent link to joint frame + + // Compute joint motion transform based on joint axis and angle + Pose_T T_motion = mathlib::Pose_T::Identity(); + if (joint.type == eJointType::REVOLUTE) { + T_motion = jointMotionTransform(joint.axis, q); // rotation about joint axis + } + else if (joint.type == eJointType::PRISMATIC) { + T_motion.template block<3, 1>(0, 3) = joint.axis.normalized() * q; // translation along joint axis + } + + // compose transforms + T = T * T_origin * T_motion; // parent -> joint -> motion -> child + + int childIdx = robot.linkIndex(joint.child); + if (childIdx < 0 || childIdx >= T_world_out.size()) { + LOG_ERROR("Invalid child link index for joint {}: {}", joint.name.c_str(), childIdx); + continue; + } + + T_world_out[childIdx] = T; // world -> child link + } + } + + // Computes the joint world poses for all joints based on the current state and robot configuration + template + std::vector> RobotKinematics::calcJointWorldPoses( + const std::vector>& T_world, + const RobotConstModel& robot + ) { + std::vector> jointWorldPoses(robot.joints.size()); + + for (size_t i = 0; i < robot.joints.size(); ++i) { + const RobotJoint& joints = robot.joints[i]; + int childIdx = robot.linkIndex(joints.child); + + if (childIdx < 0 || childIdx >= T_world.size()) { + LOG_ERROR("Invalid child link index for joint {}: {}", joints.name.c_str(), childIdx); + continue; + } + + jointWorldPoses[i] = T_world[childIdx]; + } + return jointWorldPoses; + } + + // Computes the forward kinematics for a single joint motion based on the joint axis and angle + template + mathlib::Pose_T RobotKinematics::jointMotionTransform( + const mathlib::Vec3_T& axis_joint, + Scalar q + ) const { + mathlib::Pose_T T = mathlib::Pose_T::Identity(); // homogeneous transformation matrix (4x4) + + Eigen::AngleAxis aa(q, axis_joint.normalized()); // create angle-axis rotation from joint angle and axis + T.template block<3, 3>(0, 0) = aa.toRotationMatrix(); // set upper-left 3x3 block to rotation matrix + + return T; // (4x4) homogeneous transformation + } +} \ No newline at end of file diff --git a/DSFE_App/DSFE_Core/include/Robots/RobotModel.h b/DSFE_App/DSFE_Core/include/Robots/RobotModel.h index ce600fcd..70ef1cf3 100644 --- a/DSFE_App/DSFE_Core/include/Robots/RobotModel.h +++ b/DSFE_App/DSFE_Core/include/Robots/RobotModel.h @@ -7,10 +7,12 @@ #include #include +#include + #include "Platform/Logger.h" #include "EngineLib/LogMacros.h" -constexpr double DEG2RAD = std::numbers::pi / 180.0; +constexpr double DEG2RAD = std::numebers::pi / 180.0; namespace robots { // --- Robot Model Kinematic Models --- diff --git a/DSFE_App/DSFE_Core/include/Robots/SpatialDynamics.h b/DSFE_App/DSFE_Core/include/Robots/SpatialDynamics.h index b4f65deb..55b96f3b 100644 --- a/DSFE_App/DSFE_Core/include/Robots/SpatialDynamics.h +++ b/DSFE_App/DSFE_Core/include/Robots/SpatialDynamics.h @@ -17,38 +17,7 @@ namespace robots { std::vector>& Xup_out, std::vector>& v_out, std::vector>& c_out - ) { - const size_t n = model.joints.size(); - v_out.resize(n); - Xup_out.resize(n); - c_out.resize(n); - - for (size_t i = 0; i < n; ++i) { - const SpatialJoint& j = model.joints[i]; - - // Joint Transform XJ - mathlib::SpatialMat_T XJ = mathlib::SpatialMat_T::Identity(); - - if (j.type == eJointType::REVOLUTE) { - Eigen::AngleAxisd aa(q[i], j.S.angular().normalized()); - mathlib::Mat3_T R = aa.toRotationMatrix(); - XJ = mathlib::spatialTransform(R, mathlib::Vec3_T::Zero()); - } - else if (j.type == eJointType::PRISMATIC) { - mathlib::Vec3_T r = q[i] * j.S.linear().normalized(); - XJ = mathlib::spatialTransform(mathlib::Mat3_T::Identity(), r); - } - - Xup_out[i] = XJ * j.Xtree; // Combined Transform - mathlib::SpatialVec_T vJ = j.S * qd[i]; // Joint Velocity - - // Root Link - if (j.parent < 0) { v_out[i] = vJ; } - else { v_out[i] = Xup_out[i] * v_out[j.parent] + vJ; } - - c_out[i] = crossMotion(v_out[i], vJ); // Coriolis Term - } - } + ); template static void computeAccelerations_RNEA( @@ -58,57 +27,7 @@ namespace robots { const std::vector>& c, const mathlib::VecX_T& g, std::vector>& a_out - ) { - const size_t n = model.joints.size(); - a_out.resize(n); - - SpatialVec_T a0; // base acceleration (gravity) - a0.v << - g.template segment<3>(0), - g.template segment<3>(3); - - for (size_t i = 0; i < n; ++i) { - const SpatialJoint& j = model.joints[i]; - mathlib::SpatialVec_T aJ = j.S * qdd[i]; // Joint Acceleration - - // Root Link - if (j.parent < 0) { - a_out[i] = Xup[i] * a0 + aJ + c[i]; - continue; - } - - a_out[i] = Xup[i] * a_out[j.parent] + aJ + c[i]; - } - } - - template - static void computeBackwardForces_RNEA( - const SpatialModel& model, - const std::vector>& Xup, - const std::vector>& v, - const std::vector>& a, - mathlib::VecX_T& tau_out - ) { - const size_t n = model.joints.size(); - tau_out.resize(n); - - std::vector> f(n); - - // Forward Force Computation - for (size_t i = 0; i < n; ++i) { - const SpatialJoint& j = model.joints[i]; - mathlib::SpatialVec_T I_v = j.inertia * v[i]; - mathlib::SpatialVec_T coriolis = crossForce(v[i], I_v); - f[i].v = j.inertia * a[i].v + coriolis.v; - } - - // Backward Recursion Computation - for (int i = (int)n - 1; i >= 0; --i) { - const SpatialJoint& j = model.joints[i]; - tau_out[i] = j.S.dot(f[i]); - if (j.parent >= 0) { f[j.parent] += Xup[i].transpose() * f[i]; } - } - } + ); template static mathlib::VecX_T RNEA( @@ -117,67 +36,23 @@ namespace robots { const mathlib::VecX_T& qd, const mathlib::VecX_T& qdd, DynamicsScratch& scratch - ) { - const size_t n = model.joints.size(); - - // TODO Remove these temp scratches AFTER debugging - std::vector> v(n); - std::vector> Xup(n); - std::vector> c(n); - std::vector> a(n); - mathlib::VecX_T tau; - - // Compute spatial velocities and transforms - computeSpatialKinematicsAndBias(model, q, qd, Xup, v, c); - // Compute spatial accelerations - computeAccelerations_RNEA(model, qdd, Xup, c, scratch.g, a); - // Compute inverse dynamics (joint torques) - computeBackwardForces_RNEA(model, Xup, v, a, tau); + ); - return tau; - } + template + void computeBackwardForces_RNEA( + const SpatialModel& model, + const std::vector>& Xup, + const std::vector>& v, + const std::vector>& a, + mathlib::VecX_T& tau_out + ); template static mathlib::MatX_T CRBA( const SpatialModel& model, const std::vector>& Xup, DynamicsScratch& scratch - ) { - const size_t n = model.joints.size(); - scratch.dense.M.setZero(n, n); - std::vector> Ic(n); // spatial inertia for each link - - // Initialise spatial inertia for each link based on the robot model - for (size_t i = 0; i < n; ++i) { Ic[i] = model.joints[i].inertia; } - - // Upward pass: propagate spatial inertia from child links to parent joints - for (int i = (int)n - 1; i >= 0; --i) { - const SpatialJoint& j = model.joints[i]; - if (j.type == eJointType::FIXED) { continue; } - int p = j.parent; - if (p >= 0) { - Ic[p] += Xup[i].transpose() * Ic[i] * Xup[i]; - } - } - - // Downward pass: compute mass matrix contributions for each joint - for (size_t i = 0; i < n; ++i) { - const SpatialJoint& j = model.joints[i]; - if (j.type == eJointType::FIXED) { continue; } - SpatialVec_T F = Ic[i] * j.S; - scratch.dense.M(i, i) = j.S.dot(F); - - int jIdx = (int)i; - while (model.joints[jIdx].parent >= 0) { - int p = model.joints[jIdx].parent; - F = Xup[jIdx].transpose() * F; - scratch.dense.M(i, p) = model.joints[p].S.dot(F); - scratch.dense.M(p, i) = scratch.dense.M(i, p); - jIdx = p; - } - } - return scratch.dense.M; // [kg*m^2], mass matrix computed using the Composite Rigid Body Algorithm (CRBA) - } + ); template static void computeArticulatedBodies_ABA( @@ -192,46 +67,7 @@ namespace robots { mathlib::VecX_T& u_out, mathlib::VecX_T& d_out, std::vector>& U_out - ) { - const size_t n = model.joints.size(); - - // Resize scratch buffers - IA_out.resize(n); - pA_out.resize(n); - Ia_out.resize(n); - U_out.resize(n); - u_out.resize(n); - d_out.resize(n); - - // Upward pass: compute articulated body inertias and bias forces - for (int i = (int)n - 1; i >= 0; --i) { - const SpatialJoint& j = model.joints[i]; - - if (j.type == eJointType::FIXED) { - Ia_out[i] = IA_out[i]; - if (j.parent >= 0) { - IA_out[j.parent] += Xup[i].transpose() * Ia_out[i] * Xup[i]; - pA_out[j.parent] += Xup[i].transpose() * pA_out[i]; - } - continue; - } - - U_out[i] = IA_out[i] * j.S; - d_out[i] = dot(j.S, U_out[i]); - if (std::abs(d_out[i]) < 1e-12) { d_out[i] = 1e-12; } // Regularisation to avoid singularities - - u_out[i] = tau[i] - dot(j.S, pA_out[i]); - Ia_out[i] = IA_out[i] - outer(U_out[i]) / d_out[i]; - - // pA = pA + Ia * c + U * (u/d) - pA_out[i] += Ia_out[i] * c[i] + U_out[i] * (u_out[i] / d_out[i]); - - if (j.parent >= 0) { - IA_out[j.parent] += Xup[i].transpose() * Ia_out[i] * Xup[i]; - pA_out[j.parent] += Xup[i].transpose() * pA_out[i]; - } - } - } + ); template static void computeAccelerations_ABA( @@ -244,26 +80,7 @@ namespace robots { const SpatialVec_T& a0, std::vector>& a_out, mathlib::VecX_T& qdd_out - ) { - const size_t n = model.joints.size(); - a_out.resize(n); - qdd_out.resize(n); - - for (size_t i = 0; i < n; ++i) { - const SpatialJoint& j = model.joints[i]; - - if (j.parent < 0) { a_out[i] = Xup[i] * a0 + c[i]; } - else { a_out[i] = Xup[i] * a_out[j.parent] + c[i]; } - - if (j.type == eJointType::FIXED) { - qdd_out[i] = 0.0; - continue; - } - - qdd_out[i] = (u_out[i] - U[i].dot(a_out[i])) / d_out[i]; - a_out[i] += j.S * qdd_out[i]; - } - } + ); template static mathlib::VecX_T ABA( @@ -272,43 +89,8 @@ namespace robots { const mathlib::VecX_T& qd, const mathlib::VecX_T& tau, DynamicsScratch& scratch - ) { - const size_t n = model.joints.size(); - VecX_T qdd = VecX_T::Zero(n); - - SpatialVec_T a0; // base acceleration (gravity) - a0.v << - scratch.g.template segment<3>(0), - scratch.g.template segment<3>(3); - - computeSpatialKinematicsAndBias( - model, q, qd, - scratch.spatial.Xup, - scratch.spatial.v, - scratch.spatial.c - ); - - for (size_t i = 0; i < n; ++i) { - scratch.spatial.IA[i] = model.joints[i].inertia; // Articulated Body Inertia - scratch.spatial.pA[i] = crossForce(scratch.spatial.v[i], (scratch.spatial.IA[i] * scratch.spatial.v[i])); - } - - // Compute articulated body inertias and bias forces - computeArticulatedBodies_ABA( - model, scratch.spatial.Xup, - scratch.spatial.v, scratch.spatial.c, tau, - scratch.spatial.IA, scratch.spatial.pA, scratch.spatial.Ia, - scratch.spatial.u, scratch.spatial.d, scratch.spatial.U - ); - - // Compute joint accelerations using the articulated body algorithm - computeAccelerations_ABA( - model, scratch.spatial.Xup, scratch.spatial.c, - scratch.spatial.u, scratch.spatial.d, scratch.spatial.U, - a0, scratch.spatial.a,qdd - ); - - return qdd; // [rad/s^2], joint accelerations computed using the Articulated Body Algorithm (ABA) - } + ); }; -} \ No newline at end of file +} + +#include "Robots/SpatialDynamics.inl" \ No newline at end of file diff --git a/DSFE_App/DSFE_Core/include/Robots/SpatialDynamics.inl b/DSFE_App/DSFE_Core/include/Robots/SpatialDynamics.inl new file mode 100644 index 00000000..a5617196 --- /dev/null +++ b/DSFE_App/DSFE_Core/include/Robots/SpatialDynamics.inl @@ -0,0 +1,306 @@ +// DSFE_Core SpatialDynamics.inl +#pragma once + +namespace robots { + template + void SpatialDynamics::computeSpatialKinematicsAndBias( + const SpatialModel& model, + const mathlib::VecX_T& q, + const mathlib::VecX_T& qd, + std::vector>& Xup_out, + std::vector>& v_out, + std::vector>& c_out + ) { + const size_t n = model.joints.size(); + v_out.resize(n); + Xup_out.resize(n); + c_out.resize(n); + + for (size_t i = 0; i < n; ++i) { + const SpatialJoint& j = model.joints[i]; + + // Joint Transform XJ + mathlib::SpatialMat_T XJ = mathlib::SpatialMat_T::Identity(); + + if (j.type == eJointType::REVOLUTE) { + Eigen::AngleAxis aa(q[i], j.S.angular().normalized()); + mathlib::Mat3_T R = aa.toRotationMatrix(); + XJ = mathlib::spatialTransform(R, mathlib::Vec3_T::Zero()); + } + else if (j.type == eJointType::PRISMATIC) { + mathlib::Vec3_T r = q[i] * j.S.linear().normalized(); + XJ = mathlib::spatialTransform(mathlib::Mat3_T::Identity(), r); + } + + Xup_out[i] = XJ * j.Xtree; // Combined Transform + mathlib::SpatialVec_T vJ = j.S * qd[i]; // Joint Velocity + + // Root Link + if (j.parent < 0) { v_out[i] = vJ; } + else { v_out[i] = Xup_out[i] * v_out[j.parent] + vJ; } + + c_out[i] = crossMotion(v_out[i], vJ); // Coriolis Term + } + } + + template + void SpatialDynamics::computeAccelerations_RNEA( + const SpatialModel& model, + const mathlib::VecX_T& qdd, + const std::vector>& Xup, + const std::vector>& c, + const mathlib::VecX_T& g, + std::vector>& a_out + ) { + const size_t n = model.joints.size(); + a_out.resize(n); + + SpatialVec_T a0; // base acceleration (gravity) + a0.v << + g.template segment<3>(0), + g.template segment<3>(3); + + for (size_t i = 0; i < n; ++i) { + const SpatialJoint& j = model.joints[i]; + mathlib::SpatialVec_T aJ = j.S * qdd[i]; // Joint Acceleration + + // Root Link + if (j.parent < 0) { + a_out[i] = Xup[i] * a0 + aJ + c[i]; + continue; + } + + a_out[i] = Xup[i] * a_out[j.parent] + aJ + c[i]; + } + } + + template + void SpatialDynamics::computeBackwardForces_RNEA( + const SpatialModel& model, + const std::vector>& Xup, + const std::vector>& v, + const std::vector>& a, + mathlib::VecX_T& tau_out + ) { + const size_t n = model.joints.size(); + tau_out.resize(n); + + std::vector> f(n); + + // Forward Force Computation + for (size_t i = 0; i < n; ++i) { + const SpatialJoint& j = model.joints[i]; + mathlib::SpatialVec_T I_v = j.inertia * v[i]; + mathlib::SpatialVec_T coriolis = crossForce(v[i], I_v); + f[i].v = j.inertia * a[i].v + coriolis.v; + } + + // Backward Recursion Computation + for (int i = (int)n - 1; i >= 0; --i) { + const SpatialJoint& j = model.joints[i]; + tau_out[i] = j.S.dot(f[i]); + if (j.parent >= 0) { f[j.parent] += Xup[i].transpose() * f[i]; } + } + } + + template + mathlib::VecX_T SpatialDynamics::RNEA( + const SpatialModel& model, + const mathlib::VecX_T& q, + const mathlib::VecX_T& qd, + const mathlib::VecX_T& qdd, + DynamicsScratch& scratch + ) { + const size_t n = model.joints.size(); + + // TODO Remove these temp scratches AFTER debugging + std::vector> v(n); + std::vector> Xup(n); + std::vector> c(n); + std::vector> a(n); + mathlib::VecX_T tau; + + // Compute spatial velocities and transforms + computeSpatialKinematicsAndBias(model, q, qd, Xup, v, c); + // Compute spatial accelerations + computeAccelerations_RNEA(model, qdd, Xup, c, scratch.g, a); + // Compute inverse dynamics (joint torques) + computeBackwardForces_RNEA(model, Xup, v, a, tau); + + return tau; + } + + template + mathlib::MatX_T SpatialDynamics::CRBA( + const SpatialModel& model, + const std::vector>& Xup, + DynamicsScratch& scratch + ) { + const size_t n = model.joints.size(); + scratch.dense.M.setZero(n, n); + std::vector> Ic(n); // spatial inertia for each link + + // Initialise spatial inertia for each link based on the robot model + for (size_t i = 0; i < n; ++i) { Ic[i] = model.joints[i].inertia; } + + // Upward pass: propagate spatial inertia from child links to parent joints + for (int i = (int)n - 1; i >= 0; --i) { + const SpatialJoint& j = model.joints[i]; + if (j.type == eJointType::FIXED) { continue; } + int p = j.parent; + if (p >= 0) { + Ic[p] += Xup[i].transpose() * Ic[i] * Xup[i]; + } + } + + // Downward pass: compute mass matrix contributions for each joint + for (size_t i = 0; i < n; ++i) { + const SpatialJoint& j = model.joints[i]; + if (j.type == eJointType::FIXED) { continue; } + SpatialVec_T F = Ic[i] * j.S; + scratch.dense.M(i, i) = j.S.dot(F); + + int jIdx = (int)i; + while (model.joints[jIdx].parent >= 0) { + int p = model.joints[jIdx].parent; + F = Xup[jIdx].transpose() * F; + scratch.dense.M(i, p) = model.joints[p].S.dot(F); + scratch.dense.M(p, i) = scratch.dense.M(i, p); + jIdx = p; + } + } + return scratch.dense.M; // [kg*m^2], mass matrix computed using the Composite Rigid Body Algorithm (CRBA) + } + + template + void SpatialDynamics::computeArticulatedBodies_ABA( + const SpatialModel& model, + const std::vector>& Xup, + const std::vector>& v, + const std::vector>& c, + const mathlib::VecX_T& tau, + std::vector>& IA_out, + std::vector>& pA_out, + std::vector>& Ia_out, + mathlib::VecX_T& u_out, + mathlib::VecX_T& d_out, + std::vector>& U_out + ) { + const size_t n = model.joints.size(); + + // Resize scratch buffers + IA_out.resize(n); + pA_out.resize(n); + Ia_out.resize(n); + U_out.resize(n); + u_out.resize(n); + d_out.resize(n); + + // Upward pass: compute articulated body inertias and bias forces + for (int i = (int)n - 1; i >= 0; --i) { + const SpatialJoint& j = model.joints[i]; + + if (j.type == eJointType::FIXED) { + Ia_out[i] = IA_out[i]; + if (j.parent >= 0) { + IA_out[j.parent] += Xup[i].transpose() * Ia_out[i] * Xup[i]; + pA_out[j.parent] += Xup[i].transpose() * pA_out[i]; + } + continue; + } + + U_out[i] = IA_out[i] * j.S; + d_out[i] = dot(j.S, U_out[i]); + if (std::abs(d_out[i]) < 1e-12) { d_out[i] = 1e-12; } // Regularisation to avoid singularities + + u_out[i] = tau[i] - dot(j.S, pA_out[i]); + Ia_out[i] = IA_out[i] - outer(U_out[i]) / d_out[i]; + + // pA = pA + Ia * c + U * (u/d) + pA_out[i] += Ia_out[i] * c[i] + U_out[i] * (u_out[i] / d_out[i]); + + if (j.parent >= 0) { + IA_out[j.parent] += Xup[i].transpose() * Ia_out[i] * Xup[i]; + pA_out[j.parent] += Xup[i].transpose() * pA_out[i]; + } + } + } + + template + void SpatialDynamics::computeAccelerations_ABA( + const SpatialModel& model, + const std::vector>& Xup, + const std::vector>& c, + const mathlib::VecX_T& u_out, + const mathlib::VecX_T& d_out, + const std::vector>& U, + const SpatialVec_T& a0, + std::vector>& a_out, + mathlib::VecX_T& qdd_out + ) { + const size_t n = model.joints.size(); + a_out.resize(n); + qdd_out.resize(n); + + for (size_t i = 0; i < n; ++i) { + const SpatialJoint& j = model.joints[i]; + + if (j.parent < 0) { a_out[i] = Xup[i] * a0 + c[i]; } + else { a_out[i] = Xup[i] * a_out[j.parent] + c[i]; } + + if (j.type == eJointType::FIXED) { + qdd_out[i] = Scalar(0); + continue; + } + + qdd_out[i] = (u_out[i] - U[i].dot(a_out[i])) / d_out[i]; + a_out[i] += j.S * qdd_out[i]; + } + } + + template + mathlib::VecX_T SpatialDynamics::ABA( + const SpatialModel& model, + const mathlib::VecX_T& q, + const mathlib::VecX_T& qd, + const mathlib::VecX_T& tau, + DynamicsScratch& scratch + ) { + const size_t n = model.joints.size(); + VecX_T qdd = VecX_T::Zero(n); + + SpatialVec_T a0; // base acceleration (gravity) + a0.v << + scratch.g.template segment<3>(0), + scratch.g.template segment<3>(3); + + computeSpatialKinematicsAndBias( + model, q, qd, + scratch.spatial.Xup, + scratch.spatial.v, + scratch.spatial.c + ); + + for (size_t i = 0; i < n; ++i) { + scratch.spatial.IA[i] = model.joints[i].inertia; // Articulated Body Inertia + scratch.spatial.pA[i] = crossForce(scratch.spatial.v[i], (scratch.spatial.IA[i] * scratch.spatial.v[i])); + } + + // Compute articulated body inertias and bias forces + computeArticulatedBodies_ABA( + model, scratch.spatial.Xup, + scratch.spatial.v, scratch.spatial.c, tau, + scratch.spatial.IA, scratch.spatial.pA, scratch.spatial.Ia, + scratch.spatial.u, scratch.spatial.d, scratch.spatial.U + ); + + // Compute joint accelerations using the articulated body algorithm + computeAccelerations_ABA( + model, scratch.spatial.Xup, scratch.spatial.c, + scratch.spatial.u, scratch.spatial.d, scratch.spatial.U, + a0, scratch.spatial.a, qdd + ); + + return qdd; // [rad/s^2], joint accelerations computed using the Articulated Body Algorithm (ABA) + } +} \ No newline at end of file diff --git a/DSFE_App/DSFE_Core/src/Robots/RobotDynamics.cpp b/DSFE_App/DSFE_Core/src/Robots/RobotDynamics.cpp index 059a0897..87607a85 100644 --- a/DSFE_App/DSFE_Core/src/Robots/RobotDynamics.cpp +++ b/DSFE_App/DSFE_Core/src/Robots/RobotDynamics.cpp @@ -5,9 +5,6 @@ #include "Robots/RobotKinematics.h" #include "Robots/RobotSimSnapshot.h" -#include "Robots/SpatialModel.h" -#include "Robots/SpatialDynamics.h" - #include "Robots/TrajectoryManager.h" #include @@ -22,561 +19,4 @@ namespace robots { RobotDynamics::RobotDynamics() : _kinematics(std::make_unique()) { } - - // Computes the inertia tensor of a robot link - mathlib::Mat3 RobotDynamics::computeLinkInertiaTensor(const RobotLink& link) const { - const robots::Inertia& I = link.inertial.inertia; - - // Construct the inertia tensor matrix - Mat3 M = Mat3::Zero(); - M << I.ixx, I.ixy, I.ixz, - I.ixy, I.iyy, I.iyz, - I.ixz, I.iyz, I.izz; - - return M; // [kg*m^2], (3x3) inertia tensor in link frame - } - - static bool isControlledJoint(eJointType t) { - return - t == eJointType::REVOLUTE || - t == eJointType::PRISMATIC; - } - - // Computes the contribution of a single joint and its child link to the effective inertia I_eff of the joint - double RobotDynamics::computeJointInertiaContribution( - const RobotJoint& joint, - const RobotLink& link, - const mathlib::Pose& jointWorldPose, // pose of joint frame in world - const mathlib::Pose& linkWorldPose // pose of the link in world - ) const { - const double mass = link.inertial.mass; - - // Rotation from link frame to world frame - Mat3 R_joint = jointWorldPose.block<3, 3>(0, 0); - // Joint axis in world frame - Vec3 axis_world = (R_joint * joint.axis).normalized(); - - // Position of joint in world frame - Vec3 joint_pos_world = jointWorldPose.block<3, 1>(0, 3); - - // Rotation from link frame to world frame - Mat3 R_link = linkWorldPose.block<3, 3>(0, 0); - Vec3 link_pos_world = linkWorldPose.block<3, 1>(0, 3); - Vec3 com_world = R_link * link.inertial.com_xyz + link_pos_world; - - // Translational contribution (parallel axis theorem) - Vec3 r = com_world - joint_pos_world; - - // Translational contribution to inertia about the joint axis - double I_trans = mass * (axis_world.cross(r)).squaredNorm(); - - // Rotational contribution - Mat3 I_local = computeLinkInertiaTensor(link); - Mat3 I_world = R_link * I_local * R_link.transpose(); - - // Rotational contribution to inertia about the joint axis - double I_rot = axis_world.transpose() * I_world * axis_world; - double I_eff_i = I_trans + I_rot; - - return std::max(I_eff_i, 1e-6); // [kg*m^2], I_eff contribution of this joint + floor to avoid singularities - } - - // Computes the full mass matrix M(q) based on the current state and robot configuration - void RobotDynamics::computeMassMatrix( - const RobotConstModel& robot, - const std::vector& T_world, - const std::vector& jointWorldPoses, - mathlib::MatX& M_out - ) const { - const size_t n = robot.joints.size(); - M_out.resize(n, n); - M_out.setZero(); - - // Compute its contribution to the mass matrix for each link - for (size_t k = 0; k < robot.links.size(); ++k) { - const RobotLink& link = robot.links[k]; - const double m = link.inertial.mass; - - if (m <= 0.0) { continue; } - - const Mat3 R = T_world[k].block<3, 3>(0, 0); // Rotation from link frame to world frame - const Vec3 p = T_world[k].block<3, 1>(0, 3); // Center of mass of the link in world frame - const Vec3 com = R * link.inertial.com_xyz + p; // Center of mass in world frame - - Mat3 I_local = computeLinkInertiaTensor(link); // inertia tensor in link frame - Mat3 I_world = R * I_local * R.transpose(); // inertia tensor in world frame - - // Compute Jacobian columns for each joint and accumulate mass matrix contributions - for (size_t i = 0; i < n; ++i) { - const RobotJoint& j_i = robot.joints[i]; - if (j_i.type == eJointType::FIXED) { continue; } - - if (!robot.jointAffectsLink(i, k)) { continue; } // skip if joint i does not affect link k - - const Pose& T_joint_i = jointWorldPoses[i]; // pose of joint i in world frame - - // Rotation from joint i frame to world frame - const Mat3 R_i = T_joint_i.block<3, 3>(0, 0); // rotation from joint i frame to world frame - const Vec3 p_i = T_joint_i.block<3, 1>(0, 3); // joint position in world frame - const Vec3 z_i = (R_i * j_i.axis).normalized(); // joint axis in world frame - - Vec3 J_vi = z_i.cross(com - p_i); // linear velocity Jacobian column for joint i - Vec3 J_wi = z_i; // angular velocity Jacobian column for joint i - - // Computes the contribution to the mass matrix from this link for joints i and j - for (size_t j = 0; j < n; ++j) { - const RobotJoint& j_j = robot.joints[j]; - if (j_j.type == eJointType::FIXED) { continue; } - - if (!robot.jointAffectsLink(j, k)) { continue; } // skip if joint j does not affect link k - - const Pose& T_joint_j = jointWorldPoses[j]; // pose of joint i in world frame - - const Mat3 R_j = T_joint_j.block<3, 3>(0, 0); - const Vec3 p_j = T_joint_j.block<3, 1>(0, 3); - const Vec3 z_j = (R_j * j_j.axis).normalized(); - - Vec3 J_vj = z_j.cross(com - p_j); // linear velocity Jacobian column for joint j - Vec3 J_wj = z_j; // angular velocity Jacobian column for joint j - - M_out(i, j) += m * J_vi.dot(J_vj) + J_wi.transpose() * I_world * J_wj; // [kg*m^2] - } - } - } - } - - // Computes the Coriolis and centrifugal bias vector h(q, qd) based on the current state and robot configuration - mathlib::VecX RobotDynamics::computeCoriolisVector( - const RobotConstModel& robot, - const mathlib::VecX& q, - const mathlib::VecX& qd, - const std::vector& T_world, - const mathlib::MatX& M - ) const { - const size_t n = robot.joints.size(); - const double eps = 1e-6; // small value to prevent division by zero - mathlib::VecX q_eps = q; - - std::vector dM_dq(n, MatX::Zero(n, n)); // partial derivatives of M with respect to each joint angle - VecX x_eps(2 * n); // state vector for kinematics - - std::vector T_world_eps; // forward kinematics for perturbed configurations - T_world_eps.resize(robot.links.size()); - - std::vector jointWorldPoses_eps; - jointWorldPoses_eps.resize(n); - - MatX M_plus(n, n); - - // Finite difference approximation of dM/dq for each joint - for (size_t k = 0; k < n; ++k) { - q_eps = q; // reset to original configuration for each joint perturbation - q_eps[k] += eps; // perturb joint k by a small amount - - // Construct the state vector for the perturbed configuration - for (size_t i = 0; i < n; ++i) { - x_eps[i] = q_eps[i]; - x_eps[n + i] = qd[i]; - } - - // Compute forward kinematics for the perturbed state - _kinematics->computeForwardKinematics_fromState(robot, x_eps, T_world_eps); - - jointWorldPoses_eps = _kinematics->calcJointWorldPoses(T_world_eps, robot); - - computeMassMatrix(robot, T_world_eps, jointWorldPoses_eps, M_plus); // mass matrix for the perturbed configuration - - dM_dq[k] = (M_plus - M) / eps; // [kg*m^2/rad], partial derivative of mass matrix with - } - - // Compute Coriolis and centrifugal bias vector h using Christoffel symbols of the first kind - VecX h = VecX::Zero(n); - for (size_t i = 0; i < n; ++i) { - for (size_t j = 0; j < n; ++j) { - for (size_t k = 0; k < n; ++k) { - double C_ijk = 0.5 * (dM_dq[k](i, j) + dM_dq[j](i, k) - dM_dq[i](j, k)) * qd[k]; // Christoffel symbol of the first kind for indices (i, j, k) - h(i) += C_ijk * qd[j] * qd[k]; // contribution to Coriolis and centrifugal bias for joint i from joints j and k - } - } - } - return h; // [Nm], Coriolis and centrifugal bias vector for the robot at configuration q and velocity qd - } - - // Computes the gravity torque for a joint based on the current state and robot configuration - mathlib::VecX RobotDynamics::computeGravityTorque( - const RobotConstModel& robot, - const std::vector& T_world, - const std::vector& jointWorldPoses - ) const { - const size_t n = robot.joints.size(); - mathlib::VecX tau_G = VecX::Zero(n); - double g{ _gravity }; // [m/s^2], gravity acceleration magnitude - - // For each joint, sum the gravity contributions from all links - for (size_t i = 0; i < n; ++i) { - const RobotJoint& j = robot.joints[i]; - if (j.type == eJointType::FIXED) { continue; } - - double tau_g_i = 0.0; // [Nm], gravity torque contribution for joint i - - const Pose& T_joint = jointWorldPoses[i]; // pose of joint i in world frame - - const Mat3 R_i = T_joint.block<3, 3>(0, 0); - const Vec3 p_i = T_joint.block<3, 1>(0, 3); - const Vec3 axis_world = (R_i * robot.joints[i].axis).normalized(); - - // For each link, compute the gravitational force and its torque contribution about joint i - for (size_t k = 0; k < robot.links.size(); ++k) { - const RobotLink& link = robot.links[k]; - const double m = link.inertial.mass; - if (m <= 0.0) { continue; } - - if (!robot.jointAffectsLink(i, k)) { continue; } - - // Link's center of mass in world frame - const Mat3 R_k = T_world[k].block<3, 3>(0, 0); - const Vec3 com_world = R_k * link.inertial.com_xyz + T_world[k].block<3, 1>(0, 3); - - // Gravitational force on the link - Vec3 g_world = Vec3(0.0, 0.0, -g); // [m/s^2], gravity vector in world frame - const Vec3 F_g = m * g_world; // [N], gravitational force on the link in world frame - - // Torque contribution from this link's weight about joint i - const Vec3 r = com_world - p_i; // [m] - - // Torque = r × F_g projected onto joint axis - tau_g_i += axis_world.dot(r.cross(F_g)); - } - tau_G[i] = tau_g_i; - } - return tau_G; // [Nm], gravity torques for each joint - } - - void RobotDynamics::analyticalJacobian( - const RobotConstModel& robot, - const mathlib::VecX& x, - mathlib::MatX& J_out, - DenseDynamicsScratch& scratch - ) { - const size_t n = robot.joints.size(); - - Eigen::Map q_local(x.data(), n); - Eigen::Map qd_local(x.data() + n, n); - - _kinematics->computeForwardKinematics_fromState(robot, x, scratch.T_world); - scratch.jointWorldPoses = _kinematics->calcJointWorldPoses(scratch.T_world, robot); - computeMassMatrix(robot, scratch.T_world, scratch.jointWorldPoses, scratch.M); - - J_out.setZero(2 * n, 2 * n); // [rad/rad] position part, [rad/s / rad/s] velocity part - J_out.block(0, n, n, n).setIdentity(); - - MatX dTau_dq = MatX::Zero(n, n); - MatX dTau_dv = MatX::Zero(n, n); - - for (size_t i = 0; i < n; ++i) { - const RobotJoint& joint = robot.joints[i]; - if (joint.type == eJointType::FIXED) { continue; } - - const double wn = joint.wn_target; - const double z = joint.zeta_target; - const double I_eff = std::max(scratch.M(i, i), 1e-6); - - const double k_p = I_eff * wn * wn; - const double k_d = 2.0 * z * I_eff * wn; - - dTau_dq(i, i) = -k_p; - - double qd_i = qd_local[i]; - double tanh_term = std::tanh(qd_i / 1e-2); - double stiff_friction_slope = -0.05 * (1.0 - tanh_term * tanh_term) / 1e-2; - dTau_dv(i, i) = -k_d - 0.2 + stiff_friction_slope; - } - - auto solver = scratch.M.ldlt(); - MatX da_dq = solver.solve(dTau_dq); - MatX da_dv = solver.solve(dTau_dv); - - J_out.block(n, 0, n, n) = da_dq; - J_out.block(n, n, n, n) = da_dv; - } - - // Computes the derivative of the state vector (q, qd) based on the current state and robot configurations - mathlib::VecX RobotDynamics::derivative_dense( - double /*t*/, - const mathlib::VecX& x, - const RobotSimSnapshot& snap, - DynamicsScratch& scratch, - DynamicsResult& out - ) { - const size_t n = snap.model->joints.size(); - mathlib::VecX dx(2 * n); - - // Map the input state vector to joint angles and velocities - Eigen::Map q(x.data(), n); - Eigen::Map qd(x.data() + n, n); - - _kinematics->computeForwardKinematics_fromState(*snap.model, x, scratch.dense.T_world); - - scratch.dense.jointWorldPoses = _kinematics->calcJointWorldPoses(scratch.dense.T_world, *snap.model); - - computeMassMatrix(*snap.model, scratch.dense.T_world, scratch.dense.jointWorldPoses, scratch.dense.M); // [kg*m^2], full mass matrix for the robot at configuration q - scratch.dense.h = computeCoriolisVector(*snap.model, q, qd, scratch.dense.T_world, scratch.dense.M); // [Nm], full Coriolis and centrifugal torque vector - - scratch.g.setZero(); - if (snap.torqueMode != eTorqueMode::NONE) { - scratch.g = computeGravityTorque(*snap.model, scratch.dense.T_world, scratch.dense.jointWorldPoses); - } - - scratch.dense.tau.setZero(); - for (size_t i = 0; i < n; ++i) { - const RobotJoint& joint = snap.model->joints[i]; - - // Fixed joints - if (joint.type == eJointType::FIXED) { - out.metrics.q[i] = q[i]; - out.metrics.qd[i] = qd[i]; - out.metrics.qdd[i] = 0.0; - continue; - } - - const double wn = joint.wn_target; // [rad/s], natural frequency - const double z = joint.zeta_target; // damping ratio - - const double err = snap.q_ref[i] - q[i]; // [rad], position error - const double err_d = snap.qd_ref[i] - qd[i]; // [rad/s], velocity error - - const double I_eff = std::max(scratch.dense.M(i, i), 1e-6); // [kg*m^2], effective inertia for joint i with floor to prevent singularities - const double k_p = I_eff * wn * wn; // [Nm/rad], proportional gain - const double k_d = 2.0 * z * I_eff * wn; // [Nm/(rad/s)], derivative gain - - double tau_i = k_p * err + k_d * err_d + I_eff * snap.qdd_ref[i]; // [Nm], control torque for joint i - tau_i += scratch.g[i]; // Gravity compensation - tau_i += scratch.dense.h[i]; // add Coriolis and centrifugal bias - tau_i -= /*joint.dynamics.damping*/ 0.2 * qd[i]; // subtract viscous damping - tau_i -= /*joint.dynamics.friction*/ 0.05 * std::tanh(qd[i] / 1e-2); // subtract Coulomb friction - - scratch.dense.tau[i] = tau_i; - - // metrics - out.metrics.q[i] = q[i]; - out.metrics.qd[i] = qd[i]; - - out.metrics.err[i] = err; - out.metrics.errd[i] = err_d; - - out.metrics.I_eff[i] = I_eff; - out.metrics.tau[i] = tau_i; - - //_metrics.tau_sat[i] = tau_sat; - //_metrics.sat_flag[i] = saturated; - } - - // Solve Forward Dynamics: M(q) qdd = tau - h(q, qd) - g(q) - scratch.dense.rhs.noalias() = scratch.dense.tau - scratch.dense.h - scratch.g; // [Nm], right-hand side of the dynamics equation M*qdd = tau - h - g - - // Solve for Accelerations - out.qdd = scratch.dense.M.ldlt().solve(scratch.dense.rhs); // [rad/s^2], joint accelerations computed from dynamics - out.metrics.qdd = out.qdd; - - // Fill derivatives - dx.head(n) = qd; - dx.tail(n) = out.qdd; - - return dx; - } - - mathlib::VecX RobotDynamics::derivative_spatial( - const robots::SpatialModel& model, - double /*t*/, - const mathlib::VecX& x, - const RobotSimSnapshot& snap, - DynamicsScratch& scratch, - DynamicsResult& out - ) { - const size_t n = snap.model->joints.size(); - VecX dx(2 * n); - - Eigen::Map q(x.data(), n); - Eigen::Map qd(x.data() + n, n); - - SpatialDynamics::computeSpatialKinematicsAndBias( - model, q, qd, - scratch.spatial.Xup, - scratch.spatial.v, - scratch.spatial.c - ); - - MatX M = SpatialDynamics::CRBA(model, scratch.spatial.Xup, scratch); - // RNEA to compute gravity compensation (q, 0, 0) for gravity, (q, qd, 0) for Coriolis - VecX tau_g = SpatialDynamics::RNEA(model, q, VecX::Zero(n), VecX::Zero(n), scratch); - - scratch.dense.tau.setZero(); - for (size_t i = 0; i < n; ++i) { - const SpatialJoint& joint = model.joints[i]; - if (!isControlledJoint(joint.type)) { - scratch.dense.tau[i] = 0.0; - continue; - } - - const double wn = 5.0; - const double z = 0.7; - - const double err = snap.q_ref[i] - q[i]; - const double err_d = snap.qd_ref[i] - qd[i]; - - const double I_eff = std::max(M(i, i), 1e-6); - const double k_p = I_eff * wn * wn; - const double k_d = 2.0 * z * I_eff * wn; - - double tau_i = k_p * err + k_d * err_d + I_eff * snap.qdd_ref[i]; - tau_i -= 0.2 * qd[i]; - //tau_i -= 0.05 * std::tanh(qd[i] / 1e-2); - - scratch.dense.tau[i] = tau_i; - - out.metrics.q[i] = q[i]; - out.metrics.qd[i] = qd[i]; - - out.metrics.err[i] = err; - out.metrics.errd[i] = err_d; - - out.metrics.I_eff[i] = I_eff; - out.metrics.tau[i] = tau_i; - } - - out.qdd = SpatialDynamics::ABA(model, q, qd, scratch.dense.tau, scratch); - out.metrics.qdd = out.qdd; - dx.head(n) = qd; - dx.tail(n) = out.qdd; - - return dx; - } - - void RobotDynamics::jacobian_spatial( - const robots::SpatialModel& model, - const mathlib::VecX& x, - const RobotSimSnapshot& snap, - const mathlib::VecX& kp, - const mathlib::VecX& kd, - mathlib::MatX& F_out, - DynamicsScratch& scratch - ) { - const size_t n = snap.model->joints.size(); - - F_out.setZero(2 * n, 2 * n); - F_out.block(0, n, n, n).setIdentity(); - - Eigen::Map q(x.data(), n); - Eigen::Map qd(x.data() + n, n); - - SpatialDynamics::computeSpatialKinematicsAndBias( - model, q, qd, - scratch.spatial.Xup, - scratch.spatial.v, - scratch.spatial.c - ); - - scratch.dense.M = SpatialDynamics::CRBA(model, scratch.spatial.Xup, scratch); - - MatX dTau_dq = MatX::Zero(n, n); - MatX dTau_dv = MatX::Zero(n, n); - - for (size_t i = 0; i < n; ++i) { - const SpatialJoint& joint = model.joints[i]; - if (!isControlledJoint(joint.type)) { continue; } - dTau_dq(i, i) = -kp[i]; - const double eps_fric = 1e-2; - double tanh_term = std::tanh(qd[i] / eps_fric); - double stiff_friction_slope = -0.05 * (1.0 - tanh_term * tanh_term) / eps_fric; - dTau_dv(i, i) = -kd[i] - 0.2; - } - - Eigen::LDLT solver(scratch.dense.M); // compute the Cholesky decomposition of the mass matrix for efficient solving - MatX dqdd_dtau_q = solver.solve(dTau_dq); // compute the partial derivative of qdd with respect to q - MatX dqdd_dtau_v = solver.solve(dTau_dv); // compute the partial derivative of qdd with respect to qd - - F_out.block(n, 0, n, n) = dqdd_dtau_q; // fill the Jacobian block for qdd with respect to q - F_out.block(n, n, n, n) = dqdd_dtau_v; // fill the Jacobian block for qdd with respect to qd) - } - - mathlib::VecX RobotDynamics::derivative_with_gains( - double t, const mathlib::VecX& x, const RobotSimSnapshot& snap, - const mathlib::VecX& kp, const mathlib::VecX& kd, - DynamicsScratch& scratch, - DynamicsResult& out - ) { - const size_t n = snap.model->joints.size(); - mathlib::VecX dx(2 * n); - - Eigen::Map q(x.data(), n); - Eigen::Map qd(x.data() + n, n); - - _kinematics->computeForwardKinematics_fromState(*snap.model, x, scratch.dense.T_world); - scratch.dense.jointWorldPoses = _kinematics->calcJointWorldPoses(scratch.dense.T_world, *snap.model); - - computeMassMatrix(*snap.model, scratch.dense.T_world, scratch.dense.jointWorldPoses, scratch.dense.M); - scratch.dense.h = computeCoriolisVector(*snap.model, q, qd, scratch.dense.T_world, scratch.dense.M); - - scratch.g.setZero(); - if (snap.torqueMode != eTorqueMode::NONE) { - scratch.g = computeGravityTorque(*snap.model, scratch.dense.T_world, scratch.dense.jointWorldPoses); - } - - scratch.dense.tau.setZero(); - for (size_t i = 0; i < n; ++i) { - if (snap.model->joints[i].type == eJointType::FIXED) continue; - - // FIXED: Use the constant, frozen step-start values passed down - double tau_i = kp[i] * (snap.q_ref[i] - q[i]) + kd[i] * (snap.qd_ref[i] - qd[i]) + std::max(scratch.dense.M(i, i), 1e-6) * snap.qdd_ref[i]; - tau_i += scratch.g[i] + scratch.dense.h[i]; - tau_i -= 0.2 * qd[i]; - tau_i -= 0.05 * std::tanh(qd[i] / 1e-2); - - scratch.dense.tau[i] = tau_i; - } - - scratch.dense.rhs.noalias() = scratch.dense.tau - scratch.dense.h - scratch.g; - out.qdd = scratch.dense.M.ldlt().solve(scratch.dense.rhs); - out.metrics.qdd = out.qdd; - - dx.head(n) = qd; - dx.tail(n) = out.qdd; - return dx; - } - - void RobotDynamics::jacobian_with_gains( - const mathlib::VecX& x, const RobotSimSnapshot& snap, - const mathlib::VecX& kp, const mathlib::VecX& kd, mathlib::MatX& F_out, - DenseDynamicsScratch& scratch - ) { - const size_t n = snap.model->joints.size(); - - F_out.setZero(2 * n, 2 * n); - F_out.block(0, n, n, n).setIdentity(); - - Eigen::Map q(x.data(), n); - Eigen::Map qd(x.data() + n, n); - - // Compute a local mass matrix for this exact stage evaluation frame - _kinematics->computeForwardKinematics_fromState(*snap.model, x, scratch.T_world); - scratch.jointWorldPoses = _kinematics->calcJointWorldPoses(scratch.T_world, *snap.model); - computeMassMatrix(*snap.model, scratch.T_world, scratch.jointWorldPoses, scratch.M); - - mathlib::MatX dTau_dq = mathlib::MatX::Zero(n, n); - mathlib::MatX dTau_dv = mathlib::MatX::Zero(n, n); - - for (size_t i = 0; i < n; ++i) { - if (snap.model->joints[i].type == eJointType::FIXED) continue; - - dTau_dq(i, i) = -kp[i]; - - double tanh_term = std::tanh(qd[i] / 1e-2); - double stiff_friction = -0.05 * (1.0 - tanh_term * tanh_term) / 1e-2; - dTau_dv(i, i) = -kd[i] - 0.2 + stiff_friction; - } - - auto solver = scratch.M.ldlt(); - F_out.block(n, 0, n, n) = solver.solve(dTau_dq); - F_out.block(n, n, n, n) = solver.solve(dTau_dv); - } } \ No newline at end of file diff --git a/DSFE_App/DSFE_Core/src/Robots/RobotKinematics.cpp b/DSFE_App/DSFE_Core/src/Robots/RobotKinematics.cpp index b4e64f0e..9df24909 100644 --- a/DSFE_App/DSFE_Core/src/Robots/RobotKinematics.cpp +++ b/DSFE_App/DSFE_Core/src/Robots/RobotKinematics.cpp @@ -3,10 +3,6 @@ // GitHub: SaltyJoss #include "Robots/RobotKinematics.h" #include "Robots/RobotSimSnapshot.h" -#include -#include - -#include "EngineLib/LogMacros.h" using namespace mathlib; using namespace constants; @@ -16,91 +12,6 @@ namespace robots { RobotKinematics::RobotKinematics() { } - // Computes the forward kinematics for the robot based on the current state and robot configuration - void RobotKinematics::computeForwardKinematics_fromState( - const RobotConstModel& robot, - const mathlib::VecX& x, - std::vector& T_world_out - ) const { - const auto& joints = robot.joints; - const auto& links = robot.links; - const size_t n = (size_t)joints.size(); - - T_world_out.resize(robot.links.size()); - - Pose T = Pose::Identity(); // world -> base - T_world_out[0] = T; // base link - - if (T_world_out.empty()) { - LOG_ERROR("T_world_out is empty"); - return; - } - - // Compute the transform to the next link using each joint - for (size_t i = 0; i < n; ++i) { - const auto& joint = joints[i]; - const double q = x[i]; // joint angle from state vector - - Pose T_origin = Pose::Identity(); // transform from parent link to joint frame (fixed) - T_origin.block<3, 3>(0, 0) = joint.origin_q.toRotationMatrix(); // rotation from parent link frame to joint frame, derived from rpy in JSON - T_origin.block<3, 1>(0, 3) = joint.origin_xyz; // translation from parent link to joint frame - - // Compute joint motion transform based on joint axis and angle - Pose T_motion = Pose::Identity(); - if (joint.type == eJointType::REVOLUTE) { - T_motion = jointMotionTransform(joint.axis, q); // rotation about joint axis - } - else if (joint.type == eJointType::PRISMATIC) { - T_motion.block<3, 1>(0, 3) = joint.axis.normalized() * q; // translation along joint axis - } - - // compose transforms - T = T * T_origin * T_motion; // parent -> joint -> motion -> child - - int childIdx = robot.linkIndex(joint.child); - if (childIdx < 0 || childIdx >= T_world_out.size()) { - LOG_ERROR("Invalid child link index for joint {}: {}", joint.name.c_str(), childIdx); - continue; - } - - T_world_out[childIdx] = T; // world -> child link - } - } - - // Computes the joint world poses for all joints based on the current state and robot configuration - std::vector RobotKinematics::calcJointWorldPoses( - const std::vector& T_world, - const RobotConstModel& robot - ) { - std::vector jointWorldPoses(robot.joints.size()); - - for (size_t i = 0; i < robot.joints.size(); ++i) { - const RobotJoint& joints = robot.joints[i]; - int childIdx = robot.linkIndex(joints.child); - - if (childIdx < 0 || childIdx >= T_world.size()) { - LOG_ERROR("Invalid child link index for joint {}: {}", joints.name.c_str(), childIdx); - continue; - } - - jointWorldPoses[i] = T_world[childIdx]; - } - return jointWorldPoses; - } - - // Computes the forward kinematics for a single joint motion based on the joint axis and angle - mathlib::Pose RobotKinematics::jointMotionTransform( - const mathlib::Vec3& axis_joint, - double q - ) const { - Pose T = Pose::Identity(); // homogeneous transformation matrix (4x4) - - Eigen::AngleAxisd aa(q, axis_joint.normalized()); // create angle-axis rotation from joint angle and axis - T.block<3, 3>(0, 0) = aa.toRotationMatrix(); // set upper-left 3x3 block to rotation matrix - - return T; // (4x4) homogeneous transformation - } - // Converts roll-pitch-yaw angles (in radians) to a quaternion representation mathlib::Quat RobotKinematics::rpyRadToQuat(const mathlib::Vec3& rpyRad) { const double roll = rpyRad.x(); diff --git a/PxMLib/MathLib/include/core/Types_tpl.h b/PxMLib/MathLib/include/core/Types_tpl.h index 4d3e8427..0633d27c 100644 --- a/PxMLib/MathLib/include/core/Types_tpl.h +++ b/PxMLib/MathLib/include/core/Types_tpl.h @@ -13,7 +13,7 @@ namespace mathlib { template using Mat3_T = Eigen::Matrix; - // Template version of 6D vector and 6x6 matrix for spatial algebra + // Template version of 6D vector for spatial algebra template using Vec6_T = Eigen::Matrix; @@ -28,4 +28,8 @@ namespace mathlib { // Template version of dynamic-size matrix template using MatX_T = Eigen::Matrix; + + // Template version of pose (4x4 homogeneous transformation matrix) + template + using Pose_T = Eigen::Matrix; } \ No newline at end of file From f0085f07a672501566817bc030d4cae426c68d32 Mon Sep 17 00:00:00 2001 From: saltyjoss Date: Sat, 16 May 2026 17:39:05 +0100 Subject: [PATCH 007/210] refactor: Rewrote ODE integrators class into a generic `NumericalIntegrator` class that contains Scalar typenames * Fixed smaller logic bugs in implicit euler and midpoint --- .../include/Numerics/IntegrationService.h | 32 +- DSFE_App/DSFE_Core/src/Robots/RobotSystem.cpp | 4 +- .../integrators/numerical_integrators.h | 686 +++--------------- .../integrators/numerical_integrators.inl | 626 ++++++++++++++++ 4 files changed, 746 insertions(+), 602 deletions(-) create mode 100644 PxMLib/MathLib/include/integrators/numerical_integrators.inl diff --git a/DSFE_App/DSFE_Core/include/Numerics/IntegrationService.h b/DSFE_App/DSFE_Core/include/Numerics/IntegrationService.h index a453c920..0173f9b1 100644 --- a/DSFE_App/DSFE_Core/include/Numerics/IntegrationService.h +++ b/DSFE_App/DSFE_Core/include/Numerics/IntegrationService.h @@ -28,37 +28,37 @@ namespace integration { IntegrationService(); template - StepOut stepODE(eIntegrationMethod m, VecX& x, double t, double dt, Func&& f, JacFunc&& jac) { + StepOut step(eIntegrationMethod m, VecX& x, double t, double dt, Func&& f, JacFunc&& jac) { if constexpr (std::is_pointer_v> || requires { f == nullptr; }) { if (f == nullptr) { D_WARN_ONCE("No derivative function provided for integration - Assuming constant derivative (Euler step)"); - return { _ODE->eulerStep(x, t, dt, std::forward(f)), dt, dt }; + return { _integrator->eulerStep(x, t, dt, std::forward(f)), dt, dt }; } } switch (m) { // Explicit methods // * currently all explicit methods use fixed step size, apart from RK45 as it is an adaptive method - case eIntegrationMethod::Euler: return { _ODE->eulerStep(x, t, dt, std::forward(f)), dt, dt }; - case eIntegrationMethod::Midpoint: return { _ODE->midpointStep(x, t, dt, std::forward(f)), dt, dt }; - case eIntegrationMethod::Heun: return { _ODE->heunStep(x, t, dt, std::forward(f)), dt, dt }; - case eIntegrationMethod::Ralston: return { _ODE->ralstonStep(x, t, dt, std::forward(f)), dt, dt }; - case eIntegrationMethod::RK4: return { _ODE->rk4Step(x, t, dt, std::forward(f)), dt, dt }; - case eIntegrationMethod::RK45: return stepAdaptiveODE(eIntegrationMethod::RK45, x, t, dt, std::forward(f), _rtol, _atol); + case eIntegrationMethod::Euler: return { _integrator->eulerStep(x, t, dt, std::forward(f)), dt, dt }; + case eIntegrationMethod::Midpoint: return { _integrator->midpointStep(x, t, dt, std::forward(f)), dt, dt }; + case eIntegrationMethod::Heun: return { _integrator->heunStep(x, t, dt, std::forward(f)), dt, dt }; + case eIntegrationMethod::Ralston: return { _integrator->ralstonStep(x, t, dt, std::forward(f)), dt, dt }; + case eIntegrationMethod::RK4: return { _integrator->rk4Step(x, t, dt, std::forward(f)), dt, dt }; + case eIntegrationMethod::RK45: return step_adaptive(eIntegrationMethod::RK45, x, t, dt, std::forward(f), _rtol, _atol); // Implicit methods // * currently use fixed step size (no error estimation), but are likely to support adaptive stepping in the future - case eIntegrationMethod::ImplicitEuler: return { _ODE->implicit_euler(x, t, dt, std::forward(f)), dt, dt }; - case eIntegrationMethod::ImplicitMidpoint: return { _ODE->implicit_midpoint(x, t, dt, std::forward(f)), dt, dt }; - case eIntegrationMethod::GLRK2: return { _ODE->GLRK2(x, t, dt, std::forward(f), 50, 1e-10, std::forward(jac)), dt, dt }; - case eIntegrationMethod::GLRK3: return { _ODE->GLRK3(x, t, dt, std::forward(f), 80, -1, std::forward(jac)), dt, dt }; + case eIntegrationMethod::ImplicitEuler: return { _integrator->implicit_euler(x, t, dt, std::forward(f)), dt, dt }; + case eIntegrationMethod::ImplicitMidpoint: return { _integrator->implicit_midpoint(x, t, dt, std::forward(f)), dt, dt }; + case eIntegrationMethod::GLRK2: return { _integrator->GLRK2(x, t, dt, std::forward(f), 50, 1e-10, std::forward(jac)), dt, dt }; + case eIntegrationMethod::GLRK3: return { _integrator->GLRK3(x, t, dt, std::forward(f), 80, -1, std::forward(jac)), dt, dt }; default: LOG_WARN("Unknown integration method: %s. Defaulting to RK4.", toString(m)); - return { _ODE->rk4Step(x, t, dt, std::forward(f)), dt, dt }; + return { _integrator->rk4Step(x, t, dt, std::forward(f)), dt, dt }; } } template - StepOut stepAdaptiveODE(eIntegrationMethod m, VecX& x, double t, double dt_try, Func&& f, double rtol, double atol) { + StepOut step_adaptive(eIntegrationMethod m, VecX& x, double t, double dt_try, Func&& f, double rtol, double atol) { if constexpr (std::is_pointer_v> || requires { f == nullptr; }) { if (f == nullptr) { LOG_WARN("No derivative function provided for adaptive integration - returning state unchanged"); @@ -96,7 +96,7 @@ namespace integration { double h_try = std::min(h, t_end - t_curr); double dt_used = 0.0; - VecX x_next = _ODE->rk45Step(x_curr, t_curr, h_try, dt_used, std::forward(f), rtol, atol); + VecX x_next = _integrator->rk45Step(x_curr, t_curr, h_try, dt_used, std::forward(f), rtol, atol); // Update rk45step t_curr += dt_used; @@ -132,7 +132,7 @@ namespace integration { const char* toString(eIntegrationMethod m); integration::eIntegrationMethod method; - std::unique_ptr _ODE; + std::unique_ptr _integrator; std::string _methodStr = "RK4"; diff --git a/DSFE_App/DSFE_Core/src/Robots/RobotSystem.cpp b/DSFE_App/DSFE_Core/src/Robots/RobotSystem.cpp index 42e43597..81b119bf 100644 --- a/DSFE_App/DSFE_Core/src/Robots/RobotSystem.cpp +++ b/DSFE_App/DSFE_Core/src/Robots/RobotSystem.cpp @@ -92,7 +92,7 @@ namespace robots { for (size_t i = 0; i < n; ++i) { const RobotJoint& j = _robot.joints[i]; - SpatialJoint& sj = _spatialModel.joints[i]; + auto& sj = _spatialModel.joints[i]; sj.name = j.name; sj.type = j.type; @@ -359,7 +359,7 @@ namespace robots { ); }; - auto step = _integrator->stepODE(_curIntMethod, x, simTime, dt, f_deriv, f_J); + auto step = _integrator->step(_curIntMethod, x, simTime, dt, f_deriv, f_J); unpackState(step.x_next); _dynamics->setDt(step.dt_taken); diff --git a/PxMLib/MathLib/include/integrators/numerical_integrators.h b/PxMLib/MathLib/include/integrators/numerical_integrators.h index 72e695cf..4fd1ad3b 100644 --- a/PxMLib/MathLib/include/integrators/numerical_integrators.h +++ b/PxMLib/MathLib/include/integrators/numerical_integrators.h @@ -4,622 +4,140 @@ #include "MathLibAPI.h" #include "core/Types.h" +#include "core/Types_tpl.h" #include #include #include -using namespace mathlib; - // Numerical integration methods namespace integration { // Ordinary Differential Equation (ODE) solvers - class MATHLIB_API ODE { + class MATHLIB_API NumericalIntegrator { public: // Euler method - template - inline VecX eulerStep(const VecX& x, double t, double dt, Func&& f) { - return x + dt * f(t, x); - } + template + mathlib::VecX_T eulerStep( + const mathlib::VecX_T& x, + Scalar t, + Scalar dt, + Func&& f + ); // Second-order Runge-Kutta method (Midpoint) - template - inline VecX midpointStep(const VecX& x, double t, double dt, Func&& f) { - VecX k1 = dt * f(t, x); - VecX k2 = dt * f(t + dt / 2.0, x + k1 / 2.0); - return x + k2; - } + template + mathlib::VecX_T midpointStep( + const mathlib::VecX_T& x, + Scalar t, + Scalar dt, + Func&& f + ); // Second-order Runge-Kutta method (Heun) - template - inline VecX heunStep(const VecX& x, double t, double dt, Func&& f) { - VecX k1 = dt * f(t, x); - VecX k2 = dt * f(t + dt, x + k1); - return x + (k1 + k2) / 2.0; - } + template + mathlib::VecX_T heunStep( + const mathlib::VecX_T& x, + Scalar t, + Scalar dt, + Func&& f + ); // Second-order Runge-Kutta method (Ralston) - template - inline VecX ralstonStep(const VecX& x, double t, double dt, Func&& f) { - VecX k1 = dt * f(t, x); - VecX k2 = dt * f(t + (2.0 / 3.0) * dt, x + (2.0 / 3.0) * k1); - return x + (k1 + 3.0 * k2) / 4.0; - } + template + mathlib::VecX_T ralstonStep( + const mathlib::VecX_T& x, + Scalar t, + Scalar dt, + Func&& f); // Fourth-order Runge-Kutta method - template - inline VecX rk4Step(const VecX& x, double t, double dt, Func&& f) { - VecX k1 = dt * f(t, x); - VecX k2 = dt * f(t + dt / 2.0, x + k1 / 2.0); - VecX k3 = dt * f(t + dt / 2.0, x + k2 / 2.0); - VecX k4 = dt * f(t + dt, x + k3); - return x + (k1 + 2.0 * k2 + 2.0 * k3 + k4) / 6.0; - } + template + mathlib::VecX_T rk4Step( + const mathlib::VecX_T& x, + Scalar t, + Scalar dt, + Func&& f + ); // RK45 method with adaptive step size (Dormand-Prince) - template - inline VecX rk45Step(const VecX& x, double t, double& dt, double& dt_used, Func&& f, double rtol, double atol) { - const double safety = 0.9; // safety factor to prevent aggressive step size changes - const double fac_min = 0.2; // minimum factor for reducing step size - const double fac_max = 5.0; // maximum factor for increasing step size - const double h_min = 1e-10; // minimum allowed step size - const double h_max = 1.0; // maximum allowed step size - - dt = std::clamp(dt, h_min, h_max); - - // Try up to 25 attempts to find an acceptable step size - for (int attempt = 0; attempt < 25; ++attempt) { - // Butcher tableau for Dormand-Prince method - - // Coefficients for error estimation - const double c2 = 1.0 / 5.0; - const double c3 = 3.0 / 10.0; - const double c4 = 4.0 / 5.0; - const double c5 = 8.0 / 9.0; - const double c6 = 1.0; - const double c7 = 1.0; - - // Dormand-Prince coefficients - const double a21 = 1.0 / 5.0; - const double a31 = 3.0 / 40.0; - const double a32 = 9.0 / 40.0; - const double a41 = 44.0 / 45.0; - const double a42 = -56.0 / 15.0; - const double a43 = 32.0 / 9.0; - const double a51 = 19372.0 / 6561.0; - const double a52 = -25360.0 / 2187.0; - const double a53 = 64448.0 / 6561.0; - const double a54 = -212.0 / 729.0; - const double a61 = 9017.0 / 3168.0; - const double a62 = -355.0 / 33.0; - const double a63 = 46732.0 / 5247.0; - const double a64 = 49.0 / 176.0; - const double a65 = -5103.0 / 18656.0; - const double a71 = 35.0 / 384.0; - const double a72 = 0.0; - const double a73 = 500.0 / 1113.0; - const double a74 = 125.0 / 192.0; - const double a75 = -2187.0 / 6784.0; - const double a76 = 11.0 / 84.0; - - // Weights for 4th and 5th order estimates - const double b1 = 35.0 / 384.0; - const double b2 = 0.0; - const double b3 = 500.0 / 1113.0; - const double b4 = 125.0 / 192.0; - const double b5 = -2187.0 / 6784.0; - const double b6 = 11.0 / 84.0; - const double b1s = 5179.0 / 57600.0; - const double b2s = 0.0; - const double b3s = 7571.0 / 16695.0; - const double b4s = 393.0 / 640.0; - const double b5s = -92097.0 / 339200.0; - const double b6s = 187.0 / 2100.0; - const double b7s = 1.0 / 40.0; - - // Compute the Runge-Kutta stages (DP -> 7 stages) - const VecX k1 = f(t, x); - const VecX k2 = f(t + c2 * dt, x + dt * (a21 * k1)); - const VecX k3 = f(t + c3 * dt, x + dt * (a31 * k1 + a32 * k2)); - const VecX k4 = f(t + c4 * dt, x + dt * (a41 * k1 + a42 * k2 + a43 * k3)); - const VecX k5 = f(t + c5 * dt, x + dt * (a51 * k1 + a52 * k2 + a53 * k3 + a54 * k4)); - const VecX k6 = f(t + c6 * dt, x + dt * (a61 * k1 + a62 * k2 + a63 * k3 + a64 * k4 + a65 * k5)); - const VecX k7 = f(t + c7 * dt, x + dt * (a71 * k1 + a72 * k2 + a73 * k3 + a74 * k4 + a75 * k5 + a76 * k6)); - - // Compute 4th and 5th order estimates - const VecX y5 = x + dt * ((b1 * k1) + (b3 * k3) + (b4 * k4) + (b5 * k5) + (b6 * k6)); // 5th order estimate - const VecX y4 = x + dt * ((b1s * k1) + (b3s * k3) + (b4s * k4) + (b5s * k5) + (b6s * k6) + (b7s * k7)); // 4th order estimate - - const VecX e = y5 - y4; // Error estimate - - // Compute the error norm - double errNorm = 0.0; - for (int i = 0; i < e.size(); ++i) { - double sc = atol + rtol * std::max(std::abs(x(i)), std::abs(y5(i))); - const double r = e(i) / sc; - errNorm += r * r; - } - double err = std::sqrt(errNorm / e.size()); - - // Adaptive step size control - - // Accept - if (err <= 1.0 && std::isfinite(err)) { - dt_used = dt; // Store the actual step size used for this step - - // Update step size for next iteration - const double denom = std::max(err, 1e-10); // prevent division by zero - double fac = safety * std::pow(denom, -0.2); // exponent for 5th order method - fac = std::clamp(fac, fac_min, fac_max); // limit step size change - dt = std::clamp(dt * fac, h_min, h_max); // update step size - return y5; - } - // Reject - else { - double denom = (std::isfinite(err) - ? std::max(err, 1e-16) : 1e16); // prevent division by zero & NaN - double fac = safety * std::pow(denom, -0.2); // exponent for 4th order method - fac = std::clamp(fac, fac_min, fac_max); - dt = std::clamp(dt * fac, h_min, h_max); - } - } - - // If it reaches here, it failed to converge after many attempts - // Just putting this here as a policy choice honestly, return the best effort or throw - throw std::runtime_error("RK45 failed to converge after maximum attempts"); - } + template + mathlib::VecX_T rk45Step( + const mathlib::VecX_T& x, + Scalar t, + Scalar& dt, + Scalar& dt_used, + Func&& f, + Scalar rtol, + Scalar atol + ); // Implicit Euler method - template - inline VecX implicit_euler(const VecX& x, double t, double dt, Func&& f, int maxIter = 8, double tol = 1e-6) { - VecX x_new = x + dt * f(t, x); // Initial guess - - // The function g(x_guess) = 0 that we want to solve for the implicit Euler step - auto g = [&](const VecX& x_guess) { return x_guess - x - dt * f(t + dt, x_guess); }; - - // Numerical Jacobian for Newton-Raphson - auto J = [&](const VecX& x_guess) -> MatX { - const double eps_rel = std::sqrt(std::numeric_limits::epsilon()); - VecX f_0 = f(t + dt, x_guess); - - int n = (int)x_guess.size(); - MatX J_full = MatX::Zero(n, n); - - for (int i = 0; i < n; ++i) { - VecX x_pert = x_guess; - double h = eps_rel * std::max(1.0, std::abs(x_guess(i))); - x_pert(i) += h; - VecX f_i = f(t + dt, x_pert); - J_full.col(i) = (f_i - f_0) / h; // Finite difference approximationof df/dx column i - } - - return MatX::Identity(n, n) - dt * J_full; // J = I - dt * df/dx - }; - - // Simple fixed-point iteration to solve the implicit equation: x_new = x + dt * f(t + dt, x_new) - for (int iter = 0; iter < maxIter; ++iter) { - VecX g = x_new - x - dt * f(t + dt, x_new); // Residual - - // Check for convergence - if (g.norm() < tol) { - return x_new; - } - - // Solve J * delta = -g for the Newton step - MatX A = J(x_new); - Eigen::FullPivLU lu(A); - - // Check if the Jacobian is invertible - if (!lu.isInvertible()) { - throw std::runtime_error("Jacobian is singular during Implicit Midpoint iteration " + std::to_string(iter + 1)); - } - - // Update the guess - VecX delta = lu.solve(-g); - x_new += delta; - } - throw std::runtime_error( - std::string("Implicit Euler failed to converge after ") + - std::to_string(maxIter) + - " iterations, final residual norm: " + - std::to_string((x_new - x - dt * f(t + dt, x_new)).norm()) - ); - } + template + mathlib::VecX_T implicitEuler( + const mathlib::VecX_T& x, + Scalar t, + Scalar dt, + Func&& f, + JacFunc&& jac = nullptr, + int maxIter = 8, + Scalar tol = 1e-6 + ); // Implicit Midpoint method - template - inline VecX implicit_midpoint(const VecX& x, double t, double dt, Func&& f, int maxIter = 10, double tol = 1e-7) { - VecX x_new = x + dt * f((t + dt) / 2.0, x); // Initial guess - - // The function g(x_guess) = 0 that we want to solve for the implicit midpoint step - auto g = [&](const VecX& x_guess) { return x_guess - x - dt * f((t + dt) / 2.0, (x + x_guess) / 2.0); }; - - // Numerical Jacobian for Newton-Raphson - auto J = [&](const VecX& x_guess) -> MatX { - const double eps_rel = std::sqrt(std::numeric_limits::epsilon()); - VecX f_0 = f(t + dt / 2.0, (x + x_guess) / 2.0); - - int n = (int)x_guess.size(); - MatX J_full = MatX::Zero(n, n); - - for (int i = 0; i < n; ++i) { - VecX x_pert = x_guess; - double h = eps_rel * std::max(1.0, std::abs(x_guess(i))); - x_pert(i) += h; - VecX f_i = f((t + dt) / 2.0, (x + x_pert) / 2.0); - J_full.col(i) = (f_i - f_0) / h; // Finite difference approximationof df/dx column i - } - - return MatX::Identity(n, n) - dt * J_full; // J = I - dt * df/dx - }; - - // Simple fixed-point iteration to solve the implicit equation: x_new = x + dt * f(t + dt/2, (x + x_new)/2) - for (int iter = 0; iter < maxIter; ++iter) { - VecX g = x_new - x - dt * f(t + dt / 2.0, (x + x_new) / 2.0); // Residual - - // Check for convergence - if (g.norm() < tol) { - return x_new; - } - - // Solve J * delta = -g for the Newton step - MatX A = J(x_new); - Eigen::FullPivLU lu(A); - - // Check if the Jacobian is invertible - if (!lu.isInvertible()) { - throw std::runtime_error("Jacobian is singular during Implicit Midpoint iteration " + std::to_string(iter + 1)); - } - - // Update the guess - VecX delta = lu.solve(-g); - x_new += delta; - } - throw std::runtime_error( - std::string("Implicit Midpoint failed to converge after ") + - std::to_string(maxIter) + - " iterations, final residual norm: " + - std::to_string((x_new - x - dt * f(t + dt / 2.0, (x + x_new) / 2.0)).norm()) - ); - } + template + mathlib::VecX_T implicitMidpoint( + const mathlib::VecX_T& x, + Scalar t, + Scalar dt, + Func&& f, + JacFunc&& jac = nullptr, + int maxIter = 10, + Scalar tol = 1e-7 + ); // Gauss-Legendre Runge-Kutta method (2 stages, 4th order) - template - inline VecX GLRK2(const VecX& x, double t, double dt, Func&& f, int maxIter = 50, double tol = 1e-6, JacFunc&& jac = nullptr) { - size_t n = x.size(); - - // Coefficients for the 2-stage Gauss-Legendre method (4th order) - VecX c(2); - c << - 0.5 - std::sqrt(3.0) / 6.0, - 0.5 + std::sqrt(3.0) / 6.0; // Stage time fractions - - Mat2 A = Mat2::Zero(); - A << - 0.25, 0.25 - std::sqrt(3.0) / 6.0, - 0.25 + std::sqrt(3.0) / 6.0, 0.25; - - const double b = 0.5; // Weights for final update - - // Initial guess for the stage values k1, k2, k3 - VecX k = VecX::Zero(2 * n); // 2 stages - VecX f0 = f(t, x); - k.segment(0, n) = f0; - k.segment(n, n) = f0; - - auto eval_g = [&](const VecX& k_guess, VecX& g) { - VecX k1 = k_guess.segment(0, n); - VecX k2 = k_guess.segment(n, n); - - VecX x1 = x + dt * (A(0, 0) * k1 + A(0, 1) * k2); - VecX x2 = x + dt * (A(1, 0) * k1 + A(1, 1) * k2); - - VecX f1 = f(t + c(0) * dt, x1); - VecX f2 = f(t + c(1) * dt, x2); - - g.resize(2 * n); - g.segment(0, n) = k1 - f1; - g.segment(n, n) = k2 - f2; - }; - - auto eval_j = [&](const VecX& k_guess, MatX& J) { - VecX k1 = k_guess.segment(0, n); - VecX k2 = k_guess.segment(n, n); - - VecX x1 = x + dt * (A(0, 0) * k1 + A(0, 1) * k2); - VecX x2 = x + dt * (A(1, 0) * k1 + A(1, 1) * k2); - - MatX F1(n, n), F2(n, n); - bool analytical_success = false; - - // Attempt to use the provided Jacobian function if it's not a nullptr and is callable - if constexpr (!std::is_same_v, std::nullptr_t>) { - if constexpr (std::is_pointer_v> || requires { bool(jac); }) { - if (jac) { - jac(x1, F1); - jac(x2, F2); - analytical_success = true; - } - } - else { - // Pure lambda type execution path - jac(x1, F1); - jac(x2, F2); - analytical_success = true; - } - } - - if (!analytical_success) { - printf("Using finite difference Jacobian for GLRK2\n"); - F1 = finite_difference_jacobian([&](double, const VecX& x_pert) { return f(t + c(0) * dt, x_pert); }, t + c(0) * dt, x1); - F2 = finite_difference_jacobian([&](double, const VecX& x_pert) { return f(t + c(1) * dt, x_pert); }, t + c(1) * dt, x2); - } - - J.setZero(2 * n, 2 * n); - J.block(0, 0, n, n) = MatX::Identity(n, n) - dt * A(0, 0) * F1; - J.block(0, n, n, n) = -dt * A(0, 1) * F1; - J.block(n, 0, n, n) = -dt * A(1, 0) * F2; - J.block(n, n, n, n) = MatX::Identity(n, n) - dt * A(1, 1) * F2; - }; - - // Solve the nonlinear system for the stage values using Newton-Raphson - k = newton_raphson(eval_g, eval_j, k, maxIter, tol); - - // Compute the final update for x using the stage values - VecX k1 = k.segment(0, n); - VecX k2 = k.segment(n, n); - VecX x_f = x + dt * (b * k1 + b * k2); - - // Precautionary check for divergence (NaN or Inf) - if (!x_f.allFinite()) { throw std::runtime_error("GLRK2 diverged"); } - - return x_f; - } + template + mathlib::VecX_T GLRK2( + const mathlib::VecX_T& x, + Scalar t, + Scalar dt, + Func&& f, + JacFunc&& jac = nullptr, + int maxIter = 50, + Scalar tol = 1e-6 + ); // Gauss-Legendre Runge-Kutta method (3 stages, 6th order) - template - inline VecX GLRK3(const VecX& x, double t, double dt, Func&& f, int maxIter = 150, double tol = -1, JacFunc&& jac = nullptr) { - if (tol < 0.0) { tol = std::max(1e-12, 1e-2 * std::pow(dt, 7.0)); } - - size_t n = x.size(); - - // Coefficients for the 3-stage Gauss-Legendre method (6th order) - VecX c(3); - c << - 0.5 - std::sqrt(15.0) / 10.0, - 0.5, - 0.5 + std::sqrt(15.0) / 10.0; // Stage time fractions - - Mat3 A = Mat3::Zero(); - - A << - 5.0 / 36.0, 2.0 / 9.0 - std::sqrt(15.0) / 15.0, 5.0 / 36.0 - std::sqrt(15.0) / 30.0, - 5.0 / 36.0 + std::sqrt(15.0) / 24.0, 2.0 / 9.0, 5.0 / 36.0 - std::sqrt(15.0) / 24.0, - 5.0 / 36.0 + std::sqrt(15.0) / 30.0, 2.0 / 9.0 + std::sqrt(15.0) / 15.0, 5.0 / 36.0; - - VecX b(3); - b << 5.0 / 18.0, 4.0 / 9.0, 5.0 / 18.0; // Weights for final update - - // Initial guess for the stage values k1, k2, k3 - VecX k = VecX::Zero(3 * n); // 3 stages - VecX x_pred = GLRK2(x, t, dt, f, 50, 1e-10, jac); - //VecX x_pred = rk4Step(x, t, dt, f); - VecX f0 = f(t, x); - - //VecX k1_0 = f(t + c(0) * dt, x + c(0) * dt * (x_pred - x)); - //VecX k2_0 = f(t + c(1) * dt, x + c(1) * dt * (x_pred - x)); - //VecX k3_0 = f(t + c(2) * dt, x + c(2) * dt * (x_pred - x)); - k.segment(0, n) = f0; - k.segment(n, n) = f0; - k.segment(2 * n, n) = f0; - - auto eval_g = [&](const VecX& k_guess, VecX& g) { - VecX k1 = k_guess.segment(0, n); - VecX k2 = k_guess.segment(n, n); - VecX k3 = k_guess.segment(2 * n, n); - - VecX x1 = x + dt * (A(0, 0) * k1 + A(0, 1) * k2 + A(0, 2) * k3); - VecX x2 = x + dt * (A(1, 0) * k1 + A(1, 1) * k2 + A(1, 2) * k3); - VecX x3 = x + dt * (A(2, 0) * k1 + A(2, 1) * k2 + A(2, 2) * k3); - - VecX f1 = f(t + c(0) * dt, x1); - VecX f2 = f(t + c(1) * dt, x2); - VecX f3 = f(t + c(2) * dt, x3); - - g.resize(3 * n); - - g.segment(0, n) = k1 - f1; - g.segment(n, n) = k2 - f2; - g.segment(2 * n, n) = k3 - f3; - }; - - auto eval_j = [&](const VecX& k_guess, MatX& J) { - - VecX k1 = k_guess.segment(0, n); - VecX k2 = k_guess.segment(n, n); - VecX k3 = k_guess.segment(2 * n, n); - - VecX x1 = x + dt * (A(0, 0) * k1 + A(0, 1) * k2 + A(0, 2) * k3); - VecX x2 = x + dt * (A(1, 0) * k1 + A(1, 1) * k2 + A(1, 2) * k3); - VecX x3 = x + dt * (A(2, 0) * k1 + A(2, 1) * k2 + A(2, 2) * k3); - - MatX F1(n, n), F2(n, n), F3(n, n); - bool analytical_success = false; - - // Attempt to use the provided Jacobian function if it's not a nullptr and is callable - if constexpr (!std::is_same_v, std::nullptr_t>) { - if constexpr (std::is_pointer_v> || requires { bool(jac); }) { - if (jac) { - jac(x1, F1); - jac(x2, F2); - jac(x3, F3); - analytical_success = true; - } - } - else { - // Pure lambda type execution path - jac(x1, F1); - jac(x2, F2); - jac(x3, F3); - analytical_success = true; - } - } - - if (!analytical_success) { - printf("Using finite difference Jacobian for GLRK3\n"); - F1 = finite_difference_jacobian( - [&](double, const VecX& x_pert) { - return f(t + c(0) * dt, x_pert); - }, - t + c(0) * dt, - x1 - ); - - F2 = finite_difference_jacobian( - [&](double, const VecX& x_pert) { - return f(t + c(1) * dt, x_pert); - }, - t + c(1) * dt, - x2 - ); - - F3 = finite_difference_jacobian( - [&](double, const VecX& x_pert) { - return f(t + c(2) * dt, x_pert); - }, - t + c(2) * dt, - x3 - ); - } - - J.setZero(3 * n, 3 * n); - J.block(0, 0, n, n) = MatX::Identity(n, n) - dt * A(0, 0) * F1; - J.block(0, n, n, n) = -dt * A(0, 1) * F1; - J.block(0, 2 * n, n, n) = -dt * A(0, 2) * F1; - J.block(n, 0, n, n) = -dt * A(1, 0) * F2; - J.block(n, n, n, n) = MatX::Identity(n, n) - dt * A(1, 1) * F2; - J.block(n, 2 * n, n, n) = -dt * A(1, 2) * F2; - J.block(2 * n, 0, n, n) = -dt * A(2, 0) * F3; - J.block(2 * n, n, n, n) = -dt * A(2, 1) * F3; - J.block(2 * n, 2 * n, n, n) = MatX::Identity(n, n) - dt * A(2, 2) * F3; - }; - - // Solve the nonlinear system for the stage values using Newton-Raphson - k = newton_raphson(eval_g, eval_j, k, maxIter, tol); - - VecX g_check; - eval_g(k, g_check); - - double residual = g_check.norm(); - //if (residual > 1e-8) { std::cout << "[GLRK3] Large final residual: " << residual << std::endl; } - - // Compute the final update for x using the stage values - VecX k1 = k.segment(0, n); - VecX k2 = k.segment(n, n); - VecX k3 = k.segment(2 * n, n); - VecX x_f = x + dt * (b(0) * k1 + b(1) * k2 + b(2) * k3); - - // Precautionary check for divergence (NaN or Inf) - if (!x_f.allFinite()) { throw std::runtime_error("GLRK3 diverged"); } - - return x_f; - } + template + mathlib::VecX_T GLRK3( + const mathlib::VecX_T& x, + Scalar t, + Scalar dt, + Func&& f, + JacFunc&& jac = nullptr, + int maxIter = 150, + Scalar tol = -1 + ); private: // Newton-Raphson solver for systems of nonlinear equations g(x) = 0 - template - VecX newton_raphson(EvalG&& eval_g, EvalJ&& eval_j, VecX x0, int maxIter, double tol) { - VecX x = x0, g, x_trial; - VecX delta = VecX::Constant(x0.size(), std::numeric_limits::infinity()); - MatX J; - Eigen::ColPivHouseholderQR solver; - - for (int iter = 0; iter < maxIter; ++iter) { - eval_g(x, g); - if (!g.allFinite()) { - throw std::runtime_error( - "Newton received non-finite residual at iter = " - + std::to_string(iter) - + ", residual norm = " - + std::to_string(g.norm()) - ); - } - if (g.norm() < tol) { return x; } - - eval_j(x, J); - if (!J.allFinite()) { - throw std::runtime_error( - "Newton received non-finite Jacobian at iter = " - + std::to_string(iter) - ); - } - - solver.compute(J); - delta = solver.solve(-g); - - if (!delta.allFinite()) { - throw std::runtime_error( - "Newton produced non-finite step at iter = " - + std::to_string(iter) - ); - } - - x += delta; - if (delta.norm() < tol * (1.0 + x.norm())) { return x; } - - if (!x.allFinite()) { - throw std::runtime_error( - "Newton state became non-finite at iter = " - + std::to_string(iter) - ); - } - } - - throw std::runtime_error( - "Newton-Raphson failed to converge after " - + std::to_string(maxIter) - + " iterations. Final residual norm = " - + std::to_string(g.norm()) - + ", final step norm = " - + std::to_string(delta.norm()) - ); - } + template + mathlib::VecX_T newtonRaphson( + EvalG&& eval_g, + EvalJ&& eval_j, + mathlib::VecX_T x0, + int maxIter, + Scalar tol + ); // Finite difference approximation of the Jacobian matrix df/dx for a vector-valued function f: R^n -> R^m at a point x - template - MatX finite_difference_jacobian(Func&& f, double t, const VecX& x) { - const double eps_rel = 1e-8; - VecX f_0 = f(t, x); - int n = (int)x.size(); - MatX J = MatX::Zero(f_0.size(), n); - // Compute the Jacobian column by column using central differences - for (int i = 0; i < n; ++i) { - VecX x_fwd = x; - VecX x_bwd = x; - double h = eps_rel * std::max(1.0, std::abs(x(i))); - x_fwd(i) += h; - x_bwd(i) -= h; - VecX f_fwd = f(t, x_fwd); - VecX f_bwd = f(t, x_bwd); - J.col(i) = (f_fwd - f_bwd) / (2.0 * h); - } - return J; - } + template + mathlib::MatX_T finiteDifferenceJacobian( + Func&& f, + Scalar t, + const mathlib::VecX_T& x + ); }; +}// namespace integration - // Partial Differential Equation (PDE) solvers - class MATHLIB_API PDE { - public: - - // Explicit finite difference method for 1D heat equation: u_t = alpha * u_xx - inline VecX fdmStep(const VecX& u, double dx, double dt, double alpha) { - int n = (int)u.size(); - VecX u_new = u; - double r = alpha * dt / (dx * dx); - for (int i = 1; i < n - 1; ++i) { - u_new(i) = u(i) + r * (u(i + 1) - 2 * u(i) + u(i - 1)); - } - return u_new; - } - }; -} \ No newline at end of file +#include "numerical_integrators.inl" \ No newline at end of file diff --git a/PxMLib/MathLib/include/integrators/numerical_integrators.inl b/PxMLib/MathLib/include/integrators/numerical_integrators.inl new file mode 100644 index 00000000..48dacb4b --- /dev/null +++ b/PxMLib/MathLib/include/integrators/numerical_integrators.inl @@ -0,0 +1,626 @@ +// PxM/MathLib numerical_integrators.inl +#pragma once + +namespace integration { + // Euler method + template + mathlib::VecX_T NumericalIntegrator::eulerStep( + const mathlib::VecX_T& x, + Scalar t, + Scalar dt, + Func&& f + ) { + return x + dt * f(t, x); + } + + // Second-order Runge-Kutta method (Midpoint) + template + mathlib::VecX_T NumericalIntegrator::midpointStep( + const mathlib::VecX_T& x, + Scalar t, + Scalar dt, + Func&& f + ) { + mathlib::VecX_T k1 = dt * f(t, x); + mathlib::VecX_T k2 = dt * f(t + dt / Scalar(2), x + k1 / Scalar(2)); + return x + k2; + } + + // Second-order Runge-Kutta method (Heun) + template + mathlib::VecX_T NumericalIntegrator::heunStep( + const mathlib::VecX_T& x, + Scalar t, + Scalar dt, + Func&& f + ) { + mathlib::VecX_T k1 = dt * f(t, x); + mathlib::VecX_T k2 = dt * f(t + dt, x + k1); + return x + (k1 + k2) / Scalar(2); + } + + // Second-order Runge-Kutta method (Ralston) + template + mathlib::VecX_T NumericalIntegrator::ralstonStep( + const mathlib::VecX_T& x, + Scalar t, + Scalar dt, + Func&& f + ) { + mathlib::VecX_T k1 = dt * f(t, x); + mathlib::VecX_T k2 = dt * f(t + (Scalar(2) / Scalar(3)) * dt, x + (Scalar(2) / Scalar(3)) * k1); + return x + (k1 + Scalar(3) * k2) / Scalar(4); + } + + // Fourth-order Runge-Kutta method + template + mathlib::VecX_T NumericalIntegrator::rk4Step( + const mathlib::VecX_T& x, + Scalar t, + Scalar dt, + Func&& f + ) { + mathlib::VecX_T k1 = dt * f(t, x); + mathlib::VecX_T k2 = dt * f(t + dt / Scalar(2), x + k1 / Scalar(2)); + mathlib::VecX_T k3 = dt * f(t + dt / Scalar(2), x + k2 / Scalar(2)); + mathlib::VecX_T k4 = dt * f(t + dt, x + k3); + return x + (k1 + Scalar(2) * k2 + Scalar(2) * k3 + k4) / Scalar(6); + } + + // RK45 method with adaptive step size (Dormand-Prince) + template + mathlib::VecX_T NumericalIntegrator::rk45Step( + const mathlib::VecX_T& x, + Scalar t, + Scalar& dt, + Scalar& dt_used, + Func&& f, + Scalar rtol, + Scalar atol + ) { + const Scalar safety = 0.9; // safety factor to prevent aggressive step size changes + const Scalar fac_min = 0.2; // minimum factor for reducing step size + const Scalar fac_max = 5.0; // maximum factor for increasing step size + const Scalar h_min = 1e-10; // minimum allowed step size + const Scalar h_max = 1.0; // maximum allowed step size + + dt = std::clamp(dt, h_min, h_max); + + // Try up to 25 attempts to find an acceptable step size + for (int attempt = 0; attempt < 25; ++attempt) { + // Butcher tableau for Dormand-Prince method (7 stages, 5th order, SHOULD probably be precomputed as static constants, in a matrix or equiv) + + // Coefficients for error estimation + const Scalar c2 = 1.0 / 5.0; + const Scalar c3 = 3.0 / 10.0; + const Scalar c4 = 4.0 / 5.0; + const Scalar c5 = 8.0 / 9.0; + const Scalar c6 = 1.0; + const Scalar c7 = 1.0; + + // Dormand-Prince coefficients + const Scalar a21 = 1.0 / 5.0; + const Scalar a31 = 3.0 / 40.0; + const Scalar a32 = 9.0 / 40.0; + const Scalar a41 = 44.0 / 45.0; + const Scalar a42 = -56.0 / 15.0; + const Scalar a43 = 32.0 / 9.0; + const Scalar a51 = 19372.0 / 6561.0; + const Scalar a52 = -25360.0 / 2187.0; + const Scalar a53 = 64448.0 / 6561.0; + const Scalar a54 = -212.0 / 729.0; + const Scalar a61 = 9017.0 / 3168.0; + const Scalar a62 = -355.0 / 33.0; + const Scalar a63 = 46732.0 / 5247.0; + const Scalar a64 = 49.0 / 176.0; + const Scalar a65 = -5103.0 / 18656.0; + const Scalar a71 = 35.0 / 384.0; + const Scalar a72 = 0.0; + const Scalar a73 = 500.0 / 1113.0; + const Scalar a74 = 125.0 / 192.0; + const Scalar a75 = -2187.0 / 6784.0; + const Scalar a76 = 11.0 / 84.0; + + // Weights for 4th and 5th order estimates + const Scalar b1 = 35.0 / 384.0; + const Scalar b2 = 0.0; + const Scalar b3 = 500.0 / 1113.0; + const Scalar b4 = 125.0 / 192.0; + const Scalar b5 = -2187.0 / 6784.0; + const Scalar b6 = 11.0 / 84.0; + const Scalar b1s = 5179.0 / 57600.0; + const Scalar b2s = 0.0; + const Scalar b3s = 7571.0 / 16695.0; + const Scalar b4s = 393.0 / 640.0; + const Scalar b5s = -92097.0 / 339200.0; + const Scalar b6s = 187.0 / 2100.0; + const Scalar b7s = 1.0 / 40.0; + + // Compute the Runge-Kutta stages (DP -> 7 stages) + const mathlib::VecX_T k1 = f(t, x); + const mathlib::VecX_T k2 = f(t + c2 * dt, x + dt * (a21 * k1)); + const mathlib::VecX_T k3 = f(t + c3 * dt, x + dt * (a31 * k1 + a32 * k2)); + const mathlib::VecX_T k4 = f(t + c4 * dt, x + dt * (a41 * k1 + a42 * k2 + a43 * k3)); + const mathlib::VecX_T k5 = f(t + c5 * dt, x + dt * (a51 * k1 + a52 * k2 + a53 * k3 + a54 * k4)); + const mathlib::VecX_T k6 = f(t + c6 * dt, x + dt * (a61 * k1 + a62 * k2 + a63 * k3 + a64 * k4 + a65 * k5)); + const mathlib::VecX_T k7 = f(t + c7 * dt, x + dt * (a71 * k1 + a72 * k2 + a73 * k3 + a74 * k4 + a75 * k5 + a76 * k6)); + + // Compute 4th and 5th order estimates + const mathlib::VecX_T y5 = x + dt * ((b1 * k1) + (b3 * k3) + (b4 * k4) + (b5 * k5) + (b6 * k6)); // 5th order estimate + const mathlib::VecX_T y4 = x + dt * ((b1s * k1) + (b3s * k3) + (b4s * k4) + (b5s * k5) + (b6s * k6) + (b7s * k7)); // 4th order estimate + + const mathlib::VecX_T e = y5 - y4; // Error estimate + + // Compute the error norm + Scalar errNorm = 0.0; + for (int i = 0; i < e.size(); ++i) { + Scalar sc = atol + rtol * std::max(std::abs(x(i)), std::abs(y5(i))); + const Scalar r = e(i) / sc; + errNorm += r * r; + } + Scalar err = std::sqrt(errNorm / e.size()); + + // Adaptive step size control + + // Accept + if (err <= 1.0 && std::isfinite(err)) { + dt_used = dt; // Store the actual step size used for this step + + // Update step size for next iteration + const Scalar denom = std::max(err, 1e-10); // prevent division by zero + Scalar fac = safety * std::pow(denom, -0.2); // exponent for 5th order method + fac = std::clamp(fac, fac_min, fac_max); // limit step size change + dt = std::clamp(dt * fac, h_min, h_max); // update step size + return y5; + } + // Reject + else { + Scalar denom = (std::isfinite(err) + ? std::max(err, 1e-16) : 1e16); // prevent division by zero & NaN + Scalar fac = safety * std::pow(denom, -0.2); // exponent for 4th order method + fac = std::clamp(fac, fac_min, fac_max); + dt = std::clamp(dt * fac, h_min, h_max); + } + } + throw std::runtime_error("RK45 failed to converge after maximum attempts"); + } + + // Implicit Euler method + template + mathlib::VecX_T NumericalIntegrator::implicitEuler( + const mathlib::VecX_T& x, + Scalar t, + Scalar dt, + Func&& f, + JacFunc&& jac = nullptr, + int maxIter = 8, + Scalar tol = 1e-6 + ) { + mathlib::VecX_T x_new = x + dt * f(t, x); // Initial guess + + // The function g(x_guess) = 0 that we want to solve for the implicit Euler step + auto g = [&](const mathlib::VecX_T& x_guess) { return x_guess - x - dt * f(t + dt, x_guess); }; + + // Numerical Jacobian for Newton-Raphson + auto J = [&](const mathlib::VecX_T& x_guess) -> mathlib::MatX_T { + int n = (int)x_guess.size(); + + mathlib::MatX_T F; + bool analytical_success = false; + + if constexpr (!std::is_same_v, std::nullptr_t>) { + if constexpr (std::is_pointer_v> || requires { bool(jac); }) { + if (jac) { + jac(x_guess, F); // User-provided Jacobian + analytical_success = true; + } + } + else { + jac(x_guess, F); // User-provided Jacobian + analytical_success = true; + } + } + if (!analytical_success) { + F = finiteDifferenceJacobian( + [&](Scalar, const mathlib::VecX_T& x_pert) { return f(t + dt, x_pert); }, + t + dt, + x_guess + ); + } + + return mathlib::MatX_T::Identity(n, n) - dt * F; // J = I - dt * df/dx + }; + + return newtonRaphson(g, J, x + dt * f(t + dt, x), maxIter, tol); + } + + // Implicit Midpoint method + template + mathlib::VecX_T NumericalIntegrator::implicitMidpoint( + const mathlib::VecX_T& x, + Scalar t, + Scalar dt, + Func&& f, + JacFunc&& jac = nullptr, + int maxIter = 10, + Scalar tol = 1e-7 + ) { + mathlib::VecX_T x_new = x + dt * f((t + dt) / Scalar(2), x); // Initial guess + + // The function g(x_guess) = 0 that we want to solve for the implicit midpoint step + auto g = [&](const mathlib::VecX_T& x_guess) { return x_guess - x - dt * f((t + dt) / Scalar(2), (x + x_guess) / Scalar(2)); }; + + // Numerical Jacobian for Newton-Raphson + auto J = [&](const mathlib::VecX_T& x_guess) -> mathlib::MatX_T { + int n = (int)x_guess.size(); + + mathlib::MatX_T F; + bool analytical_success = false; + + if constexpr (!std::is_same_v, std::nullptr_t>) { + if constexpr (std::is_pointer_v> || requires { bool(jac); }) { + if (jac) { + jac(x_guess, F); // User-provided Jacobian + analytical_success = true; + } + } + else { + jac(x_guess, F); // User-provided Jacobian + analytical_success = true; + } + } + if (!analytical_success) { + F = finiteDifferenceJacobian( + [&](Scalar, const mathlib::VecX_T& x_pert) { return f((t + dt) / Scalar(2), (x + x_pert) / Scalar(2)); }, + t + dt, + x_guess + ); + } + + return mathlib::MatX_T::Identity(n, n) - Scalar(0.5) * dt * F; // J = I - dt * df/dx + }; + + return newtonRaphson(g, J, x + dt + f(t + dt / Scalar(2), x), maxIter, tol); + } + + // Gauss-Legendre Runge-Kutta method (2 stages, 4th order) + template + mathlib::VecX_T NumericalIntegrator::GLRK2( + const mathlib::VecX_T& x, + Scalar t, + Scalar dt, + Func&& f, + JacFunc&& jac = nullptr, + int maxIter = 50, + Scalar tol = 1e-6 + ) { + size_t n = x.size(); + + // Coefficients for the 2-stage Gauss-Legendre method (4th order) + mathlib::VecX_T c(2); + c << + 0.5 - std::sqrt(3.0) / 6.0, + 0.5 + std::sqrt(3.0) / 6.0; // Stage time fractions + + mathlib::MatX_T A = mathlib::MatX_T::Zero(); + A << + 0.25, 0.25 - std::sqrt(3.0) / 6.0, + 0.25 + std::sqrt(3.0) / 6.0, 0.25; + + const Scalar b = 0.5; // Weights for final update + + // Initial guess for the stage values k1, k2, k3 + mathlib::VecX_T k = mathlib::VecX_T::Zero(2 * n); // 2 stages + mathlib::VecX_T f0 = f(t, x); + k.segment(0, n) = f0; + k.segment(n, n) = f0; + + auto eval_g = [&](const mathlib::VecX_T& k_guess, mathlib::VecX_T& g) { + mathlib::VecX_T k1 = k_guess.segment(0, n); + mathlib::VecX_T k2 = k_guess.segment(n, n); + + mathlib::VecX_T x1 = x + dt * (A(0, 0) * k1 + A(0, 1) * k2); + mathlib::VecX_T x2 = x + dt * (A(1, 0) * k1 + A(1, 1) * k2); + + mathlib::VecX_T f1 = f(t + c(0) * dt, x1); + mathlib::VecX_T f2 = f(t + c(1) * dt, x2); + + g.resize(2 * n); + g.segment(0, n) = k1 - f1; + g.segment(n, n) = k2 - f2; + }; + + auto eval_j = [&](const VecX_T& k_guess, MatX_T& J) { + mathlib::VecX_T k1 = k_guess.segment(0, n); + mathlib::VecX_T k2 = k_guess.segment(n, n); + + mathlib::VecX_T x1 = x + dt * (A(0, 0) * k1 + A(0, 1) * k2); + mathlib::VecX_T x2 = x + dt * (A(1, 0) * k1 + A(1, 1) * k2); + + mathlib::MatX_T F1(n, n), F2(n, n); + bool analytical_success = false; + + // Attempt to use the provided Jacobian function if it's not a nullptr and is callable + if constexpr (!std::is_same_v, std::nullptr_t>) { + if constexpr (std::is_pointer_v> || requires { bool(jac); }) { + if (jac) { + jac(x1, F1); + jac(x2, F2); + analytical_success = true; + } + } + else { + // Pure lambda type execution path + jac(x1, F1); + jac(x2, F2); + analytical_success = true; + } + } + + if (!analytical_success) { + printf("Using finite difference Jacobian for GLRK2\n"); + F1 = finiteDifferenceJacobian([&](Scalar, const mathlib::VecX_T& x_pert) { return f(t + c(0) * dt, x_pert); }, t + c(0) * dt, x1); + F2 = finiteDifferenceJacobian([&](Scalar, const mathlib::VecX_T& x_pert) { return f(t + c(1) * dt, x_pert); }, t + c(1) * dt, x2); + } + + J.setZero(2 * n, 2 * n); + J.block(0, 0, n, n) = mathlib::MatX_T::Identity(n, n) - dt * A(0, 0) * F1; + J.block(0, n, n, n) = -dt * A(0, 1) * F1; + J.block(n, 0, n, n) = -dt * A(1, 0) * F2; + J.block(n, n, n, n) = mathlib::MatX_T::Identity(n, n) - dt * A(1, 1) * F2; + }; + + // Solve the nonlinear system for the stage values using Newton-Raphson + k = newtonRaphson(eval_g, eval_j, k, maxIter, tol); + + // Compute the final update for x using the stage values + mathlib::VecX_T k1 = k.segment(0, n); + mathlib::VecX_T k2 = k.segment(n, n); + mathlib::VecX_T x_f = x + dt * (b * k1 + b * k2); + + // Precautionary check for divergence (NaN or Inf) + if (!x_f.allFinite()) { throw std::runtime_error("GLRK2 diverged"); } + + return x_f; + } + + // Gauss-Legendre Runge-Kutta method (3 stages, 6th order) + template + mathlib::VecX_T NumericalIntegrator::GLRK3( + const mathlib::VecX_T& x, + Scalar t, + Scalar dt, + Func&& f, + JacFunc&& jac = nullptr, + int maxIter = 150, + Scalar tol = -1 + ) { + if (tol < 0.0) { tol = std::max(1e-12, 1e-2 * std::pow(dt, 7.0)); } + + size_t n = x.size(); + + // Coefficients for the 3-stage Gauss-Legendre method (6th order) + mathlib::VecX_T c(3); + c << + 0.5 - std::sqrt(15.0) / 10.0, + 0.5, + 0.5 + std::sqrt(15.0) / 10.0; // Stage time fractions + + mathlib::Mat3_T A = mathlib::Mat3_T::Zero(); + + A << + 5.0 / 36.0, 2.0 / 9.0 - std::sqrt(15.0) / 15.0, 5.0 / 36.0 - std::sqrt(15.0) / 30.0, + 5.0 / 36.0 + std::sqrt(15.0) / 24.0, 2.0 / 9.0, 5.0 / 36.0 - std::sqrt(15.0) / 24.0, + 5.0 / 36.0 + std::sqrt(15.0) / 30.0, 2.0 / 9.0 + std::sqrt(15.0) / 15.0, 5.0 / 36.0; + + mathlib::VecX_T b(3); + b << 5.0 / 18.0, 4.0 / 9.0, 5.0 / 18.0; // Weights for final update + + // Initial guess for the stage values k1, k2, k3 + mathlib::VecX_T k = mathlib::VecX_T::Zero(3 * n); // 3 stages + mathlib::VecX_T f0 = f(t, x); + k.segment(0, n) = f0; + k.segment(n, n) = f0; + k.segment(2 * n, n) = f0; + + auto eval_g = [&](const VecX_T& k_guess, VecX_T& g) { + mathlib::VecX_T k1 = k_guess.segment(0, n); + mathlib::VecX_T k2 = k_guess.segment(n, n); + mathlib::VecX_T k3 = k_guess.segment(2 * n, n); + + mathlib::VecX_T x1 = x + dt * (A(0, 0) * k1 + A(0, 1) * k2 + A(0, 2) * k3); + mathlib::VecX_T x2 = x + dt * (A(1, 0) * k1 + A(1, 1) * k2 + A(1, 2) * k3); + mathlib::VecX_T x3 = x + dt * (A(2, 0) * k1 + A(2, 1) * k2 + A(2, 2) * k3); + + mathlib::VecX_T f1 = f(t + c(0) * dt, x1); + mathlib::VecX_T f2 = f(t + c(1) * dt, x2); + mathlib::VecX_T f3 = f(t + c(2) * dt, x3); + + g.resize(3 * n); + + g.segment(0, n) = k1 - f1; + g.segment(n, n) = k2 - f2; + g.segment(2 * n, n) = k3 - f3; + }; + + auto eval_j = [&](const VecX_T& k_guess, MatX_T& J) { + mathlib::VecX_T k1 = k_guess.segment(0, n); + mathlib::VecX_T k2 = k_guess.segment(n, n); + mathlib::VecX_T k3 = k_guess.segment(2 * n, n); + + mathlib::VecX_T x1 = x + dt * (A(0, 0) * k1 + A(0, 1) * k2 + A(0, 2) * k3); + mathlib::VecX_T x2 = x + dt * (A(1, 0) * k1 + A(1, 1) * k2 + A(1, 2) * k3); + mathlib::VecX_T x3 = x + dt * (A(2, 0) * k1 + A(2, 1) * k2 + A(2, 2) * k3); + + mathlib::MatX_T F1(n, n), F2(n, n), F3(n, n); + bool analytical_success = false; + + // Attempt to use the provided Jacobian function if it's not a nullptr and is callable + if constexpr (!std::is_same_v, std::nullptr_t>) { + if constexpr (std::is_pointer_v> || requires { bool(jac); }) { + if (jac) { + jac(x1, F1); + jac(x2, F2); + jac(x3, F3); + analytical_success = true; + } + } + else { + // Pure lambda type execution path + jac(x1, F1); + jac(x2, F2); + jac(x3, F3); + analytical_success = true; + } + } + + if (!analytical_success) { + printf("Using finite difference Jacobian for GLRK3\n"); + F1 = finiteDifferenceJacobian( + [&](Scalar, const mathlib::VecX_T& x_pert) { + return f(t + c(0) * dt, x_pert); + }, + t + c(0) * dt, + x1 + ); + + F2 = finiteDifferenceJacobian( + [&](Scalar, const mathlib::VecX_T& x_pert) { + return f(t + c(1) * dt, x_pert); + }, + t + c(1) * dt, + x2 + ); + + F3 = finiteDifferenceJacobian( + [&](Scalar, const mathlib::VecX_T& x_pert) { + return f(t + c(2) * dt, x_pert); + }, + t + c(2) * dt, + x3 + ); + } + + J.setZero(3 * n, 3 * n); + J.block(0, 0, n, n) = mathlib::MatX_T::Identity(n, n) - dt * A(0, 0) * F1; + J.block(0, n, n, n) = -dt * A(0, 1) * F1; + J.block(0, 2 * n, n, n) = -dt * A(0, 2) * F1; + J.block(n, 0, n, n) = -dt * A(1, 0) * F2; + J.block(n, n, n, n) = mathlib::MatX_T::Identity(n, n) - dt * A(1, 1) * F2; + J.block(n, 2 * n, n, n) = -dt * A(1, 2) * F2; + J.block(2 * n, 0, n, n) = -dt * A(2, 0) * F3; + J.block(2 * n, n, n, n) = -dt * A(2, 1) * F3; + J.block(2 * n, 2 * n, n, n) = mathlib::MatX_T::Identity(n, n) - dt * A(2, 2) * F3; + }; + + // Solve the nonlinear system for the stage values using Newton-Raphson + k = newtonRaphson(eval_g, eval_j, k, maxIter, tol); + + mathlib::VecX_T g_check; + eval_g(k, g_check); + + Scalar residual = g_check.norm(); + //if (residual > 1e-8) { std::cout << "[GLRK3] Large final residual: " << residual << std::endl; } + + // Compute the final update for x using the stage values + mathlib::VecX_T k1 = k.segment(0, n); + mathlib::VecX_T k2 = k.segment(n, n); + mathlib::VecX_T k3 = k.segment(2 * n, n); + mathlib::VecX_T x_f = x + dt * (b(0) * k1 + b(1) * k2 + b(2) * k3); + + // Precautionary check for divergence (NaN or Inf) + if (!x_f.allFinite()) { throw std::runtime_error("GLRK3 diverged"); } + + return x_f; + } + + // Newton-Raphson solver for systems of nonlinear equations g(x) = 0 + template + mathlib::VecX_T NumericalIntegrator::newtonRaphson( + EvalG&& eval_g, + EvalJ&& eval_j, + mathlib::VecX_T x0, + int maxIter, + Scalar tol + ) { + mathlib::VecX_T x = x0, g, x_trial; + mathlib::VecX_T delta = mathlib::VecX_T::Constant(x0.size(), std::numeric_limits::infinity()); + mathlib::MatX_T J; + Eigen::ColPivHouseholderQR> solver; + + for (int iter = 0; iter < maxIter; ++iter) { + eval_g(x, g); + if (!g.allFinite()) { + throw std::runtime_error( + "Newton received non-finite residual at iter = " + + std::to_string(iter) + + ", residual norm = " + + std::to_string(g.norm()) + ); + } + if (g.norm() < tol) { return x; } + + eval_j(x, J); + if (!J.allFinite()) { + throw std::runtime_error( + "Newton received non-finite Jacobian at iter = " + + std::to_string(iter) + ); + } + + solver.compute(J); + delta = solver.solve(-g); + + if (!delta.allFinite()) { + throw std::runtime_error( + "Newton produced non-finite step at iter = " + + std::to_string(iter) + ); + } + + x += delta; + if (delta.norm() < tol * (1.0 + x.norm())) { return x; } + + if (!x.allFinite()) { + throw std::runtime_error( + "Newton state became non-finite at iter = " + + std::to_string(iter) + ); + } + } + + throw std::runtime_error( + "Newton-Raphson failed to converge after " + + std::to_string(maxIter) + + " iterations. Final residual norm = " + + std::to_string(g.norm()) + + ", final step norm = " + + std::to_string(delta.norm()) + ); + } + + // Finite difference approximation of the Jacobian matrix df/dx for a vector-valued function f: R^n -> R^m at a point x + template + mathlib::MatX_T NumericalIntegrator::finiteDifferenceJacobian( + Func&& f, + Scalar t, + const mathlib::VecX_T& x + ) { + const Scalar eps_rel = 1e-8; + mathlib::VecX_T f_0 = f(t, x); + int n = (int)x.size(); + mathlib::MatX_T J = mathlib::MatX_T::Zero(f_0.size(), n); + // Compute the Jacobian column by column using central differences + for (int i = 0; i < n; ++i) { + mathlib::VecX_T x_fwd = x; + mathlib::VecX_T x_bwd = x; + Scalar h = eps_rel * std::max(1.0, std::abs(x(i))); + x_fwd(i) += h; + x_bwd(i) -= h; + mathlib::VecX_T f_fwd = f(t, x_fwd); + mathlib::VecX_T f_bwd = f(t, x_bwd); + J.col(i) = (f_fwd - f_bwd) / (2.0 * h); + } + return J; + } +} \ No newline at end of file From 5bdc0fad896c1b50731806500352058bf9b2bc41 Mon Sep 17 00:00:00 2001 From: saltyjoss Date: Sat, 16 May 2026 19:07:14 +0100 Subject: [PATCH 008/210] fixes: Minor code errors fixed, only observed when built * THIS TOOK SO LONG --- .../include/Numerics/IntegrationService.h | 22 ++++---- .../DSFE_Core/include/Robots/DynamicsTypes.h | 4 +- .../DSFE_Core/include/Robots/RobotDynamics.h | 10 ++++ .../include/Robots/RobotDynamics.inl | 53 +++++++++---------- .../include/Robots/RobotKinematics.h | 4 +- .../include/Robots/RobotKinematics.inl | 18 ++++++- .../DSFE_Core/include/Robots/RobotModel.h | 2 +- .../DSFE_Core/include/Robots/RobotSystem.h | 6 +-- .../include/Robots/SpatialDynamics.h | 18 +++---- .../include/Robots/SpatialDynamics.inl | 18 ++++--- .../src/Numerics/IntegrationService.cpp | 2 +- .../DSFE_Core/src/Robots/RobotDynamics.cpp | 11 ---- .../DSFE_Core/src/Robots/RobotKinematics.cpp | 14 ----- DSFE_App/DSFE_Core/src/Robots/RobotSystem.cpp | 40 +++++++------- .../src/AdaptiveMethodTests.cpp | 14 ++--- .../src/ConvergenceOrderTests.cpp | 20 +++---- .../src/ExplicitMethodTests.cpp | 24 ++++----- .../src/ImplicitMethodTests.cpp | 42 +++++++-------- PxMLib/MathLib/include/core/SpatialMath.h | 10 ++++ .../integrators/numerical_integrators.inl | 48 +++++++++-------- 20 files changed, 199 insertions(+), 181 deletions(-) diff --git a/DSFE_App/DSFE_Core/include/Numerics/IntegrationService.h b/DSFE_App/DSFE_Core/include/Numerics/IntegrationService.h index 0173f9b1..78cbd09d 100644 --- a/DSFE_App/DSFE_Core/include/Numerics/IntegrationService.h +++ b/DSFE_App/DSFE_Core/include/Numerics/IntegrationService.h @@ -16,7 +16,7 @@ namespace integration { // Struct representing the result of a single integration step struct DSFE_API StepOut { - VecX x_next; // next state vector + mathlib::VecX x_next; // next state vector double dt_taken = 0.0; // actual step size taken double dt_sug = 0.0; // suggested next step size }; @@ -28,7 +28,7 @@ namespace integration { IntegrationService(); template - StepOut step(eIntegrationMethod m, VecX& x, double t, double dt, Func&& f, JacFunc&& jac) { + StepOut step(eIntegrationMethod m, mathlib::VecX& x, double t, double dt, Func&& f, JacFunc&& jac) { if constexpr (std::is_pointer_v> || requires { f == nullptr; }) { if (f == nullptr) { D_WARN_ONCE("No derivative function provided for integration - Assuming constant derivative (Euler step)"); @@ -45,12 +45,12 @@ namespace integration { case eIntegrationMethod::Ralston: return { _integrator->ralstonStep(x, t, dt, std::forward(f)), dt, dt }; case eIntegrationMethod::RK4: return { _integrator->rk4Step(x, t, dt, std::forward(f)), dt, dt }; case eIntegrationMethod::RK45: return step_adaptive(eIntegrationMethod::RK45, x, t, dt, std::forward(f), _rtol, _atol); - // Implicit methods - // * currently use fixed step size (no error estimation), but are likely to support adaptive stepping in the future - case eIntegrationMethod::ImplicitEuler: return { _integrator->implicit_euler(x, t, dt, std::forward(f)), dt, dt }; - case eIntegrationMethod::ImplicitMidpoint: return { _integrator->implicit_midpoint(x, t, dt, std::forward(f)), dt, dt }; - case eIntegrationMethod::GLRK2: return { _integrator->GLRK2(x, t, dt, std::forward(f), 50, 1e-10, std::forward(jac)), dt, dt }; - case eIntegrationMethod::GLRK3: return { _integrator->GLRK3(x, t, dt, std::forward(f), 80, -1, std::forward(jac)), dt, dt }; + // Implicit methods + // * currently use fixed step size (no error estimation), but are likely to support adaptive stepping in the future + case eIntegrationMethod::ImplicitEuler: return { _integrator->implicitEuler(x, t, dt, std::forward(f), std::forward(jac)), dt, dt }; + case eIntegrationMethod::ImplicitMidpoint: return { _integrator->implicitMidpoint(x, t, dt, std::forward(f), std::forward(jac)), dt, dt }; + case eIntegrationMethod::GLRK2: return { _integrator->GLRK2(x, t, dt, std::forward(f), std::forward(jac), 50, 1e-10), dt, dt }; + case eIntegrationMethod::GLRK3: return { _integrator->GLRK3(x, t, dt, std::forward(f), std::forward(jac), 150, 1e-14), dt, dt }; default: LOG_WARN("Unknown integration method: %s. Defaulting to RK4.", toString(m)); return { _integrator->rk4Step(x, t, dt, std::forward(f)), dt, dt }; @@ -58,7 +58,7 @@ namespace integration { } template - StepOut step_adaptive(eIntegrationMethod m, VecX& x, double t, double dt_try, Func&& f, double rtol, double atol) { + StepOut step_adaptive(eIntegrationMethod m, mathlib::VecX& x, double t, double dt_try, Func&& f, double rtol, double atol) { if constexpr (std::is_pointer_v> || requires { f == nullptr; }) { if (f == nullptr) { LOG_WARN("No derivative function provided for adaptive integration - returning state unchanged"); @@ -83,7 +83,7 @@ namespace integration { const double eps = 1e-12 * dt_try; // small epsilon to prevent division by zero // Initialise current state and time for the adaptive stepping loop - VecX x_curr = x; // current state during the adaptive step + mathlib::VecX x_curr = x; // current state during the adaptive step double t_curr = t; // current time during the adaptive step double t_total = 0.0; // total time taken for the step @@ -96,7 +96,7 @@ namespace integration { double h_try = std::min(h, t_end - t_curr); double dt_used = 0.0; - VecX x_next = _integrator->rk45Step(x_curr, t_curr, h_try, dt_used, std::forward(f), rtol, atol); + mathlib::VecX x_next = _integrator->rk45Step(x_curr, t_curr, h_try, dt_used, std::forward(f), rtol, atol); // Update rk45step t_curr += dt_used; diff --git a/DSFE_App/DSFE_Core/include/Robots/DynamicsTypes.h b/DSFE_App/DSFE_Core/include/Robots/DynamicsTypes.h index 12c6f43f..350f516b 100644 --- a/DSFE_App/DSFE_Core/include/Robots/DynamicsTypes.h +++ b/DSFE_App/DSFE_Core/include/Robots/DynamicsTypes.h @@ -150,8 +150,8 @@ namespace robots { // Central scratch structure that contains all buffers needed for dynamics computations, both dense and spatial template struct DynamicsScratch { - DenseDynamicsScratch dense; - SpatialDynamicsScratch spatial; + DenseDynamicsScratch dense; + SpatialDynamicsScratch spatial; // TODO add kinematics scratch // Gravity scratch buffer diff --git a/DSFE_App/DSFE_Core/include/Robots/RobotDynamics.h b/DSFE_App/DSFE_Core/include/Robots/RobotDynamics.h index 27377f76..c9345cdb 100644 --- a/DSFE_App/DSFE_Core/include/Robots/RobotDynamics.h +++ b/DSFE_App/DSFE_Core/include/Robots/RobotDynamics.h @@ -9,6 +9,16 @@ #include "Robots/SpatialDynamics.h" #include "Robots/SpatialModel.h" +#include "Robots/RobotKinematics.h" +#include "Robots/RobotSimSnapshot.h" + +#include "Robots/TrajectoryManager.h" + +#include +#include +#include + +#include "EngineLib/LogMacros.h" // Forward declarations namespace control { class TrajectoryManager; } diff --git a/DSFE_App/DSFE_Core/include/Robots/RobotDynamics.inl b/DSFE_App/DSFE_Core/include/Robots/RobotDynamics.inl index e97266f6..cf01499b 100644 --- a/DSFE_App/DSFE_Core/include/Robots/RobotDynamics.inl +++ b/DSFE_App/DSFE_Core/include/Robots/RobotDynamics.inl @@ -157,8 +157,8 @@ namespace robots { } // Compute forward kinematics for the perturbed state - _kinematics->computeForwardKinematics_fromState(robot, x_eps, T_world_eps); - jointWorldPoses_eps = _kinematics->calcJointWorldPoses(T_world_eps, robot); + _kinematics->computeForwardKinematics_fromState(robot, x_eps, T_world_eps); + jointWorldPoses_eps = _kinematics->calcJointWorldPoses(T_world_eps, robot); computeMassMatrix(robot, T_world_eps, jointWorldPoses_eps, M_plus); // mass matrix for the perturbed configuration dM_dq[k] = (M_plus - M) / eps; // [kg*m^2/rad], partial derivative of mass matrix with @@ -215,11 +215,8 @@ namespace robots { // Gravitational force on the link mathlib::Vec3_T g_world; - g << mathlib::Vec3_T(0.0, 0.0, -g); // [m/s^2], gravity vector in world frame - const mathlib::Vec3_T F_g; - F_g = m * g_world; // [N], gravitational force on the link in world frame - - // Torque contribution from this link's weight about joint i + g_world << mathlib::Vec3_T(0.0, 0.0, -g); // [m/s^2], gravity vector in world frame + const mathlib::Vec3_T F_g = m * g_world; // [N], gravitational force on the link in world frame const mathlib::Vec3_T r = com_world - p_i; // [m] // Torque = r × F_g projected onto joint axis @@ -243,9 +240,9 @@ namespace robots { Eigen::Map> q_local(x.data(), n); Eigen::Map> qd_local(x.data() + n, n); - _kinematics->computeForwardKinematics_fromState(robot, x, scratch.T_world); - scratch.jointWorldPoses = _kinematics->calcJointWorldPoses(scratch.T_world, robot); - computeMassMatrix(robot, scratch.T_world, scratch.jointWorldPoses, scratch.M); + _kinematics->computeForwardKinematics_fromState(robot, x, scratch.T_world); + scratch.jointWorldPoses = _kinematics->calcJointWorldPoses(scratch.T_world, robot); + computeMassMatrix(robot, scratch.T_world, scratch.jointWorldPoses, scratch.M); J_out.setZero(2 * n, 2 * n); // [rad/rad] position part, [rad/s / rad/s] velocity part J_out.block(0, n, n, n).setIdentity(); @@ -301,16 +298,16 @@ namespace robots { Eigen::Map> q(x.data(), n); Eigen::Map> qd(x.data() + n, n); - _kinematics->computeForwardKinematics_fromState(*snap.model, x, scratch.dense.T_world); + _kinematics->computeForwardKinematics_fromState(*snap.model, x, scratch.dense.T_world); - scratch.dense.jointWorldPoses = _kinematics->calcJointWorldPoses(scratch.dense.T_world, *snap.model); + scratch.dense.jointWorldPoses = _kinematics->calcJointWorldPoses(scratch.dense.T_world, *snap.model); - computeMassMatrix(*snap.model, scratch.dense.T_world, scratch.dense.jointWorldPoses, scratch.dense.M); // [kg*m^2], full mass matrix for the robot at configuration q - scratch.dense.h = computeCoriolisVector(*snap.model, q, qd, scratch.dense.T_world, scratch.dense.M); // [Nm], full Coriolis and centrifugal torque vector + computeMassMatrix(*snap.model, scratch.dense.T_world, scratch.dense.jointWorldPoses, scratch.dense.M); // [kg*m^2], full mass matrix for the robot at configuration q + scratch.dense.h = computeCoriolisVector(*snap.model, q, qd, scratch.dense.T_world, scratch.dense.M); // [Nm], full Coriolis and centrifugal torque vector scratch.g.setZero(); if (snap.torqueMode != eTorqueMode::NONE) { - scratch.g = computeGravityTorque(*snap.model, scratch.dense.T_world, scratch.dense.jointWorldPoses); + scratch.g = computeGravityTorque(*snap.model, scratch.dense.T_world, scratch.dense.jointWorldPoses); } scratch.dense.tau.setZero(); @@ -392,16 +389,18 @@ namespace robots { Eigen::Map> q(x.data(), n); Eigen::Map> qd(x.data() + n, n); - SpatialDynamics::computeSpatialKinematicsAndBias( + SpatialDynamics::computeSpatialKinematicsAndBias( model, q, qd, scratch.spatial.Xup, scratch.spatial.v, scratch.spatial.c ); - mathlib::MatX_T M = SpatialDynamics::CRBA(model, scratch.spatial.Xup, scratch); + mathlib::MatX_T M = SpatialDynamics::CRBA(model, scratch.spatial.Xup, scratch); // RNEA to compute gravity compensation (q, 0, 0) for gravity, (q, qd, 0) for Coriolis - mathlib::VecX_T tau_g = SpatialDynamics::RNEA(model, q, VecX::Zero(n), VecX::Zero(n), scratch); + mathlib::VecX_T qd_zero = mathlib::VecX_T::Zero(n); + mathlib::VecX_T qdd_zero = mathlib::VecX_T::Zero(n); + mathlib::VecX_T tau_g = SpatialDynamics::RNEA(model, q, qd_zero, qdd_zero, scratch); scratch.dense.tau.setZero(); for (size_t i = 0; i < n; ++i) { @@ -443,7 +442,7 @@ namespace robots { out.metrics.tau[i] = tau_i; } - out.qdd = SpatialDynamics::ABA(model, q, qd, scratch.dense.tau, scratch); + out.qdd = SpatialDynamics::ABA(model, q, qd, scratch.dense.tau, scratch); out.metrics.qdd = out.qdd; dx.head(n) = qd; dx.tail(n) = out.qdd; @@ -469,7 +468,7 @@ namespace robots { Eigen::Map> q(x.data(), n); Eigen::Map> qd(x.data() + n, n); - SpatialDynamics::computeSpatialKinematicsAndBias( + SpatialDynamics::computeSpatialKinematicsAndBias( model, q, qd, scratch.spatial.Xup, scratch.spatial.v, @@ -519,15 +518,15 @@ namespace robots { Eigen::Map> q(x.data(), n); Eigen::Map> qd(x.data() + n, n); - _kinematics->computeForwardKinematics_fromState(*snap.model, x, scratch.dense.T_world); - scratch.dense.jointWorldPoses = _kinematics->calcJointWorldPoses(scratch.dense.T_world, *snap.model); + _kinematics->computeForwardKinematics_fromState(*snap.model, x, scratch.dense.T_world); + scratch.dense.jointWorldPoses = _kinematics->calcJointWorldPoses(scratch.dense.T_world, *snap.model); computeMassMatrix(*snap.model, scratch.dense.T_world, scratch.dense.jointWorldPoses, scratch.dense.M); - scratch.dense.h = computeCoriolisVector(*snap.model, q, qd, scratch.dense.T_world, scratch.dense.M); + scratch.dense.h = computeCoriolisVector(*snap.model, q, qd, scratch.dense.T_world, scratch.dense.M); scratch.g.setZero(); if (snap.torqueMode != eTorqueMode::NONE) { - scratch.g = computeGravityTorque(*snap.model, scratch.dense.T_world, scratch.dense.jointWorldPoses); + scratch.g = computeGravityTorque(*snap.model, scratch.dense.T_world, scratch.dense.jointWorldPoses); } scratch.dense.tau.setZero(); @@ -575,9 +574,9 @@ namespace robots { Eigen::Map> qd(x.data() + n, n); // Compute a local mass matrix for this exact stage evaluation frame - _kinematics->computeForwardKinematics_fromState(*snap.model, x, scratch.T_world); - scratch.jointWorldPoses = _kinematics->calcJointWorldPoses(scratch.T_world, *snap.model); - computeMassMatrix(*snap.model, scratch.T_world, scratch.jointWorldPoses, scratch.M); + _kinematics->computeForwardKinematics_fromState(*snap.model, x, scratch.T_world); + scratch.jointWorldPoses = _kinematics->calcJointWorldPoses(scratch.T_world, *snap.model); + computeMassMatrix(*snap.model, scratch.T_world, scratch.jointWorldPoses, scratch.M); mathlib::MatX dTau_dq = mathlib::MatX_T::Zero(n, n); mathlib::MatX dTau_dv = mathlib::MatX_T::Zero(n, n); diff --git a/DSFE_App/DSFE_Core/include/Robots/RobotKinematics.h b/DSFE_App/DSFE_Core/include/Robots/RobotKinematics.h index 6a820745..e5968d5e 100644 --- a/DSFE_App/DSFE_Core/include/Robots/RobotKinematics.h +++ b/DSFE_App/DSFE_Core/include/Robots/RobotKinematics.h @@ -7,6 +7,7 @@ #include #include #include +#include "Robots/RobotSimSnapshot.h" #include "EngineLib/LogMacros.h" @@ -47,7 +48,8 @@ namespace robots { ) const; // Converts roll-pitch-yaw angles (in radians) to a quaternion representation - mathlib::Quat rpyRadToQuat(const mathlib::Vec3& rpyRad); + template + mathlib::Quat rpyRadToQuat(const mathlib::Vec3_T& rpyRad); }; } diff --git a/DSFE_App/DSFE_Core/include/Robots/RobotKinematics.inl b/DSFE_App/DSFE_Core/include/Robots/RobotKinematics.inl index d67d5f75..cd1ca6e6 100644 --- a/DSFE_App/DSFE_Core/include/Robots/RobotKinematics.inl +++ b/DSFE_App/DSFE_Core/include/Robots/RobotKinematics.inl @@ -25,7 +25,7 @@ namespace robots { // Compute the transform to the next link using each joint for (size_t i = 0; i < n; ++i) { - const Scalar& joint = joints[i]; + const auto& joint = joints[i]; const Scalar q = x[i]; // joint angle from state vector mathlib::Pose_T T_origin = mathlib::Pose_T::Identity(); // transform from parent link to joint frame (fixed) @@ -85,8 +85,22 @@ namespace robots { mathlib::Pose_T T = mathlib::Pose_T::Identity(); // homogeneous transformation matrix (4x4) Eigen::AngleAxis aa(q, axis_joint.normalized()); // create angle-axis rotation from joint angle and axis - T.template block<3, 3>(0, 0) = aa.toRotationMatrix(); // set upper-left 3x3 block to rotation matrix + T.template block<3, 3>(0, 0) = aa.toRotationMatrix(); // set upper-left 3x3 block to rotation matrix return T; // (4x4) homogeneous transformation } + + // Converts roll-pitch-yaw angles (in radians) to a quaternion representation + template + mathlib::Quat RobotKinematics::rpyRadToQuat(const mathlib::Vec3_T& rpyRad) { + const double roll = rpyRad.x(); + const double pitch = rpyRad.y(); + const double yaw = rpyRad.z(); + + const Quat qx(Eigen::AngleAxis(roll, Vec3(1.0, 0.0, 0.0))); + const Quat qy(Eigen::AngleAxis(pitch, Vec3(0.0, 1.0, 0.0))); + const Quat qz(Eigen::AngleAxis(yaw, Vec3(0.0, 0.0, 1.0))); + + return (qz * qy * qx).normalized(); + } } \ No newline at end of file diff --git a/DSFE_App/DSFE_Core/include/Robots/RobotModel.h b/DSFE_App/DSFE_Core/include/Robots/RobotModel.h index 70ef1cf3..9ed9e8d8 100644 --- a/DSFE_App/DSFE_Core/include/Robots/RobotModel.h +++ b/DSFE_App/DSFE_Core/include/Robots/RobotModel.h @@ -12,7 +12,7 @@ #include "Platform/Logger.h" #include "EngineLib/LogMacros.h" -constexpr double DEG2RAD = std::numebers::pi / 180.0; +constexpr double DEG2RAD = std::numbers::pi / 180.0; namespace robots { // --- Robot Model Kinematic Models --- diff --git a/DSFE_App/DSFE_Core/include/Robots/RobotSystem.h b/DSFE_App/DSFE_Core/include/Robots/RobotSystem.h index a1364835..d886eff6 100644 --- a/DSFE_App/DSFE_Core/include/Robots/RobotSystem.h +++ b/DSFE_App/DSFE_Core/include/Robots/RobotSystem.h @@ -200,11 +200,11 @@ namespace robots { RobotModel _robot; eTorqueMode _torqueMode = _robot.torqueMode; - SpatialModel _spatialModel; + SpatialModel _spatialModel; RobotConstModel _constModel; - DynamicsScratch _dynScratch; - DynamicsResult _dynResult; + DynamicsScratch _dynScratch; + DynamicsResult _dynResult; // World to robot base transform (meters) std::vector _worldTransforms; diff --git a/DSFE_App/DSFE_Core/include/Robots/SpatialDynamics.h b/DSFE_App/DSFE_Core/include/Robots/SpatialDynamics.h index 55b96f3b..5a5c7893 100644 --- a/DSFE_App/DSFE_Core/include/Robots/SpatialDynamics.h +++ b/DSFE_App/DSFE_Core/include/Robots/SpatialDynamics.h @@ -29,6 +29,15 @@ namespace robots { std::vector>& a_out ); + template + static void computeBackwardForces_RNEA( + const SpatialModel& model, + const std::vector>& Xup, + const std::vector>& v, + const std::vector>& a, + mathlib::VecX_T& tau_out + ); + template static mathlib::VecX_T RNEA( const SpatialModel& model, @@ -38,15 +47,6 @@ namespace robots { DynamicsScratch& scratch ); - template - void computeBackwardForces_RNEA( - const SpatialModel& model, - const std::vector>& Xup, - const std::vector>& v, - const std::vector>& a, - mathlib::VecX_T& tau_out - ); - template static mathlib::MatX_T CRBA( const SpatialModel& model, diff --git a/DSFE_App/DSFE_Core/include/Robots/SpatialDynamics.inl b/DSFE_App/DSFE_Core/include/Robots/SpatialDynamics.inl index a5617196..ab138683 100644 --- a/DSFE_App/DSFE_Core/include/Robots/SpatialDynamics.inl +++ b/DSFE_App/DSFE_Core/include/Robots/SpatialDynamics.inl @@ -25,11 +25,13 @@ namespace robots { if (j.type == eJointType::REVOLUTE) { Eigen::AngleAxis aa(q[i], j.S.angular().normalized()); mathlib::Mat3_T R = aa.toRotationMatrix(); - XJ = mathlib::spatialTransform(R, mathlib::Vec3_T::Zero()); + mathlib::Vec3_T r = mathlib::Vec3_T::Zero(); + XJ = mathlib::spatialTransform(R, r); } else if (j.type == eJointType::PRISMATIC) { mathlib::Vec3_T r = q[i] * j.S.linear().normalized(); - XJ = mathlib::spatialTransform(mathlib::Mat3_T::Identity(), r); + mathlib::Mat3_T R = mathlib::Mat3_T::Identity(); + XJ = mathlib::spatialTransform(R, r); } Xup_out[i] = XJ * j.Xtree; // Combined Transform @@ -89,7 +91,7 @@ namespace robots { // Forward Force Computation for (size_t i = 0; i < n; ++i) { - const SpatialJoint& j = model.joints[i]; + const SpatialJoint& j = model.joints[i]; mathlib::SpatialVec_T I_v = j.inertia * v[i]; mathlib::SpatialVec_T coriolis = crossForce(v[i], I_v); f[i].v = j.inertia * a[i].v + coriolis.v; @@ -97,9 +99,10 @@ namespace robots { // Backward Recursion Computation for (int i = (int)n - 1; i >= 0; --i) { - const SpatialJoint& j = model.joints[i]; + const SpatialJoint& j = model.joints[i]; tau_out[i] = j.S.dot(f[i]); - if (j.parent >= 0) { f[j.parent] += Xup[i].transpose() * f[i]; } + mathlib::SpatialMat_T XupT = Xup[i].transpose(); + if (j.parent >= 0) { f[j.parent] += XupT * f[i]; } } } @@ -145,7 +148,7 @@ namespace robots { // Upward pass: propagate spatial inertia from child links to parent joints for (int i = (int)n - 1; i >= 0; --i) { - const SpatialJoint& j = model.joints[i]; + const SpatialJoint& j = model.joints[i]; if (j.type == eJointType::FIXED) { continue; } int p = j.parent; if (p >= 0) { @@ -163,7 +166,8 @@ namespace robots { int jIdx = (int)i; while (model.joints[jIdx].parent >= 0) { int p = model.joints[jIdx].parent; - F = Xup[jIdx].transpose() * F; + mathlib::SpatialMat_T XupT = Xup[jIdx].transpose(); // TODO Make Eigen-copatible operator overloads for spatial transforms to avoid the errors from this transpose operation in a matrix multiplication context + F = XupT * F; scratch.dense.M(i, p) = model.joints[p].S.dot(F); scratch.dense.M(p, i) = scratch.dense.M(i, p); jIdx = p; diff --git a/DSFE_App/DSFE_Core/src/Numerics/IntegrationService.cpp b/DSFE_App/DSFE_Core/src/Numerics/IntegrationService.cpp index 22ec082c..9d07a161 100644 --- a/DSFE_App/DSFE_Core/src/Numerics/IntegrationService.cpp +++ b/DSFE_App/DSFE_Core/src/Numerics/IntegrationService.cpp @@ -61,5 +61,5 @@ namespace integration { // Constructor IntegrationService::IntegrationService() - : _ODE(std::make_unique()), method(eIntegrationMethod::RK4), _rtol(1e-3), _atol(1e-6), _dt_last(), _dt_max() {} + : _integrator(std::make_unique()), method(eIntegrationMethod::RK4), _rtol(1e-3), _atol(1e-6), _dt_last(), _dt_max() {} } // namespace integration \ No newline at end of file diff --git a/DSFE_App/DSFE_Core/src/Robots/RobotDynamics.cpp b/DSFE_App/DSFE_Core/src/Robots/RobotDynamics.cpp index 87607a85..34204c43 100644 --- a/DSFE_App/DSFE_Core/src/Robots/RobotDynamics.cpp +++ b/DSFE_App/DSFE_Core/src/Robots/RobotDynamics.cpp @@ -2,17 +2,6 @@ // File: RobotDynamics.cpp // GitHub: SaltyJoss #include "Robots/RobotDynamics.h" -#include "Robots/RobotKinematics.h" -#include "Robots/RobotSimSnapshot.h" - -#include "Robots/TrajectoryManager.h" - -#include - -#include -#include - -#include "EngineLib/LogMacros.h" namespace robots { // Constructor diff --git a/DSFE_App/DSFE_Core/src/Robots/RobotKinematics.cpp b/DSFE_App/DSFE_Core/src/Robots/RobotKinematics.cpp index 9df24909..c7b181ac 100644 --- a/DSFE_App/DSFE_Core/src/Robots/RobotKinematics.cpp +++ b/DSFE_App/DSFE_Core/src/Robots/RobotKinematics.cpp @@ -2,7 +2,6 @@ // File: RobotKinematics.cpp // GitHub: SaltyJoss #include "Robots/RobotKinematics.h" -#include "Robots/RobotSimSnapshot.h" using namespace mathlib; using namespace constants; @@ -11,17 +10,4 @@ namespace robots { // Constructor RobotKinematics::RobotKinematics() { } - - // Converts roll-pitch-yaw angles (in radians) to a quaternion representation - mathlib::Quat RobotKinematics::rpyRadToQuat(const mathlib::Vec3& rpyRad) { - const double roll = rpyRad.x(); - const double pitch = rpyRad.y(); - const double yaw = rpyRad.z(); - - const Quat qx(Eigen::AngleAxisd(roll, Vec3(1.0, 0.0, 0.0))); - const Quat qy(Eigen::AngleAxisd(pitch, Vec3(0.0, 1.0, 0.0))); - const Quat qz(Eigen::AngleAxisd(yaw, Vec3(0.0, 0.0, 1.0))); - - return (qz * qy * qx).normalized(); - } } \ No newline at end of file diff --git a/DSFE_App/DSFE_Core/src/Robots/RobotSystem.cpp b/DSFE_App/DSFE_Core/src/Robots/RobotSystem.cpp index 81b119bf..099697af 100644 --- a/DSFE_App/DSFE_Core/src/Robots/RobotSystem.cpp +++ b/DSFE_App/DSFE_Core/src/Robots/RobotSystem.cpp @@ -298,17 +298,17 @@ namespace robots { RobotSimSnapshot snap = takeSnapshot(simTime); const size_t n = snap.model->joints.size(); - Eigen::Map q(x.data(), n); - Eigen::Map qd(x.data() + n, n); + Eigen::Map> q(x.data(), n); + Eigen::Map> qd(x.data() + n, n); - mathlib::VecX qdd(n); + mathlib::VecX_T qdd(n); for (size_t i = 0; i < n; ++i) { qdd[i] = _robot.joints[i].qdd_ref; } std::vector T_start(snap.model->links.size()); _kinematics->computeForwardKinematics_fromState(*snap.model, x, T_start); std::vector jointWorldPoses_start = _kinematics->calcJointWorldPoses(T_start, *snap.model); - SpatialDynamics::computeSpatialKinematicsAndBias( + SpatialDynamics::computeSpatialKinematicsAndBias( _spatialModel, q, qd, _dynScratch.spatial.Xup, @@ -316,14 +316,14 @@ namespace robots { ); // CRBA only for controller inertia scaling - mathlib::MatX M_start = SpatialDynamics::CRBA( + mathlib::MatX_T M_start = SpatialDynamics::CRBA( _spatialModel, _dynScratch.spatial.Xup, _dynScratch ); // Cache frozen joint gains for this step - mathlib::VecX kp_frozen(n), kd_frozen(n); + mathlib::VecX_T kp_frozen(n), kd_frozen(n); for (size_t i = 0; i < n; ++i) { const auto& joint = snap.model->joints[i]; if (joint.type == eJointType::FIXED) { continue; } @@ -336,7 +336,7 @@ namespace robots { } // Compute RNEA torques for feedforward control - mathlib::VecX tau_rnea = SpatialDynamics::RNEA( + mathlib::VecX_T tau_rnea = SpatialDynamics::RNEA( _spatialModel, q, qd, qdd, _dynScratch @@ -344,15 +344,17 @@ namespace robots { LOG_INFO_ONCE("tau_rnea size = %lld", (long long)tau_rnea.size()); // Define the derivative function for integration, capturing necessary variables by reference - auto f_deriv = [&, kp_frozen, kd_frozen](double t, const mathlib::VecX& xIn) { - return _dynamics->derivative_spatial(_spatialModel, + auto f_deriv = [&, kp_frozen, kd_frozen](double t, const mathlib::VecX_T& xIn) { + return _dynamics->derivative_spatial( + _spatialModel, t, xIn, snap, _dynScratch, _dynResult ); }; // Define the Jacobian function for integration, capturing necessary variables by reference - auto f_J = [&, kp_frozen, kd_frozen](const mathlib::VecX& xIn, mathlib::MatX& J_out) { - _dynamics->jacobian_spatial(_spatialModel, + auto f_J = [&, kp_frozen, kd_frozen](const mathlib::VecX_T& xIn, mathlib::MatX_T& J_out) { + _dynamics->jacobian_spatial( + _spatialModel, xIn, snap, kp_frozen, kd_frozen, J_out, _dynScratch @@ -364,32 +366,32 @@ namespace robots { unpackState(step.x_next); _dynamics->setDt(step.dt_taken); - Eigen::Map q_next(step.x_next.data(), n); - Eigen::Map qd_next(step.x_next.data() + n, n); + Eigen::Map> q_next(step.x_next.data(), n); + Eigen::Map> qd_next(step.x_next.data() + n, n); // Enforce joint limits for (auto& j : _robot.joints) { enforceJointLimits(j); } // Recompute kinematics and dynamics at the new state for logging and control purposes - std::vector T_world(snap.model->links.size()); - _kinematics->computeForwardKinematics_fromState(*snap.model, step.x_next, T_world); - std::vector jointWorldPoses = _kinematics->calcJointWorldPoses(T_world, *snap.model); + std::vector> T_world(snap.model->links.size()); + _kinematics->computeForwardKinematics_fromState(*snap.model, step.x_next, T_world); + std::vector> jointWorldPoses = _kinematics->calcJointWorldPoses(T_world, *snap.model); // Compute spatial kinematics and bias terms for the new state - SpatialDynamics::computeSpatialKinematicsAndBias( + SpatialDynamics::computeSpatialKinematicsAndBias( _spatialModel, q_next, qd_next, _dynScratch.spatial.Xup, _dynScratch.spatial.v, _dynScratch.spatial.c ); // Compute mass matrix at the new state - MatX M_full = SpatialDynamics::CRBA( + mathlib::MatX_T M_full = SpatialDynamics::CRBA( _spatialModel, _dynScratch.spatial.Xup, _dynScratch ); - VecX tau_g = _dynamics->computeGravityTorque(*snap.model, T_world, jointWorldPoses); + mathlib::VecX_T tau_g = _dynamics->computeGravityTorque(*snap.model, T_world, jointWorldPoses); // Compute system kinetic energy: E_kin = 0.5 * qd^T * M(q) * qd double sys_KE = 0.5 * qd_next.transpose() * M_full * qd_next; // [J], kinetic energy of the robot at configuration q and velocity qd diff --git a/DSFE_App/DSFE_Unit_Tests/src/AdaptiveMethodTests.cpp b/DSFE_App/DSFE_Unit_Tests/src/AdaptiveMethodTests.cpp index 065730eb..4ab8689b 100644 --- a/DSFE_App/DSFE_Unit_Tests/src/AdaptiveMethodTests.cpp +++ b/DSFE_App/DSFE_Unit_Tests/src/AdaptiveMethodTests.cpp @@ -22,18 +22,18 @@ namespace { }; // Helper function to integrate from t=0 to t=T using RK45 with adaptive step size - VecX integrateRK45(integration::ODE& ode, const VecX& x0, double T, std::function f, double rtol, double atol) { + VecX integrateRK45(integration::NumericalIntegrator& integrator, const VecX& x0, double T, std::function f, double rtol, double atol) { VecX x = x0; double t = 0.0, dt = T / 10.0; for (int s = 0; t < T && s < 10000; ++s) { double h = std::min(dt, T - t), dt_used = 0.0; - x = ode.rk45Step(x, t, h, dt_used, f, rtol, atol); + x = integrator.rk45Step(x, t, h, dt_used, f, rtol, atol); t += dt_used; } return x; } - integration::ODE ode; + integration::NumericalIntegrator integrator; } // RK45 (Dormand-Prince) Tests @@ -41,21 +41,21 @@ namespace { TEST("RK45 Adaptive Step-Size", RK45_ExponentialDecay_TightTolerance) { VecX x0(1); x0 << 1.0; - VecX r = integrateRK45(ode, x0, 1.0, rk45ExpDecay, 1e-8, 1e-10); + VecX r = integrateRK45(integrator, x0, 1.0, rk45ExpDecay, 1e-8, 1e-10); ASSERT_TRUE(std::abs(r(0) - std::exp(-1.0)) < 1e-6, "RK45 tight-tol exponential decay error too large"); } // Exponential Decay Test with Loose Tolerance TEST("RK45 Adaptive Step-Size", RK45_ExponentialDecay_LooseTolerance) { VecX x0(1); x0 << 1.0; - VecX r = integrateRK45(ode, x0, 1.0, rk45ExpDecay, 1e-3, 1e-6); + VecX r = integrateRK45(integrator, x0, 1.0, rk45ExpDecay, 1e-3, 1e-6); ASSERT_TRUE(std::abs(r(0) - std::exp(-1.0)) < 1e-2, "RK45 loose-tol exponential decay error too large"); } // Simple Harmonic Oscillator Test TEST("RK45 Adaptive Step-Size", RK45_HarmonicOscillator) { VecX x0(2); x0 << 1.0, 0.0; - VecX r = integrateRK45(ode, x0, 2.0, rk45HarmonicOsc, 1e-8, 1e-10); + VecX r = integrateRK45(integrator, x0, 2.0, rk45HarmonicOsc, 1e-8, 1e-10); ASSERT_TRUE(std::abs(r(0) - std::cos(2.0)) < 1e-6, "RK45 SHO position error too large"); ASSERT_TRUE(std::abs(r(1) + std::sin(2.0)) < 1e-6, "RK45 SHO velocity error too large"); } @@ -63,6 +63,6 @@ TEST("RK45 Adaptive Step-Size", RK45_HarmonicOscillator) TEST("RK45 Adaptive Step-Size", RK45_LongerIntegration) { VecX x0(1); x0 << 1.0; - VecX r = integrateRK45(ode, x0, 5.0, rk45ExpDecay, 1e-6, 1e-9); + VecX r = integrateRK45(integrator, x0, 5.0, rk45ExpDecay, 1e-6, 1e-9); ASSERT_TRUE(std::abs(r(0) - std::exp(-5.0)) < 1e-5, "RK45 longer integration error too large"); } \ No newline at end of file diff --git a/DSFE_App/DSFE_Unit_Tests/src/ConvergenceOrderTests.cpp b/DSFE_App/DSFE_Unit_Tests/src/ConvergenceOrderTests.cpp index 3be75d6a..3b4b6e27 100644 --- a/DSFE_App/DSFE_Unit_Tests/src/ConvergenceOrderTests.cpp +++ b/DSFE_App/DSFE_Unit_Tests/src/ConvergenceOrderTests.cpp @@ -43,62 +43,62 @@ namespace { test::assertTrue(std::abs(observed - expected) < tol, fullMsg.c_str()); } - integration::ODE ode; + integration::NumericalIntegrator integrator; } // Forward Euler Convergence Order Test TEST("Explicit Euler Convergence Order", Euler_Order1) { verifyOrder([](const VecX& x, double t, double dt) { - return ode.eulerStep(x, t, dt, convExpDecay); + return integrator.eulerStep(x, t, dt, convExpDecay); }, 1.0, 500, 1.0, 0.3, "Euler should be order 1"); } // Midpoint (RK2) Convergence Order Test TEST("Explicit Midpoint Convergence Order", Midpoint_Order2) { verifyOrder([](const VecX& x, double t, double dt) { - return ode.midpointStep(x, t, dt, convExpDecay); + return integrator.midpointStep(x, t, dt, convExpDecay); }, 1.0, 200, 2.0, 0.3, "Midpoint should be order 2"); } // Heun (RK2) Convergence Order Test TEST("Heun Convergence Order", Heun_Order2) { verifyOrder([](const VecX& x, double t, double dt) { - return ode.heunStep(x, t, dt, convExpDecay); + return integrator.heunStep(x, t, dt, convExpDecay); }, 1.0, 200, 2.0, 0.3, "Heun should be order 2"); } // Ralston (RK2) Convergence Order Test TEST("Ralston Convergence Order", Ralston_Order2) { verifyOrder([](const VecX& x, double t, double dt) { - return ode.ralstonStep(x, t, dt, convExpDecay); + return integrator.ralstonStep(x, t, dt, convExpDecay); }, 1.0, 200, 2.0, 0.3, "Ralston should be order 2"); } // RK4 Convergence Order Test TEST("RK4 Convergence Order", RK4_Order4) { verifyOrder([](const VecX& x, double t, double dt) { - return ode.rk4Step(x, t, dt, convExpDecay); + return integrator.rk4Step(x, t, dt, convExpDecay); }, 1.0, 50, 4.0, 0.3, "RK4 should be order 4"); } // Implicit Euler Convergence Order Test TEST("Implicit Euler Convergence Order", ImplicitEuler_Order1) { verifyOrder([](const VecX& x, double t, double dt) { - return ode.implicit_euler(x, t, dt, convExpDecay); + return integrator.implicitEuler(x, t, dt, convExpDecay); }, 1.0, 500, 1.0, 0.3, "Implicit Euler should be order 1"); } // Implicit Midpoint Convergence Order Test TEST("Implicit Midpoint Convergence Order", ImplicitMidpoint_Order2) { verifyOrder([](const VecX& x, double t, double dt) { - return ode.implicit_midpoint(x, t, dt, convExpDecay); + return integrator.implicitMidpoint(x, t, dt, convExpDecay); }, 1.0, 200, 2.0, 0.3, "Implicit Midpoint should be order 2"); } // GLRK2 Convergence Order Test TEST("GLRK2 Convergence Order", GLRK2_Order4) { verifyOrder([](const VecX& x, double t, double dt) { - return ode.GLRK2(x, t, dt, convExpDecay, 50, 1e-10, convExpDecayJac); + return integrator.GLRK2(x, t, dt, convExpDecay, convExpDecayJac, 50, 1e-10); }, 1.0, 50, 4.0, 0.3, "GLRK2 should be order 4"); } // GLRK3 Convergence Order Test TEST("GLRK3 Convergence Order", GLRK3_Order6) { verifyOrder([](const VecX& x, double t, double dt) { - return ode.GLRK3(x, t, dt, convExpDecay, 150, 1e-16, convExpDecayJac); + return integrator.GLRK3(x, t, dt, convExpDecay, convExpDecayJac, 150, 1e-16); }, 1.0, 25, 6.0, 0.3, "GLRK3 should be order 6"); } \ No newline at end of file diff --git a/DSFE_App/DSFE_Unit_Tests/src/ExplicitMethodTests.cpp b/DSFE_App/DSFE_Unit_Tests/src/ExplicitMethodTests.cpp index 8e698c73..7a02dd08 100644 --- a/DSFE_App/DSFE_Unit_Tests/src/ExplicitMethodTests.cpp +++ b/DSFE_App/DSFE_Unit_Tests/src/ExplicitMethodTests.cpp @@ -31,7 +31,7 @@ namespace { return x; } - integration::ODE ode; + integration::NumericalIntegrator integrator; } // Forward Euler Tests @@ -39,14 +39,14 @@ namespace { TEST("Euler Method", Euler_ExponentialDecay_SmallStep) { VecX x0(1); x0 << 1.0; - VecX r = integrateToTime([](const VecX& x, double t, double dt) { return ode.eulerStep(x, t, dt, expDecay); }, x0, 1.0, 1000); + VecX r = integrateToTime([](const VecX& x, double t, double dt) { return integrator.eulerStep(x, t, dt, expDecay); }, x0, 1.0, 1000); ASSERT_TRUE(std::abs(r(0) - std::exp(-1.0)) < 1e-2, "Euler exponential decay error too large"); } // Simple Harmonic Oscillator Test TEST("Euler Method", Euler_HarmonicOscillator_SmallStep) { VecX x0(2); x0 << 1.0, 0.0; - VecX r = integrateToTime([](const VecX& x, double t, double dt) { return ode.eulerStep(x, t, dt, harmonicOsc); }, x0, 2.0, 2000); + VecX r = integrateToTime([](const VecX& x, double t, double dt) { return integrator.eulerStep(x, t, dt, harmonicOsc); }, x0, 2.0, 2000); ASSERT_TRUE(std::abs(r(0) - std::cos(2.0)) < 5e-2, "Euler SHO position error too large"); ASSERT_TRUE(std::abs(r(1) + std::sin(2.0)) < 5e-2, "Euler SHO velocity error too large"); } @@ -56,14 +56,14 @@ TEST("Euler Method", Euler_HarmonicOscillator_SmallStep) TEST("Midpoint Method (RK2)", Midpoint_ExponentialDecay) { VecX x0(1); x0 << 1.0; - VecX r = integrateToTime([](const VecX& x, double t, double dt) { return ode.midpointStep(x, t, dt, expDecay); }, x0, 1.0, 500); + VecX r = integrateToTime([](const VecX& x, double t, double dt) { return integrator.midpointStep(x, t, dt, expDecay); }, x0, 1.0, 500); ASSERT_TRUE(std::abs(r(0) - std::exp(-1.0)) < 1e-5, "Midpoint exponential decay error too large"); } // Simple Harmonic Oscillator Test TEST("Midpoint Method (RK2)", Midpoint_HarmonicOscillator) { VecX x0(2); x0 << 1.0, 0.0; - VecX r = integrateToTime([](const VecX& x, double t, double dt) { return ode.midpointStep(x, t, dt, harmonicOsc); }, x0, 2.0, 1000); + VecX r = integrateToTime([](const VecX& x, double t, double dt) { return integrator.midpointStep(x, t, dt, harmonicOsc); }, x0, 2.0, 1000); ASSERT_TRUE(std::abs(r(0) - std::cos(2.0)) < 1e-4, "Midpoint SHO position error too large"); ASSERT_TRUE(std::abs(r(1) + std::sin(2.0)) < 1e-4, "Midpoint SHO velocity error too large"); } @@ -73,14 +73,14 @@ TEST("Midpoint Method (RK2)", Midpoint_HarmonicOscillator) TEST("Heun Method (RK2)", Heun_ExponentialDecay) { VecX x0(1); x0 << 1.0; - VecX r = integrateToTime([](const VecX& x, double t, double dt) { return ode.heunStep(x, t, dt, expDecay); }, x0, 1.0, 500); + VecX r = integrateToTime([](const VecX& x, double t, double dt) { return integrator.heunStep(x, t, dt, expDecay); }, x0, 1.0, 500); ASSERT_TRUE(std::abs(r(0) - std::exp(-1.0)) < 1e-5, "Heun exponential decay error too large"); } // Simple Harmonic Oscillator Test TEST("Heun Method (RK2)", Heun_HarmonicOscillator) { VecX x0(2); x0 << 1.0, 0.0; - VecX r = integrateToTime([](const VecX& x, double t, double dt) { return ode.heunStep(x, t, dt, harmonicOsc); }, x0, 2.0, 1000); + VecX r = integrateToTime([](const VecX& x, double t, double dt) { return integrator.heunStep(x, t, dt, harmonicOsc); }, x0, 2.0, 1000); ASSERT_TRUE(std::abs(r(0) - std::cos(2.0)) < 1e-4, "Heun SHO position error too large"); ASSERT_TRUE(std::abs(r(1) + std::sin(2.0)) < 1e-4, "Heun SHO velocity error too large"); } @@ -90,14 +90,14 @@ TEST("Heun Method (RK2)", Heun_HarmonicOscillator) TEST("Ralston Method (RK2)", Ralston_ExponentialDecay) { VecX x0(1); x0 << 1.0; - VecX r = integrateToTime([](const VecX& x, double t, double dt) { return ode.ralstonStep(x, t, dt, expDecay); }, x0, 1.0, 500); + VecX r = integrateToTime([](const VecX& x, double t, double dt) { return integrator.ralstonStep(x, t, dt, expDecay); }, x0, 1.0, 500); ASSERT_TRUE(std::abs(r(0) - std::exp(-1.0)) < 1e-5, "Ralston exponential decay error too large"); } // Simple Harmonic Oscillator Test TEST("Ralston Method (RK2)", Ralston_HarmonicOscillator) { VecX x0(2); x0 << 1.0, 0.0; - VecX r = integrateToTime([](const VecX& x, double t, double dt) { return ode.ralstonStep(x, t, dt, harmonicOsc); }, x0, 2.0, 1000); + VecX r = integrateToTime([](const VecX& x, double t, double dt) { return integrator.ralstonStep(x, t, dt, harmonicOsc); }, x0, 2.0, 1000); ASSERT_TRUE(std::abs(r(0) - std::cos(2.0)) < 1e-4, "Ralston SHO position error too large"); ASSERT_TRUE(std::abs(r(1) + std::sin(2.0)) < 1e-4, "Ralston SHO velocity error too large"); } @@ -107,14 +107,14 @@ TEST("Ralston Method (RK2)", Ralston_HarmonicOscillator) TEST("RK4 Method", RK4_ExponentialDecay) { VecX x0(1); x0 << 1.0; - VecX r = integrateToTime([](const VecX& x, double t, double dt) { return ode.rk4Step(x, t, dt, expDecay); }, x0, 1.0, 100); + VecX r = integrateToTime([](const VecX& x, double t, double dt) { return integrator.rk4Step(x, t, dt, expDecay); }, x0, 1.0, 100); ASSERT_TRUE(std::abs(r(0) - std::exp(-1.0)) < 1e-9, "RK4 exponential decay error too large"); } // Simple Harmonic Oscillator Test TEST("RK4 Method", RK4_HarmonicOscillator) { VecX x0(2); x0 << 1.0, 0.0; - VecX r = integrateToTime([](const VecX& x, double t, double dt) { return ode.rk4Step(x, t, dt, harmonicOsc); }, x0, 2.0, 200); + VecX r = integrateToTime([](const VecX& x, double t, double dt) { return integrator.rk4Step(x, t, dt, harmonicOsc); }, x0, 2.0, 200); ASSERT_TRUE(std::abs(r(0) - std::cos(2.0)) < 1e-9, "RK4 SHO position error too large"); ASSERT_TRUE(std::abs(r(1) + std::sin(2.0)) < 1e-9, "RK4 SHO velocity error too large"); } @@ -122,6 +122,6 @@ TEST("RK4 Method", RK4_HarmonicOscillator) TEST("RK4 Method", RK4_ExponentialDecay_LargeStep) { VecX x0(1); x0 << 1.0; - VecX r = integrateToTime([](const VecX& x, double t, double dt) { return ode.rk4Step(x, t, dt, expDecay); }, x0, 1.0, 10); + VecX r = integrateToTime([](const VecX& x, double t, double dt) { return integrator.rk4Step(x, t, dt, expDecay); }, x0, 1.0, 10); ASSERT_TRUE(std::abs(r(0) - std::exp(-1.0)) < 1e-5, "RK4 large-step exponential decay error too large"); } \ No newline at end of file diff --git a/DSFE_App/DSFE_Unit_Tests/src/ImplicitMethodTests.cpp b/DSFE_App/DSFE_Unit_Tests/src/ImplicitMethodTests.cpp index b5bf4e3b..9194461d 100644 --- a/DSFE_App/DSFE_Unit_Tests/src/ImplicitMethodTests.cpp +++ b/DSFE_App/DSFE_Unit_Tests/src/ImplicitMethodTests.cpp @@ -52,7 +52,7 @@ namespace { F_out(0, 0) = -100.0; }; - integration::ODE ode; + integration::NumericalIntegrator integrator; } // Backward Euler Tests @@ -60,14 +60,14 @@ namespace { TEST("Implicit Euler Method", ImplicitEuler_ExponentialDecay) { VecX x(1); x << 1.0; double dt = 1.0 / 1000, t = 0.0; - for (int i = 0; i < 1000; ++i) { x = ode.implicit_euler(x, t, dt, impExpDecay); t += dt; } + for (int i = 0; i < 1000; ++i) { x = integrator.implicitEuler(x, t, dt, impExpDecay, impExpDecayJac); t += dt; } ASSERT_TRUE(std::abs(x(0) - std::exp(-1.0)) < 1e-2, "Backward Euler exponential decay error too large"); } // Linear Decay Test TEST("Implicit Euler Method", ImplicitEuler_LinearDecay) { VecX x(1); x << 1.0; double T = 0.5, dt = T / 500, t = 0.0; - for (int i = 0; i < 500; ++i) { x = ode.implicit_euler(x, t, dt, linearDecay); t += dt; } + for (int i = 0; i < 500; ++i) { x = integrator.implicitEuler(x, t, dt, linearDecay, linearDecayJac); t += dt; } ASSERT_TRUE(std::abs(x(0) - std::exp(-1.0)) < 1e-2, "Backward Euler linear decay error too large"); } // Stability Test with Large Time Step @@ -76,7 +76,7 @@ TEST("Implicit Euler Method", ImplicitEuler_Stability_LargeStep) { double T = 2.0, dt = T / 20, t = 0.0; double prev = x(0); for (int i = 0; i < 20; ++i) { - x = ode.implicit_euler(x, t, dt, impExpDecay); + x = integrator.implicitEuler(x, t, dt, impExpDecay, impExpDecayJac); t += dt; ASSERT_TRUE(x(0) < prev * (1.0 + 1e-12), "Not monotone"); ASSERT_TRUE(x(0) < prev, "Not strictly decaying"); @@ -89,7 +89,7 @@ TEST("Implicit Euler Method", ImplicitEuler_Stability_LargeStep) { TEST("Implicit Midpoint Method", ImplicitMidpoint_ExponentialDecay) { VecX x(1); x << 1.0; double dt = 1.0 / 1000, t = 0.0; - for (int i = 0; i < 1000; ++i) { x = ode.implicit_midpoint(x, t, dt, impExpDecay); t += dt; } + for (int i = 0; i < 1000; ++i) { x = integrator.implicitMidpoint(x, t, dt, impExpDecay, impExpDecayJac); t += dt; } ASSERT_TRUE(std::abs(x(0) - std::exp(-1.0)) < 1e-2, "Implicit Midpoint exponential decay error too large"); } // Energy Preservation Test for Simple Harmonic Oscillator @@ -97,7 +97,7 @@ TEST("Implicit Midpoint Method", ImplicitMidpoint_HarmonicOscillator_EnergyPrese VecX x(2); x << 1.0, 0.0; double E0 = 0.5 * (x(0)*x(0) + x(1)*x(1)); double dt = 5.0 / 2500, t = 0.0; - for (int i = 0; i < 2500; ++i) { x = ode.implicit_midpoint(x, t, dt, impHarmonicOsc); t += dt; } + for (int i = 0; i < 2500; ++i) { x = integrator.implicitMidpoint(x, t, dt, impHarmonicOsc, impExpDecayJac); t += dt; } double Ef = 0.5 * (x(0)*x(0) + x(1)*x(1)); double drift = std::abs(Ef - E0); ASSERT_TRUE(drift < tol_high * (1.0 + E0), "Implicit Midpoint energy drift too large for SHO"); @@ -108,7 +108,7 @@ TEST("Implicit Midpoint Method", ImplicitMidpoint_Stability_LargeStep) { double T = 2.0, dt = T / 20, t = 0.0; double prev = x(0); for (int i = 0; i < 20; ++i) { - x = ode.implicit_midpoint(x, t, dt, impExpDecay); + x = integrator.implicitMidpoint(x, t, dt, impExpDecay, impExpDecayJac); t += dt; ASSERT_TRUE(x(0) < prev * (1.0 + 1e-12), "Not monotone"); ASSERT_TRUE(x(0) < prev, "Not strictly decaying"); @@ -123,7 +123,7 @@ TEST("GLRK2 Method", GLRK2_HarmonicOscillator_EnergyPreservation) { double E0 = 0.5 * (x(0) * x(0) + x(1) * x(1)); double T = 5.0, dt = T / 2500, t = 0.0; for (int i = 0; i < 2500; ++i) { - x = ode.GLRK2(x, t, dt, impHarmonicOsc, 50, 1e-6, impHarmonicOscJac); + x = integrator.GLRK2(x, t, dt, impHarmonicOsc, impHarmonicOscJac, 50, 1e-6); t += dt; } double Ef = 0.5 * (x(0) * x(0) + x(1) * x(1)); @@ -135,7 +135,7 @@ TEST("GLRK2 Method", GLRK2_ExponentialDecay) { VecX x(1); x << 1.0; double T = 1.0, dt = T / 1000, t = 0.0; for (int i = 0; i < 1000; ++i) { - x = ode.GLRK2(x, t, dt, impExpDecay, 50, 1e-6, impExpDecayJac); + x = integrator.GLRK2(x, t, dt, impExpDecay, impExpDecayJac, 50, 1e-6); t += dt; } ASSERT_TRUE(std::abs(x(0) - std::exp(-1.0)) < tol_low, "GLRK2 exponential decay error too large"); @@ -146,7 +146,7 @@ TEST("GLRK2 Method", GLRK2_Stability_LargeStep) { double T = 2.0, dt = T / 20, t = 0.0; double prev = x(0); for (int i = 0; i < 20; ++i) { - x = ode.GLRK2(x, t, dt, impExpDecay, 50, 1e-6, impExpDecayJac); + x = integrator.GLRK2(x, t, dt, impExpDecay, impExpDecayJac, 50, 1e-6); t += dt; ASSERT_TRUE(x(0) < prev * (1.0 + 1e-12), "Not monotone"); ASSERT_TRUE(x(0) < prev, "Not strictly decaying"); @@ -158,7 +158,7 @@ TEST("GLRK2 Method", GLRK2_LinearDecay) { VecX x(1); x << 1.0; double T = 0.5, dt = T / 500, t = 0.0; for (int i = 0; i < 500; ++i) { - x = ode.GLRK2(x, t, dt, linearDecay, 50, 1e-6, linearDecayJac); + x = integrator.GLRK2(x, t, dt, linearDecay, linearDecayJac, 50, 1e-6); t += dt; } ASSERT_TRUE(std::abs(x(0) - std::exp(-1.0)) < tol_low, "GLRK2 linear decay error too large"); @@ -171,7 +171,7 @@ TEST("GLRK2 Method", GLRK2_StiffDecay) { auto stiff_f = [](double, const VecX& x) { return -100.0 * x; }; for (int i = 0; i < 500; ++i) { - x = ode.GLRK2(x, t, dt, stiff_f, 50, 1e-10, stiffDecayJac); + x = integrator.GLRK2(x, t, dt, stiff_f, stiffDecayJac, 50, 1e-10); t += dt; } ASSERT_TRUE(std::isfinite(x(0)), "GLRK2 produced a non-finite result."); @@ -191,7 +191,7 @@ TEST("GLRK2 Method", GLRK2_NonlinearDecay) { }; for (int i = 0; i < 500; ++i) { - x = ode.GLRK2(x, t, dt, nl_f, 50, 1e-6, nl_jac); + x = integrator.GLRK2(x, t, dt, nl_f, nl_jac, 50, 1e-6); t += dt; } ASSERT_TRUE(std::abs(x(0) - expected) < tol_low, "GLRK2 nonlinear decay error too large"); @@ -208,7 +208,7 @@ TEST("GLRK2 Method", GLRK2_StiffNonlinearDecay) { }; for (int i = 0; i < 500; ++i) { - x = ode.GLRK2(x, t, dt, stiff_nl_f, 50, 1e-6, stiff_nl_jac); + x = integrator.GLRK2(x, t, dt, stiff_nl_f, stiff_nl_jac, 50, 1e-6); t += dt; } ASSERT_TRUE(std::abs(x(0) - (1.0 / 51.0)) < tol_low, "GLRK2 stiff nonlinear decay error too large"); @@ -221,7 +221,7 @@ TEST("GLRK3 Method", GLRK3_HarmonicOscillator_EnergyPreservation) { double E0 = 0.5 * (x(0) * x(0) + x(1) * x(1)); double T = 5.0, dt = T / 2500, t = 0.0; for (int i = 0; i < 2500; ++i) { - x = ode.GLRK3(x, t, dt, impHarmonicOsc, 80, 1e-7, impHarmonicOscJac); + x = integrator.GLRK3(x, t, dt, impHarmonicOsc, impHarmonicOscJac, 80, 1e-7); t += dt; } double Ef = 0.5 * (x(0) * x(0) + x(1) * x(1)); @@ -233,7 +233,7 @@ TEST("GLRK3 Method", GLRK3_ExponentialDecay) { VecX x(1); x << 1.0; double T = 1.0, dt = T / 1000, t = 0.0; for (int i = 0; i < 1000; ++i) { - x = ode.GLRK3(x, t, dt, impExpDecay, 80, 1e-7, impExpDecayJac); + x = integrator.GLRK3(x, t, dt, impExpDecay, impExpDecayJac, 80, 1e-7); t += dt; } ASSERT_TRUE(std::abs(x(0) - std::exp(-1.0)) < tol_low, "GLRK3 exponential decay error too large"); @@ -244,7 +244,7 @@ TEST("GLRK3 Method", GLRK3_Stability_LargeStep) { double T = 2.0, dt = T / 20, t = 0.0; double prev = x(0); for (int i = 0; i < 20; ++i) { - x = ode.GLRK3(x, t, dt, impExpDecay, 80, 1e-7, impExpDecayJac); + x = integrator.GLRK3(x, t, dt, impExpDecay, impExpDecayJac, 80, 1e-7); t += dt; ASSERT_TRUE(x(0) < prev * (1.0 + 1e-12), "Not monotone"); ASSERT_TRUE(x(0) < prev, "Not strictly decaying"); @@ -256,7 +256,7 @@ TEST("GLRK3 Method", GLRK3_LinearDecay) { VecX x(1); x << 1.0; double T = 0.5, dt = T / 500, t = 0.0; for (int i = 0; i < 500; ++i) { - x = ode.GLRK3(x, t, dt, linearDecay, 80, 1e-7, linearDecayJac); + x = integrator.GLRK3(x, t, dt, linearDecay, linearDecayJac, 80, 1e-7); t += dt; } ASSERT_TRUE(std::abs(x(0) - std::exp(-1.0)) < tol_low, "GLRK3 linear decay error too large"); @@ -269,7 +269,7 @@ TEST("GLRK3 Method", GLRK3_StiffDecay) { auto stiff_f = [](double, const VecX& x) { return -100.0 * x; }; for (int i = 0; i < 500; ++i) { - x = ode.GLRK3(x, t, dt, stiff_f, 150, 1e-12, stiffDecayJac); + x = integrator.GLRK3(x, t, dt, stiff_f, stiffDecayJac, 150, 1e-12); t += dt; } ASSERT_TRUE(std::isfinite(x(0)), "GLRK3 produced a non-finite result."); @@ -290,7 +290,7 @@ TEST("GLRK3 Method", GLRK3_NonlinearDecay) { }; for (int i = 0; i < 500; ++i) { - x = ode.GLRK3(x, t, dt, nl_f, 80, 1e-7, nl_jac); + x = integrator.GLRK3(x, t, dt, nl_f, nl_jac, 80, 1e-7); t += dt; } double rel_err = std::abs(x(0) - expected) / expected; @@ -310,7 +310,7 @@ TEST("GLRK3 Method", GLRK3_StiffNonlinearDecay) { }; for (int i = 0; i < 500; ++i) { - x = ode.GLRK3(x, t, dt, stiff_nl_f, 80, 1e-7, stiff_nl_jac); + x = integrator.GLRK3(x, t, dt, stiff_nl_f, stiff_nl_jac, 80, 1e-7); t += dt; } double rel_err = std::abs(x(0) - expected) / expected; diff --git a/PxMLib/MathLib/include/core/SpatialMath.h b/PxMLib/MathLib/include/core/SpatialMath.h index b21aafe6..5a223065 100644 --- a/PxMLib/MathLib/include/core/SpatialMath.h +++ b/PxMLib/MathLib/include/core/SpatialMath.h @@ -151,6 +151,16 @@ namespace mathlib { return out; } + template + inline SpatialVec_T operator*( + const Eigen::MatrixBase& lhs, + const SpatialVec_T& rhs + ) { + SpatialVec_T out; + out.v = lhs * rhs.v; + return out; + } + // SpatialVec1 * SpatialVec2 (outer product) template inline SpatialMat_T outer( diff --git a/PxMLib/MathLib/include/integrators/numerical_integrators.inl b/PxMLib/MathLib/include/integrators/numerical_integrators.inl index 48dacb4b..09325b0f 100644 --- a/PxMLib/MathLib/include/integrators/numerical_integrators.inl +++ b/PxMLib/MathLib/include/integrators/numerical_integrators.inl @@ -192,17 +192,17 @@ namespace integration { Scalar t, Scalar dt, Func&& f, - JacFunc&& jac = nullptr, - int maxIter = 8, - Scalar tol = 1e-6 + JacFunc&& jac, + int maxIter, + Scalar tol ) { mathlib::VecX_T x_new = x + dt * f(t, x); // Initial guess // The function g(x_guess) = 0 that we want to solve for the implicit Euler step - auto g = [&](const mathlib::VecX_T& x_guess) { return x_guess - x - dt * f(t + dt, x_guess); }; + auto g = [&](const mathlib::VecX_T& x_guess, mathlib::VecX_T& g_out) { g_out = x_guess - x - dt * f(t + dt, x_guess); }; // Numerical Jacobian for Newton-Raphson - auto J = [&](const mathlib::VecX_T& x_guess) -> mathlib::MatX_T { + auto J = [&](const mathlib::VecX_T& x_guess, mathlib::MatX_T& J_out) { int n = (int)x_guess.size(); mathlib::MatX_T F; @@ -228,10 +228,11 @@ namespace integration { ); } - return mathlib::MatX_T::Identity(n, n) - dt * F; // J = I - dt * df/dx + J_out = mathlib::MatX_T::Identity(n, n) - dt * F; // J = I - dt * df/dx }; - return newtonRaphson(g, J, x + dt * f(t + dt, x), maxIter, tol); + mathlib::VecX_T x0 = x + dt * f(t + dt, x); // Initial guess for Newton-Raphson + return newtonRaphson(g, J, x0, maxIter, tol); } // Implicit Midpoint method @@ -241,17 +242,17 @@ namespace integration { Scalar t, Scalar dt, Func&& f, - JacFunc&& jac = nullptr, - int maxIter = 10, - Scalar tol = 1e-7 + JacFunc&& jac, + int maxIter, + Scalar tol ) { mathlib::VecX_T x_new = x + dt * f((t + dt) / Scalar(2), x); // Initial guess // The function g(x_guess) = 0 that we want to solve for the implicit midpoint step - auto g = [&](const mathlib::VecX_T& x_guess) { return x_guess - x - dt * f((t + dt) / Scalar(2), (x + x_guess) / Scalar(2)); }; + auto g = [&](const mathlib::VecX_T& x_guess, mathlib::VecX_T& g_out) { g_out = x_guess - x - dt * f((t + dt) / Scalar(2), (x + x_guess) / Scalar(2)); }; // Numerical Jacobian for Newton-Raphson - auto J = [&](const mathlib::VecX_T& x_guess) -> mathlib::MatX_T { + auto J = [&](const mathlib::VecX_T& x_guess, mathlib::MatX_T& J_out) { int n = (int)x_guess.size(); mathlib::MatX_T F; @@ -277,10 +278,11 @@ namespace integration { ); } - return mathlib::MatX_T::Identity(n, n) - Scalar(0.5) * dt * F; // J = I - dt * df/dx + J_out = mathlib::MatX_T::Identity(n, n) - Scalar(0.5) * dt * F; // J = I - dt * df/dx }; - return newtonRaphson(g, J, x + dt + f(t + dt / Scalar(2), x), maxIter, tol); + mathlib::VecX_T x0 = x + dt * f((t + dt) / Scalar(2), x); // Initial guess for Newton-Raphson + return newtonRaphson(g, J, x0, maxIter, tol); } // Gauss-Legendre Runge-Kutta method (2 stages, 4th order) @@ -290,9 +292,9 @@ namespace integration { Scalar t, Scalar dt, Func&& f, - JacFunc&& jac = nullptr, - int maxIter = 50, - Scalar tol = 1e-6 + JacFunc&& jac, + int maxIter, + Scalar tol ) { size_t n = x.size(); @@ -330,7 +332,7 @@ namespace integration { g.segment(n, n) = k2 - f2; }; - auto eval_j = [&](const VecX_T& k_guess, MatX_T& J) { + auto eval_j = [&](const mathlib::VecX_T& k_guess, mathlib::MatX_T& J) { mathlib::VecX_T k1 = k_guess.segment(0, n); mathlib::VecX_T k2 = k_guess.segment(n, n); @@ -391,9 +393,9 @@ namespace integration { Scalar t, Scalar dt, Func&& f, - JacFunc&& jac = nullptr, - int maxIter = 150, - Scalar tol = -1 + JacFunc&& jac, + int maxIter, + Scalar tol ) { if (tol < 0.0) { tol = std::max(1e-12, 1e-2 * std::pow(dt, 7.0)); } @@ -423,7 +425,7 @@ namespace integration { k.segment(n, n) = f0; k.segment(2 * n, n) = f0; - auto eval_g = [&](const VecX_T& k_guess, VecX_T& g) { + auto eval_g = [&](const mathlib::VecX_T& k_guess, mathlib::VecX_T& g) { mathlib::VecX_T k1 = k_guess.segment(0, n); mathlib::VecX_T k2 = k_guess.segment(n, n); mathlib::VecX_T k3 = k_guess.segment(2 * n, n); @@ -443,7 +445,7 @@ namespace integration { g.segment(2 * n, n) = k3 - f3; }; - auto eval_j = [&](const VecX_T& k_guess, MatX_T& J) { + auto eval_j = [&](const mathlib::VecX_T& k_guess, mathlib::MatX_T& J) { mathlib::VecX_T k1 = k_guess.segment(0, n); mathlib::VecX_T k2 = k_guess.segment(n, n); mathlib::VecX_T k3 = k_guess.segment(2 * n, n); From 0f965b96ce47fd350b0f87faf59daa8267718ab6 Mon Sep 17 00:00:00 2001 From: saltyjoss Date: Sat, 16 May 2026 20:07:41 +0100 Subject: [PATCH 009/210] fixes: Further fixes from templating relevant files --- .../include/Robots/SpatialDynamics.inl | 13 +++++---- .../src/ConvergenceOrderTests.cpp | 2 +- .../src/ImplicitMethodTests.cpp | 2 +- PxMLib/MathLib/include/core/SpatialMath.h | 2 +- .../integrators/numerical_integrators.inl | 28 +++++++------------ 5 files changed, 21 insertions(+), 26 deletions(-) diff --git a/DSFE_App/DSFE_Core/include/Robots/SpatialDynamics.inl b/DSFE_App/DSFE_Core/include/Robots/SpatialDynamics.inl index ab138683..db58c452 100644 --- a/DSFE_App/DSFE_Core/include/Robots/SpatialDynamics.inl +++ b/DSFE_App/DSFE_Core/include/Robots/SpatialDynamics.inl @@ -152,7 +152,8 @@ namespace robots { if (j.type == eJointType::FIXED) { continue; } int p = j.parent; if (p >= 0) { - Ic[p] += Xup[i].transpose() * Ic[i] * Xup[i]; + mathlib::MatX_T XupT = Xup[i].transpose(); + Ic[p] += XupT * Ic[i] * Xup[i]; } } @@ -207,8 +208,9 @@ namespace robots { if (j.type == eJointType::FIXED) { Ia_out[i] = IA_out[i]; if (j.parent >= 0) { - IA_out[j.parent] += Xup[i].transpose() * Ia_out[i] * Xup[i]; - pA_out[j.parent] += Xup[i].transpose() * pA_out[i]; + mathlib::SpatialMat_T XupT = Xup[i].transpose(); + IA_out[j.parent] += XupT * Ia_out[i] * Xup[i]; + pA_out[j.parent] += XupT * pA_out[i]; } continue; } @@ -224,8 +226,9 @@ namespace robots { pA_out[i] += Ia_out[i] * c[i] + U_out[i] * (u_out[i] / d_out[i]); if (j.parent >= 0) { - IA_out[j.parent] += Xup[i].transpose() * Ia_out[i] * Xup[i]; - pA_out[j.parent] += Xup[i].transpose() * pA_out[i]; + mathlib::SpatialMat_T XupT = Xup[i].transpose(); + IA_out[j.parent] += XupT * Ia_out[i] * Xup[i]; + pA_out[j.parent] += XupT * pA_out[i]; } } } diff --git a/DSFE_App/DSFE_Unit_Tests/src/ConvergenceOrderTests.cpp b/DSFE_App/DSFE_Unit_Tests/src/ConvergenceOrderTests.cpp index 3b4b6e27..766d2d3d 100644 --- a/DSFE_App/DSFE_Unit_Tests/src/ConvergenceOrderTests.cpp +++ b/DSFE_App/DSFE_Unit_Tests/src/ConvergenceOrderTests.cpp @@ -100,5 +100,5 @@ TEST("GLRK2 Convergence Order", GLRK2_Order4) { TEST("GLRK3 Convergence Order", GLRK3_Order6) { verifyOrder([](const VecX& x, double t, double dt) { return integrator.GLRK3(x, t, dt, convExpDecay, convExpDecayJac, 150, 1e-16); - }, 1.0, 25, 6.0, 0.3, "GLRK3 should be order 6"); + }, 10.0, 10, 6.0, 0.5, "GLRK3 should be order 6"); } \ No newline at end of file diff --git a/DSFE_App/DSFE_Unit_Tests/src/ImplicitMethodTests.cpp b/DSFE_App/DSFE_Unit_Tests/src/ImplicitMethodTests.cpp index 9194461d..e931b234 100644 --- a/DSFE_App/DSFE_Unit_Tests/src/ImplicitMethodTests.cpp +++ b/DSFE_App/DSFE_Unit_Tests/src/ImplicitMethodTests.cpp @@ -97,7 +97,7 @@ TEST("Implicit Midpoint Method", ImplicitMidpoint_HarmonicOscillator_EnergyPrese VecX x(2); x << 1.0, 0.0; double E0 = 0.5 * (x(0)*x(0) + x(1)*x(1)); double dt = 5.0 / 2500, t = 0.0; - for (int i = 0; i < 2500; ++i) { x = integrator.implicitMidpoint(x, t, dt, impHarmonicOsc, impExpDecayJac); t += dt; } + for (int i = 0; i < 2500; ++i) { x = integrator.implicitMidpoint(x, t, dt, impHarmonicOsc, impHarmonicOscJac); t += dt; } double Ef = 0.5 * (x(0)*x(0) + x(1)*x(1)); double drift = std::abs(Ef - E0); ASSERT_TRUE(drift < tol_high * (1.0 + E0), "Implicit Midpoint energy drift too large for SHO"); diff --git a/PxMLib/MathLib/include/core/SpatialMath.h b/PxMLib/MathLib/include/core/SpatialMath.h index 5a223065..d71f60dc 100644 --- a/PxMLib/MathLib/include/core/SpatialMath.h +++ b/PxMLib/MathLib/include/core/SpatialMath.h @@ -157,7 +157,7 @@ namespace mathlib { const SpatialVec_T& rhs ) { SpatialVec_T out; - out.v = lhs * rhs.v; + out.v.noalias() = lhs.derived() * rhs.v; return out; } diff --git a/PxMLib/MathLib/include/integrators/numerical_integrators.inl b/PxMLib/MathLib/include/integrators/numerical_integrators.inl index 09325b0f..beb4512c 100644 --- a/PxMLib/MathLib/include/integrators/numerical_integrators.inl +++ b/PxMLib/MathLib/include/integrators/numerical_integrators.inl @@ -246,38 +246,34 @@ namespace integration { int maxIter, Scalar tol ) { - mathlib::VecX_T x_new = x + dt * f((t + dt) / Scalar(2), x); // Initial guess - // The function g(x_guess) = 0 that we want to solve for the implicit midpoint step auto g = [&](const mathlib::VecX_T& x_guess, mathlib::VecX_T& g_out) { g_out = x_guess - x - dt * f((t + dt) / Scalar(2), (x + x_guess) / Scalar(2)); }; // Numerical Jacobian for Newton-Raphson auto J = [&](const mathlib::VecX_T& x_guess, mathlib::MatX_T& J_out) { int n = (int)x_guess.size(); - mathlib::MatX_T F; bool analytical_success = false; if constexpr (!std::is_same_v, std::nullptr_t>) { if constexpr (std::is_pointer_v> || requires { bool(jac); }) { if (jac) { - jac(x_guess, F); // User-provided Jacobian + jac((x + x_guess) / Scalar(2), F); // User-provided Jacobian analytical_success = true; } } else { - jac(x_guess, F); // User-provided Jacobian + jac((x + x_guess) / Scalar(2), F); // User-provided Jacobian analytical_success = true; } } if (!analytical_success) { F = finiteDifferenceJacobian( [&](Scalar, const mathlib::VecX_T& x_pert) { return f((t + dt) / Scalar(2), (x + x_pert) / Scalar(2)); }, - t + dt, + t + dt / Scalar(2), x_guess ); } - J_out = mathlib::MatX_T::Identity(n, n) - Scalar(0.5) * dt * F; // J = I - dt * df/dx }; @@ -296,7 +292,7 @@ namespace integration { int maxIter, Scalar tol ) { - size_t n = x.size(); + const Eigen::Index n = x.size(); // Coefficients for the 2-stage Gauss-Legendre method (4th order) mathlib::VecX_T c(2); @@ -304,7 +300,7 @@ namespace integration { 0.5 - std::sqrt(3.0) / 6.0, 0.5 + std::sqrt(3.0) / 6.0; // Stage time fractions - mathlib::MatX_T A = mathlib::MatX_T::Zero(); + mathlib::MatX_T A(2, 2); A << 0.25, 0.25 - std::sqrt(3.0) / 6.0, 0.25 + std::sqrt(3.0) / 6.0, 0.25; @@ -312,7 +308,7 @@ namespace integration { const Scalar b = 0.5; // Weights for final update // Initial guess for the stage values k1, k2, k3 - mathlib::VecX_T k = mathlib::VecX_T::Zero(2 * n); // 2 stages + mathlib::VecX_T k(2 * n); // 2 stages mathlib::VecX_T f0 = f(t, x); k.segment(0, n) = f0; k.segment(n, n) = f0; @@ -335,10 +331,8 @@ namespace integration { auto eval_j = [&](const mathlib::VecX_T& k_guess, mathlib::MatX_T& J) { mathlib::VecX_T k1 = k_guess.segment(0, n); mathlib::VecX_T k2 = k_guess.segment(n, n); - mathlib::VecX_T x1 = x + dt * (A(0, 0) * k1 + A(0, 1) * k2); mathlib::VecX_T x2 = x + dt * (A(1, 0) * k1 + A(1, 1) * k2); - mathlib::MatX_T F1(n, n), F2(n, n); bool analytical_success = false; @@ -398,8 +392,7 @@ namespace integration { Scalar tol ) { if (tol < 0.0) { tol = std::max(1e-12, 1e-2 * std::pow(dt, 7.0)); } - - size_t n = x.size(); + const Eigen::Index n = x.size(); // Coefficients for the 3-stage Gauss-Legendre method (6th order) mathlib::VecX_T c(3); @@ -408,8 +401,7 @@ namespace integration { 0.5, 0.5 + std::sqrt(15.0) / 10.0; // Stage time fractions - mathlib::Mat3_T A = mathlib::Mat3_T::Zero(); - + mathlib::Mat3_T A(3, 3); A << 5.0 / 36.0, 2.0 / 9.0 - std::sqrt(15.0) / 15.0, 5.0 / 36.0 - std::sqrt(15.0) / 30.0, 5.0 / 36.0 + std::sqrt(15.0) / 24.0, 2.0 / 9.0, 5.0 / 36.0 - std::sqrt(15.0) / 24.0, @@ -419,7 +411,7 @@ namespace integration { b << 5.0 / 18.0, 4.0 / 9.0, 5.0 / 18.0; // Weights for final update // Initial guess for the stage values k1, k2, k3 - mathlib::VecX_T k = mathlib::VecX_T::Zero(3 * n); // 3 stages + mathlib::VecX_T k(3 * n); // 3 stages mathlib::VecX_T f0 = f(t, x); k.segment(0, n) = f0; k.segment(n, n) = f0; @@ -611,7 +603,7 @@ namespace integration { const Scalar eps_rel = 1e-8; mathlib::VecX_T f_0 = f(t, x); int n = (int)x.size(); - mathlib::MatX_T J = mathlib::MatX_T::Zero(f_0.size(), n); + mathlib::MatX_T J(f_0.size(), n); // Compute the Jacobian column by column using central differences for (int i = 0; i < n; ++i) { mathlib::VecX_T x_fwd = x; From d69e94019b0963b29b5a84825dae91841f1a2564 Mon Sep 17 00:00:00 2001 From: saltyjoss Date: Sun, 17 May 2026 08:45:07 +0100 Subject: [PATCH 010/210] feat: Added new `DualNumbers` type header using Scalar typename, with inlined oeprators. --- PxMLib/MathLib/include/core/DualNumbers.h | 173 ++++++++++++++++++++++ 1 file changed, 173 insertions(+) create mode 100644 PxMLib/MathLib/include/core/DualNumbers.h diff --git a/PxMLib/MathLib/include/core/DualNumbers.h b/PxMLib/MathLib/include/core/DualNumbers.h new file mode 100644 index 00000000..8ddd2902 --- /dev/null +++ b/PxMLib/MathLib/include/core/DualNumbers.h @@ -0,0 +1,173 @@ +// PxM/MathLib DualNumbers.h +#pragma once + +#include "MathLibAPI.h" +#include "core/Types_tpl.h" + +namespace mathlib { + // Template version of dual number (single variable) + template + class DualNumber_T { + public: + DualNumber_T(Scalar real = Scalar(0), Scalar dual = Scalar(0)) : _real(real), _dual(dual) {} + Scalar real; + Scalar dual; + }; + + // Addition + template + inline DualNumber_T operator+( + const DualNumber_T& a, + const DualNumber_T& b + ) { + return DualNumber_T(a.real + b.real, a.dual + b.dual); + } + + // Subtraction + template + inline DualNumber_T operator-( + const DualNumber_T& a, + const DualNumber_T& b + ) { + return DualNumber_T(a.real - b.real, a.dual - b.dual); + } + + // Multiplication + template + inline DualNumber_T operator*( + const DualNumber_T& a, + const DualNumber_T& b + ) { + return DualNumber_T( + a.real * b.real, + a.real * b.dual + a.dual * b.real + ); + } + + // Division + template + inline DualNumber_T operator/( + const DualNumber_T& a, + const DualNumber_T& b + ) { + return DualNumber_T( + a.real / b.real, + (a.dual * b.real - a.real * b.dual) / (b.real * b.real) + ); + } + + // Exponential function + template + inline DualNumber_T exp( + const DualNumber_T& a + ) { + Scalar expReal = std::exp(a.real); + return DualNumber_T(expReal, expReal * a.dual); + } + + // Power function (x^n) + template + inline DualNumber_T pow( + const DualNumber_T& a, + Scalar n + ) { + Scalar realPow = std::pow(a.real, n); + return DualNumber_T( + realPow, + n * std::pow(x.real, a - Scalar(1)) * a.dual + ); + } + + // Square root function + template + inline DualNumber_T sqrt( + const DualNumber_T& a + ) { + Scalar sqrtReal = std::sqrt(a.real); + return DualNumber_T( + sqrtReal, + Scalar(0.5) * a.dual / sqrtReal + ); + } + + // Sine function + template + inline DualNumber_T sin( + const DualNumber_T& a + ) { + return DualNumber_T( + std::sin(a.real), + std::cos(a.real) * a.dual + ); + } + + // Cosine function + template + inline DualNumber_T cos( + const DualNumber_T& a + ) { + return DualNumber_T( + std::cos(a.real), + -std::sin(a.real) * a.dual + ); + } + + // Tangent function + template + inline DualNumber_T tan( + const DualNumber_T& a + ) { + Scalar cosReal = std::cos(a.real); + return DualNumber_T( + std::tan(a.real), + a.dual / (cosReal * cosReal) + ); + } + + // Arctangent function + template + inline DualNumber_T atan( + const DualNumber_T& a + ) { + return DualNumber_T( + std::atan(a.real), + a.dual / (1 + a.real * a.real) + ); + } + + // Smoothstep function for transition from 0 to 1 as x goes from 0 to 1 + template + inline DualNumber_T smoothStep( + DualNumber_T x + ) { + return x * x * (DualNumber_T(3) - DualNumber_T(2) * x); + } + + // Smoothstep function with edge parameters + // * Derivative of smoothstep with respect to t is 6*t*(1-t), and dt/da = 1/(edge1 - edge0) + template + inline DualNumber_T smoothStep( + DualNumber_T edge0, + DualNumber_T edge1, + DualNumber_T a + ) { + DualNumber_T t = (a - edge0) / (edge1 - edge0); + return DualNumber_T( + t.real * t.real * (Scalar(3) - Scalar(2) * t.real), + t.dual * (Scalar(6) * t.real * (Scalar(1) - t.real)) + ); + } + + // Sigmoid function + template + inline DualNumber_T sigmoid( + DualNumber_T a + ) { + Scalar expNegReal = std::exp(-a.real); + Scalar sigmoidReal = Scalar(1) / (Scalar(1) + expNegReal); + return DualNumber_T( + sigmoidReal, + a.dual * sigmoidReal * (Scalar(1) - sigmoidReal) + ); + } +} \ No newline at end of file From 6844d1c67115fe3f97bfc89d07d59cd12c11c124 Mon Sep 17 00:00:00 2001 From: saltyjoss Date: Sun, 17 May 2026 09:20:44 +0100 Subject: [PATCH 011/210] feat: Added new `M_DualNumbers` type header for multivariate dual numbers, using Scalar typename alongsideinlined oeprators. --- PxMLib/MathLib/include/core/M_DualNumbers.h | 170 ++++++++++++++++++++ 1 file changed, 170 insertions(+) create mode 100644 PxMLib/MathLib/include/core/M_DualNumbers.h diff --git a/PxMLib/MathLib/include/core/M_DualNumbers.h b/PxMLib/MathLib/include/core/M_DualNumbers.h new file mode 100644 index 00000000..b636125b --- /dev/null +++ b/PxMLib/MathLib/include/core/M_DualNumbers.h @@ -0,0 +1,170 @@ +// PxM/MathLib M_DualNumbers.h +#pragma once + +#include "MathLibAPI.h" +#include "core/Types_tpl.h" + +namespace mathlib { + // Template version of dual number (Multivariate) + template + class M_DualNumber_T { + public: + M_DualNumber_T( + Scalar real = Scalar(0), + const std::array& duals = std::array() + ) : real(real), duals(duals) {} + + M_DualNumber_T( + Scalar real, + std::initializer_list duals + ) : real_T(real) { + std::copy(duals.begin(), duals.end(), duals.begin()); + } + + Scalar m_real; + std::array m_duals; + }; + + // Addition + template + inline M_DualNumber_T operator+( + const M_DualNumber_T& a, + const M_DualNumber_T& b + ) { + M_DualNumber_T out; + out.m_real = a.m_real + b.m_real; + for (size_t i = 0; i < NVar; ++i) { + out.m_duals[i] = a.m_duals[i] + b.m_duals[i]; + } + return out; + } + + // Subtraction + template + inline M_DualNumber_T operator-( + const M_DualNumber_T& a, + const M_DualNumber_T& b + ) { + M_DualNumber_T out; + out.m_real = a.m_real - b.m_real; + for (size_t i = 0; i < NVar; ++i) { + out.m_duals[i] = a.m_duals[i] - b.m_duals[i]; + } + return out; + } + + // Multiplication + template + inline M_DualNumber_T operator*( + const M_DualNumber_T& a, + const M_DualNumber_T& b + ) { + M_DualNumber_T out; + out.m_real = a.m_real * b.m_real; + for (size_t i = 0; i < NVar; ++i) { + out.m_duals[i] = a.m_real * b.m_duals[i] + a.m_duals[i] * b.m_real; + } + return out; + } + + // Division + template + inline M_DualNumber_T operator/( + const M_DualNumber_T& a, + const M_DualNumber_T& b + ) { + M_DualNumber_T out; + out.m_real = a.m_real / b.m_real; + for (size_t i = 0; i < NVar; ++i) { + out.m_duals[i] = (a.m_duals[i] * b.m_real - a.m_real * b.m_duals[i]) / (b.m_real / b.m_real); + } + return out; + } + + // Power function (x^n) + template + inline M_DualNumber_T pow( + const M_DualNumber_T& a, + Scalar n + ) { + M_DualNumber_T out; + out.m_real = std::pow(a.m_real, n); + for (size_t i = 0; i < NVar; ++i) { + out.m_duals[i] = n * std::pow(a.m_real, n - Scalar(1)) * a.m_duals[i]; + } + return out; + } + + // Square root function + template + inline M_DualNumber_T sqrt( + const M_DualNumber_T& a + ) { + M_DualNumber_T out; + Scalar sqrtReal = std::sqrt(a.real()); + for (size_t i = 0; i < NVar; ++i) { + out.m_duals[i] = Scalar(0.5) * a.m_duals[i] / sqrtReal; + } + return out; + } + + // Sine function + template + inline M_DualNumber_T sin( + const M_DualNumber_T& a + ) { + M_DualNumber_T out; + out.m_real = std::sin(a.m_real); + for (size_t i = 0; i < NVar; ++i) { + out.m_duals[i] = std::cos(a.m_real) * a.m_duals[i]; + } + return out; + } + + // Cosine function + template + inline M_DualNumber_T cos( + const M_DualNumber_T& a + ) { + M_DualNumber_T out; + out.m_real = std::cos(a.m_real); + for (size_t i = 0; i < NVar; ++i) { + out.m_duals[i] = -std::sin(a.m_real) * a.m_duals[i]; + } + return out; + } + + // Tangent function + template + inline M_DualNumber_T tan( + const M_DualNumber_T& a + ) { + M_DualNumber_T out; + out.m_real = std::tan(a.m_real); + for (size_t i = 0; i < NVar; ++i) { + out.m_duals[i] = a.m_duals[i] / (std::cos(a.m_real) * std::cos(a.m_real)); + } + return out; + } + + // Arctangent function + template + inline M_DualNumber_T atan( + const M_DualNumber_T& a + ) { + M_DualNumber_T out; + out.m_real = tan(a.m_real); + for (size_t i = 0; i < NVar; ++i) { + out.m_duals[i] = a.m_duals[i] / (Scalar(1) + a.m_real * a.m_real); + } + return out; + } + + // Smooth step function for smooth interpolation between 0 and 1 + template + inline Scalar smoothStep( + const Scalar& x + ) { + return x * x * (Scalar(3) - Scalar(2) * x); + } +} \ No newline at end of file From 7d0ed18c53779db328218fa795f1cb27e00fb655 Mon Sep 17 00:00:00 2001 From: saltyjoss Date: Sun, 17 May 2026 09:22:52 +0100 Subject: [PATCH 012/210] fixes: Correct small naming errors in `DualNumber_T` --- PxMLib/MathLib/include/core/DualNumbers.h | 8 ++++---- 1 file changed, 4 insertions(+), 4 deletions(-) diff --git a/PxMLib/MathLib/include/core/DualNumbers.h b/PxMLib/MathLib/include/core/DualNumbers.h index 8ddd2902..6d2fd1f9 100644 --- a/PxMLib/MathLib/include/core/DualNumbers.h +++ b/PxMLib/MathLib/include/core/DualNumbers.h @@ -9,7 +9,7 @@ namespace mathlib { template class DualNumber_T { public: - DualNumber_T(Scalar real = Scalar(0), Scalar dual = Scalar(0)) : _real(real), _dual(dual) {} + DualNumber_T(Scalar real = Scalar(0), Scalar dual = Scalar(0)) : real(real), dual(dual) {} Scalar real; Scalar dual; }; @@ -67,14 +67,14 @@ namespace mathlib { // Power function (x^n) template - inline DualNumber_T pow( + inline DualNumber_T pow(< const DualNumber_T& a, Scalar n ) { Scalar realPow = std::pow(a.real, n); return DualNumber_T( realPow, - n * std::pow(x.real, a - Scalar(1)) * a.dual + n * std::pow(a.real, n - Scalar(1)) * a.dual ); } @@ -140,7 +140,7 @@ namespace mathlib { inline DualNumber_T smoothStep( DualNumber_T x ) { - return x * x * (DualNumber_T(3) - DualNumber_T(2) * x); + return x * x * (DualNumber_T(3) - DualNumber_T(2) * x); } // Smoothstep function with edge parameters From 62d56d86aa557aa5bf89b4c6849a5c7965f528c8 Mon Sep 17 00:00:00 2001 From: saltyjoss Date: Sun, 17 May 2026 09:27:56 +0100 Subject: [PATCH 013/210] refactor: Merged both dual number headers --- PxMLib/MathLib/include/core/DualNumbers.h | 242 ++++++++++---------- PxMLib/MathLib/include/core/M_DualNumbers.h | 170 -------------- 2 files changed, 120 insertions(+), 292 deletions(-) delete mode 100644 PxMLib/MathLib/include/core/M_DualNumbers.h diff --git a/PxMLib/MathLib/include/core/DualNumbers.h b/PxMLib/MathLib/include/core/DualNumbers.h index 6d2fd1f9..337d460a 100644 --- a/PxMLib/MathLib/include/core/DualNumbers.h +++ b/PxMLib/MathLib/include/core/DualNumbers.h @@ -1,173 +1,171 @@ -// PxM/MathLib DualNumbers.h +// PxM/MathLib M_DualNumbers.h #pragma once #include "MathLibAPI.h" #include "core/Types_tpl.h" namespace mathlib { - // Template version of dual number (single variable) - template + // Template version of dual number + template class DualNumber_T { public: - DualNumber_T(Scalar real = Scalar(0), Scalar dual = Scalar(0)) : real(real), dual(dual) {} - Scalar real; - Scalar dual; + DualNumber_T( + Scalar real = Scalar(0), + const std::array& duals = std::array() + ) : m_real(real), m_duals(duals) { + } + + DualNumber_T( + Scalar real, + std::initializer_list duals + ) : m_real(real) { + std::copy(duals.begin(), duals.end(), m_duals.begin()); + } + + Scalar m_real; + std::array m_duals; }; // Addition - template - inline DualNumber_T operator+( - const DualNumber_T& a, - const DualNumber_T& b - ) { - return DualNumber_T(a.real + b.real, a.dual + b.dual); + template + inline DualNumber_T operator+( + const DualNumber_T& a, + const DualNumber_T& b + ) { + DualNumber_T out; + out.m_real = a.m_real + b.m_real; + for (size_t i = 0; i < NVar; ++i) { + out.m_duals[i] = a.m_duals[i] + b.m_duals[i]; + } + return out; } // Subtraction - template - inline DualNumber_T operator-( - const DualNumber_T& a, - const DualNumber_T& b - ) { - return DualNumber_T(a.real - b.real, a.dual - b.dual); + template + inline DualNumber_T operator-( + const DualNumber_T& a, + const DualNumber_T& b + ) { + DualNumber_T out; + out.m_real = a.m_real - b.m_real; + for (size_t i = 0; i < NVar; ++i) { + out.m_duals[i] = a.m_duals[i] - b.m_duals[i]; + } + return out; } // Multiplication - template - inline DualNumber_T operator*( - const DualNumber_T& a, - const DualNumber_T& b - ) { - return DualNumber_T( - a.real * b.real, - a.real * b.dual + a.dual * b.real - ); + template + inline DualNumber_T operator*( + const DualNumber_T& a, + const DualNumber_T& b + ) { + DualNumber_T out; + out.m_real = a.m_real * b.m_real; + for (size_t i = 0; i < NVar; ++i) { + out.m_duals[i] = a.m_real * b.m_duals[i] + a.m_duals[i] * b.m_real; + } + return out; } // Division - template - inline DualNumber_T operator/( - const DualNumber_T& a, - const DualNumber_T& b - ) { - return DualNumber_T( - a.real / b.real, - (a.dual * b.real - a.real * b.dual) / (b.real * b.real) - ); - } - - // Exponential function - template - inline DualNumber_T exp( - const DualNumber_T& a - ) { - Scalar expReal = std::exp(a.real); - return DualNumber_T(expReal, expReal * a.dual); + template + inline DualNumber_T operator/( + const DualNumber_T& a, + const DualNumber_T& b + ) { + DualNumber_T out; + out.m_real = a.m_real / b.m_real; + for (size_t i = 0; i < NVar; ++i) { + out.m_duals[i] = (a.m_duals[i] * b.m_real - a.m_real * b.m_duals[i]) / (b.m_real * b.m_real); + } + return out; } // Power function (x^n) - template - inline DualNumber_T pow(< - const DualNumber_T& a, + template + inline DualNumber_T pow( + const DualNumber_T& a, Scalar n ) { - Scalar realPow = std::pow(a.real, n); - return DualNumber_T( - realPow, - n * std::pow(a.real, n - Scalar(1)) * a.dual - ); + DualNumber_T out; + out.m_real = std::pow(a.m_real, n); + for (size_t i = 0; i < NVar; ++i) { + out.m_duals[i] = n * std::pow(a.m_real, n - Scalar(1)) * a.m_duals[i]; + } + return out; } // Square root function - template - inline DualNumber_T sqrt( - const DualNumber_T& a + template + inline DualNumber_T sqrt( + const DualNumber_T& a ) { - Scalar sqrtReal = std::sqrt(a.real); - return DualNumber_T( - sqrtReal, - Scalar(0.5) * a.dual / sqrtReal - ); + DualNumber_T out; + Scalar sqrtReal = std::sqrt(a.m_real); + for (size_t i = 0; i < NVar; ++i) { + out.m_duals[i] = Scalar(0.5) * a.m_duals[i] / sqrtReal; + } + return out; } // Sine function - template - inline DualNumber_T sin( - const DualNumber_T& a + template + inline DualNumber_T sin( + const DualNumber_T& a ) { - return DualNumber_T( - std::sin(a.real), - std::cos(a.real) * a.dual - ); + DualNumber_T out; + out.m_real = std::sin(a.m_real); + for (size_t i = 0; i < NVar; ++i) { + out.m_duals[i] = std::cos(a.m_real) * a.m_duals[i]; + } + return out; } // Cosine function - template - inline DualNumber_T cos( - const DualNumber_T& a + template + inline DualNumber_T cos( + const DualNumber_T& a ) { - return DualNumber_T( - std::cos(a.real), - -std::sin(a.real) * a.dual - ); + DualNumber_T out; + out.m_real = std::cos(a.m_real); + for (size_t i = 0; i < NVar; ++i) { + out.m_duals[i] = -std::sin(a.m_real) * a.m_duals[i]; + } + return out; } // Tangent function - template - inline DualNumber_T tan( - const DualNumber_T& a + template + inline DualNumber_T tan( + const DualNumber_T& a ) { - Scalar cosReal = std::cos(a.real); - return DualNumber_T( - std::tan(a.real), - a.dual / (cosReal * cosReal) - ); + DualNumber_T out; + out.m_real = std::tan(a.m_real); + for (size_t i = 0; i < NVar; ++i) { + out.m_duals[i] = a.m_duals[i] / (std::cos(a.m_real) * std::cos(a.m_real)); + } + return out; } // Arctangent function - template - inline DualNumber_T atan( - const DualNumber_T& a - ) { - return DualNumber_T( - std::atan(a.real), - a.dual / (1 + a.real * a.real) - ); - } - - // Smoothstep function for transition from 0 to 1 as x goes from 0 to 1 - template - inline DualNumber_T smoothStep( - DualNumber_T x - ) { - return x * x * (DualNumber_T(3) - DualNumber_T(2) * x); - } - - // Smoothstep function with edge parameters - // * Derivative of smoothstep with respect to t is 6*t*(1-t), and dt/da = 1/(edge1 - edge0) - template - inline DualNumber_T smoothStep( - DualNumber_T edge0, - DualNumber_T edge1, - DualNumber_T a + template + inline DualNumber_T atan( + const DualNumber_T& a ) { - DualNumber_T t = (a - edge0) / (edge1 - edge0); - return DualNumber_T( - t.real * t.real * (Scalar(3) - Scalar(2) * t.real), - t.dual * (Scalar(6) * t.real * (Scalar(1) - t.real)) - ); + DualNumber_T out; + out.m_real = std::atan(a.m_real); + for (size_t i = 0; i < NVar; ++i) { + out.m_duals[i] = a.m_duals[i] / (Scalar(1) + a.m_real * a.m_real); + } + return out; } - // Sigmoid function + // Smooth step function for smooth interpolation between 0 and 1 template - inline DualNumber_T sigmoid( - DualNumber_T a + inline Scalar smoothStep( + const Scalar& x ) { - Scalar expNegReal = std::exp(-a.real); - Scalar sigmoidReal = Scalar(1) / (Scalar(1) + expNegReal); - return DualNumber_T( - sigmoidReal, - a.dual * sigmoidReal * (Scalar(1) - sigmoidReal) - ); + return x * x * (Scalar(3) - Scalar(2) * x); } } \ No newline at end of file diff --git a/PxMLib/MathLib/include/core/M_DualNumbers.h b/PxMLib/MathLib/include/core/M_DualNumbers.h deleted file mode 100644 index b636125b..00000000 --- a/PxMLib/MathLib/include/core/M_DualNumbers.h +++ /dev/null @@ -1,170 +0,0 @@ -// PxM/MathLib M_DualNumbers.h -#pragma once - -#include "MathLibAPI.h" -#include "core/Types_tpl.h" - -namespace mathlib { - // Template version of dual number (Multivariate) - template - class M_DualNumber_T { - public: - M_DualNumber_T( - Scalar real = Scalar(0), - const std::array& duals = std::array() - ) : real(real), duals(duals) {} - - M_DualNumber_T( - Scalar real, - std::initializer_list duals - ) : real_T(real) { - std::copy(duals.begin(), duals.end(), duals.begin()); - } - - Scalar m_real; - std::array m_duals; - }; - - // Addition - template - inline M_DualNumber_T operator+( - const M_DualNumber_T& a, - const M_DualNumber_T& b - ) { - M_DualNumber_T out; - out.m_real = a.m_real + b.m_real; - for (size_t i = 0; i < NVar; ++i) { - out.m_duals[i] = a.m_duals[i] + b.m_duals[i]; - } - return out; - } - - // Subtraction - template - inline M_DualNumber_T operator-( - const M_DualNumber_T& a, - const M_DualNumber_T& b - ) { - M_DualNumber_T out; - out.m_real = a.m_real - b.m_real; - for (size_t i = 0; i < NVar; ++i) { - out.m_duals[i] = a.m_duals[i] - b.m_duals[i]; - } - return out; - } - - // Multiplication - template - inline M_DualNumber_T operator*( - const M_DualNumber_T& a, - const M_DualNumber_T& b - ) { - M_DualNumber_T out; - out.m_real = a.m_real * b.m_real; - for (size_t i = 0; i < NVar; ++i) { - out.m_duals[i] = a.m_real * b.m_duals[i] + a.m_duals[i] * b.m_real; - } - return out; - } - - // Division - template - inline M_DualNumber_T operator/( - const M_DualNumber_T& a, - const M_DualNumber_T& b - ) { - M_DualNumber_T out; - out.m_real = a.m_real / b.m_real; - for (size_t i = 0; i < NVar; ++i) { - out.m_duals[i] = (a.m_duals[i] * b.m_real - a.m_real * b.m_duals[i]) / (b.m_real / b.m_real); - } - return out; - } - - // Power function (x^n) - template - inline M_DualNumber_T pow( - const M_DualNumber_T& a, - Scalar n - ) { - M_DualNumber_T out; - out.m_real = std::pow(a.m_real, n); - for (size_t i = 0; i < NVar; ++i) { - out.m_duals[i] = n * std::pow(a.m_real, n - Scalar(1)) * a.m_duals[i]; - } - return out; - } - - // Square root function - template - inline M_DualNumber_T sqrt( - const M_DualNumber_T& a - ) { - M_DualNumber_T out; - Scalar sqrtReal = std::sqrt(a.real()); - for (size_t i = 0; i < NVar; ++i) { - out.m_duals[i] = Scalar(0.5) * a.m_duals[i] / sqrtReal; - } - return out; - } - - // Sine function - template - inline M_DualNumber_T sin( - const M_DualNumber_T& a - ) { - M_DualNumber_T out; - out.m_real = std::sin(a.m_real); - for (size_t i = 0; i < NVar; ++i) { - out.m_duals[i] = std::cos(a.m_real) * a.m_duals[i]; - } - return out; - } - - // Cosine function - template - inline M_DualNumber_T cos( - const M_DualNumber_T& a - ) { - M_DualNumber_T out; - out.m_real = std::cos(a.m_real); - for (size_t i = 0; i < NVar; ++i) { - out.m_duals[i] = -std::sin(a.m_real) * a.m_duals[i]; - } - return out; - } - - // Tangent function - template - inline M_DualNumber_T tan( - const M_DualNumber_T& a - ) { - M_DualNumber_T out; - out.m_real = std::tan(a.m_real); - for (size_t i = 0; i < NVar; ++i) { - out.m_duals[i] = a.m_duals[i] / (std::cos(a.m_real) * std::cos(a.m_real)); - } - return out; - } - - // Arctangent function - template - inline M_DualNumber_T atan( - const M_DualNumber_T& a - ) { - M_DualNumber_T out; - out.m_real = tan(a.m_real); - for (size_t i = 0; i < NVar; ++i) { - out.m_duals[i] = a.m_duals[i] / (Scalar(1) + a.m_real * a.m_real); - } - return out; - } - - // Smooth step function for smooth interpolation between 0 and 1 - template - inline Scalar smoothStep( - const Scalar& x - ) { - return x * x * (Scalar(3) - Scalar(2) * x); - } -} \ No newline at end of file From ad6f27ae0768825b4d560ba539e1e881084de3b0 Mon Sep 17 00:00:00 2001 From: saltyjoss Date: Sun, 17 May 2026 09:45:19 +0100 Subject: [PATCH 014/210] refactor: Updated `DualNumbers_T` to define unary and equality operators, along with scalar operations --- PxMLib/MathLib/include/core/DualNumbers.h | 228 +++++++++++++++++++++- 1 file changed, 224 insertions(+), 4 deletions(-) diff --git a/PxMLib/MathLib/include/core/DualNumbers.h b/PxMLib/MathLib/include/core/DualNumbers.h index 337d460a..3fbe388c 100644 --- a/PxMLib/MathLib/include/core/DualNumbers.h +++ b/PxMLib/MathLib/include/core/DualNumbers.h @@ -26,12 +26,212 @@ namespace mathlib { std::array m_duals; }; - // Addition + // ---- + // Unary Operators + // ---- + + // Negation + template + inline DualNumber_T operator-( + const DualNumber_T& a + ) { + DualNumber_T out; + out.m_real = -a.m_real; + for (size_t i = 0; i < NVar; ++i) { + out.m_duals[i] = -a.m_duals[i]; + } + return out; + } + + // Conjugate (negate dual part, keep real part) + template + inline DualNumber_T conjugate( + const DualNumber_T& a + ) { + DualNumber_T out; + out.m_real = a.m_real; + for (size_t i = 0; i < NVar; ++i) { + out.m_duals[i] = -a.m_duals[i]; + } + return out; + } + + // Identity + template + inline DualNumber_T operator+( + const DualNumber_T& a + ) { + return a; + } + + // ---- + // Comparison Operators (compare only the real part) + // ---- + + // Equality + template + inline bool operator==( + const DualNumber_T& a, + const DualNumber_T& b + ) { + return a.m_real == b.m_real; + } + + // Inequality + template + inline bool operator!=( + const DualNumber_T& a, + const DualNumber_T& b + ) { + return !(a == b); + } + + // Less than + template + inline bool operator<( + const DualNumber_T& a, + const DualNumber_T& b + ) { + return a.m_real < b.m_real; + } + + // Less than or equal + template + inline bool operator<=( + const DualNumber_T& a, + const DualNumber_T& b + ) { + return a.m_real <= b.m_real; + } + + // Greater than + template + inline bool operator>( + const DualNumber_T& a, + const DualNumber_T& b + ) { + return a.m_real > b.m_real; + } + + // Greater than or equal + template + inline bool operator>=( + const DualNumber_T& a, + const DualNumber_T& b + ) { + return a.m_real >= b.m_real; + } + + // ---- + // Scalar Interactions + // ---- + + // Scalar Addition (dual + scalar) + template + inline DualNumber_T operator+( + const DualNumber_T& a, + Scalar b + ) { + DualNumber_T out = a; + out.m_real += b; + return out; + } + + // Scalar Addition (scalar + dual) template inline DualNumber_T operator+( + Scalar a, + const DualNumber_T& b + ) { + return b + a; // Reuse dual + scalar + } + + // Scalar Subtraction (dual - scalar) + template + inline DualNumber_T operator-( const DualNumber_T& a, + Scalar b + ) { + DualNumber_T out = a; + out.m_real -= b; + return out; + } + + // Scalar Subtraction (scalar - dual) + template + inline DualNumber_T operator-( + Scalar a, + const DualNumber_T& b + ) { + DualNumber_T out; + out.m_real = a - b.m_real; + for (size_t i = 0; i < NVar; ++i) { + out.m_duals[i] = -b.m_duals[i]; + } + return out; + } + + // Scalar Multiplication (dual * scalar) + template + inline DualNumber_T operator*( + const DualNumber_T& a, + Scalar b + ) { + DualNumber_T out; + out.m_real = a.m_real * b; + for (size_t i = 0; i < NVar; ++i) { + out.m_duals[i] = a.m_duals[i] * b; + } + return out; + } + + // Scalar Multiplication (scalar * dual) + template + inline DualNumber_T operator*( + Scalar a, + const DualNumber_T& b + ) { + return b * a; // Reuse dual * scalar + } + + // Scalar Division (dual / scalar) + template + inline DualNumber_T operator/( + const DualNumber_T& a, + Scalar b + ) { + DualNumber_T out; + out.m_real = a.m_real / b; + for (size_t i = 0; i < NVar; ++i) { + out.m_duals[i] = a.m_duals[i] / b; + } + return out; + } + + // Scalar Division (scalar / dual) + template + inline DualNumber_T operator/( + Scalar a, const DualNumber_T& b ) { + DualNumber_T out; + out.m_real = a / b.m_real; + for (size_t i = 0; i < NVar; ++i) { + out.m_duals[i] = -a * b.m_duals[i] / (b.m_real * b.m_real); + } + return out; + } + + // ---- + // Dual Number Interactions + // ---- + + // Addition + template + inline DualNumber_T operator+( + const DualNumber_T& a, + const DualNumber_T& b + ) { DualNumber_T out; out.m_real = a.m_real + b.m_real; for (size_t i = 0; i < NVar; ++i) { @@ -45,7 +245,7 @@ namespace mathlib { inline DualNumber_T operator-( const DualNumber_T& a, const DualNumber_T& b - ) { + ) { DualNumber_T out; out.m_real = a.m_real - b.m_real; for (size_t i = 0; i < NVar; ++i) { @@ -59,7 +259,7 @@ namespace mathlib { inline DualNumber_T operator*( const DualNumber_T& a, const DualNumber_T& b - ) { + ) { DualNumber_T out; out.m_real = a.m_real * b.m_real; for (size_t i = 0; i < NVar; ++i) { @@ -73,7 +273,7 @@ namespace mathlib { inline DualNumber_T operator/( const DualNumber_T& a, const DualNumber_T& b - ) { + ) { DualNumber_T out; out.m_real = a.m_real / b.m_real; for (size_t i = 0; i < NVar; ++i) { @@ -168,4 +368,24 @@ namespace mathlib { ) { return x * x * (Scalar(3) - Scalar(2) * x); } + + // Smooth step function for dual numbers (applies smooth step to the real part, scales dual part by the derivative of the smooth step) + template + inline DualNumber_T smoothStep( + const DualNumber_T& x + ) { + DualNumber_T out; + out.m_real = smoothStep(x.m_real); + Scalar derivative = Scalar(6) * x.m_real * (Scalar(1) - x.m_real); // Derivative of smooth step with respect to x + for (size_t i = 0; i < NVar; ++i) { + out.m_duals[i] = derivative * x.m_duals[i]; + } + return out; + } + + // ---- + // Numeric Limits Specialisation for DualNumber_T + // ---- + + // TODO: Add specialisation of std::numeric_limits for DualNumber_T to define properties like infinity, NaN, epsilon, etc. based on the underlying Scalar type. } \ No newline at end of file From 5406fad6a173ef72b3fbd7c07157821e6926b932 Mon Sep 17 00:00:00 2001 From: saltyjoss Date: Sun, 17 May 2026 09:50:28 +0100 Subject: [PATCH 015/210] chore: Corrected naming and assignment errors, and added fill to reduce risk of uninitialised arrays (means reduced overflow risk) --- PxMLib/MathLib/include/core/DualNumbers.h | 97 ++++++++++++----------- 1 file changed, 50 insertions(+), 47 deletions(-) diff --git a/PxMLib/MathLib/include/core/DualNumbers.h b/PxMLib/MathLib/include/core/DualNumbers.h index 3fbe388c..dab4eaa8 100644 --- a/PxMLib/MathLib/include/core/DualNumbers.h +++ b/PxMLib/MathLib/include/core/DualNumbers.h @@ -12,18 +12,20 @@ namespace mathlib { DualNumber_T( Scalar real = Scalar(0), const std::array& duals = std::array() - ) : m_real(real), m_duals(duals) { + ) : real(real), dual(duals) { + dual.fill(Scalar(0)); } DualNumber_T( Scalar real, std::initializer_list duals - ) : m_real(real) { - std::copy(duals.begin(), duals.end(), m_duals.begin()); + ) : real(real) { + dual.fill(Scalar(0)); + std::copy(duals.begin(), duals.end(), dual.begin()); } - Scalar m_real; - std::array m_duals; + Scalar real; + std::array duals; }; // ---- @@ -36,9 +38,9 @@ namespace mathlib { const DualNumber_T& a ) { DualNumber_T out; - out.m_real = -a.m_real; + out.real = -a.real; for (size_t i = 0; i < NVar; ++i) { - out.m_duals[i] = -a.m_duals[i]; + out.dual[i] = -a.dual[i]; } return out; } @@ -49,9 +51,9 @@ namespace mathlib { const DualNumber_T& a ) { DualNumber_T out; - out.m_real = a.m_real; + out.real = a.real; for (size_t i = 0; i < NVar; ++i) { - out.m_duals[i] = -a.m_duals[i]; + out.dual[i] = -a.dual[i]; } return out; } @@ -74,7 +76,7 @@ namespace mathlib { const DualNumber_T& a, const DualNumber_T& b ) { - return a.m_real == b.m_real; + return a.real == b.real; } // Inequality @@ -92,7 +94,7 @@ namespace mathlib { const DualNumber_T& a, const DualNumber_T& b ) { - return a.m_real < b.m_real; + return a.real < b.real; } // Less than or equal @@ -101,7 +103,7 @@ namespace mathlib { const DualNumber_T& a, const DualNumber_T& b ) { - return a.m_real <= b.m_real; + return a.real <= b.real; } // Greater than @@ -110,7 +112,7 @@ namespace mathlib { const DualNumber_T& a, const DualNumber_T& b ) { - return a.m_real > b.m_real; + return a.real > b.real; } // Greater than or equal @@ -119,7 +121,7 @@ namespace mathlib { const DualNumber_T& a, const DualNumber_T& b ) { - return a.m_real >= b.m_real; + return a.real >= b.real; } // ---- @@ -133,7 +135,7 @@ namespace mathlib { Scalar b ) { DualNumber_T out = a; - out.m_real += b; + out.real += b; return out; } @@ -153,7 +155,7 @@ namespace mathlib { Scalar b ) { DualNumber_T out = a; - out.m_real -= b; + out.real -= b; return out; } @@ -164,9 +166,9 @@ namespace mathlib { const DualNumber_T& b ) { DualNumber_T out; - out.m_real = a - b.m_real; + out.real = a - b.real; for (size_t i = 0; i < NVar; ++i) { - out.m_duals[i] = -b.m_duals[i]; + out.dual[i] = -b.dual[i]; } return out; } @@ -178,9 +180,9 @@ namespace mathlib { Scalar b ) { DualNumber_T out; - out.m_real = a.m_real * b; + out.real = a.real * b; for (size_t i = 0; i < NVar; ++i) { - out.m_duals[i] = a.m_duals[i] * b; + out.dual[i] = a.dual[i] * b; } return out; } @@ -201,9 +203,9 @@ namespace mathlib { Scalar b ) { DualNumber_T out; - out.m_real = a.m_real / b; + out.real = a.real / b; for (size_t i = 0; i < NVar; ++i) { - out.m_duals[i] = a.m_duals[i] / b; + out.dual[i] = a.dual[i] / b; } return out; } @@ -215,9 +217,9 @@ namespace mathlib { const DualNumber_T& b ) { DualNumber_T out; - out.m_real = a / b.m_real; + out.real = a / b.real; for (size_t i = 0; i < NVar; ++i) { - out.m_duals[i] = -a * b.m_duals[i] / (b.m_real * b.m_real); + out.dual[i] = -a * b.dual[i] / (b.real * b.real); } return out; } @@ -233,9 +235,9 @@ namespace mathlib { const DualNumber_T& b ) { DualNumber_T out; - out.m_real = a.m_real + b.m_real; + out.real = a.real + b.real; for (size_t i = 0; i < NVar; ++i) { - out.m_duals[i] = a.m_duals[i] + b.m_duals[i]; + out.dual[i] = a.dual[i] + b.dual[i]; } return out; } @@ -247,9 +249,9 @@ namespace mathlib { const DualNumber_T& b ) { DualNumber_T out; - out.m_real = a.m_real - b.m_real; + out.real = a.real - b.real; for (size_t i = 0; i < NVar; ++i) { - out.m_duals[i] = a.m_duals[i] - b.m_duals[i]; + out.dual[i] = a.dual[i] - b.dual[i]; } return out; } @@ -261,9 +263,9 @@ namespace mathlib { const DualNumber_T& b ) { DualNumber_T out; - out.m_real = a.m_real * b.m_real; + out.real = a.real * b.real; for (size_t i = 0; i < NVar; ++i) { - out.m_duals[i] = a.m_real * b.m_duals[i] + a.m_duals[i] * b.m_real; + out.dual[i] = a.real * b.dual[i] + a.dual[i] * b.real; } return out; } @@ -275,9 +277,9 @@ namespace mathlib { const DualNumber_T& b ) { DualNumber_T out; - out.m_real = a.m_real / b.m_real; + out.real = a.real / b.real; for (size_t i = 0; i < NVar; ++i) { - out.m_duals[i] = (a.m_duals[i] * b.m_real - a.m_real * b.m_duals[i]) / (b.m_real * b.m_real); + out.dual[i] = (a.dual[i] * b.real - a.real * b.dual[i]) / (b.real * b.real); } return out; } @@ -289,9 +291,9 @@ namespace mathlib { Scalar n ) { DualNumber_T out; - out.m_real = std::pow(a.m_real, n); + out.real = std::pow(a.real, n); for (size_t i = 0; i < NVar; ++i) { - out.m_duals[i] = n * std::pow(a.m_real, n - Scalar(1)) * a.m_duals[i]; + out.dual[i] = n * std::pow(a.real, n - Scalar(1)) * a.dual[i]; } return out; } @@ -302,9 +304,10 @@ namespace mathlib { const DualNumber_T& a ) { DualNumber_T out; - Scalar sqrtReal = std::sqrt(a.m_real); + Scalar sqrtReal = std::sqrt(a.real); + out.real = sqrtReal; for (size_t i = 0; i < NVar; ++i) { - out.m_duals[i] = Scalar(0.5) * a.m_duals[i] / sqrtReal; + out.dual[i] = Scalar(0.5) * a.dual[i] / sqrtReal; } return out; } @@ -315,9 +318,9 @@ namespace mathlib { const DualNumber_T& a ) { DualNumber_T out; - out.m_real = std::sin(a.m_real); + out.real = std::sin(a.real); for (size_t i = 0; i < NVar; ++i) { - out.m_duals[i] = std::cos(a.m_real) * a.m_duals[i]; + out.dual[i] = std::cos(a.real) * a.dual[i]; } return out; } @@ -328,9 +331,9 @@ namespace mathlib { const DualNumber_T& a ) { DualNumber_T out; - out.m_real = std::cos(a.m_real); + out.real = std::cos(a.real); for (size_t i = 0; i < NVar; ++i) { - out.m_duals[i] = -std::sin(a.m_real) * a.m_duals[i]; + out.dual[i] = -std::sin(a.real) * a.dual[i]; } return out; } @@ -341,9 +344,9 @@ namespace mathlib { const DualNumber_T& a ) { DualNumber_T out; - out.m_real = std::tan(a.m_real); + out.real = std::tan(a.real); for (size_t i = 0; i < NVar; ++i) { - out.m_duals[i] = a.m_duals[i] / (std::cos(a.m_real) * std::cos(a.m_real)); + out.dual[i] = a.dual[i] / (std::cos(a.real) * std::cos(a.real)); } return out; } @@ -354,9 +357,9 @@ namespace mathlib { const DualNumber_T& a ) { DualNumber_T out; - out.m_real = std::atan(a.m_real); + out.real = std::atan(a.real); for (size_t i = 0; i < NVar; ++i) { - out.m_duals[i] = a.m_duals[i] / (Scalar(1) + a.m_real * a.m_real); + out.dual[i] = a.dual[i] / (Scalar(1) + a.real * a.real); } return out; } @@ -375,10 +378,10 @@ namespace mathlib { const DualNumber_T& x ) { DualNumber_T out; - out.m_real = smoothStep(x.m_real); - Scalar derivative = Scalar(6) * x.m_real * (Scalar(1) - x.m_real); // Derivative of smooth step with respect to x + out.real = smoothStep(x.real); + Scalar derivative = Scalar(6) * x.real * (Scalar(1) - x.real); // Derivative of smooth step with respect to x for (size_t i = 0; i < NVar; ++i) { - out.m_duals[i] = derivative * x.m_duals[i]; + out.dual[i] = derivative * x.dual[i]; } return out; } From 11031e6d56fc1e8ce984bc664281eb13829d97a5 Mon Sep 17 00:00:00 2001 From: saltyjoss Date: Sun, 17 May 2026 09:54:17 +0100 Subject: [PATCH 016/210] feat: Added Assignement operators, fixed minor bugs --- PxMLib/MathLib/include/core/DualNumbers.h | 117 +++++++++++++++++++--- 1 file changed, 102 insertions(+), 15 deletions(-) diff --git a/PxMLib/MathLib/include/core/DualNumbers.h b/PxMLib/MathLib/include/core/DualNumbers.h index dab4eaa8..f0691f9f 100644 --- a/PxMLib/MathLib/include/core/DualNumbers.h +++ b/PxMLib/MathLib/include/core/DualNumbers.h @@ -11,9 +11,8 @@ namespace mathlib { public: DualNumber_T( Scalar real = Scalar(0), - const std::array& duals = std::array() - ) : real(real), dual(duals) { - dual.fill(Scalar(0)); + const std::array& dual = std::array() + ) : real(real), dual(dual) { } DualNumber_T( @@ -21,16 +20,20 @@ namespace mathlib { std::initializer_list duals ) : real(real) { dual.fill(Scalar(0)); - std::copy(duals.begin(), duals.end(), dual.begin()); + std::copy_n( + duals.begin(), + std::min(duals.size(), NVar), + dual.begin() + ); } Scalar real; - std::array duals; + std::array dual; }; - // ---- + // ----- // Unary Operators - // ---- + // ----- // Negation template @@ -66,9 +69,9 @@ namespace mathlib { return a; } - // ---- + // ----- // Comparison Operators (compare only the real part) - // ---- + // ----- // Equality template @@ -124,9 +127,9 @@ namespace mathlib { return a.real >= b.real; } - // ---- + // ----- // Scalar Interactions - // ---- + // ----- // Scalar Addition (dual + scalar) template @@ -224,9 +227,9 @@ namespace mathlib { return out; } - // ---- + // ----- // Dual Number Interactions - // ---- + // ----- // Addition template @@ -386,9 +389,93 @@ namespace mathlib { return out; } - // ---- + // ----- + // Assignment Operators + // ----- + + // Addition assignment operator + template + inline DualNumber_T& operator+=( + DualNumber_T& a, + const DualNumber_T& b + ) { + a.real += b.real; + for (size_t i = 0; i < NVar; ++i) { + a.dual[i] += b.dual[i]; + } + return a; + } + + // Subtraction assignment operator + template + inline DualNumber_T& operator-=( + DualNumber_T& a, + const DualNumber_T& b + ) { + a.real -= b.real; + for (size_t i = 0; i < NVar; ++i) { + a.dual[i] -= b.dual[i]; + } + return a; + } + + // Scalar multiplication assignment operator + template + inline DualNumber_T& operator*=( + DualNumber_T& a, + Scalar b + ) { + a.real *= b; + for (size_t i = 0; i < NVar; ++i) { + a.dual[i] *= b; + } + return a; + } + + // Element-wise multiplication assignment operator + template + inline DualNumber_T& operator*=( + DualNumber_T& a, + const DualNumber_T& b + ) { + Scalar ogReal = a.real; + for (size_t i = 0; i < NVar; ++i) { + a.dual[i] = ogReal * b.dual[i] + a.dual[i] * b.real; + } + a.real = ogReal * b.real; + return a; + } + + // Scalar division assignment operator + template + inline DualNumber_T& operator/=( + DualNumber_T& a, + Scalar b + ) { + a.real /= b; + for (size_t i = 0; i < NVar; ++i) { + a.dual[i] /= b; + } + return a; + } + + // Element-wise division assignment operator + template + inline DualNumber_T& operator/=( + DualNumber_T& a, + const DualNumber_T& b + ) { + Scalar ogReal = a.real; + for (size_t i = 0; i < NVar; ++i) { + a.dual[i] = (a.dual[i] * b.real - ogReal * b.dual[i]) / (b.real * b.real); + } + a.real = ogReal / b.real; + return a; + } + + // ----- // Numeric Limits Specialisation for DualNumber_T - // ---- + // ----- // TODO: Add specialisation of std::numeric_limits for DualNumber_T to define properties like infinity, NaN, epsilon, etc. based on the underlying Scalar type. } \ No newline at end of file From ffe16a3ed03fba30f9bcf7a087451d08fa740738 Mon Sep 17 00:00:00 2001 From: saltyjoss Date: Sun, 17 May 2026 12:03:39 +0100 Subject: [PATCH 017/210] refactor: Move current unit testing into PxM, added new `DualNumberTests`, separating each subset of tests into its own .exe --- CMakeLists.txt | 2 + DSFE_App/CMakeLists.txt | 1 - DSFE_App/DSFE_Core/CMakeLists.txt | 2 +- DSFE_App/DSFE_Unit_Tests/CMakeLists.txt | 36 ----- PxMLib/CMakeLists.txt | 34 +--- PxMLib/MathLib/CMakeLists.txt | 33 ++++ PxMLib/MathLib_Unit/CMakeLists.txt | 6 + .../MathLib_Unit/Common}/TestHarness.h | 0 .../DualNumberTests/ArithmeticTests.cpp | 149 ++++++++++++++++++ .../AssignmentOperatorTests.cpp | 83 ++++++++++ .../DualNumberTests/CMakeLists.txt | 41 +++++ .../DualNumberTests/DifferentiationTests.cpp | 100 ++++++++++++ .../DualNumberTests/DivisionTests.cpp | 91 +++++++++++ .../DualNumberTests/TranscendentalTests.cpp | 102 ++++++++++++ PxMLib/MathLib_Unit/DualNumberTests/main.cpp | 29 ++++ .../IntegratorTests}/AdaptiveMethodTests.cpp | 0 .../IntegratorTests/CMakeLists.txt | 40 +++++ .../ConvergenceOrderTests.cpp | 18 +-- .../IntegratorTests}/ExplicitMethodTests.cpp | 0 .../IntegratorTests}/ImplicitMethodTests.cpp | 0 .../MathLib_Unit/IntegratorTests}/main.cpp | 3 +- 21 files changed, 691 insertions(+), 79 deletions(-) delete mode 100644 DSFE_App/DSFE_Unit_Tests/CMakeLists.txt create mode 100644 PxMLib/MathLib/CMakeLists.txt create mode 100644 PxMLib/MathLib_Unit/CMakeLists.txt rename {DSFE_App/DSFE_Unit_Tests/src => PxMLib/MathLib_Unit/Common}/TestHarness.h (100%) create mode 100644 PxMLib/MathLib_Unit/DualNumberTests/ArithmeticTests.cpp create mode 100644 PxMLib/MathLib_Unit/DualNumberTests/AssignmentOperatorTests.cpp create mode 100644 PxMLib/MathLib_Unit/DualNumberTests/CMakeLists.txt create mode 100644 PxMLib/MathLib_Unit/DualNumberTests/DifferentiationTests.cpp create mode 100644 PxMLib/MathLib_Unit/DualNumberTests/DivisionTests.cpp create mode 100644 PxMLib/MathLib_Unit/DualNumberTests/TranscendentalTests.cpp create mode 100644 PxMLib/MathLib_Unit/DualNumberTests/main.cpp rename {DSFE_App/DSFE_Unit_Tests/src => PxMLib/MathLib_Unit/IntegratorTests}/AdaptiveMethodTests.cpp (100%) create mode 100644 PxMLib/MathLib_Unit/IntegratorTests/CMakeLists.txt rename {DSFE_App/DSFE_Unit_Tests/src => PxMLib/MathLib_Unit/IntegratorTests}/ConvergenceOrderTests.cpp (88%) rename {DSFE_App/DSFE_Unit_Tests/src => PxMLib/MathLib_Unit/IntegratorTests}/ExplicitMethodTests.cpp (100%) rename {DSFE_App/DSFE_Unit_Tests/src => PxMLib/MathLib_Unit/IntegratorTests}/ImplicitMethodTests.cpp (100%) rename {DSFE_App/DSFE_Unit_Tests/src => PxMLib/MathLib_Unit/IntegratorTests}/main.cpp (98%) diff --git a/CMakeLists.txt b/CMakeLists.txt index 1f8a85cf..a41f29ab 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -10,6 +10,7 @@ project(DSFE LANGUAGES C CXX) set(CMAKE_CXX_STANDARD 23) set(CMAKE_CXX_STANDARD_REQUIRED ON) +set(CMAKE_MSVC_RUNTIME_LIBRARY "MultiThreaded$<$:Debug>DLL") # Set common output directories for all targets set(CMAKE_ARCHIVE_OUTPUT_DIRECTORY ${CMAKE_BINARY_DIR}/lib) @@ -58,6 +59,7 @@ FetchContent_Declare( GIT_TAG v1.14.0 GIT_SHALLOW TRUE ) +set(gtest_force_shared_crt ON CACHE BOOL "" FORCE) FetchContent_MakeAvailable(googletest) FetchContent_Declare( diff --git a/DSFE_App/CMakeLists.txt b/DSFE_App/CMakeLists.txt index c594df20..1fd34a64 100644 --- a/DSFE_App/CMakeLists.txt +++ b/DSFE_App/CMakeLists.txt @@ -6,4 +6,3 @@ project(DSFE_App LANGUAGES C CXX) add_subdirectory(DSFE_Core) add_subdirectory(DSFE_GUI) add_subdirectory(DSFE_Engine) -add_subdirectory(DSFE_Unit_Tests) diff --git a/DSFE_App/DSFE_Core/CMakeLists.txt b/DSFE_App/DSFE_Core/CMakeLists.txt index feac49ce..aae5a99b 100644 --- a/DSFE_App/DSFE_Core/CMakeLists.txt +++ b/DSFE_App/DSFE_Core/CMakeLists.txt @@ -84,7 +84,7 @@ target_precompile_headers(DSFE_Core PRIVATE "${CMAKE_CURRENT_SOURCE_DIR}/pch.h") # Link DSFE_Core against its dependencies target_link_libraries(DSFE_Core PUBLIC - PxMLib + MathLib Eigen3::Eigen hdf5::hdf5-shared Threads::Threads diff --git a/DSFE_App/DSFE_Unit_Tests/CMakeLists.txt b/DSFE_App/DSFE_Unit_Tests/CMakeLists.txt deleted file mode 100644 index 0da1fde0..00000000 --- a/DSFE_App/DSFE_Unit_Tests/CMakeLists.txt +++ /dev/null @@ -1,36 +0,0 @@ -# CMakeLists.txt for DSFE_Unit_Tests executable -cmake_minimum_required(VERSION 3.10) -project(DSFE_Unit_Tests LANGUAGES C CXX) - -enable_testing() - -add_executable(DSFE_Unit_Tests - src/main.cpp - src/AdaptiveMethodTests.cpp - src/ConvergenceOrderTests.cpp - src/ExplicitMethodTests.cpp - src/ImplicitMethodTests.cpp -) - -target_link_libraries(DSFE_Unit_Tests PRIVATE - DSFE_Core - gtest_main -) - -include(GoogleTest) - -gtest_discover_tests( - DSFE_Unit_Tests - DISCOVERY_MODE PRE_TEST - DISCOVERY_TIMEOUT 10 -) - -# if (CMAKE_BUILD_TYPE STREQUAL "Debug") -# gtest_discover_tests( -# DSFE_Unit_Tests -# DISCOVERY_MODE PRE_TEST -# DISCOVERY_TIMEOUT 10 -# ) -# else() -# gtest_discover_tests(DSFE_Unit_Tests) -# endif() diff --git a/PxMLib/CMakeLists.txt b/PxMLib/CMakeLists.txt index f5920670..73994c9f 100644 --- a/PxMLib/CMakeLists.txt +++ b/PxMLib/CMakeLists.txt @@ -1,33 +1,7 @@ -# CMakeLists.txt for MathLib - +# CMakeLists.txt for PxMLib project cmake_minimum_required(VERSION 3.10) project(PxMLib LANGUAGES CXX) -add_library(PxMLib STATIC - ${CMAKE_CURRENT_SOURCE_DIR}/MathLib/pch.cpp - ${CMAKE_CURRENT_SOURCE_DIR}/MathLib/src/control/Error.cpp - ${CMAKE_CURRENT_SOURCE_DIR}/MathLib/src/control/PID.cpp - ${CMAKE_CURRENT_SOURCE_DIR}/MathLib/src/dynamics/MassMatrix.cpp - ${CMAKE_CURRENT_SOURCE_DIR}/MathLib/src/dynamics/Nonlinear_Terms.cpp - ${CMAKE_CURRENT_SOURCE_DIR}/MathLib/src/dynamics/StateSpace.cpp - ${CMAKE_CURRENT_SOURCE_DIR}/MathLib/src/integrators/Integration.cpp - ${CMAKE_CURRENT_SOURCE_DIR}/MathLib/src/integrators/IntegrationAnalysis.cpp - ${CMAKE_CURRENT_SOURCE_DIR}/MathLib/src/kinematics/DH_Params.cpp - ${CMAKE_CURRENT_SOURCE_DIR}/MathLib/src/kinematics/Jacobian.cpp - ${CMAKE_CURRENT_SOURCE_DIR}/MathLib/src/kinematics/Transform_Utils.cpp -) - -target_precompile_headers(PxMLib PRIVATE - ${CMAKE_CURRENT_SOURCE_DIR}/MathLib/pch.h -) - -target_include_directories(PxMLib - PUBLIC - $ - PRIVATE - ${CMAKE_CURRENT_SOURCE_DIR}/MathLib/ -) - -target_link_libraries(PxMLib PUBLIC - Eigen3::Eigen -) \ No newline at end of file +# Add projects within the DSFE_App directory +add_subdirectory(MathLib) +add_subdirectory(MathLib_Unit) diff --git a/PxMLib/MathLib/CMakeLists.txt b/PxMLib/MathLib/CMakeLists.txt new file mode 100644 index 00000000..d36da191 --- /dev/null +++ b/PxMLib/MathLib/CMakeLists.txt @@ -0,0 +1,33 @@ +# CMakeLists.txt for MathLib + +cmake_minimum_required(VERSION 3.10) +project(MathLib LANGUAGES CXX) + +add_library(MathLib STATIC + ${CMAKE_CURRENT_SOURCE_DIR}/pch.cpp + ${CMAKE_CURRENT_SOURCE_DIR}/src/control/Error.cpp + ${CMAKE_CURRENT_SOURCE_DIR}/src/control/PID.cpp + ${CMAKE_CURRENT_SOURCE_DIR}/src/dynamics/MassMatrix.cpp + ${CMAKE_CURRENT_SOURCE_DIR}/src/dynamics/Nonlinear_Terms.cpp + ${CMAKE_CURRENT_SOURCE_DIR}/src/dynamics/StateSpace.cpp + ${CMAKE_CURRENT_SOURCE_DIR}/src/integrators/Integration.cpp + ${CMAKE_CURRENT_SOURCE_DIR}/src/integrators/IntegrationAnalysis.cpp + ${CMAKE_CURRENT_SOURCE_DIR}/src/kinematics/DH_Params.cpp + ${CMAKE_CURRENT_SOURCE_DIR}/src/kinematics/Jacobian.cpp + ${CMAKE_CURRENT_SOURCE_DIR}/src/kinematics/Transform_Utils.cpp +) + +target_precompile_headers(MathLib PRIVATE + ${CMAKE_CURRENT_SOURCE_DIR}/pch.h +) + +target_include_directories(MathLib + PUBLIC + $ + PRIVATE + ${CMAKE_CURRENT_SOURCE_DIR}/ +) + +target_link_libraries(MathLib PUBLIC + Eigen3::Eigen +) \ No newline at end of file diff --git a/PxMLib/MathLib_Unit/CMakeLists.txt b/PxMLib/MathLib_Unit/CMakeLists.txt new file mode 100644 index 00000000..63eb4b8b --- /dev/null +++ b/PxMLib/MathLib_Unit/CMakeLists.txt @@ -0,0 +1,6 @@ +# CMakeLists.txt for MathLib_Unit_Tests executable +cmake_minimum_required(VERSION 3.10) +project(MathLib_Unit_Tests LANGUAGES C CXX) + +add_subdirectory(IntegratorTests) +add_subdirectory(DualNumberTests) \ No newline at end of file diff --git a/DSFE_App/DSFE_Unit_Tests/src/TestHarness.h b/PxMLib/MathLib_Unit/Common/TestHarness.h similarity index 100% rename from DSFE_App/DSFE_Unit_Tests/src/TestHarness.h rename to PxMLib/MathLib_Unit/Common/TestHarness.h diff --git a/PxMLib/MathLib_Unit/DualNumberTests/ArithmeticTests.cpp b/PxMLib/MathLib_Unit/DualNumberTests/ArithmeticTests.cpp new file mode 100644 index 00000000..7d8d687d --- /dev/null +++ b/PxMLib/MathLib_Unit/DualNumberTests/ArithmeticTests.cpp @@ -0,0 +1,149 @@ +// MathLib_UnitTests ArithmeticTests.cpp + +#include "TestHarness.h" +#include +#include +#include + +using namespace mathlib; + +namespace { + // Test that addition of dual numbers works correctly + TEST("Basic Arithmetic", DualNumber_Addition) { + DualNumber_T a(1.0, { 0.5, 0.25 }); + DualNumber_T b(2.0, { 0.1, 0.05 }); + DualNumber_T c = a + b; + ASSERT_TRUE(std::abs(c.real - 3.0) < 1e-12, "Addition real part incorrect"); + ASSERT_TRUE(std::abs(c.dual[0] - 0.6) < 1e-12, "Addition dual[0] part incorrect"); + ASSERT_TRUE(std::abs(c.dual[1] - 0.3) < 1e-12, "Addition dual[1] part incorrect"); + } + + // Test subtraction of dual numbers works correctly + TEST("Basic Arithmetic", DualNumber_Subtraction) { + DualNumber_T a(1.0, { 0.5, 0.25 }); + DualNumber_T b(2.0, { 0.1, 0.05 }); + DualNumber_T c = a - b; + ASSERT_TRUE(std::abs(c.real + 1.0) < 1e-12, "Subtraction real part incorrect"); + ASSERT_TRUE(std::abs(c.dual[0] - 0.4) < 1e-12, "Subtraction dual[0] part incorrect"); + ASSERT_TRUE(std::abs(c.dual[1] - 0.2) < 1e-12, "Subtraction dual[1] part incorrect"); + } + // Test that addition and subtraction are inverses for dual numbers + TEST("Basic Arithmetic", DualNumber_AddSubInverse) { + DualNumber_T a(1.0, { 0.5, 0.25 }); + DualNumber_T b(2.0, { 0.1, 0.05 }); + DualNumber_T c = (a + b) - b; + ASSERT_TRUE(std::abs(c.real - a.real) < 1e-12, "Add/Sub inverse real part incorrect"); + ASSERT_TRUE(std::abs(c.dual[0] - a.dual[0]) < 1e-12, "Add/Sub inverse dual[0] part incorrect"); + ASSERT_TRUE(std::abs(c.dual[1] - a.dual[1]) < 1e-12, "Add/Sub inverse dual[1] part incorrect"); + } + + // Test that multiplication of dual numbers works correctly + TEST("Multiplication", DualNumber_Multiplication_1Var) { + DualNumber_T a(1.0, { 0.5 }); + DualNumber_T b(2.0, { 0.1 }); + DualNumber_T c = a * b; + ASSERT_TRUE(std::abs(c.real - 2.0) < 1e-12, "Multiplication real part incorrect"); + ASSERT_TRUE(std::abs(c.dual[0] - (1.0 * 0.1 + 2.0 * 0.5)) < 1e-12, "Multiplication dual part incorrect"); + } + // Test that multiplication of dual numbers with variable value of two works correctly + TEST("Multiplication", DualNumber_Multiplication_2Var) { + DualNumber_T a(1.0, { 0.5, 0.25 }); + DualNumber_T b(2.0, { 0.1, 0.05 }); + DualNumber_T c = a * b; + ASSERT_TRUE(std::abs(c.real - 2.0) < 1e-12, "Multiplication real part incorrect"); + ASSERT_TRUE(std::abs(c.dual[0] - (1.0 * 0.1 + 2.0 * 0.5)) < 1e-12, "Multiplication dual[0] part incorrect"); + ASSERT_TRUE(std::abs(c.dual[1] - (1.0 * 0.05 + 2.0 * 0.25)) < 1e-12, "Multiplication dual[1] part incorrect"); + } + // Test that multiplication of dual numbers works correctly with float type + TEST("Multiplication", DualNumber_Multiplication_f) { + DualNumber_T a(1.0f, { 0.5f }); + DualNumber_T b(2.0f, { 0.1f }); + DualNumber_T c = a * b; + ASSERT_TRUE(std::abs(c.real - 2.0f) < 1e-5f, "Multiplication real part incorrect"); + ASSERT_TRUE(std::abs(c.dual[0] - (1.0f * 0.1f + 2.0f * 0.5f)) < 1e-5f, "Multiplication dual part incorrect"); + } + // Test that multiplication distributes over addition for dual numbers + TEST("Multiplication", DualNumber_Distributivity) { + DualNumber_T a(1.0, { 0.5, 0.25 }); + DualNumber_T b(2.0, { 0.1, 0.05 }); + DualNumber_T c(3.0, { 0.2, 0.1 }); + DualNumber_T left = a * (b + c); + DualNumber_T right = a * b + a * c; + ASSERT_TRUE(std::abs(left.real - right.real) < 1e-12, "Distributivity real part incorrect"); + ASSERT_TRUE(std::abs(left.dual[0] - right.dual[0]) < 1e-12, "Distributivity dual[0] part incorrect"); + ASSERT_TRUE(std::abs(left.dual[1] - right.dual[1]) < 1e-12, "Distributivity dual[1] part incorrect"); + } + // Test that multiplication is commutative for dual numbers + TEST("Multiplication", DualNumber_Commutativity) { + DualNumber_T a(1.0, { 0.5, 0.25 }); + DualNumber_T b(2.0, { 0.1, 0.05 }); + DualNumber_T ab = a * b; + DualNumber_T ba = b * a; + ASSERT_TRUE(std::abs(ab.real - ba.real) < 1e-12, "Commutativity real part incorrect"); + ASSERT_TRUE(std::abs(ab.dual[0] - ba.dual[0]) < 1e-12, "Commutativity dual[0] part incorrect"); + ASSERT_TRUE(std::abs(ab.dual[1] - ba.dual[1]) < 1e-12, "Commutativity dual[1] part incorrect"); + } + // Test that multiplication is associative for dual numbers + TEST("Multiplication", DualNumber_Associativity) { + DualNumber_T a(1.0, { 0.5, 0.25 }); + DualNumber_T b(2.0, { 0.1, 0.05 }); + DualNumber_T c(3.0, { 0.2, 0.1 }); + DualNumber_T ab_c = (a * b) * c; + DualNumber_T a_bc = a * (b * c); + ASSERT_TRUE(std::abs(ab_c.real - a_bc.real) < 1e-12, "Associativity real part incorrect"); + ASSERT_TRUE(std::abs(ab_c.dual[0] - a_bc.dual[0]) < 1e-12, "Associativity dual[0] part incorrect"); + ASSERT_TRUE(std::abs(ab_c.dual[1] - a_bc.dual[1]) < 1e-12, "Associativity dual[1] part incorrect"); + } + // Test that multiplication by the identity element works correctly for dual numbers + TEST("Multiplication", DualNumber_Identity) { + DualNumber_T a(1.0, { 0.5, 0.25 }); + DualNumber_T identity(1.0, { 0.0, 0.0 }); + DualNumber_T left = a * identity; + DualNumber_T right = identity * a; + ASSERT_TRUE(std::abs(left.real - a.real) < 1e-12, "Identity left real part incorrect"); + ASSERT_TRUE(std::abs(left.dual[0] - a.dual[0]) < 1e-12, "Identity left dual[0] part incorrect"); + ASSERT_TRUE(std::abs(left.dual[1] - a.dual[1]) < 1e-12, "Identity left dual[1] part incorrect"); + ASSERT_TRUE(std::abs(right.real - a.real) < 1e-12, "Identity right real part incorrect"); + ASSERT_TRUE(std::abs(right.dual[0] - a.dual[0]) < 1e-12, "Identity right dual[0] part incorrect"); + ASSERT_TRUE(std::abs(right.dual[1] - a.dual[1]) < 1e-12, "Identity right dual[1] part incorrect"); + } + // Test that multiplication by zero works correctly for dual numbers + TEST("Multiplication", DualNumber_ZeroMultiplication) { + DualNumber_T a(1.0, { 0.5, 0.25 }); + DualNumber_T zero(0.0, { 0.0, 0.0 }); + DualNumber_T left = a * zero; + DualNumber_T right = zero * a; + ASSERT_TRUE(std::abs(left.real) < 1e-12, "Zero multiplication left real part incorrect"); + ASSERT_TRUE(std::abs(left.dual[0]) < 1e-12, "Zero multiplication left dual[0] part incorrect"); + ASSERT_TRUE(std::abs(left.dual[1]) < 1e-12, "Zero multiplication left dual[1] part incorrect"); + ASSERT_TRUE(std::abs(right.real) < 1e-12, "Zero multiplication right real part incorrect"); + ASSERT_TRUE(std::abs(right.dual[0]) < 1e-12, "Zero multiplication right dual[0] part incorrect"); + ASSERT_TRUE(std::abs(right.dual[1]) < 1e-12, "Zero multiplication right dual[1] part incorrect"); + } + // Test that multiplication by a scalar works correctly for dual numbers + TEST("Multiplication", DualNumber_ScalarMultiplication) { + DualNumber_T a(1.0, { 0.5, 0.25 }); + double scalar = 2.0; + DualNumber_T left = a * scalar; + DualNumber_T right = scalar * a; + ASSERT_TRUE(std::abs(left.real - 2.0) < 1e-12, "Scalar multiplication left real part incorrect"); + ASSERT_TRUE(std::abs(left.dual[0] - 1.0) < 1e-12, "Scalar multiplication left dual[0] part incorrect"); + ASSERT_TRUE(std::abs(left.dual[1] - 0.5) < 1e-12, "Scalar multiplication left dual[1] part incorrect"); + ASSERT_TRUE(std::abs(right.real - 2.0) < 1e-12, "Scalar multiplication right real part incorrect"); + ASSERT_TRUE(std::abs(right.dual[0] - 1.0) < 1e-12, "Scalar multiplication right dual[0] part incorrect"); + ASSERT_TRUE(std::abs(right.dual[1] - 0.5) < 1e-12, "Scalar multiplication right dual[1] part incorrect"); + } + // Test that multiplication by a negative scalar works correctly for dual numbers + TEST("Multiplication", DualNumber_NegativeScalarMultiplication) { + DualNumber_T a(1.0, { 0.5, 0.25 }); + double scalar = -2.0; + DualNumber_T left = a * scalar; + DualNumber_T right = scalar * a; + ASSERT_TRUE(std::abs(left.real + 2.0) < 1e-12, "Negative scalar multiplication left real part incorrect"); + ASSERT_TRUE(std::abs(left.dual[0] + 1.0) < 1e-12, "Negative scalar multiplication left dual[0] part incorrect"); + ASSERT_TRUE(std::abs(left.dual[1] + 0.5) < 1e-12, "Negative scalar multiplication left dual[1] part incorrect"); + ASSERT_TRUE(std::abs(right.real + 2.0) < 1e-12, "Negative scalar multiplication right real part incorrect"); + ASSERT_TRUE(std::abs(right.dual[0] + 1.0) < 1e-12, "Negative scalar multiplication right dual[0] part incorrect"); + ASSERT_TRUE(std::abs(right.dual[1] + 0.5) < 1e-12, "Negative scalar multiplication right dual[1] part incorrect"); + } +} \ No newline at end of file diff --git a/PxMLib/MathLib_Unit/DualNumberTests/AssignmentOperatorTests.cpp b/PxMLib/MathLib_Unit/DualNumberTests/AssignmentOperatorTests.cpp new file mode 100644 index 00000000..fb1f7ff0 --- /dev/null +++ b/PxMLib/MathLib_Unit/DualNumberTests/AssignmentOperatorTests.cpp @@ -0,0 +1,83 @@ +// MathLib_UnitTests AssignmentOperatorTests.cpp + +#include "TestHarness.h" +#include +#include +#include + +using namespace mathlib; + +namespace { + // Test that addition assignment works correctly for dual numbers + TEST("Assignment Operators", DualNumber_AdditionAssignment) { + DualNumber_T a(1.0, { 0.5, 0.25 }); + DualNumber_T b(2.0, { 0.1, 0.05 }); + a += b; + ASSERT_TRUE(std::abs(a.real - 3.0) < 1e-12, "Addition assignment real part incorrect"); + ASSERT_TRUE(std::abs(a.dual[0] - 0.6) < 1e-12, "Addition assignment dual[0] part incorrect"); + ASSERT_TRUE(std::abs(a.dual[1] - 0.3) < 1e-12, "Addition assignment dual[1] part incorrect"); + } + // Test that subtraction assignment works correctly for dual numbers + TEST("Assignment Operators", DualNumber_SubtractionAssignment) { + DualNumber_T a(1.0, { 0.5, 0.25 }); + DualNumber_T b(2.0, { 0.1, 0.05 }); + a -= b; + ASSERT_TRUE(std::abs(a.real + 1.0) < 1e-12, "Subtraction assignment real part incorrect"); + ASSERT_TRUE(std::abs(a.dual[0] - 0.4) < 1e-12, "Subtraction assignment dual[0] part incorrect"); + ASSERT_TRUE(std::abs(a.dual[1] - 0.2) < 1e-12, "Subtraction assignment dual[1] part incorrect"); + } + // Test that addition assignment and subtraction assignment are inverses for dual numbers + TEST("Assignment Operators", DualNumber_AddSubAssignmentInverse) { + DualNumber_T a(1.0, { 0.5, 0.25 }); + DualNumber_T b(2.0, { 0.1, 0.05 }); + a += b; + a -= b; + ASSERT_TRUE(std::abs(a.real - 1.0) < 1e-12, "Add/Sub assignment inverse real part incorrect"); + ASSERT_TRUE(std::abs(a.dual[0] - 0.5) < 1e-12, "Add/Sub assignment inverse dual[0] part incorrect"); + ASSERT_TRUE(std::abs(a.dual[1] - 0.25) < 1e-12, "Add/Sub assignment inverse dual[1] part incorrect"); + } + // Test that multiplication assignment works correctly for dual numbers + TEST("Assignment Operators", DualNumber_MultiplicationAssignment) { + DualNumber_T a(1.0, { 0.5, 0.25 }); + DualNumber_T b(2.0, { 0.1, 0.05 }); + a *= b; + ASSERT_TRUE(std::abs(a.real - 2.0) < 1e-12, "Multiplication assignment real part incorrect"); + ASSERT_TRUE(std::abs(a.dual[0] - (1.0 * 0.1 + 2.0 * 0.5)) < 1e-12, "Multiplication assignment dual[0] part incorrect"); + ASSERT_TRUE(std::abs(a.dual[1] - (1.0 * 0.05 + 2.0 * 0.25)) < 1e-12, "Multiplication assignment dual[1] part incorrect"); + } + // Test that division assignment works correctly for dual numbers + TEST("Assignment Operators", DualNumber_DivisionAssignment) { + DualNumber_T a(1.0, { 0.5, 0.25 }); + DualNumber_T b(2.0, { 0.1, 0.05 }); + a /= b; + double expected0 = (0.5f * 2.0f - 1.0f * 0.1f) / (2.0f * 2.0f); + double expected1 = (0.25f * 2.0f - 1.0f * 0.05f) / (2.0f * 2.0f); + ASSERT_TRUE(std::abs(a.real - 0.5) < 1e-12, "Division real part incorrect"); + ASSERT_TRUE(std::abs(a.dual[0] - expected0) < 1e-12, "Division dual[0] part incorrect"); + ASSERT_TRUE(std::abs(a.dual[1] - expected1) < 1e-12, "Division dual[1] part incorrect"); + } + // Test that division assignment by zero throws an exception for dual numbers + TEST("Assignment Operators", DualNumber_DivisionAssignmentByZero) { + DualNumber_T a(1.0, { 0.5, 0.25 }); + double scalar = 0.0; + try { + a /= scalar; + ASSERT_TRUE(false, "Division assignment by zero did not throw an exception"); + } + catch (const std::exception&) { + // Expected to catch an exception + } + } + // Test that division assignment by a dual number with zero real part throws an exception for dual numbers + TEST("Assignment Operators", DualNumber_DivisionAssignmentByZeroReal) { + DualNumber_T a(1.0, { 0.5, 0.25 }); + DualNumber_T zeroReal(0.0, { 0.1, 0.05 }); + try { + a /= zeroReal; + ASSERT_TRUE(false, "Division assignment by dual number with zero real part did not throw an exception"); + } + catch (const std::exception&) { + // Expected to catch an exception + } + } +} \ No newline at end of file diff --git a/PxMLib/MathLib_Unit/DualNumberTests/CMakeLists.txt b/PxMLib/MathLib_Unit/DualNumberTests/CMakeLists.txt new file mode 100644 index 00000000..1f488b9d --- /dev/null +++ b/PxMLib/MathLib_Unit/DualNumberTests/CMakeLists.txt @@ -0,0 +1,41 @@ +# CMakeLists.txt for MathLib_Unit_Tests executable +cmake_minimum_required(VERSION 3.10) +project(MathLib_DualNumberTests LANGUAGES C CXX) + +enable_testing() + +add_executable(MathLib_DualNumberTests + main.cpp + ArithmeticTests.cpp + AssignmentOperatorTests.cpp + DivisionTests.cpp + DifferentiationTests.cpp + TranscendentalTests.cpp +) + +target_link_libraries(MathLib_DualNumberTests PRIVATE + MathLib + gtest_main +) + +target_include_directories(MathLib_DualNumberTests PRIVATE + ${CMAKE_CURRENT_SOURCE_DIR}/../Common +) + +include(GoogleTest) + +gtest_discover_tests( + MathLib_DualNumberTests + DISCOVERY_MODE PRE_TEST + DISCOVERY_TIMEOUT 10 +) + +# if (CMAKE_BUILD_TYPE STREQUAL "Debug") +# gtest_discover_tests( +# MathLib_DualNumberTests +# DISCOVERY_MODE PRE_TEST +# DISCOVERY_TIMEOUT 10 +# ) +# else() +# gtest_discover_tests(MathLib_DualNumberTests) +# endif() diff --git a/PxMLib/MathLib_Unit/DualNumberTests/DifferentiationTests.cpp b/PxMLib/MathLib_Unit/DualNumberTests/DifferentiationTests.cpp new file mode 100644 index 00000000..44d23fa4 --- /dev/null +++ b/PxMLib/MathLib_Unit/DualNumberTests/DifferentiationTests.cpp @@ -0,0 +1,100 @@ +// MathLib_UnitTests DifferentiationTests.cpp + +#include "TestHarness.h" +#include +#include +#include + +using namespace mathlib; + +namespace { + // Test that the derivative of a simple polynomial function is computed correctly using dual numbers + TEST("Differentiation", DualNumber_PolynomialDerivative) { + auto f = [](const DualNumber_T& x) { + return 3.0 * x * x + 2.0 * x + 1.0; + }; + DualNumber_T x(1.0, { 1.0 }); // Set dual part to 1 to compute derivative + DualNumber_T y = f(x); + ASSERT_TRUE(std::abs(y.real - 6.0) < 1e-12, "Polynomial function value incorrect"); + ASSERT_TRUE(std::abs(y.dual[0] - 4.0) < 1e-12, "Polynomial derivative incorrect"); + } + // Test that the derivative of x^2 is computed correctly by the product rule using dual numbers + TEST("Differentiation", DualNumber_ProductRule_xSquared) { + auto f = [](const DualNumber_T& x) { + return x * x; // f(x) = x^2 + }; + DualNumber_T x(2.0, { 1.0 }); // Set dual part to 1 to compute derivative + DualNumber_T y = f(x); + ASSERT_TRUE(std::abs(y.real - 4.0) < 1e-12, "Product rule function value incorrect"); + ASSERT_TRUE(std::abs(y.dual[0] - 8.0) < 1e-12, "Product rule derivative incorrect"); + } + // Test that the sine function's derivative is computed correctly using dual numbers + TEST("Differentiation", DualNumber_SineDerivative) { + auto f = [](const DualNumber_T& x) { + return sin(x); + }; + DualNumber_T x(0.5, { 1.0 }); // Set dual part to 1 to compute derivative + DualNumber_T y = f(x); + ASSERT_TRUE(std::abs(y.real - std::sin(0.5)) < 1e-12, "Sine function value incorrect"); + ASSERT_TRUE(std::abs(y.dual[0] - std::cos(0.5)) < 1e-12, "Sine derivative incorrect"); + } + // Test that the cosine function's derivative is computed correctly using dual numbers + TEST("Differentiation", DualNumber_CosineDerivative) { + auto f = [](const DualNumber_T& x) { + return cos(x); + }; + DualNumber_T x(0.5, { 1.0 }); // Set dual part to 1 to compute derivative + DualNumber_T y = f(x); + ASSERT_TRUE(std::abs(y.real - std::cos(0.5)) < 1e-12, "Cosine function value incorrect"); + ASSERT_TRUE(std::abs(y.dual[0] + std::sin(0.5)) < 1e-12, "Cosine derivative incorrect"); + } + // Test that the tangent function's derivative is computed correctly using dual numbers + TEST("Differentiation", DualNumber_TangentDerivative) { + auto f = [](const DualNumber_T& x) { + return tan(x); + }; + DualNumber_T x(0.5, { 1.0 }); // Set dual part to 1 to compute derivative + DualNumber_T y = f(x); + ASSERT_TRUE(std::abs(y.real - std::tan(0.5)) < 1e-12, "Tangent function value incorrect"); + double sec2 = 1.0 / (std::cos(0.5) * std::cos(0.5)); + ASSERT_TRUE(std::abs(y.dual[0] - sec2) < 1e-12, "Tangent derivative incorrect"); + } + // Test that the derivative of the hyperbolic tangent function is computed correctly using dual numbers + TEST("Differentiation", DualNumber_HyperbolicTangentDerivative) { + auto f = [](const DualNumber_T& x) { + return tanh(x); + }; + DualNumber_T x(0.5, { 1.0 }); // Set dual part to 1 to compute derivative + DualNumber_T y = f(x); + ASSERT_TRUE(std::abs(y.real - std::tanh(0.5)) < 1e-12, "Hyperbolic tangent function value incorrect"); + double sech2 = 1.0 / (std::cosh(0.5) * std::cosh(0.5)); + ASSERT_TRUE(std::abs(y.dual[0] - sech2) < 1e-12, "Hyperbolic tangent derivative incorrect"); + } + // Test that the partial derivatives of a multivariable function are computed correctly using dual numbers + TEST("Differentiation", DualNumber_MultivariableFunction) { + auto f = [](const DualNumber_T& x, const DualNumber_T& y) { + return 3.0 * x * x + 2.0 * y + sin(x); // f(x, y) = 3x^2 + 2y + sin(x) + }; + + DualNumber_T x(2.0, { 1.0, 0.0 }); // Set dual part to (1, 0) to compute partial derivative with respect to x + DualNumber_T y(3.0, { 0.0, 1.0 }); // Set dual part to (0, 1) to compute partial derivative with respect to y + + double dfdx = 6.0 * x.real + std::cos(x.real); // df/dx = 6x + cos(x) + double dfdy = 2.0; // df/dy = 2 + + ASSERT_TRUE(std::abs(f(x, y).real - (3.0 * 4.0 + 2.0 * 3.0 + std::sin(2.0))) < 1e-12, "Multivariable function value incorrect"); + ASSERT_TRUE(std::abs(f(x, y).dual[0] - dfdx) < 1e-12, "Partial derivative with respect to x incorrect"); + ASSERT_TRUE(std::abs(f(x, y).dual[1] - dfdy) < 1e-12, "Partial derivative with respect to y incorrect"); + } + // Test that the exponential function's derivative is computed correctly using dual numbers + TEST("Differentiation", DualNumber_ExponentialDerivative) { + auto f = [](const DualNumber_T& x) { + return exp(x); + }; + DualNumber_T x(0.5, { 1.0 }); // Set dual part to 1 to compute derivative + DualNumber_T y = f(x); + double expected0 = std::exp(0.5); // f(x) = exp(x) -> f'(x) = exp(x) + ASSERT_TRUE(std::abs(y.real - expected0) < 1e-12, "Exponential function value incorrect"); + ASSERT_TRUE(std::abs(y.dual[0] - expected0) < 1e-12, "Exponential derivative incorrect"); + } +} \ No newline at end of file diff --git a/PxMLib/MathLib_Unit/DualNumberTests/DivisionTests.cpp b/PxMLib/MathLib_Unit/DualNumberTests/DivisionTests.cpp new file mode 100644 index 00000000..fc34691d --- /dev/null +++ b/PxMLib/MathLib_Unit/DualNumberTests/DivisionTests.cpp @@ -0,0 +1,91 @@ +// MathLib_UnitTests DivisionTests.cpp + +#include "TestHarness.h" +#include +#include +#include + +using namespace mathlib; + +namespace { + // Test that division of dual numbers works with variable value of two + TEST("Division", DualNumber_Division_1Var) { + DualNumber_T a(1.0, { 0.5 }); + DualNumber_T b(2.0, { 0.1 }); + DualNumber_T c = a / b; + double expected0 = (0.5f * 2.0f - 1.0f * 0.1f) / (2.0f * 2.0f); + ASSERT_TRUE(std::abs(c.real - 0.5) < 1e-12, "Division real part incorrect"); + ASSERT_TRUE(std::abs(c.dual[0] - expected0) < 1e-12, "Division dual[0] part incorrect"); + } + // Test that division of dual numbers works with variable value of two + TEST("Division", DualNumber_Division_2Var) { + DualNumber_T a(1.0, { 0.5, 0.25 }); + DualNumber_T b(2.0, { 0.1, 0.05 }); + DualNumber_T c = a / b; + double expected0 = (0.5f * 2.0f - 1.0f * 0.1f) / (2.0f * 2.0f); + double expected1 = (0.25f * 2.0f - 1.0f * 0.05f) / (2.0f * 2.0f); + ASSERT_TRUE(std::abs(c.real - 0.5) < 1e-12, "Division real part incorrect"); + ASSERT_TRUE(std::abs(c.dual[0] - expected0) < 1e-12, "Division dual[0] part incorrect"); + ASSERT_TRUE(std::abs(c.dual[1] - expected1) < 1e-12, "Division dual[1] part incorrect"); + } + // Test that division of a dual number defined with a float and double type works correctly for dual numbers + TEST("Division", DualNumber_Division_f) { + DualNumber_T a(1.0f, { 0.5f }); + DualNumber_T b(2.0f, { 0.1f }); + DualNumber_T c = a / b; + double expected0 = (0.5f * 2.0f - 1.0f * 0.1f) / (2.0f * 2.0f); + ASSERT_TRUE(std::abs(c.real - 0.5f) < 1e-5f, "Division real part incorrect"); + ASSERT_TRUE(std::abs(c.dual[0] - expected0) < 1e-5f, "Division dual[0] part incorrect"); + } + // Test that division by the identity element works correctly for dual numbers + TEST("Division", DualNumber_DivisionIdentity) { + DualNumber_T a(1.0, { 0.5, 0.25 }); + DualNumber_T identity(1.0, { 0.0, 0.0 }); + DualNumber_T left = a / identity; + ASSERT_TRUE(std::abs(left.real - a.real) < 1e-12, "Division by identity real part incorrect"); + ASSERT_TRUE(std::abs(left.dual[0] - a.dual[0]) < 1e-12, "Division by identity dual[0] part incorrect"); + ASSERT_TRUE(std::abs(left.dual[1] - a.dual[1]) < 1e-12, "Division by identity dual[1] part incorrect"); + } + // Test that division by zero throws an exception for dual numbers + TEST("Division", DualNumber_DivisionByScalar) { + DualNumber_T a(1.0, { 0.5, 0.25 }); + double scalar = 2.0; + DualNumber_T left = a / scalar; + ASSERT_TRUE(std::abs(left.real - 0.5) < 1e-12, "Division by scalar real part incorrect"); + ASSERT_TRUE(std::abs(left.dual[0] - 0.25) < 1e-12, "Division by scalar dual[0] part incorrect"); + ASSERT_TRUE(std::abs(left.dual[1] - 0.125) < 1e-12, "Division by scalar dual[1] part incorrect"); + } + // Test that division by a negative scalar works correctly for dual numbers + TEST("Division", DualNumber_DivisionByNegativeScalar) { + DualNumber_T a(1.0, { 0.5, 0.25 }); + double scalar = -2.0; + DualNumber_T left = a / scalar; + ASSERT_TRUE(std::abs(left.real + 0.5) < 1e-12, "Division by negative scalar real part incorrect"); + ASSERT_TRUE(std::abs(left.dual[0] + 0.25) < 1e-12, "Division by negative scalar dual[0] part incorrect"); + ASSERT_TRUE(std::abs(left.dual[1] + 0.125) < 1e-12, "Division by negative scalar dual[1] part incorrect"); + } + // Test that division by zero throws an exception for dual numbers + TEST("Division", DualNumber_DivisionByZero) { + DualNumber_T a(1.0, { 0.5, 0.25 }); + double scalar = 0.0; + try { + DualNumber_T left = a / scalar; + ASSERT_TRUE(false, "Division by zero did not throw an exception"); + } + catch (const std::exception&) { + // Expected to catch an exception + } + } + // Test that division by a dual number with zero real part throws an exception for dual numbers + TEST("Division", DualNumber_DivisionByZeroReal) { + DualNumber_T a(1.0, { 0.5, 0.25 }); + DualNumber_T zeroReal(0.0, { 0.1, 0.05 }); + try { + DualNumber_T left = a / zeroReal; + ASSERT_TRUE(false, "Division by dual number with zero real part did not throw an exception"); + } + catch (const std::exception&) { + // Expected to catch an exception + } + } +} \ No newline at end of file diff --git a/PxMLib/MathLib_Unit/DualNumberTests/TranscendentalTests.cpp b/PxMLib/MathLib_Unit/DualNumberTests/TranscendentalTests.cpp new file mode 100644 index 00000000..4c6f4b22 --- /dev/null +++ b/PxMLib/MathLib_Unit/DualNumberTests/TranscendentalTests.cpp @@ -0,0 +1,102 @@ +// MathLib_UnitTests TranscendentalTests.cpp + +#include "TestHarness.h" +#include +#include +#include + +using namespace mathlib; + +namespace { + // Test that the sine function works correctly for dual + TEST("Transcendental Functions", DualNumber_Sine) { + DualNumber_T a(0.5, { 0.1, 0.05 }); + DualNumber_T s = sin(a); + ASSERT_TRUE(std::abs(s.real - std::sin(0.5)) < 1e-12, "Sine real part incorrect"); + ASSERT_TRUE(std::abs(s.dual[0] - (std::cos(0.5) * 0.1)) < 1e-12, "Sine dual[0] part incorrect"); + ASSERT_TRUE(std::abs(s.dual[1] - (std::cos(0.5) * 0.05)) < 1e-12, "Sine dual[1] part incorrect"); + } + // Test that the sine function works correctly for dual numbers defined with float type + TEST("Transcendental Functions", DualNumber_Sine_f) { + DualNumber_T a(0.5f, { 0.1f, 0.05f }); + DualNumber_T s = sin(a); + ASSERT_TRUE(std::abs(s.real - std::sin(0.5f)) < 1e-5f, "Sine real part incorrect"); + ASSERT_TRUE(std::abs(s.dual[0] - (std::cos(0.5f) * 0.1f)) < 1e-5f, "Sine dual[0] part incorrect"); + ASSERT_TRUE(std::abs(s.dual[1] - (std::cos(0.5f) * 0.05f)) < 1e-5f, "Sine dual[1] part incorrect"); + } + // Test that the cosine function works correctly for dual + TEST("Transcendental Functions", DualNumber_Cosine) { + DualNumber_T a(0.5, { 0.1, 0.05 }); + DualNumber_T c = cos(a); + ASSERT_TRUE(std::abs(c.real - std::cos(0.5)) < 1e-12, "Cosine real part incorrect"); + ASSERT_TRUE(std::abs(c.dual[0] - (-std::sin(0.5) * 0.1)) < 1e-12, "Cosine dual[0] part incorrect"); + ASSERT_TRUE(std::abs(c.dual[1] - (-std::sin(0.5) * 0.05)) < 1e-12, "Cosine dual[1] part incorrect"); + } + // Test that the cosine function works correctly for dual numbers defined with float type + TEST("Transcendental Functions", DualNumber_Cosine_f) { + DualNumber_T a(0.5f, { 0.1f, 0.05f }); + DualNumber_T c = cos(a); + ASSERT_TRUE(std::abs(c.real - std::cos(0.5f)) < 1e-5f, "Cosine real part incorrect"); + ASSERT_TRUE(std::abs(c.dual[0] - (-std::sin(0.5f) * 0.1f)) < 1e-5f, "Cosine dual[0] part incorrect"); + ASSERT_TRUE(std::abs(c.dual[1] - (-std::sin(0.5f) * 0.05f)) < 1e-5f, "Cosine dual[1] part incorrect"); + } + // Test that the tangent function works correctly for dual + TEST("Transcendental Functions", DualNumber_Tangent) { + DualNumber_T a(0.5, { 0.1, 0.05 }); + DualNumber_T t = tan(a); + ASSERT_TRUE(std::abs(t.real - std::tan(0.5)) < 1e-12, "Tangent real part incorrect"); + double sec2 = 1.0 / (std::cos(0.5) * std::cos(0.5)); + ASSERT_TRUE(std::abs(t.dual[0] - (sec2 * 0.1)) < 1e-12, "Tangent dual[0] part incorrect"); + ASSERT_TRUE(std::abs(t.dual[1] - (sec2 * 0.05)) < 1e-12, "Tangent dual[1] part incorrect"); + } + // Test that the tangent function works correctly for dual numbers defined with float type + TEST("Transcendental Functions", DualNumber_Tangent_f) { + DualNumber_T a(0.5f, { 0.1f, 0.05f }); + DualNumber_T t = tan(a); + ASSERT_TRUE(std::abs(t.real - std::tan(0.5f)) < 1e-5f, "Tangent real part incorrect"); + float sec2 = 1.0f / (std::cos(0.5f) * std::cos(0.5f)); + ASSERT_TRUE(std::abs(t.dual[0] - (sec2 * 0.1f)) < 1e-5f, "Tangent dual[0] part incorrect"); + ASSERT_TRUE(std::abs(t.dual[1] - (sec2 * 0.05f)) < 1e-5f, "Tangent dual[1] part incorrect"); + } + // Test that the sine and cosine functions satisfy the Pythagorean identity for dual numbers + TEST("Transcendental Functions", DualNumber_PythagoreanIdentity) { + DualNumber_T a(0.5, { 0.1, 0.05 }); + DualNumber_T s = sin(a); + DualNumber_T c = cos(a); + double sumReal = s.real * s.real + c.real * c.real; + double sumDual0 = 2.0 * (s.real * s.dual[0] + c.real * c.dual[0]); + double sumDual1 = 2.0 * (s.real * s.dual[1] + c.real * c.dual[1]); + ASSERT_TRUE(std::abs(sumReal - 1.0) < 1e-12, "Pythagorean identity real part incorrect"); + ASSERT_TRUE(std::abs(sumDual0) < 1e-12, "Pythagorean identity dual[0] part incorrect"); + ASSERT_TRUE(std::abs(sumDual1) < 1e-12, "Pythagorean identity dual[1] part incorrect"); + } + // Test that the tangent function can be expressed as sine divided by cosine for dual numbers + TEST("Transcendental Functions", DualNumber_TangentAsSineOverCosine) { + DualNumber_T a(0.5, { 0.1, 0.05 }); + DualNumber_T s = sin(a); + DualNumber_T c = cos(a); + DualNumber_T t = tan(a); + DualNumber_T t_from_sc = s / c; + ASSERT_TRUE(std::abs(t.real - t_from_sc.real) < 1e-12, "Tangent as sine/cosine real part incorrect"); + ASSERT_TRUE(std::abs(t.dual[0] - t_from_sc.dual[0]) < 1e-12, "Tangent as sine/cosine dual[0] part incorrect"); + ASSERT_TRUE(std::abs(t.dual[1] - t_from_sc.dual[1]) < 1e-12, "Tangent as sine/cosine dual[1] part incorrect"); + } + // Tests that the hyperbolic tangent function works correctly for dual numbers + TEST("Transcendental Functions", DualNumber_HyperbolicTangent) { + DualNumber_T a(0.5, { 0.1, 0.05 }); + DualNumber_T t = tanh(a); + ASSERT_TRUE(std::abs(t.real - std::tanh(0.5)) < 1e-12, "Hyperbolic tangent real part incorrect"); + double sech2 = 1.0 / (std::cosh(0.5) * std::cosh(0.5)); // sech^2(x) = 1 - tanh^2(x) <-> cosh has a more stable implementation for large x + ASSERT_TRUE(std::abs(t.dual[0] - (sech2 * 0.1)) < 1e-12, "Hyperbolic tangent dual[0] part incorrect"); + ASSERT_TRUE(std::abs(t.dual[1] - (sech2 * 0.05)) < 1e-12, "Hyperbolic tangent dual[1] part incorrect"); + } + // Tests that the exponential function works correctly for dual numbers + TEST("Transcendental Functions", DualNumber_Exponential) { + DualNumber_T a(0.5, { 0.1, 0.05 }); + DualNumber_T e = exp(a); + double expReal = std::exp(0.5); + ASSERT_TRUE(std::abs(e.real - expReal) < 1e-12, "Exponential real part incorrect"); + ASSERT_TRUE(std::abs(e.dual[0] - (expReal * 0.1)) < 1e-12, "Exponential dual[0] part incorrect"); + ASSERT_TRUE(std::abs(e.dual[1] - (expReal * 0.05)) < 1e-12, "Exponential dual[1] part incorrect"); + } +} \ No newline at end of file diff --git a/PxMLib/MathLib_Unit/DualNumberTests/main.cpp b/PxMLib/MathLib_Unit/DualNumberTests/main.cpp new file mode 100644 index 00000000..c3361213 --- /dev/null +++ b/PxMLib/MathLib_Unit/DualNumberTests/main.cpp @@ -0,0 +1,29 @@ +// PxMLib/MathLib_Unit DualNumberTests.cpp +#include "TestHarness.h" + +#ifdef _WIN32 +#define NOMINMAX +#include +#endif + +int main() { +#ifdef _WIN32 + // Enable ANSI escape codes on Windows 10+ + HANDLE hOut = GetStdHandle(STD_OUTPUT_HANDLE); + DWORD mode = 0; + if (GetConsoleMode(hOut, &mode)) { + SetConsoleMode(hOut, mode | ENABLE_VIRTUAL_TERMINAL_PROCESSING); + } +#endif + + std::cout << "========================================\n"; + std::cout << " Dual Number Type Unit Tests\n"; + std::cout << "========================================\n"; + + int failures = test::runAll(); + + std::cout << "\nPress Enter to exit..."; + std::cin.get(); + + return failures; +} diff --git a/DSFE_App/DSFE_Unit_Tests/src/AdaptiveMethodTests.cpp b/PxMLib/MathLib_Unit/IntegratorTests/AdaptiveMethodTests.cpp similarity index 100% rename from DSFE_App/DSFE_Unit_Tests/src/AdaptiveMethodTests.cpp rename to PxMLib/MathLib_Unit/IntegratorTests/AdaptiveMethodTests.cpp diff --git a/PxMLib/MathLib_Unit/IntegratorTests/CMakeLists.txt b/PxMLib/MathLib_Unit/IntegratorTests/CMakeLists.txt new file mode 100644 index 00000000..a4f82d1d --- /dev/null +++ b/PxMLib/MathLib_Unit/IntegratorTests/CMakeLists.txt @@ -0,0 +1,40 @@ +# CMakeLists.txt for MathLib_Unit_Tests executable +cmake_minimum_required(VERSION 3.10) +project(MathLib_IntegratorTests LANGUAGES C CXX) + +enable_testing() + +add_executable(MathLib_IntegratorTests + main.cpp + AdaptiveMethodTests.cpp + ConvergenceOrderTests.cpp + ExplicitMethodTests.cpp + ImplicitMethodTests.cpp +) + +target_link_libraries(MathLib_IntegratorTests PRIVATE + MathLib + gtest_main +) + +target_include_directories(MathLib_IntegratorTests PRIVATE + ${CMAKE_CURRENT_SOURCE_DIR}/../Common +) + +include(GoogleTest) + +gtest_discover_tests( + MathLib_IntegratorTests + DISCOVERY_MODE PRE_TEST + DISCOVERY_TIMEOUT 10 +) + +# if (CMAKE_BUILD_TYPE STREQUAL "Debug") +# gtest_discover_tests( +# MathLib_Unit_Tests +# DISCOVERY_MODE PRE_TEST +# DISCOVERY_TIMEOUT 10 +# ) +# else() +# gtest_discover_tests(MathLib_Unit_Tests) +# endif() diff --git a/DSFE_App/DSFE_Unit_Tests/src/ConvergenceOrderTests.cpp b/PxMLib/MathLib_Unit/IntegratorTests/ConvergenceOrderTests.cpp similarity index 88% rename from DSFE_App/DSFE_Unit_Tests/src/ConvergenceOrderTests.cpp rename to PxMLib/MathLib_Unit/IntegratorTests/ConvergenceOrderTests.cpp index 766d2d3d..09228661 100644 --- a/DSFE_App/DSFE_Unit_Tests/src/ConvergenceOrderTests.cpp +++ b/PxMLib/MathLib_Unit/IntegratorTests/ConvergenceOrderTests.cpp @@ -47,57 +47,57 @@ namespace { } // Forward Euler Convergence Order Test -TEST("Explicit Euler Convergence Order", Euler_Order1) { +TEST("Euler Method", Euler_Order1) { verifyOrder([](const VecX& x, double t, double dt) { return integrator.eulerStep(x, t, dt, convExpDecay); }, 1.0, 500, 1.0, 0.3, "Euler should be order 1"); } // Midpoint (RK2) Convergence Order Test -TEST("Explicit Midpoint Convergence Order", Midpoint_Order2) { +TEST("Midpoint Method (RK2)", Midpoint_Order2) { verifyOrder([](const VecX& x, double t, double dt) { return integrator.midpointStep(x, t, dt, convExpDecay); }, 1.0, 200, 2.0, 0.3, "Midpoint should be order 2"); } // Heun (RK2) Convergence Order Test -TEST("Heun Convergence Order", Heun_Order2) { +TEST("Heun Method (RK2)", Heun_Order2) { verifyOrder([](const VecX& x, double t, double dt) { return integrator.heunStep(x, t, dt, convExpDecay); }, 1.0, 200, 2.0, 0.3, "Heun should be order 2"); } // Ralston (RK2) Convergence Order Test -TEST("Ralston Convergence Order", Ralston_Order2) { +TEST("Ralston Method (RK2)", Ralston_Order2) { verifyOrder([](const VecX& x, double t, double dt) { return integrator.ralstonStep(x, t, dt, convExpDecay); }, 1.0, 200, 2.0, 0.3, "Ralston should be order 2"); } // RK4 Convergence Order Test -TEST("RK4 Convergence Order", RK4_Order4) { +TEST("RK4 Method", RK4_Order4) { verifyOrder([](const VecX& x, double t, double dt) { return integrator.rk4Step(x, t, dt, convExpDecay); }, 1.0, 50, 4.0, 0.3, "RK4 should be order 4"); } // Implicit Euler Convergence Order Test -TEST("Implicit Euler Convergence Order", ImplicitEuler_Order1) { +TEST("Implicit Euler Method", ImplicitEuler_Order1) { verifyOrder([](const VecX& x, double t, double dt) { return integrator.implicitEuler(x, t, dt, convExpDecay); }, 1.0, 500, 1.0, 0.3, "Implicit Euler should be order 1"); } // Implicit Midpoint Convergence Order Test -TEST("Implicit Midpoint Convergence Order", ImplicitMidpoint_Order2) +TEST("Implicit Midpoint Method", ImplicitMidpoint_Order2) { verifyOrder([](const VecX& x, double t, double dt) { return integrator.implicitMidpoint(x, t, dt, convExpDecay); }, 1.0, 200, 2.0, 0.3, "Implicit Midpoint should be order 2"); } // GLRK2 Convergence Order Test -TEST("GLRK2 Convergence Order", GLRK2_Order4) { +TEST("GLRK2 Method", GLRK2_Order4) { verifyOrder([](const VecX& x, double t, double dt) { return integrator.GLRK2(x, t, dt, convExpDecay, convExpDecayJac, 50, 1e-10); }, 1.0, 50, 4.0, 0.3, "GLRK2 should be order 4"); } // GLRK3 Convergence Order Test -TEST("GLRK3 Convergence Order", GLRK3_Order6) { +TEST("GLRK3 Method", GLRK3_Order6) { verifyOrder([](const VecX& x, double t, double dt) { return integrator.GLRK3(x, t, dt, convExpDecay, convExpDecayJac, 150, 1e-16); }, 10.0, 10, 6.0, 0.5, "GLRK3 should be order 6"); diff --git a/DSFE_App/DSFE_Unit_Tests/src/ExplicitMethodTests.cpp b/PxMLib/MathLib_Unit/IntegratorTests/ExplicitMethodTests.cpp similarity index 100% rename from DSFE_App/DSFE_Unit_Tests/src/ExplicitMethodTests.cpp rename to PxMLib/MathLib_Unit/IntegratorTests/ExplicitMethodTests.cpp diff --git a/DSFE_App/DSFE_Unit_Tests/src/ImplicitMethodTests.cpp b/PxMLib/MathLib_Unit/IntegratorTests/ImplicitMethodTests.cpp similarity index 100% rename from DSFE_App/DSFE_Unit_Tests/src/ImplicitMethodTests.cpp rename to PxMLib/MathLib_Unit/IntegratorTests/ImplicitMethodTests.cpp diff --git a/DSFE_App/DSFE_Unit_Tests/src/main.cpp b/PxMLib/MathLib_Unit/IntegratorTests/main.cpp similarity index 98% rename from DSFE_App/DSFE_Unit_Tests/src/main.cpp rename to PxMLib/MathLib_Unit/IntegratorTests/main.cpp index 6b8ae36a..3fc07309 100644 --- a/DSFE_App/DSFE_Unit_Tests/src/main.cpp +++ b/PxMLib/MathLib_Unit/IntegratorTests/main.cpp @@ -9,8 +9,7 @@ #include #endif -int main() -{ +int main() { #ifdef _WIN32 // Enable ANSI escape codes on Windows 10+ HANDLE hOut = GetStdHandle(STD_OUTPUT_HANDLE); From 5fb504c6a6d5df8f41957194b6247f777890a80d Mon Sep 17 00:00:00 2001 From: saltyjoss Date: Sun, 17 May 2026 12:04:05 +0100 Subject: [PATCH 018/210] feat: Added missing operator and mathematical methods from `DualNumbers` --- PxMLib/MathLib/include/core/DualNumbers.h | 26 +++++++++++++++++++++++ 1 file changed, 26 insertions(+) diff --git a/PxMLib/MathLib/include/core/DualNumbers.h b/PxMLib/MathLib/include/core/DualNumbers.h index f0691f9f..fd826a81 100644 --- a/PxMLib/MathLib/include/core/DualNumbers.h +++ b/PxMLib/MathLib/include/core/DualNumbers.h @@ -367,6 +367,32 @@ namespace mathlib { return out; } + // Hyperbolic tangent function (tanh) + template + inline DualNumber_T tanh( + const DualNumber_T& a + ) { + DualNumber_T out; + out.real = std::tanh(a.real); + for (size_t i = 0; i < NVar; ++i) { + out.dual[i] = a.dual[i] * (Scalar(1) - out.real * out.real); + } + return out; + } + + // Exponential function + template + inline DualNumber_T exp( + const DualNumber_T& a + ) { + DualNumber_T out; + out.real = std::exp(a.real); + for (size_t i = 0; i < NVar; ++i) { + out.dual[i] = out.real * a.dual[i]; + } + return out; + } + // Smooth step function for smooth interpolation between 0 and 1 template inline Scalar smoothStep( From 6af9c9ceb4ca0f663d8c7bb5bd6a37c7e4cb2a15 Mon Sep 17 00:00:00 2001 From: saltyjoss Date: Sun, 17 May 2026 13:17:30 +0100 Subject: [PATCH 019/210] feat: Added new Automatic Differentiation (AD) tests, prior to implementation in main code --- PxMLib/MathLib/include/core/DualNumbers.h | 17 +- .../ADIntegratorTests.cpp | 189 ++++++++++++++++++ .../ADJacobianTests.cpp | 12 ++ .../ADNewtonTests.cpp | 12 ++ .../CMakeLists.txt | 39 ++++ .../AutomaticDifferentiationTests/main.cpp | 29 +++ PxMLib/MathLib_Unit/CMakeLists.txt | 3 +- .../IntegratorTests/ExplicitMethodTests.cpp | 165 +++++++-------- 8 files changed, 363 insertions(+), 103 deletions(-) create mode 100644 PxMLib/MathLib_Unit/AutomaticDifferentiationTests/ADIntegratorTests.cpp create mode 100644 PxMLib/MathLib_Unit/AutomaticDifferentiationTests/ADJacobianTests.cpp create mode 100644 PxMLib/MathLib_Unit/AutomaticDifferentiationTests/ADNewtonTests.cpp create mode 100644 PxMLib/MathLib_Unit/AutomaticDifferentiationTests/CMakeLists.txt create mode 100644 PxMLib/MathLib_Unit/AutomaticDifferentiationTests/main.cpp diff --git a/PxMLib/MathLib/include/core/DualNumbers.h b/PxMLib/MathLib/include/core/DualNumbers.h index fd826a81..c4abfe77 100644 --- a/PxMLib/MathLib/include/core/DualNumbers.h +++ b/PxMLib/MathLib/include/core/DualNumbers.h @@ -3,6 +3,8 @@ #include "MathLibAPI.h" #include "core/Types_tpl.h" +#include +#include namespace mathlib { // Template version of dual number @@ -22,7 +24,7 @@ namespace mathlib { dual.fill(Scalar(0)); std::copy_n( duals.begin(), - std::min(duals.size(), NVar), + std::min(duals.size(), static_cast(NVar)), dual.begin() ); } @@ -48,19 +50,6 @@ namespace mathlib { return out; } - // Conjugate (negate dual part, keep real part) - template - inline DualNumber_T conjugate( - const DualNumber_T& a - ) { - DualNumber_T out; - out.real = a.real; - for (size_t i = 0; i < NVar; ++i) { - out.dual[i] = -a.dual[i]; - } - return out; - } - // Identity template inline DualNumber_T operator+( diff --git a/PxMLib/MathLib_Unit/AutomaticDifferentiationTests/ADIntegratorTests.cpp b/PxMLib/MathLib_Unit/AutomaticDifferentiationTests/ADIntegratorTests.cpp new file mode 100644 index 00000000..8bad043a --- /dev/null +++ b/PxMLib/MathLib_Unit/AutomaticDifferentiationTests/ADIntegratorTests.cpp @@ -0,0 +1,189 @@ +// MathLib_UnitTests ADIntegratorTests.cpp + +#include "TestHarness.h" +#include +#include +#include +#include +#include + +#define M_PI std::numbers::pi + +using namespace mathlib; + +namespace { + // Simple Exponential Decay Function: dx/dt = -x, which should decay to exp(-1) at t=1 + template + VecX_T> expDecay(DualNumber_T, const VecX_T>& x) { + VecX_T> dx = x; + for (int i = 0; i < x.size(); ++i) { dx(i) = -x(i); } + return dx; + } + + // Simple Harmonic Oscillator: dx/dt = v, dv/dt = -x, which should preserve energy + template + VecX_T> harmonicOsc(DualNumber_T, const VecX_T>& s) { + VecX_T> d(2); + d(0) = s(1); + d(1) = -s(0); + return d; + } + + // Helper function to integrate from t=0 to t=T using N steps of the given step function, for dual numbers + template + VecX_T> integrateToTime(StepFn stepFn, const VecX_T>& x0, Scalar T, int N) { + DualNumber_T dt(T / Scalar(N)); + DualNumber_T t(0.0); + VecX_T> x = x0; + for (int i = 0; i < N; ++i) { x = stepFn(x, t, dt); t += dt; } + return x; + } + + // Helper function to verify that the exponential decay ODE is integrated correctly, and that the sensitivity of the solution with respect to the initial condition matches the analytical derivative. + template + void verifyExpDecaySensitivity(StepFn stepFn, double tol, int N) { + VecX_T> x0(1); + x0(0) = DualNumber_T(1.0, { 1.0 }); // Initial condition with derivative 1.0 + VecX_T> r = integrateToTime(stepFn, x0, 1.0, N); + double expectedValue = std::exp(-1.0); + ASSERT_TRUE(std::abs(r(0).real - expectedValue) < tol, "Exponential decay error too large"); + ASSERT_TRUE(std::abs(r(0).dual[0] - expectedValue) < tol, "Exponential decay sensitivity error too large"); + } + + template + void verifyHarmonicOscillatorSensitivity(StepFn stepFn, double tol, int N) { + VecX_T> x0(2); + x0(0) = DualNumber_T(1.0, { 1.0, 0.0 }); // Initial position with sensitivity to itself + x0(1) = DualNumber_T(0.0, { 0.0, 1.0 }); // Initial velocity with sensitivity to itself + VecX_T> r = integrateToTime(stepFn, x0, 2.0 * M_PI, N); + double energy = 0.5 * (r(0).real * r(0).real + r(1).real * r(1).real); + + ASSERT_TRUE(std::abs(r(0).real - 1.0) < tol, "Harmonic oscillator position error too large"); + ASSERT_TRUE(std::abs(r(1).real - 0.0) < tol, "Harmonic oscillator velocity error too large"); + ASSERT_TRUE(std::abs(r(0).dual[0] - 1.0) < tol, "Harmonic oscillator position sensitivity error too large"); + ASSERT_TRUE(std::abs(r(1).dual[1] - 1.0) < tol, "Harmonic oscillator velocity sensitivity error too large"); + ASSERT_TRUE(std::abs(r(0).dual[1]) < tol, "Harmonic oscillator position sensitivity to velocity should be near zero"); + ASSERT_TRUE(std::abs(r(1).dual[0]) < tol, "Harmonic oscillator velocity sensitivity to position should be near zero"); + ASSERT_TRUE(std::abs(energy - 0.5) < tol, "Harmonic oscillator energy error too large"); + } + + integration::NumericalIntegrator integrator; + + // Test that the Euler method correctly integrates the exponential decay ODE, and that the dual number derivatives match the analytical derivatives. + TEST("AD Euler Method", Euler_ExponentialDecaySensitivity) { + auto stepFn = [&]( + const VecX_T>& x, + DualNumber_T t, + DualNumber_T dt + ) { + return integrator.eulerStep(x, t, dt, expDecay); + }; + + verifyExpDecaySensitivity(stepFn, 1e-2, 1000); + } + // Test that the Euler method correctly integrates a simple harmonic oscillator, and that the dual number derivatives match the analytical derivatives. + TEST("AD Euler Method", Euler_HarmonicOscillatorSensitivity) { + auto stepFn = [&]( + const VecX_T>& x, + DualNumber_T t, + DualNumber_T dt + ) { + return integrator.eulerStep(x, t, dt, harmonicOsc); + }; + verifyHarmonicOscillatorSensitivity(stepFn, 5e-2, 2000); + } + + // Tests that the Midpoint method (RK2) correctly integrates the exponential decay ODE, and that the dual number derivatives match the analytical derivatives. + TEST("AD Midpoint Method (RK2)", Midpoint_ExponentialDecaySensitivity) { + auto stepFn = [&]( + const VecX_T>& x, + DualNumber_T t, + DualNumber_T dt + ) { + return integrator.midpointStep(x, t, dt, expDecay); + }; + verifyExpDecaySensitivity(stepFn, 1e-5, 500); + } + // Test that the Midpoint method (RK2) correctly integrates a simple harmonic oscillator, and that the dual number derivatives match the analytical derivatives. + TEST("AD Midpoint Method (RK2)", Midpoint_HarmonicOscillatorSensitivity) { + auto stepFn = [&]( + const VecX_T>& x, + DualNumber_T t, + DualNumber_T dt + ) { + return integrator.midpointStep(x, t, dt, harmonicOsc); + }; + verifyHarmonicOscillatorSensitivity(stepFn, 1e-5, 1000); + } + + // Tests that the Heun method (RK2) correctly integrates the exponential decay ODE, and that the dual number derivatives match the analytical derivatives. + TEST("AD Heun Method (RK2)", Heun_ExponentialDecaySensitivity) { + auto stepFn = [&]( + const VecX_T>& x, + DualNumber_T t, + DualNumber_T dt + ) { + return integrator.heunStep(x, t, dt, expDecay); + }; + verifyExpDecaySensitivity(stepFn, 1e-5, 500); + } + // Test that the Heun method (RK2) correctly integrates a simple harmonic oscillator, and that the dual number derivatives match the analytical derivatives. + TEST("AD Heun Method (RK2)", Heun_HarmonicOscillatorSensitivity) { + auto stepFn = [&]( + const VecX_T>& x, + DualNumber_T t, + DualNumber_T dt + ) { + return integrator.heunStep(x, t, dt, harmonicOsc); + }; + verifyHarmonicOscillatorSensitivity(stepFn, 1e-5, 1000); + } + + // Tests that the Ralston method (RK2) correctly integrates the exponential decay ODE, and that the dual number derivatives match the analytical derivatives. + TEST("AD Ralston Method (RK2)", Ralston_ExponentialDecaySensitivity) { + auto stepFn = [&]( + const VecX_T>& x, + DualNumber_T t, + DualNumber_T dt + ) { + return integrator.ralstonStep(x, t, dt, expDecay); + }; + verifyExpDecaySensitivity(stepFn, 1e-5, 500); + } + // Test that the Ralston method (RK2) correctly integrates a simple harmonic oscillator, and that the dual number derivatives match the analytical derivatives. + TEST("AD Ralston Method (RK2)", Ralston_HarmonicOscillatorSensitivity) { + auto stepFn = [&]( + const VecX_T>& x, + DualNumber_T t, + DualNumber_T dt + ) { + return integrator.ralstonStep(x, t, dt, harmonicOsc); + }; + verifyHarmonicOscillatorSensitivity(stepFn, 1e-5, 1000); + } + + // Tests that the RK4 method correctly integrates the exponential decay ODE, and that the dual number derivatives match the analytical derivatives. + TEST("AD RK4 Method", RK4_ExponentialDecaySensitivity) { + auto stepFn = [&]( + const VecX_T>& x, + DualNumber_T t, + DualNumber_T dt + ) { + return integrator.rk4Step(x, t, dt, expDecay); + }; + verifyExpDecaySensitivity(stepFn, 1e-6, 200); + } + // Test that the RK4 method correctly integrates a simple harmonic oscillator, and that the dual number derivatives match the analytical derivatives. + TEST("AD RK4 Method", RK4_HarmonicOscillatorSensitivity) { + auto stepFn = [&]( + const VecX_T>& x, + DualNumber_T t, + DualNumber_T dt + ) { + return integrator.rk4Step(x, t, dt, harmonicOsc); + }; + verifyHarmonicOscillatorSensitivity(stepFn, 1e-5, 100); + } + + // RK45 method is not tested due to further complexity with using an adapative step, implementation will be tested separately LATER once confirmed these work. +} \ No newline at end of file diff --git a/PxMLib/MathLib_Unit/AutomaticDifferentiationTests/ADJacobianTests.cpp b/PxMLib/MathLib_Unit/AutomaticDifferentiationTests/ADJacobianTests.cpp new file mode 100644 index 00000000..6eb6d19d --- /dev/null +++ b/PxMLib/MathLib_Unit/AutomaticDifferentiationTests/ADJacobianTests.cpp @@ -0,0 +1,12 @@ +// MathLib_UnitTests ADJacobianTests.cpp + +#include "TestHarness.h" +#include +#include +#include + +using namespace mathlib; + +namespace { + +} \ No newline at end of file diff --git a/PxMLib/MathLib_Unit/AutomaticDifferentiationTests/ADNewtonTests.cpp b/PxMLib/MathLib_Unit/AutomaticDifferentiationTests/ADNewtonTests.cpp new file mode 100644 index 00000000..39c3b0eb --- /dev/null +++ b/PxMLib/MathLib_Unit/AutomaticDifferentiationTests/ADNewtonTests.cpp @@ -0,0 +1,12 @@ +// MathLib_UnitTests ADNewtonTests.cpp + +#include "TestHarness.h" +#include +#include +#include + +using namespace mathlib; + +namespace { + +} \ No newline at end of file diff --git a/PxMLib/MathLib_Unit/AutomaticDifferentiationTests/CMakeLists.txt b/PxMLib/MathLib_Unit/AutomaticDifferentiationTests/CMakeLists.txt new file mode 100644 index 00000000..78449aff --- /dev/null +++ b/PxMLib/MathLib_Unit/AutomaticDifferentiationTests/CMakeLists.txt @@ -0,0 +1,39 @@ +# CMakeLists.txt for MathLib_Unit_Tests executable +cmake_minimum_required(VERSION 3.10) +project(MathLib_ADTests LANGUAGES C CXX) + +enable_testing() + +add_executable(MathLib_ADTests + main.cpp + ADIntegratorTests.cpp + ADNewtonTests.cpp + ADJacobianTests.cpp +) + +target_link_libraries(MathLib_ADTests PRIVATE + MathLib + gtest_main +) + +target_include_directories(MathLib_ADTests PRIVATE + ${CMAKE_CURRENT_SOURCE_DIR}/../Common +) + +include(GoogleTest) + +gtest_discover_tests( + MathLib_ADTests + DISCOVERY_MODE PRE_TEST + DISCOVERY_TIMEOUT 10 +) + +# if (CMAKE_BUILD_TYPE STREQUAL "Debug") +# gtest_discover_tests( +# MathLib_DualNumberTests +# DISCOVERY_MODE PRE_TEST +# DISCOVERY_TIMEOUT 10 +# ) +# else() +# gtest_discover_tests(MathLib_DualNumberTests) +# endif() diff --git a/PxMLib/MathLib_Unit/AutomaticDifferentiationTests/main.cpp b/PxMLib/MathLib_Unit/AutomaticDifferentiationTests/main.cpp new file mode 100644 index 00000000..c48945c8 --- /dev/null +++ b/PxMLib/MathLib_Unit/AutomaticDifferentiationTests/main.cpp @@ -0,0 +1,29 @@ +// PxMLib/MathLib_Unit DualNumberTests.cpp +#include "TestHarness.h" + +#ifdef _WIN32 +#define NOMINMAX +#include +#endif + +int main() { +#ifdef _WIN32 + // Enable ANSI escape codes on Windows 10+ + HANDLE hOut = GetStdHandle(STD_OUTPUT_HANDLE); + DWORD mode = 0; + if (GetConsoleMode(hOut, &mode)) { + SetConsoleMode(hOut, mode | ENABLE_VIRTUAL_TERMINAL_PROCESSING); + } +#endif + + std::cout << "========================================\n"; + std::cout << " Automatic Differentation Unit Tests\n"; + std::cout << "========================================\n"; + + int failures = test::runAll(); + + std::cout << "\nPress Enter to exit..."; + std::cin.get(); + + return failures; +} diff --git a/PxMLib/MathLib_Unit/CMakeLists.txt b/PxMLib/MathLib_Unit/CMakeLists.txt index 63eb4b8b..0910ece5 100644 --- a/PxMLib/MathLib_Unit/CMakeLists.txt +++ b/PxMLib/MathLib_Unit/CMakeLists.txt @@ -3,4 +3,5 @@ cmake_minimum_required(VERSION 3.10) project(MathLib_Unit_Tests LANGUAGES C CXX) add_subdirectory(IntegratorTests) -add_subdirectory(DualNumberTests) \ No newline at end of file +add_subdirectory(DualNumberTests) +add_subdirectory(AutomaticDifferentiationTests) \ No newline at end of file diff --git a/PxMLib/MathLib_Unit/IntegratorTests/ExplicitMethodTests.cpp b/PxMLib/MathLib_Unit/IntegratorTests/ExplicitMethodTests.cpp index 7a02dd08..418bbe0b 100644 --- a/PxMLib/MathLib_Unit/IntegratorTests/ExplicitMethodTests.cpp +++ b/PxMLib/MathLib_Unit/IntegratorTests/ExplicitMethodTests.cpp @@ -19,7 +19,7 @@ namespace { d(0) = s(1); d(1) = -s(0); return d; - }; + }; // Helper function to integrate from t=0 to t=T using N steps of the given step function template @@ -32,96 +32,85 @@ namespace { } integration::NumericalIntegrator integrator; -} -// Forward Euler Tests -// Exponential Decay Test -TEST("Euler Method", Euler_ExponentialDecay_SmallStep) -{ - VecX x0(1); x0 << 1.0; - VecX r = integrateToTime([](const VecX& x, double t, double dt) { return integrator.eulerStep(x, t, dt, expDecay); }, x0, 1.0, 1000); - ASSERT_TRUE(std::abs(r(0) - std::exp(-1.0)) < 1e-2, "Euler exponential decay error too large"); -} -// Simple Harmonic Oscillator Test -TEST("Euler Method", Euler_HarmonicOscillator_SmallStep) -{ - VecX x0(2); x0 << 1.0, 0.0; - VecX r = integrateToTime([](const VecX& x, double t, double dt) { return integrator.eulerStep(x, t, dt, harmonicOsc); }, x0, 2.0, 2000); - ASSERT_TRUE(std::abs(r(0) - std::cos(2.0)) < 5e-2, "Euler SHO position error too large"); - ASSERT_TRUE(std::abs(r(1) + std::sin(2.0)) < 5e-2, "Euler SHO velocity error too large"); -} + // Forward Euler Tests + // Exponential Decay Test + TEST("Euler Method", Euler_ExponentialDecay_SmallStep) { + VecX x0(1); x0 << 1.0; + VecX r = integrateToTime([](const VecX& x, double t, double dt) { return integrator.eulerStep(x, t, dt, expDecay); }, x0, 1.0, 1000); + ASSERT_TRUE(std::abs(r(0) - std::exp(-1.0)) < 1e-2, "Euler exponential decay error too large"); + } + // Simple Harmonic Oscillator Test + TEST("Euler Method", Euler_HarmonicOscillator_SmallStep) { + VecX x0(2); x0 << 1.0, 0.0; + VecX r = integrateToTime([](const VecX& x, double t, double dt) { return integrator.eulerStep(x, t, dt, harmonicOsc); }, x0, 2.0, 2000); + ASSERT_TRUE(std::abs(r(0) - std::cos(2.0)) < 5e-2, "Euler SHO position error too large"); + ASSERT_TRUE(std::abs(r(1) + std::sin(2.0)) < 5e-2, "Euler SHO velocity error too large"); + } -// Midpoint Method (RK2) Tests -// Exponential Decay Test -TEST("Midpoint Method (RK2)", Midpoint_ExponentialDecay) -{ - VecX x0(1); x0 << 1.0; - VecX r = integrateToTime([](const VecX& x, double t, double dt) { return integrator.midpointStep(x, t, dt, expDecay); }, x0, 1.0, 500); - ASSERT_TRUE(std::abs(r(0) - std::exp(-1.0)) < 1e-5, "Midpoint exponential decay error too large"); -} -// Simple Harmonic Oscillator Test -TEST("Midpoint Method (RK2)", Midpoint_HarmonicOscillator) -{ - VecX x0(2); x0 << 1.0, 0.0; - VecX r = integrateToTime([](const VecX& x, double t, double dt) { return integrator.midpointStep(x, t, dt, harmonicOsc); }, x0, 2.0, 1000); - ASSERT_TRUE(std::abs(r(0) - std::cos(2.0)) < 1e-4, "Midpoint SHO position error too large"); - ASSERT_TRUE(std::abs(r(1) + std::sin(2.0)) < 1e-4, "Midpoint SHO velocity error too large"); -} + // Midpoint Method (RK2) Tests + // Exponential Decay Test + TEST("Midpoint Method (RK2)", Midpoint_ExponentialDecay) { + VecX x0(1); x0 << 1.0; + VecX r = integrateToTime([](const VecX& x, double t, double dt) { return integrator.midpointStep(x, t, dt, expDecay); }, x0, 1.0, 500); + ASSERT_TRUE(std::abs(r(0) - std::exp(-1.0)) < 1e-5, "Midpoint exponential decay error too large"); + } + // Simple Harmonic Oscillator Test + TEST("Midpoint Method (RK2)", Midpoint_HarmonicOscillator) { + VecX x0(2); x0 << 1.0, 0.0; + VecX r = integrateToTime([](const VecX& x, double t, double dt) { return integrator.midpointStep(x, t, dt, harmonicOsc); }, x0, 2.0, 1000); + ASSERT_TRUE(std::abs(r(0) - std::cos(2.0)) < 1e-4, "Midpoint SHO position error too large"); + ASSERT_TRUE(std::abs(r(1) + std::sin(2.0)) < 1e-4, "Midpoint SHO velocity error too large"); + } -// Heun's Method (RK2) Tests -// Exponential Decay Test -TEST("Heun Method (RK2)", Heun_ExponentialDecay) -{ - VecX x0(1); x0 << 1.0; - VecX r = integrateToTime([](const VecX& x, double t, double dt) { return integrator.heunStep(x, t, dt, expDecay); }, x0, 1.0, 500); - ASSERT_TRUE(std::abs(r(0) - std::exp(-1.0)) < 1e-5, "Heun exponential decay error too large"); -} -// Simple Harmonic Oscillator Test -TEST("Heun Method (RK2)", Heun_HarmonicOscillator) -{ - VecX x0(2); x0 << 1.0, 0.0; - VecX r = integrateToTime([](const VecX& x, double t, double dt) { return integrator.heunStep(x, t, dt, harmonicOsc); }, x0, 2.0, 1000); - ASSERT_TRUE(std::abs(r(0) - std::cos(2.0)) < 1e-4, "Heun SHO position error too large"); - ASSERT_TRUE(std::abs(r(1) + std::sin(2.0)) < 1e-4, "Heun SHO velocity error too large"); -} + // Heun's Method (RK2) Tests + // Exponential Decay Test + TEST("Heun Method (RK2)", Heun_ExponentialDecay) { + VecX x0(1); x0 << 1.0; + VecX r = integrateToTime([](const VecX& x, double t, double dt) { return integrator.heunStep(x, t, dt, expDecay); }, x0, 1.0, 500); + ASSERT_TRUE(std::abs(r(0) - std::exp(-1.0)) < 1e-5, "Heun exponential decay error too large"); + } + // Simple Harmonic Oscillator Test + TEST("Heun Method (RK2)", Heun_HarmonicOscillator) { + VecX x0(2); x0 << 1.0, 0.0; + VecX r = integrateToTime([](const VecX& x, double t, double dt) { return integrator.heunStep(x, t, dt, harmonicOsc); }, x0, 2.0, 1000); + ASSERT_TRUE(std::abs(r(0) - std::cos(2.0)) < 1e-4, "Heun SHO position error too large"); + ASSERT_TRUE(std::abs(r(1) + std::sin(2.0)) < 1e-4, "Heun SHO velocity error too large"); + } -// Ralston's Method (RK2) Tests -// Exponential Decay Test -TEST("Ralston Method (RK2)", Ralston_ExponentialDecay) -{ - VecX x0(1); x0 << 1.0; - VecX r = integrateToTime([](const VecX& x, double t, double dt) { return integrator.ralstonStep(x, t, dt, expDecay); }, x0, 1.0, 500); - ASSERT_TRUE(std::abs(r(0) - std::exp(-1.0)) < 1e-5, "Ralston exponential decay error too large"); -} -// Simple Harmonic Oscillator Test -TEST("Ralston Method (RK2)", Ralston_HarmonicOscillator) -{ - VecX x0(2); x0 << 1.0, 0.0; - VecX r = integrateToTime([](const VecX& x, double t, double dt) { return integrator.ralstonStep(x, t, dt, harmonicOsc); }, x0, 2.0, 1000); - ASSERT_TRUE(std::abs(r(0) - std::cos(2.0)) < 1e-4, "Ralston SHO position error too large"); - ASSERT_TRUE(std::abs(r(1) + std::sin(2.0)) < 1e-4, "Ralston SHO velocity error too large"); -} + // Ralston's Method (RK2) Tests + // Exponential Decay Test + TEST("Ralston Method (RK2)", Ralston_ExponentialDecay) { + VecX x0(1); x0 << 1.0; + VecX r = integrateToTime([](const VecX& x, double t, double dt) { return integrator.ralstonStep(x, t, dt, expDecay); }, x0, 1.0, 500); + ASSERT_TRUE(std::abs(r(0) - std::exp(-1.0)) < 1e-5, "Ralston exponential decay error too large"); + } + // Simple Harmonic Oscillator Test + TEST("Ralston Method (RK2)", Ralston_HarmonicOscillator) { + VecX x0(2); x0 << 1.0, 0.0; + VecX r = integrateToTime([](const VecX& x, double t, double dt) { return integrator.ralstonStep(x, t, dt, harmonicOsc); }, x0, 2.0, 1000); + ASSERT_TRUE(std::abs(r(0) - std::cos(2.0)) < 1e-4, "Ralston SHO position error too large"); + ASSERT_TRUE(std::abs(r(1) + std::sin(2.0)) < 1e-4, "Ralston SHO velocity error too large"); + } -// RK4 Method Tests -// Exponential Decay Test -TEST("RK4 Method", RK4_ExponentialDecay) -{ - VecX x0(1); x0 << 1.0; - VecX r = integrateToTime([](const VecX& x, double t, double dt) { return integrator.rk4Step(x, t, dt, expDecay); }, x0, 1.0, 100); - ASSERT_TRUE(std::abs(r(0) - std::exp(-1.0)) < 1e-9, "RK4 exponential decay error too large"); -} -// Simple Harmonic Oscillator Test -TEST("RK4 Method", RK4_HarmonicOscillator) -{ - VecX x0(2); x0 << 1.0, 0.0; - VecX r = integrateToTime([](const VecX& x, double t, double dt) { return integrator.rk4Step(x, t, dt, harmonicOsc); }, x0, 2.0, 200); - ASSERT_TRUE(std::abs(r(0) - std::cos(2.0)) < 1e-9, "RK4 SHO position error too large"); - ASSERT_TRUE(std::abs(r(1) + std::sin(2.0)) < 1e-9, "RK4 SHO velocity error too large"); -} -// RK4 with larger step size to test stability -TEST("RK4 Method", RK4_ExponentialDecay_LargeStep) -{ - VecX x0(1); x0 << 1.0; - VecX r = integrateToTime([](const VecX& x, double t, double dt) { return integrator.rk4Step(x, t, dt, expDecay); }, x0, 1.0, 10); - ASSERT_TRUE(std::abs(r(0) - std::exp(-1.0)) < 1e-5, "RK4 large-step exponential decay error too large"); + // RK4 Method Tests + // Exponential Decay Test + TEST("RK4 Method", RK4_ExponentialDecay) { + VecX x0(1); x0 << 1.0; + VecX r = integrateToTime([](const VecX& x, double t, double dt) { return integrator.rk4Step(x, t, dt, expDecay); }, x0, 1.0, 100); + ASSERT_TRUE(std::abs(r(0) - std::exp(-1.0)) < 1e-9, "RK4 exponential decay error too large"); + } + // Simple Harmonic Oscillator Test + TEST("RK4 Method", RK4_HarmonicOscillator) { + VecX x0(2); x0 << 1.0, 0.0; + VecX r = integrateToTime([](const VecX& x, double t, double dt) { return integrator.rk4Step(x, t, dt, harmonicOsc); }, x0, 2.0, 200); + ASSERT_TRUE(std::abs(r(0) - std::cos(2.0)) < 1e-9, "RK4 SHO position error too large"); + ASSERT_TRUE(std::abs(r(1) + std::sin(2.0)) < 1e-9, "RK4 SHO velocity error too large"); + } + // RK4 with larger step size to test stability + TEST("RK4 Method", RK4_ExponentialDecay_LargeStep) { + VecX x0(1); x0 << 1.0; + VecX r = integrateToTime([](const VecX& x, double t, double dt) { return integrator.rk4Step(x, t, dt, expDecay); }, x0, 1.0, 10); + ASSERT_TRUE(std::abs(r(0) - std::exp(-1.0)) < 1e-5, "RK4 large-step exponential decay error too large"); + } } \ No newline at end of file From 2bae541145b7515931e06fd61163ca6bb7f7672e Mon Sep 17 00:00:00 2001 From: saltyjoss Date: Sun, 17 May 2026 13:37:25 +0100 Subject: [PATCH 020/210] refactor: Correct minor bug in `ADIntegratorTests` to now use 5e-4 fo RK2 harmonic oscillator state and sensitivity tests (rather than a much smaller 1e-5), separating state, sensitivity, and energy tolerances --- .../ADIntegratorTests.cpp | 63 ++++++++++++------- 1 file changed, 42 insertions(+), 21 deletions(-) diff --git a/PxMLib/MathLib_Unit/AutomaticDifferentiationTests/ADIntegratorTests.cpp b/PxMLib/MathLib_Unit/AutomaticDifferentiationTests/ADIntegratorTests.cpp index 8bad043a..b0a99cf6 100644 --- a/PxMLib/MathLib_Unit/AutomaticDifferentiationTests/ADIntegratorTests.cpp +++ b/PxMLib/MathLib_Unit/AutomaticDifferentiationTests/ADIntegratorTests.cpp @@ -5,16 +5,18 @@ #include #include #include -#include - -#define M_PI std::numbers::pi +#include using namespace mathlib; +using namespace constants; namespace { // Simple Exponential Decay Function: dx/dt = -x, which should decay to exp(-1) at t=1 template - VecX_T> expDecay(DualNumber_T, const VecX_T>& x) { + VecX_T> expDecay( + DualNumber_T, + const VecX_T>& x + ) { VecX_T> dx = x; for (int i = 0; i < x.size(); ++i) { dx(i) = -x(i); } return dx; @@ -22,7 +24,10 @@ namespace { // Simple Harmonic Oscillator: dx/dt = v, dv/dt = -x, which should preserve energy template - VecX_T> harmonicOsc(DualNumber_T, const VecX_T>& s) { + VecX_T> harmonicOsc( + DualNumber_T, + const VecX_T>& s + ) { VecX_T> d(2); d(0) = s(1); d(1) = -s(0); @@ -31,7 +36,12 @@ namespace { // Helper function to integrate from t=0 to t=T using N steps of the given step function, for dual numbers template - VecX_T> integrateToTime(StepFn stepFn, const VecX_T>& x0, Scalar T, int N) { + VecX_T> integrateToTime( + StepFn stepFn, + const VecX_T>& x0, + Scalar T, + int N + ) { DualNumber_T dt(T / Scalar(N)); DualNumber_T t(0.0); VecX_T> x = x0; @@ -41,30 +51,41 @@ namespace { // Helper function to verify that the exponential decay ODE is integrated correctly, and that the sensitivity of the solution with respect to the initial condition matches the analytical derivative. template - void verifyExpDecaySensitivity(StepFn stepFn, double tol, int N) { + void verifyExpDecaySensitivity( + StepFn stepFn, + double tol, + int N + ) { VecX_T> x0(1); x0(0) = DualNumber_T(1.0, { 1.0 }); // Initial condition with derivative 1.0 VecX_T> r = integrateToTime(stepFn, x0, 1.0, N); double expectedValue = std::exp(-1.0); ASSERT_TRUE(std::abs(r(0).real - expectedValue) < tol, "Exponential decay error too large"); ASSERT_TRUE(std::abs(r(0).dual[0] - expectedValue) < tol, "Exponential decay sensitivity error too large"); + ASSERT_TRUE(std::abs(r(0).dual[0]) > 0.0, "Sensitivity vanished during propagation"); } template - void verifyHarmonicOscillatorSensitivity(StepFn stepFn, double tol, int N) { + void verifyHarmonicOscillatorSensitivity( + StepFn stepFn, + double stateTol, + double sensitivityTol, + double energyTol, + int N + ) { VecX_T> x0(2); x0(0) = DualNumber_T(1.0, { 1.0, 0.0 }); // Initial position with sensitivity to itself x0(1) = DualNumber_T(0.0, { 0.0, 1.0 }); // Initial velocity with sensitivity to itself - VecX_T> r = integrateToTime(stepFn, x0, 2.0 * M_PI, N); + VecX_T> r = integrateToTime(stepFn, x0, 2.0 * PI_d, N); double energy = 0.5 * (r(0).real * r(0).real + r(1).real * r(1).real); - ASSERT_TRUE(std::abs(r(0).real - 1.0) < tol, "Harmonic oscillator position error too large"); - ASSERT_TRUE(std::abs(r(1).real - 0.0) < tol, "Harmonic oscillator velocity error too large"); - ASSERT_TRUE(std::abs(r(0).dual[0] - 1.0) < tol, "Harmonic oscillator position sensitivity error too large"); - ASSERT_TRUE(std::abs(r(1).dual[1] - 1.0) < tol, "Harmonic oscillator velocity sensitivity error too large"); - ASSERT_TRUE(std::abs(r(0).dual[1]) < tol, "Harmonic oscillator position sensitivity to velocity should be near zero"); - ASSERT_TRUE(std::abs(r(1).dual[0]) < tol, "Harmonic oscillator velocity sensitivity to position should be near zero"); - ASSERT_TRUE(std::abs(energy - 0.5) < tol, "Harmonic oscillator energy error too large"); + ASSERT_TRUE(std::abs(r(0).real - 1.0) < stateTol, "Harmonic oscillator position error too large"); + ASSERT_TRUE(std::abs(r(1).real - 0.0) < stateTol, "Harmonic oscillator velocity error too large"); + ASSERT_TRUE(std::abs(r(0).dual[0] - 1.0) < sensitivityTol, "Harmonic oscillator position sensitivity error too large"); + ASSERT_TRUE(std::abs(r(1).dual[1] - 1.0) < sensitivityTol, "Harmonic oscillator velocity sensitivity error too large"); + ASSERT_TRUE(std::abs(r(0).dual[1]) < sensitivityTol, "Harmonic oscillator position sensitivity to velocity should be near zero"); + ASSERT_TRUE(std::abs(r(1).dual[0]) < sensitivityTol, "Harmonic oscillator velocity sensitivity to position should be near zero"); + ASSERT_TRUE(std::abs(energy - 0.5) < energyTol, "Harmonic oscillator energy error too large"); } integration::NumericalIntegrator integrator; @@ -90,7 +111,7 @@ namespace { ) { return integrator.eulerStep(x, t, dt, harmonicOsc); }; - verifyHarmonicOscillatorSensitivity(stepFn, 5e-2, 2000); + verifyHarmonicOscillatorSensitivity(stepFn, 5e-2, 5e-2, 1e-1, 2000); } // Tests that the Midpoint method (RK2) correctly integrates the exponential decay ODE, and that the dual number derivatives match the analytical derivatives. @@ -113,7 +134,7 @@ namespace { ) { return integrator.midpointStep(x, t, dt, harmonicOsc); }; - verifyHarmonicOscillatorSensitivity(stepFn, 1e-5, 1000); + verifyHarmonicOscillatorSensitivity(stepFn, 5e-4, 5e-4, 5e-3, 1000); } // Tests that the Heun method (RK2) correctly integrates the exponential decay ODE, and that the dual number derivatives match the analytical derivatives. @@ -136,7 +157,7 @@ namespace { ) { return integrator.heunStep(x, t, dt, harmonicOsc); }; - verifyHarmonicOscillatorSensitivity(stepFn, 1e-5, 1000); + verifyHarmonicOscillatorSensitivity(stepFn, 5e-4, 5e-4, 5e-3, 1000); } // Tests that the Ralston method (RK2) correctly integrates the exponential decay ODE, and that the dual number derivatives match the analytical derivatives. @@ -159,7 +180,7 @@ namespace { ) { return integrator.ralstonStep(x, t, dt, harmonicOsc); }; - verifyHarmonicOscillatorSensitivity(stepFn, 1e-5, 1000); + verifyHarmonicOscillatorSensitivity(stepFn, 5e-4, 5e-4, 5e-3, 1000); } // Tests that the RK4 method correctly integrates the exponential decay ODE, and that the dual number derivatives match the analytical derivatives. @@ -182,7 +203,7 @@ namespace { ) { return integrator.rk4Step(x, t, dt, harmonicOsc); }; - verifyHarmonicOscillatorSensitivity(stepFn, 1e-5, 100); + verifyHarmonicOscillatorSensitivity(stepFn, 1e-5, 1e-5, 1e-6, 100); } // RK45 method is not tested due to further complexity with using an adapative step, implementation will be tested separately LATER once confirmed these work. From 94f49ed277785a6300b14ab1e827c81d1830a0a3 Mon Sep 17 00:00:00 2001 From: saltyjoss Date: Sun, 17 May 2026 13:57:50 +0100 Subject: [PATCH 021/210] fixes: Added basic guards to DualNumber division and power operators to reduce accidental singularities --- PxMLib/MathLib/include/core/DualNumbers.h | 34 +++++++++++++++-------- 1 file changed, 22 insertions(+), 12 deletions(-) diff --git a/PxMLib/MathLib/include/core/DualNumbers.h b/PxMLib/MathLib/include/core/DualNumbers.h index c4abfe77..67d0f4d6 100644 --- a/PxMLib/MathLib/include/core/DualNumbers.h +++ b/PxMLib/MathLib/include/core/DualNumbers.h @@ -67,7 +67,7 @@ namespace mathlib { inline bool operator==( const DualNumber_T& a, const DualNumber_T& b - ) { + ) { return a.real == b.real; } @@ -76,7 +76,7 @@ namespace mathlib { inline bool operator!=( const DualNumber_T& a, const DualNumber_T& b - ) { + ) { return !(a == b); } @@ -85,7 +85,7 @@ namespace mathlib { inline bool operator<( const DualNumber_T& a, const DualNumber_T& b - ) { + ) { return a.real < b.real; } @@ -94,7 +94,7 @@ namespace mathlib { inline bool operator<=( const DualNumber_T& a, const DualNumber_T& b - ) { + ) { return a.real <= b.real; } @@ -112,7 +112,7 @@ namespace mathlib { inline bool operator>=( const DualNumber_T& a, const DualNumber_T& b - ) { + ) { return a.real >= b.real; } @@ -170,7 +170,7 @@ namespace mathlib { inline DualNumber_T operator*( const DualNumber_T& a, Scalar b - ) { + ) { DualNumber_T out; out.real = a.real * b; for (size_t i = 0; i < NVar; ++i) { @@ -184,7 +184,7 @@ namespace mathlib { inline DualNumber_T operator*( Scalar a, const DualNumber_T& b - ) { + ) { return b * a; // Reuse dual * scalar } @@ -193,9 +193,9 @@ namespace mathlib { inline DualNumber_T operator/( const DualNumber_T& a, Scalar b - ) { + ) { DualNumber_T out; - out.real = a.real / b; + out.real = a.real / b ? a.real / b : throw std::runtime_error("Division by zero in dual number scalar division"); for (size_t i = 0; i < NVar; ++i) { out.dual[i] = a.dual[i] / b; } @@ -207,7 +207,7 @@ namespace mathlib { inline DualNumber_T operator/( Scalar a, const DualNumber_T& b - ) { + ) { DualNumber_T out; out.real = a / b.real; for (size_t i = 0; i < NVar; ++i) { @@ -283,6 +283,16 @@ namespace mathlib { Scalar n ) { DualNumber_T out; + if (a.real == Scalar(0) && n < Scalar(0)) { + throw std::runtime_error("Invalid dual power: division by zero"); + } + if (n == Scalar(0)) { + out.real = Scalar(1); + for (size_t i = 0; i < NVar; ++i) { + out.dual[i] = Scalar(0); + } + return out; + } out.real = std::pow(a.real, n); for (size_t i = 0; i < NVar; ++i) { out.dual[i] = n * std::pow(a.real, n - Scalar(1)) * a.dual[i]; @@ -467,7 +477,7 @@ namespace mathlib { DualNumber_T& a, Scalar b ) { - a.real /= b; + a.real /= b ? b : throw std::runtime_error("Division by zero in DualNumber_T operator/="); for (size_t i = 0; i < NVar; ++i) { a.dual[i] /= b; } @@ -484,7 +494,7 @@ namespace mathlib { for (size_t i = 0; i < NVar; ++i) { a.dual[i] = (a.dual[i] * b.real - ogReal * b.dual[i]) / (b.real * b.real); } - a.real = ogReal / b.real; + a.real = ogReal / b.real ? ogReal / b.real : throw std::runtime_error("Division by zero in DualNumber_T operator/="); return a; } From fa9ded6b48e536a76ba74034ed46623e8b4141ff Mon Sep 17 00:00:00 2001 From: saltyjoss Date: Sun, 17 May 2026 14:41:28 +0100 Subject: [PATCH 022/210] feat: Added basic AD Newton-Raphson tests for root finding, convergence, and jacobian accuracy --- .../ADNewtonTests.cpp | 54 +++++++++++++++++++ .../DualNumberTests/CMakeLists.txt | 2 +- 2 files changed, 55 insertions(+), 1 deletion(-) diff --git a/PxMLib/MathLib_Unit/AutomaticDifferentiationTests/ADNewtonTests.cpp b/PxMLib/MathLib_Unit/AutomaticDifferentiationTests/ADNewtonTests.cpp index 39c3b0eb..7132f988 100644 --- a/PxMLib/MathLib_Unit/AutomaticDifferentiationTests/ADNewtonTests.cpp +++ b/PxMLib/MathLib_Unit/AutomaticDifferentiationTests/ADNewtonTests.cpp @@ -8,5 +8,59 @@ using namespace mathlib; namespace { + // Simple nonlinear function: f(x) = x^2 - 2, which has a root at sqrt(2) + template + DualNumber_T nonlinearFunc(const DualNumber_T& x) { + return pow(x, Scalar(2)) - DualNumber_T(Scalar(2)); + }; + // Analytical derivative of the nonlinear function: f'(x) = 2x + template + DualNumber_T nonlinearFuncDerivative(const DualNumber_T& x) { + return Scalar(2) * x; + }; + + // Helper function to perform a single Newton-Raphson iteration using dual numbers + template + DualNumber_T newtonRaphsonStep( + const DualNumber_T& x + ) { + auto g = nonlinearFunc(x); + auto J = nonlinearFuncDerivative(x); + return x - g / J; + } + + // Test that the Newton-Raphson step using dual numbers correctly finds the root of the nonlinear function and that the Jacobian is computed correctly. + TEST("AD Newton-Raphson", ADNewton_RootFinding) { + DualNumber_T x(1.0, { 1.0 }); + for (int iter = 0; iter < 10; ++iter) { + x = newtonRaphsonStep(x); + } + double root = x.real; + ASSERT_TRUE(std::abs(root - std::sqrt(2.0)) < 1e-6, "Root finding error too large"); + } + // Test that the Newton-Raphson step using dual numbers converges to the root of the nonlinear function from different initial guesses. + TEST("AD Newton-Raphson", ADNewton_Convergence) { + std::vector x0 = { 0.5, 1.0, 1.5, 2.0 }; + for (double x_guess : x0) { + DualNumber_T x(x_guess, { 1.0 }); + for (int iter = 0; iter < 10; ++iter) { + x = newtonRaphsonStep(x); + } + double root = x.real; + double residual = std::abs(nonlinearFunc(x).real); + + std::string errorMsg = "Root finding error too large for initial guess " + std::to_string(x_guess); + ASSERT_TRUE(std::abs(root - std::sqrt(2.0)) < 1e-6, errorMsg.c_str()); + ASSERT_TRUE(residual < 1e-10, "Residual is too large after Newton solve"); + } + } + // Test that the Jacobian computed using dual numbers matches the analytical derivative of the nonlinear function. + TEST("AD Newton-Raphson", ADNewton_JacobianAccuracy) { + DualNumber_T x(1.0, { 1.0 }); + DualNumber_T g = nonlinearFunc(x); + double computedJacobian = g.dual[0]; + double expectedJacobian = nonlinearFuncDerivative(x).real; + ASSERT_TRUE(std::abs(computedJacobian - expectedJacobian) < 1e-6, "Jacobian accuracy error too large"); + } } \ No newline at end of file diff --git a/PxMLib/MathLib_Unit/DualNumberTests/CMakeLists.txt b/PxMLib/MathLib_Unit/DualNumberTests/CMakeLists.txt index 1f488b9d..f70db1f1 100644 --- a/PxMLib/MathLib_Unit/DualNumberTests/CMakeLists.txt +++ b/PxMLib/MathLib_Unit/DualNumberTests/CMakeLists.txt @@ -7,8 +7,8 @@ enable_testing() add_executable(MathLib_DualNumberTests main.cpp ArithmeticTests.cpp - AssignmentOperatorTests.cpp DivisionTests.cpp + AssignmentOperatorTests.cpp DifferentiationTests.cpp TranscendentalTests.cpp ) From 0e4968f158f7f500d80b7c5eb03bf738a02ded96 Mon Sep 17 00:00:00 2001 From: saltyjoss Date: Sun, 17 May 2026 17:25:13 +0100 Subject: [PATCH 023/210] feat: Added basic AD Jacobian tests for matching the AD Jacobian to analytical and fdm Jacobians, aswell as exploring the cross-variable propogation of coupled non-linear systems --- .../ADJacobianTests.cpp | 193 ++++++++++++++++++ 1 file changed, 193 insertions(+) diff --git a/PxMLib/MathLib_Unit/AutomaticDifferentiationTests/ADJacobianTests.cpp b/PxMLib/MathLib_Unit/AutomaticDifferentiationTests/ADJacobianTests.cpp index 6eb6d19d..42a275b9 100644 --- a/PxMLib/MathLib_Unit/AutomaticDifferentiationTests/ADJacobianTests.cpp +++ b/PxMLib/MathLib_Unit/AutomaticDifferentiationTests/ADJacobianTests.cpp @@ -8,5 +8,198 @@ using namespace mathlib; namespace { + // Simple vector-valued function: f(x) = [x^2 + y, sin(x) + y^2] + template + VecX_T> vectorFunction( + const VecX_T>& x + ) { + VecX_T> f(2); + f(0) = x(0) * x(0) + x(1); // f1 = x^2 + y + f(1) = sin(x(0)) + x(1) * x(1); // f2 = sin(x) + y^2 + return f; + } + // Dense coupled nonlinear system: F(x) = [x^2 + xy - y, sin(xy) + y^3] + template + VecX_T> coupledNonlinearSystem( + const VecX_T>& x + ) { + VecX_T> F(2); + F(0) = x(0) * x(0) + x(0) * x(1) - x(1); // F1 = x^2 + xy - y + F(1) = sin(x(0) * x(1)) + pow(x(1), Scalar(3)); // F2 = sin(xy) + y^3 + return F; + } + + // Analytical Jacobian of a simple vector-valued function f(x) = [x^2 + y, sin(x) + y^2] + template + void analyticalJacobian( + const VecX_T>& x, + MatX_T& J_out + ) { + double x0 = x(0).real; + double y0 = x(1).real; + J_out(0, 0) = Scalar(2) * x0; // df1/dx + J_out(0, 1) = Scalar(1); // df1/dy + J_out(1, 0) = std::cos(x0); // df2/dx + J_out(1, 1) = Scalar(2) * y0; // df2/dy + } + + // Finite difference approximation of the Jacobian for the same vector-valued function, to compare against the AD-computed Jacobian. + template + void finiteDifferenceJacobian( + const VecX_T>& x, + MatX_T& J_out + ) { + Scalar h = (Scalar)1e-6; + VecX_T> f0 = vectorFunction(x); + for (int i = 0; i < x.size(); ++i) { + auto x_perturbed = x; + x_perturbed(i).real += h; + auto f_perturbed = vectorFunction(x_perturbed); + for (int j = 0; j < x.size(); ++j) { + J_out(j, i) = (f_perturbed(j).real - f0(j).real) / h; + } + } + } + + // Helper function to construct the Jacobian matrix using dual numbers for a simple vector-valued function, to be used in the AD Jacobian tests. + template + void constructADJacobian( + const VecX_T>& x, + MatX_T& J_ad + ) { + VecX_T> f = vectorFunction(x); + for (int i = 0; i < f.size(); ++i) { + for (int j = 0; j < NVar; ++j) { + J_ad(i, j) = f(i).dual[j]; + } + } + } + + // Test that the Jacobian computed using dual numbers matches the analytical Jacobian for a simple vector-valued function. + TEST("AD Jacobian", ADJacobian_anaylticalMatch) { + VecX_T> x(2); + x(0) = DualNumber_T(1.0, { 1.0, 0.0 }); // x = 1.0 with dual part for df/dx + x(1) = DualNumber_T(2.0, { 0.0, 1.0 }); // y = 2.0 with dual part for df/dy + MatX_T J_ad(2, 2); + MatX_T J_analytical(2, 2); + analyticalJacobian(x, J_analytical); + constructADJacobian(x, J_ad); + for (int i = 0; i < J_analytical.rows(); ++i) { + for (int j = 0; j < J_analytical.cols(); ++j) { + std::string errorMsg = "Jacobian mismatch at (" + std::to_string(i) + + ", " + std::to_string(j) + + "): " + std::to_string(J_analytical(i, j)) + + " vs " + std::to_string(J_ad(i, j)); + ASSERT_TRUE(std::abs(J_analytical(i, j) - J_ad(i, j)) < 1e-6, errorMsg.c_str()); + ASSERT_TRUE(std::isfinite(J_ad(i, j)), "AD Jacobian contains non-finite value"); + } + } + ASSERT_TRUE(J_analytical.isApprox(J_ad, 1e-6), "Analytical and AD Jacobians do not match"); + } + + // Test that the Jacobian computed using dual numbers matches a finite difference approximation of the Jacobian for the same vector-valued function. + TEST("AD Jacobian", ADJacobian_finiteDifferenceMatch) { + VecX_T> x(2); + x(0) = DualNumber_T(1.0, { 1.0, 0.0 }); // x = 1.0 with dual part for df/dx + x(1) = DualNumber_T(2.0, { 0.0, 1.0 }); // y = 2.0 with dual part for df/dy + MatX_T J_ad(2, 2); + MatX_T J_fd(2, 2); + finiteDifferenceJacobian(x, J_fd); + constructADJacobian(x, J_ad); + for (int i = 0; i < J_fd.rows(); ++i) { + for (int j = 0; j < J_fd.cols(); ++j) { + std::string errorMsg = "Jacobian mismatch at (" + std::to_string(i) + + ", " + std::to_string(j) + + "): " + std::to_string(J_fd(i, j)) + + " vs " + std::to_string(J_ad(i, j)); + ASSERT_TRUE(std::abs(J_fd(i, j) - J_ad(i, j)) < 1e-5, errorMsg.c_str()); + ASSERT_TRUE(std::isfinite(J_ad(i, j)), "AD Jacobian contains non-finite value"); + } + } + ASSERT_TRUE(J_fd.isApprox(J_ad, 1e-6), "Finite difference and AD Jacobians do not match"); + } + + // Tests the cross-variable propagation of derivatives in a coupled nonlinear system, ensuring that the Jacobian captures the interactions between variables correctly. + TEST("AD Jacobian", ADJacobian_coupledNonlinearSystem) { + VecX_T> x(2); + x(0) = DualNumber_T(1.0, { 1.0, 0.0 }); // x = 1.0 with dual part for df/dx + x(1) = DualNumber_T(2.0, { 0.0, 1.0 }); // y = 2.0 with dual part for df/dy + VecX_T> F = coupledNonlinearSystem(x); + MatX_T J_ad(2, 2); + for (int i = 0; i < F.size(); ++i) { + for (int j = 0; j < 2; ++j) { + J_ad(i, j) = F(i).dual[j]; + } + } + MatX_T J_expected(2, 2); + J_expected(0, 0) = 2.0 * x(0).real + x(1).real; // dF1/dx + J_expected(0, 1) = x(0).real - 1.0; // dF1/dy + J_expected(1, 0) = std::cos(x(0).real * x(1).real) * x(1).real; // dF2/dx + J_expected(1, 1) = std::cos(x(0).real * x(1).real) * x(0).real + 3.0 * pow(x(1).real, 2.0); // dF2/dy + for (int i = 0; i < J_ad.rows(); ++i) { + for (int j = 0; j < J_ad.cols(); ++j) { + std::string errorMsg = "Coupled system Jacobian mismatch at (" + std::to_string(i) + + ", " + std::to_string(j) + + "): " + std::to_string(J_expected(i, j)) + + " vs " + std::to_string(J_ad(i, j)); + ASSERT_TRUE(std::abs(J_expected(i, j) - J_ad(i, j)) < 1e-6, errorMsg.c_str()); + ASSERT_TRUE(std::isfinite(J_ad(i, j)), "AD Jacobian contains non-finite value"); + } + } + ASSERT_TRUE(J_expected.isApprox(J_ad, 1e-6), "Expected and AD Jacobians do not match for coupled nonlinear system"); + } + + // Tests the conditioning of the Jacobian matrix computed via automatic differentiation, ensuring that it does not contain excessively large or small values that could indicate numerical instability. + TEST("AD Jacobian", ADJacobian_numericalStability) { + VecX_T> x(2); + x(0) = DualNumber_T(1.0, { 1.0, 0.0 }); // x = 1.0 with dual part for df/dx + x(1) = DualNumber_T(2.0, { 0.0, 1.0 }); // y = 2.0 with dual part for df/dy + VecX_T> f = vectorFunction(x); + MatX_T J_ad(2, 2); + J_ad(0, 0) = f(0).dual[0]; // df1/dx + J_ad(0, 1) = f(0).dual[1]; // df1/dy + J_ad(1, 0) = f(1).dual[0]; // df2/dx + J_ad(1, 1) = f(1).dual[1]; // df2/dy + for (int i = 0; i < J_ad.rows(); ++i) { + for (int j = 0; j < J_ad.cols(); ++j) { + std::string errorMsg = "AD Jacobian contains non-finite value at (" + std::to_string(i) + + ", " + std::to_string(j) + + "): " + std::to_string(J_ad(i, j)); + ASSERT_TRUE(std::isfinite(J_ad(i, j)), errorMsg.c_str()); + ASSERT_TRUE(std::abs(J_ad(i, j)) < 1e6, "AD Jacobian contains excessively large value"); + } + } + ASSERT_TRUE(J_ad.norm() < 1e6, "AD Jacobian norm is excessively large, indicating potential numerical instability"); + } + + // Tests the consistency of the Jacobian computed via automatic differentiation across multiple evaluations, ensuring that repeated computations yield the same results. + TEST("AD Jacobian", ADJacobian_consistency) { + VecX_T> x(2); + x(0) = DualNumber_T(1.0, { 1.0, 0.0 }); // x = 1.0 with dual part for df/dx + x(1) = DualNumber_T(2.0, { 0.0, 1.0 }); // y = 2.0 with dual part for df/dy + MatX_T J_first(2, 2); + MatX_T J_second(2, 2); + VecX_T> f_first = vectorFunction(x); + J_first(0, 0) = f_first(0).dual[0]; // df1/dx + J_first(0, 1) = f_first(0).dual[1]; // df1/dy + J_first(1, 0) = f_first(1).dual[0]; // df2/dx + J_first(1, 1) = f_first(1).dual[1]; // df2/dy + VecX_T> f_second = vectorFunction(x); + J_second(0, 0) = f_second(0).dual[0]; // df1/dx + J_second(0, 1) = f_second(0).dual[1]; // df1/dy + J_second(1, 0) = f_second(1).dual[0]; // df2/dx + J_second(1, 1) = f_second(1).dual[1]; // df2/dy + for (int i = 0; i < J_first.rows(); ++i) { + for (int j = 0; j < J_first.cols(); ++j) { + std::string errorMsg = "Inconsistent AD Jacobian at (" + std::to_string(i) + + ", " + std::to_string(j) + + "): " + std::to_string(J_first(i, j)) + + " vs " + std::to_string(J_second(i, j)); + ASSERT_TRUE(std::abs(J_first(i, j) - J_second(i, j)) < 1e-6, errorMsg.c_str()); + ASSERT_TRUE(std::isfinite(J_first(i, j)) && std::isfinite(J_second(i, j)), "AD Jacobian contains non-finite value"); + } + } + ASSERT_TRUE(J_first.isApprox(J_second, 1e-6), "AD Jacobians from repeated evaluations do not match"); + } } \ No newline at end of file From 878fc6d7b89f0766f5f154b4e2e98dd3e1062435 Mon Sep 17 00:00:00 2001 From: saltyjoss Date: Sun, 17 May 2026 17:25:42 +0100 Subject: [PATCH 024/210] fixes: Updated `DualNumbers` to reflect correct errors found by unit tests. --- PxMLib/MathLib/include/core/DualNumbers.h | 32 +++++++++++++++++++++-- 1 file changed, 30 insertions(+), 2 deletions(-) diff --git a/PxMLib/MathLib/include/core/DualNumbers.h b/PxMLib/MathLib/include/core/DualNumbers.h index 67d0f4d6..aaaa25e6 100644 --- a/PxMLib/MathLib/include/core/DualNumbers.h +++ b/PxMLib/MathLib/include/core/DualNumbers.h @@ -430,6 +430,15 @@ namespace mathlib { } return a; } + // Addition assignment operator with scalar + template + inline DualNumber_T& operator+=( + DualNumber_T& a, + Scalar b + ) { + a.real += b; + return a; + } // Subtraction assignment operator template @@ -443,6 +452,15 @@ namespace mathlib { } return a; } + // Subtraction assignment operator with scalar + template + inline DualNumber_T& operator-=( + DualNumber_T& a, + Scalar b + ) { + a.real -= b; + return a; + } // Scalar multiplication assignment operator template @@ -477,7 +495,12 @@ namespace mathlib { DualNumber_T& a, Scalar b ) { - a.real /= b ? b : throw std::runtime_error("Division by zero in DualNumber_T operator/="); + if (b == Scalar(0)) { + throw std::runtime_error( + "Division by zero in DualNumber_T operator/=" + ); + } + a.real /= b; for (size_t i = 0; i < NVar; ++i) { a.dual[i] /= b; } @@ -494,7 +517,12 @@ namespace mathlib { for (size_t i = 0; i < NVar; ++i) { a.dual[i] = (a.dual[i] * b.real - ogReal * b.dual[i]) / (b.real * b.real); } - a.real = ogReal / b.real ? ogReal / b.real : throw std::runtime_error("Division by zero in DualNumber_T operator/="); + if (b.real == Scalar(0)) { + throw std::runtime_error( + "Division by zero in DualNumber_T operator/=" + ); + } + a.real = ogReal / b.real; return a; } From 918316a0990c76d21ba794f02614f12cb326b3b9 Mon Sep 17 00:00:00 2001 From: saltyjoss Date: Sun, 17 May 2026 17:29:45 +0100 Subject: [PATCH 025/210] fixes: Corrected faulty test logic * ALL TESTS PASS, given they are basic, but my logic is good * Though my brain is frazzled by the abstraction, i am going to try get the implicit testing done relatiove to the dual numbers --- .../AssignmentOperatorTests.cpp | 22 +++++++++++++++++-- .../DualNumberTests/DifferentiationTests.cpp | 6 ++--- .../DualNumberTests/DivisionTests.cpp | 6 ++--- 3 files changed, 26 insertions(+), 8 deletions(-) diff --git a/PxMLib/MathLib_Unit/DualNumberTests/AssignmentOperatorTests.cpp b/PxMLib/MathLib_Unit/DualNumberTests/AssignmentOperatorTests.cpp index fb1f7ff0..660010ca 100644 --- a/PxMLib/MathLib_Unit/DualNumberTests/AssignmentOperatorTests.cpp +++ b/PxMLib/MathLib_Unit/DualNumberTests/AssignmentOperatorTests.cpp @@ -17,6 +17,15 @@ namespace { ASSERT_TRUE(std::abs(a.dual[0] - 0.6) < 1e-12, "Addition assignment dual[0] part incorrect"); ASSERT_TRUE(std::abs(a.dual[1] - 0.3) < 1e-12, "Addition assignment dual[1] part incorrect"); } + // Test that addition assignment works with dual numbers and scalars + TEST("Assignment Operators", DualNumber_AdditionAssignmentScalar) { + DualNumber_T a(1.0, { 0.5, 0.25 }); + double scalar = 2.0; + a += scalar; + ASSERT_TRUE(std::abs(a.real - 3.0) < 1e-12, "Addition assignment with scalar real part incorrect"); + ASSERT_TRUE(std::abs(a.dual[0] - 0.5) < 1e-12, "Addition assignment with scalar dual[0] part incorrect"); + ASSERT_TRUE(std::abs(a.dual[1] - 0.25) < 1e-12, "Addition assignment with scalar dual[1] part incorrect"); + } // Test that subtraction assignment works correctly for dual numbers TEST("Assignment Operators", DualNumber_SubtractionAssignment) { DualNumber_T a(1.0, { 0.5, 0.25 }); @@ -26,6 +35,15 @@ namespace { ASSERT_TRUE(std::abs(a.dual[0] - 0.4) < 1e-12, "Subtraction assignment dual[0] part incorrect"); ASSERT_TRUE(std::abs(a.dual[1] - 0.2) < 1e-12, "Subtraction assignment dual[1] part incorrect"); } + // Test that subtraction assignment works with dual numbers and scalars + TEST("Assignment Operators", DualNumber_SubtractionAssignmentScalar) { + DualNumber_T a(1.0, { 0.5, 0.25 }); + double scalar = 2.0; + a -= scalar; + ASSERT_TRUE(std::abs(a.real + 1.0) < 1e-12, "Subtraction assignment with scalar real part incorrect"); + ASSERT_TRUE(std::abs(a.dual[0] - 0.5) < 1e-12, "Subtraction assignment with scalar dual[0] part incorrect"); + ASSERT_TRUE(std::abs(a.dual[1] - 0.25) < 1e-12, "Subtraction assignment with scalar dual[1] part incorrect"); + } // Test that addition assignment and subtraction assignment are inverses for dual numbers TEST("Assignment Operators", DualNumber_AddSubAssignmentInverse) { DualNumber_T a(1.0, { 0.5, 0.25 }); @@ -50,8 +68,8 @@ namespace { DualNumber_T a(1.0, { 0.5, 0.25 }); DualNumber_T b(2.0, { 0.1, 0.05 }); a /= b; - double expected0 = (0.5f * 2.0f - 1.0f * 0.1f) / (2.0f * 2.0f); - double expected1 = (0.25f * 2.0f - 1.0f * 0.05f) / (2.0f * 2.0f); + double expected0 = (0.5 * 2.0 - 1.0 * 0.1) / (2.0 * 2.0); + double expected1 = (0.25 * 2.0 - 1.0 * 0.05) / (2.0 * 2.0); ASSERT_TRUE(std::abs(a.real - 0.5) < 1e-12, "Division real part incorrect"); ASSERT_TRUE(std::abs(a.dual[0] - expected0) < 1e-12, "Division dual[0] part incorrect"); ASSERT_TRUE(std::abs(a.dual[1] - expected1) < 1e-12, "Division dual[1] part incorrect"); diff --git a/PxMLib/MathLib_Unit/DualNumberTests/DifferentiationTests.cpp b/PxMLib/MathLib_Unit/DualNumberTests/DifferentiationTests.cpp index 44d23fa4..a24b4477 100644 --- a/PxMLib/MathLib_Unit/DualNumberTests/DifferentiationTests.cpp +++ b/PxMLib/MathLib_Unit/DualNumberTests/DifferentiationTests.cpp @@ -11,12 +11,12 @@ namespace { // Test that the derivative of a simple polynomial function is computed correctly using dual numbers TEST("Differentiation", DualNumber_PolynomialDerivative) { auto f = [](const DualNumber_T& x) { - return 3.0 * x * x + 2.0 * x + 1.0; + return 3.0 * x * x + 2.0 * x + 1.0; // f(x) = 3x^2 + 2x + 1, f'(x) = 6x + 2 }; DualNumber_T x(1.0, { 1.0 }); // Set dual part to 1 to compute derivative DualNumber_T y = f(x); ASSERT_TRUE(std::abs(y.real - 6.0) < 1e-12, "Polynomial function value incorrect"); - ASSERT_TRUE(std::abs(y.dual[0] - 4.0) < 1e-12, "Polynomial derivative incorrect"); + ASSERT_TRUE(std::abs(y.dual[0] - 8.0) < 1e-12, "Polynomial derivative incorrect"); } // Test that the derivative of x^2 is computed correctly by the product rule using dual numbers TEST("Differentiation", DualNumber_ProductRule_xSquared) { @@ -26,7 +26,7 @@ namespace { DualNumber_T x(2.0, { 1.0 }); // Set dual part to 1 to compute derivative DualNumber_T y = f(x); ASSERT_TRUE(std::abs(y.real - 4.0) < 1e-12, "Product rule function value incorrect"); - ASSERT_TRUE(std::abs(y.dual[0] - 8.0) < 1e-12, "Product rule derivative incorrect"); + ASSERT_TRUE(std::abs(y.dual[0] - 4.0) < 1e-12, "Product rule derivative incorrect"); } // Test that the sine function's derivative is computed correctly using dual numbers TEST("Differentiation", DualNumber_SineDerivative) { diff --git a/PxMLib/MathLib_Unit/DualNumberTests/DivisionTests.cpp b/PxMLib/MathLib_Unit/DualNumberTests/DivisionTests.cpp index fc34691d..26224ac0 100644 --- a/PxMLib/MathLib_Unit/DualNumberTests/DivisionTests.cpp +++ b/PxMLib/MathLib_Unit/DualNumberTests/DivisionTests.cpp @@ -13,7 +13,7 @@ namespace { DualNumber_T a(1.0, { 0.5 }); DualNumber_T b(2.0, { 0.1 }); DualNumber_T c = a / b; - double expected0 = (0.5f * 2.0f - 1.0f * 0.1f) / (2.0f * 2.0f); + double expected0 = (0.5 * 2.0 - 1.0 * 0.1) / (2.0 * 2.0); ASSERT_TRUE(std::abs(c.real - 0.5) < 1e-12, "Division real part incorrect"); ASSERT_TRUE(std::abs(c.dual[0] - expected0) < 1e-12, "Division dual[0] part incorrect"); } @@ -22,8 +22,8 @@ namespace { DualNumber_T a(1.0, { 0.5, 0.25 }); DualNumber_T b(2.0, { 0.1, 0.05 }); DualNumber_T c = a / b; - double expected0 = (0.5f * 2.0f - 1.0f * 0.1f) / (2.0f * 2.0f); - double expected1 = (0.25f * 2.0f - 1.0f * 0.05f) / (2.0f * 2.0f); + double expected0 = (0.5 * 2.0 - 1.0 * 0.1) / (2.0 * 2.0); + double expected1 = (0.25 * 2.0 - 1.0f * 0.05) / (2.0 * 2.0); ASSERT_TRUE(std::abs(c.real - 0.5) < 1e-12, "Division real part incorrect"); ASSERT_TRUE(std::abs(c.dual[0] - expected0) < 1e-12, "Division dual[0] part incorrect"); ASSERT_TRUE(std::abs(c.dual[1] - expected1) < 1e-12, "Division dual[1] part incorrect"); From 1bdc264ca9c1071b1556523bf059908f1677e450 Mon Sep 17 00:00:00 2001 From: saltyjoss Date: Sun, 17 May 2026 18:09:13 +0100 Subject: [PATCH 026/210] chore: Updated test naming --- .../{ADIntegratorTests.cpp => ADExplicitIntegratorTests.cpp} | 2 +- .../MathLib_Unit/AutomaticDifferentiationTests/CMakeLists.txt | 3 ++- 2 files changed, 3 insertions(+), 2 deletions(-) rename PxMLib/MathLib_Unit/AutomaticDifferentiationTests/{ADIntegratorTests.cpp => ADExplicitIntegratorTests.cpp} (99%) diff --git a/PxMLib/MathLib_Unit/AutomaticDifferentiationTests/ADIntegratorTests.cpp b/PxMLib/MathLib_Unit/AutomaticDifferentiationTests/ADExplicitIntegratorTests.cpp similarity index 99% rename from PxMLib/MathLib_Unit/AutomaticDifferentiationTests/ADIntegratorTests.cpp rename to PxMLib/MathLib_Unit/AutomaticDifferentiationTests/ADExplicitIntegratorTests.cpp index b0a99cf6..d47d0fb4 100644 --- a/PxMLib/MathLib_Unit/AutomaticDifferentiationTests/ADIntegratorTests.cpp +++ b/PxMLib/MathLib_Unit/AutomaticDifferentiationTests/ADExplicitIntegratorTests.cpp @@ -1,4 +1,4 @@ -// MathLib_UnitTests ADIntegratorTests.cpp +// MathLib_UnitTests ADExplicitIntegratorTests.cpp #include "TestHarness.h" #include diff --git a/PxMLib/MathLib_Unit/AutomaticDifferentiationTests/CMakeLists.txt b/PxMLib/MathLib_Unit/AutomaticDifferentiationTests/CMakeLists.txt index 78449aff..4652f2d8 100644 --- a/PxMLib/MathLib_Unit/AutomaticDifferentiationTests/CMakeLists.txt +++ b/PxMLib/MathLib_Unit/AutomaticDifferentiationTests/CMakeLists.txt @@ -6,9 +6,10 @@ enable_testing() add_executable(MathLib_ADTests main.cpp - ADIntegratorTests.cpp ADNewtonTests.cpp ADJacobianTests.cpp + ADExplicitIntegratorTests.cpp + ADImplicitIntegratorTests.cpp ) target_link_libraries(MathLib_ADTests PRIVATE From 4e871c0a22f222275332db6e44f51bd6a98b4191 Mon Sep 17 00:00:00 2001 From: saltyjoss Date: Sun, 17 May 2026 18:09:42 +0100 Subject: [PATCH 027/210] feat: Added implicit integrator tests placeholder, will fill once moved tests over to AD --- .../ADImplicitIntegratorTests.cpp | 15 +++++++++++++++ 1 file changed, 15 insertions(+) create mode 100644 PxMLib/MathLib_Unit/AutomaticDifferentiationTests/ADImplicitIntegratorTests.cpp diff --git a/PxMLib/MathLib_Unit/AutomaticDifferentiationTests/ADImplicitIntegratorTests.cpp b/PxMLib/MathLib_Unit/AutomaticDifferentiationTests/ADImplicitIntegratorTests.cpp new file mode 100644 index 00000000..b043193f --- /dev/null +++ b/PxMLib/MathLib_Unit/AutomaticDifferentiationTests/ADImplicitIntegratorTests.cpp @@ -0,0 +1,15 @@ +// MathLib_UnitTests ADImplicitIntegratorTests.cpp + +#include "TestHarness.h" +#include +#include +#include +#include +#include + +using namespace mathlib; +using namespace constants; + +namespace { + // Will test once implicit methods have been moved to their AD versions +} \ No newline at end of file From 6c55db288276c731783953f47634930f96f7cc67 Mon Sep 17 00:00:00 2001 From: saltyjoss Date: Sun, 17 May 2026 18:13:26 +0100 Subject: [PATCH 028/210] feat: Added `automaticDifferenceJacobian` method --- .../include/integrators/numerical_integrators.h | 10 +++++++++- 1 file changed, 9 insertions(+), 1 deletion(-) diff --git a/PxMLib/MathLib/include/integrators/numerical_integrators.h b/PxMLib/MathLib/include/integrators/numerical_integrators.h index 4fd1ad3b..16bd5bbe 100644 --- a/PxMLib/MathLib/include/integrators/numerical_integrators.h +++ b/PxMLib/MathLib/include/integrators/numerical_integrators.h @@ -11,6 +11,8 @@ // Numerical integration methods namespace integration { + constexpr bool USE_AD_JACOBIANS = true; + // Ordinary Differential Equation (ODE) solvers class MATHLIB_API NumericalIntegrator { public: @@ -115,7 +117,7 @@ namespace integration { Func&& f, JacFunc&& jac = nullptr, int maxIter = 150, - Scalar tol = -1 + Scalar tol = 1e-14 ); private: @@ -137,6 +139,12 @@ namespace integration { Scalar t, const mathlib::VecX_T& x ); + + template + mathlib::MatX_T automaticDifferenceJacobian( + Func&& f, + const mathlib::VecX_T& x + ); }; }// namespace integration From d16051cec58aefd80250c894ef88f7fc42fd3e82 Mon Sep 17 00:00:00 2001 From: saltyjoss Date: Sun, 17 May 2026 18:31:22 +0100 Subject: [PATCH 029/210] feat: Added first implementation of AD jacobian method logic * This is going to change, however, I want something that fits with the current templating. --- .../integrators/numerical_integrators.inl | 24 +++++++++++++++++-- 1 file changed, 22 insertions(+), 2 deletions(-) diff --git a/PxMLib/MathLib/include/integrators/numerical_integrators.inl b/PxMLib/MathLib/include/integrators/numerical_integrators.inl index beb4512c..9242d7a8 100644 --- a/PxMLib/MathLib/include/integrators/numerical_integrators.inl +++ b/PxMLib/MathLib/include/integrators/numerical_integrators.inl @@ -200,7 +200,6 @@ namespace integration { // The function g(x_guess) = 0 that we want to solve for the implicit Euler step auto g = [&](const mathlib::VecX_T& x_guess, mathlib::VecX_T& g_out) { g_out = x_guess - x - dt * f(t + dt, x_guess); }; - // Numerical Jacobian for Newton-Raphson auto J = [&](const mathlib::VecX_T& x_guess, mathlib::MatX_T& J_out) { int n = (int)x_guess.size(); @@ -601,8 +600,8 @@ namespace integration { const mathlib::VecX_T& x ) { const Scalar eps_rel = 1e-8; + const int n = static_cast(x.size()); mathlib::VecX_T f_0 = f(t, x); - int n = (int)x.size(); mathlib::MatX_T J(f_0.size(), n); // Compute the Jacobian column by column using central differences for (int i = 0; i < n; ++i) { @@ -617,4 +616,25 @@ namespace integration { } return J; } + + // Automatic Difference Jacobian + template + mathlib::MatX_T NumericalIntegrator::automaticDifferenceJacobian( + Func&& f, + const mathlib::VecX_T& x + ) { + const int n = static_cast(x.size()); + mathlib::MatX_T J(n, n); + for (int i = 0; i < n; ++i) { + mathlib::VecX_T> x_dual(n); + for (int k = 0; k < n; ++k) { + x_dual(k) = DualNumber_T(x(k), (k == i) ? Scalar(1) : Scalar(0)); + } + auto f_dual = f(x_dual); + for (int j = 0; j < n; ++j) { + J(j, i) = f_dual(j).dual[0]; + } + } + return J; + } } \ No newline at end of file From 1caace5b7d3dace7ff0d133d4d35e5631f49ef08 Mon Sep 17 00:00:00 2001 From: saltyjoss Date: Sun, 17 May 2026 18:41:36 +0100 Subject: [PATCH 030/210] refactor: Updated implicit methods to use the current AD jacobian method --- .../integrators/numerical_integrators.inl | 154 +++++++++++++----- 1 file changed, 117 insertions(+), 37 deletions(-) diff --git a/PxMLib/MathLib/include/integrators/numerical_integrators.inl b/PxMLib/MathLib/include/integrators/numerical_integrators.inl index 9242d7a8..a44cc2d3 100644 --- a/PxMLib/MathLib/include/integrators/numerical_integrators.inl +++ b/PxMLib/MathLib/include/integrators/numerical_integrators.inl @@ -220,11 +220,25 @@ namespace integration { } } if (!analytical_success) { - F = finiteDifferenceJacobian( - [&](Scalar, const mathlib::VecX_T& x_pert) { return f(t + dt, x_pert); }, - t + dt, - x_guess - ); + if (USE_AD_JACOBIANS == true) { + printf("Using AD Jacobian for Implicit Euler\n"); + F = automaticDifferenceJacobian( + [&](Scalar, const mathlib::VecX_T& x_pert) { + return f(t + c(0) * dt, x_pert); + }, + x_guess + ); + } + else { + printf("Using finite difference Jacobian for Implicit Euler\n"); + F = finiteDifferenceJacobian( + [&](Scalar, const mathlib::VecX_T& x_pert) { + return f((t + dt) / Scalar(2), (x + x_pert) / Scalar(2)); + }, + t + dt / Scalar(2), + x_guess + ); + } } J_out = mathlib::MatX_T::Identity(n, n) - dt * F; // J = I - dt * df/dx @@ -267,11 +281,25 @@ namespace integration { } } if (!analytical_success) { - F = finiteDifferenceJacobian( - [&](Scalar, const mathlib::VecX_T& x_pert) { return f((t + dt) / Scalar(2), (x + x_pert) / Scalar(2)); }, - t + dt / Scalar(2), - x_guess - ); + if (USE_AD_JACOBIANS == true) { + printf("Using AD Jacobian for Implicit Midpoint\n"); + F = automaticDifferenceJacobian( + [&](Scalar, const mathlib::VecX_T& x_pert) { + return f(t + c(0) * dt, x_pert); + }, + x_guess + ); + } + else { + printf("Using finite difference Jacobian for Implicit Midpoint\n"); + F = finiteDifferenceJacobian( + [&](Scalar, const mathlib::VecX_T& x_pert) { + return f((t + dt) / Scalar(2), (x + x_pert) / Scalar(2)); + }, + t + dt / Scalar(2), + x_guess + ); + } } J_out = mathlib::MatX_T::Identity(n, n) - Scalar(0.5) * dt * F; // J = I - dt * df/dx }; @@ -353,9 +381,38 @@ namespace integration { } if (!analytical_success) { - printf("Using finite difference Jacobian for GLRK2\n"); - F1 = finiteDifferenceJacobian([&](Scalar, const mathlib::VecX_T& x_pert) { return f(t + c(0) * dt, x_pert); }, t + c(0) * dt, x1); - F2 = finiteDifferenceJacobian([&](Scalar, const mathlib::VecX_T& x_pert) { return f(t + c(1) * dt, x_pert); }, t + c(1) * dt, x2); + if (USE_AD_JACOBIANS == true) { + printf("Using AD Jacobian for GLRK2\n"); + F1 = automaticDifferenceJacobian( + [&](Scalar, const mathlib::VecX_T& x_pert) { + return f(t + c(0) * dt, x_pert); + }, + x1 + ); + F2 = automaticDifferenceJacobian( + [&](Scalar, const mathlib::VecX_T& x_pert) { + return f(t + c(0) * dt, x_pert); + }, + x2 + ); + } + else { + printf("Using finite difference Jacobian for GLRK2\n"); + F1 = finiteDifferenceJacobian( + [&](Scalar, const mathlib::VecX_T& x_pert) { + return f(t + c(0) * dt, x_pert); + }, + t + c(0) * dt, + x1 + ); + F2 = finiteDifferenceJacobian( + [&](Scalar, const mathlib::VecX_T& x_pert) { + return f(t + c(1) * dt, x_pert); + }, + t + c(1) * dt, + x2 + ); + } } J.setZero(2 * n, 2 * n); @@ -468,30 +525,53 @@ namespace integration { } if (!analytical_success) { - printf("Using finite difference Jacobian for GLRK3\n"); - F1 = finiteDifferenceJacobian( - [&](Scalar, const mathlib::VecX_T& x_pert) { - return f(t + c(0) * dt, x_pert); - }, - t + c(0) * dt, - x1 - ); - - F2 = finiteDifferenceJacobian( - [&](Scalar, const mathlib::VecX_T& x_pert) { - return f(t + c(1) * dt, x_pert); - }, - t + c(1) * dt, - x2 - ); - - F3 = finiteDifferenceJacobian( - [&](Scalar, const mathlib::VecX_T& x_pert) { - return f(t + c(2) * dt, x_pert); - }, - t + c(2) * dt, - x3 - ); + if (USE_AD_JACOBIANS == true) { + printf("Using AD Jacobian for GLRK3\n"); + F1 = automaticDifferenceJacobian( + [&](Scalar, const mathlib::VecX_T& x_pert) { + return f(t + c(0) * dt, x_pert); + }, + x1 + ); + F2 = automaticDifferenceJacobian( + [&](Scalar, const mathlib::VecX_T& x_pert) { + return f(t + c(0) * dt, x_pert); + }, + x2 + ); + F3 = automaticDifferenceJacobian( + [&](Scalar, const mathlib::VecX_T& x_pert) { + return f(t + c(0) * dt, x_pert); + }, + x3 + ); + } + else { + printf("Using finite difference Jacobian for GLRK3\n"); + F1 = finiteDifferenceJacobian( + [&](Scalar, const mathlib::VecX_T& x_pert) { + return f(t + c(0) * dt, x_pert); + }, + t + c(0) * dt, + x1 + ); + + F2 = finiteDifferenceJacobian( + [&](Scalar, const mathlib::VecX_T& x_pert) { + return f(t + c(1) * dt, x_pert); + }, + t + c(1) * dt, + x2 + ); + + F3 = finiteDifferenceJacobian( + [&](Scalar, const mathlib::VecX_T& x_pert) { + return f(t + c(2) * dt, x_pert); + }, + t + c(2) * dt, + x3 + ); + } } J.setZero(3 * n, 3 * n); From bbc17d980724ba8aa3e3faa1cb60a2c5d9218796 Mon Sep 17 00:00:00 2001 From: saltyjoss Date: Sun, 17 May 2026 23:05:54 +0100 Subject: [PATCH 031/210] feat: Add AD Implicit Tests --- .../ADImplicitIntegratorTests.cpp | 666 +++++++++++++++++- 1 file changed, 665 insertions(+), 1 deletion(-) diff --git a/PxMLib/MathLib_Unit/AutomaticDifferentiationTests/ADImplicitIntegratorTests.cpp b/PxMLib/MathLib_Unit/AutomaticDifferentiationTests/ADImplicitIntegratorTests.cpp index b043193f..936b5b74 100644 --- a/PxMLib/MathLib_Unit/AutomaticDifferentiationTests/ADImplicitIntegratorTests.cpp +++ b/PxMLib/MathLib_Unit/AutomaticDifferentiationTests/ADImplicitIntegratorTests.cpp @@ -11,5 +11,669 @@ using namespace mathlib; using namespace constants; namespace { - // Will test once implicit methods have been moved to their AD versions + constexpr double tol_low = 1e-3; + constexpr double tol_high = 1e-6; + + integration::NumericalIntegrator integrator; + + // Exponential Decay function for dual numbers + template + VecX_T> expDecay( + DualNumber_T /*t*/, + const VecX_T>& x + ) { + VecX_T> dx = x; + for (int i = 0; i < x.size(); ++i) { dx(i) = -x(i); } + return dx; + } + + // Harmonic Oscillator function for dual numbers + template + VecX_T> harmonicOsc( + DualNumber_T /*t*/, + const VecX_T>& s + ) { + VecX_T> d(2); + d(0) = s(1); + d(1) = -s(0); + return d; + } + + // Linear Decay function for dual numbers + template + VecX_T> linearDecay( + DualNumber_T /*t*/, + const VecX_T>& x + ) { + VecX_T> dx = x; + for (int i = 0; i < x.size(); ++i) { dx(i) = -2.0 * x(i); } + return dx; + } + + // Nonlinear Decay function for dual numbers + template + VecX_T> nonlinearDecay( + DualNumber_T /*t*/, + const VecX_T>& x + ) { + VecX_T> dx = x; + for (int i = 0; i < x.size(); ++i) { dx(i) = -x(i) * x(i); } + return dx; + } + + // Stiff Decay function for dual numbers + template + VecX_T> stiffDecay( + DualNumber_T /*t*/, + const VecX_T>& x + ) { + VecX_T> dx = x; + for (int i = 0; i < x.size(); ++i) { dx(i) = -100.0 * x(i); } + return dx; + } + + // Helper function to integrate from t=0 to t=T using N steps of the given step function, for dual numbers + template + VecX_T> integrateToTime( + StepFn stepFn, + const VecX_T>& x0, + Scalar T, + int N + ) { + DualNumber_T dt(T / Scalar(N)); + DualNumber_T t(0.0); + VecX_T> x = x0; + for (int i = 0; i < N; ++i) { x = stepFn(x, t, dt); t += dt; } + return x; + } + + // Helper function to verify that the exponential decay ODE is integrated correctly, and that the sensitivity of the solution with respect to the initial condition matches the analytical derivative. + template + void verifyExpDecaySensitivity( + StepFn stepFn, + double tol, + int N + ) { + VecX_T> x0(1); + x0(0) = DualNumber_T(1.0, { 1.0 }); // Initial condition with derivative 1.0 + VecX_T> r = integrateToTime(stepFn, x0, 1.0, N); + double expectedValue = std::exp(-1.0); + ASSERT_TRUE(std::abs(r(0).real - expectedValue) < tol, "Exponential decay error too large"); + ASSERT_TRUE(std::abs(r(0).dual[0] - expectedValue) < tol, "Exponential decay sensitivity error too large"); + ASSERT_TRUE(std::abs(r(0).dual[0]) > 0.0, "Sensitivity vanished during propagation"); + } + + // Helper function to verify that a harmonic oscillator ODE is integrated correctly, and that the sensitivity of the solution with respect to the initial condition matches the analytical derivative. + template + void verifyHarmonicOscillatorSensitivity( + StepFn stepFn, + double tol, + int N + ) { + VecX_T> x0(2); + x0(0) = DualNumber_T(1.0, { 1.0, 0.0 }); // Initial position with sensitivity + x0(1) = DualNumber_T(0.0, { 0.0, 1.0 }); // Initial velocity with sensitivity + VecX_T> r = integrateToTime(stepFn, x0, TWO_PI_d, N); + double expectedPosition = std::cos(TWO_PI_d); + double expectedVelocity = -std::sin(TWO_PI_d); + double expectedVelocitySens = std::cos(TWO_PI_d); + ASSERT_TRUE(std::abs(r(0).real - expectedPosition) < tol, "Harmonic oscillator position error too large"); + ASSERT_TRUE(std::abs(r(1).real - expectedVelocity) < tol, "Harmonic oscillator velocity error too large"); + ASSERT_TRUE(std::abs(r(0).dual[0] - expectedPosition) < tol, "Harmonic oscillator position sensitivity error too large"); + ASSERT_TRUE(std::abs(r(1).dual[1] - expectedVelocitySens) < tol, "Harmonic oscillator velocity sensitivity error too large"); + ASSERT_TRUE(std::abs(r(0).dual[0]) > 0.0, "Position sensitivity vanished during propagation"); + ASSERT_TRUE(std::abs(r(1).dual[1]) > 0.0, "Velocity sensitivity vanished during propagation"); + } + + // AD Implicit Euler Tests + // Test case for the GLRK2 method on the exponenital decay ODE. + TEST("AD Implicit Euler Method", ImplicitEuler_ExponentialDecay) { + VecX_T> x(1); + x(0) = DualNumber_T(1.0, { 0.0 }); + double T = 1.0; + DualNumber_T dt(T / 1000.0, { 0.0 }); + DualNumber_T t(0.0, { 0.0 }); + for (int i = 0; i < 1000; ++i) { + x = integrator.implicitEuler(x, t, dt, expDecay); + t += dt; + } + ASSERT_TRUE(std::abs(x(0).real - std::exp(-1.0)) < tol_low, "Implicit Euler exponential decay error too large"); + } + // Test case for the implicit Euler method on the exponential decay ODE. + TEST("AD Implicit Euler Method", ImplicitEuler_ExponentialDecaySensitivity) { + auto stepFn = [&]( + const VecX_T>& x, + DualNumber_T t, + DualNumber_T dt + ) { + return integrator.implicitEuler(x, t, dt, expDecay); + }; + verifyExpDecaySensitivity(stepFn, 1e-2, 1000); + } + // Test case for the Implicit Midpoint method on the linear decay ODE. + TEST("AD Implicit Midpoint Method", ImplicitEuler_LinearDecay) { + VecX_T> x(1); + x(0) = DualNumber_T(1.0, { 0.0 }); + double T = 0.5; + DualNumber_T dt(T / 500.0); + DualNumber_T t(0.0); + for (int i = 0; i < 500; ++i) { + x = integrator.implicitEuler(x, t, dt, linearDecay, 50, 1e-6); + t += dt; + } + ASSERT_TRUE(std::abs(x(0).real - std::exp(-1.0)) < tol_low, "Implicit Euler linear decay error too large"); + } + // Test case for the implicit Euler method on the linear decay ODE. + TEST("AD Implicit Euler Method", ImplicitEuler_LinearDecaySensitivity) { + auto stepFn = [&]( + const VecX_T>& x, + DualNumber_T t, + DualNumber_T dt + ) { + return integrator.implicitEuler(x, t, dt, linearDecay); + }; + verifyExpDecaySensitivity(stepFn, 1e-2, 1000); + } + // Test case for the Implicit Euler method on the stiff decay ODE. + TEST("AD Implicit Euler Method", ImplicitEuler_StiffDecay) { + VecX_T> x(1); + x(0) = DualNumber_T(1.0, { 0.0 }); + double T = 0.5; + DualNumber_T dt(T / 500.0); + DualNumber_T t(0.0); + double expected = std::exp(-50.0); + auto stiff_f = [](auto t, const auto& x) { + auto dx = x; + dx(0) = -100.0 * x(0); + return dx; + }; + for (int i = 0; i < 500; ++i) { + x = integrator.implicitEuler(x, t, dt, stiff_f); + t += dt; + } + ASSERT_TRUE(std::isfinite(x(0).real), "Implicit Euler produced a non-finite result."); + ASSERT_TRUE(x(0).real < 1e-12, "Implicit Euler did not sufficiently damp stiff mode"); + } + // Test case for the implicit Euler method on the stiff decay ODE. + TEST("AD Implicit Euler Method", ImplicitEuler_StiffDecaySensitivity) { + auto stepFn = [&]( + const VecX_T>& x, + DualNumber_T t, + DualNumber_T dt + ) { + return integrator.implicitEuler(x, t, dt, stiffDecay); + }; + verifyExpDecaySensitivity(stepFn, 1e-2, 1000); + } + // Test large step performance of implicit midpoint + TEST("AD Implicit Euler Method", ImplicitEuler_Stability_LargeStep) { + VecX_T> x(1); + x(0) = DualNumber_T(1.0, { 1.0 }); + double T = 1.0; + DualNumber_T t(0.0); + DualNumber_T dt(1.0 / 10.0); + DualNumber_T prev = x(0); + for (int i = 0; i < 10; ++i) { + x = integrator.implicitEuler(x, t, dt, stiffDecay); + t += dt; + ASSERT_TRUE(std::abs(x(0).real) < std::abs(prev.real), "Not monotone"); + ASSERT_TRUE(std::abs(x(0).dual[0]) < std::abs(prev.dual[0]), "Not strictly decaying"); + prev = x(0); + } + } + + // AD Implicit Midpoint Tests + // Test case for the Implicit Midpoint method on the exponenital decay ODE. + TEST("AD Implicit Midpoint Method", ImplicitMidpoint_ExponentialDecay) { + VecX_T> x(1); + x(0) = DualNumber_T(1.0, { 0.0 }); + double T = 1.0; + DualNumber_T dt(T / 1000.0, { 0.0 }); + DualNumber_T t(0.0, { 0.0 }); + for (int i = 0; i < 1000; ++i) { + x = integrator.implicitMidpoint(x, t, dt, expDecay); + t += dt; + } + ASSERT_TRUE(std::abs(x(0).real - std::exp(-1.0)) < tol_low, "Implicit Midpoint exponential decay error too large"); + } + // Test case for the implicit midpoint method on the exponential decay ODE. + TEST("AD Implicit Midpoint Method", ImplicitMidpoint_ExponentialDecaySensitivity) { + auto stepFn = [&]( + const VecX_T>& x, + DualNumber_T t, + DualNumber_T dt + ) { + return integrator.implicitMidpoint(x, t, dt, expDecay); + }; + verifyExpDecaySensitivity(stepFn, 1e-3, 1000); + } + // Test case for the Implicit Midpoint method on the harmonic oscillator ODE. + TEST("AD Implicit Midpoint Method", ImplicitMidpoint_HarmonicOscillator_EnergyPreservation) { + VecX_T> x(2); + x(0) = DualNumber_T(1.0, { 0.0, 0.0 }); + x(1) = DualNumber_T(0.0, { 0.0, 0.0 }); + double E0 = 0.5 * (x(0).real * x(0).real + x(1).real * x(1).real); + double T = 5.0; + DualNumber_T dt(T / 2500.0); + DualNumber_T t(0.0); + for (int i = 0; i < 2500; ++i) { + x = integrator.implicitMidpoint(x, t, dt, harmonicOsc); + t += dt; + } + double Ef = 0.5 * (x(0).real * x(0).real + x(1).real * x(1).real); + double drift = std::abs(Ef - E0); + ASSERT_TRUE(drift < tol_high * (1.0 + E0), "Implicit Midpoint energy drift too large for SHO"); + } + // Test case for the implicit midpoint method on the harmonic oscillator ODE. + TEST("AD Implicit Midpoint Method", ImplicitMidpoint_HarmonicOscillatorSensitivity) { + auto stepFn = [&]( + const VecX_T>& x, + DualNumber_T t, + DualNumber_T dt + ) { + return integrator.implicitMidpoint(x, t, dt, harmonicOsc); + }; + verifyHarmonicOscillatorSensitivity(stepFn, 1e-3, 1000); + } + // Test case for the Implicit Midpoint method on the linear decay ODE. + TEST("AD Implicit Midpoint Method", ImplicitMidpoint_LinearDecay) { + VecX_T> x(1); + x(0) = DualNumber_T(1.0, { 0.0 }); + double T = 0.5; + DualNumber_T dt(T / 500.0); + DualNumber_T t(0.0); + for (int i = 0; i < 500; ++i) { + x = integrator.implicitMidpoint(x, t, dt, linearDecay); + t += dt; + } + ASSERT_TRUE(std::abs(x(0).real - std::exp(-1.0)) < tol_low, "Implicit Midpoint linear decay error too large"); + } + // Test case for the implicit midpoint method on the linear decay ODE. + TEST("AD Implicit Midpoint Method", ImplicitMidpoint_LinearDecaySensitivity) { + auto stepFn = [&]( + const VecX_T>& x, + DualNumber_T t, + DualNumber_T dt + ) { + return integrator.implicitMidpoint(x, t, dt, linearDecay); + }; + verifyExpDecaySensitivity(stepFn, 1e-3, 1000); + } + // Test case for the Implicit Midpoint method on the stiff decay ODE. + TEST("AD Implicit Midpoint Method", ImplicitMidpoint_StiffDecay) { + VecX_T> x(1); + x(0) = DualNumber_T(1.0, { 0.0 }); + double T = 0.5; + DualNumber_T dt(T / 500.0); + DualNumber_T t(0.0); + double expected = std::exp(-50.0); + auto stiff_f = [](auto t, const auto& x) { + auto dx = x; + dx(0) = -100.0 * x(0); + return dx; + }; + for (int i = 0; i < 500; ++i) { + x = integrator.implicitMidpoint(x, t, dt, stiff_f); + t += dt; + } + ASSERT_TRUE(std::isfinite(x(0).real), "Implicit Midpoint produced a non-finite result."); + ASSERT_TRUE(x(0).real < 1e-12, "Implicit Midpoint did not sufficiently damp stiff mode"); + } + // Test case for the implicit midpoint method on the stiff decay ODE. + TEST("AD Implicit Midpoint Method", ImplicitMidpoint_StiffDecaySensitivity) { + auto stepFn = [&]( + const VecX_T>& x, + DualNumber_T t, + DualNumber_T dt + ) { + return integrator.implicitMidpoint(x, t, dt, stiffDecay); + }; + verifyExpDecaySensitivity(stepFn, 1e-3, 1000); + } + // Test large step performance of implicit midpoint + TEST("AD Implicit Midpoint Method", ImplicitMidpoint_Stability_LargeStep) { + VecX_T> x(1); + x(0) = DualNumber_T(1.0, { 1.0 }); + double T = 1.0; + DualNumber_T t(0.0); + DualNumber_T dt(1.0 / 10.0); + DualNumber_T prev = x(0); + for (int i = 0; i < 10; ++i) { + x = integrator.implicitMidpoint(x, t, dt, stiffDecay); + t += dt; + ASSERT_TRUE(std::abs(x(0).real) < std::abs(prev.real), "Not monotone"); + ASSERT_TRUE(std::abs(x(0).dual[0]) < std::abs(prev.dual[0]), "Not strictly decaying"); + prev = x(0); + } + } + + // AD GLRK2 Tests + // Test case for the GLRK2 method on the exponenital decay ODE. + TEST("AD GLRK2 Method", GLRK2_ExponentialDecay) { + VecX_T> x(1); + x(0) = DualNumber_T(1.0, { 0.0 }); + double T = 1.0; + DualNumber_T dt(T / 1000.0, { 0.0 }); + DualNumber_T t(0.0, { 0.0 }); + for (int i = 0; i < 1000; ++i) { + x = integrator.GLRK2(x, t, dt, expDecay, 50, 1e-6); + t += dt; + } + ASSERT_TRUE(std::abs(x(0).real - std::exp(-1.0)) < tol_low, "GLRK3 exponential decay error too large"); + } + // Test case for the GLRK2 method on the exponential decay ODE. + TEST("AD GLRK2 Method", GLRK2_ExponentialDecaySensitivity) { + auto stepFn = [&]( + const VecX_T>& x, + DualNumber_T t, + DualNumber_T dt + ) { + return integrator.GLRK2(x, t, dt, expDecay); + }; + verifyExpDecaySensitivity(stepFn, 1e-4, 1000); + } + // Test case for the GLRK2 method on the harmonic oscillator ODE. + TEST("AD GLRK2 Method", GLRK2_HarmonicOscillator_EnergyPreservation) { + VecX_T> x(2); + x(0) = DualNumber_T(1.0, { 0.0, 0.0 }); + x(1) = DualNumber_T(0.0, { 0.0, 0.0 }); + double E0 = 0.5 * (x(0).real * x(0).real + x(1).real * x(1).real); + double T = 5.0; + DualNumber_T dt(T / 2500.0); + DualNumber_T t(0.0); + for (int i = 0; i < 2500; ++i) { + x = integrator.GLRK2(x, t, dt, harmonicOsc, 50, 1e-6); + t += dt; + } + double Ef = 0.5 * (x(0).real * x(0).real + x(1).real * x(1).real); + double drift = std::abs(Ef - E0); + ASSERT_TRUE(drift < tol_high * (1.0 + E0), "GLRK2 energy drift too large for SHO"); + } + // Test case for the GLRK2 method on the harmonic oscillator ODE. + TEST("AD GLRK2 Method", GLRK2_HarmonicOscillatorSensitivity) { + auto stepFn = [&]( + const VecX_T>& x, + DualNumber_T t, + DualNumber_T dt + ) { + return integrator.GLRK2(x, t, dt, harmonicOsc); + }; + verifyHarmonicOscillatorSensitivity(stepFn, 1e-4, 1000); + } + // Test case for the GLRK2 method on the linear decay ODE. + TEST("AD GLRK2 Method", GLRK2_LinearDecay) { + VecX_T> x(1); + x(0) = DualNumber_T(1.0, { 0.0 }); + double T = 0.5; + DualNumber_T dt(T / 500.0); + DualNumber_T t(0.0); + for (int i = 0; i < 500; ++i) { + x = integrator.GLRK2(x, t, dt, linearDecay, 50, 1e-6); + t += dt; + } + ASSERT_TRUE(std::abs(x(0).real - std::exp(-1.0)) < tol_low, "GLRK2 linear decay error too large"); + } + // Test case for the GLRK2 method on the linear decay ODE. + TEST("AD GLRK2 Method", GLRK2_LinearDecaySensitivity) { + auto stepFn = [&]( + const VecX_T>& x, + DualNumber_T t, + DualNumber_T dt + ) { + return integrator.GLRK2(x, t, dt, linearDecay); + }; + verifyExpDecaySensitivity(stepFn, 1e-4, 1000); + } + // Test case for the GLRK2 method on the stiff decay ODE. + TEST("AD GLRK2 Method", GLRK2_StiffDecay) { + VecX_T> x(1); + x(0) = DualNumber_T(1.0, { 0.0 }); + double T = 0.5; + DualNumber_T dt(T / 500.0); + DualNumber_T t(0.0); + double expected = std::exp(-50.0); + auto stiff_f = [](auto t, const auto& x) { + auto dx = x; + dx(0) = -100.0 * x(0); + return dx; + }; + for (int i = 0; i < 500; ++i) { + x = integrator.GLRK2(x, t, dt, stiff_f, 50, 1e-10); + t += dt; + } + ASSERT_TRUE(std::isfinite(x(0).real), "GLRK2 produced a non-finite result."); + ASSERT_TRUE(x(0).real < 1e-12, "GLRK2 did not sufficiently damp stiff mode"); + } + // Test case for the GLRK2 method on the stiff decay ODE. + TEST("AD GLRK2 Method", GLRK2_StiffDecaySensitivity) { + auto stepFn = [&]( + const VecX_T>& x, + DualNumber_T t, + DualNumber_T dt + ) { + return integrator.GLRK2(x, t, dt, stiffDecay); + }; + verifyExpDecaySensitivity(stepFn, 1e-4, 1000); + } + // Test case for the GLRK2 method on the nonlinear decay ODE. + TEST("AD GLRK2 Method", GLRK2_NonlinearDecay) { + VecX_T> x(1); + x(0) = DualNumber_T(1.0, { 0.0 }); + double T = 0.5; + DualNumber_T dt(T / 500.0); + DualNumber_T t(0.0); + double expected = 1.0 / (1.0 + T); + auto nl_f = [](auto, const auto& x) { + auto dx = x; + dx(0) = -x(0) * x(0); + return dx; + }; + for (int i = 0; i < 500; ++i) { + x = integrator.GLRK2(x, t, dt, nl_f, 50, 1e-6); + t += dt; + } + double rel_err = std::abs(x(0).real - expected) / expected; + ASSERT_TRUE(rel_err < tol_low, "GLRK2 nonlinear decay error too large"); + } + // Test case for the GLRK2 method on the stiff nonlinear decay ODE. + TEST("AD GLRK2 Method", GLRK2_StiffNonlinearDecay) { + VecX_T> x(1); + x(0) = DualNumber_T(1.0, { 0.0 }); + double T = 0.5; + DualNumber_T dt(T / 500.0); + DualNumber_T t(0.0); + double expected = 1.0 / 51.0; + auto stiff_nl_f = [](auto t, const auto& x) { + auto dx = x; + dx(0) = -100.0 * x(0) - x(0) * x(0); + return dx; + }; + for (int i = 0; i < 500; ++i) { + x = integrator.GLRK2(x, t, dt, stiff_nl_f, 50, 1e-6); + t += dt; + } + double rel_err = std::abs(x(0).real - expected) / expected; + ASSERT_TRUE(rel_err < tol_low, "GLRK2 stiff nonlinear decay error too large"); + } + // Test large step performance of GLRK2 + TEST("AD GLRK2 Method", GLRK2_Stability_LargeStep) { + VecX_T> x(1); + x(0) = DualNumber_T(1.0, { 1.0 }); + double T = 1.0; + DualNumber_T t(0.0); + DualNumber_T dt(1.0 / 10.0); + DualNumber_T prev = x(0); + for (int i = 0; i < 10; ++i) { + x = integrator.GLRK2(x, t, dt, stiffDecay); + t += dt; + ASSERT_TRUE(std::abs(x(0).real) < std::abs(prev.real), "Not monotone"); + ASSERT_TRUE(std::abs(x(0).dual[0]) < std::abs(prev.dual[0]), "Not strictly decaying"); + prev = x(0); + } + } + + // AD GLRK3 Tests + // Test case for the GLRK3 method on the exponenital decay ODE. + TEST("AD GLRK3 Method", GLRK3_ExponentialDecay) { + VecX_T> x(1); + x(0) = DualNumber_T(1.0, { 0.0 }); + double T = 1.0; + DualNumber_T dt(T / 1000.0, { 0.0 }); + DualNumber_T t(0.0, { 0.0 }); + for (int i = 0; i < 1000; ++i) { + x = integrator.GLRK3(x, t, dt, expDecay, 80, 1e-7); + t += dt; + } + ASSERT_TRUE(std::abs(x(0).real - std::exp(-1.0)) < tol_low, "GLRK3 exponential decay error too large"); + } + // Test case for the GLRK3 method on the exponential decay ODE sensitivity. + TEST("AD GLRK3 Method", GLRK3_ExponentialDecaySensitivity) { + auto stepFn = [&]( + const VecX_T>& x, + DualNumber_T t, + DualNumber_T dt + ) { + return integrator.GLRK3(x, t, dt, expDecay); + }; + verifyExpDecaySensitivity(stepFn, 1e-4, 1000); + } + // Test case for the GLRK3 method on the harmonic oscillator ODE. + TEST("AD GLRK3 Method", GLRK3_HarmonicOscillator_EnergyPreservation) { + VecX_T> x(2); + x(0) = DualNumber_T(1.0, { 0.0, 0.0 }); + x(1) = DualNumber_T(0.0, { 0.0, 0.0 }); + double E0 = 0.5 * (x(0).real * x(0).real + x(1).real * x(1).real); + double T = 5.0; + DualNumber_T dt(T / 2500.0); + DualNumber_T t(0.0); + for (int i = 0; i < 2500; ++i) { + x = integrator.GLRK3(x, t, dt, harmonicOsc, 80, 1e-7); + t += dt; + } + double Ef = 0.5 * (x(0).real * x(0).real + x(1).real * x(1).real); + double drift = std::abs(Ef - E0); + ASSERT_TRUE(drift < tol_high * (1.0 + E0), "GLRK3 energy drift too large for SHO"); + } + // Test case for the GLRK3 method on the harmonic oscillator ODE. + TEST("AD GLRK3 Method", GLRK3_HarmonicOscillatorSensitivity) { + auto stepFn = [&]( + const VecX_T>& x, + DualNumber_T t, + DualNumber_T dt + ) { + return integrator.GLRK3(x, t, dt, harmonicOsc); + }; + verifyHarmonicOscillatorSensitivity(stepFn, 1e-4, 1000); + } + // Test case for the GLRK3 method on the linear decay ODE. + TEST("AD GLRK3 Method", GLRK3_LinearDecay) { + VecX_T> x(1); + x(0) = DualNumber_T(1.0, { 0.0 }); + double T = 0.5; + DualNumber_T dt(T / 500.0); + DualNumber_T t(0.0); + for (int i = 0; i < 500; ++i) { + x = integrator.GLRK3(x, t, dt, linearDecay, 80, 1e-7); + t += dt; + } + ASSERT_TRUE(std::abs(x(0).real - std::exp(-1.0)) < tol_low, "GLRK3 linear decay error too large"); + } + // Test case for the GLRK3 method on the linear decay ODE. + TEST("AD GLRK3 Method", GLRK3_LinearDecaySensitivity) { + auto stepFn = [&]( + const VecX_T>& x, + DualNumber_T t, + DualNumber_T dt + ) { + return integrator.GLRK3(x, t, dt, linearDecay); + }; + verifyExpDecaySensitivity(stepFn, 1e-12, 1000); + } + // Test case for the GLRK3 method on the stiff decay ODE. + TEST("AD GLRK3 Method", GLRK3_StiffDecay) { + VecX_T> x(1); + x(0) = DualNumber_T(1.0, { 0.0 }); + double T = 0.5; + DualNumber_T dt(T / 500.0); + DualNumber_T t(0.0); + double expected = std::exp(-50.0); + auto stiff_f = [](auto t, const auto& x) { + auto dx = x; + dx(0) = -100.0 * x(0); + return dx; + }; + for (int i = 0; i < 500; ++i) { + x = integrator.GLRK3(x, t, dt, stiff_f, 150, 1e-12); + t += dt; + } + ASSERT_TRUE(std::isfinite(x(0).real), "GLRK3 produced a non-finite result."); + ASSERT_TRUE(x(0).real < 1e-14, "GLRK3 did not sufficiently damp stiff mode"); + } + // Test case for the GLRK3 method on the stiff decay ODE. + TEST("AD GLRK3 Method", GLRK3_StiffDecaySensitivity) { + auto stepFn = [&]( + const VecX_T>& x, + DualNumber_T t, + DualNumber_T dt + ) { + return integrator.GLRK3(x, t, dt, stiffDecay); + }; + verifyExpDecaySensitivity(stepFn, 1e-12, 1000); + } + // Test case for the GLRK3 method on the nonlinear decay ODE. + TEST("AD GLRK3 Method", GLRK3_NonlinearDecay) { + VecX_T> x(1); + x(0) = DualNumber_T(1.0, { 0.0 }); + double T = 0.5; + DualNumber_T dt(T / 500.0); + DualNumber_T t(0.0); + double expected = 1.0 / (1.0 + T); + auto nl_f = [](auto t, const auto& x) { + auto dx = x; + dx(0) = -x(0) * x(0); + return dx; + }; + for (int i = 0; i < 500; ++i) { + x = integrator.GLRK3(x, t, dt, nl_f, 80, 1e-7); + t += dt; + } + double rel_err = std::abs(x(0).real -expected) / expected; + ASSERT_TRUE(rel_err < tol_low, "GLRK3 nonlinear decay error too large"); + } + // Test case for the GLRK3 method on the stiff nonlinear decay ODE. + TEST("AD GLRK3 Method", GLRK3_StiffNonlinearDecay) { + VecX_T> x(1); + x(0) = DualNumber_T(1.0, { 0.0 }); + double T = 0.5; + DualNumber_T dt(T / 500.0); + DualNumber_T t(0.0); + double expected = 1.0 / 51.0; + auto stiff_nl_f = [](auto t, const auto& x) { + auto dx = x.eval(); + dx(0) = -100.0 * x(0) - x(0) * x(0); + return dx; + }; + for (int i = 0; i < 500; ++i) { + x = integrator.GLRK3(x, t, dt, stiff_nl_f, 80, 1e-7); + t += dt; + } + double rel_err = std::abs(x(0).real - expected) / expected; + ASSERT_TRUE(rel_err < tol_low, "GLRK3 stiff nonlinear decay error too large"); + } + // Test case for the GLRK3 method on the stiff decay ODE. + TEST("AD GLRK3 Method", GLRK3_Stability_LargeStep) { + VecX_T> x(1); + x(0) = DualNumber_T(1.0, { 1.0 }); + double T = 1.0; + DualNumber_T t(0.0); + DualNumber_T dt(1.0 / 10.0); + DualNumber_T prev = x(0); + for (int i = 0; i < 10; ++i) { + x = integrator.GLRK3(x, t, dt, stiffDecay); + t += dt; + ASSERT_TRUE(std::abs(x(0).real) < std::abs(prev.real), "Not monotone"); + ASSERT_TRUE(std::abs(x(0).dual[0]) < std::abs(prev.dual[0]), "Not strictly decaying"); + prev = x(0); + } + } } \ No newline at end of file From 6978ce1fe3356ff541ad8a22434ad57c7e4d807c Mon Sep 17 00:00:00 2001 From: saltyjoss Date: Sun, 17 May 2026 23:06:25 +0100 Subject: [PATCH 032/210] fixes(INCOMPLETE): Solve template disconnect with dual numbers and current pipeline * I have been trying to fix this for neartly four hours. --- .../DSFE_Core/include/Robots/RobotDynamics.h | 13 ++-- .../include/Robots/RobotDynamics.inl | 31 ++++---- .../include/Robots/RobotKinematics.h | 4 +- .../include/Robots/RobotKinematics.inl | 8 +-- .../include/Robots/RobotSimSnapshot.h | 28 +++++--- .../include/Robots/SpatialDynamics.inl | 29 ++++++-- DSFE_App/DSFE_Core/src/Robots/RobotSystem.cpp | 19 +++-- PxMLib/MathLib/include/core/Types_tpl.h | 8 +++ .../integrators/numerical_integrators.h | 6 +- .../integrators/numerical_integrators.inl | 71 ++++++++++++------- 10 files changed, 141 insertions(+), 76 deletions(-) diff --git a/DSFE_App/DSFE_Core/include/Robots/RobotDynamics.h b/DSFE_App/DSFE_Core/include/Robots/RobotDynamics.h index c9345cdb..8644750c 100644 --- a/DSFE_App/DSFE_Core/include/Robots/RobotDynamics.h +++ b/DSFE_App/DSFE_Core/include/Robots/RobotDynamics.h @@ -26,9 +26,6 @@ namespace integration { class IntegrationService; enum class eIntegrationMethod; namespace robots { // Forward declarations - class RobotKinematics; - struct RobotConstModel; - struct RobotSimSnapshot; struct RobotLink; struct RobotJoint; enum class eTorqueMode; @@ -93,7 +90,7 @@ namespace robots { mathlib::VecX_T derivative_dense( Scalar t, const mathlib::VecX_T& x, - const RobotSimSnapshot& snap, + const RobotSimSnapshot_T& snap, DynamicsScratch& scratch, DynamicsResult& out ); @@ -103,7 +100,7 @@ namespace robots { const robots::SpatialModel& model, Scalar t, const mathlib::VecX_T& x, - const RobotSimSnapshot& snap, + const RobotSimSnapshot_T& snap, DynamicsScratch& scratch, DynamicsResult& out ); @@ -112,7 +109,7 @@ namespace robots { void jacobian_spatial( const robots::SpatialModel& model, const mathlib::VecX_T& x, - const RobotSimSnapshot& snap, + const RobotSimSnapshot_T& snap, const mathlib::VecX_T& kp, const mathlib::VecX_T& kd, mathlib::MatX_T& F_out, @@ -124,7 +121,7 @@ namespace robots { mathlib::VecX_T derivative_with_gains( Scalar t, const mathlib::VecX_T& x, - const RobotSimSnapshot& snap, + const RobotSimSnapshot_T& snap, const mathlib::VecX_T& kp, const mathlib::VecX_T& kd, DynamicsScratch& scratch, @@ -135,7 +132,7 @@ namespace robots { template void jacobian_with_gains( const mathlib::VecX_T& x, - const RobotSimSnapshot& snap, + const RobotSimSnapshot_T& snap, const mathlib::VecX_T& kp, const mathlib::VecX_T& kd, mathlib::MatX_T& F_out, diff --git a/DSFE_App/DSFE_Core/include/Robots/RobotDynamics.inl b/DSFE_App/DSFE_Core/include/Robots/RobotDynamics.inl index cf01499b..8b0b2cad 100644 --- a/DSFE_App/DSFE_Core/include/Robots/RobotDynamics.inl +++ b/DSFE_App/DSFE_Core/include/Robots/RobotDynamics.inl @@ -54,7 +54,7 @@ namespace robots { Scalar I_rot = axis_world.transpose() * I_world * axis_world; Scalar I_eff_i = I_trans + I_rot; - return std::max(I_eff_i, 1e-6); // [kg*m^2], I_eff contribution of this joint + floor to avoid singularities + return Eigen::numext::maxi(I_eff_i, Scalar(1e-6)); // [kg*m^2], I_eff contribution of this joint + floor to avoid singularities } // Computes the full mass matrix M(q) based on the current state and robot configuration @@ -131,7 +131,7 @@ namespace robots { const mathlib::MatX_T& M ) const { const size_t n = robot.joints.size(); - const Scalar eps = 1e-6; // small value to prevent division by zero + const Scalar eps = Scalar(1e-6); // small value to prevent division by zero mathlib::VecX_T q_eps = q; std::vector> dM_dq(n, mathlib::MatX_T::Zero(n, n)); // partial derivatives of M with respect to each joint angle @@ -205,7 +205,7 @@ namespace robots { for (size_t k = 0; k < robot.links.size(); ++k) { const RobotLink& link = robot.links[k]; const Scalar m = (Scalar)link.inertial.mass; - if (m <= 0.0) { continue; } + if (m <= Scalar(0)) { continue; } if (!robot.jointAffectsLink(i, k)) { continue; } @@ -215,7 +215,7 @@ namespace robots { // Gravitational force on the link mathlib::Vec3_T g_world; - g_world << mathlib::Vec3_T(0.0, 0.0, -g); // [m/s^2], gravity vector in world frame + g_world = mathlib::Vec3_T(0.0, 0.0, -g); // [m/s^2], gravity vector in world frame const mathlib::Vec3_T F_g = m * g_world; // [N], gravitational force on the link in world frame const mathlib::Vec3_T r = com_world - p_i; // [m] @@ -287,7 +287,7 @@ namespace robots { mathlib::VecX_T RobotDynamics::derivative_dense( Scalar t, const mathlib::VecX_T& x, - const RobotSimSnapshot& snap, + const RobotSimSnapshot_T& snap, DynamicsScratch& scratch, DynamicsResult& out ) { @@ -379,7 +379,7 @@ namespace robots { const robots::SpatialModel& model, Scalar t, const mathlib::VecX_T& x, - const RobotSimSnapshot& snap, + const RobotSimSnapshot_T& snap, DynamicsScratch& scratch, DynamicsResult& out ) { @@ -447,6 +447,13 @@ namespace robots { dx.head(n) = qd; dx.tail(n) = out.qdd; + using DXScalar = typename std::decay_t; + + static_assert( + !std::is_same_v, + "dx collapsed to double" + ); + return dx; } @@ -454,7 +461,7 @@ namespace robots { void RobotDynamics::jacobian_spatial( const robots::SpatialModel& model, const mathlib::VecX_T& x, - const RobotSimSnapshot& snap, + const RobotSimSnapshot_T& snap, const mathlib::VecX_T& kp, const mathlib::VecX_T& kd, mathlib::MatX_T& F_out, @@ -484,7 +491,7 @@ namespace robots { const SpatialJoint& joint = model.joints[i]; if (!isControlledJoint(joint.type)) { continue; } dTau_dq(i, i) = -kp[i]; - const Scalar eps_f = 1e-2; + const Scalar eps_f = Scalar(1e-2); const Scalar b = Scalar(0.2); const Scalar c = Scalar(0.05); @@ -506,7 +513,7 @@ namespace robots { mathlib::VecX_T RobotDynamics::derivative_with_gains( Scalar t, const mathlib::VecX_T& x, - const RobotSimSnapshot& snap, + const RobotSimSnapshot_T& snap, const mathlib::VecX_T& kp, const mathlib::VecX_T& kd, DynamicsScratch& scratch, @@ -559,7 +566,7 @@ namespace robots { template void RobotDynamics::jacobian_with_gains( const mathlib::VecX_T& x, - const RobotSimSnapshot& snap, + const RobotSimSnapshot_T& snap, const mathlib::VecX_T& kp, const mathlib::VecX_T& kd, mathlib::MatX_T& F_out, @@ -578,8 +585,8 @@ namespace robots { scratch.jointWorldPoses = _kinematics->calcJointWorldPoses(scratch.T_world, *snap.model); computeMassMatrix(*snap.model, scratch.T_world, scratch.jointWorldPoses, scratch.M); - mathlib::MatX dTau_dq = mathlib::MatX_T::Zero(n, n); - mathlib::MatX dTau_dv = mathlib::MatX_T::Zero(n, n); + mathlib::MatX_T dTau_dq = mathlib::MatX_T::Zero(n, n); + mathlib::MatX_T dTau_dv = mathlib::MatX_T::Zero(n, n); for (size_t i = 0; i < n; ++i) { if (snap.model->joints[i].type == eJointType::FIXED) continue; diff --git a/DSFE_App/DSFE_Core/include/Robots/RobotKinematics.h b/DSFE_App/DSFE_Core/include/Robots/RobotKinematics.h index e5968d5e..c6c56d72 100644 --- a/DSFE_App/DSFE_Core/include/Robots/RobotKinematics.h +++ b/DSFE_App/DSFE_Core/include/Robots/RobotKinematics.h @@ -13,8 +13,6 @@ namespace robots { // Forward declarations - struct RobotConstModel; - struct RobotSimSnapshot; struct RobotLink; struct RobotJoint; struct RobotMetrics; @@ -49,7 +47,7 @@ namespace robots { // Converts roll-pitch-yaw angles (in radians) to a quaternion representation template - mathlib::Quat rpyRadToQuat(const mathlib::Vec3_T& rpyRad); + mathlib::Quat_T rpyRadToQuat(const mathlib::Vec3_T& rpyRad); }; } diff --git a/DSFE_App/DSFE_Core/include/Robots/RobotKinematics.inl b/DSFE_App/DSFE_Core/include/Robots/RobotKinematics.inl index cd1ca6e6..a621df12 100644 --- a/DSFE_App/DSFE_Core/include/Robots/RobotKinematics.inl +++ b/DSFE_App/DSFE_Core/include/Robots/RobotKinematics.inl @@ -92,14 +92,14 @@ namespace robots { // Converts roll-pitch-yaw angles (in radians) to a quaternion representation template - mathlib::Quat RobotKinematics::rpyRadToQuat(const mathlib::Vec3_T& rpyRad) { + mathlib::Quat_T RobotKinematics::rpyRadToQuat(const mathlib::Vec3_T& rpyRad) { const double roll = rpyRad.x(); const double pitch = rpyRad.y(); const double yaw = rpyRad.z(); - const Quat qx(Eigen::AngleAxis(roll, Vec3(1.0, 0.0, 0.0))); - const Quat qy(Eigen::AngleAxis(pitch, Vec3(0.0, 1.0, 0.0))); - const Quat qz(Eigen::AngleAxis(yaw, Vec3(0.0, 0.0, 1.0))); + const Quat_T qx(Eigen::AngleAxis(roll, mathlib::Vec3_T(Scalar(1), Scalar(0), Scalar(0)))); + const Quat_T qy(Eigen::AngleAxis(pitch, mathlib::Vec3_T(Scalar(0), Scalar(1), Scalar(0)))); + const Quat_T qz(Eigen::AngleAxis(yaw, mathlib::Vec3_T(Scalar(0), Scalar(0), Scalar(1)))); return (qz * qy * qx).normalized(); } diff --git a/DSFE_App/DSFE_Core/include/Robots/RobotSimSnapshot.h b/DSFE_App/DSFE_Core/include/Robots/RobotSimSnapshot.h index 75417d43..7d2038ff 100644 --- a/DSFE_App/DSFE_Core/include/Robots/RobotSimSnapshot.h +++ b/DSFE_App/DSFE_Core/include/Robots/RobotSimSnapshot.h @@ -3,6 +3,7 @@ #include "EngineCore.h" #include "Robots/RobotModel.h" +#include namespace robots { // Immutable robot data needed by solver threads @@ -22,24 +23,29 @@ namespace robots { }; // Runtime snapshot for one integration/derivative step - struct DSFE_API RobotSimSnapshot { + template + struct RobotSimSnapshot_T { + const RobotConstModel* model = nullptr; - mathlib::VecX q; // joint angles - mathlib::VecX qd; // joint velocities + mathlib::VecX_T q; // joint angles + mathlib::VecX_T qd; // joint velocities - mathlib::VecX q_ref; // reference joint angles - mathlib::VecX qd_ref; // reference joint velocities - mathlib::VecX qdd_ref; // reference joint accelerations + mathlib::VecX_T q_ref; // reference joint angles + mathlib::VecX_T qd_ref; // reference joint velocities + mathlib::VecX_T qdd_ref; // reference joint accelerations - mathlib::Mat4 robotRootPose = mathlib::Mat4::Identity(); + mathlib::Mat4_T robotRootPose = mathlib::Mat4_T::Identity(); bool baseIsFree = false; - double lastBaseForwardForce = 0.0; - double gravity = 0.0; + + Scalar lastBaseForwardForce = Scalar(0); + Scalar gravity = Scalar(0); eTorqueMode torqueMode = eTorqueMode::CONTROLLED; - double dt = 0.0; - double simTime = 0.0; // simulation time in seconds + + Scalar dt = Scalar(0); + Scalar simTime = Scalar(0); }; + using RobotSimSnapshot = RobotSimSnapshot_T; } // namespace robots \ No newline at end of file diff --git a/DSFE_App/DSFE_Core/include/Robots/SpatialDynamics.inl b/DSFE_App/DSFE_Core/include/Robots/SpatialDynamics.inl index db58c452..9b9308e9 100644 --- a/DSFE_App/DSFE_Core/include/Robots/SpatialDynamics.inl +++ b/DSFE_App/DSFE_Core/include/Robots/SpatialDynamics.inl @@ -42,6 +42,12 @@ namespace robots { else { v_out[i] = Xup_out[i] * v_out[j.parent] + vJ; } c_out[i] = crossMotion(v_out[i], vJ); // Coriolis Term + + using corScalar = typename std::decay_t; + static_assert( + std::is_same_v, + "c_out scalar type does not match model scalar type" + ); } } @@ -57,7 +63,7 @@ namespace robots { const size_t n = model.joints.size(); a_out.resize(n); - SpatialVec_T a0; // base acceleration (gravity) + mathlib::SpatialVec_T a0; // base acceleration (gravity) a0.v << g.template segment<3>(0), g.template segment<3>(3); @@ -141,7 +147,7 @@ namespace robots { ) { const size_t n = model.joints.size(); scratch.dense.M.setZero(n, n); - std::vector> Ic(n); // spatial inertia for each link + std::vector> Ic(n); // spatial inertia for each link // Initialise spatial inertia for each link based on the robot model for (size_t i = 0; i < n; ++i) { Ic[i] = model.joints[i].inertia; } @@ -161,7 +167,7 @@ namespace robots { for (size_t i = 0; i < n; ++i) { const SpatialJoint& j = model.joints[i]; if (j.type == eJointType::FIXED) { continue; } - SpatialVec_T F = Ic[i] * j.S; + mathlib::SpatialVec_T F = Ic[i] * j.S; scratch.dense.M(i, i) = j.S.dot(F); int jIdx = (int)i; @@ -217,7 +223,18 @@ namespace robots { U_out[i] = IA_out[i] * j.S; d_out[i] = dot(j.S, U_out[i]); - if (std::abs(d_out[i]) < 1e-12) { d_out[i] = 1e-12; } // Regularisation to avoid singularities + if (d_out[i] < Scalar(1e-12)) { + d_out[i] = Scalar(1e-12); + } + + using DScalar = typename std::decay_t< + decltype(d_out[i]) + >; + + static_assert( + !std::is_same_v, + "d_out collapsed" + ); u_out[i] = tau[i] - dot(j.S, pA_out[i]); Ia_out[i] = IA_out[i] - outer(U_out[i]) / d_out[i]; @@ -274,9 +291,9 @@ namespace robots { DynamicsScratch& scratch ) { const size_t n = model.joints.size(); - VecX_T qdd = VecX_T::Zero(n); + mathlib::VecX_T qdd = mathlib::VecX_T::Zero(n); - SpatialVec_T a0; // base acceleration (gravity) + mathlib::SpatialVec_T a0; // base acceleration (gravity) a0.v << scratch.g.template segment<3>(0), scratch.g.template segment<3>(3); diff --git a/DSFE_App/DSFE_Core/src/Robots/RobotSystem.cpp b/DSFE_App/DSFE_Core/src/Robots/RobotSystem.cpp index 099697af..57244fda 100644 --- a/DSFE_App/DSFE_Core/src/Robots/RobotSystem.cpp +++ b/DSFE_App/DSFE_Core/src/Robots/RobotSystem.cpp @@ -259,19 +259,21 @@ namespace robots { RobotSimSnapshot RobotSystem::takeSnapshot(double simTime) const { RobotSimSnapshot snap; snap.model = &_constModel; - const size_t n = (size_t)_robot.joints.size(); snap.q.resize(n); snap.qd.resize(n); + snap.q_ref.resize(n); snap.qd_ref.resize(n); snap.qdd_ref.resize(n); for (size_t i = 0; i < n; ++i) { const auto& j = _robot.joints[i]; + snap.q[i] = j.q; snap.qd[i] = j.qd; + snap.q_ref[i] = j.q_ref; snap.qd_ref[i] = j.qd_ref; snap.qdd_ref[i] = j.qdd_ref; @@ -283,6 +285,7 @@ namespace robots { snap.gravity = _gravity; snap.torqueMode = _robot.torqueMode; + snap.dt = _dynamics->dt(); snap.simTime = simTime; @@ -344,11 +347,17 @@ namespace robots { LOG_INFO_ONCE("tau_rnea size = %lld", (long long)tau_rnea.size()); // Define the derivative function for integration, capturing necessary variables by reference - auto f_deriv = [&, kp_frozen, kd_frozen](double t, const mathlib::VecX_T& xIn) { - return _dynamics->derivative_spatial( + auto f_deriv = [&, kp_frozen, kd_frozen](auto t, const auto& xIn) { + using Scalar = std::decay_t; + + DynamicsScratch scratch; + DynamicsResult result; + + return _dynamics->template derivative_spatial( _spatialModel, - t, xIn, snap, - _dynScratch, _dynResult + t, xIn, + snap, + scratch, result ); }; // Define the Jacobian function for integration, capturing necessary variables by reference diff --git a/PxMLib/MathLib/include/core/Types_tpl.h b/PxMLib/MathLib/include/core/Types_tpl.h index 0633d27c..0682044e 100644 --- a/PxMLib/MathLib/include/core/Types_tpl.h +++ b/PxMLib/MathLib/include/core/Types_tpl.h @@ -13,6 +13,10 @@ namespace mathlib { template using Mat3_T = Eigen::Matrix; + // Template version of 4x4 matrix for homogeneous transformations + template + using Mat4_T = Eigen::Matrix; + // Template version of 6D vector for spatial algebra template using Vec6_T = Eigen::Matrix; @@ -32,4 +36,8 @@ namespace mathlib { // Template version of pose (4x4 homogeneous transformation matrix) template using Pose_T = Eigen::Matrix; + + // Template version of quaternion + template + using Quat_T = Eigen::Quaternion; } \ No newline at end of file diff --git a/PxMLib/MathLib/include/integrators/numerical_integrators.h b/PxMLib/MathLib/include/integrators/numerical_integrators.h index 16bd5bbe..044eaf56 100644 --- a/PxMLib/MathLib/include/integrators/numerical_integrators.h +++ b/PxMLib/MathLib/include/integrators/numerical_integrators.h @@ -3,8 +3,9 @@ // GitHub: SaltyJoss #include "MathLibAPI.h" -#include "core/Types.h" -#include "core/Types_tpl.h" +#include +#include +#include #include #include #include @@ -143,6 +144,7 @@ namespace integration { template mathlib::MatX_T automaticDifferenceJacobian( Func&& f, + Scalar t, const mathlib::VecX_T& x ); }; diff --git a/PxMLib/MathLib/include/integrators/numerical_integrators.inl b/PxMLib/MathLib/include/integrators/numerical_integrators.inl index a44cc2d3..d04392ff 100644 --- a/PxMLib/MathLib/include/integrators/numerical_integrators.inl +++ b/PxMLib/MathLib/include/integrators/numerical_integrators.inl @@ -223,9 +223,10 @@ namespace integration { if (USE_AD_JACOBIANS == true) { printf("Using AD Jacobian for Implicit Euler\n"); F = automaticDifferenceJacobian( - [&](Scalar, const mathlib::VecX_T& x_pert) { - return f(t + c(0) * dt, x_pert); - }, + [&](auto, const auto& x_pert) { + return f(t + dt, x_pert); + }, + t + dt, x_guess ); } @@ -233,9 +234,9 @@ namespace integration { printf("Using finite difference Jacobian for Implicit Euler\n"); F = finiteDifferenceJacobian( [&](Scalar, const mathlib::VecX_T& x_pert) { - return f((t + dt) / Scalar(2), (x + x_pert) / Scalar(2)); - }, - t + dt / Scalar(2), + return f(t + dt, x + x_pert); + }, + t + dt, x_guess ); } @@ -284,9 +285,12 @@ namespace integration { if (USE_AD_JACOBIANS == true) { printf("Using AD Jacobian for Implicit Midpoint\n"); F = automaticDifferenceJacobian( - [&](Scalar, const mathlib::VecX_T& x_pert) { - return f(t + c(0) * dt, x_pert); + [&](auto, const auto& x_pert) { + using Dual = typename std::remove_reference_t; + auto x_dual = x.template cast(); + return f((t + dt) / Scalar(2), (x_dual + x_pert) / Scalar(2)); }, + t + dt / Scalar(2), x_guess ); } @@ -384,15 +388,17 @@ namespace integration { if (USE_AD_JACOBIANS == true) { printf("Using AD Jacobian for GLRK2\n"); F1 = automaticDifferenceJacobian( - [&](Scalar, const mathlib::VecX_T& x_pert) { - return f(t + c(0) * dt, x_pert); - }, + [&](auto, const auto& x_pert) { + return f(t + c(0) * dt, x_pert); + }, + t + c(0) * dt, x1 ); F2 = automaticDifferenceJacobian( - [&](Scalar, const mathlib::VecX_T& x_pert) { - return f(t + c(0) * dt, x_pert); - }, + [&](auto, const auto& x_pert) { + return f(t + c(1) * dt, x_pert); + }, + t + c(1) * dt, x2 ); } @@ -447,7 +453,9 @@ namespace integration { int maxIter, Scalar tol ) { - if (tol < 0.0) { tol = std::max(1e-12, 1e-2 * std::pow(dt, 7.0)); } + if constexpr (std::is_same_v) { + if (tol < 0.0) { tol = std::max(1e-12, 1e-2 * std::pow(dt, 7.0)); } + } const Eigen::Index n = x.size(); // Coefficients for the 3-stage Gauss-Legendre method (6th order) @@ -528,21 +536,24 @@ namespace integration { if (USE_AD_JACOBIANS == true) { printf("Using AD Jacobian for GLRK3\n"); F1 = automaticDifferenceJacobian( - [&](Scalar, const mathlib::VecX_T& x_pert) { + [&](auto, const auto& x_pert) { return f(t + c(0) * dt, x_pert); }, + t + c(0) * dt, x1 ); F2 = automaticDifferenceJacobian( - [&](Scalar, const mathlib::VecX_T& x_pert) { - return f(t + c(0) * dt, x_pert); - }, + [&](auto, const auto& x_pert) { + return f(t + c(1) * dt, x_pert); + }, + t + c(1) * dt, x2 ); F3 = automaticDifferenceJacobian( - [&](Scalar, const mathlib::VecX_T& x_pert) { - return f(t + c(0) * dt, x_pert); - }, + [&](auto, const auto& x_pert) { + return f(t + c(2) * dt, x_pert); + }, + t + c(2) * dt, x3 ); } @@ -701,16 +712,26 @@ namespace integration { template mathlib::MatX_T NumericalIntegrator::automaticDifferenceJacobian( Func&& f, + Scalar t, const mathlib::VecX_T& x ) { const int n = static_cast(x.size()); mathlib::MatX_T J(n, n); for (int i = 0; i < n; ++i) { - mathlib::VecX_T> x_dual(n); + mathlib::VecX_T> x_dual(n); for (int k = 0; k < n; ++k) { - x_dual(k) = DualNumber_T(x(k), (k == i) ? Scalar(1) : Scalar(0)); + x_dual(k) = mathlib::DualNumber_T(x(k), std::array{ (k == i) ? Scalar(1) : Scalar(0) }); } - auto f_dual = f(x_dual); + mathlib::DualNumber_T t_dual(t); + auto f_dual = f(t_dual, x_dual); + + using FDualScalar = std::remove_reference_t; + + static_assert( + !std::is_same_v, + "f_dual collapsed to double" + ); + for (int j = 0; j < n; ++j) { J(j, i) = f_dual(j).dual[0]; } From 641e4f9936f802e7ed08d580f932e389cd6ba0a7 Mon Sep 17 00:00:00 2001 From: saltyjoss Date: Mon, 18 May 2026 17:10:38 +0100 Subject: [PATCH 033/210] fixes(INCOMPLETE): Added overloads for implicit methods, and corrected value assignement --- PxMLib/MathLib/include/core/DualNumbers.h | 257 ++++++++++++++++-- .../integrators/numerical_integrators.h | 88 +++++- .../integrators/numerical_integrators.inl | 114 +++++--- 3 files changed, 381 insertions(+), 78 deletions(-) diff --git a/PxMLib/MathLib/include/core/DualNumbers.h b/PxMLib/MathLib/include/core/DualNumbers.h index aaaa25e6..abbf3bbc 100644 --- a/PxMLib/MathLib/include/core/DualNumbers.h +++ b/PxMLib/MathLib/include/core/DualNumbers.h @@ -3,6 +3,8 @@ #include "MathLibAPI.h" #include "core/Types_tpl.h" +#include +#include #include #include @@ -11,6 +13,8 @@ namespace mathlib { template class DualNumber_T { public: + using ScalarT = Scalar; + DualNumber_T( Scalar real = Scalar(0), const std::array& dual = std::array() @@ -41,7 +45,7 @@ namespace mathlib { template inline DualNumber_T operator-( const DualNumber_T& a - ) { + ) { DualNumber_T out; out.real = -a.real; for (size_t i = 0; i < NVar; ++i) { @@ -54,7 +58,7 @@ namespace mathlib { template inline DualNumber_T operator+( const DualNumber_T& a - ) { + ) { return a; } @@ -67,7 +71,7 @@ namespace mathlib { inline bool operator==( const DualNumber_T& a, const DualNumber_T& b - ) { + ) { return a.real == b.real; } @@ -76,7 +80,7 @@ namespace mathlib { inline bool operator!=( const DualNumber_T& a, const DualNumber_T& b - ) { + ) { return !(a == b); } @@ -85,7 +89,7 @@ namespace mathlib { inline bool operator<( const DualNumber_T& a, const DualNumber_T& b - ) { + ) { return a.real < b.real; } @@ -94,7 +98,7 @@ namespace mathlib { inline bool operator<=( const DualNumber_T& a, const DualNumber_T& b - ) { + ) { return a.real <= b.real; } @@ -112,10 +116,10 @@ namespace mathlib { inline bool operator>=( const DualNumber_T& a, const DualNumber_T& b - ) { + ) { return a.real >= b.real; } - + // ----- // Scalar Interactions // ----- @@ -125,7 +129,7 @@ namespace mathlib { inline DualNumber_T operator+( const DualNumber_T& a, Scalar b - ) { + ) { DualNumber_T out = a; out.real += b; return out; @@ -136,7 +140,7 @@ namespace mathlib { inline DualNumber_T operator+( Scalar a, const DualNumber_T& b - ) { + ) { return b + a; // Reuse dual + scalar } @@ -145,7 +149,7 @@ namespace mathlib { inline DualNumber_T operator-( const DualNumber_T& a, Scalar b - ) { + ) { DualNumber_T out = a; out.real -= b; return out; @@ -156,7 +160,7 @@ namespace mathlib { inline DualNumber_T operator-( Scalar a, const DualNumber_T& b - ) { + ) { DualNumber_T out; out.real = a - b.real; for (size_t i = 0; i < NVar; ++i) { @@ -170,7 +174,7 @@ namespace mathlib { inline DualNumber_T operator*( const DualNumber_T& a, Scalar b - ) { + ) { DualNumber_T out; out.real = a.real * b; for (size_t i = 0; i < NVar; ++i) { @@ -184,7 +188,7 @@ namespace mathlib { inline DualNumber_T operator*( Scalar a, const DualNumber_T& b - ) { + ) { return b * a; // Reuse dual * scalar } @@ -193,7 +197,7 @@ namespace mathlib { inline DualNumber_T operator/( const DualNumber_T& a, Scalar b - ) { + ) { DualNumber_T out; out.real = a.real / b ? a.real / b : throw std::runtime_error("Division by zero in dual number scalar division"); for (size_t i = 0; i < NVar; ++i) { @@ -207,7 +211,7 @@ namespace mathlib { inline DualNumber_T operator/( Scalar a, const DualNumber_T& b - ) { + ) { DualNumber_T out; out.real = a / b.real; for (size_t i = 0; i < NVar; ++i) { @@ -225,7 +229,7 @@ namespace mathlib { inline DualNumber_T operator+( const DualNumber_T& a, const DualNumber_T& b - ) { + ) { DualNumber_T out; out.real = a.real + b.real; for (size_t i = 0; i < NVar; ++i) { @@ -239,7 +243,7 @@ namespace mathlib { inline DualNumber_T operator-( const DualNumber_T& a, const DualNumber_T& b - ) { + ) { DualNumber_T out; out.real = a.real - b.real; for (size_t i = 0; i < NVar; ++i) { @@ -253,7 +257,7 @@ namespace mathlib { inline DualNumber_T operator*( const DualNumber_T& a, const DualNumber_T& b - ) { + ) { DualNumber_T out; out.real = a.real * b.real; for (size_t i = 0; i < NVar; ++i) { @@ -267,7 +271,7 @@ namespace mathlib { inline DualNumber_T operator/( const DualNumber_T& a, const DualNumber_T& b - ) { + ) { DualNumber_T out; out.real = a.real / b.real; for (size_t i = 0; i < NVar; ++i) { @@ -423,7 +427,7 @@ namespace mathlib { inline DualNumber_T& operator+=( DualNumber_T& a, const DualNumber_T& b - ) { + ) { a.real += b.real; for (size_t i = 0; i < NVar; ++i) { a.dual[i] += b.dual[i]; @@ -435,7 +439,7 @@ namespace mathlib { inline DualNumber_T& operator+=( DualNumber_T& a, Scalar b - ) { + ) { a.real += b; return a; } @@ -445,7 +449,7 @@ namespace mathlib { inline DualNumber_T& operator-=( DualNumber_T& a, const DualNumber_T& b - ) { + ) { a.real -= b.real; for (size_t i = 0; i < NVar; ++i) { a.dual[i] -= b.dual[i]; @@ -457,7 +461,7 @@ namespace mathlib { inline DualNumber_T& operator-=( DualNumber_T& a, Scalar b - ) { + ) { a.real -= b; return a; } @@ -467,7 +471,7 @@ namespace mathlib { inline DualNumber_T& operator*=( DualNumber_T& a, Scalar b - ) { + ) { a.real *= b; for (size_t i = 0; i < NVar; ++i) { a.dual[i] *= b; @@ -480,7 +484,7 @@ namespace mathlib { inline DualNumber_T& operator*=( DualNumber_T& a, const DualNumber_T& b - ) { + ) { Scalar ogReal = a.real; for (size_t i = 0; i < NVar; ++i) { a.dual[i] = ogReal * b.dual[i] + a.dual[i] * b.real; @@ -494,7 +498,7 @@ namespace mathlib { inline DualNumber_T& operator/=( DualNumber_T& a, Scalar b - ) { + ) { if (b == Scalar(0)) { throw std::runtime_error( "Division by zero in DualNumber_T operator/=" @@ -512,7 +516,7 @@ namespace mathlib { inline DualNumber_T& operator/=( DualNumber_T& a, const DualNumber_T& b - ) { + ) { Scalar ogReal = a.real; for (size_t i = 0; i < NVar; ++i) { a.dual[i] = (a.dual[i] * b.real - ogReal * b.dual[i]) / (b.real * b.real); @@ -530,5 +534,200 @@ namespace mathlib { // Numeric Limits Specialisation for DualNumber_T // ----- - // TODO: Add specialisation of std::numeric_limits for DualNumber_T to define properties like infinity, NaN, epsilon, etc. based on the underlying Scalar type. + // Function to return a DualNumber_T representing infinity (real part is infinity, dual parts are zero) + template + inline DualNumber_T numeric_limits_infinity() { + return DualNumber_T( + std::numeric_limits::infinity(), + std::array() + ); + } + + // Alternative absolute value function that simply negates the dual number if the real part is negative (this is less smooth but can be more efficient and avoids issues with zero) + template + inline DualNumber_T abs( + const DualNumber_T& a + ) { + DualNumber_T out; + out = (a.real < Scalar(0)) ? -a : a; + return out; + } + // Absolute value function with epsilon threshold to avoid non-differentiability at zero (scales dual part by the sign of the real part, but treats values within epsilon of zero as zero) + template + inline DualNumber_T abs( + const DualNumber_T& a, + Scalar epsilon + ) { + DualNumber_T out; + if (std::abs(a.real) < epsilon) { + out.real = Scalar(0); + for (size_t i = 0; i < NVar; ++i) { + out.dual[i] = Scalar(0); + } + } + else { + out.real = std::abs(a.real); + Scalar sign = (a.real > Scalar(0)) - (a.real < Scalar(0)); // Sign of the real part + for (size_t i = 0; i < NVar; ++i) { + out.dual[i] = sign * a.dual[i]; + } + } + return out; + } + + + // Sign function for DualNumber_T (returns the sign of the real part, dual parts are zero) + template + inline DualNumber_T sign( + const DualNumber_T& a + ) { + DualNumber_T out; + out.real = (a.real > Scalar(0)) - (a.real < Scalar(0)); // Sign of the real part + for (size_t i = 0; i < NVar; ++i) { + out.dual[i] = Scalar(0); + } + return out; + } + + // Minimum function for DualNumber_T (returns the minimum of the real parts, scales dual part by the indicator of which real part is smaller) + template + inline DualNumber_T min( + const DualNumber_T& a, + const DualNumber_T& b + ) { + DualNumber_T out; + if (a.real < b.real) { + out.real = a.real; + for (size_t i = 0; i < NVar; ++i) { + out.dual[i] = a.dual[i]; + } + } + else { + out.real = b.real; + for (size_t i = 0; i < NVar; ++i) { + out.dual[i] = b.dual[i]; + } + } + return out; + } + + // Maximum function for DualNumber_T (returns the maximum of the real parts, scales dual part by the indicator of which real part is larger) + template + inline DualNumber_T max( + const DualNumber_T& a, + const DualNumber_T& b + ) { + DualNumber_T out; + if (a.real > b.real) { + out.real = a.real; + for (size_t i = 0; i < NVar; ++i) { + out.dual[i] = a.dual[i]; + } + } + else { + out.real = b.real; + for (size_t i = 0; i < NVar; ++i) { + out.dual[i] = b.dual[i]; + } + } + return out; + } + + // Since dual numbers are not complex, the real part is just the real part of the dual number (for non-dual types, this is just the value itself) + template + inline T real(const T& x) { + return x; + } + + // Since dual numbers are not complex, the real part is just the real part of the dual number + template + inline Scalar real( + const DualNumber_T& a + ) { + return a.real; + } + + // Since dual numbers are not complex, the imaginary part is always zero + template + inline Scalar imaginary( + const DualNumber_T& /*a*/ + ) { + return Scalar(0); + } + + // Since dual numbers are not complex, the complex conjugate is just the dual number itself + template + inline DualNumber_T conj( + const DualNumber_T& a + ) { + return a; + } + + // Since dual numbers are not complex, the norm is just the absolute value of the real part + template + inline Scalar norm( + const DualNumber_T& a + ) { + return std::abs(a.real); + } + + // Since dual numbers are not complex, the squared norm is just the square of the absolute value of the real part + template + inline Scalar abs2( + const DualNumber_T& a + ) { + return a.real * a.real; + } + + template + inline bool is_dual_number_v( + const DualNumber_T& /*a*/ + ) { + return true; + } + + // Traits class to identify dual numbers and extract the base scalar type + template + struct DualTraits { + using BaseScalar = T; + static constexpr bool is_dual = false; + }; + + // Specialization of DualTraits for DualNumber_T + template + struct DualTraits> { + using BaseScalar = Scalar; + static constexpr bool is_dual = true; + }; +} + +namespace Eigen { + // Specialization of NumTraits for DualNumber_T to allow it to be used with Eigen's matrix and vector types + template + struct NumTraits> : GenericNumTraits> { + typedef mathlib::DualNumber_T Real; + typedef mathlib::DualNumber_T NonInteger; + typedef mathlib::DualNumber_T Nested; + enum { + IsComplex = 0, + IsInteger = 0, + IsSigned = 1, + RequireInitialization = 1, + ReadCost = 1, + AddCost = 3, + MulCost = 3 + }; + static EIGEN_DEVICE_FUNC Real epsilon() { + return Real(std::numeric_limits::epsilon()); + } + static EIGEN_DEVICE_FUNC Real dummy_precision() { + return Real(Scalar(1e-12)); + } + static EIGEN_DEVICE_FUNC Real highest() { + return Real(std::numeric_limits::max()); + } + static EIGEN_DEVICE_FUNC Real lowest() { + return Real(std::numeric_limits::lowest()); + } + }; } \ No newline at end of file diff --git a/PxMLib/MathLib/include/integrators/numerical_integrators.h b/PxMLib/MathLib/include/integrators/numerical_integrators.h index 044eaf56..777717b9 100644 --- a/PxMLib/MathLib/include/integrators/numerical_integrators.h +++ b/PxMLib/MathLib/include/integrators/numerical_integrators.h @@ -82,8 +82,28 @@ namespace integration { Func&& f, JacFunc&& jac = nullptr, int maxIter = 8, - Scalar tol = 1e-6 + Scalar tol = Scalar(1e-6) ); + // Overload without Jacobian + template + mathlib::VecX_T ImplicitEuler( + const mathlib::VecX_T& x, + Scalar t, + Scalar dt, + Func&& f, + int maxIter, + Scalar tol + ) { + return ImplicitEuler( + x, + t, + dt, + std::forward(f), + nullptr, + maxIter, + tol + ); + } // Implicit Midpoint method template @@ -94,8 +114,28 @@ namespace integration { Func&& f, JacFunc&& jac = nullptr, int maxIter = 10, - Scalar tol = 1e-7 + Scalar tol = Scalar(1e-7) ); + // Overload without Jacobian + template + mathlib::VecX_T ImplicitMidpoint( + const mathlib::VecX_T& x, + Scalar t, + Scalar dt, + Func&& f, + int maxIter, + Scalar tol + ) { + return ImplicitMidpoint( + x, + t, + dt, + std::forward(f), + nullptr, + maxIter, + tol + ); + } // Gauss-Legendre Runge-Kutta method (2 stages, 4th order) template @@ -106,8 +146,28 @@ namespace integration { Func&& f, JacFunc&& jac = nullptr, int maxIter = 50, - Scalar tol = 1e-6 + Scalar tol = Scalar(1e-6) ); + // Overload without Jacobian + template + mathlib::VecX_T GLRK2( + const mathlib::VecX_T& x, + Scalar t, + Scalar dt, + Func&& f, + int maxIter, + Scalar tol + ) { + return GLRK2( + x, + t, + dt, + std::forward(f), + nullptr, + maxIter, + tol + ); + } // Gauss-Legendre Runge-Kutta method (3 stages, 6th order) template @@ -118,8 +178,28 @@ namespace integration { Func&& f, JacFunc&& jac = nullptr, int maxIter = 150, - Scalar tol = 1e-14 + Scalar tol = Scalar(1e-14) ); + // Overload without Jacobian + template + mathlib::VecX_T GLRK3( + const mathlib::VecX_T& x, + Scalar t, + Scalar dt, + Func&& f, + int maxIter, + Scalar tol + ) { + return GLRK3( + x, + t, + dt, + std::forward(f), + nullptr, + maxIter, + tol + ); + } private: diff --git a/PxMLib/MathLib/include/integrators/numerical_integrators.inl b/PxMLib/MathLib/include/integrators/numerical_integrators.inl index d04392ff..f172bbb2 100644 --- a/PxMLib/MathLib/include/integrators/numerical_integrators.inl +++ b/PxMLib/MathLib/include/integrators/numerical_integrators.inl @@ -196,8 +196,6 @@ namespace integration { int maxIter, Scalar tol ) { - mathlib::VecX_T x_new = x + dt * f(t, x); // Initial guess - // The function g(x_guess) = 0 that we want to solve for the implicit Euler step auto g = [&](const mathlib::VecX_T& x_guess, mathlib::VecX_T& g_out) { g_out = x_guess - x - dt * f(t + dt, x_guess); }; // Numerical Jacobian for Newton-Raphson @@ -223,8 +221,10 @@ namespace integration { if (USE_AD_JACOBIANS == true) { printf("Using AD Jacobian for Implicit Euler\n"); F = automaticDifferenceJacobian( - [&](auto, const auto& x_pert) { - return f(t + dt, x_pert); + [&](auto t_pert, const auto& x_pert) { + using Dual = std::decay_t; + Dual t_eval = Dual(t_pert) + Dual(dt); + return f(t_eval, x_pert); }, t + dt, x_guess @@ -234,7 +234,7 @@ namespace integration { printf("Using finite difference Jacobian for Implicit Euler\n"); F = finiteDifferenceJacobian( [&](Scalar, const mathlib::VecX_T& x_pert) { - return f(t + dt, x + x_pert); + return f(t + dt, x_pert); }, t + dt, x_guess @@ -285,10 +285,11 @@ namespace integration { if (USE_AD_JACOBIANS == true) { printf("Using AD Jacobian for Implicit Midpoint\n"); F = automaticDifferenceJacobian( - [&](auto, const auto& x_pert) { - using Dual = typename std::remove_reference_t; + [&](auto t_pert, const auto& x_pert) { + using Dual = std::decay_t; auto x_dual = x.template cast(); - return f((t + dt) / Scalar(2), (x_dual + x_pert) / Scalar(2)); + Dual t_mid = Dual(t_pert + dt) / Dual(2); + return f(t_mid , (x_dual + x_pert) / Dual(2)); }, t + dt / Scalar(2), x_guess @@ -323,20 +324,22 @@ namespace integration { int maxIter, Scalar tol ) { + using std::sqrt; + const Eigen::Index n = x.size(); // Coefficients for the 2-stage Gauss-Legendre method (4th order) mathlib::VecX_T c(2); c << - 0.5 - std::sqrt(3.0) / 6.0, - 0.5 + std::sqrt(3.0) / 6.0; // Stage time fractions + Scalar(0.5) - sqrt(Scalar(3)) / Scalar(6), + Scalar(0.5) + sqrt(Scalar(3)) / Scalar(6); // Stage time fractions mathlib::MatX_T A(2, 2); A << - 0.25, 0.25 - std::sqrt(3.0) / 6.0, - 0.25 + std::sqrt(3.0) / 6.0, 0.25; + Scalar(0.25), Scalar(0.25) - sqrt(Scalar(3)) / Scalar(6), + Scalar(0.25) + sqrt(Scalar(3)) / Scalar(6), Scalar(0.25); - const Scalar b = 0.5; // Weights for final update + const Scalar b = Scalar(0.5); // Weights for final update // Initial guess for the stage values k1, k2, k3 mathlib::VecX_T k(2 * n); // 2 stages @@ -388,15 +391,19 @@ namespace integration { if (USE_AD_JACOBIANS == true) { printf("Using AD Jacobian for GLRK2\n"); F1 = automaticDifferenceJacobian( - [&](auto, const auto& x_pert) { - return f(t + c(0) * dt, x_pert); + [&](auto t_pert, const auto& x_pert) { + using Dual = std::decay_t; + Dual t_eval = Dual(t_pert) + Dual(c(0)) * Dual(dt); + return f(t_eval, x_pert); }, t + c(0) * dt, x1 ); F2 = automaticDifferenceJacobian( - [&](auto, const auto& x_pert) { - return f(t + c(1) * dt, x_pert); + [&](auto t_pert, const auto& x_pert) { + using Dual = std::decay_t; + Dual t_eval = Dual(t_pert) + Dual(c(1)) * Dual(dt); + return f(t_eval, x_pert); }, t + c(1) * dt, x2 @@ -453,23 +460,31 @@ namespace integration { int maxIter, Scalar tol ) { - if constexpr (std::is_same_v) { - if (tol < 0.0) { tol = std::max(1e-12, 1e-2 * std::pow(dt, 7.0)); } + using Real = typename mathlib::DualTraits::BaseScalar; + using std::sqrt; + using std::pow; + using std::abs; + using std::max; + + if (mathlib::real(tol) < Real(0)) { + Real dt_r = mathlib::real(dt); + Real tol_r = max(Real(1e-12), Real(1e-2) * pow(dt_r, Real(7))); + tol = Scalar(tol_r); } const Eigen::Index n = x.size(); // Coefficients for the 3-stage Gauss-Legendre method (6th order) mathlib::VecX_T c(3); c << - 0.5 - std::sqrt(15.0) / 10.0, - 0.5, - 0.5 + std::sqrt(15.0) / 10.0; // Stage time fractions + Scalar(0.5) - sqrt(Scalar(15)) / Scalar(10), + Scalar(0.5), + Scalar(0.5) + sqrt(Scalar(15)) / Scalar(10); // Stage time fractions mathlib::Mat3_T A(3, 3); A << - 5.0 / 36.0, 2.0 / 9.0 - std::sqrt(15.0) / 15.0, 5.0 / 36.0 - std::sqrt(15.0) / 30.0, - 5.0 / 36.0 + std::sqrt(15.0) / 24.0, 2.0 / 9.0, 5.0 / 36.0 - std::sqrt(15.0) / 24.0, - 5.0 / 36.0 + std::sqrt(15.0) / 30.0, 2.0 / 9.0 + std::sqrt(15.0) / 15.0, 5.0 / 36.0; + Scalar(5) / Scalar(36), Scalar(2) / Scalar(9) - sqrt(Scalar(15)) / Scalar(15), Scalar(5) / Scalar(36) - sqrt(Scalar(15)) / Scalar(30), + Scalar(5) / Scalar(36) + sqrt(Scalar(15)) / Scalar(24), Scalar(2) / Scalar(9), Scalar(5) / Scalar(36) - sqrt(Scalar(15)) / Scalar(24), + Scalar(5) / Scalar(36) + sqrt(Scalar(15)) / Scalar(30), Scalar(2) / Scalar(9) - sqrt(Scalar(15)) / Scalar(15), Scalar(5) / Scalar(36); mathlib::VecX_T b(3); b << 5.0 / 18.0, 4.0 / 9.0, 5.0 / 18.0; // Weights for final update @@ -536,22 +551,28 @@ namespace integration { if (USE_AD_JACOBIANS == true) { printf("Using AD Jacobian for GLRK3\n"); F1 = automaticDifferenceJacobian( - [&](auto, const auto& x_pert) { - return f(t + c(0) * dt, x_pert); + [&](auto t_pert, const auto& x_pert) { + using Dual = std::decay_t; + Dual t_eval = Dual(t_pert) + Dual(c(0)) * Dual(dt); + return f(t_eval, x_pert); }, t + c(0) * dt, x1 ); F2 = automaticDifferenceJacobian( - [&](auto, const auto& x_pert) { - return f(t + c(1) * dt, x_pert); + [&](auto t_pert, const auto& x_pert) { + using Dual = std::decay_t; + Dual t_eval = Dual(t_pert) + Dual(c(1)) * Dual(dt); + return f(t_eval, x_pert); }, t + c(1) * dt, x2 ); F3 = automaticDifferenceJacobian( - [&](auto, const auto& x_pert) { - return f(t + c(2) * dt, x_pert); + [&](auto t_pert, const auto& x_pert) { + using Dual = std::decay_t; + Dual t_eval = Dual(t_pert) + Dual(c(2)) * Dual(dt); + return f(t_eval, x_pert); }, t + c(2) * dt, x3 @@ -603,7 +624,7 @@ namespace integration { mathlib::VecX_T g_check; eval_g(k, g_check); - Scalar residual = g_check.norm(); + auto residual = mathlib::real(g_check.norm()); //if (residual > 1e-8) { std::cout << "[GLRK3] Large final residual: " << residual << std::endl; } // Compute the final update for x using the stage values @@ -663,7 +684,7 @@ namespace integration { } x += delta; - if (delta.norm() < tol * (1.0 + x.norm())) { return x; } + if (delta.norm() < tol * (Scalar(1) + x.norm())) { return x; } if (!x.allFinite()) { throw std::runtime_error( @@ -690,7 +711,7 @@ namespace integration { Scalar t, const mathlib::VecX_T& x ) { - const Scalar eps_rel = 1e-8; + const Scalar eps_rel = Scalar(1e-8); const int n = static_cast(x.size()); mathlib::VecX_T f_0 = f(t, x); mathlib::MatX_T J(f_0.size(), n); @@ -703,7 +724,7 @@ namespace integration { x_bwd(i) -= h; mathlib::VecX_T f_fwd = f(t, x_fwd); mathlib::VecX_T f_bwd = f(t, x_bwd); - J.col(i) = (f_fwd - f_bwd) / (2.0 * h); + J.col(i) = (f_fwd - f_bwd) / (Scalar(2) * h); } return J; } @@ -718,19 +739,22 @@ namespace integration { const int n = static_cast(x.size()); mathlib::MatX_T J(n, n); for (int i = 0; i < n; ++i) { - mathlib::VecX_T> x_dual(n); + using BaseScalar = typename mathlib::DualTraits::BaseScalar; + using Dual_T = mathlib::DualNumber_T; + + mathlib::VecX_T x_dual(n); for (int k = 0; k < n; ++k) { - x_dual(k) = mathlib::DualNumber_T(x(k), std::array{ (k == i) ? Scalar(1) : Scalar(0) }); + x_dual(k) = Dual_T( + x(k), + std::array{ + (k == i) ? Scalar(1) : Scalar(0) + } + ); } - mathlib::DualNumber_T t_dual(t); - auto f_dual = f(t_dual, x_dual); - - using FDualScalar = std::remove_reference_t; - static_assert( - !std::is_same_v, - "f_dual collapsed to double" - ); + Dual_T t_dual(t); + using ReturnT = decltype(f(t_dual, x_dual)); + ReturnT f_dual = f(t_dual, x_dual); for (int j = 0; j < n; ++j) { J(j, i) = f_dual(j).dual[0]; From 6e017210c99eb05d9b574a3bca6dfa21169c3ad1 Mon Sep 17 00:00:00 2001 From: saltyjoss Date: Mon, 18 May 2026 17:10:38 +0100 Subject: [PATCH 034/210] fixes(INCOMPLETE): Added overloads for implicit methods, and corrected value assignement --- .../DSFE_Core/include/Robots/RobotDynamics.h | 1 + .../include/Robots/RobotDynamics.inl | 52 ++-- .../include/Robots/RobotSimSnapshot.h | 30 ++ .../include/Robots/SpatialDynamics.inl | 9 - DSFE_App/DSFE_Core/src/Robots/RobotSystem.cpp | 27 +- PxMLib/MathLib/include/core/DualNumbers.h | 257 ++++++++++++++++-- .../integrators/numerical_integrators.h | 88 +++++- .../integrators/numerical_integrators.inl | 116 ++++---- 8 files changed, 459 insertions(+), 121 deletions(-) diff --git a/DSFE_App/DSFE_Core/include/Robots/RobotDynamics.h b/DSFE_App/DSFE_Core/include/Robots/RobotDynamics.h index 8644750c..90235787 100644 --- a/DSFE_App/DSFE_Core/include/Robots/RobotDynamics.h +++ b/DSFE_App/DSFE_Core/include/Robots/RobotDynamics.h @@ -4,6 +4,7 @@ #include "EngineCore.h" #include "MathLibAPI.h" #include "core/Types.h" +#include #include "Robots/DynamicsTypes.h" #include "Robots/RobotMetrics.h" diff --git a/DSFE_App/DSFE_Core/include/Robots/RobotDynamics.inl b/DSFE_App/DSFE_Core/include/Robots/RobotDynamics.inl index 8b0b2cad..f2564c63 100644 --- a/DSFE_App/DSFE_Core/include/Robots/RobotDynamics.inl +++ b/DSFE_App/DSFE_Core/include/Robots/RobotDynamics.inl @@ -344,17 +344,17 @@ namespace robots { tau_i -= b * qd[i]; // subtract viscous damping tau_i -= c * std::tanh(qd[i] / eps_f); // subtract Coulomb friction - scratch.dense.tau[i] = tau_i; - // metrics - out.metrics.q[i] = q[i]; - out.metrics.qd[i] = qd[i]; + scratch.dense.tau[i] = mathlib::real(tau_i); + + out.metrics.q[i] = mathlib::real(q[i]); + out.metrics.qd[i] = mathlib::real(qd[i]); - out.metrics.err[i] = err; - out.metrics.errd[i] = err_d; + out.metrics.err[i] = mathlib::real(err); + out.metrics.errd[i] = mathlib::real(err_d); - out.metrics.I_eff[i] = I_eff; - out.metrics.tau[i] = tau_i; + out.metrics.I_eff[i] = mathlib::real(I_eff); + out.metrics.tau[i] = mathlib::real(tau_i); //_metrics.tau_sat[i] = tau_sat; //_metrics.sat_flag[i] = saturated; @@ -406,54 +406,46 @@ namespace robots { for (size_t i = 0; i < n; ++i) { const SpatialJoint& joint = model.joints[i]; if (!isControlledJoint(joint.type)) { - scratch.dense.tau[i] = 0.0; + scratch.dense.tau[i] = Scalar(0); continue; } - const Scalar wn = Scalar(5.0); + const Scalar wn = Scalar(5); const Scalar z = Scalar(0.7); const Scalar err = snap.q_ref[i] - q[i]; const Scalar err_d = snap.qd_ref[i] - qd[i]; - const Scalar eps = (Scalar)1e-6; + const Scalar eps = Scalar(1e-6); const Scalar I_eff = std::max(M(i, i), eps); const Scalar k_p = I_eff * wn * wn; - const Scalar k_d = Scalar(2.0) * z * I_eff * wn; + const Scalar k_d = Scalar(2) * z * I_eff * wn; const Scalar b = Scalar(0.2); // viscous damping coefficient const Scalar c = Scalar(0.05); // Coulomb friction coefficient - const Scalar eps_f = (Scalar)1e-2; + const Scalar eps_f = Scalar(1e-2); Scalar tau_i = k_p * err + k_d * err_d + I_eff * snap.qdd_ref[i]; tau_i -= b * qd[i]; - //tau_i -= 0.05 * std::tanh(qd[i] / 1e-2); + //tau_i -= b * std::tanh(qd[i] / eps_f); - scratch.dense.tau[i] = tau_i; + scratch.dense.tau[i] = mathlib::real(tau_i); - out.metrics.q[i] = q[i]; - out.metrics.qd[i] = qd[i]; + out.metrics.q[i] = mathlib::real(q[i]); + out.metrics.qd[i] = mathlib::real(qd[i]); - out.metrics.err[i] = err; - out.metrics.errd[i] = err_d; + out.metrics.err[i] = mathlib::real(err); + out.metrics.errd[i] = mathlib::real(err_d); - out.metrics.I_eff[i] = I_eff; - out.metrics.tau[i] = tau_i; + out.metrics.I_eff[i] = mathlib::real(I_eff); + out.metrics.tau[i] = mathlib::real(tau_i); } out.qdd = SpatialDynamics::ABA(model, q, qd, scratch.dense.tau, scratch); - out.metrics.qdd = out.qdd; + out.metrics.qdd = mathlib::real(out.qdd); dx.head(n) = qd; dx.tail(n) = out.qdd; - - using DXScalar = typename std::decay_t; - - static_assert( - !std::is_same_v, - "dx collapsed to double" - ); - return dx; } diff --git a/DSFE_App/DSFE_Core/include/Robots/RobotSimSnapshot.h b/DSFE_App/DSFE_Core/include/Robots/RobotSimSnapshot.h index 7d2038ff..bb176b7f 100644 --- a/DSFE_App/DSFE_Core/include/Robots/RobotSimSnapshot.h +++ b/DSFE_App/DSFE_Core/include/Robots/RobotSimSnapshot.h @@ -48,4 +48,34 @@ namespace robots { Scalar simTime = Scalar(0); }; using RobotSimSnapshot = RobotSimSnapshot_T; + + template + inline RobotSimSnapshot_T castSnapshot( + const RobotSimSnapshot_T& src + ) { + RobotSimSnapshot_T dst; + + dst.model = src.model; + + dst.q = src.q.template cast(); + dst.qd = src.qd.template cast(); + + dst.q_ref = src.q_ref.template cast(); + dst.qd_ref = src.qd_ref.template cast(); + dst.qdd_ref = src.qdd_ref.template cast(); + + dst.robotRootPose = src.robotRootPose.template cast(); + + dst.baseIsFree = src.baseIsFree; + + dst.lastBaseForwardForce = ToScalar(src.lastBaseForwardForce); + dst.gravity = ToScalar(src.gravity); + + dst.torqueMode = src.torqueMode; + + dst.dt = ToScalar(src.dt); + dst.simTime = ToScalar(src.simTime); + + return dst; + } } // namespace robots \ No newline at end of file diff --git a/DSFE_App/DSFE_Core/include/Robots/SpatialDynamics.inl b/DSFE_App/DSFE_Core/include/Robots/SpatialDynamics.inl index 9b9308e9..4691e1f6 100644 --- a/DSFE_App/DSFE_Core/include/Robots/SpatialDynamics.inl +++ b/DSFE_App/DSFE_Core/include/Robots/SpatialDynamics.inl @@ -227,15 +227,6 @@ namespace robots { d_out[i] = Scalar(1e-12); } - using DScalar = typename std::decay_t< - decltype(d_out[i]) - >; - - static_assert( - !std::is_same_v, - "d_out collapsed" - ); - u_out[i] = tau[i] - dot(j.S, pA_out[i]); Ia_out[i] = IA_out[i] - outer(U_out[i]) / d_out[i]; diff --git a/DSFE_App/DSFE_Core/src/Robots/RobotSystem.cpp b/DSFE_App/DSFE_Core/src/Robots/RobotSystem.cpp index 57244fda..279b6d0e 100644 --- a/DSFE_App/DSFE_Core/src/Robots/RobotSystem.cpp +++ b/DSFE_App/DSFE_Core/src/Robots/RobotSystem.cpp @@ -298,7 +298,7 @@ namespace robots { _simTime = simTime; mathlib::VecX x = packState(); - RobotSimSnapshot snap = takeSnapshot(simTime); + RobotSimSnapshot_T snap = takeSnapshot(simTime); const size_t n = snap.model->joints.size(); Eigen::Map> q(x.data(), n); @@ -353,10 +353,31 @@ namespace robots { DynamicsScratch scratch; DynamicsResult result; + SpatialModel spatialModel_s; + + spatialModel_s.linkNameToIndex = _spatialModel.linkNameToIndex; + spatialModel_s.joints.resize(_spatialModel.joints.size()); + + for (size_t i = 0; i < _spatialModel.joints.size(); ++i) { + + const auto& src = _spatialModel.joints[i]; + auto& dst = spatialModel_s.joints[i]; + + dst.parent = src.parent; + dst.type = src.type; + dst.name = src.name; + + dst.Xtree = src.Xtree.template cast(); + dst.inertia = src.inertia.template cast(); + dst.S.v = src.S.v.template cast(); + } + + auto snap_s = robots::castSnapshot(snap); + return _dynamics->template derivative_spatial( - _spatialModel, + spatialModel_s, t, xIn, - snap, + snap_s, scratch, result ); }; diff --git a/PxMLib/MathLib/include/core/DualNumbers.h b/PxMLib/MathLib/include/core/DualNumbers.h index aaaa25e6..abbf3bbc 100644 --- a/PxMLib/MathLib/include/core/DualNumbers.h +++ b/PxMLib/MathLib/include/core/DualNumbers.h @@ -3,6 +3,8 @@ #include "MathLibAPI.h" #include "core/Types_tpl.h" +#include +#include #include #include @@ -11,6 +13,8 @@ namespace mathlib { template class DualNumber_T { public: + using ScalarT = Scalar; + DualNumber_T( Scalar real = Scalar(0), const std::array& dual = std::array() @@ -41,7 +45,7 @@ namespace mathlib { template inline DualNumber_T operator-( const DualNumber_T& a - ) { + ) { DualNumber_T out; out.real = -a.real; for (size_t i = 0; i < NVar; ++i) { @@ -54,7 +58,7 @@ namespace mathlib { template inline DualNumber_T operator+( const DualNumber_T& a - ) { + ) { return a; } @@ -67,7 +71,7 @@ namespace mathlib { inline bool operator==( const DualNumber_T& a, const DualNumber_T& b - ) { + ) { return a.real == b.real; } @@ -76,7 +80,7 @@ namespace mathlib { inline bool operator!=( const DualNumber_T& a, const DualNumber_T& b - ) { + ) { return !(a == b); } @@ -85,7 +89,7 @@ namespace mathlib { inline bool operator<( const DualNumber_T& a, const DualNumber_T& b - ) { + ) { return a.real < b.real; } @@ -94,7 +98,7 @@ namespace mathlib { inline bool operator<=( const DualNumber_T& a, const DualNumber_T& b - ) { + ) { return a.real <= b.real; } @@ -112,10 +116,10 @@ namespace mathlib { inline bool operator>=( const DualNumber_T& a, const DualNumber_T& b - ) { + ) { return a.real >= b.real; } - + // ----- // Scalar Interactions // ----- @@ -125,7 +129,7 @@ namespace mathlib { inline DualNumber_T operator+( const DualNumber_T& a, Scalar b - ) { + ) { DualNumber_T out = a; out.real += b; return out; @@ -136,7 +140,7 @@ namespace mathlib { inline DualNumber_T operator+( Scalar a, const DualNumber_T& b - ) { + ) { return b + a; // Reuse dual + scalar } @@ -145,7 +149,7 @@ namespace mathlib { inline DualNumber_T operator-( const DualNumber_T& a, Scalar b - ) { + ) { DualNumber_T out = a; out.real -= b; return out; @@ -156,7 +160,7 @@ namespace mathlib { inline DualNumber_T operator-( Scalar a, const DualNumber_T& b - ) { + ) { DualNumber_T out; out.real = a - b.real; for (size_t i = 0; i < NVar; ++i) { @@ -170,7 +174,7 @@ namespace mathlib { inline DualNumber_T operator*( const DualNumber_T& a, Scalar b - ) { + ) { DualNumber_T out; out.real = a.real * b; for (size_t i = 0; i < NVar; ++i) { @@ -184,7 +188,7 @@ namespace mathlib { inline DualNumber_T operator*( Scalar a, const DualNumber_T& b - ) { + ) { return b * a; // Reuse dual * scalar } @@ -193,7 +197,7 @@ namespace mathlib { inline DualNumber_T operator/( const DualNumber_T& a, Scalar b - ) { + ) { DualNumber_T out; out.real = a.real / b ? a.real / b : throw std::runtime_error("Division by zero in dual number scalar division"); for (size_t i = 0; i < NVar; ++i) { @@ -207,7 +211,7 @@ namespace mathlib { inline DualNumber_T operator/( Scalar a, const DualNumber_T& b - ) { + ) { DualNumber_T out; out.real = a / b.real; for (size_t i = 0; i < NVar; ++i) { @@ -225,7 +229,7 @@ namespace mathlib { inline DualNumber_T operator+( const DualNumber_T& a, const DualNumber_T& b - ) { + ) { DualNumber_T out; out.real = a.real + b.real; for (size_t i = 0; i < NVar; ++i) { @@ -239,7 +243,7 @@ namespace mathlib { inline DualNumber_T operator-( const DualNumber_T& a, const DualNumber_T& b - ) { + ) { DualNumber_T out; out.real = a.real - b.real; for (size_t i = 0; i < NVar; ++i) { @@ -253,7 +257,7 @@ namespace mathlib { inline DualNumber_T operator*( const DualNumber_T& a, const DualNumber_T& b - ) { + ) { DualNumber_T out; out.real = a.real * b.real; for (size_t i = 0; i < NVar; ++i) { @@ -267,7 +271,7 @@ namespace mathlib { inline DualNumber_T operator/( const DualNumber_T& a, const DualNumber_T& b - ) { + ) { DualNumber_T out; out.real = a.real / b.real; for (size_t i = 0; i < NVar; ++i) { @@ -423,7 +427,7 @@ namespace mathlib { inline DualNumber_T& operator+=( DualNumber_T& a, const DualNumber_T& b - ) { + ) { a.real += b.real; for (size_t i = 0; i < NVar; ++i) { a.dual[i] += b.dual[i]; @@ -435,7 +439,7 @@ namespace mathlib { inline DualNumber_T& operator+=( DualNumber_T& a, Scalar b - ) { + ) { a.real += b; return a; } @@ -445,7 +449,7 @@ namespace mathlib { inline DualNumber_T& operator-=( DualNumber_T& a, const DualNumber_T& b - ) { + ) { a.real -= b.real; for (size_t i = 0; i < NVar; ++i) { a.dual[i] -= b.dual[i]; @@ -457,7 +461,7 @@ namespace mathlib { inline DualNumber_T& operator-=( DualNumber_T& a, Scalar b - ) { + ) { a.real -= b; return a; } @@ -467,7 +471,7 @@ namespace mathlib { inline DualNumber_T& operator*=( DualNumber_T& a, Scalar b - ) { + ) { a.real *= b; for (size_t i = 0; i < NVar; ++i) { a.dual[i] *= b; @@ -480,7 +484,7 @@ namespace mathlib { inline DualNumber_T& operator*=( DualNumber_T& a, const DualNumber_T& b - ) { + ) { Scalar ogReal = a.real; for (size_t i = 0; i < NVar; ++i) { a.dual[i] = ogReal * b.dual[i] + a.dual[i] * b.real; @@ -494,7 +498,7 @@ namespace mathlib { inline DualNumber_T& operator/=( DualNumber_T& a, Scalar b - ) { + ) { if (b == Scalar(0)) { throw std::runtime_error( "Division by zero in DualNumber_T operator/=" @@ -512,7 +516,7 @@ namespace mathlib { inline DualNumber_T& operator/=( DualNumber_T& a, const DualNumber_T& b - ) { + ) { Scalar ogReal = a.real; for (size_t i = 0; i < NVar; ++i) { a.dual[i] = (a.dual[i] * b.real - ogReal * b.dual[i]) / (b.real * b.real); @@ -530,5 +534,200 @@ namespace mathlib { // Numeric Limits Specialisation for DualNumber_T // ----- - // TODO: Add specialisation of std::numeric_limits for DualNumber_T to define properties like infinity, NaN, epsilon, etc. based on the underlying Scalar type. + // Function to return a DualNumber_T representing infinity (real part is infinity, dual parts are zero) + template + inline DualNumber_T numeric_limits_infinity() { + return DualNumber_T( + std::numeric_limits::infinity(), + std::array() + ); + } + + // Alternative absolute value function that simply negates the dual number if the real part is negative (this is less smooth but can be more efficient and avoids issues with zero) + template + inline DualNumber_T abs( + const DualNumber_T& a + ) { + DualNumber_T out; + out = (a.real < Scalar(0)) ? -a : a; + return out; + } + // Absolute value function with epsilon threshold to avoid non-differentiability at zero (scales dual part by the sign of the real part, but treats values within epsilon of zero as zero) + template + inline DualNumber_T abs( + const DualNumber_T& a, + Scalar epsilon + ) { + DualNumber_T out; + if (std::abs(a.real) < epsilon) { + out.real = Scalar(0); + for (size_t i = 0; i < NVar; ++i) { + out.dual[i] = Scalar(0); + } + } + else { + out.real = std::abs(a.real); + Scalar sign = (a.real > Scalar(0)) - (a.real < Scalar(0)); // Sign of the real part + for (size_t i = 0; i < NVar; ++i) { + out.dual[i] = sign * a.dual[i]; + } + } + return out; + } + + + // Sign function for DualNumber_T (returns the sign of the real part, dual parts are zero) + template + inline DualNumber_T sign( + const DualNumber_T& a + ) { + DualNumber_T out; + out.real = (a.real > Scalar(0)) - (a.real < Scalar(0)); // Sign of the real part + for (size_t i = 0; i < NVar; ++i) { + out.dual[i] = Scalar(0); + } + return out; + } + + // Minimum function for DualNumber_T (returns the minimum of the real parts, scales dual part by the indicator of which real part is smaller) + template + inline DualNumber_T min( + const DualNumber_T& a, + const DualNumber_T& b + ) { + DualNumber_T out; + if (a.real < b.real) { + out.real = a.real; + for (size_t i = 0; i < NVar; ++i) { + out.dual[i] = a.dual[i]; + } + } + else { + out.real = b.real; + for (size_t i = 0; i < NVar; ++i) { + out.dual[i] = b.dual[i]; + } + } + return out; + } + + // Maximum function for DualNumber_T (returns the maximum of the real parts, scales dual part by the indicator of which real part is larger) + template + inline DualNumber_T max( + const DualNumber_T& a, + const DualNumber_T& b + ) { + DualNumber_T out; + if (a.real > b.real) { + out.real = a.real; + for (size_t i = 0; i < NVar; ++i) { + out.dual[i] = a.dual[i]; + } + } + else { + out.real = b.real; + for (size_t i = 0; i < NVar; ++i) { + out.dual[i] = b.dual[i]; + } + } + return out; + } + + // Since dual numbers are not complex, the real part is just the real part of the dual number (for non-dual types, this is just the value itself) + template + inline T real(const T& x) { + return x; + } + + // Since dual numbers are not complex, the real part is just the real part of the dual number + template + inline Scalar real( + const DualNumber_T& a + ) { + return a.real; + } + + // Since dual numbers are not complex, the imaginary part is always zero + template + inline Scalar imaginary( + const DualNumber_T& /*a*/ + ) { + return Scalar(0); + } + + // Since dual numbers are not complex, the complex conjugate is just the dual number itself + template + inline DualNumber_T conj( + const DualNumber_T& a + ) { + return a; + } + + // Since dual numbers are not complex, the norm is just the absolute value of the real part + template + inline Scalar norm( + const DualNumber_T& a + ) { + return std::abs(a.real); + } + + // Since dual numbers are not complex, the squared norm is just the square of the absolute value of the real part + template + inline Scalar abs2( + const DualNumber_T& a + ) { + return a.real * a.real; + } + + template + inline bool is_dual_number_v( + const DualNumber_T& /*a*/ + ) { + return true; + } + + // Traits class to identify dual numbers and extract the base scalar type + template + struct DualTraits { + using BaseScalar = T; + static constexpr bool is_dual = false; + }; + + // Specialization of DualTraits for DualNumber_T + template + struct DualTraits> { + using BaseScalar = Scalar; + static constexpr bool is_dual = true; + }; +} + +namespace Eigen { + // Specialization of NumTraits for DualNumber_T to allow it to be used with Eigen's matrix and vector types + template + struct NumTraits> : GenericNumTraits> { + typedef mathlib::DualNumber_T Real; + typedef mathlib::DualNumber_T NonInteger; + typedef mathlib::DualNumber_T Nested; + enum { + IsComplex = 0, + IsInteger = 0, + IsSigned = 1, + RequireInitialization = 1, + ReadCost = 1, + AddCost = 3, + MulCost = 3 + }; + static EIGEN_DEVICE_FUNC Real epsilon() { + return Real(std::numeric_limits::epsilon()); + } + static EIGEN_DEVICE_FUNC Real dummy_precision() { + return Real(Scalar(1e-12)); + } + static EIGEN_DEVICE_FUNC Real highest() { + return Real(std::numeric_limits::max()); + } + static EIGEN_DEVICE_FUNC Real lowest() { + return Real(std::numeric_limits::lowest()); + } + }; } \ No newline at end of file diff --git a/PxMLib/MathLib/include/integrators/numerical_integrators.h b/PxMLib/MathLib/include/integrators/numerical_integrators.h index 044eaf56..07af33d4 100644 --- a/PxMLib/MathLib/include/integrators/numerical_integrators.h +++ b/PxMLib/MathLib/include/integrators/numerical_integrators.h @@ -82,8 +82,28 @@ namespace integration { Func&& f, JacFunc&& jac = nullptr, int maxIter = 8, - Scalar tol = 1e-6 + Scalar tol = Scalar(1e-6) ); + // Overload without Jacobian + template + mathlib::VecX_T implicitEuler( + const mathlib::VecX_T& x, + Scalar t, + Scalar dt, + Func&& f, + int maxIter, + Scalar tol + ) { + return implicitEuler( + x, + t, + dt, + std::forward(f), + nullptr, + maxIter, + tol + ); + } // Implicit Midpoint method template @@ -94,8 +114,28 @@ namespace integration { Func&& f, JacFunc&& jac = nullptr, int maxIter = 10, - Scalar tol = 1e-7 + Scalar tol = Scalar(1e-7) ); + // Overload without Jacobian + template + mathlib::VecX_T implicitMidpoint( + const mathlib::VecX_T& x, + Scalar t, + Scalar dt, + Func&& f, + int maxIter, + Scalar tol + ) { + return implicitMidpoint( + x, + t, + dt, + std::forward(f), + nullptr, + maxIter, + tol + ); + } // Gauss-Legendre Runge-Kutta method (2 stages, 4th order) template @@ -106,8 +146,28 @@ namespace integration { Func&& f, JacFunc&& jac = nullptr, int maxIter = 50, - Scalar tol = 1e-6 + Scalar tol = Scalar(1e-6) ); + // Overload without Jacobian + template + mathlib::VecX_T GLRK2( + const mathlib::VecX_T& x, + Scalar t, + Scalar dt, + Func&& f, + int maxIter, + Scalar tol + ) { + return GLRK2( + x, + t, + dt, + std::forward(f), + nullptr, + maxIter, + tol + ); + } // Gauss-Legendre Runge-Kutta method (3 stages, 6th order) template @@ -118,8 +178,28 @@ namespace integration { Func&& f, JacFunc&& jac = nullptr, int maxIter = 150, - Scalar tol = 1e-14 + Scalar tol = Scalar(1e-14) ); + // Overload without Jacobian + template + mathlib::VecX_T GLRK3( + const mathlib::VecX_T& x, + Scalar t, + Scalar dt, + Func&& f, + int maxIter, + Scalar tol + ) { + return GLRK3( + x, + t, + dt, + std::forward(f), + nullptr, + maxIter, + tol + ); + } private: diff --git a/PxMLib/MathLib/include/integrators/numerical_integrators.inl b/PxMLib/MathLib/include/integrators/numerical_integrators.inl index d04392ff..82a80736 100644 --- a/PxMLib/MathLib/include/integrators/numerical_integrators.inl +++ b/PxMLib/MathLib/include/integrators/numerical_integrators.inl @@ -196,8 +196,6 @@ namespace integration { int maxIter, Scalar tol ) { - mathlib::VecX_T x_new = x + dt * f(t, x); // Initial guess - // The function g(x_guess) = 0 that we want to solve for the implicit Euler step auto g = [&](const mathlib::VecX_T& x_guess, mathlib::VecX_T& g_out) { g_out = x_guess - x - dt * f(t + dt, x_guess); }; // Numerical Jacobian for Newton-Raphson @@ -223,8 +221,10 @@ namespace integration { if (USE_AD_JACOBIANS == true) { printf("Using AD Jacobian for Implicit Euler\n"); F = automaticDifferenceJacobian( - [&](auto, const auto& x_pert) { - return f(t + dt, x_pert); + [&](auto t_pert, const auto& x_pert) { + using Dual = std::decay_t; + Dual t_eval = Dual(t_pert) + Dual(dt); + return f(t_eval, x_pert); }, t + dt, x_guess @@ -234,7 +234,7 @@ namespace integration { printf("Using finite difference Jacobian for Implicit Euler\n"); F = finiteDifferenceJacobian( [&](Scalar, const mathlib::VecX_T& x_pert) { - return f(t + dt, x + x_pert); + return f(t + dt, x_pert); }, t + dt, x_guess @@ -285,10 +285,11 @@ namespace integration { if (USE_AD_JACOBIANS == true) { printf("Using AD Jacobian for Implicit Midpoint\n"); F = automaticDifferenceJacobian( - [&](auto, const auto& x_pert) { - using Dual = typename std::remove_reference_t; + [&](auto t_pert, const auto& x_pert) { + using Dual = std::decay_t; auto x_dual = x.template cast(); - return f((t + dt) / Scalar(2), (x_dual + x_pert) / Scalar(2)); + Dual t_mid = Dual(t_pert + dt) / Dual(2); + return f(t_mid , (x_dual + x_pert) / Dual(2)); }, t + dt / Scalar(2), x_guess @@ -323,22 +324,24 @@ namespace integration { int maxIter, Scalar tol ) { + using std::sqrt; + const Eigen::Index n = x.size(); // Coefficients for the 2-stage Gauss-Legendre method (4th order) mathlib::VecX_T c(2); c << - 0.5 - std::sqrt(3.0) / 6.0, - 0.5 + std::sqrt(3.0) / 6.0; // Stage time fractions + Scalar(0.5) - sqrt(Scalar(3)) / Scalar(6), + Scalar(0.5) + sqrt(Scalar(3)) / Scalar(6); // Stage time fractions mathlib::MatX_T A(2, 2); A << - 0.25, 0.25 - std::sqrt(3.0) / 6.0, - 0.25 + std::sqrt(3.0) / 6.0, 0.25; + Scalar(0.25), Scalar(0.25) - sqrt(Scalar(3)) / Scalar(6), + Scalar(0.25) + sqrt(Scalar(3)) / Scalar(6), Scalar(0.25); - const Scalar b = 0.5; // Weights for final update + const Scalar b = Scalar(0.5); // Weights for final update - // Initial guess for the stage values k1, k2, k3 + // Initial guess for the stage values k1, k2 mathlib::VecX_T k(2 * n); // 2 stages mathlib::VecX_T f0 = f(t, x); k.segment(0, n) = f0; @@ -388,15 +391,19 @@ namespace integration { if (USE_AD_JACOBIANS == true) { printf("Using AD Jacobian for GLRK2\n"); F1 = automaticDifferenceJacobian( - [&](auto, const auto& x_pert) { - return f(t + c(0) * dt, x_pert); + [&](auto t_pert, const auto& x_pert) { + using Dual = std::decay_t; + Dual t_eval = Dual(t_pert) + Dual(c(0)) * Dual(dt); + return f(t_eval, x_pert); }, t + c(0) * dt, x1 ); F2 = automaticDifferenceJacobian( - [&](auto, const auto& x_pert) { - return f(t + c(1) * dt, x_pert); + [&](auto t_pert, const auto& x_pert) { + using Dual = std::decay_t; + Dual t_eval = Dual(t_pert) + Dual(c(1)) * Dual(dt); + return f(t_eval, x_pert); }, t + c(1) * dt, x2 @@ -453,23 +460,31 @@ namespace integration { int maxIter, Scalar tol ) { - if constexpr (std::is_same_v) { - if (tol < 0.0) { tol = std::max(1e-12, 1e-2 * std::pow(dt, 7.0)); } + using Real = typename mathlib::DualTraits::BaseScalar; + using std::sqrt; + using std::pow; + using std::abs; + using std::max; + + if (mathlib::real(tol) < Real(0)) { + Real dt_r = mathlib::real(dt); + Real tol_r = max(Real(1e-12), Real(1e-2) * pow(dt_r, Real(7))); + tol = Scalar(tol_r); } const Eigen::Index n = x.size(); // Coefficients for the 3-stage Gauss-Legendre method (6th order) mathlib::VecX_T c(3); c << - 0.5 - std::sqrt(15.0) / 10.0, - 0.5, - 0.5 + std::sqrt(15.0) / 10.0; // Stage time fractions + Scalar(0.5) - sqrt(Scalar(15)) / Scalar(10), + Scalar(0.5), + Scalar(0.5) + sqrt(Scalar(15)) / Scalar(10); // Stage time fractions mathlib::Mat3_T A(3, 3); A << - 5.0 / 36.0, 2.0 / 9.0 - std::sqrt(15.0) / 15.0, 5.0 / 36.0 - std::sqrt(15.0) / 30.0, - 5.0 / 36.0 + std::sqrt(15.0) / 24.0, 2.0 / 9.0, 5.0 / 36.0 - std::sqrt(15.0) / 24.0, - 5.0 / 36.0 + std::sqrt(15.0) / 30.0, 2.0 / 9.0 + std::sqrt(15.0) / 15.0, 5.0 / 36.0; + Scalar(5) / Scalar(36), Scalar(2) / Scalar(9) - sqrt(Scalar(15)) / Scalar(15), Scalar(5) / Scalar(36) - sqrt(Scalar(15)) / Scalar(30), + Scalar(5) / Scalar(36) + sqrt(Scalar(15)) / Scalar(24), Scalar(2) / Scalar(9), Scalar(5) / Scalar(36) - sqrt(Scalar(15)) / Scalar(24), + Scalar(5) / Scalar(36) + sqrt(Scalar(15)) / Scalar(30), Scalar(2) / Scalar(9) - sqrt(Scalar(15)) / Scalar(15), Scalar(5) / Scalar(36); mathlib::VecX_T b(3); b << 5.0 / 18.0, 4.0 / 9.0, 5.0 / 18.0; // Weights for final update @@ -536,22 +551,28 @@ namespace integration { if (USE_AD_JACOBIANS == true) { printf("Using AD Jacobian for GLRK3\n"); F1 = automaticDifferenceJacobian( - [&](auto, const auto& x_pert) { - return f(t + c(0) * dt, x_pert); + [&](auto t_pert, const auto& x_pert) { + using Dual = std::decay_t; + Dual t_eval = Dual(t_pert) + Dual(c(0)) * Dual(dt); + return f(t_eval, x_pert); }, t + c(0) * dt, x1 ); F2 = automaticDifferenceJacobian( - [&](auto, const auto& x_pert) { - return f(t + c(1) * dt, x_pert); + [&](auto t_pert, const auto& x_pert) { + using Dual = std::decay_t; + Dual t_eval = Dual(t_pert) + Dual(c(1)) * Dual(dt); + return f(t_eval, x_pert); }, t + c(1) * dt, x2 ); F3 = automaticDifferenceJacobian( - [&](auto, const auto& x_pert) { - return f(t + c(2) * dt, x_pert); + [&](auto t_pert, const auto& x_pert) { + using Dual = std::decay_t; + Dual t_eval = Dual(t_pert) + Dual(c(2)) * Dual(dt); + return f(t_eval, x_pert); }, t + c(2) * dt, x3 @@ -603,7 +624,7 @@ namespace integration { mathlib::VecX_T g_check; eval_g(k, g_check); - Scalar residual = g_check.norm(); + auto residual = mathlib::real(g_check.norm()); //if (residual > 1e-8) { std::cout << "[GLRK3] Large final residual: " << residual << std::endl; } // Compute the final update for x using the stage values @@ -663,7 +684,7 @@ namespace integration { } x += delta; - if (delta.norm() < tol * (1.0 + x.norm())) { return x; } + if (delta.norm() < tol * (Scalar(1) + x.norm())) { return x; } if (!x.allFinite()) { throw std::runtime_error( @@ -690,7 +711,7 @@ namespace integration { Scalar t, const mathlib::VecX_T& x ) { - const Scalar eps_rel = 1e-8; + const Scalar eps_rel = Scalar(1e-8); const int n = static_cast(x.size()); mathlib::VecX_T f_0 = f(t, x); mathlib::MatX_T J(f_0.size(), n); @@ -703,7 +724,7 @@ namespace integration { x_bwd(i) -= h; mathlib::VecX_T f_fwd = f(t, x_fwd); mathlib::VecX_T f_bwd = f(t, x_bwd); - J.col(i) = (f_fwd - f_bwd) / (2.0 * h); + J.col(i) = (f_fwd - f_bwd) / (Scalar(2) * h); } return J; } @@ -718,19 +739,22 @@ namespace integration { const int n = static_cast(x.size()); mathlib::MatX_T J(n, n); for (int i = 0; i < n; ++i) { - mathlib::VecX_T> x_dual(n); + using BaseScalar = typename mathlib::DualTraits::BaseScalar; + using Dual_T = mathlib::DualNumber_T; + + mathlib::VecX_T x_dual(n); for (int k = 0; k < n; ++k) { - x_dual(k) = mathlib::DualNumber_T(x(k), std::array{ (k == i) ? Scalar(1) : Scalar(0) }); + x_dual(k) = Dual_T( + x(k), + std::array{ + (k == i) ? Scalar(1) : Scalar(0) + } + ); } - mathlib::DualNumber_T t_dual(t); - auto f_dual = f(t_dual, x_dual); - - using FDualScalar = std::remove_reference_t; - static_assert( - !std::is_same_v, - "f_dual collapsed to double" - ); + Dual_T t_dual(t); + using ReturnT = decltype(f(t_dual, x_dual)); + ReturnT f_dual = f(t_dual, x_dual); for (int j = 0; j < n; ++j) { J(j, i) = f_dual(j).dual[0]; From 679e4a1df947ea94ab682f8fc728e4455232aa1b Mon Sep 17 00:00:00 2001 From: saltyjoss Date: Mon, 18 May 2026 20:29:56 +0100 Subject: [PATCH 035/210] fixes(PARTIAL): Updated many things surrouding dual number templates and usage in code * I cannot be bothered to write everything, or commit small changes right now, I have been debugging since I got home from work (5hours 30mins ago) so I am just gonna try solve it without caring too much abotu VC. --- .../include/Numerics/IntegrationService.h | 2 +- .../include/Robots/RobotDynamics.inl | 2 +- .../src/Numerics/IntegrationService.cpp | 3 +- PxMLib/MathLib/include/core/DualNumbers.h | 8 + .../integrators/numerical_integrators.h | 67 +- .../integrators/numerical_integrators.inl | 368 +++-- .../ADImplicitIntegratorTests.cpp | 1306 ++++++++--------- 7 files changed, 896 insertions(+), 860 deletions(-) diff --git a/DSFE_App/DSFE_Core/include/Numerics/IntegrationService.h b/DSFE_App/DSFE_Core/include/Numerics/IntegrationService.h index 78cbd09d..77fcf24e 100644 --- a/DSFE_App/DSFE_Core/include/Numerics/IntegrationService.h +++ b/DSFE_App/DSFE_Core/include/Numerics/IntegrationService.h @@ -50,7 +50,7 @@ namespace integration { case eIntegrationMethod::ImplicitEuler: return { _integrator->implicitEuler(x, t, dt, std::forward(f), std::forward(jac)), dt, dt }; case eIntegrationMethod::ImplicitMidpoint: return { _integrator->implicitMidpoint(x, t, dt, std::forward(f), std::forward(jac)), dt, dt }; case eIntegrationMethod::GLRK2: return { _integrator->GLRK2(x, t, dt, std::forward(f), std::forward(jac), 50, 1e-10), dt, dt }; - case eIntegrationMethod::GLRK3: return { _integrator->GLRK3(x, t, dt, std::forward(f), std::forward(jac), 150, 1e-14), dt, dt }; + case eIntegrationMethod::GLRK3: return { _integrator->GLRK3(x, t, dt, std::forward(f), std::forward(jac), 50, 1e-10), dt, dt }; default: LOG_WARN("Unknown integration method: %s. Defaulting to RK4.", toString(m)); return { _integrator->rk4Step(x, t, dt, std::forward(f)), dt, dt }; diff --git a/DSFE_App/DSFE_Core/include/Robots/RobotDynamics.inl b/DSFE_App/DSFE_Core/include/Robots/RobotDynamics.inl index f2564c63..8371ae58 100644 --- a/DSFE_App/DSFE_Core/include/Robots/RobotDynamics.inl +++ b/DSFE_App/DSFE_Core/include/Robots/RobotDynamics.inl @@ -443,7 +443,7 @@ namespace robots { } out.qdd = SpatialDynamics::ABA(model, q, qd, scratch.dense.tau, scratch); - out.metrics.qdd = mathlib::real(out.qdd); + out.metrics.qdd = out.qdd; dx.head(n) = qd; dx.tail(n) = out.qdd; return dx; diff --git a/DSFE_App/DSFE_Core/src/Numerics/IntegrationService.cpp b/DSFE_App/DSFE_Core/src/Numerics/IntegrationService.cpp index 9d07a161..01d4b792 100644 --- a/DSFE_App/DSFE_Core/src/Numerics/IntegrationService.cpp +++ b/DSFE_App/DSFE_Core/src/Numerics/IntegrationService.cpp @@ -61,5 +61,6 @@ namespace integration { // Constructor IntegrationService::IntegrationService() - : _integrator(std::make_unique()), method(eIntegrationMethod::RK4), _rtol(1e-3), _atol(1e-6), _dt_last(), _dt_max() {} + : _integrator(std::make_unique()), method(eIntegrationMethod::RK4), _rtol(1e-3), _atol(1e-6), _dt_last(), _dt_max() { + } } // namespace integration \ No newline at end of file diff --git a/PxMLib/MathLib/include/core/DualNumbers.h b/PxMLib/MathLib/include/core/DualNumbers.h index abbf3bbc..37c08f81 100644 --- a/PxMLib/MathLib/include/core/DualNumbers.h +++ b/PxMLib/MathLib/include/core/DualNumbers.h @@ -699,6 +699,14 @@ namespace mathlib { using BaseScalar = Scalar; static constexpr bool is_dual = true; }; + + // Dual Part + template + inline std::array dualPart( + const DualNumber_T& a + ) { + return a.dual; + } } namespace Eigen { diff --git a/PxMLib/MathLib/include/integrators/numerical_integrators.h b/PxMLib/MathLib/include/integrators/numerical_integrators.h index 07af33d4..aff75e1d 100644 --- a/PxMLib/MathLib/include/integrators/numerical_integrators.h +++ b/PxMLib/MathLib/include/integrators/numerical_integrators.h @@ -12,8 +12,7 @@ // Numerical integration methods namespace integration { - constexpr bool USE_AD_JACOBIANS = true; - + constexpr bool USE_AD_JACOBIANS = false; // Ordinary Differential Equation (ODE) solvers class MATHLIB_API NumericalIntegrator { public: @@ -84,26 +83,16 @@ namespace integration { int maxIter = 8, Scalar tol = Scalar(1e-6) ); - // Overload without Jacobian + // AD version of Implicit Euler method template - mathlib::VecX_T implicitEuler( + mathlib::VecX_T implicitEuler_AD( const mathlib::VecX_T& x, Scalar t, Scalar dt, Func&& f, int maxIter, Scalar tol - ) { - return implicitEuler( - x, - t, - dt, - std::forward(f), - nullptr, - maxIter, - tol - ); - } + ); // Implicit Midpoint method template @@ -116,26 +105,16 @@ namespace integration { int maxIter = 10, Scalar tol = Scalar(1e-7) ); - // Overload without Jacobian + // AD version of Implicit Midpoint method template - mathlib::VecX_T implicitMidpoint( + mathlib::VecX_T implicitMidpoint_AD( const mathlib::VecX_T& x, Scalar t, Scalar dt, Func&& f, int maxIter, Scalar tol - ) { - return implicitMidpoint( - x, - t, - dt, - std::forward(f), - nullptr, - maxIter, - tol - ); - } + ); // Gauss-Legendre Runge-Kutta method (2 stages, 4th order) template @@ -148,26 +127,16 @@ namespace integration { int maxIter = 50, Scalar tol = Scalar(1e-6) ); - // Overload without Jacobian + // AD version of GLRK2 template - mathlib::VecX_T GLRK2( + mathlib::VecX_T GLRK2_AD( const mathlib::VecX_T& x, Scalar t, Scalar dt, Func&& f, int maxIter, Scalar tol - ) { - return GLRK2( - x, - t, - dt, - std::forward(f), - nullptr, - maxIter, - tol - ); - } + ); // Gauss-Legendre Runge-Kutta method (3 stages, 6th order) template @@ -180,26 +149,16 @@ namespace integration { int maxIter = 150, Scalar tol = Scalar(1e-14) ); - // Overload without Jacobian + // AD version of GLRK3 template - mathlib::VecX_T GLRK3( + mathlib::VecX_T GLRK3_AD( const mathlib::VecX_T& x, Scalar t, Scalar dt, Func&& f, int maxIter, Scalar tol - ) { - return GLRK3( - x, - t, - dt, - std::forward(f), - nullptr, - maxIter, - tol - ); - } + ); private: diff --git a/PxMLib/MathLib/include/integrators/numerical_integrators.inl b/PxMLib/MathLib/include/integrators/numerical_integrators.inl index 82a80736..4732e8d5 100644 --- a/PxMLib/MathLib/include/integrators/numerical_integrators.inl +++ b/PxMLib/MathLib/include/integrators/numerical_integrators.inl @@ -218,28 +218,14 @@ namespace integration { } } if (!analytical_success) { - if (USE_AD_JACOBIANS == true) { - printf("Using AD Jacobian for Implicit Euler\n"); - F = automaticDifferenceJacobian( - [&](auto t_pert, const auto& x_pert) { - using Dual = std::decay_t; - Dual t_eval = Dual(t_pert) + Dual(dt); - return f(t_eval, x_pert); - }, - t + dt, - x_guess - ); - } - else { - printf("Using finite difference Jacobian for Implicit Euler\n"); - F = finiteDifferenceJacobian( - [&](Scalar, const mathlib::VecX_T& x_pert) { - return f(t + dt, x_pert); - }, - t + dt, - x_guess - ); - } + printf("Using finite difference Jacobian for Implicit Euler\n"); + F = finiteDifferenceJacobian( + [&](Scalar, const mathlib::VecX_T& x_pert) { + return f(t + dt, x_pert); + }, + t + dt, + x_guess + ); } J_out = mathlib::MatX_T::Identity(n, n) - dt * F; // J = I - dt * df/dx @@ -248,6 +234,30 @@ namespace integration { mathlib::VecX_T x0 = x + dt * f(t + dt, x); // Initial guess for Newton-Raphson return newtonRaphson(g, J, x0, maxIter, tol); } + // AD version of Implicit Euler method + template + mathlib::VecX_T NumericalIntegrator::implicitEuler_AD( + const mathlib::VecX_T& x, + Scalar t, + Scalar dt, + Func&& f, + int maxIter, + Scalar tol + ) { + auto jac = [&](const mathlib::VecX_T& x_guess, mathlib::MatX_T& J_out) { + J_out = automaticDifferenceJacobian( + [&](auto t_pert, const auto& x_pert) { + using Dual = std::decay_t; + Dual t_eval = Dual(t_pert) + Dual(dt); + return f(t_eval, x_pert); + }, + t, + x_guess + ); + J_out = mathlib::MatX_T::Identity(x.size(), x.size()) - dt * J_out; // J = I - dt * df/dx + }; + return implicitEuler(x, t, dt, f, jac, maxIter, tol); + } // Implicit Midpoint method template @@ -282,29 +292,14 @@ namespace integration { } } if (!analytical_success) { - if (USE_AD_JACOBIANS == true) { - printf("Using AD Jacobian for Implicit Midpoint\n"); - F = automaticDifferenceJacobian( - [&](auto t_pert, const auto& x_pert) { - using Dual = std::decay_t; - auto x_dual = x.template cast(); - Dual t_mid = Dual(t_pert + dt) / Dual(2); - return f(t_mid , (x_dual + x_pert) / Dual(2)); - }, - t + dt / Scalar(2), - x_guess - ); - } - else { - printf("Using finite difference Jacobian for Implicit Midpoint\n"); - F = finiteDifferenceJacobian( - [&](Scalar, const mathlib::VecX_T& x_pert) { - return f((t + dt) / Scalar(2), (x + x_pert) / Scalar(2)); - }, - t + dt / Scalar(2), - x_guess - ); - } + printf("Using finite difference Jacobian for Implicit Midpoint\n"); + F = finiteDifferenceJacobian( + [&](Scalar, const mathlib::VecX_T& x_pert) { + return f((t + dt) / Scalar(2), (x + x_pert) / Scalar(2)); + }, + t + dt / Scalar(2), + x_guess + ); } J_out = mathlib::MatX_T::Identity(n, n) - Scalar(0.5) * dt * F; // J = I - dt * df/dx }; @@ -312,6 +307,31 @@ namespace integration { mathlib::VecX_T x0 = x + dt * f((t + dt) / Scalar(2), x); // Initial guess for Newton-Raphson return newtonRaphson(g, J, x0, maxIter, tol); } + // AD version of Implicit Midpoint method + template + mathlib::VecX_T NumericalIntegrator::implicitMidpoint_AD( + const mathlib::VecX_T& x, + Scalar t, + Scalar dt, + Func&& f, + int maxIter, + Scalar tol + ) { + auto jac = [&](const mathlib::VecX_T& x_guess, mathlib::MatX_T& J_out) { + J_out = automaticDifferenceJacobian( + [&](auto t_pert, const auto& x_pert) { + using Dual = std::decay_t; + auto x_dual = x.template cast(); + Dual t_mid = Dual(t_pert + dt) / Dual(2); + return f(t_mid, (x_dual + x_pert) / Dual(2)); + }, + t, + x_guess + ); + J_out = mathlib::MatX_T::Identity(x.size(), x.size()) - Scalar(0.5) * dt * J_out; // J = I - dt * df/dx + }; + return implicitMidpoint(x, t, dt, f, jac, maxIter, tol); + } // Gauss-Legendre Runge-Kutta method (2 stages, 4th order) template @@ -388,44 +408,21 @@ namespace integration { } if (!analytical_success) { - if (USE_AD_JACOBIANS == true) { - printf("Using AD Jacobian for GLRK2\n"); - F1 = automaticDifferenceJacobian( - [&](auto t_pert, const auto& x_pert) { - using Dual = std::decay_t; - Dual t_eval = Dual(t_pert) + Dual(c(0)) * Dual(dt); - return f(t_eval, x_pert); - }, - t + c(0) * dt, - x1 - ); - F2 = automaticDifferenceJacobian( - [&](auto t_pert, const auto& x_pert) { - using Dual = std::decay_t; - Dual t_eval = Dual(t_pert) + Dual(c(1)) * Dual(dt); - return f(t_eval, x_pert); - }, - t + c(1) * dt, - x2 - ); - } - else { - printf("Using finite difference Jacobian for GLRK2\n"); - F1 = finiteDifferenceJacobian( - [&](Scalar, const mathlib::VecX_T& x_pert) { - return f(t + c(0) * dt, x_pert); - }, - t + c(0) * dt, - x1 - ); - F2 = finiteDifferenceJacobian( - [&](Scalar, const mathlib::VecX_T& x_pert) { - return f(t + c(1) * dt, x_pert); - }, - t + c(1) * dt, - x2 - ); - } + printf("Using finite difference Jacobian for GLRK2\n"); + F1 = finiteDifferenceJacobian( + [&](Scalar, const mathlib::VecX_T& x_pert) { + return f(t + c(0) * dt, x_pert); + }, + t + c(0) * dt, + x1 + ); + F2 = finiteDifferenceJacobian( + [&](Scalar, const mathlib::VecX_T& x_pert) { + return f(t + c(1) * dt, x_pert); + }, + t + c(1) * dt, + x2 + ); } J.setZero(2 * n, 2 * n); @@ -448,6 +445,55 @@ namespace integration { return x_f; } + // AD version of Gauss-Legendre Runge-Kutta method (2 stages, 4th order) + template + mathlib::VecX_T NumericalIntegrator::GLRK2_AD( + const mathlib::VecX_T& x, + Scalar t, + Scalar dt, + Func&& f, + int maxIter, + Scalar tol + ) { + + mathlib::VecX_T c(2); + c << + Scalar(0.5) - sqrt(Scalar(3)) / Scalar(6), + Scalar(0.5) + sqrt(Scalar(3)) / Scalar(6); // Stage time fractions + mathlib::MatX_T A(2, 2); + auto jac = [&](const mathlib::VecX_T& x_guess, mathlib::MatX_T& J_out) { + int n = (int)x_guess.size(); + mathlib::VecX_T x1 = x + dt * (A(0, 0) * x_guess.segment(0, n) + A(0, 1) * x_guess.segment(n, n)); + mathlib::VecX_T x2 = x + dt * (A(1, 0) * x_guess.segment(0, n) + A(1, 1) * x_guess.segment(n, n)); + J_out = automaticDifferenceJacobian( + [&](auto t_pert, const auto& x_pert) { + using Dual = std::decay_t; + + mathlib::VecX_T k1 = x_pert.segment(0, n); + mathlib::VecX_T k2 = x_pert.segment(n, n); + + mathlib::VecX_T x_base = x.template cast(); + + mathlib::VecX_T x1 = x_base + Dual(dt) * (Dual(A(0, 0)) * k1 + Dual(A(0, 1)) * k2); + mathlib::VecX_T x2 = x_base + Dual(dt) * (Dual(A(1, 0)) * k1 + Dual(A(1, 1)) * k2); + + Dual t_eval1 = Dual(t_pert) + Dual(c(0)) * Dual(dt); + Dual t_eval2 = Dual(t_pert) + Dual(c(1)) * Dual(dt); + + mathlib::VecX_T out(2 * n); + + out.segment(0, n) = k1 - f(t_eval1, x1); + out.segment(n, n) = k2 - f(t_eval2, x2); + + return out; + }, + t, + x_guess + ); + J_out = mathlib::MatX_T::Identity(x.size(), x.size()) - dt * J_out; // J = I - dt * df/dx + }; + return GLRK2(x, t, dt, f, jac, maxIter, tol); + } // Gauss-Legendre Runge-Kutta method (3 stages, 6th order) template @@ -548,62 +594,30 @@ namespace integration { } if (!analytical_success) { - if (USE_AD_JACOBIANS == true) { - printf("Using AD Jacobian for GLRK3\n"); - F1 = automaticDifferenceJacobian( - [&](auto t_pert, const auto& x_pert) { - using Dual = std::decay_t; - Dual t_eval = Dual(t_pert) + Dual(c(0)) * Dual(dt); - return f(t_eval, x_pert); - }, - t + c(0) * dt, - x1 - ); - F2 = automaticDifferenceJacobian( - [&](auto t_pert, const auto& x_pert) { - using Dual = std::decay_t; - Dual t_eval = Dual(t_pert) + Dual(c(1)) * Dual(dt); - return f(t_eval, x_pert); - }, - t + c(1) * dt, - x2 - ); - F3 = automaticDifferenceJacobian( - [&](auto t_pert, const auto& x_pert) { - using Dual = std::decay_t; - Dual t_eval = Dual(t_pert) + Dual(c(2)) * Dual(dt); - return f(t_eval, x_pert); - }, - t + c(2) * dt, - x3 - ); - } - else { - printf("Using finite difference Jacobian for GLRK3\n"); - F1 = finiteDifferenceJacobian( - [&](Scalar, const mathlib::VecX_T& x_pert) { - return f(t + c(0) * dt, x_pert); - }, - t + c(0) * dt, - x1 - ); - - F2 = finiteDifferenceJacobian( - [&](Scalar, const mathlib::VecX_T& x_pert) { - return f(t + c(1) * dt, x_pert); - }, - t + c(1) * dt, - x2 - ); - - F3 = finiteDifferenceJacobian( - [&](Scalar, const mathlib::VecX_T& x_pert) { - return f(t + c(2) * dt, x_pert); - }, - t + c(2) * dt, - x3 - ); - } + printf("Using finite difference Jacobian for GLRK3\n"); + F1 = finiteDifferenceJacobian( + [&](Scalar, const mathlib::VecX_T& x_pert) { + return f(t + c(0) * dt, x_pert); + }, + t + c(0) * dt, + x1 + ); + + F2 = finiteDifferenceJacobian( + [&](Scalar, const mathlib::VecX_T& x_pert) { + return f(t + c(1) * dt, x_pert); + }, + t + c(1) * dt, + x2 + ); + + F3 = finiteDifferenceJacobian( + [&](Scalar, const mathlib::VecX_T& x_pert) { + return f(t + c(2) * dt, x_pert); + }, + t + c(2) * dt, + x3 + ); } J.setZero(3 * n, 3 * n); @@ -638,6 +652,59 @@ namespace integration { return x_f; } + // AD version of Gauss-Legendre Runge-Kutta method (3 stages, 6th order) + template + mathlib::VecX_T NumericalIntegrator::GLRK3_AD( + const mathlib::VecX_T& x, + Scalar t, + Scalar dt, + Func&& f, + int maxIter, + Scalar tol + ) { + mathlib::VecX_T c(3); + c << + Scalar(0.5) - sqrt(Scalar(15)) / Scalar(10), + Scalar(0.5), + Scalar(0.5) + sqrt(Scalar(15)) / Scalar(10); // Stage time fractions + mathlib::Mat3_T A(3, 3); + auto jac = [&](const mathlib::VecX_T& x_guess, mathlib::MatX_T& J_out) { + int n = (int)x_guess.size(); + mathlib::VecX_T x1 = x + dt * (A(0, 0) * x_guess.segment(0, n) + A(0, 1) * x_guess.segment(n, n) + A(0, 2) * x_guess.segment(2 * n, n)); + mathlib::VecX_T x2 = x + dt * (A(1, 0) * x_guess.segment(0, n) + A(1, 1) * x_guess.segment(n, n) + A(1, 2) * x_guess.segment(2 * n, n)); + mathlib::VecX_T x3 = x + dt * (A(2, 0) * x_guess.segment(0, n) + A(2, 1) * x_guess.segment(n, n) + A(2, 2) * x_guess.segment(2 * n, n)); + J_out = automaticDifferenceJacobian( + [&](auto t_pert, const auto& x_pert) { + using Dual = std::decay_t; + + mathlib::VecX_T k1 = x_pert.segment(0, n); + mathlib::VecX_T k2 = x_pert.segment(n, n); + mathlib::VecX_T k3 = x_pert.segment(2 * n, n); + + mathlib::VecX_T x_base = x.template cast(); + + mathlib::VecX_T x1 = x_base + Dual(dt) * (Dual(A(0, 0)) * k1 + Dual(A(0, 1)) * k2 + Dual(A(0, 2)) * k3); + mathlib::VecX_T x2 = x_base + Dual(dt) * (Dual(A(1, 0)) * k1 + Dual(A(1, 1)) * k2 + Dual(A(1, 2)) * k3); + mathlib::VecX_T x3 = x_base + Dual(dt) * (Dual(A(2, 0)) * k1 + Dual(A(2, 1)) * k2 + Dual(A(2, 2)) * k3); + + Dual t_eval1 = Dual(t_pert) + Dual(c(0)) * Dual(dt); + Dual t_eval2 = Dual(t_pert) + Dual(c(1)) * Dual(dt); + Dual t_eval3 = Dual(t_pert) + Dual(c(2)) * Dual(dt); + + mathlib::VecX_T out(3 * n); + + out.segment(0, n) = k1 - f(t_eval1, x1); + out.segment(n, n) = k2 - f(t_eval2, x2); + out.segment(2 * n, n) = k3 - f(t_eval3, x3); + + return out; + }, + t, + x_guess + ); + }; + return GLRK3(x, t, dt, f, jac, maxIter, tol); + } // Newton-Raphson solver for systems of nonlinear equations g(x) = 0 template @@ -737,7 +804,10 @@ namespace integration { const mathlib::VecX_T& x ) { const int n = static_cast(x.size()); - mathlib::MatX_T J(n, n); + auto f0 = f(t, x); + const int m = static_cast(f0.size()); + + mathlib::MatX_T J(m, n); for (int i = 0; i < n; ++i) { using BaseScalar = typename mathlib::DualTraits::BaseScalar; using Dual_T = mathlib::DualNumber_T; @@ -745,19 +815,17 @@ namespace integration { mathlib::VecX_T x_dual(n); for (int k = 0; k < n; ++k) { x_dual(k) = Dual_T( - x(k), - std::array{ - (k == i) ? Scalar(1) : Scalar(0) - } + mathlib::real(x(k)), + std::array{ + (k == i) ? BaseScalar(1) : BaseScalar(0) + } ); } - Dual_T t_dual(t); - using ReturnT = decltype(f(t_dual, x_dual)); - ReturnT f_dual = f(t_dual, x_dual); - - for (int j = 0; j < n; ++j) { - J(j, i) = f_dual(j).dual[0]; + Dual_T t_dual(mathlib::real(t)); + auto f_dual = f(t_dual, x_dual); + for (int j = 0; j < m; ++j) { + J(j, i) = mathlib::dualPart(f_dual(j)); } } return J; diff --git a/PxMLib/MathLib_Unit/AutomaticDifferentiationTests/ADImplicitIntegratorTests.cpp b/PxMLib/MathLib_Unit/AutomaticDifferentiationTests/ADImplicitIntegratorTests.cpp index 936b5b74..f64910e2 100644 --- a/PxMLib/MathLib_Unit/AutomaticDifferentiationTests/ADImplicitIntegratorTests.cpp +++ b/PxMLib/MathLib_Unit/AutomaticDifferentiationTests/ADImplicitIntegratorTests.cpp @@ -11,669 +11,669 @@ using namespace mathlib; using namespace constants; namespace { - constexpr double tol_low = 1e-3; - constexpr double tol_high = 1e-6; + //constexpr double tol_low = 1e-3; + //constexpr double tol_high = 1e-6; - integration::NumericalIntegrator integrator; + //integration::NumericalIntegrator integrator; - // Exponential Decay function for dual numbers - template - VecX_T> expDecay( - DualNumber_T /*t*/, - const VecX_T>& x - ) { - VecX_T> dx = x; - for (int i = 0; i < x.size(); ++i) { dx(i) = -x(i); } - return dx; - } + //// Exponential Decay function for dual numbers + //template + //VecX_T> expDecay( + // DualNumber_T /*t*/, + // const VecX_T>& x + //) { + // VecX_T> dx = x; + // for (int i = 0; i < x.size(); ++i) { dx(i) = -x(i); } + // return dx; + //} - // Harmonic Oscillator function for dual numbers - template - VecX_T> harmonicOsc( - DualNumber_T /*t*/, - const VecX_T>& s - ) { - VecX_T> d(2); - d(0) = s(1); - d(1) = -s(0); - return d; - } + //// Harmonic Oscillator function for dual numbers + //template + //VecX_T> harmonicOsc( + // DualNumber_T /*t*/, + // const VecX_T>& s + //) { + // VecX_T> d(2); + // d(0) = s(1); + // d(1) = -s(0); + // return d; + //} - // Linear Decay function for dual numbers - template - VecX_T> linearDecay( - DualNumber_T /*t*/, - const VecX_T>& x - ) { - VecX_T> dx = x; - for (int i = 0; i < x.size(); ++i) { dx(i) = -2.0 * x(i); } - return dx; - } + //// Linear Decay function for dual numbers + //template + //VecX_T> linearDecay( + // DualNumber_T /*t*/, + // const VecX_T>& x + //) { + // VecX_T> dx = x; + // for (int i = 0; i < x.size(); ++i) { dx(i) = -2.0 * x(i); } + // return dx; + //} - // Nonlinear Decay function for dual numbers - template - VecX_T> nonlinearDecay( - DualNumber_T /*t*/, - const VecX_T>& x - ) { - VecX_T> dx = x; - for (int i = 0; i < x.size(); ++i) { dx(i) = -x(i) * x(i); } - return dx; - } + //// Nonlinear Decay function for dual numbers + //template + //VecX_T> nonlinearDecay( + // DualNumber_T /*t*/, + // const VecX_T>& x + //) { + // VecX_T> dx = x; + // for (int i = 0; i < x.size(); ++i) { dx(i) = -x(i) * x(i); } + // return dx; + //} - // Stiff Decay function for dual numbers - template - VecX_T> stiffDecay( - DualNumber_T /*t*/, - const VecX_T>& x - ) { - VecX_T> dx = x; - for (int i = 0; i < x.size(); ++i) { dx(i) = -100.0 * x(i); } - return dx; - } + //// Stiff Decay function for dual numbers + //template + //VecX_T> stiffDecay( + // DualNumber_T /*t*/, + // const VecX_T>& x + //) { + // VecX_T> dx = x; + // for (int i = 0; i < x.size(); ++i) { dx(i) = -100.0 * x(i); } + // return dx; + //} - // Helper function to integrate from t=0 to t=T using N steps of the given step function, for dual numbers - template - VecX_T> integrateToTime( - StepFn stepFn, - const VecX_T>& x0, - Scalar T, - int N - ) { - DualNumber_T dt(T / Scalar(N)); - DualNumber_T t(0.0); - VecX_T> x = x0; - for (int i = 0; i < N; ++i) { x = stepFn(x, t, dt); t += dt; } - return x; - } + //// Helper function to integrate from t=0 to t=T using N steps of the given step function, for dual numbers + //template + //VecX_T> integrateToTime( + // StepFn stepFn, + // const VecX_T>& x0, + // Scalar T, + // int N + //) { + // DualNumber_T dt(T / Scalar(N)); + // DualNumber_T t(0.0); + // VecX_T> x = x0; + // for (int i = 0; i < N; ++i) { x = stepFn(x, t, dt); t += dt; } + // return x; + //} - // Helper function to verify that the exponential decay ODE is integrated correctly, and that the sensitivity of the solution with respect to the initial condition matches the analytical derivative. - template - void verifyExpDecaySensitivity( - StepFn stepFn, - double tol, - int N - ) { - VecX_T> x0(1); - x0(0) = DualNumber_T(1.0, { 1.0 }); // Initial condition with derivative 1.0 - VecX_T> r = integrateToTime(stepFn, x0, 1.0, N); - double expectedValue = std::exp(-1.0); - ASSERT_TRUE(std::abs(r(0).real - expectedValue) < tol, "Exponential decay error too large"); - ASSERT_TRUE(std::abs(r(0).dual[0] - expectedValue) < tol, "Exponential decay sensitivity error too large"); - ASSERT_TRUE(std::abs(r(0).dual[0]) > 0.0, "Sensitivity vanished during propagation"); - } + //// Helper function to verify that the exponential decay ODE is integrated correctly, and that the sensitivity of the solution with respect to the initial condition matches the analytical derivative. + //template + //void verifyExpDecaySensitivity( + // StepFn stepFn, + // double tol, + // int N + //) { + // VecX_T> x0(1); + // x0(0) = DualNumber_T(1.0, { 1.0 }); // Initial condition with derivative 1.0 + // VecX_T> r = integrateToTime(stepFn, x0, 1.0, N); + // double expectedValue = std::exp(-1.0); + // ASSERT_TRUE(std::abs(r(0).real - expectedValue) < tol, "Exponential decay error too large"); + // ASSERT_TRUE(std::abs(r(0).dual[0] - expectedValue) < tol, "Exponential decay sensitivity error too large"); + // ASSERT_TRUE(std::abs(r(0).dual[0]) > 0.0, "Sensitivity vanished during propagation"); + //} - // Helper function to verify that a harmonic oscillator ODE is integrated correctly, and that the sensitivity of the solution with respect to the initial condition matches the analytical derivative. - template - void verifyHarmonicOscillatorSensitivity( - StepFn stepFn, - double tol, - int N - ) { - VecX_T> x0(2); - x0(0) = DualNumber_T(1.0, { 1.0, 0.0 }); // Initial position with sensitivity - x0(1) = DualNumber_T(0.0, { 0.0, 1.0 }); // Initial velocity with sensitivity - VecX_T> r = integrateToTime(stepFn, x0, TWO_PI_d, N); - double expectedPosition = std::cos(TWO_PI_d); - double expectedVelocity = -std::sin(TWO_PI_d); - double expectedVelocitySens = std::cos(TWO_PI_d); - ASSERT_TRUE(std::abs(r(0).real - expectedPosition) < tol, "Harmonic oscillator position error too large"); - ASSERT_TRUE(std::abs(r(1).real - expectedVelocity) < tol, "Harmonic oscillator velocity error too large"); - ASSERT_TRUE(std::abs(r(0).dual[0] - expectedPosition) < tol, "Harmonic oscillator position sensitivity error too large"); - ASSERT_TRUE(std::abs(r(1).dual[1] - expectedVelocitySens) < tol, "Harmonic oscillator velocity sensitivity error too large"); - ASSERT_TRUE(std::abs(r(0).dual[0]) > 0.0, "Position sensitivity vanished during propagation"); - ASSERT_TRUE(std::abs(r(1).dual[1]) > 0.0, "Velocity sensitivity vanished during propagation"); - } - - // AD Implicit Euler Tests - // Test case for the GLRK2 method on the exponenital decay ODE. - TEST("AD Implicit Euler Method", ImplicitEuler_ExponentialDecay) { - VecX_T> x(1); - x(0) = DualNumber_T(1.0, { 0.0 }); - double T = 1.0; - DualNumber_T dt(T / 1000.0, { 0.0 }); - DualNumber_T t(0.0, { 0.0 }); - for (int i = 0; i < 1000; ++i) { - x = integrator.implicitEuler(x, t, dt, expDecay); - t += dt; - } - ASSERT_TRUE(std::abs(x(0).real - std::exp(-1.0)) < tol_low, "Implicit Euler exponential decay error too large"); - } - // Test case for the implicit Euler method on the exponential decay ODE. - TEST("AD Implicit Euler Method", ImplicitEuler_ExponentialDecaySensitivity) { - auto stepFn = [&]( - const VecX_T>& x, - DualNumber_T t, - DualNumber_T dt - ) { - return integrator.implicitEuler(x, t, dt, expDecay); - }; - verifyExpDecaySensitivity(stepFn, 1e-2, 1000); - } - // Test case for the Implicit Midpoint method on the linear decay ODE. - TEST("AD Implicit Midpoint Method", ImplicitEuler_LinearDecay) { - VecX_T> x(1); - x(0) = DualNumber_T(1.0, { 0.0 }); - double T = 0.5; - DualNumber_T dt(T / 500.0); - DualNumber_T t(0.0); - for (int i = 0; i < 500; ++i) { - x = integrator.implicitEuler(x, t, dt, linearDecay, 50, 1e-6); - t += dt; - } - ASSERT_TRUE(std::abs(x(0).real - std::exp(-1.0)) < tol_low, "Implicit Euler linear decay error too large"); - } - // Test case for the implicit Euler method on the linear decay ODE. - TEST("AD Implicit Euler Method", ImplicitEuler_LinearDecaySensitivity) { - auto stepFn = [&]( - const VecX_T>& x, - DualNumber_T t, - DualNumber_T dt - ) { - return integrator.implicitEuler(x, t, dt, linearDecay); - }; - verifyExpDecaySensitivity(stepFn, 1e-2, 1000); - } - // Test case for the Implicit Euler method on the stiff decay ODE. - TEST("AD Implicit Euler Method", ImplicitEuler_StiffDecay) { - VecX_T> x(1); - x(0) = DualNumber_T(1.0, { 0.0 }); - double T = 0.5; - DualNumber_T dt(T / 500.0); - DualNumber_T t(0.0); - double expected = std::exp(-50.0); - auto stiff_f = [](auto t, const auto& x) { - auto dx = x; - dx(0) = -100.0 * x(0); - return dx; - }; - for (int i = 0; i < 500; ++i) { - x = integrator.implicitEuler(x, t, dt, stiff_f); - t += dt; - } - ASSERT_TRUE(std::isfinite(x(0).real), "Implicit Euler produced a non-finite result."); - ASSERT_TRUE(x(0).real < 1e-12, "Implicit Euler did not sufficiently damp stiff mode"); - } - // Test case for the implicit Euler method on the stiff decay ODE. - TEST("AD Implicit Euler Method", ImplicitEuler_StiffDecaySensitivity) { - auto stepFn = [&]( - const VecX_T>& x, - DualNumber_T t, - DualNumber_T dt - ) { - return integrator.implicitEuler(x, t, dt, stiffDecay); - }; - verifyExpDecaySensitivity(stepFn, 1e-2, 1000); - } - // Test large step performance of implicit midpoint - TEST("AD Implicit Euler Method", ImplicitEuler_Stability_LargeStep) { - VecX_T> x(1); - x(0) = DualNumber_T(1.0, { 1.0 }); - double T = 1.0; - DualNumber_T t(0.0); - DualNumber_T dt(1.0 / 10.0); - DualNumber_T prev = x(0); - for (int i = 0; i < 10; ++i) { - x = integrator.implicitEuler(x, t, dt, stiffDecay); - t += dt; - ASSERT_TRUE(std::abs(x(0).real) < std::abs(prev.real), "Not monotone"); - ASSERT_TRUE(std::abs(x(0).dual[0]) < std::abs(prev.dual[0]), "Not strictly decaying"); - prev = x(0); - } - } + //// Helper function to verify that a harmonic oscillator ODE is integrated correctly, and that the sensitivity of the solution with respect to the initial condition matches the analytical derivative. + //template + //void verifyHarmonicOscillatorSensitivity( + // StepFn stepFn, + // double tol, + // int N + //) { + // VecX_T> x0(2); + // x0(0) = DualNumber_T(1.0, { 1.0, 0.0 }); // Initial position with sensitivity + // x0(1) = DualNumber_T(0.0, { 0.0, 1.0 }); // Initial velocity with sensitivity + // VecX_T> r = integrateToTime(stepFn, x0, TWO_PI_d, N); + // double expectedPosition = std::cos(TWO_PI_d); + // double expectedVelocity = -std::sin(TWO_PI_d); + // double expectedVelocitySens = std::cos(TWO_PI_d); + // ASSERT_TRUE(std::abs(r(0).real - expectedPosition) < tol, "Harmonic oscillator position error too large"); + // ASSERT_TRUE(std::abs(r(1).real - expectedVelocity) < tol, "Harmonic oscillator velocity error too large"); + // ASSERT_TRUE(std::abs(r(0).dual[0] - expectedPosition) < tol, "Harmonic oscillator position sensitivity error too large"); + // ASSERT_TRUE(std::abs(r(1).dual[1] - expectedVelocitySens) < tol, "Harmonic oscillator velocity sensitivity error too large"); + // ASSERT_TRUE(std::abs(r(0).dual[0]) > 0.0, "Position sensitivity vanished during propagation"); + // ASSERT_TRUE(std::abs(r(1).dual[1]) > 0.0, "Velocity sensitivity vanished during propagation"); + //} + // + //// AD Implicit Euler Tests + //// Test case for the GLRK2 method on the exponenital decay ODE. + //TEST("AD Implicit Euler Method", ImplicitEuler_ExponentialDecay) { + // VecX_T> x(1); + // x(0) = DualNumber_T(1.0, { 0.0 }); + // double T = 1.0; + // DualNumber_T dt(T / 1000.0, { 0.0 }); + // DualNumber_T t(0.0, { 0.0 }); + // for (int i = 0; i < 1000; ++i) { + // x = integrator.implicitEuler(x, t, dt, expDecay); + // t += dt; + // } + // ASSERT_TRUE(std::abs(x(0).real - std::exp(-1.0)) < tol_low, "Implicit Euler exponential decay error too large"); + //} + //// Test case for the implicit Euler method on the exponential decay ODE. + //TEST("AD Implicit Euler Method", ImplicitEuler_ExponentialDecaySensitivity) { + // auto stepFn = [&]( + // const VecX_T>& x, + // DualNumber_T t, + // DualNumber_T dt + // ) { + // return integrator.implicitEuler(x, t, dt, expDecay); + // }; + // verifyExpDecaySensitivity(stepFn, 1e-2, 1000); + //} + //// Test case for the Implicit Midpoint method on the linear decay ODE. + //TEST("AD Implicit Midpoint Method", ImplicitEuler_LinearDecay) { + // VecX_T> x(1); + // x(0) = DualNumber_T(1.0, { 0.0 }); + // double T = 0.5; + // DualNumber_T dt(T / 500.0); + // DualNumber_T t(0.0); + // for (int i = 0; i < 500; ++i) { + // x = integrator.implicitEuler(x, t, dt, linearDecay, 50, 1e-6); + // t += dt; + // } + // ASSERT_TRUE(std::abs(x(0).real - std::exp(-1.0)) < tol_low, "Implicit Euler linear decay error too large"); + //} + //// Test case for the implicit Euler method on the linear decay ODE. + //TEST("AD Implicit Euler Method", ImplicitEuler_LinearDecaySensitivity) { + // auto stepFn = [&]( + // const VecX_T>& x, + // DualNumber_T t, + // DualNumber_T dt + // ) { + // return integrator.implicitEuler(x, t, dt, linearDecay); + // }; + // verifyExpDecaySensitivity(stepFn, 1e-2, 1000); + //} + //// Test case for the Implicit Euler method on the stiff decay ODE. + //TEST("AD Implicit Euler Method", ImplicitEuler_StiffDecay) { + // VecX_T> x(1); + // x(0) = DualNumber_T(1.0, { 0.0 }); + // double T = 0.5; + // DualNumber_T dt(T / 500.0); + // DualNumber_T t(0.0); + // double expected = std::exp(-50.0); + // auto stiff_f = [](auto t, const auto& x) { + // auto dx = x; + // dx(0) = -100.0 * x(0); + // return dx; + // }; + // for (int i = 0; i < 500; ++i) { + // x = integrator.implicitEuler(x, t, dt, stiff_f); + // t += dt; + // } + // ASSERT_TRUE(std::isfinite(x(0).real), "Implicit Euler produced a non-finite result."); + // ASSERT_TRUE(x(0).real < 1e-12, "Implicit Euler did not sufficiently damp stiff mode"); + //} + //// Test case for the implicit Euler method on the stiff decay ODE. + //TEST("AD Implicit Euler Method", ImplicitEuler_StiffDecaySensitivity) { + // auto stepFn = [&]( + // const VecX_T>& x, + // DualNumber_T t, + // DualNumber_T dt + // ) { + // return integrator.implicitEuler(x, t, dt, stiffDecay); + // }; + // verifyExpDecaySensitivity(stepFn, 1e-2, 1000); + //} + //// Test large step performance of implicit midpoint + //TEST("AD Implicit Euler Method", ImplicitEuler_Stability_LargeStep) { + // VecX_T> x(1); + // x(0) = DualNumber_T(1.0, { 1.0 }); + // double T = 1.0; + // DualNumber_T t(0.0); + // DualNumber_T dt(1.0 / 10.0); + // DualNumber_T prev = x(0); + // for (int i = 0; i < 10; ++i) { + // x = integrator.implicitEuler(x, t, dt, stiffDecay); + // t += dt; + // ASSERT_TRUE(std::abs(x(0).real) < std::abs(prev.real), "Not monotone"); + // ASSERT_TRUE(std::abs(x(0).dual[0]) < std::abs(prev.dual[0]), "Not strictly decaying"); + // prev = x(0); + // } + //} - // AD Implicit Midpoint Tests - // Test case for the Implicit Midpoint method on the exponenital decay ODE. - TEST("AD Implicit Midpoint Method", ImplicitMidpoint_ExponentialDecay) { - VecX_T> x(1); - x(0) = DualNumber_T(1.0, { 0.0 }); - double T = 1.0; - DualNumber_T dt(T / 1000.0, { 0.0 }); - DualNumber_T t(0.0, { 0.0 }); - for (int i = 0; i < 1000; ++i) { - x = integrator.implicitMidpoint(x, t, dt, expDecay); - t += dt; - } - ASSERT_TRUE(std::abs(x(0).real - std::exp(-1.0)) < tol_low, "Implicit Midpoint exponential decay error too large"); - } - // Test case for the implicit midpoint method on the exponential decay ODE. - TEST("AD Implicit Midpoint Method", ImplicitMidpoint_ExponentialDecaySensitivity) { - auto stepFn = [&]( - const VecX_T>& x, - DualNumber_T t, - DualNumber_T dt - ) { - return integrator.implicitMidpoint(x, t, dt, expDecay); - }; - verifyExpDecaySensitivity(stepFn, 1e-3, 1000); - } - // Test case for the Implicit Midpoint method on the harmonic oscillator ODE. - TEST("AD Implicit Midpoint Method", ImplicitMidpoint_HarmonicOscillator_EnergyPreservation) { - VecX_T> x(2); - x(0) = DualNumber_T(1.0, { 0.0, 0.0 }); - x(1) = DualNumber_T(0.0, { 0.0, 0.0 }); - double E0 = 0.5 * (x(0).real * x(0).real + x(1).real * x(1).real); - double T = 5.0; - DualNumber_T dt(T / 2500.0); - DualNumber_T t(0.0); - for (int i = 0; i < 2500; ++i) { - x = integrator.implicitMidpoint(x, t, dt, harmonicOsc); - t += dt; - } - double Ef = 0.5 * (x(0).real * x(0).real + x(1).real * x(1).real); - double drift = std::abs(Ef - E0); - ASSERT_TRUE(drift < tol_high * (1.0 + E0), "Implicit Midpoint energy drift too large for SHO"); - } - // Test case for the implicit midpoint method on the harmonic oscillator ODE. - TEST("AD Implicit Midpoint Method", ImplicitMidpoint_HarmonicOscillatorSensitivity) { - auto stepFn = [&]( - const VecX_T>& x, - DualNumber_T t, - DualNumber_T dt - ) { - return integrator.implicitMidpoint(x, t, dt, harmonicOsc); - }; - verifyHarmonicOscillatorSensitivity(stepFn, 1e-3, 1000); - } - // Test case for the Implicit Midpoint method on the linear decay ODE. - TEST("AD Implicit Midpoint Method", ImplicitMidpoint_LinearDecay) { - VecX_T> x(1); - x(0) = DualNumber_T(1.0, { 0.0 }); - double T = 0.5; - DualNumber_T dt(T / 500.0); - DualNumber_T t(0.0); - for (int i = 0; i < 500; ++i) { - x = integrator.implicitMidpoint(x, t, dt, linearDecay); - t += dt; - } - ASSERT_TRUE(std::abs(x(0).real - std::exp(-1.0)) < tol_low, "Implicit Midpoint linear decay error too large"); - } - // Test case for the implicit midpoint method on the linear decay ODE. - TEST("AD Implicit Midpoint Method", ImplicitMidpoint_LinearDecaySensitivity) { - auto stepFn = [&]( - const VecX_T>& x, - DualNumber_T t, - DualNumber_T dt - ) { - return integrator.implicitMidpoint(x, t, dt, linearDecay); - }; - verifyExpDecaySensitivity(stepFn, 1e-3, 1000); - } - // Test case for the Implicit Midpoint method on the stiff decay ODE. - TEST("AD Implicit Midpoint Method", ImplicitMidpoint_StiffDecay) { - VecX_T> x(1); - x(0) = DualNumber_T(1.0, { 0.0 }); - double T = 0.5; - DualNumber_T dt(T / 500.0); - DualNumber_T t(0.0); - double expected = std::exp(-50.0); - auto stiff_f = [](auto t, const auto& x) { - auto dx = x; - dx(0) = -100.0 * x(0); - return dx; - }; - for (int i = 0; i < 500; ++i) { - x = integrator.implicitMidpoint(x, t, dt, stiff_f); - t += dt; - } - ASSERT_TRUE(std::isfinite(x(0).real), "Implicit Midpoint produced a non-finite result."); - ASSERT_TRUE(x(0).real < 1e-12, "Implicit Midpoint did not sufficiently damp stiff mode"); - } - // Test case for the implicit midpoint method on the stiff decay ODE. - TEST("AD Implicit Midpoint Method", ImplicitMidpoint_StiffDecaySensitivity) { - auto stepFn = [&]( - const VecX_T>& x, - DualNumber_T t, - DualNumber_T dt - ) { - return integrator.implicitMidpoint(x, t, dt, stiffDecay); - }; - verifyExpDecaySensitivity(stepFn, 1e-3, 1000); - } - // Test large step performance of implicit midpoint - TEST("AD Implicit Midpoint Method", ImplicitMidpoint_Stability_LargeStep) { - VecX_T> x(1); - x(0) = DualNumber_T(1.0, { 1.0 }); - double T = 1.0; - DualNumber_T t(0.0); - DualNumber_T dt(1.0 / 10.0); - DualNumber_T prev = x(0); - for (int i = 0; i < 10; ++i) { - x = integrator.implicitMidpoint(x, t, dt, stiffDecay); - t += dt; - ASSERT_TRUE(std::abs(x(0).real) < std::abs(prev.real), "Not monotone"); - ASSERT_TRUE(std::abs(x(0).dual[0]) < std::abs(prev.dual[0]), "Not strictly decaying"); - prev = x(0); - } - } + //// AD Implicit Midpoint Tests + //// Test case for the Implicit Midpoint method on the exponenital decay ODE. + //TEST("AD Implicit Midpoint Method", ImplicitMidpoint_ExponentialDecay) { + // VecX_T> x(1); + // x(0) = DualNumber_T(1.0, { 0.0 }); + // double T = 1.0; + // DualNumber_T dt(T / 1000.0, { 0.0 }); + // DualNumber_T t(0.0, { 0.0 }); + // for (int i = 0; i < 1000; ++i) { + // x = integrator.implicitMidpoint(x, t, dt, expDecay); + // t += dt; + // } + // ASSERT_TRUE(std::abs(x(0).real - std::exp(-1.0)) < tol_low, "Implicit Midpoint exponential decay error too large"); + //} + //// Test case for the implicit midpoint method on the exponential decay ODE. + //TEST("AD Implicit Midpoint Method", ImplicitMidpoint_ExponentialDecaySensitivity) { + // auto stepFn = [&]( + // const VecX_T>& x, + // DualNumber_T t, + // DualNumber_T dt + // ) { + // return integrator.implicitMidpoint(x, t, dt, expDecay); + // }; + // verifyExpDecaySensitivity(stepFn, 1e-3, 1000); + //} + //// Test case for the Implicit Midpoint method on the harmonic oscillator ODE. + //TEST("AD Implicit Midpoint Method", ImplicitMidpoint_HarmonicOscillator_EnergyPreservation) { + // VecX_T> x(2); + // x(0) = DualNumber_T(1.0, { 0.0, 0.0 }); + // x(1) = DualNumber_T(0.0, { 0.0, 0.0 }); + // double E0 = 0.5 * (x(0).real * x(0).real + x(1).real * x(1).real); + // double T = 5.0; + // DualNumber_T dt(T / 2500.0); + // DualNumber_T t(0.0); + // for (int i = 0; i < 2500; ++i) { + // x = integrator.implicitMidpoint(x, t, dt, harmonicOsc); + // t += dt; + // } + // double Ef = 0.5 * (x(0).real * x(0).real + x(1).real * x(1).real); + // double drift = std::abs(Ef - E0); + // ASSERT_TRUE(drift < tol_high * (1.0 + E0), "Implicit Midpoint energy drift too large for SHO"); + //} + //// Test case for the implicit midpoint method on the harmonic oscillator ODE. + //TEST("AD Implicit Midpoint Method", ImplicitMidpoint_HarmonicOscillatorSensitivity) { + // auto stepFn = [&]( + // const VecX_T>& x, + // DualNumber_T t, + // DualNumber_T dt + // ) { + // return integrator.implicitMidpoint(x, t, dt, harmonicOsc); + // }; + // verifyHarmonicOscillatorSensitivity(stepFn, 1e-3, 1000); + //} + //// Test case for the Implicit Midpoint method on the linear decay ODE. + //TEST("AD Implicit Midpoint Method", ImplicitMidpoint_LinearDecay) { + // VecX_T> x(1); + // x(0) = DualNumber_T(1.0, { 0.0 }); + // double T = 0.5; + // DualNumber_T dt(T / 500.0); + // DualNumber_T t(0.0); + // for (int i = 0; i < 500; ++i) { + // x = integrator.implicitMidpoint(x, t, dt, linearDecay); + // t += dt; + // } + // ASSERT_TRUE(std::abs(x(0).real - std::exp(-1.0)) < tol_low, "Implicit Midpoint linear decay error too large"); + //} + //// Test case for the implicit midpoint method on the linear decay ODE. + //TEST("AD Implicit Midpoint Method", ImplicitMidpoint_LinearDecaySensitivity) { + // auto stepFn = [&]( + // const VecX_T>& x, + // DualNumber_T t, + // DualNumber_T dt + // ) { + // return integrator.implicitMidpoint(x, t, dt, linearDecay); + // }; + // verifyExpDecaySensitivity(stepFn, 1e-3, 1000); + //} + //// Test case for the Implicit Midpoint method on the stiff decay ODE. + //TEST("AD Implicit Midpoint Method", ImplicitMidpoint_StiffDecay) { + // VecX_T> x(1); + // x(0) = DualNumber_T(1.0, { 0.0 }); + // double T = 0.5; + // DualNumber_T dt(T / 500.0); + // DualNumber_T t(0.0); + // double expected = std::exp(-50.0); + // auto stiff_f = [](auto t, const auto& x) { + // auto dx = x; + // dx(0) = -100.0 * x(0); + // return dx; + // }; + // for (int i = 0; i < 500; ++i) { + // x = integrator.implicitMidpoint(x, t, dt, stiff_f); + // t += dt; + // } + // ASSERT_TRUE(std::isfinite(x(0).real), "Implicit Midpoint produced a non-finite result."); + // ASSERT_TRUE(x(0).real < 1e-12, "Implicit Midpoint did not sufficiently damp stiff mode"); + //} + //// Test case for the implicit midpoint method on the stiff decay ODE. + //TEST("AD Implicit Midpoint Method", ImplicitMidpoint_StiffDecaySensitivity) { + // auto stepFn = [&]( + // const VecX_T>& x, + // DualNumber_T t, + // DualNumber_T dt + // ) { + // return integrator.implicitMidpoint(x, t, dt, stiffDecay); + // }; + // verifyExpDecaySensitivity(stepFn, 1e-3, 1000); + //} + //// Test large step performance of implicit midpoint + //TEST("AD Implicit Midpoint Method", ImplicitMidpoint_Stability_LargeStep) { + // VecX_T> x(1); + // x(0) = DualNumber_T(1.0, { 1.0 }); + // double T = 1.0; + // DualNumber_T t(0.0); + // DualNumber_T dt(1.0 / 10.0); + // DualNumber_T prev = x(0); + // for (int i = 0; i < 10; ++i) { + // x = integrator.implicitMidpoint(x, t, dt, stiffDecay); + // t += dt; + // ASSERT_TRUE(std::abs(x(0).real) < std::abs(prev.real), "Not monotone"); + // ASSERT_TRUE(std::abs(x(0).dual[0]) < std::abs(prev.dual[0]), "Not strictly decaying"); + // prev = x(0); + // } + //} - // AD GLRK2 Tests - // Test case for the GLRK2 method on the exponenital decay ODE. - TEST("AD GLRK2 Method", GLRK2_ExponentialDecay) { - VecX_T> x(1); - x(0) = DualNumber_T(1.0, { 0.0 }); - double T = 1.0; - DualNumber_T dt(T / 1000.0, { 0.0 }); - DualNumber_T t(0.0, { 0.0 }); - for (int i = 0; i < 1000; ++i) { - x = integrator.GLRK2(x, t, dt, expDecay, 50, 1e-6); - t += dt; - } - ASSERT_TRUE(std::abs(x(0).real - std::exp(-1.0)) < tol_low, "GLRK3 exponential decay error too large"); - } - // Test case for the GLRK2 method on the exponential decay ODE. - TEST("AD GLRK2 Method", GLRK2_ExponentialDecaySensitivity) { - auto stepFn = [&]( - const VecX_T>& x, - DualNumber_T t, - DualNumber_T dt - ) { - return integrator.GLRK2(x, t, dt, expDecay); - }; - verifyExpDecaySensitivity(stepFn, 1e-4, 1000); - } - // Test case for the GLRK2 method on the harmonic oscillator ODE. - TEST("AD GLRK2 Method", GLRK2_HarmonicOscillator_EnergyPreservation) { - VecX_T> x(2); - x(0) = DualNumber_T(1.0, { 0.0, 0.0 }); - x(1) = DualNumber_T(0.0, { 0.0, 0.0 }); - double E0 = 0.5 * (x(0).real * x(0).real + x(1).real * x(1).real); - double T = 5.0; - DualNumber_T dt(T / 2500.0); - DualNumber_T t(0.0); - for (int i = 0; i < 2500; ++i) { - x = integrator.GLRK2(x, t, dt, harmonicOsc, 50, 1e-6); - t += dt; - } - double Ef = 0.5 * (x(0).real * x(0).real + x(1).real * x(1).real); - double drift = std::abs(Ef - E0); - ASSERT_TRUE(drift < tol_high * (1.0 + E0), "GLRK2 energy drift too large for SHO"); - } - // Test case for the GLRK2 method on the harmonic oscillator ODE. - TEST("AD GLRK2 Method", GLRK2_HarmonicOscillatorSensitivity) { - auto stepFn = [&]( - const VecX_T>& x, - DualNumber_T t, - DualNumber_T dt - ) { - return integrator.GLRK2(x, t, dt, harmonicOsc); - }; - verifyHarmonicOscillatorSensitivity(stepFn, 1e-4, 1000); - } - // Test case for the GLRK2 method on the linear decay ODE. - TEST("AD GLRK2 Method", GLRK2_LinearDecay) { - VecX_T> x(1); - x(0) = DualNumber_T(1.0, { 0.0 }); - double T = 0.5; - DualNumber_T dt(T / 500.0); - DualNumber_T t(0.0); - for (int i = 0; i < 500; ++i) { - x = integrator.GLRK2(x, t, dt, linearDecay, 50, 1e-6); - t += dt; - } - ASSERT_TRUE(std::abs(x(0).real - std::exp(-1.0)) < tol_low, "GLRK2 linear decay error too large"); - } - // Test case for the GLRK2 method on the linear decay ODE. - TEST("AD GLRK2 Method", GLRK2_LinearDecaySensitivity) { - auto stepFn = [&]( - const VecX_T>& x, - DualNumber_T t, - DualNumber_T dt - ) { - return integrator.GLRK2(x, t, dt, linearDecay); - }; - verifyExpDecaySensitivity(stepFn, 1e-4, 1000); - } - // Test case for the GLRK2 method on the stiff decay ODE. - TEST("AD GLRK2 Method", GLRK2_StiffDecay) { - VecX_T> x(1); - x(0) = DualNumber_T(1.0, { 0.0 }); - double T = 0.5; - DualNumber_T dt(T / 500.0); - DualNumber_T t(0.0); - double expected = std::exp(-50.0); - auto stiff_f = [](auto t, const auto& x) { - auto dx = x; - dx(0) = -100.0 * x(0); - return dx; - }; - for (int i = 0; i < 500; ++i) { - x = integrator.GLRK2(x, t, dt, stiff_f, 50, 1e-10); - t += dt; - } - ASSERT_TRUE(std::isfinite(x(0).real), "GLRK2 produced a non-finite result."); - ASSERT_TRUE(x(0).real < 1e-12, "GLRK2 did not sufficiently damp stiff mode"); - } - // Test case for the GLRK2 method on the stiff decay ODE. - TEST("AD GLRK2 Method", GLRK2_StiffDecaySensitivity) { - auto stepFn = [&]( - const VecX_T>& x, - DualNumber_T t, - DualNumber_T dt - ) { - return integrator.GLRK2(x, t, dt, stiffDecay); - }; - verifyExpDecaySensitivity(stepFn, 1e-4, 1000); - } - // Test case for the GLRK2 method on the nonlinear decay ODE. - TEST("AD GLRK2 Method", GLRK2_NonlinearDecay) { - VecX_T> x(1); - x(0) = DualNumber_T(1.0, { 0.0 }); - double T = 0.5; - DualNumber_T dt(T / 500.0); - DualNumber_T t(0.0); - double expected = 1.0 / (1.0 + T); - auto nl_f = [](auto, const auto& x) { - auto dx = x; - dx(0) = -x(0) * x(0); - return dx; - }; - for (int i = 0; i < 500; ++i) { - x = integrator.GLRK2(x, t, dt, nl_f, 50, 1e-6); - t += dt; - } - double rel_err = std::abs(x(0).real - expected) / expected; - ASSERT_TRUE(rel_err < tol_low, "GLRK2 nonlinear decay error too large"); - } - // Test case for the GLRK2 method on the stiff nonlinear decay ODE. - TEST("AD GLRK2 Method", GLRK2_StiffNonlinearDecay) { - VecX_T> x(1); - x(0) = DualNumber_T(1.0, { 0.0 }); - double T = 0.5; - DualNumber_T dt(T / 500.0); - DualNumber_T t(0.0); - double expected = 1.0 / 51.0; - auto stiff_nl_f = [](auto t, const auto& x) { - auto dx = x; - dx(0) = -100.0 * x(0) - x(0) * x(0); - return dx; - }; - for (int i = 0; i < 500; ++i) { - x = integrator.GLRK2(x, t, dt, stiff_nl_f, 50, 1e-6); - t += dt; - } - double rel_err = std::abs(x(0).real - expected) / expected; - ASSERT_TRUE(rel_err < tol_low, "GLRK2 stiff nonlinear decay error too large"); - } - // Test large step performance of GLRK2 - TEST("AD GLRK2 Method", GLRK2_Stability_LargeStep) { - VecX_T> x(1); - x(0) = DualNumber_T(1.0, { 1.0 }); - double T = 1.0; - DualNumber_T t(0.0); - DualNumber_T dt(1.0 / 10.0); - DualNumber_T prev = x(0); - for (int i = 0; i < 10; ++i) { - x = integrator.GLRK2(x, t, dt, stiffDecay); - t += dt; - ASSERT_TRUE(std::abs(x(0).real) < std::abs(prev.real), "Not monotone"); - ASSERT_TRUE(std::abs(x(0).dual[0]) < std::abs(prev.dual[0]), "Not strictly decaying"); - prev = x(0); - } - } + //// AD GLRK2 Tests + //// Test case for the GLRK2 method on the exponenital decay ODE. + //TEST("AD GLRK2 Method", GLRK2_ExponentialDecay) { + // VecX_T> x(1); + // x(0) = DualNumber_T(1.0, { 0.0 }); + // double T = 1.0; + // DualNumber_T dt(T / 1000.0, { 0.0 }); + // DualNumber_T t(0.0, { 0.0 }); + // for (int i = 0; i < 1000; ++i) { + // x = integrator.GLRK2(x, t, dt, expDecay, 50, 1e-6); + // t += dt; + // } + // ASSERT_TRUE(std::abs(x(0).real - std::exp(-1.0)) < tol_low, "GLRK3 exponential decay error too large"); + //} + //// Test case for the GLRK2 method on the exponential decay ODE. + //TEST("AD GLRK2 Method", GLRK2_ExponentialDecaySensitivity) { + // auto stepFn = [&]( + // const VecX_T>& x, + // DualNumber_T t, + // DualNumber_T dt + // ) { + // return integrator.GLRK2(x, t, dt, expDecay); + // }; + // verifyExpDecaySensitivity(stepFn, 1e-4, 1000); + //} + //// Test case for the GLRK2 method on the harmonic oscillator ODE. + //TEST("AD GLRK2 Method", GLRK2_HarmonicOscillator_EnergyPreservation) { + // VecX_T> x(2); + // x(0) = DualNumber_T(1.0, { 0.0, 0.0 }); + // x(1) = DualNumber_T(0.0, { 0.0, 0.0 }); + // double E0 = 0.5 * (x(0).real * x(0).real + x(1).real * x(1).real); + // double T = 5.0; + // DualNumber_T dt(T / 2500.0); + // DualNumber_T t(0.0); + // for (int i = 0; i < 2500; ++i) { + // x = integrator.GLRK2(x, t, dt, harmonicOsc, 50, 1e-6); + // t += dt; + // } + // double Ef = 0.5 * (x(0).real * x(0).real + x(1).real * x(1).real); + // double drift = std::abs(Ef - E0); + // ASSERT_TRUE(drift < tol_high * (1.0 + E0), "GLRK2 energy drift too large for SHO"); + //} + //// Test case for the GLRK2 method on the harmonic oscillator ODE. + //TEST("AD GLRK2 Method", GLRK2_HarmonicOscillatorSensitivity) { + // auto stepFn = [&]( + // const VecX_T>& x, + // DualNumber_T t, + // DualNumber_T dt + // ) { + // return integrator.GLRK2(x, t, dt, harmonicOsc); + // }; + // verifyHarmonicOscillatorSensitivity(stepFn, 1e-4, 1000); + //} + //// Test case for the GLRK2 method on the linear decay ODE. + //TEST("AD GLRK2 Method", GLRK2_LinearDecay) { + // VecX_T> x(1); + // x(0) = DualNumber_T(1.0, { 0.0 }); + // double T = 0.5; + // DualNumber_T dt(T / 500.0); + // DualNumber_T t(0.0); + // for (int i = 0; i < 500; ++i) { + // x = integrator.GLRK2(x, t, dt, linearDecay, 50, 1e-6); + // t += dt; + // } + // ASSERT_TRUE(std::abs(x(0).real - std::exp(-1.0)) < tol_low, "GLRK2 linear decay error too large"); + //} + //// Test case for the GLRK2 method on the linear decay ODE. + //TEST("AD GLRK2 Method", GLRK2_LinearDecaySensitivity) { + // auto stepFn = [&]( + // const VecX_T>& x, + // DualNumber_T t, + // DualNumber_T dt + // ) { + // return integrator.GLRK2(x, t, dt, linearDecay); + // }; + // verifyExpDecaySensitivity(stepFn, 1e-4, 1000); + //} + //// Test case for the GLRK2 method on the stiff decay ODE. + //TEST("AD GLRK2 Method", GLRK2_StiffDecay) { + // VecX_T> x(1); + // x(0) = DualNumber_T(1.0, { 0.0 }); + // double T = 0.5; + // DualNumber_T dt(T / 500.0); + // DualNumber_T t(0.0); + // double expected = std::exp(-50.0); + // auto stiff_f = [](auto t, const auto& x) { + // auto dx = x; + // dx(0) = -100.0 * x(0); + // return dx; + // }; + // for (int i = 0; i < 500; ++i) { + // x = integrator.GLRK2(x, t, dt, stiff_f, 50, 1e-10); + // t += dt; + // } + // ASSERT_TRUE(std::isfinite(x(0).real), "GLRK2 produced a non-finite result."); + // ASSERT_TRUE(x(0).real < 1e-12, "GLRK2 did not sufficiently damp stiff mode"); + //} + //// Test case for the GLRK2 method on the stiff decay ODE. + //TEST("AD GLRK2 Method", GLRK2_StiffDecaySensitivity) { + // auto stepFn = [&]( + // const VecX_T>& x, + // DualNumber_T t, + // DualNumber_T dt + // ) { + // return integrator.GLRK2(x, t, dt, stiffDecay); + // }; + // verifyExpDecaySensitivity(stepFn, 1e-4, 1000); + //} + //// Test case for the GLRK2 method on the nonlinear decay ODE. + //TEST("AD GLRK2 Method", GLRK2_NonlinearDecay) { + // VecX_T> x(1); + // x(0) = DualNumber_T(1.0, { 0.0 }); + // double T = 0.5; + // DualNumber_T dt(T / 500.0); + // DualNumber_T t(0.0); + // double expected = 1.0 / (1.0 + T); + // auto nl_f = [](auto, const auto& x) { + // auto dx = x; + // dx(0) = -x(0) * x(0); + // return dx; + // }; + // for (int i = 0; i < 500; ++i) { + // x = integrator.GLRK2(x, t, dt, nl_f, 50, 1e-6); + // t += dt; + // } + // double rel_err = std::abs(x(0).real - expected) / expected; + // ASSERT_TRUE(rel_err < tol_low, "GLRK2 nonlinear decay error too large"); + //} + //// Test case for the GLRK2 method on the stiff nonlinear decay ODE. + //TEST("AD GLRK2 Method", GLRK2_StiffNonlinearDecay) { + // VecX_T> x(1); + // x(0) = DualNumber_T(1.0, { 0.0 }); + // double T = 0.5; + // DualNumber_T dt(T / 500.0); + // DualNumber_T t(0.0); + // double expected = 1.0 / 51.0; + // auto stiff_nl_f = [](auto t, const auto& x) { + // auto dx = x; + // dx(0) = -100.0 * x(0) - x(0) * x(0); + // return dx; + // }; + // for (int i = 0; i < 500; ++i) { + // x = integrator.GLRK2(x, t, dt, stiff_nl_f, 50, 1e-6); + // t += dt; + // } + // double rel_err = std::abs(x(0).real - expected) / expected; + // ASSERT_TRUE(rel_err < tol_low, "GLRK2 stiff nonlinear decay error too large"); + //} + //// Test large step performance of GLRK2 + //TEST("AD GLRK2 Method", GLRK2_Stability_LargeStep) { + // VecX_T> x(1); + // x(0) = DualNumber_T(1.0, { 1.0 }); + // double T = 1.0; + // DualNumber_T t(0.0); + // DualNumber_T dt(1.0 / 10.0); + // DualNumber_T prev = x(0); + // for (int i = 0; i < 10; ++i) { + // x = integrator.GLRK2(x, t, dt, stiffDecay); + // t += dt; + // ASSERT_TRUE(std::abs(x(0).real) < std::abs(prev.real), "Not monotone"); + // ASSERT_TRUE(std::abs(x(0).dual[0]) < std::abs(prev.dual[0]), "Not strictly decaying"); + // prev = x(0); + // } + //} - // AD GLRK3 Tests - // Test case for the GLRK3 method on the exponenital decay ODE. - TEST("AD GLRK3 Method", GLRK3_ExponentialDecay) { - VecX_T> x(1); - x(0) = DualNumber_T(1.0, { 0.0 }); - double T = 1.0; - DualNumber_T dt(T / 1000.0, { 0.0 }); - DualNumber_T t(0.0, { 0.0 }); - for (int i = 0; i < 1000; ++i) { - x = integrator.GLRK3(x, t, dt, expDecay, 80, 1e-7); - t += dt; - } - ASSERT_TRUE(std::abs(x(0).real - std::exp(-1.0)) < tol_low, "GLRK3 exponential decay error too large"); - } - // Test case for the GLRK3 method on the exponential decay ODE sensitivity. - TEST("AD GLRK3 Method", GLRK3_ExponentialDecaySensitivity) { - auto stepFn = [&]( - const VecX_T>& x, - DualNumber_T t, - DualNumber_T dt - ) { - return integrator.GLRK3(x, t, dt, expDecay); - }; - verifyExpDecaySensitivity(stepFn, 1e-4, 1000); - } - // Test case for the GLRK3 method on the harmonic oscillator ODE. - TEST("AD GLRK3 Method", GLRK3_HarmonicOscillator_EnergyPreservation) { - VecX_T> x(2); - x(0) = DualNumber_T(1.0, { 0.0, 0.0 }); - x(1) = DualNumber_T(0.0, { 0.0, 0.0 }); - double E0 = 0.5 * (x(0).real * x(0).real + x(1).real * x(1).real); - double T = 5.0; - DualNumber_T dt(T / 2500.0); - DualNumber_T t(0.0); - for (int i = 0; i < 2500; ++i) { - x = integrator.GLRK3(x, t, dt, harmonicOsc, 80, 1e-7); - t += dt; - } - double Ef = 0.5 * (x(0).real * x(0).real + x(1).real * x(1).real); - double drift = std::abs(Ef - E0); - ASSERT_TRUE(drift < tol_high * (1.0 + E0), "GLRK3 energy drift too large for SHO"); - } - // Test case for the GLRK3 method on the harmonic oscillator ODE. - TEST("AD GLRK3 Method", GLRK3_HarmonicOscillatorSensitivity) { - auto stepFn = [&]( - const VecX_T>& x, - DualNumber_T t, - DualNumber_T dt - ) { - return integrator.GLRK3(x, t, dt, harmonicOsc); - }; - verifyHarmonicOscillatorSensitivity(stepFn, 1e-4, 1000); - } - // Test case for the GLRK3 method on the linear decay ODE. - TEST("AD GLRK3 Method", GLRK3_LinearDecay) { - VecX_T> x(1); - x(0) = DualNumber_T(1.0, { 0.0 }); - double T = 0.5; - DualNumber_T dt(T / 500.0); - DualNumber_T t(0.0); - for (int i = 0; i < 500; ++i) { - x = integrator.GLRK3(x, t, dt, linearDecay, 80, 1e-7); - t += dt; - } - ASSERT_TRUE(std::abs(x(0).real - std::exp(-1.0)) < tol_low, "GLRK3 linear decay error too large"); - } - // Test case for the GLRK3 method on the linear decay ODE. - TEST("AD GLRK3 Method", GLRK3_LinearDecaySensitivity) { - auto stepFn = [&]( - const VecX_T>& x, - DualNumber_T t, - DualNumber_T dt - ) { - return integrator.GLRK3(x, t, dt, linearDecay); - }; - verifyExpDecaySensitivity(stepFn, 1e-12, 1000); - } - // Test case for the GLRK3 method on the stiff decay ODE. - TEST("AD GLRK3 Method", GLRK3_StiffDecay) { - VecX_T> x(1); - x(0) = DualNumber_T(1.0, { 0.0 }); - double T = 0.5; - DualNumber_T dt(T / 500.0); - DualNumber_T t(0.0); - double expected = std::exp(-50.0); - auto stiff_f = [](auto t, const auto& x) { - auto dx = x; - dx(0) = -100.0 * x(0); - return dx; - }; - for (int i = 0; i < 500; ++i) { - x = integrator.GLRK3(x, t, dt, stiff_f, 150, 1e-12); - t += dt; - } - ASSERT_TRUE(std::isfinite(x(0).real), "GLRK3 produced a non-finite result."); - ASSERT_TRUE(x(0).real < 1e-14, "GLRK3 did not sufficiently damp stiff mode"); - } - // Test case for the GLRK3 method on the stiff decay ODE. - TEST("AD GLRK3 Method", GLRK3_StiffDecaySensitivity) { - auto stepFn = [&]( - const VecX_T>& x, - DualNumber_T t, - DualNumber_T dt - ) { - return integrator.GLRK3(x, t, dt, stiffDecay); - }; - verifyExpDecaySensitivity(stepFn, 1e-12, 1000); - } - // Test case for the GLRK3 method on the nonlinear decay ODE. - TEST("AD GLRK3 Method", GLRK3_NonlinearDecay) { - VecX_T> x(1); - x(0) = DualNumber_T(1.0, { 0.0 }); - double T = 0.5; - DualNumber_T dt(T / 500.0); - DualNumber_T t(0.0); - double expected = 1.0 / (1.0 + T); - auto nl_f = [](auto t, const auto& x) { - auto dx = x; - dx(0) = -x(0) * x(0); - return dx; - }; - for (int i = 0; i < 500; ++i) { - x = integrator.GLRK3(x, t, dt, nl_f, 80, 1e-7); - t += dt; - } - double rel_err = std::abs(x(0).real -expected) / expected; - ASSERT_TRUE(rel_err < tol_low, "GLRK3 nonlinear decay error too large"); - } - // Test case for the GLRK3 method on the stiff nonlinear decay ODE. - TEST("AD GLRK3 Method", GLRK3_StiffNonlinearDecay) { - VecX_T> x(1); - x(0) = DualNumber_T(1.0, { 0.0 }); - double T = 0.5; - DualNumber_T dt(T / 500.0); - DualNumber_T t(0.0); - double expected = 1.0 / 51.0; - auto stiff_nl_f = [](auto t, const auto& x) { - auto dx = x.eval(); - dx(0) = -100.0 * x(0) - x(0) * x(0); - return dx; - }; - for (int i = 0; i < 500; ++i) { - x = integrator.GLRK3(x, t, dt, stiff_nl_f, 80, 1e-7); - t += dt; - } - double rel_err = std::abs(x(0).real - expected) / expected; - ASSERT_TRUE(rel_err < tol_low, "GLRK3 stiff nonlinear decay error too large"); - } - // Test case for the GLRK3 method on the stiff decay ODE. - TEST("AD GLRK3 Method", GLRK3_Stability_LargeStep) { - VecX_T> x(1); - x(0) = DualNumber_T(1.0, { 1.0 }); - double T = 1.0; - DualNumber_T t(0.0); - DualNumber_T dt(1.0 / 10.0); - DualNumber_T prev = x(0); - for (int i = 0; i < 10; ++i) { - x = integrator.GLRK3(x, t, dt, stiffDecay); - t += dt; - ASSERT_TRUE(std::abs(x(0).real) < std::abs(prev.real), "Not monotone"); - ASSERT_TRUE(std::abs(x(0).dual[0]) < std::abs(prev.dual[0]), "Not strictly decaying"); - prev = x(0); - } - } + //// AD GLRK3 Tests + //// Test case for the GLRK3 method on the exponenital decay ODE. + //TEST("AD GLRK3 Method", GLRK3_ExponentialDecay) { + // VecX_T> x(1); + // x(0) = DualNumber_T(1.0, { 0.0 }); + // double T = 1.0; + // DualNumber_T dt(T / 1000.0, { 0.0 }); + // DualNumber_T t(0.0, { 0.0 }); + // for (int i = 0; i < 1000; ++i) { + // x = integrator.GLRK3(x, t, dt, expDecay, 80, 1e-7); + // t += dt; + // } + // ASSERT_TRUE(std::abs(x(0).real - std::exp(-1.0)) < tol_low, "GLRK3 exponential decay error too large"); + //} + //// Test case for the GLRK3 method on the exponential decay ODE sensitivity. + //TEST("AD GLRK3 Method", GLRK3_ExponentialDecaySensitivity) { + // auto stepFn = [&]( + // const VecX_T>& x, + // DualNumber_T t, + // DualNumber_T dt + // ) { + // return integrator.GLRK3(x, t, dt, expDecay); + // }; + // verifyExpDecaySensitivity(stepFn, 1e-4, 1000); + //} + //// Test case for the GLRK3 method on the harmonic oscillator ODE. + //TEST("AD GLRK3 Method", GLRK3_HarmonicOscillator_EnergyPreservation) { + // VecX_T> x(2); + // x(0) = DualNumber_T(1.0, { 0.0, 0.0 }); + // x(1) = DualNumber_T(0.0, { 0.0, 0.0 }); + // double E0 = 0.5 * (x(0).real * x(0).real + x(1).real * x(1).real); + // double T = 5.0; + // DualNumber_T dt(T / 2500.0); + // DualNumber_T t(0.0); + // for (int i = 0; i < 2500; ++i) { + // x = integrator.GLRK3(x, t, dt, harmonicOsc, 80, 1e-7); + // t += dt; + // } + // double Ef = 0.5 * (x(0).real * x(0).real + x(1).real * x(1).real); + // double drift = std::abs(Ef - E0); + // ASSERT_TRUE(drift < tol_high * (1.0 + E0), "GLRK3 energy drift too large for SHO"); + //} + //// Test case for the GLRK3 method on the harmonic oscillator ODE. + //TEST("AD GLRK3 Method", GLRK3_HarmonicOscillatorSensitivity) { + // auto stepFn = [&]( + // const VecX_T>& x, + // DualNumber_T t, + // DualNumber_T dt + // ) { + // return integrator.GLRK3(x, t, dt, harmonicOsc); + // }; + // verifyHarmonicOscillatorSensitivity(stepFn, 1e-4, 1000); + //} + //// Test case for the GLRK3 method on the linear decay ODE. + //TEST("AD GLRK3 Method", GLRK3_LinearDecay) { + // VecX_T> x(1); + // x(0) = DualNumber_T(1.0, { 0.0 }); + // double T = 0.5; + // DualNumber_T dt(T / 500.0); + // DualNumber_T t(0.0); + // for (int i = 0; i < 500; ++i) { + // x = integrator.GLRK3(x, t, dt, linearDecay, 80, 1e-7); + // t += dt; + // } + // ASSERT_TRUE(std::abs(x(0).real - std::exp(-1.0)) < tol_low, "GLRK3 linear decay error too large"); + //} + //// Test case for the GLRK3 method on the linear decay ODE. + //TEST("AD GLRK3 Method", GLRK3_LinearDecaySensitivity) { + // auto stepFn = [&]( + // const VecX_T>& x, + // DualNumber_T t, + // DualNumber_T dt + // ) { + // return integrator.GLRK3(x, t, dt, linearDecay); + // }; + // verifyExpDecaySensitivity(stepFn, 1e-12, 1000); + //} + //// Test case for the GLRK3 method on the stiff decay ODE. + //TEST("AD GLRK3 Method", GLRK3_StiffDecay) { + // VecX_T> x(1); + // x(0) = DualNumber_T(1.0, { 0.0 }); + // double T = 0.5; + // DualNumber_T dt(T / 500.0); + // DualNumber_T t(0.0); + // double expected = std::exp(-50.0); + // auto stiff_f = [](auto t, const auto& x) { + // auto dx = x; + // dx(0) = -100.0 * x(0); + // return dx; + // }; + // for (int i = 0; i < 500; ++i) { + // x = integrator.GLRK3(x, t, dt, stiff_f, 150, 1e-12); + // t += dt; + // } + // ASSERT_TRUE(std::isfinite(x(0).real), "GLRK3 produced a non-finite result."); + // ASSERT_TRUE(x(0).real < 1e-14, "GLRK3 did not sufficiently damp stiff mode"); + //} + //// Test case for the GLRK3 method on the stiff decay ODE. + //TEST("AD GLRK3 Method", GLRK3_StiffDecaySensitivity) { + // auto stepFn = [&]( + // const VecX_T>& x, + // DualNumber_T t, + // DualNumber_T dt + // ) { + // return integrator.GLRK3(x, t, dt, stiffDecay); + // }; + // verifyExpDecaySensitivity(stepFn, 1e-12, 1000); + //} + //// Test case for the GLRK3 method on the nonlinear decay ODE. + //TEST("AD GLRK3 Method", GLRK3_NonlinearDecay) { + // VecX_T> x(1); + // x(0) = DualNumber_T(1.0, { 0.0 }); + // double T = 0.5; + // DualNumber_T dt(T / 500.0); + // DualNumber_T t(0.0); + // double expected = 1.0 / (1.0 + T); + // auto nl_f = [](auto t, const auto& x) { + // auto dx = x; + // dx(0) = -x(0) * x(0); + // return dx; + // }; + // for (int i = 0; i < 500; ++i) { + // x = integrator.GLRK3(x, t, dt, nl_f, 80, 1e-7); + // t += dt; + // } + // double rel_err = std::abs(x(0).real -expected) / expected; + // ASSERT_TRUE(rel_err < tol_low, "GLRK3 nonlinear decay error too large"); + //} + //// Test case for the GLRK3 method on the stiff nonlinear decay ODE. + //TEST("AD GLRK3 Method", GLRK3_StiffNonlinearDecay) { + // VecX_T> x(1); + // x(0) = DualNumber_T(1.0, { 0.0 }); + // double T = 0.5; + // DualNumber_T dt(T / 500.0); + // DualNumber_T t(0.0); + // double expected = 1.0 / 51.0; + // auto stiff_nl_f = [](auto t, const auto& x) { + // auto dx = x.eval(); + // dx(0) = -100.0 * x(0) - x(0) * x(0); + // return dx; + // }; + // for (int i = 0; i < 500; ++i) { + // x = integrator.GLRK3(x, t, dt, stiff_nl_f, 80, 1e-7); + // t += dt; + // } + // double rel_err = std::abs(x(0).real - expected) / expected; + // ASSERT_TRUE(rel_err < tol_low, "GLRK3 stiff nonlinear decay error too large"); + //} + //// Test case for the GLRK3 method on the stiff decay ODE. + //TEST("AD GLRK3 Method", GLRK3_Stability_LargeStep) { + // VecX_T> x(1); + // x(0) = DualNumber_T(1.0, { 1.0 }); + // double T = 1.0; + // DualNumber_T t(0.0); + // DualNumber_T dt(1.0 / 10.0); + // DualNumber_T prev = x(0); + // for (int i = 0; i < 10; ++i) { + // x = integrator.GLRK3(x, t, dt, stiffDecay); + // t += dt; + // ASSERT_TRUE(std::abs(x(0).real) < std::abs(prev.real), "Not monotone"); + // ASSERT_TRUE(std::abs(x(0).dual[0]) < std::abs(prev.dual[0]), "Not strictly decaying"); + // prev = x(0); + // } + //} } \ No newline at end of file From 21254afd058e7b4cccbeb3f6fad28ec4dde18c42 Mon Sep 17 00:00:00 2001 From: saltyjoss Date: Mon, 18 May 2026 21:09:57 +0100 Subject: [PATCH 036/210] feat(fixes): First succesful AD unit test run on an implicit integration method (Euler) GENERALISED LIST OF FIXES: * Separated automatic differentiation implicit integrators from finite-difference / analytical Jacobian paths * Added dedicated AD implementations for: * `implicitEuler_AD` * `implicitMidpoint_AD` * `GLRK2_AD` * `GLRK3_AD` * Refactored AD implicit solver architecture to use explicit AD Jacobian generation * Corrected GLRK2 AD residual system construction * Corrected GLRK3 AD residual system construction * Fixed stage-vector decomposition for AD GLRK methods * Fixed AD residual assembly to match nonlinear Newton systems * Added dynamic Jacobian sizing support in `automaticDifferenceJacobian` * Fixed DualNumber derivative extraction in AD Jacobian generation * Fixed scalar/dual conversion issues in Eigen AD paths * Fixed finite-difference perturbation sizing bug in `finiteDifferenceJacobian` * Fixed vector/scalar misuse in FD epsilon scaling logic * Fixed Dual-safe norm and tolerance handling * Fixed Dual-safe logging and residual formatting * Fixed stale build / duplicate template instantiation issues after integrator refactor * Validated AD implicit Euler convergence against exponential decay test case * Cleaned implicit integrator separation between production and AD solver paths TODO * Re-introduce previous unit tests, and try get a fullset! --- PxMLib/MathLib/include/core/DualNumbers.h | 22 +- .../integrators/numerical_integrators.h | 9 + .../integrators/numerical_integrators.inl | 343 +++++++++++--- .../ADImplicitIntegratorTests.cpp | 422 +++++++++--------- 4 files changed, 508 insertions(+), 288 deletions(-) diff --git a/PxMLib/MathLib/include/core/DualNumbers.h b/PxMLib/MathLib/include/core/DualNumbers.h index 37c08f81..78e7e7da 100644 --- a/PxMLib/MathLib/include/core/DualNumbers.h +++ b/PxMLib/MathLib/include/core/DualNumbers.h @@ -701,11 +701,25 @@ namespace mathlib { }; // Dual Part - template - inline std::array dualPart( - const DualNumber_T& a + template + Scalar dualPart( + const DualNumber_T& x ) { - return a.dual; + return x.dual[0]; + } + + template + auto makeMutableCopy(const Eigen::MatrixBase& x) + -> typename std::decay_t::PlainObject { + return x.derived().eval(); + } + + template + inline DualNumber_T operator*( + T a, + const DualNumber_T& b + ) { + return b * static_cast(a); } } diff --git a/PxMLib/MathLib/include/integrators/numerical_integrators.h b/PxMLib/MathLib/include/integrators/numerical_integrators.h index aff75e1d..2a3804c0 100644 --- a/PxMLib/MathLib/include/integrators/numerical_integrators.h +++ b/PxMLib/MathLib/include/integrators/numerical_integrators.h @@ -171,6 +171,15 @@ namespace integration { int maxIter, Scalar tol ); + // AD version of Newton-Raphson solver + template + mathlib::VecX_T newtonRaphson_AD( + EvalG&& eval_g, + EvalJ&& eval_j, + mathlib::VecX_T x0, + int maxIter, + Scalar tol + ); // Finite difference approximation of the Jacobian matrix df/dx for a vector-valued function f: R^n -> R^m at a point x template diff --git a/PxMLib/MathLib/include/integrators/numerical_integrators.inl b/PxMLib/MathLib/include/integrators/numerical_integrators.inl index 4732e8d5..da793f4a 100644 --- a/PxMLib/MathLib/include/integrators/numerical_integrators.inl +++ b/PxMLib/MathLib/include/integrators/numerical_integrators.inl @@ -244,19 +244,27 @@ namespace integration { int maxIter, Scalar tol ) { - auto jac = [&](const mathlib::VecX_T& x_guess, mathlib::MatX_T& J_out) { - J_out = automaticDifferenceJacobian( + // The function g(x_guess) = 0 that we want to solve for the implicit Euler step + auto g = [&](const mathlib::VecX_T& x_guess, mathlib::VecX_T& g_out) { g_out = x_guess - x - dt * f(t + dt, x_guess); }; + // Numerical Jacobian for Newton-Raphson + auto J = [&](const mathlib::VecX_T& x_guess, mathlib::MatX_T& J_out) { + int n = (int)x_guess.size(); + mathlib::MatX_T F; + F = automaticDifferenceJacobian( [&](auto t_pert, const auto& x_pert) { - using Dual = std::decay_t; - Dual t_eval = Dual(t_pert) + Dual(dt); - return f(t_eval, x_pert); - }, - t, + using Dual = std::decay_t; + Dual t_eval = Dual(t_pert) + Dual(dt); + return f(t_eval, x_pert); + }, + t + dt, x_guess ); - J_out = mathlib::MatX_T::Identity(x.size(), x.size()) - dt * J_out; // J = I - dt * df/dx + + J_out = mathlib::MatX_T::Identity(n, n) - dt * F; // J = I - dt * df/dx }; - return implicitEuler(x, t, dt, f, jac, maxIter, tol); + + mathlib::VecX_T x0 = x + dt * f(t + dt, x); // Initial guess for Newton-Raphson + return newtonRaphson_AD(g, J, x0, maxIter, tol); } // Implicit Midpoint method @@ -292,7 +300,6 @@ namespace integration { } } if (!analytical_success) { - printf("Using finite difference Jacobian for Implicit Midpoint\n"); F = finiteDifferenceJacobian( [&](Scalar, const mathlib::VecX_T& x_pert) { return f((t + dt) / Scalar(2), (x + x_pert) / Scalar(2)); @@ -317,20 +324,30 @@ namespace integration { int maxIter, Scalar tol ) { - auto jac = [&](const mathlib::VecX_T& x_guess, mathlib::MatX_T& J_out) { - J_out = automaticDifferenceJacobian( + // The function g(x_guess) = 0 that we want to solve for the implicit midpoint step + auto g = [&](const mathlib::VecX_T& x_guess, mathlib::VecX_T& g_out) { g_out = x_guess - x - dt * f((t + dt) / Scalar(2), (x + x_guess) / Scalar(2)); }; + + // Numerical Jacobian for Newton-Raphson + auto J = [&](const mathlib::VecX_T& x_guess, mathlib::MatX_T& J_out) { + int n = (int)x_guess.size(); + mathlib::MatX_T F; + bool analytical_success = false; + + F = automaticDifferenceJacobian( [&](auto t_pert, const auto& x_pert) { - using Dual = std::decay_t; - auto x_dual = x.template cast(); - Dual t_mid = Dual(t_pert + dt) / Dual(2); - return f(t_mid, (x_dual + x_pert) / Dual(2)); - }, - t, + using Dual = std::decay_t; + mathlib::VecX_T x_mid = (x.template cast() + x_pert) / Dual(2); + Dual t_mid = (t_pert + dt) / Dual(2); + return f(t_mid, x_mid); + }, + t + dt / Scalar(2), x_guess ); - J_out = mathlib::MatX_T::Identity(x.size(), x.size()) - Scalar(0.5) * dt * J_out; // J = I - dt * df/dx + J_out = mathlib::MatX_T::Identity(n, n) - Scalar(0.5) * dt * F; // J = I - dt * df/dx }; - return implicitMidpoint(x, t, dt, f, jac, maxIter, tol); + + mathlib::VecX_T x0 = x + dt * f((t + dt) / Scalar(2), x); // Initial guess for Newton-Raphson + return newtonRaphson_AD(g, J, x0, maxIter, tol); } // Gauss-Legendre Runge-Kutta method (2 stages, 4th order) @@ -408,7 +425,6 @@ namespace integration { } if (!analytical_success) { - printf("Using finite difference Jacobian for GLRK2\n"); F1 = finiteDifferenceJacobian( [&](Scalar, const mathlib::VecX_T& x_pert) { return f(t + c(0) * dt, x_pert); @@ -455,44 +471,88 @@ namespace integration { int maxIter, Scalar tol ) { + using std::sqrt; + const Eigen::Index n = x.size(); + + // Coefficients for the 2-stage Gauss-Legendre method (4th order) mathlib::VecX_T c(2); c << Scalar(0.5) - sqrt(Scalar(3)) / Scalar(6), Scalar(0.5) + sqrt(Scalar(3)) / Scalar(6); // Stage time fractions + mathlib::MatX_T A(2, 2); - auto jac = [&](const mathlib::VecX_T& x_guess, mathlib::MatX_T& J_out) { - int n = (int)x_guess.size(); - mathlib::VecX_T x1 = x + dt * (A(0, 0) * x_guess.segment(0, n) + A(0, 1) * x_guess.segment(n, n)); - mathlib::VecX_T x2 = x + dt * (A(1, 0) * x_guess.segment(0, n) + A(1, 1) * x_guess.segment(n, n)); - J_out = automaticDifferenceJacobian( - [&](auto t_pert, const auto& x_pert) { - using Dual = std::decay_t; + A << + Scalar(0.25), Scalar(0.25) - sqrt(Scalar(3)) / Scalar(6), + Scalar(0.25) + sqrt(Scalar(3)) / Scalar(6), Scalar(0.25); - mathlib::VecX_T k1 = x_pert.segment(0, n); - mathlib::VecX_T k2 = x_pert.segment(n, n); + const Scalar b = Scalar(0.5); // Weights for final update - mathlib::VecX_T x_base = x.template cast(); + // Initial guess for the stage values k1, k2, k3 + mathlib::VecX_T k(2 * n); // 2 stages + mathlib::VecX_T f0 = f(t, x); + k.segment(0, n) = f0; + k.segment(n, n) = f0; - mathlib::VecX_T x1 = x_base + Dual(dt) * (Dual(A(0, 0)) * k1 + Dual(A(0, 1)) * k2); - mathlib::VecX_T x2 = x_base + Dual(dt) * (Dual(A(1, 0)) * k1 + Dual(A(1, 1)) * k2); + auto eval_g = [&](const mathlib::VecX_T& k_guess, mathlib::VecX_T& g) { + mathlib::VecX_T k1 = k_guess.segment(0, n); + mathlib::VecX_T k2 = k_guess.segment(n, n); - Dual t_eval1 = Dual(t_pert) + Dual(c(0)) * Dual(dt); - Dual t_eval2 = Dual(t_pert) + Dual(c(1)) * Dual(dt); + mathlib::VecX_T x1 = x + dt * (A(0, 0) * k1 + A(0, 1) * k2); + mathlib::VecX_T x2 = x + dt * (A(1, 0) * k1 + A(1, 1) * k2); - mathlib::VecX_T out(2 * n); + mathlib::VecX_T f1 = f(t + c(0) * dt, x1); + mathlib::VecX_T f2 = f(t + c(1) * dt, x2); - out.segment(0, n) = k1 - f(t_eval1, x1); - out.segment(n, n) = k2 - f(t_eval2, x2); + g.resize(2 * n); + g.segment(0, n) = k1 - f1; + g.segment(n, n) = k2 - f2; + }; - return out; - }, - t, - x_guess + auto eval_j = [&](const mathlib::VecX_T& k_guess, mathlib::MatX_T& J) { + mathlib::VecX_T k1 = k_guess.segment(0, n); + mathlib::VecX_T k2 = k_guess.segment(n, n); + mathlib::VecX_T x1 = x + dt * (A(0, 0) * k1 + A(0, 1) * k2); + mathlib::VecX_T x2 = x + dt * (A(1, 0) * k1 + A(1, 1) * k2); + mathlib::MatX_T F1(n, n), F2(n, n); + bool analytical_success = false; + + F1 = automaticDifferenceJacobian( + [&](auto t_pert, const auto& x_pert) { + using Dual = std::decay_t; + Dual t_eval = Dual(t_pert) + Dual(c(0)) * Dual(dt); + return f(t_eval, x_pert); + }, + t + c(0) * dt, + x1 ); - J_out = mathlib::MatX_T::Identity(x.size(), x.size()) - dt * J_out; // J = I - dt * df/dx + F2 = automaticDifferenceJacobian( + [&](auto t_pert, const auto& x_pert) { + using Dual = std::decay_t; + Dual t_eval = Dual(t_pert) + Dual(c(1)) * Dual(dt); + return f(t_eval, x_pert); + }, + t + c(1) * dt, + x2 + ); + + J.setZero(2 * n, 2 * n); + J.block(0, 0, n, n) = mathlib::MatX_T::Identity(n, n) - dt * A(0, 0) * F1; + J.block(0, n, n, n) = -dt * A(0, 1) * F1; + J.block(n, 0, n, n) = -dt * A(1, 0) * F2; + J.block(n, n, n, n) = mathlib::MatX_T::Identity(n, n) - dt * A(1, 1) * F2; }; - return GLRK2(x, t, dt, f, jac, maxIter, tol); + + // Solve the nonlinear system for the stage values using Newton-Raphson + k = newtonRaphson_AD(eval_g, eval_j, k, maxIter, tol); + + // Compute the final update for x using the stage values + mathlib::VecX_T k1 = k.segment(0, n); + mathlib::VecX_T k2 = k.segment(n, n); + mathlib::VecX_T x_f = x + dt * (b * k1 + b * k2); + + return x_f; + } // Gauss-Legendre Runge-Kutta method (3 stages, 6th order) @@ -594,7 +654,6 @@ namespace integration { } if (!analytical_success) { - printf("Using finite difference Jacobian for GLRK3\n"); F1 = finiteDifferenceJacobian( [&](Scalar, const mathlib::VecX_T& x_pert) { return f(t + c(0) * dt, x_pert); @@ -662,48 +721,129 @@ namespace integration { int maxIter, Scalar tol ) { + using Real = typename mathlib::DualTraits::BaseScalar; + using std::sqrt; + using std::pow; + using std::abs; + using std::max; + + if (mathlib::real(tol) < Real(0)) { + Real dt_r = mathlib::real(dt); + Real tol_r = max(Real(1e-12), Real(1e-2) * pow(dt_r, Real(7))); + tol = Scalar(tol_r); + } + const Eigen::Index n = x.size(); + + // Coefficients for the 3-stage Gauss-Legendre method (6th order) mathlib::VecX_T c(3); c << Scalar(0.5) - sqrt(Scalar(15)) / Scalar(10), Scalar(0.5), Scalar(0.5) + sqrt(Scalar(15)) / Scalar(10); // Stage time fractions + mathlib::Mat3_T A(3, 3); - auto jac = [&](const mathlib::VecX_T& x_guess, mathlib::MatX_T& J_out) { - int n = (int)x_guess.size(); - mathlib::VecX_T x1 = x + dt * (A(0, 0) * x_guess.segment(0, n) + A(0, 1) * x_guess.segment(n, n) + A(0, 2) * x_guess.segment(2 * n, n)); - mathlib::VecX_T x2 = x + dt * (A(1, 0) * x_guess.segment(0, n) + A(1, 1) * x_guess.segment(n, n) + A(1, 2) * x_guess.segment(2 * n, n)); - mathlib::VecX_T x3 = x + dt * (A(2, 0) * x_guess.segment(0, n) + A(2, 1) * x_guess.segment(n, n) + A(2, 2) * x_guess.segment(2 * n, n)); - J_out = automaticDifferenceJacobian( - [&](auto t_pert, const auto& x_pert) { - using Dual = std::decay_t; + A << + Scalar(5) / Scalar(36), Scalar(2) / Scalar(9) - sqrt(Scalar(15)) / Scalar(15), Scalar(5) / Scalar(36) - sqrt(Scalar(15)) / Scalar(30), + Scalar(5) / Scalar(36) + sqrt(Scalar(15)) / Scalar(24), Scalar(2) / Scalar(9), Scalar(5) / Scalar(36) - sqrt(Scalar(15)) / Scalar(24), + Scalar(5) / Scalar(36) + sqrt(Scalar(15)) / Scalar(30), Scalar(2) / Scalar(9) - sqrt(Scalar(15)) / Scalar(15), Scalar(5) / Scalar(36); - mathlib::VecX_T k1 = x_pert.segment(0, n); - mathlib::VecX_T k2 = x_pert.segment(n, n); - mathlib::VecX_T k3 = x_pert.segment(2 * n, n); + mathlib::VecX_T b(3); + b << 5.0 / 18.0, 4.0 / 9.0, 5.0 / 18.0; // Weights for final update - mathlib::VecX_T x_base = x.template cast(); + // Initial guess for the stage values k1, k2, k3 + mathlib::VecX_T k(3 * n); // 3 stages + mathlib::VecX_T f0 = f(t, x); + k.segment(0, n) = f0; + k.segment(n, n) = f0; + k.segment(2 * n, n) = f0; - mathlib::VecX_T x1 = x_base + Dual(dt) * (Dual(A(0, 0)) * k1 + Dual(A(0, 1)) * k2 + Dual(A(0, 2)) * k3); - mathlib::VecX_T x2 = x_base + Dual(dt) * (Dual(A(1, 0)) * k1 + Dual(A(1, 1)) * k2 + Dual(A(1, 2)) * k3); - mathlib::VecX_T x3 = x_base + Dual(dt) * (Dual(A(2, 0)) * k1 + Dual(A(2, 1)) * k2 + Dual(A(2, 2)) * k3); + auto eval_g = [&](const mathlib::VecX_T& k_guess, mathlib::VecX_T& g) { + mathlib::VecX_T k1 = k_guess.segment(0, n); + mathlib::VecX_T k2 = k_guess.segment(n, n); + mathlib::VecX_T k3 = k_guess.segment(2 * n, n); - Dual t_eval1 = Dual(t_pert) + Dual(c(0)) * Dual(dt); - Dual t_eval2 = Dual(t_pert) + Dual(c(1)) * Dual(dt); - Dual t_eval3 = Dual(t_pert) + Dual(c(2)) * Dual(dt); + mathlib::VecX_T x1 = x + dt * (A(0, 0) * k1 + A(0, 1) * k2 + A(0, 2) * k3); + mathlib::VecX_T x2 = x + dt * (A(1, 0) * k1 + A(1, 1) * k2 + A(1, 2) * k3); + mathlib::VecX_T x3 = x + dt * (A(2, 0) * k1 + A(2, 1) * k2 + A(2, 2) * k3); - mathlib::VecX_T out(3 * n); + mathlib::VecX_T f1 = f(t + c(0) * dt, x1); + mathlib::VecX_T f2 = f(t + c(1) * dt, x2); + mathlib::VecX_T f3 = f(t + c(2) * dt, x3); - out.segment(0, n) = k1 - f(t_eval1, x1); - out.segment(n, n) = k2 - f(t_eval2, x2); - out.segment(2 * n, n) = k3 - f(t_eval3, x3); + g.resize(3 * n); - return out; - }, - t, - x_guess + g.segment(0, n) = k1 - f1; + g.segment(n, n) = k2 - f2; + g.segment(2 * n, n) = k3 - f3; + }; + + auto eval_j = [&](const mathlib::VecX_T& k_guess, mathlib::MatX_T& J) { + mathlib::VecX_T k1 = k_guess.segment(0, n); + mathlib::VecX_T k2 = k_guess.segment(n, n); + mathlib::VecX_T k3 = k_guess.segment(2 * n, n); + + mathlib::VecX_T x1 = x + dt * (A(0, 0) * k1 + A(0, 1) * k2 + A(0, 2) * k3); + mathlib::VecX_T x2 = x + dt * (A(1, 0) * k1 + A(1, 1) * k2 + A(1, 2) * k3); + mathlib::VecX_T x3 = x + dt * (A(2, 0) * k1 + A(2, 1) * k2 + A(2, 2) * k3); + + mathlib::MatX_T F1(n, n), F2(n, n), F3(n, n); + + F1 = automaticDifferenceJacobian( + [&](auto t_pert, const auto& x_pert) { + using Dual = std::decay_t; + Dual t_eval = Dual(t_pert) + Dual(c(0)) * Dual(dt); + return f(t_eval, x_pert); + }, + t + c(0) * dt, + x1 + ); + F2 = automaticDifferenceJacobian( + [&](auto t_pert, const auto& x_pert) { + using Dual = std::decay_t; + Dual t_eval = Dual(t_pert) + Dual(c(1)) * Dual(dt); + return f(t_eval, x_pert); + }, + t + c(1) * dt, + x2 ); + F3 = automaticDifferenceJacobian( + [&](auto t_pert, const auto& x_pert) { + using Dual = std::decay_t; + Dual t_eval = Dual(t_pert) + Dual(c(2)) * Dual(dt); + return f(t_eval, x_pert); + }, + t + c(2) * dt, + x3 + ); + + + J.setZero(3 * n, 3 * n); + J.block(0, 0, n, n) = mathlib::MatX_T::Identity(n, n) - dt * A(0, 0) * F1; + J.block(0, n, n, n) = -dt * A(0, 1) * F1; + J.block(0, 2 * n, n, n) = -dt * A(0, 2) * F1; + J.block(n, 0, n, n) = -dt * A(1, 0) * F2; + J.block(n, n, n, n) = mathlib::MatX_T::Identity(n, n) - dt * A(1, 1) * F2; + J.block(n, 2 * n, n, n) = -dt * A(1, 2) * F2; + J.block(2 * n, 0, n, n) = -dt * A(2, 0) * F3; + J.block(2 * n, n, n, n) = -dt * A(2, 1) * F3; + J.block(2 * n, 2 * n, n, n) = mathlib::MatX_T::Identity(n, n) - dt * A(2, 2) * F3; }; - return GLRK3(x, t, dt, f, jac, maxIter, tol); + + // Solve the nonlinear system for the stage values using Newton-Raphson + k = newtonRaphson_AD(eval_g, eval_j, k, maxIter, tol); + + mathlib::VecX_T g_check; + eval_g(k, g_check); + + //if (residual > 1e-8) { std::cout << "[GLRK3] Large final residual: " << residual << std::endl; } + + // Compute the final update for x using the stage values + mathlib::VecX_T k1 = k.segment(0, n); + mathlib::VecX_T k2 = k.segment(n, n); + mathlib::VecX_T k3 = k.segment(2 * n, n); + mathlib::VecX_T x_f = x + dt * (b(0) * k1 + b(1) * k2 + b(2) * k3); + + return x_f; } // Newton-Raphson solver for systems of nonlinear equations g(x) = 0 @@ -770,6 +910,63 @@ namespace integration { + std::to_string(delta.norm()) ); } + // AD version of Newton-Raphson solver for systems of nonlinear equations g(x) = 0 (DOES NOT USE NEWTON RAPHSON CALL) + template + mathlib::VecX_T NumericalIntegrator::newtonRaphson_AD( + EvalG&& eval_g, + EvalJ&& eval_j, + mathlib::VecX_T x0, + int maxIter, + Scalar tol + ) { + mathlib::VecX_T x = x0, g, x_trial; + using BaseScalar = typename mathlib::DualTraits::BaseScalar; + mathlib::VecX_T delta = mathlib::VecX_T::Constant(x0.size(), Scalar(std::numeric_limits::infinity())); + mathlib::MatX_T J; + Eigen::ColPivHouseholderQR> solver; + for (int iter = 0; iter < maxIter; ++iter) { + eval_g(x, g); + if (!g.allFinite()) { + throw std::runtime_error( + "Newton received non-finite residual at iter = " + + std::to_string(iter) + + ", residual norm = " + + std::to_string(mathlib::real(g.norm())) + ); + } + eval_j(x, J); + if (!J.allFinite()) { + throw std::runtime_error( + "Newton received non-finite Jacobian at iter = " + + std::to_string(iter) + ); + } + solver.compute(J); + delta = solver.solve(-g); + if (!delta.allFinite()) { + throw std::runtime_error( + "Newton produced non-finite step at iter = " + + std::to_string(iter) + ); + } + x += delta; + if (mathlib::real(g.norm()) < mathlib::real(tol)) { return x; } + if (!x.allFinite()) { + throw std::runtime_error( + "Newton state became non-finite at iter = " + + std::to_string(iter) + ); + } + } + throw std::runtime_error( + "Newton-Raphson failed to converge after " + + std::to_string(maxIter) + + " iterations. Final residual norm = " + + std::to_string(mathlib::real(g.norm())) + + ", final step norm = " + + std::to_string(mathlib::real(delta.norm())) + ); + } // Finite difference approximation of the Jacobian matrix df/dx for a vector-valued function f: R^n -> R^m at a point x template @@ -786,7 +983,7 @@ namespace integration { for (int i = 0; i < n; ++i) { mathlib::VecX_T x_fwd = x; mathlib::VecX_T x_bwd = x; - Scalar h = eps_rel * std::max(1.0, std::abs(x(i))); + Scalar h = eps_rel * std::max(Scalar(1), Scalar(std::abs(mathlib::real(x(i))))); x_fwd(i) += h; x_bwd(i) -= h; mathlib::VecX_T f_fwd = f(t, x_fwd); diff --git a/PxMLib/MathLib_Unit/AutomaticDifferentiationTests/ADImplicitIntegratorTests.cpp b/PxMLib/MathLib_Unit/AutomaticDifferentiationTests/ADImplicitIntegratorTests.cpp index f64910e2..40b92123 100644 --- a/PxMLib/MathLib_Unit/AutomaticDifferentiationTests/ADImplicitIntegratorTests.cpp +++ b/PxMLib/MathLib_Unit/AutomaticDifferentiationTests/ADImplicitIntegratorTests.cpp @@ -11,216 +11,216 @@ using namespace mathlib; using namespace constants; namespace { - //constexpr double tol_low = 1e-3; - //constexpr double tol_high = 1e-6; + constexpr double tol_low = 1e-3; + constexpr double tol_high = 1e-6; - //integration::NumericalIntegrator integrator; + integration::NumericalIntegrator integrator; - //// Exponential Decay function for dual numbers - //template - //VecX_T> expDecay( - // DualNumber_T /*t*/, - // const VecX_T>& x - //) { - // VecX_T> dx = x; - // for (int i = 0; i < x.size(); ++i) { dx(i) = -x(i); } - // return dx; - //} + // Exponential Decay function for dual numbers + template + VecX_T> expDecay( + DualNumber_T /*t*/, + const VecX_T>& x + ) { + VecX_T> dx = x; + for (int i = 0; i < x.size(); ++i) { dx(i) = -x(i); } + return dx; + } - //// Harmonic Oscillator function for dual numbers - //template - //VecX_T> harmonicOsc( - // DualNumber_T /*t*/, - // const VecX_T>& s - //) { - // VecX_T> d(2); - // d(0) = s(1); - // d(1) = -s(0); - // return d; - //} + // Harmonic Oscillator function for dual numbers + template + VecX_T> harmonicOsc( + DualNumber_T /*t*/, + const VecX_T>& s + ) { + VecX_T> d(2); + d(0) = s(1); + d(1) = -s(0); + return d; + } - //// Linear Decay function for dual numbers - //template - //VecX_T> linearDecay( - // DualNumber_T /*t*/, - // const VecX_T>& x - //) { - // VecX_T> dx = x; - // for (int i = 0; i < x.size(); ++i) { dx(i) = -2.0 * x(i); } - // return dx; - //} + // Linear Decay function for dual numbers + template + VecX_T> linearDecay( + DualNumber_T /*t*/, + const VecX_T>& x + ) { + VecX_T> dx = x; + for (int i = 0; i < x.size(); ++i) { dx(i) = -Scalar(2) * x(i); } + return dx; + } - //// Nonlinear Decay function for dual numbers - //template - //VecX_T> nonlinearDecay( - // DualNumber_T /*t*/, - // const VecX_T>& x - //) { - // VecX_T> dx = x; - // for (int i = 0; i < x.size(); ++i) { dx(i) = -x(i) * x(i); } - // return dx; - //} + // Nonlinear Decay function for dual numbers + template + VecX_T> nonlinearDecay( + DualNumber_T /*t*/, + const VecX_T>& x + ) { + VecX_T> dx = x; + for (int i = 0; i < x.size(); ++i) { dx(i) = -x(i) * x(i); } + return dx; + } - //// Stiff Decay function for dual numbers - //template - //VecX_T> stiffDecay( - // DualNumber_T /*t*/, - // const VecX_T>& x - //) { - // VecX_T> dx = x; - // for (int i = 0; i < x.size(); ++i) { dx(i) = -100.0 * x(i); } - // return dx; - //} + // Stiff Decay function for dual numbers + template + VecX_T> stiffDecay( + DualNumber_T /*t*/, + const VecX_T>& x + ) { + VecX_T> dx = x; + for (int i = 0; i < x.size(); ++i) { dx(i) = -Scalar(100) * x(i); } + return dx; + } - //// Helper function to integrate from t=0 to t=T using N steps of the given step function, for dual numbers - //template - //VecX_T> integrateToTime( - // StepFn stepFn, - // const VecX_T>& x0, - // Scalar T, - // int N - //) { - // DualNumber_T dt(T / Scalar(N)); - // DualNumber_T t(0.0); - // VecX_T> x = x0; - // for (int i = 0; i < N; ++i) { x = stepFn(x, t, dt); t += dt; } - // return x; - //} + // Helper function to integrate from t=0 to t=T using N steps of the given step function, for dual numbers + template + VecX_T> integrateToTime( + StepFn stepFn, + const VecX_T>& x0, + Scalar T, + int N + ) { + DualNumber_T dt(T / Scalar(N)); + DualNumber_T t(0.0); + VecX_T> x = x0; + for (int i = 0; i < N; ++i) { x = stepFn(x, t, dt); t += dt; } + return x; + } - //// Helper function to verify that the exponential decay ODE is integrated correctly, and that the sensitivity of the solution with respect to the initial condition matches the analytical derivative. - //template - //void verifyExpDecaySensitivity( - // StepFn stepFn, - // double tol, - // int N - //) { - // VecX_T> x0(1); - // x0(0) = DualNumber_T(1.0, { 1.0 }); // Initial condition with derivative 1.0 - // VecX_T> r = integrateToTime(stepFn, x0, 1.0, N); - // double expectedValue = std::exp(-1.0); - // ASSERT_TRUE(std::abs(r(0).real - expectedValue) < tol, "Exponential decay error too large"); - // ASSERT_TRUE(std::abs(r(0).dual[0] - expectedValue) < tol, "Exponential decay sensitivity error too large"); - // ASSERT_TRUE(std::abs(r(0).dual[0]) > 0.0, "Sensitivity vanished during propagation"); - //} + // Helper function to verify that the exponential decay ODE is integrated correctly, and that the sensitivity of the solution with respect to the initial condition matches the analytical derivative. + template + void verifyExpDecaySensitivity( + StepFn stepFn, + double tol, + int N + ) { + VecX_T> x0(1); + x0(0) = DualNumber_T(1.0, { 1.0 }); // Initial condition with derivative 1.0 + VecX_T> r = integrateToTime(stepFn, x0, 1.0, N); + double expectedValue = std::exp(-1.0); + ASSERT_TRUE(std::abs(r(0).real - expectedValue) < tol, "Exponential decay error too large"); + ASSERT_TRUE(std::abs(r(0).dual[0] - expectedValue) < tol, "Exponential decay sensitivity error too large"); + ASSERT_TRUE(std::abs(r(0).dual[0]) > 0.0, "Sensitivity vanished during propagation"); + } - //// Helper function to verify that a harmonic oscillator ODE is integrated correctly, and that the sensitivity of the solution with respect to the initial condition matches the analytical derivative. - //template - //void verifyHarmonicOscillatorSensitivity( - // StepFn stepFn, - // double tol, - // int N - //) { - // VecX_T> x0(2); - // x0(0) = DualNumber_T(1.0, { 1.0, 0.0 }); // Initial position with sensitivity - // x0(1) = DualNumber_T(0.0, { 0.0, 1.0 }); // Initial velocity with sensitivity - // VecX_T> r = integrateToTime(stepFn, x0, TWO_PI_d, N); - // double expectedPosition = std::cos(TWO_PI_d); - // double expectedVelocity = -std::sin(TWO_PI_d); - // double expectedVelocitySens = std::cos(TWO_PI_d); - // ASSERT_TRUE(std::abs(r(0).real - expectedPosition) < tol, "Harmonic oscillator position error too large"); - // ASSERT_TRUE(std::abs(r(1).real - expectedVelocity) < tol, "Harmonic oscillator velocity error too large"); - // ASSERT_TRUE(std::abs(r(0).dual[0] - expectedPosition) < tol, "Harmonic oscillator position sensitivity error too large"); - // ASSERT_TRUE(std::abs(r(1).dual[1] - expectedVelocitySens) < tol, "Harmonic oscillator velocity sensitivity error too large"); - // ASSERT_TRUE(std::abs(r(0).dual[0]) > 0.0, "Position sensitivity vanished during propagation"); - // ASSERT_TRUE(std::abs(r(1).dual[1]) > 0.0, "Velocity sensitivity vanished during propagation"); - //} - // - //// AD Implicit Euler Tests - //// Test case for the GLRK2 method on the exponenital decay ODE. - //TEST("AD Implicit Euler Method", ImplicitEuler_ExponentialDecay) { - // VecX_T> x(1); - // x(0) = DualNumber_T(1.0, { 0.0 }); - // double T = 1.0; - // DualNumber_T dt(T / 1000.0, { 0.0 }); - // DualNumber_T t(0.0, { 0.0 }); - // for (int i = 0; i < 1000; ++i) { - // x = integrator.implicitEuler(x, t, dt, expDecay); - // t += dt; - // } - // ASSERT_TRUE(std::abs(x(0).real - std::exp(-1.0)) < tol_low, "Implicit Euler exponential decay error too large"); - //} - //// Test case for the implicit Euler method on the exponential decay ODE. - //TEST("AD Implicit Euler Method", ImplicitEuler_ExponentialDecaySensitivity) { - // auto stepFn = [&]( - // const VecX_T>& x, - // DualNumber_T t, - // DualNumber_T dt - // ) { - // return integrator.implicitEuler(x, t, dt, expDecay); - // }; - // verifyExpDecaySensitivity(stepFn, 1e-2, 1000); - //} - //// Test case for the Implicit Midpoint method on the linear decay ODE. - //TEST("AD Implicit Midpoint Method", ImplicitEuler_LinearDecay) { - // VecX_T> x(1); - // x(0) = DualNumber_T(1.0, { 0.0 }); - // double T = 0.5; - // DualNumber_T dt(T / 500.0); - // DualNumber_T t(0.0); - // for (int i = 0; i < 500; ++i) { - // x = integrator.implicitEuler(x, t, dt, linearDecay, 50, 1e-6); - // t += dt; - // } - // ASSERT_TRUE(std::abs(x(0).real - std::exp(-1.0)) < tol_low, "Implicit Euler linear decay error too large"); - //} - //// Test case for the implicit Euler method on the linear decay ODE. - //TEST("AD Implicit Euler Method", ImplicitEuler_LinearDecaySensitivity) { - // auto stepFn = [&]( - // const VecX_T>& x, - // DualNumber_T t, - // DualNumber_T dt - // ) { - // return integrator.implicitEuler(x, t, dt, linearDecay); - // }; - // verifyExpDecaySensitivity(stepFn, 1e-2, 1000); - //} - //// Test case for the Implicit Euler method on the stiff decay ODE. - //TEST("AD Implicit Euler Method", ImplicitEuler_StiffDecay) { - // VecX_T> x(1); - // x(0) = DualNumber_T(1.0, { 0.0 }); - // double T = 0.5; - // DualNumber_T dt(T / 500.0); - // DualNumber_T t(0.0); - // double expected = std::exp(-50.0); - // auto stiff_f = [](auto t, const auto& x) { - // auto dx = x; - // dx(0) = -100.0 * x(0); - // return dx; - // }; - // for (int i = 0; i < 500; ++i) { - // x = integrator.implicitEuler(x, t, dt, stiff_f); - // t += dt; - // } - // ASSERT_TRUE(std::isfinite(x(0).real), "Implicit Euler produced a non-finite result."); - // ASSERT_TRUE(x(0).real < 1e-12, "Implicit Euler did not sufficiently damp stiff mode"); - //} - //// Test case for the implicit Euler method on the stiff decay ODE. - //TEST("AD Implicit Euler Method", ImplicitEuler_StiffDecaySensitivity) { - // auto stepFn = [&]( - // const VecX_T>& x, - // DualNumber_T t, - // DualNumber_T dt - // ) { - // return integrator.implicitEuler(x, t, dt, stiffDecay); - // }; - // verifyExpDecaySensitivity(stepFn, 1e-2, 1000); - //} - //// Test large step performance of implicit midpoint - //TEST("AD Implicit Euler Method", ImplicitEuler_Stability_LargeStep) { - // VecX_T> x(1); - // x(0) = DualNumber_T(1.0, { 1.0 }); - // double T = 1.0; - // DualNumber_T t(0.0); - // DualNumber_T dt(1.0 / 10.0); - // DualNumber_T prev = x(0); - // for (int i = 0; i < 10; ++i) { - // x = integrator.implicitEuler(x, t, dt, stiffDecay); - // t += dt; - // ASSERT_TRUE(std::abs(x(0).real) < std::abs(prev.real), "Not monotone"); - // ASSERT_TRUE(std::abs(x(0).dual[0]) < std::abs(prev.dual[0]), "Not strictly decaying"); - // prev = x(0); - // } - //} + // Helper function to verify that a harmonic oscillator ODE is integrated correctly, and that the sensitivity of the solution with respect to the initial condition matches the analytical derivative. + template + void verifyHarmonicOscillatorSensitivity( + StepFn stepFn, + double tol, + int N + ) { + VecX_T> x0(2); + x0(0) = DualNumber_T(1.0, { 1.0, 0.0 }); // Initial position with sensitivity + x0(1) = DualNumber_T(0.0, { 0.0, 1.0 }); // Initial velocity with sensitivity + VecX_T> r = integrateToTime(stepFn, x0, TWO_PI_d, N); + double expectedPosition = std::cos(TWO_PI_d); + double expectedVelocity = -std::sin(TWO_PI_d); + double expectedVelocitySens = std::cos(TWO_PI_d); + ASSERT_TRUE(std::abs(r(0).real - expectedPosition) < tol, "Harmonic oscillator position error too large"); + ASSERT_TRUE(std::abs(r(1).real - expectedVelocity) < tol, "Harmonic oscillator velocity error too large"); + ASSERT_TRUE(std::abs(r(0).dual[0] - expectedPosition) < tol, "Harmonic oscillator position sensitivity error too large"); + ASSERT_TRUE(std::abs(r(1).dual[1] - expectedVelocitySens) < tol, "Harmonic oscillator velocity sensitivity error too large"); + ASSERT_TRUE(std::abs(r(0).dual[0]) > 0.0, "Position sensitivity vanished during propagation"); + ASSERT_TRUE(std::abs(r(1).dual[1]) > 0.0, "Velocity sensitivity vanished during propagation"); + } + + // AD Implicit Euler Tests + // Test case for the GLRK2 method on the exponenital decay ODE. + TEST("AD Implicit Euler Method", ImplicitEuler_ExponentialDecay) { + VecX_T> x(1); + x(0) = DualNumber_T(1.0, { 0.0 }); + double T = 1.0; + DualNumber_T dt(T / 1000.0, { 0.0 }); + DualNumber_T t(0.0, { 0.0 }); + for (int i = 0; i < 1000; ++i) { + x = integrator.implicitEuler_AD(x, t, dt, expDecay, 8, DualNumber_T(1e-6, { 0.0 })); + t += dt; + } + ASSERT_TRUE(std::abs(x(0).real - std::exp(-1.0)) < tol_low, "Implicit Euler exponential decay error too large"); + } + // Test case for the implicit Euler method on the exponential decay ODE. + TEST("AD Implicit Euler Method", ImplicitEuler_ExponentialDecaySensitivity) { + auto stepFn = [&]( + const VecX_T>& x, + DualNumber_T t, + DualNumber_T dt + ) { + return integrator.implicitEuler_AD(x, t, dt, expDecay, 8, DualNumber_T(1e-6, { 0.0 })); + }; + verifyExpDecaySensitivity(stepFn, 1e-2, 1000); + } + // Test case for the Implicit Midpoint method on the linear decay ODE. + TEST("AD Implicit Euler Method", ImplicitEuler_LinearDecay) { + VecX_T> x(1); + x(0) = DualNumber_T(1.0, { 0.0 }); + double T = 0.5; + DualNumber_T dt(T / 500.0); + DualNumber_T t(0.0); + for (int i = 0; i < 500; ++i) { + x = integrator.implicitEuler_AD(x, t, dt, linearDecay, 50, DualNumber_T(1e-6, { 0.0 })); + t += dt; + } + ASSERT_TRUE(std::abs(x(0).real - std::exp(-1.0)) < tol_low, "Implicit Euler linear decay error too large"); + } + // Test case for the implicit Euler method on the linear decay ODE. + TEST("AD Implicit Euler Method", ImplicitEuler_LinearDecaySensitivity) { + auto stepFn = [&]( + const VecX_T>& x, + DualNumber_T t, + DualNumber_T dt + ) { + return integrator.implicitEuler_AD(x, t, dt, expDecay, 8, DualNumber_T(1e-6, { 0.0 })); + }; + verifyExpDecaySensitivity(stepFn, 1e-2, 1000); + } + // Test case for the Implicit Euler method on the stiff decay ODE. + TEST("AD Implicit Euler Method", ImplicitEuler_StiffDecay) { + VecX_T> x(1); + x(0) = DualNumber_T(1.0, { 0.0 }); + double T = 0.5; + DualNumber_T dt(T / 500.0); + DualNumber_T t(0.0); + double expected = std::exp(-50.0); + auto stiff_f = [](auto t, const auto& x) { + auto dx = x; + dx(0) = -100.0 * x(0); + return dx; + }; + for (int i = 0; i < 500; ++i) { + x = integrator.implicitEuler_AD(x, t, dt, stiff_f, 50, DualNumber_T(1e-6, { 0.0 })); + t += dt; + } + ASSERT_TRUE(std::isfinite(x(0).real), "Implicit Euler produced a non-finite result."); + ASSERT_TRUE(x(0).real < 1e-12, "Implicit Euler did not sufficiently damp stiff mode"); + } + // Test case for the implicit Euler method on the stiff decay ODE. + TEST("AD Implicit Euler Method", ImplicitEuler_StiffDecaySensitivity) { + auto stepFn = [&]( + const VecX_T>& x, + DualNumber_T t, + DualNumber_T dt + ) { + return integrator.implicitEuler_AD(x, t, dt, stiffDecay, 50, DualNumber_T(1e-6, { 0.0 })); + }; + verifyExpDecaySensitivity(stepFn, 1e-2, 1000); + } + // Test large step performance of implicit midpoint + TEST("AD Implicit Euler Method", ImplicitEuler_Stability_LargeStep) { + VecX_T> x(1); + x(0) = DualNumber_T(1.0, { 1.0 }); + double T = 1.0; + DualNumber_T t(0.0); + DualNumber_T dt(1.0 / 10.0); + DualNumber_T prev = x(0); + for (int i = 0; i < 10; ++i) { + x = integrator.implicitEuler_AD(x, t, dt, stiffDecay, 50, DualNumber_T(1e-6, { 0.0 })); + t += dt; + ASSERT_TRUE(std::abs(x(0).real) < std::abs(prev.real), "Not monotone"); + ASSERT_TRUE(std::abs(x(0).dual[0]) < std::abs(prev.dual[0]), "Not strictly decaying"); + prev = x(0); + } + } //// AD Implicit Midpoint Tests //// Test case for the Implicit Midpoint method on the exponenital decay ODE. @@ -231,7 +231,7 @@ namespace { // DualNumber_T dt(T / 1000.0, { 0.0 }); // DualNumber_T t(0.0, { 0.0 }); // for (int i = 0; i < 1000; ++i) { - // x = integrator.implicitMidpoint(x, t, dt, expDecay); + // x = integrator.implicitMidpoint_AD(x, t, dt, expDecay, 8, DualNumber_T(1e-6, { 0.0 })); // t += dt; // } // ASSERT_TRUE(std::abs(x(0).real - std::exp(-1.0)) < tol_low, "Implicit Midpoint exponential decay error too large"); @@ -243,7 +243,7 @@ namespace { // DualNumber_T t, // DualNumber_T dt // ) { - // return integrator.implicitMidpoint(x, t, dt, expDecay); + // return integrator.implicitMidpoint_AD(x, t, dt, expDecay, 8, DualNumber_T(1e-6, { 0.0 })); // }; // verifyExpDecaySensitivity(stepFn, 1e-3, 1000); //} @@ -257,7 +257,7 @@ namespace { // DualNumber_T dt(T / 2500.0); // DualNumber_T t(0.0); // for (int i = 0; i < 2500; ++i) { - // x = integrator.implicitMidpoint(x, t, dt, harmonicOsc); + // x = integrator.implicitMidpoint_AD(x, t, dt, harmonicOsc, 10, DualNumber_T(1e-7, { 0.0, 0.0 })); // t += dt; // } // double Ef = 0.5 * (x(0).real * x(0).real + x(1).real * x(1).real); @@ -271,7 +271,7 @@ namespace { // DualNumber_T t, // DualNumber_T dt // ) { - // return integrator.implicitMidpoint(x, t, dt, harmonicOsc); + // return integrator.implicitMidpoint_AD(x, t, dt, harmonicOsc, 10, DualNumber_T(1e-7, { 0.0, 0.0 })); // }; // verifyHarmonicOscillatorSensitivity(stepFn, 1e-3, 1000); //} @@ -283,7 +283,7 @@ namespace { // DualNumber_T dt(T / 500.0); // DualNumber_T t(0.0); // for (int i = 0; i < 500; ++i) { - // x = integrator.implicitMidpoint(x, t, dt, linearDecay); + // x = integrator.implicitMidpoint_AD(x, t, dt, linearDecay, 10, DualNumber_T(1e-6, { 0.0 })); // t += dt; // } // ASSERT_TRUE(std::abs(x(0).real - std::exp(-1.0)) < tol_low, "Implicit Midpoint linear decay error too large"); @@ -295,7 +295,7 @@ namespace { // DualNumber_T t, // DualNumber_T dt // ) { - // return integrator.implicitMidpoint(x, t, dt, linearDecay); + // return integrator.implicitMidpoint_AD(x, t, dt, linearDecay, 10, DualNumber_T(1e-6, { 0.0 })); // }; // verifyExpDecaySensitivity(stepFn, 1e-3, 1000); //} @@ -307,13 +307,13 @@ namespace { // DualNumber_T dt(T / 500.0); // DualNumber_T t(0.0); // double expected = std::exp(-50.0); - // auto stiff_f = [](auto t, const auto& x) { + // auto stiff_f = [](auto t, const auto& x) { // why is this necessary? Why can't I just pass stiffDecay directly? the answer is that the template parameters can't be deduced from the function pointer, but they can be deduced from the // auto dx = x; // dx(0) = -100.0 * x(0); // return dx; // }; // for (int i = 0; i < 500; ++i) { - // x = integrator.implicitMidpoint(x, t, dt, stiff_f); + // x = integrator.implicitMidpoint_AD(x, t, dt, stiffDecay, 10, DualNumber_T(1e-6, { 0.0 })); // t += dt; // } // ASSERT_TRUE(std::isfinite(x(0).real), "Implicit Midpoint produced a non-finite result."); @@ -326,7 +326,7 @@ namespace { // DualNumber_T t, // DualNumber_T dt // ) { - // return integrator.implicitMidpoint(x, t, dt, stiffDecay); + // return integrator.implicitMidpoint_AD(x, t, dt, stiffDecay, 10, DualNumber_T(1e-6, { 0.0 })); // }; // verifyExpDecaySensitivity(stepFn, 1e-3, 1000); //} @@ -339,7 +339,7 @@ namespace { // DualNumber_T dt(1.0 / 10.0); // DualNumber_T prev = x(0); // for (int i = 0; i < 10; ++i) { - // x = integrator.implicitMidpoint(x, t, dt, stiffDecay); + // x = integrator.implicitMidpoint_AD(x, t, dt, stiffDecay, 10, DualNumber_T(1e-6, { 0.0 })); // t += dt; // ASSERT_TRUE(std::abs(x(0).real) < std::abs(prev.real), "Not monotone"); // ASSERT_TRUE(std::abs(x(0).dual[0]) < std::abs(prev.dual[0]), "Not strictly decaying"); From 18bc8e36d36091d2b5292b7ea9ac7877a3e4a315 Mon Sep 17 00:00:00 2001 From: saltyjoss Date: Tue, 19 May 2026 17:47:07 +0100 Subject: [PATCH 037/210] fixes: Fixed Eigen pivot scoring integration for `DualNumber_T` * Added Eigen::internal::scalar_score_coeff_op specialisation for DualNumber_T to support pivot-based decompositions * Implement result_type contract matching Eigen custom scalar API * Used real-part magnitude as LU pivot score * Resolved cwiseAbs().maxCoeff() compatibility failure * Unblocked PartialPivLU decomposition instantiation *Added layered Eigen compatibility unit tests for: * scalar semantics * matrix arithmetic * reductions * decomposition compatibility * Improved isolation of Eigen scalar integration regressions --- PxMLib/MathLib/include/core/DualNumbers.h | 628 +++++++----------- .../integrators/numerical_integrators.inl | 4 +- .../CMakeLists.txt | 4 + .../DualNumber_Eigen_ADTests.cpp | 9 + .../DualNumber_Eigen_ArithmeticTests.cpp | 73 ++ .../DualNumber_Eigen_BasicTests.cpp | 115 ++++ .../DualNumber_Eigen_SolverTests.cpp | 53 ++ 7 files changed, 480 insertions(+), 406 deletions(-) create mode 100644 PxMLib/MathLib_Unit/AutomaticDifferentiationTests/DualNumber_Eigen_ADTests.cpp create mode 100644 PxMLib/MathLib_Unit/AutomaticDifferentiationTests/DualNumber_Eigen_ArithmeticTests.cpp create mode 100644 PxMLib/MathLib_Unit/AutomaticDifferentiationTests/DualNumber_Eigen_BasicTests.cpp create mode 100644 PxMLib/MathLib_Unit/AutomaticDifferentiationTests/DualNumber_Eigen_SolverTests.cpp diff --git a/PxMLib/MathLib/include/core/DualNumbers.h b/PxMLib/MathLib/include/core/DualNumbers.h index 78e7e7da..ef5fcc22 100644 --- a/PxMLib/MathLib/include/core/DualNumbers.h +++ b/PxMLib/MathLib/include/core/DualNumbers.h @@ -3,10 +3,16 @@ #include "MathLibAPI.h" #include "core/Types_tpl.h" + +#define EIGEN_DONT_VECTORIZE +#define EIGEN_DISABLE_UNALIGNED_ARRAY_ASSERT + #include + #include #include #include +#include namespace mathlib { // Template version of dual number @@ -15,16 +21,14 @@ namespace mathlib { public: using ScalarT = Scalar; - DualNumber_T( - Scalar real = Scalar(0), - const std::array& dual = std::array() - ) : real(real), dual(dual) { - } + DualNumber_T(const Scalar& real = Scalar(0)) + : real(real) { dual.fill(Scalar(0)); } - DualNumber_T( - Scalar real, - std::initializer_list duals - ) : real(real) { + DualNumber_T(const Scalar& real, const std::array& dual) + : real(real), dual(dual) {} + + DualNumber_T(Scalar real, std::initializer_list duals) + : real(real) { dual.fill(Scalar(0)); std::copy_n( duals.begin(), @@ -43,14 +47,10 @@ namespace mathlib { // Negation template - inline DualNumber_T operator-( - const DualNumber_T& a - ) { + inline DualNumber_T operator-(const DualNumber_T& a) { DualNumber_T out; out.real = -a.real; - for (size_t i = 0; i < NVar; ++i) { - out.dual[i] = -a.dual[i]; - } + for (size_t i = 0; i < NVar; ++i) { out.dual[i] = -a.dual[i]; } return out; } @@ -66,59 +66,18 @@ namespace mathlib { // Comparison Operators (compare only the real part) // ----- - // Equality template - inline bool operator==( - const DualNumber_T& a, - const DualNumber_T& b - ) { - return a.real == b.real; - } - - // Inequality + inline bool operator==(const DualNumber_T& a, const DualNumber_T& b) { return a.real == b.real; } template - inline bool operator!=( - const DualNumber_T& a, - const DualNumber_T& b - ) { - return !(a == b); - } - - // Less than + inline bool operator!=(const DualNumber_T& a, const DualNumber_T& b) { return !(a == b); } template - inline bool operator<( - const DualNumber_T& a, - const DualNumber_T& b - ) { - return a.real < b.real; - } - - // Less than or equal + inline bool operator<(const DualNumber_T& a, const DualNumber_T& b) { return a.real < b.real; } template - inline bool operator<=( - const DualNumber_T& a, - const DualNumber_T& b - ) { - return a.real <= b.real; - } - - // Greater than + inline bool operator<=(const DualNumber_T& a, const DualNumber_T& b) { return a.real <= b.real; } template - inline bool operator>( - const DualNumber_T& a, - const DualNumber_T& b - ) { - return a.real > b.real; - } - - // Greater than or equal + inline bool operator>(const DualNumber_T& a, const DualNumber_T& b) { return a.real > b.real; } template - inline bool operator>=( - const DualNumber_T& a, - const DualNumber_T& b - ) { - return a.real >= b.real; - } + inline bool operator>=(const DualNumber_T& a, const DualNumber_T& b) { return a.real >= b.real; } // ----- // Scalar Interactions @@ -126,97 +85,56 @@ namespace mathlib { // Scalar Addition (dual + scalar) template - inline DualNumber_T operator+( - const DualNumber_T& a, - Scalar b - ) { + inline DualNumber_T operator+(const DualNumber_T& a, Scalar b) { DualNumber_T out = a; out.real += b; return out; } - // Scalar Addition (scalar + dual) template - inline DualNumber_T operator+( - Scalar a, - const DualNumber_T& b - ) { - return b + a; // Reuse dual + scalar - } - + inline DualNumber_T operator+(Scalar a, const DualNumber_T& b) { return b + a; } // Scalar Subtraction (dual - scalar) template - inline DualNumber_T operator-( - const DualNumber_T& a, - Scalar b - ) { + inline DualNumber_T operator-(const DualNumber_T& a, Scalar b) { DualNumber_T out = a; out.real -= b; return out; } - // Scalar Subtraction (scalar - dual) template - inline DualNumber_T operator-( - Scalar a, - const DualNumber_T& b - ) { + inline DualNumber_T operator-(Scalar a, const DualNumber_T& b) { DualNumber_T out; out.real = a - b.real; - for (size_t i = 0; i < NVar; ++i) { - out.dual[i] = -b.dual[i]; - } + for (size_t i = 0; i < NVar; ++i) { out.dual[i] = -b.dual[i]; } return out; } - // Scalar Multiplication (dual * scalar) template - inline DualNumber_T operator*( - const DualNumber_T& a, - Scalar b - ) { + inline DualNumber_T operator*(const DualNumber_T& a, Scalar b) { DualNumber_T out; out.real = a.real * b; - for (size_t i = 0; i < NVar; ++i) { - out.dual[i] = a.dual[i] * b; - } + for (size_t i = 0; i < NVar; ++i) { out.dual[i] = a.dual[i] * b; } return out; } - // Scalar Multiplication (scalar * dual) template - inline DualNumber_T operator*( - Scalar a, - const DualNumber_T& b - ) { - return b * a; // Reuse dual * scalar - } + inline DualNumber_T operator*(Scalar a, const DualNumber_T& b) { return b * a; } // Scalar Division (dual / scalar) template - inline DualNumber_T operator/( - const DualNumber_T& a, - Scalar b - ) { + inline DualNumber_T operator/(const DualNumber_T& a, Scalar b) { + if (b == Scalar(0)) { throw std::runtime_error("Division by zero in dual number scalar division"); } DualNumber_T out; - out.real = a.real / b ? a.real / b : throw std::runtime_error("Division by zero in dual number scalar division"); - for (size_t i = 0; i < NVar; ++i) { - out.dual[i] = a.dual[i] / b; - } + out.real = a.real / b; + for (size_t i = 0; i < NVar; ++i) { out.dual[i] = a.dual[i] / b; } return out; } - // Scalar Division (scalar / dual) template - inline DualNumber_T operator/( - Scalar a, - const DualNumber_T& b - ) { + inline DualNumber_T operator/(Scalar a, const DualNumber_T& b) { DualNumber_T out; out.real = a / b.real; - for (size_t i = 0; i < NVar; ++i) { - out.dual[i] = -a * b.dual[i] / (b.real * b.real); - } + for (size_t i = 0; i < NVar; ++i) { out.dual[i] = -a * b.dual[i] / (b.real * b.real); } return out; } @@ -226,60 +144,39 @@ namespace mathlib { // Addition template - inline DualNumber_T operator+( - const DualNumber_T& a, - const DualNumber_T& b - ) { + inline DualNumber_T operator+(const DualNumber_T& a, const DualNumber_T& b) { DualNumber_T out; out.real = a.real + b.real; - for (size_t i = 0; i < NVar; ++i) { - out.dual[i] = a.dual[i] + b.dual[i]; - } + for (size_t i = 0; i < NVar; ++i) { out.dual[i] = a.dual[i] + b.dual[i]; } return out; } - // Subtraction template - inline DualNumber_T operator-( - const DualNumber_T& a, - const DualNumber_T& b - ) { + inline DualNumber_T operator-(const DualNumber_T& a, const DualNumber_T& b) { DualNumber_T out; out.real = a.real - b.real; - for (size_t i = 0; i < NVar; ++i) { - out.dual[i] = a.dual[i] - b.dual[i]; - } + for (size_t i = 0; i < NVar; ++i) { out.dual[i] = a.dual[i] - b.dual[i]; } return out; } - // Multiplication template - inline DualNumber_T operator*( - const DualNumber_T& a, - const DualNumber_T& b - ) { + inline DualNumber_T operator*(const DualNumber_T& a, const DualNumber_T& b) { DualNumber_T out; out.real = a.real * b.real; - for (size_t i = 0; i < NVar; ++i) { - out.dual[i] = a.real * b.dual[i] + a.dual[i] * b.real; - } + for (size_t i = 0; i < NVar; ++i) { out.dual[i] = a.real * b.dual[i] + a.dual[i] * b.real; } return out; } - // Division template - inline DualNumber_T operator/( - const DualNumber_T& a, - const DualNumber_T& b - ) { + inline DualNumber_T operator/(const DualNumber_T& a, const DualNumber_T& b) { + if (b.real == Scalar(0)) { throw std::runtime_error("Division by zero in dual number division"); } DualNumber_T out; - out.real = a.real / b.real; - for (size_t i = 0; i < NVar; ++i) { - out.dual[i] = (a.dual[i] * b.real - a.real * b.dual[i]) / (b.real * b.real); - } + const Scalar invDenom = Scalar(1) / b.real; + const Scalar invDenomSq = invDenom * invDenom; + out.real = a.real * invDenom; + for (size_t i = 0; i < NVar; ++i) { out.dual[i] = (a.dual[i] * b.real - a.real * b.dual[i]) * invDenomSq; } return out; } - // Power function (x^n) template inline DualNumber_T pow( @@ -287,134 +184,89 @@ namespace mathlib { Scalar n ) { DualNumber_T out; - if (a.real == Scalar(0) && n < Scalar(0)) { - throw std::runtime_error("Invalid dual power: division by zero"); - } + if (a.real == Scalar(0) && n < Scalar(0)) { throw std::runtime_error("Invalid dual power: division by zero"); } if (n == Scalar(0)) { out.real = Scalar(1); - for (size_t i = 0; i < NVar; ++i) { - out.dual[i] = Scalar(0); - } + for (size_t i = 0; i < NVar; ++i) { out.dual[i] = Scalar(0); } return out; } out.real = std::pow(a.real, n); - for (size_t i = 0; i < NVar; ++i) { - out.dual[i] = n * std::pow(a.real, n - Scalar(1)) * a.dual[i]; - } + for (size_t i = 0; i < NVar; ++i) { out.dual[i] = n * std::pow(a.real, n - Scalar(1)) * a.dual[i]; } return out; } - // Square root function template - inline DualNumber_T sqrt( - const DualNumber_T& a - ) { + inline DualNumber_T sqrt(const DualNumber_T& a) { + if (a.real < Scalar(0)) { throw std::runtime_error("sqrt() domain error for DualNumber_T"); } DualNumber_T out; - Scalar sqrtReal = std::sqrt(a.real); + const Scalar sqrtReal = std::sqrt(a.real); out.real = sqrtReal; - for (size_t i = 0; i < NVar; ++i) { - out.dual[i] = Scalar(0.5) * a.dual[i] / sqrtReal; + if (sqrtReal == Scalar(0)) { + for (size_t i = 0; i < NVar; ++i) { out.dual[i] = Scalar(0); } + return out; } + const Scalar inv2sqrt = Scalar(0.5) / sqrtReal; + for (size_t i = 0; i < NVar; ++i) { out.dual[i] = a.dual[i] * inv2sqrt; } return out; } - // Sine function template - inline DualNumber_T sin( - const DualNumber_T& a - ) { + inline DualNumber_T sin(const DualNumber_T& a) { DualNumber_T out; out.real = std::sin(a.real); - for (size_t i = 0; i < NVar; ++i) { - out.dual[i] = std::cos(a.real) * a.dual[i]; - } + for (size_t i = 0; i < NVar; ++i) { out.dual[i] = std::cos(a.real) * a.dual[i]; } return out; } - // Cosine function template - inline DualNumber_T cos( - const DualNumber_T& a - ) { + inline DualNumber_T cos(const DualNumber_T& a) { DualNumber_T out; out.real = std::cos(a.real); - for (size_t i = 0; i < NVar; ++i) { - out.dual[i] = -std::sin(a.real) * a.dual[i]; - } + for (size_t i = 0; i < NVar; ++i) { out.dual[i] = -std::sin(a.real) * a.dual[i]; } return out; } - // Tangent function template - inline DualNumber_T tan( - const DualNumber_T& a - ) { + inline DualNumber_T tan(const DualNumber_T& a) { DualNumber_T out; out.real = std::tan(a.real); - for (size_t i = 0; i < NVar; ++i) { - out.dual[i] = a.dual[i] / (std::cos(a.real) * std::cos(a.real)); - } + for (size_t i = 0; i < NVar; ++i) { out.dual[i] = a.dual[i] / (std::cos(a.real) * std::cos(a.real)); } return out; } - // Arctangent function template - inline DualNumber_T atan( - const DualNumber_T& a - ) { + inline DualNumber_T atan(const DualNumber_T& a) { DualNumber_T out; out.real = std::atan(a.real); - for (size_t i = 0; i < NVar; ++i) { - out.dual[i] = a.dual[i] / (Scalar(1) + a.real * a.real); - } + for (size_t i = 0; i < NVar; ++i) { out.dual[i] = a.dual[i] / (Scalar(1) + a.real * a.real); } return out; } - // Hyperbolic tangent function (tanh) template - inline DualNumber_T tanh( - const DualNumber_T& a - ) { + inline DualNumber_T tanh(const DualNumber_T& a) { DualNumber_T out; out.real = std::tanh(a.real); - for (size_t i = 0; i < NVar; ++i) { - out.dual[i] = a.dual[i] * (Scalar(1) - out.real * out.real); - } + for (size_t i = 0; i < NVar; ++i) { out.dual[i] = a.dual[i] * (Scalar(1) - out.real * out.real); } return out; } - // Exponential function template - inline DualNumber_T exp( - const DualNumber_T& a - ) { + inline DualNumber_T exp(const DualNumber_T& a) { DualNumber_T out; out.real = std::exp(a.real); - for (size_t i = 0; i < NVar; ++i) { - out.dual[i] = out.real * a.dual[i]; - } + for (size_t i = 0; i < NVar; ++i) { out.dual[i] = out.real * a.dual[i]; } return out; } - // Smooth step function for smooth interpolation between 0 and 1 template - inline Scalar smoothStep( - const Scalar& x - ) { - return x * x * (Scalar(3) - Scalar(2) * x); - } - + inline Scalar smoothStep(const Scalar& x) { return x * x * (Scalar(3) - Scalar(2) * x); } // Smooth step function for dual numbers (applies smooth step to the real part, scales dual part by the derivative of the smooth step) template - inline DualNumber_T smoothStep( - const DualNumber_T& x - ) { + inline DualNumber_T smoothStep(const DualNumber_T& x) { DualNumber_T out; out.real = smoothStep(x.real); Scalar derivative = Scalar(6) * x.real * (Scalar(1) - x.real); // Derivative of smooth step with respect to x - for (size_t i = 0; i < NVar; ++i) { - out.dual[i] = derivative * x.dual[i]; - } + for (size_t i = 0; i < NVar; ++i) { out.dual[i] = derivative * x.dual[i]; } return out; } @@ -424,108 +276,59 @@ namespace mathlib { // Addition assignment operator template - inline DualNumber_T& operator+=( - DualNumber_T& a, - const DualNumber_T& b - ) { + inline DualNumber_T& operator+=(DualNumber_T& a, const DualNumber_T& b) { a.real += b.real; - for (size_t i = 0; i < NVar; ++i) { - a.dual[i] += b.dual[i]; - } + for (size_t i = 0; i < NVar; ++i) { a.dual[i] += b.dual[i]; } return a; } // Addition assignment operator with scalar template - inline DualNumber_T& operator+=( - DualNumber_T& a, - Scalar b - ) { + inline DualNumber_T& operator+=(DualNumber_T& a, Scalar b) { a.real += b; return a; } - // Subtraction assignment operator template - inline DualNumber_T& operator-=( - DualNumber_T& a, - const DualNumber_T& b - ) { + inline DualNumber_T& operator-=(DualNumber_T& a, const DualNumber_T& b) { a.real -= b.real; - for (size_t i = 0; i < NVar; ++i) { - a.dual[i] -= b.dual[i]; - } + for (size_t i = 0; i < NVar; ++i) { a.dual[i] -= b.dual[i]; } return a; } // Subtraction assignment operator with scalar template - inline DualNumber_T& operator-=( - DualNumber_T& a, - Scalar b - ) { + inline DualNumber_T& operator-=(DualNumber_T& a, Scalar b) { a.real -= b; return a; } - // Scalar multiplication assignment operator template - inline DualNumber_T& operator*=( - DualNumber_T& a, - Scalar b - ) { + inline DualNumber_T& operator*=(DualNumber_T& a, Scalar b) { a.real *= b; - for (size_t i = 0; i < NVar; ++i) { - a.dual[i] *= b; - } + for (size_t i = 0; i < NVar; ++i) { a.dual[i] *= b; } return a; } - // Element-wise multiplication assignment operator template - inline DualNumber_T& operator*=( - DualNumber_T& a, - const DualNumber_T& b - ) { + inline DualNumber_T& operator*=(DualNumber_T& a, const DualNumber_T& b) { Scalar ogReal = a.real; - for (size_t i = 0; i < NVar; ++i) { - a.dual[i] = ogReal * b.dual[i] + a.dual[i] * b.real; - } + for (size_t i = 0; i < NVar; ++i) { a.dual[i] = ogReal * b.dual[i] + a.dual[i] * b.real; } a.real = ogReal * b.real; return a; } - // Scalar division assignment operator template - inline DualNumber_T& operator/=( - DualNumber_T& a, - Scalar b - ) { - if (b == Scalar(0)) { - throw std::runtime_error( - "Division by zero in DualNumber_T operator/=" - ); - } + inline DualNumber_T& operator/=(DualNumber_T& a, Scalar b) { + if (b == Scalar(0)) { throw std::runtime_error("Division by zero in DualNumber_T operator/="); } a.real /= b; - for (size_t i = 0; i < NVar; ++i) { - a.dual[i] /= b; - } + for (size_t i = 0; i < NVar; ++i) { a.dual[i] /= b; } return a; } - // Element-wise division assignment operator template - inline DualNumber_T& operator/=( - DualNumber_T& a, - const DualNumber_T& b - ) { + inline DualNumber_T& operator/=(DualNumber_T& a, const DualNumber_T& b) { Scalar ogReal = a.real; - for (size_t i = 0; i < NVar; ++i) { - a.dual[i] = (a.dual[i] * b.real - ogReal * b.dual[i]) / (b.real * b.real); - } - if (b.real == Scalar(0)) { - throw std::runtime_error( - "Division by zero in DualNumber_T operator/=" - ); - } + for (size_t i = 0; i < NVar; ++i) { a.dual[i] = (a.dual[i] * b.real - ogReal * b.dual[i]) / (b.real * b.real); } + if (b.real == Scalar(0)) { throw std::runtime_error("Division by zero in DualNumber_T operator/="); } a.real = ogReal / b.real; return a; } @@ -537,154 +340,88 @@ namespace mathlib { // Function to return a DualNumber_T representing infinity (real part is infinity, dual parts are zero) template inline DualNumber_T numeric_limits_infinity() { - return DualNumber_T( - std::numeric_limits::infinity(), - std::array() - ); + return DualNumber_T(std::numeric_limits::infinity(), std::array()); } // Alternative absolute value function that simply negates the dual number if the real part is negative (this is less smooth but can be more efficient and avoids issues with zero) template - inline DualNumber_T abs( - const DualNumber_T& a - ) { + inline DualNumber_T abs(const DualNumber_T& a) { DualNumber_T out; out = (a.real < Scalar(0)) ? -a : a; return out; } // Absolute value function with epsilon threshold to avoid non-differentiability at zero (scales dual part by the sign of the real part, but treats values within epsilon of zero as zero) template - inline DualNumber_T abs( - const DualNumber_T& a, - Scalar epsilon - ) { + inline DualNumber_T abs(const DualNumber_T& a, Scalar epsilon) { DualNumber_T out; if (std::abs(a.real) < epsilon) { out.real = Scalar(0); - for (size_t i = 0; i < NVar; ++i) { - out.dual[i] = Scalar(0); - } + for (size_t i = 0; i < NVar; ++i) { out.dual[i] = Scalar(0); } } else { out.real = std::abs(a.real); Scalar sign = (a.real > Scalar(0)) - (a.real < Scalar(0)); // Sign of the real part - for (size_t i = 0; i < NVar; ++i) { - out.dual[i] = sign * a.dual[i]; - } + for (size_t i = 0; i < NVar; ++i) { out.dual[i] = sign * a.dual[i]; } } return out; } - - // Sign function for DualNumber_T (returns the sign of the real part, dual parts are zero) template - inline DualNumber_T sign( - const DualNumber_T& a - ) { + inline DualNumber_T sign(const DualNumber_T& a) { DualNumber_T out; out.real = (a.real > Scalar(0)) - (a.real < Scalar(0)); // Sign of the real part - for (size_t i = 0; i < NVar; ++i) { - out.dual[i] = Scalar(0); - } + for (size_t i = 0; i < NVar; ++i) { out.dual[i] = Scalar(0); } return out; } - // Minimum function for DualNumber_T (returns the minimum of the real parts, scales dual part by the indicator of which real part is smaller) template - inline DualNumber_T min( - const DualNumber_T& a, - const DualNumber_T& b - ) { + inline DualNumber_T min(const DualNumber_T& a, const DualNumber_T& b) { DualNumber_T out; if (a.real < b.real) { out.real = a.real; - for (size_t i = 0; i < NVar; ++i) { - out.dual[i] = a.dual[i]; - } + for (size_t i = 0; i < NVar; ++i) { out.dual[i] = a.dual[i]; } } else { out.real = b.real; - for (size_t i = 0; i < NVar; ++i) { - out.dual[i] = b.dual[i]; - } + for (size_t i = 0; i < NVar; ++i) { out.dual[i] = b.dual[i]; } } return out; } - // Maximum function for DualNumber_T (returns the maximum of the real parts, scales dual part by the indicator of which real part is larger) template - inline DualNumber_T max( - const DualNumber_T& a, - const DualNumber_T& b - ) { + inline DualNumber_T max(const DualNumber_T& a, const DualNumber_T& b) { DualNumber_T out; if (a.real > b.real) { out.real = a.real; - for (size_t i = 0; i < NVar; ++i) { - out.dual[i] = a.dual[i]; - } + for (size_t i = 0; i < NVar; ++i) { out.dual[i] = a.dual[i]; } } else { out.real = b.real; - for (size_t i = 0; i < NVar; ++i) { - out.dual[i] = b.dual[i]; - } + for (size_t i = 0; i < NVar; ++i) { out.dual[i] = b.dual[i]; } } return out; } // Since dual numbers are not complex, the real part is just the real part of the dual number (for non-dual types, this is just the value itself) template - inline T real(const T& x) { - return x; - } - + inline T real(const T& x) { return x; } // Since dual numbers are not complex, the real part is just the real part of the dual number template - inline Scalar real( - const DualNumber_T& a - ) { - return a.real; - } - + inline Scalar real(const DualNumber_T& a) { return a.real; } // Since dual numbers are not complex, the imaginary part is always zero template - inline Scalar imaginary( - const DualNumber_T& /*a*/ - ) { - return Scalar(0); - } - + inline Scalar imaginary(const DualNumber_T& /*a*/) { return Scalar(0); } // Since dual numbers are not complex, the complex conjugate is just the dual number itself template - inline DualNumber_T conj( - const DualNumber_T& a - ) { - return a; - } - + inline DualNumber_T conj(const DualNumber_T& a) { return a; } // Since dual numbers are not complex, the norm is just the absolute value of the real part template - inline Scalar norm( - const DualNumber_T& a - ) { - return std::abs(a.real); - } - + inline Scalar norm(const DualNumber_T& a) { return std::abs(a.real); } // Since dual numbers are not complex, the squared norm is just the square of the absolute value of the real part template - inline Scalar abs2( - const DualNumber_T& a - ) { - return a.real * a.real; - } - + inline Scalar abs2(const DualNumber_T& a) { return a.real * a.real; } template - inline bool is_dual_number_v( - const DualNumber_T& /*a*/ - ) { - return true; - } + inline bool is_dual_number_v(const DualNumber_T& /*a*/) { return true; } // Traits class to identify dual numbers and extract the base scalar type template @@ -692,7 +429,6 @@ namespace mathlib { using BaseScalar = T; static constexpr bool is_dual = false; }; - // Specialization of DualTraits for DualNumber_T template struct DualTraits> { @@ -702,34 +438,74 @@ namespace mathlib { // Dual Part template - Scalar dualPart( - const DualNumber_T& x - ) { - return x.dual[0]; + Scalar dualPart(const DualNumber_T& x) { return x.dual[0]; } + // Multiple Dual Parts (returns the entire dual part as an array) + template, int> = 0> + inline DualNumber_T operator*(T a, const DualNumber_T& b) { return b * static_cast(a); } + // Multiple Dual Parts (returns the entire dual part as an array) + template + inline std::ostream& operator<<(std::ostream& os, const DualNumber_T& d) { + os << "{ real: " << d.real << ", dual: ["; + for (size_t i = 0; i < NVar; ++i) { + os << d.dual[i]; + if (i + 1 < NVar) { + os << ", "; + } + } + os << "] }"; + return os; } + // Check if the real part and all dual parts are finite + template + inline bool isFinite(const DualNumber_T& x) { + if (!std::isfinite(x.real)) { return false; } - template - auto makeMutableCopy(const Eigen::MatrixBase& x) - -> typename std::decay_t::PlainObject { - return x.derived().eval(); + for (size_t i = 0; i < NVar; ++i) { + if (!std::isfinite(x.dual[i])) { return false; } + } + + return true; } - template - inline DualNumber_T operator*( - T a, - const DualNumber_T& b + // ----- + // Eigen Matrix Interactions + // ----- + + // Helper function to create a mutable copy of an Eigen expression + template + auto makeMutableCopy(const Eigen::MatrixBase& x) -> typename std::decay_t::PlainObject { return x.derived().eval(); } + // Element-wise addition for Eigen matrices of dual numbers + template + inline Eigen::Matrix, Eigen::Dynamic, Eigen::Dynamic> operator*( + const Eigen::Matrix, Eigen::Dynamic, Eigen::Dynamic>& a, + const Eigen::Matrix, Eigen::Dynamic, Eigen::Dynamic>& b ) { - return b * static_cast(a); + // (a.real() * b.real(), a.real() * b.dual() + a.dual() * b.real()) + return a.derived() * b.derived(); + } + // Element-wise division for Eigen matrices of dual numbers + template + inline Eigen::Matrix, Eigen::Dynamic, Eigen::Dynamic> operator/( + const Eigen::Matrix, Eigen::Dynamic, Eigen::Dynamic>& a, + const Eigen::Matrix, Eigen::Dynamic, Eigen::Dynamic>& b + ) { + // (a.real() / b.real(), (a.dual() * b.real() - a.real() * b.dual()) / (b.real() * b.real())) + return a.derived() / b.derived(); } } +// Specialisation of Eigen's NumTraits for DualNumber_T to allow Eigen to work with dual numbers in its expressions and algorithms namespace Eigen { - // Specialization of NumTraits for DualNumber_T to allow it to be used with Eigen's matrix and vector types template - struct NumTraits> : GenericNumTraits> { - typedef mathlib::DualNumber_T Real; - typedef mathlib::DualNumber_T NonInteger; - typedef mathlib::DualNumber_T Nested; + struct NumTraits> + : GenericNumTraits> { + using Dual = mathlib::DualNumber_T; + // Define the types that Eigen uses for various purposes when working with DualNumber_T + typedef Scalar Real; + typedef Dual NonInteger; + typedef Dual Nested; + typedef Scalar Literal; + // Define properties of DualNumber_T for Eigen's internal use enum { IsComplex = 0, IsInteger = 0, @@ -739,17 +515,61 @@ namespace Eigen { AddCost = 3, MulCost = 3 }; - static EIGEN_DEVICE_FUNC Real epsilon() { - return Real(std::numeric_limits::epsilon()); - } - static EIGEN_DEVICE_FUNC Real dummy_precision() { - return Real(Scalar(1e-12)); - } - static EIGEN_DEVICE_FUNC Real highest() { - return Real(std::numeric_limits::max()); - } - static EIGEN_DEVICE_FUNC Real lowest() { - return Real(std::numeric_limits::lowest()); - } + // Set of methods to provide numeric limits for DualNumber_T + static inline Real epsilon() { return std::numeric_limits::epsilon(); } + static inline Real dummy_precision() { return Real(1e-12); } + static inline Real highest() { return std::numeric_limits::max(); } + static inline Real lowest() { return std::numeric_limits::lowest(); } + }; +} // namespace Eigen +// Specialisation of Eigen's internal traits to ensure that DualNumber_T is treated as a non-arithmetic type +namespace Eigen::internal { + template + struct is_arithmetic> : std::false_type {}; + + template + struct scalar_abs_op> { + using Dual = mathlib::DualNumber_T; + + EIGEN_DEVICE_FUNC Scalar operator()(const Dual& x) const { return std::abs(x.real); } }; -} \ No newline at end of file + + // Custom scoring function for DualNumber_T that compares based on the absolute value of the real part + template + struct scalar_score_coeff_op> { + // The result_type is a simple wrapper around the score (which is the absolute value of the real part of the dual number) + struct result_type { + Scalar score; + result_type(int i = 0) : score(i) {} // Default constructor for compatibility with Eigen's internal mechanisms + result_type(const mathlib::DualNumber_T& x) // Constructor to compute score from DualNumber_T + : score(std::abs(x.real)) {} + + friend bool operator <(const result_type& a, const result_type& b) { return a.score < b.score; } + friend bool operator >(const result_type& a, const result_type& b) { return a.score > b.score; } + friend bool operator ==(const result_type& a, const result_type& b) { return a.score == b.score; } + }; + // The operator() computes the score for a given DualNumber_T by taking the absolute value of its real part (this allows Eigen's internal sorting and selection algorithms to work correctly with dual numbers) + EIGEN_DEVICE_FUNC result_type operator()(const mathlib::DualNumber_T& x) const { return result_type(x); } + }; +} // namespace Eigen::internal +// Specialisation of Eigen's numext functions for DualNumber_T to allow Eigen's algorithms that rely on these functions (like abs, real, imag, conj) to work correctly with dual numbers +namespace Eigen::numext { + // Returns the real part of the dual number + template + inline Scalar real(const mathlib::DualNumber_T& x) { return x.real; } + // Returns the imaginary part of the dual number + template + inline Scalar imag(const mathlib::DualNumber_T&) { return Scalar(0); } + // Returns the absolute value of the dual number + template + inline Scalar abs(const mathlib::DualNumber_T& x) { return std::abs(x.real); } + // Returns the squared absolute value of the dual number + template + inline Scalar abs2(const mathlib::DualNumber_T& x) { return x.real * x.real; } + // Returns the 1-norm of the dual number + template + inline Scalar norm1(const mathlib::DualNumber_T& x) { return std::abs(x.real); } + // Returns the 2-norm of the dual number + template + inline mathlib::DualNumber_T conj(const mathlib::DualNumber_T& x) { return x; } +}// namespace Eigen::numext \ No newline at end of file diff --git a/PxMLib/MathLib/include/integrators/numerical_integrators.inl b/PxMLib/MathLib/include/integrators/numerical_integrators.inl index da793f4a..b5ddea61 100644 --- a/PxMLib/MathLib/include/integrators/numerical_integrators.inl +++ b/PxMLib/MathLib/include/integrators/numerical_integrators.inl @@ -858,7 +858,7 @@ namespace integration { mathlib::VecX_T x = x0, g, x_trial; mathlib::VecX_T delta = mathlib::VecX_T::Constant(x0.size(), std::numeric_limits::infinity()); mathlib::MatX_T J; - Eigen::ColPivHouseholderQR> solver; + Eigen::PartialPivLU> solver; for (int iter = 0; iter < maxIter; ++iter) { eval_g(x, g); @@ -923,7 +923,7 @@ namespace integration { using BaseScalar = typename mathlib::DualTraits::BaseScalar; mathlib::VecX_T delta = mathlib::VecX_T::Constant(x0.size(), Scalar(std::numeric_limits::infinity())); mathlib::MatX_T J; - Eigen::ColPivHouseholderQR> solver; + Eigen::PartialPivLU> solver; for (int iter = 0; iter < maxIter; ++iter) { eval_g(x, g); if (!g.allFinite()) { diff --git a/PxMLib/MathLib_Unit/AutomaticDifferentiationTests/CMakeLists.txt b/PxMLib/MathLib_Unit/AutomaticDifferentiationTests/CMakeLists.txt index 4652f2d8..be6c8bf9 100644 --- a/PxMLib/MathLib_Unit/AutomaticDifferentiationTests/CMakeLists.txt +++ b/PxMLib/MathLib_Unit/AutomaticDifferentiationTests/CMakeLists.txt @@ -10,6 +10,10 @@ add_executable(MathLib_ADTests ADJacobianTests.cpp ADExplicitIntegratorTests.cpp ADImplicitIntegratorTests.cpp + DualNumber_Eigen_BasicTests.cpp + DualNumber_Eigen_ArithmeticTests.cpp + DualNumber_Eigen_SolverTests.cpp + DualNumber_Eigen_ADTests.cpp ) target_link_libraries(MathLib_ADTests PRIVATE diff --git a/PxMLib/MathLib_Unit/AutomaticDifferentiationTests/DualNumber_Eigen_ADTests.cpp b/PxMLib/MathLib_Unit/AutomaticDifferentiationTests/DualNumber_Eigen_ADTests.cpp new file mode 100644 index 00000000..101fdec1 --- /dev/null +++ b/PxMLib/MathLib_Unit/AutomaticDifferentiationTests/DualNumber_Eigen_ADTests.cpp @@ -0,0 +1,9 @@ +#include "TestHarness.h" +#include + +using namespace mathlib; + +namespace { + using Dual = DualNumber_T; + constexpr double EPS = 1e-9; +} \ No newline at end of file diff --git a/PxMLib/MathLib_Unit/AutomaticDifferentiationTests/DualNumber_Eigen_ArithmeticTests.cpp b/PxMLib/MathLib_Unit/AutomaticDifferentiationTests/DualNumber_Eigen_ArithmeticTests.cpp new file mode 100644 index 00000000..44f943c6 --- /dev/null +++ b/PxMLib/MathLib_Unit/AutomaticDifferentiationTests/DualNumber_Eigen_ArithmeticTests.cpp @@ -0,0 +1,73 @@ +#include "TestHarness.h" +#include + +using namespace mathlib; + +namespace { + using Dual = DualNumber_T; + constexpr double EPS = 1e-9; +} + +TEST("DualNumber_T Eigen Arithmetic", MatrixConstruction) { + Eigen::Matrix M; + M.setZero(); + ASSERT_TRUE(M(0, 0).real == 0.0 && M(0, 0).dual[0] == 0.0, "M(0, 0) should be zero"); + ASSERT_TRUE(M(1, 1).real == 0.0 && M(1, 1).dual[0] == 0.0, "M(1, 1) should be zero"); +} + +TEST("DualNumber_T Eigen Arithmetic", MatrixAddition) { + Eigen::Matrix A, B; + A(0, 0) = Dual(1.0, { 0.5 }); + A(1, 1) = Dual(2.0, { 1.5 }); + B(0, 0) = Dual(3.0, { 2.5 }); + B(1, 1) = Dual(4.0, { 3.5 }); + Eigen::Matrix C = A + B; + ASSERT_TRUE(C(0, 0).real == 4.0 && C(0, 0).dual[0] == 3.0, "C(0, 0) should be the sum of A and B"); + ASSERT_TRUE(C(1, 1).real == 6.0 && C(1, 1).dual[0] == 5.0, "C(1, 1) should be the sum of A and B"); +} + +TEST("DualNumber_T Eigen Arithmetic", MatrixSubtraction) { + Eigen::Matrix A, B; + A(0, 0) = Dual(1.0, { 0.5 }); + A(1, 1) = Dual(2.0, { 1.5 }); + B(0, 0) = Dual(3.0, { 2.5 }); + B(1, 1) = Dual(4.0, { 3.5 }); + Eigen::Matrix C = A - B; + ASSERT_TRUE(C(0, 0).real == -2.0 && C(0, 0).dual[0] == -2.0, + "C(0, 0) should be the difference of A and B"); + ASSERT_TRUE(C(1, 1).real == -2.0 && C(1, 1).dual[0] == -2.0, + "C(1, 1) should be the difference of A and B"); +} + +TEST("DualNumber_T Eigen Arithmetic", MatrixMultiplication) { + Eigen::Matrix A, B; + A(0, 0) = Dual(1.0, { 0.5 }); + A(1, 1) = Dual(2.0, { 1.5 }); + B(0, 0) = Dual(3.0, { 2.5 }); + B(1, 1) = Dual(4.0, { 3.5 }); + // (a + a'ε)(b + b'ε) = ab + (ab' + a'b)ε + Eigen::Matrix C = A * B; + ASSERT_TRUE(C(0, 0).real == 3.0 && C(0, 0).dual[0] == 4.0, + "C(0, 0) should be the product of A and B"); + ASSERT_TRUE(C(1, 1).real == 8.0 && C(1, 1).dual[0] == 13.0, + "C(1, 1) should be the product of A and B"); +} + +TEST("DualNumber_T Eigen Arithmetic", DotProduct) { + Eigen::Matrix a, b; + a << Dual(1.0, { 0.5 }), Dual(2.0, { 1.5 }); + b << Dual(3.0, { 2.5 }), Dual(4.0, { 3.5 }); + Dual dotProduct = a.dot(b); + ASSERT_TRUE(dotProduct.real == 11.0 && dotProduct.dual[0] == 17.0, + "The dot product should be the sum of the products of corresponding elements"); +} + +//TEST("DualNumber_T Eigen Arithmetic", MatrixScalarMultiplication) { +// Eigen::Matrix A; +// A(0, 0) = Dual(1.0, { 0.5 }); +// A(1, 1) = Dual(2.0, { 1.5 }); +// double scalar = 2.0; +// Eigen::Matrix B = A * scalar; +// ASSERT_TRUE(B(0, 0).real == 2.0 && B(0, 0).dual[0] == 1.0, "B(0, 0) should be the product of A and scalar"); +// ASSERT_TRUE(B(1, 1).real == 4.0 && B(1, 1).dual[0] == 3.0, "B(1, 1) should be the product of A and scalar"); +//} \ No newline at end of file diff --git a/PxMLib/MathLib_Unit/AutomaticDifferentiationTests/DualNumber_Eigen_BasicTests.cpp b/PxMLib/MathLib_Unit/AutomaticDifferentiationTests/DualNumber_Eigen_BasicTests.cpp new file mode 100644 index 00000000..e97d9f70 --- /dev/null +++ b/PxMLib/MathLib_Unit/AutomaticDifferentiationTests/DualNumber_Eigen_BasicTests.cpp @@ -0,0 +1,115 @@ +#include "TestHarness.h" +#include + +using namespace mathlib; + +namespace { + using Dual = DualNumber_T; + constexpr double EPS = 1e-9; +} + +TEST("DualNumber_T Basic Function", DefaultConstruction) { + Dual d; + ASSERT_TRUE(d.real == 0.0, "Default real part should be 0"); + ASSERT_TRUE(d.dual[0] == 0.0, "Default dual part should be 0"); +} + +TEST("DualNumber_T Basic Function", ArithmeticOperations_Additions) { + Dual a(1.0, { 0.5 }); + Dual b(2.0, { 1.5 }); + // Test addition + Dual c = a + b; + ASSERT_TRUE(c.real == 3.0, "Addition real part incorrect"); + ASSERT_TRUE(c.dual[0] == 2.0, "Addition dual part incorrect"); +} + +TEST("DualNumber_T Basic Function", ArithmeticOperations_Subtraction) { + Dual a(1.0, { 0.5 }); + Dual b(2.0, { 1.5 }); + // Test subtraction + Dual c = a - b; + ASSERT_TRUE(c.real == -1.0, "Subtraction real part incorrect"); + ASSERT_TRUE(c.dual[0] == -1.0, "Subtraction dual part incorrect"); +} + +TEST("DualNumber_T Basic Function", ArithmeticOperations_Multiplication) { + Dual a(1.0, { 0.5 }); + Dual b(2.0, { 1.5 }); + // Test multiplication + Dual c = a * b; + ASSERT_TRUE(c.real == 2.0, "Multiplication real part incorrect"); + ASSERT_TRUE(c.dual[0] == 2.5, "Multiplication dual part incorrect"); +} + +TEST("DualNumber_T Basic Function", ArithmeticOperations_Division) { + Dual a(1.0, { 0.5 }); + Dual b(2.0, { 1.5 }); + // Test division + Dual c = a / b; + ASSERT_TRUE(std::abs(c.real - 0.5) < EPS, "Division real part incorrect"); + ASSERT_TRUE(std::abs(c.dual[0] - 0.375) < EPS, "Division dual part incorrect"); +} + +TEST("DualNumber_T Basic Function", ScalarPromotion_Multiplcation) { + Dual a(1.0, { 0.5 }); + double scalar = 2.0; + // Test scalar multiplication + Dual c = a * scalar; + ASSERT_TRUE(c.real == 2.0, "Scalar multiplication real part incorrect"); + ASSERT_TRUE(c.dual[0] == 1.0, "Scalar multiplication dual part incorrect"); +} + +TEST("DualNumber_T Basic Function", ScalarPromotion_Division) { + Dual a(1.0, { 0.5 }); + double scalar = 2.0; + // Test scalar division + Dual c = a / scalar; + ASSERT_TRUE(c.real == 0.5, "Scalar division real part incorrect"); + ASSERT_TRUE(c.dual[0] == 0.25, "Scalar division dual part incorrect"); +} + +TEST("DualNumber_T Basic Function", ScalarPromotion_Division_ScalarByDual) { + Dual a(2.0, { 1.5 }); + double scalar = 1.0; + // Test scalar divided by dual + Dual c = scalar / a; + ASSERT_TRUE(c.real == 0.5, "Scalar by dual division real part incorrect"); + ASSERT_TRUE(c.dual[0] == -0.375, "Scalar by dual division dual part incorrect"); +} + +TEST("DualNumber_T Basic Function", ComparisonOperators_GreaterThan) { + Dual a(1.0, { 0.5 }); + Dual b(2.0, { 1.5 }); + // Test greater than + ASSERT_TRUE(b.real > a.real, "Greater than comparison failed"); +} +TEST("DualNumber_T Basic Function", ComparisonOperators_LessThan) { + Dual a(1.0, { 0.5 }); + Dual b(2.0, { 1.5 }); + // Test less than + ASSERT_TRUE(a.real < b.real, "Less than comparison failed"); +} +TEST("DualNumber_T Basic Function", ComparisonOperators_Equality) { + Dual a(1.0, { 0.5 }); + Dual b(1.0, { 0.5 }); + // Test equality + ASSERT_TRUE(a.real == b.real && a.dual[0] == b.dual[0], "Equality comparison failed"); +} +TEST("DualNumber_T Basic Function", ComparisonOperators_Inequality) { + Dual a(1.0, { 0.5 }); + Dual b(1.0, { 0.6 }); + // Test inequality + ASSERT_TRUE(a.real == b.real && a.dual[0] != b.dual[0], "Inequality comparison failed"); +} +TEST("DualNumber_T Basic Function", ComparisonOperators_GreaterThanOrEqual) { + Dual a(1.0, { 0.5 }); + Dual b(1.0, { 0.5 }); + // Test greater than or equal + ASSERT_TRUE(a.real >= b.real && a.dual[0] >= b.dual[0], "Greater than or equal comparison failed"); +} +TEST("DualNumber_T Basic Function", ComparisonOperators_LessThanOrEqual) { + Dual a(1.0, { 0.5 }); + Dual b(1.0, { 0.5 }); + // Test less than or equal + ASSERT_TRUE(a.real <= b.real && a.dual[0] <= b.dual[0], "Less than or equal comparison failed"); +} \ No newline at end of file diff --git a/PxMLib/MathLib_Unit/AutomaticDifferentiationTests/DualNumber_Eigen_SolverTests.cpp b/PxMLib/MathLib_Unit/AutomaticDifferentiationTests/DualNumber_Eigen_SolverTests.cpp new file mode 100644 index 00000000..10ce3604 --- /dev/null +++ b/PxMLib/MathLib_Unit/AutomaticDifferentiationTests/DualNumber_Eigen_SolverTests.cpp @@ -0,0 +1,53 @@ +#include "TestHarness.h" +#include + +using namespace mathlib; + +namespace { + using Dual = DualNumber_T; + constexpr double EPS = 1e-9; +} + +TEST("DualNumber_T Eigen Solvers", PartialPivLU) { + Eigen::Matrix A; + Eigen::Matrix b; + + A(0, 0) = Dual(4.0, { 0.5 }); + A(0, 1) = Dual(2.0, { 0.5 }); + A(1, 0) = Dual(1.0, { 0.5 }); + A(1, 1) = Dual(3.0, { 0.5 }); + b << Dual(10.0, { 1.0 }), Dual(11.0, { 1.0 }); + + auto solver = A.partialPivLu(); + + auto x = solver.solve(b); + + ASSERT_TRUE(std::isfinite(x(0).real), "Real part of solution should be finite"); + ASSERT_TRUE(std::isfinite(x(0).dual[0]), "Dual part of solution should be finite"); +} + +TEST("DualNumber_T Eigen Solvers", MaxCoeff) { + Eigen::Matrix A; + + A(0, 0) = Dual(4.0); + A(0, 1) = Dual(2.0); + A(1, 0) = Dual(1.0); + A(1, 1) = Dual(3.0); + + auto m = A.maxCoeff(); + + ASSERT_TRUE(std::isfinite(m.real), "Max coefficient real part should be finite"); +} + +TEST("DualNumber_T Eigen Solvers", CwiseAbsMaxCoeff) { + Eigen::Matrix A; + + A(0, 0) = Dual(4.0); + A(0, 1) = Dual(2.0); + A(1, 0) = Dual(1.0); + A(1, 1) = Dual(3.0); + + auto m = A.cwiseAbs().maxCoeff(); + + ASSERT_TRUE(std::isfinite(m), "CwiseAbsMaxCoeff real part should be finite"); +} \ No newline at end of file From 28ad0571c826c1fe7c8b5fb0d7a45d263923fb67 Mon Sep 17 00:00:00 2001 From: saltyjoss Date: Tue, 19 May 2026 17:47:19 +0100 Subject: [PATCH 038/210] chore: syntax corrections --- .../ADExplicitIntegratorTests.cpp | 224 ++-- .../ADImplicitIntegratorTests.cpp | 1102 ++++++++--------- .../ADJacobianTests.cpp | 222 ++-- .../ADNewtonTests.cpp | 56 +- 4 files changed, 802 insertions(+), 802 deletions(-) diff --git a/PxMLib/MathLib_Unit/AutomaticDifferentiationTests/ADExplicitIntegratorTests.cpp b/PxMLib/MathLib_Unit/AutomaticDifferentiationTests/ADExplicitIntegratorTests.cpp index d47d0fb4..31582469 100644 --- a/PxMLib/MathLib_Unit/AutomaticDifferentiationTests/ADExplicitIntegratorTests.cpp +++ b/PxMLib/MathLib_Unit/AutomaticDifferentiationTests/ADExplicitIntegratorTests.cpp @@ -89,122 +89,122 @@ namespace { } integration::NumericalIntegrator integrator; - - // Test that the Euler method correctly integrates the exponential decay ODE, and that the dual number derivatives match the analytical derivatives. - TEST("AD Euler Method", Euler_ExponentialDecaySensitivity) { - auto stepFn = [&]( - const VecX_T>& x, - DualNumber_T t, - DualNumber_T dt +} + +// Test that the Euler method correctly integrates the exponential decay ODE, and that the dual number derivatives match the analytical derivatives. +TEST("AD Euler Method", Euler_ExponentialDecaySensitivity) { + auto stepFn = [&]( + const VecX_T>& x, + DualNumber_T t, + DualNumber_T dt ) { - return integrator.eulerStep(x, t, dt, expDecay); - }; - - verifyExpDecaySensitivity(stepFn, 1e-2, 1000); - } - // Test that the Euler method correctly integrates a simple harmonic oscillator, and that the dual number derivatives match the analytical derivatives. - TEST("AD Euler Method", Euler_HarmonicOscillatorSensitivity) { - auto stepFn = [&]( - const VecX_T>& x, - DualNumber_T t, - DualNumber_T dt + return integrator.eulerStep(x, t, dt, expDecay); + }; + + verifyExpDecaySensitivity(stepFn, 1e-2, 1000); +} +// Test that the Euler method correctly integrates a simple harmonic oscillator, and that the dual number derivatives match the analytical derivatives. +TEST("AD Euler Method", Euler_HarmonicOscillatorSensitivity) { + auto stepFn = [&]( + const VecX_T>& x, + DualNumber_T t, + DualNumber_T dt ) { - return integrator.eulerStep(x, t, dt, harmonicOsc); - }; - verifyHarmonicOscillatorSensitivity(stepFn, 5e-2, 5e-2, 1e-1, 2000); - } - - // Tests that the Midpoint method (RK2) correctly integrates the exponential decay ODE, and that the dual number derivatives match the analytical derivatives. - TEST("AD Midpoint Method (RK2)", Midpoint_ExponentialDecaySensitivity) { - auto stepFn = [&]( - const VecX_T>& x, - DualNumber_T t, - DualNumber_T dt - ) { - return integrator.midpointStep(x, t, dt, expDecay); - }; - verifyExpDecaySensitivity(stepFn, 1e-5, 500); - } - // Test that the Midpoint method (RK2) correctly integrates a simple harmonic oscillator, and that the dual number derivatives match the analytical derivatives. - TEST("AD Midpoint Method (RK2)", Midpoint_HarmonicOscillatorSensitivity) { - auto stepFn = [&]( - const VecX_T>& x, - DualNumber_T t, - DualNumber_T dt - ) { - return integrator.midpointStep(x, t, dt, harmonicOsc); - }; - verifyHarmonicOscillatorSensitivity(stepFn, 5e-4, 5e-4, 5e-3, 1000); - } - - // Tests that the Heun method (RK2) correctly integrates the exponential decay ODE, and that the dual number derivatives match the analytical derivatives. - TEST("AD Heun Method (RK2)", Heun_ExponentialDecaySensitivity) { - auto stepFn = [&]( - const VecX_T>& x, - DualNumber_T t, - DualNumber_T dt - ) { - return integrator.heunStep(x, t, dt, expDecay); - }; - verifyExpDecaySensitivity(stepFn, 1e-5, 500); - } - // Test that the Heun method (RK2) correctly integrates a simple harmonic oscillator, and that the dual number derivatives match the analytical derivatives. - TEST("AD Heun Method (RK2)", Heun_HarmonicOscillatorSensitivity) { - auto stepFn = [&]( - const VecX_T>& x, - DualNumber_T t, - DualNumber_T dt - ) { - return integrator.heunStep(x, t, dt, harmonicOsc); - }; - verifyHarmonicOscillatorSensitivity(stepFn, 5e-4, 5e-4, 5e-3, 1000); - } - - // Tests that the Ralston method (RK2) correctly integrates the exponential decay ODE, and that the dual number derivatives match the analytical derivatives. - TEST("AD Ralston Method (RK2)", Ralston_ExponentialDecaySensitivity) { - auto stepFn = [&]( - const VecX_T>& x, - DualNumber_T t, - DualNumber_T dt + return integrator.eulerStep(x, t, dt, harmonicOsc); + }; + verifyHarmonicOscillatorSensitivity(stepFn, 5e-2, 5e-2, 1e-1, 2000); +} + +// Tests that the Midpoint method (RK2) correctly integrates the exponential decay ODE, and that the dual number derivatives match the analytical derivatives. +TEST("AD Midpoint Method (RK2)", Midpoint_ExponentialDecaySensitivity) { + auto stepFn = [&]( + const VecX_T>& x, + DualNumber_T t, + DualNumber_T dt ) { - return integrator.ralstonStep(x, t, dt, expDecay); - }; - verifyExpDecaySensitivity(stepFn, 1e-5, 500); - } - // Test that the Ralston method (RK2) correctly integrates a simple harmonic oscillator, and that the dual number derivatives match the analytical derivatives. - TEST("AD Ralston Method (RK2)", Ralston_HarmonicOscillatorSensitivity) { - auto stepFn = [&]( - const VecX_T>& x, - DualNumber_T t, - DualNumber_T dt + return integrator.midpointStep(x, t, dt, expDecay); + }; + verifyExpDecaySensitivity(stepFn, 1e-5, 500); +} +// Test that the Midpoint method (RK2) correctly integrates a simple harmonic oscillator, and that the dual number derivatives match the analytical derivatives. +TEST("AD Midpoint Method (RK2)", Midpoint_HarmonicOscillatorSensitivity) { + auto stepFn = [&]( + const VecX_T>& x, + DualNumber_T t, + DualNumber_T dt ) { - return integrator.ralstonStep(x, t, dt, harmonicOsc); - }; - verifyHarmonicOscillatorSensitivity(stepFn, 5e-4, 5e-4, 5e-3, 1000); - } - - // Tests that the RK4 method correctly integrates the exponential decay ODE, and that the dual number derivatives match the analytical derivatives. - TEST("AD RK4 Method", RK4_ExponentialDecaySensitivity) { - auto stepFn = [&]( - const VecX_T>& x, - DualNumber_T t, - DualNumber_T dt + return integrator.midpointStep(x, t, dt, harmonicOsc); + }; + verifyHarmonicOscillatorSensitivity(stepFn, 5e-4, 5e-4, 5e-3, 1000); +} + +// Tests that the Heun method (RK2) correctly integrates the exponential decay ODE, and that the dual number derivatives match the analytical derivatives. +TEST("AD Heun Method (RK2)", Heun_ExponentialDecaySensitivity) { + auto stepFn = [&]( + const VecX_T>& x, + DualNumber_T t, + DualNumber_T dt ) { - return integrator.rk4Step(x, t, dt, expDecay); - }; - verifyExpDecaySensitivity(stepFn, 1e-6, 200); - } - // Test that the RK4 method correctly integrates a simple harmonic oscillator, and that the dual number derivatives match the analytical derivatives. - TEST("AD RK4 Method", RK4_HarmonicOscillatorSensitivity) { - auto stepFn = [&]( - const VecX_T>& x, - DualNumber_T t, - DualNumber_T dt + return integrator.heunStep(x, t, dt, expDecay); + }; + verifyExpDecaySensitivity(stepFn, 1e-5, 500); +} +// Test that the Heun method (RK2) correctly integrates a simple harmonic oscillator, and that the dual number derivatives match the analytical derivatives. +TEST("AD Heun Method (RK2)", Heun_HarmonicOscillatorSensitivity) { + auto stepFn = [&]( + const VecX_T>& x, + DualNumber_T t, + DualNumber_T dt ) { - return integrator.rk4Step(x, t, dt, harmonicOsc); - }; - verifyHarmonicOscillatorSensitivity(stepFn, 1e-5, 1e-5, 1e-6, 100); - } + return integrator.heunStep(x, t, dt, harmonicOsc); + }; + verifyHarmonicOscillatorSensitivity(stepFn, 5e-4, 5e-4, 5e-3, 1000); +} + +// Tests that the Ralston method (RK2) correctly integrates the exponential decay ODE, and that the dual number derivatives match the analytical derivatives. +TEST("AD Ralston Method (RK2)", Ralston_ExponentialDecaySensitivity) { + auto stepFn = [&]( + const VecX_T>& x, + DualNumber_T t, + DualNumber_T dt + ) { + return integrator.ralstonStep(x, t, dt, expDecay); + }; + verifyExpDecaySensitivity(stepFn, 1e-5, 500); +} +// Test that the Ralston method (RK2) correctly integrates a simple harmonic oscillator, and that the dual number derivatives match the analytical derivatives. +TEST("AD Ralston Method (RK2)", Ralston_HarmonicOscillatorSensitivity) { + auto stepFn = [&]( + const VecX_T>& x, + DualNumber_T t, + DualNumber_T dt + ) { + return integrator.ralstonStep(x, t, dt, harmonicOsc); + }; + verifyHarmonicOscillatorSensitivity(stepFn, 5e-4, 5e-4, 5e-3, 1000); +} + +// Tests that the RK4 method correctly integrates the exponential decay ODE, and that the dual number derivatives match the analytical derivatives. +TEST("AD RK4 Method", RK4_ExponentialDecaySensitivity) { + auto stepFn = [&]( + const VecX_T>& x, + DualNumber_T t, + DualNumber_T dt + ) { + return integrator.rk4Step(x, t, dt, expDecay); + }; + verifyExpDecaySensitivity(stepFn, 1e-6, 200); +} +// Test that the RK4 method correctly integrates a simple harmonic oscillator, and that the dual number derivatives match the analytical derivatives. +TEST("AD RK4 Method", RK4_HarmonicOscillatorSensitivity) { + auto stepFn = [&]( + const VecX_T>& x, + DualNumber_T t, + DualNumber_T dt + ) { + return integrator.rk4Step(x, t, dt, harmonicOsc); + }; + verifyHarmonicOscillatorSensitivity(stepFn, 1e-5, 1e-5, 1e-6, 100); +} - // RK45 method is not tested due to further complexity with using an adapative step, implementation will be tested separately LATER once confirmed these work. -} \ No newline at end of file +// RK45 method is not tested due to further complexity with using an adapative step, implementation will be tested separately LATER once confirmed these work. \ No newline at end of file diff --git a/PxMLib/MathLib_Unit/AutomaticDifferentiationTests/ADImplicitIntegratorTests.cpp b/PxMLib/MathLib_Unit/AutomaticDifferentiationTests/ADImplicitIntegratorTests.cpp index 40b92123..8abbe6ae 100644 --- a/PxMLib/MathLib_Unit/AutomaticDifferentiationTests/ADImplicitIntegratorTests.cpp +++ b/PxMLib/MathLib_Unit/AutomaticDifferentiationTests/ADImplicitIntegratorTests.cpp @@ -37,7 +37,7 @@ namespace { d(0) = s(1); d(1) = -s(0); return d; - } + } // Linear Decay function for dual numbers template @@ -124,556 +124,556 @@ namespace { ASSERT_TRUE(std::abs(r(0).dual[0]) > 0.0, "Position sensitivity vanished during propagation"); ASSERT_TRUE(std::abs(r(1).dual[1]) > 0.0, "Velocity sensitivity vanished during propagation"); } - - // AD Implicit Euler Tests - // Test case for the GLRK2 method on the exponenital decay ODE. - TEST("AD Implicit Euler Method", ImplicitEuler_ExponentialDecay) { - VecX_T> x(1); - x(0) = DualNumber_T(1.0, { 0.0 }); - double T = 1.0; - DualNumber_T dt(T / 1000.0, { 0.0 }); - DualNumber_T t(0.0, { 0.0 }); - for (int i = 0; i < 1000; ++i) { - x = integrator.implicitEuler_AD(x, t, dt, expDecay, 8, DualNumber_T(1e-6, { 0.0 })); - t += dt; - } - ASSERT_TRUE(std::abs(x(0).real - std::exp(-1.0)) < tol_low, "Implicit Euler exponential decay error too large"); - } - // Test case for the implicit Euler method on the exponential decay ODE. - TEST("AD Implicit Euler Method", ImplicitEuler_ExponentialDecaySensitivity) { - auto stepFn = [&]( - const VecX_T>& x, - DualNumber_T t, - DualNumber_T dt - ) { - return integrator.implicitEuler_AD(x, t, dt, expDecay, 8, DualNumber_T(1e-6, { 0.0 })); - }; - verifyExpDecaySensitivity(stepFn, 1e-2, 1000); - } - // Test case for the Implicit Midpoint method on the linear decay ODE. - TEST("AD Implicit Euler Method", ImplicitEuler_LinearDecay) { - VecX_T> x(1); - x(0) = DualNumber_T(1.0, { 0.0 }); - double T = 0.5; - DualNumber_T dt(T / 500.0); - DualNumber_T t(0.0); - for (int i = 0; i < 500; ++i) { - x = integrator.implicitEuler_AD(x, t, dt, linearDecay, 50, DualNumber_T(1e-6, { 0.0 })); - t += dt; - } - ASSERT_TRUE(std::abs(x(0).real - std::exp(-1.0)) < tol_low, "Implicit Euler linear decay error too large"); - } - // Test case for the implicit Euler method on the linear decay ODE. - TEST("AD Implicit Euler Method", ImplicitEuler_LinearDecaySensitivity) { - auto stepFn = [&]( - const VecX_T>& x, - DualNumber_T t, - DualNumber_T dt - ) { - return integrator.implicitEuler_AD(x, t, dt, expDecay, 8, DualNumber_T(1e-6, { 0.0 })); - }; - verifyExpDecaySensitivity(stepFn, 1e-2, 1000); - } - // Test case for the Implicit Euler method on the stiff decay ODE. - TEST("AD Implicit Euler Method", ImplicitEuler_StiffDecay) { - VecX_T> x(1); - x(0) = DualNumber_T(1.0, { 0.0 }); - double T = 0.5; - DualNumber_T dt(T / 500.0); - DualNumber_T t(0.0); - double expected = std::exp(-50.0); - auto stiff_f = [](auto t, const auto& x) { - auto dx = x; - dx(0) = -100.0 * x(0); - return dx; - }; - for (int i = 0; i < 500; ++i) { - x = integrator.implicitEuler_AD(x, t, dt, stiff_f, 50, DualNumber_T(1e-6, { 0.0 })); - t += dt; - } - ASSERT_TRUE(std::isfinite(x(0).real), "Implicit Euler produced a non-finite result."); - ASSERT_TRUE(x(0).real < 1e-12, "Implicit Euler did not sufficiently damp stiff mode"); - } - // Test case for the implicit Euler method on the stiff decay ODE. - TEST("AD Implicit Euler Method", ImplicitEuler_StiffDecaySensitivity) { - auto stepFn = [&]( - const VecX_T>& x, - DualNumber_T t, - DualNumber_T dt - ) { - return integrator.implicitEuler_AD(x, t, dt, stiffDecay, 50, DualNumber_T(1e-6, { 0.0 })); - }; - verifyExpDecaySensitivity(stepFn, 1e-2, 1000); - } - // Test large step performance of implicit midpoint - TEST("AD Implicit Euler Method", ImplicitEuler_Stability_LargeStep) { - VecX_T> x(1); - x(0) = DualNumber_T(1.0, { 1.0 }); - double T = 1.0; - DualNumber_T t(0.0); - DualNumber_T dt(1.0 / 10.0); - DualNumber_T prev = x(0); - for (int i = 0; i < 10; ++i) { - x = integrator.implicitEuler_AD(x, t, dt, stiffDecay, 50, DualNumber_T(1e-6, { 0.0 })); - t += dt; - ASSERT_TRUE(std::abs(x(0).real) < std::abs(prev.real), "Not monotone"); - ASSERT_TRUE(std::abs(x(0).dual[0]) < std::abs(prev.dual[0]), "Not strictly decaying"); - prev = x(0); - } - } +} + +//// AD Implicit Euler Tests +//// Test case for the GLRK2 method on the exponenital decay ODE. +//TEST("AD Implicit Euler Method", ImplicitEuler_ExponentialDecay) { +// VecX_T> x(1); +// x(0) = DualNumber_T(1.0, { 0.0 }); +// double T = 1.0; +// DualNumber_T dt(T / 1000.0, { 0.0 }); +// DualNumber_T t(0.0, { 0.0 }); +// for (int i = 0; i < 1000; ++i) { +// x = integrator.implicitEuler_AD(x, t, dt, expDecay, 8, DualNumber_T(1e-6, { 0.0 })); +// t += dt; +// } +// ASSERT_TRUE(std::abs(x(0).real - std::exp(-1.0)) < tol_low, "Implicit Euler exponential decay error too large"); +//} +//// Test case for the implicit Euler method on the exponential decay ODE. +//TEST("AD Implicit Euler Method", ImplicitEuler_ExponentialDecaySensitivity) { +// auto stepFn = [&]( +// const VecX_T>& x, +// DualNumber_T t, +// DualNumber_T dt +// ) { +// return integrator.implicitEuler_AD(x, t, dt, expDecay, 8, DualNumber_T(1e-6, { 0.0 })); +// }; +// verifyExpDecaySensitivity(stepFn, 1e-2, 1000); +//} +//// Test case for the Implicit Midpoint method on the linear decay ODE. +//TEST("AD Implicit Euler Method", ImplicitEuler_LinearDecay) { +// VecX_T> x(1); +// x(0) = DualNumber_T(1.0, { 0.0 }); +// double T = 0.5; +// DualNumber_T dt(T / 500.0); +// DualNumber_T t(0.0); +// for (int i = 0; i < 500; ++i) { +// x = integrator.implicitEuler_AD(x, t, dt, linearDecay, 50, DualNumber_T(1e-6, { 0.0 })); +// t += dt; +// } +// ASSERT_TRUE(std::abs(x(0).real - std::exp(-1.0)) < tol_low, "Implicit Euler linear decay error too large"); +//} +//// Test case for the implicit Euler method on the linear decay ODE. +//TEST("AD Implicit Euler Method", ImplicitEuler_LinearDecaySensitivity) { +// auto stepFn = [&]( +// const VecX_T>& x, +// DualNumber_T t, +// DualNumber_T dt +// ) { +// return integrator.implicitEuler_AD(x, t, dt, expDecay, 8, DualNumber_T(1e-6, { 0.0 })); +// }; +// verifyExpDecaySensitivity(stepFn, 1e-2, 1000); +//} +//// Test case for the Implicit Euler method on the stiff decay ODE. +//TEST("AD Implicit Euler Method", ImplicitEuler_StiffDecay) { +// VecX_T> x(1); +// x(0) = DualNumber_T(1.0, { 0.0 }); +// double T = 0.5; +// DualNumber_T dt(T / 500.0); +// DualNumber_T t(0.0); +// double expected = std::exp(-50.0); +// auto stiff_f = [](auto t, const auto& x) { +// auto dx = x; +// dx(0) = -100.0 * x(0); +// return dx; +// }; +// for (int i = 0; i < 500; ++i) { +// x = integrator.implicitEuler_AD(x, t, dt, stiff_f, 50, DualNumber_T(1e-6, { 0.0 })); +// t += dt; +// } +// ASSERT_TRUE(std::isfinite(x(0).real), "Implicit Euler produced a non-finite result."); +// ASSERT_TRUE(x(0).real < 1e-12, "Implicit Euler did not sufficiently damp stiff mode"); +//} +//// Test case for the implicit Euler method on the stiff decay ODE. +//TEST("AD Implicit Euler Method", ImplicitEuler_StiffDecaySensitivity) { +// auto stepFn = [&]( +// const VecX_T>& x, +// DualNumber_T t, +// DualNumber_T dt +// ) { +// return integrator.implicitEuler_AD(x, t, dt, stiffDecay, 50, DualNumber_T(1e-6, { 0.0 })); +// }; +// verifyExpDecaySensitivity(stepFn, 1e-2, 1000); +//} +//// Test large step performance of implicit midpoint +//TEST("AD Implicit Euler Method", ImplicitEuler_Stability_LargeStep) { +// VecX_T> x(1); +// x(0) = DualNumber_T(1.0, { 1.0 }); +// double T = 1.0; +// DualNumber_T t(0.0); +// DualNumber_T dt(1.0 / 10.0); +// DualNumber_T prev = x(0); +// for (int i = 0; i < 10; ++i) { +// x = integrator.implicitEuler_AD(x, t, dt, stiffDecay, 50, DualNumber_T(1e-6, { 0.0 })); +// t += dt; +// ASSERT_TRUE(std::abs(x(0).real) < std::abs(prev.real), "Not monotone"); +// ASSERT_TRUE(std::abs(x(0).dual[0]) < std::abs(prev.dual[0]), "Not strictly decaying"); +// prev = x(0); +// } +//} - //// AD Implicit Midpoint Tests - //// Test case for the Implicit Midpoint method on the exponenital decay ODE. - //TEST("AD Implicit Midpoint Method", ImplicitMidpoint_ExponentialDecay) { - // VecX_T> x(1); - // x(0) = DualNumber_T(1.0, { 0.0 }); - // double T = 1.0; - // DualNumber_T dt(T / 1000.0, { 0.0 }); - // DualNumber_T t(0.0, { 0.0 }); - // for (int i = 0; i < 1000; ++i) { - // x = integrator.implicitMidpoint_AD(x, t, dt, expDecay, 8, DualNumber_T(1e-6, { 0.0 })); - // t += dt; - // } - // ASSERT_TRUE(std::abs(x(0).real - std::exp(-1.0)) < tol_low, "Implicit Midpoint exponential decay error too large"); - //} - //// Test case for the implicit midpoint method on the exponential decay ODE. - //TEST("AD Implicit Midpoint Method", ImplicitMidpoint_ExponentialDecaySensitivity) { - // auto stepFn = [&]( - // const VecX_T>& x, - // DualNumber_T t, - // DualNumber_T dt - // ) { - // return integrator.implicitMidpoint_AD(x, t, dt, expDecay, 8, DualNumber_T(1e-6, { 0.0 })); - // }; - // verifyExpDecaySensitivity(stepFn, 1e-3, 1000); - //} - //// Test case for the Implicit Midpoint method on the harmonic oscillator ODE. - //TEST("AD Implicit Midpoint Method", ImplicitMidpoint_HarmonicOscillator_EnergyPreservation) { - // VecX_T> x(2); - // x(0) = DualNumber_T(1.0, { 0.0, 0.0 }); - // x(1) = DualNumber_T(0.0, { 0.0, 0.0 }); - // double E0 = 0.5 * (x(0).real * x(0).real + x(1).real * x(1).real); - // double T = 5.0; - // DualNumber_T dt(T / 2500.0); - // DualNumber_T t(0.0); - // for (int i = 0; i < 2500; ++i) { - // x = integrator.implicitMidpoint_AD(x, t, dt, harmonicOsc, 10, DualNumber_T(1e-7, { 0.0, 0.0 })); - // t += dt; - // } - // double Ef = 0.5 * (x(0).real * x(0).real + x(1).real * x(1).real); - // double drift = std::abs(Ef - E0); - // ASSERT_TRUE(drift < tol_high * (1.0 + E0), "Implicit Midpoint energy drift too large for SHO"); - //} - //// Test case for the implicit midpoint method on the harmonic oscillator ODE. - //TEST("AD Implicit Midpoint Method", ImplicitMidpoint_HarmonicOscillatorSensitivity) { - // auto stepFn = [&]( - // const VecX_T>& x, - // DualNumber_T t, - // DualNumber_T dt - // ) { - // return integrator.implicitMidpoint_AD(x, t, dt, harmonicOsc, 10, DualNumber_T(1e-7, { 0.0, 0.0 })); - // }; - // verifyHarmonicOscillatorSensitivity(stepFn, 1e-3, 1000); - //} - //// Test case for the Implicit Midpoint method on the linear decay ODE. - //TEST("AD Implicit Midpoint Method", ImplicitMidpoint_LinearDecay) { - // VecX_T> x(1); - // x(0) = DualNumber_T(1.0, { 0.0 }); - // double T = 0.5; - // DualNumber_T dt(T / 500.0); - // DualNumber_T t(0.0); - // for (int i = 0; i < 500; ++i) { - // x = integrator.implicitMidpoint_AD(x, t, dt, linearDecay, 10, DualNumber_T(1e-6, { 0.0 })); - // t += dt; - // } - // ASSERT_TRUE(std::abs(x(0).real - std::exp(-1.0)) < tol_low, "Implicit Midpoint linear decay error too large"); - //} - //// Test case for the implicit midpoint method on the linear decay ODE. - //TEST("AD Implicit Midpoint Method", ImplicitMidpoint_LinearDecaySensitivity) { - // auto stepFn = [&]( - // const VecX_T>& x, - // DualNumber_T t, - // DualNumber_T dt - // ) { - // return integrator.implicitMidpoint_AD(x, t, dt, linearDecay, 10, DualNumber_T(1e-6, { 0.0 })); - // }; - // verifyExpDecaySensitivity(stepFn, 1e-3, 1000); - //} - //// Test case for the Implicit Midpoint method on the stiff decay ODE. - //TEST("AD Implicit Midpoint Method", ImplicitMidpoint_StiffDecay) { - // VecX_T> x(1); - // x(0) = DualNumber_T(1.0, { 0.0 }); - // double T = 0.5; - // DualNumber_T dt(T / 500.0); - // DualNumber_T t(0.0); - // double expected = std::exp(-50.0); - // auto stiff_f = [](auto t, const auto& x) { // why is this necessary? Why can't I just pass stiffDecay directly? the answer is that the template parameters can't be deduced from the function pointer, but they can be deduced from the - // auto dx = x; - // dx(0) = -100.0 * x(0); - // return dx; - // }; - // for (int i = 0; i < 500; ++i) { - // x = integrator.implicitMidpoint_AD(x, t, dt, stiffDecay, 10, DualNumber_T(1e-6, { 0.0 })); - // t += dt; - // } - // ASSERT_TRUE(std::isfinite(x(0).real), "Implicit Midpoint produced a non-finite result."); - // ASSERT_TRUE(x(0).real < 1e-12, "Implicit Midpoint did not sufficiently damp stiff mode"); - //} - //// Test case for the implicit midpoint method on the stiff decay ODE. - //TEST("AD Implicit Midpoint Method", ImplicitMidpoint_StiffDecaySensitivity) { - // auto stepFn = [&]( - // const VecX_T>& x, - // DualNumber_T t, - // DualNumber_T dt - // ) { - // return integrator.implicitMidpoint_AD(x, t, dt, stiffDecay, 10, DualNumber_T(1e-6, { 0.0 })); - // }; - // verifyExpDecaySensitivity(stepFn, 1e-3, 1000); - //} - //// Test large step performance of implicit midpoint - //TEST("AD Implicit Midpoint Method", ImplicitMidpoint_Stability_LargeStep) { - // VecX_T> x(1); - // x(0) = DualNumber_T(1.0, { 1.0 }); - // double T = 1.0; - // DualNumber_T t(0.0); - // DualNumber_T dt(1.0 / 10.0); - // DualNumber_T prev = x(0); - // for (int i = 0; i < 10; ++i) { - // x = integrator.implicitMidpoint_AD(x, t, dt, stiffDecay, 10, DualNumber_T(1e-6, { 0.0 })); - // t += dt; - // ASSERT_TRUE(std::abs(x(0).real) < std::abs(prev.real), "Not monotone"); - // ASSERT_TRUE(std::abs(x(0).dual[0]) < std::abs(prev.dual[0]), "Not strictly decaying"); - // prev = x(0); - // } - //} +//// AD Implicit Midpoint Tests +//// Test case for the Implicit Midpoint method on the exponenital decay ODE. +//TEST("AD Implicit Midpoint Method", ImplicitMidpoint_ExponentialDecay) { +// VecX_T> x(1); +// x(0) = DualNumber_T(1.0, { 0.0 }); +// double T = 1.0; +// DualNumber_T dt(T / 1000.0, { 0.0 }); +// DualNumber_T t(0.0, { 0.0 }); +// for (int i = 0; i < 1000; ++i) { +// x = integrator.implicitMidpoint_AD(x, t, dt, expDecay, 8, DualNumber_T(1e-6, { 0.0 })); +// t += dt; +// } +// ASSERT_TRUE(std::abs(x(0).real - std::exp(-1.0)) < tol_low, "Implicit Midpoint exponential decay error too large"); +//} +//// Test case for the implicit midpoint method on the exponential decay ODE. +//TEST("AD Implicit Midpoint Method", ImplicitMidpoint_ExponentialDecaySensitivity) { +// auto stepFn = [&]( +// const VecX_T>& x, +// DualNumber_T t, +// DualNumber_T dt +// ) { +// return integrator.implicitMidpoint_AD(x, t, dt, expDecay, 8, DualNumber_T(1e-6, { 0.0 })); +// }; +// verifyExpDecaySensitivity(stepFn, 1e-3, 1000); +//} +//// Test case for the Implicit Midpoint method on the harmonic oscillator ODE. +//TEST("AD Implicit Midpoint Method", ImplicitMidpoint_HarmonicOscillator_EnergyPreservation) { +// VecX_T> x(2); +// x(0) = DualNumber_T(1.0, { 0.0, 0.0 }); +// x(1) = DualNumber_T(0.0, { 0.0, 0.0 }); +// double E0 = 0.5 * (x(0).real * x(0).real + x(1).real * x(1).real); +// double T = 5.0; +// DualNumber_T dt(T / 2500.0); +// DualNumber_T t(0.0); +// for (int i = 0; i < 2500; ++i) { +// x = integrator.implicitMidpoint_AD(x, t, dt, harmonicOsc, 10, DualNumber_T(1e-7, { 0.0, 0.0 })); +// t += dt; +// } +// double Ef = 0.5 * (x(0).real * x(0).real + x(1).real * x(1).real); +// double drift = std::abs(Ef - E0); +// ASSERT_TRUE(drift < tol_high * (1.0 + E0), "Implicit Midpoint energy drift too large for SHO"); +//} +//// Test case for the implicit midpoint method on the harmonic oscillator ODE. +//TEST("AD Implicit Midpoint Method", ImplicitMidpoint_HarmonicOscillatorSensitivity) { +// auto stepFn = [&]( +// const VecX_T>& x, +// DualNumber_T t, +// DualNumber_T dt +// ) { +// return integrator.implicitMidpoint_AD(x, t, dt, harmonicOsc, 10, DualNumber_T(1e-7, { 0.0, 0.0 })); +// }; +// verifyHarmonicOscillatorSensitivity(stepFn, 1e-3, 1000); +//} +//// Test case for the Implicit Midpoint method on the linear decay ODE. +//TEST("AD Implicit Midpoint Method", ImplicitMidpoint_LinearDecay) { +// VecX_T> x(1); +// x(0) = DualNumber_T(1.0, { 0.0 }); +// double T = 0.5; +// DualNumber_T dt(T / 500.0); +// DualNumber_T t(0.0); +// for (int i = 0; i < 500; ++i) { +// x = integrator.implicitMidpoint_AD(x, t, dt, linearDecay, 10, DualNumber_T(1e-6, { 0.0 })); +// t += dt; +// } +// ASSERT_TRUE(std::abs(x(0).real - std::exp(-1.0)) < tol_low, "Implicit Midpoint linear decay error too large"); +//} +//// Test case for the implicit midpoint method on the linear decay ODE. +//TEST("AD Implicit Midpoint Method", ImplicitMidpoint_LinearDecaySensitivity) { +// auto stepFn = [&]( +// const VecX_T>& x, +// DualNumber_T t, +// DualNumber_T dt +// ) { +// return integrator.implicitMidpoint_AD(x, t, dt, linearDecay, 10, DualNumber_T(1e-6, { 0.0 })); +// }; +// verifyExpDecaySensitivity(stepFn, 1e-3, 1000); +//} +//// Test case for the Implicit Midpoint method on the stiff decay ODE. +//TEST("AD Implicit Midpoint Method", ImplicitMidpoint_StiffDecay) { +// VecX_T> x(1); +// x(0) = DualNumber_T(1.0, { 0.0 }); +// double T = 0.5; +// DualNumber_T dt(T / 500.0); +// DualNumber_T t(0.0); +// double expected = std::exp(-50.0); +// auto stiff_f = [](auto t, const auto& x) { // why is this necessary? Why can't I just pass stiffDecay directly? the answer is that the template parameters can't be deduced from the function pointer, but they can be deduced from the +// auto dx = x; +// dx(0) = -100.0 * x(0); +// return dx; +// }; +// for (int i = 0; i < 500; ++i) { +// x = integrator.implicitMidpoint_AD(x, t, dt, stiffDecay, 10, DualNumber_T(1e-6, { 0.0 })); +// t += dt; +// } +// ASSERT_TRUE(std::isfinite(x(0).real), "Implicit Midpoint produced a non-finite result."); +// ASSERT_TRUE(x(0).real < 1e-12, "Implicit Midpoint did not sufficiently damp stiff mode"); +//} +//// Test case for the implicit midpoint method on the stiff decay ODE. +//TEST("AD Implicit Midpoint Method", ImplicitMidpoint_StiffDecaySensitivity) { +// auto stepFn = [&]( +// const VecX_T>& x, +// DualNumber_T t, +// DualNumber_T dt +// ) { +// return integrator.implicitMidpoint_AD(x, t, dt, stiffDecay, 10, DualNumber_T(1e-6, { 0.0 })); +// }; +// verifyExpDecaySensitivity(stepFn, 1e-3, 1000); +//} +//// Test large step performance of implicit midpoint +//TEST("AD Implicit Midpoint Method", ImplicitMidpoint_Stability_LargeStep) { +// VecX_T> x(1); +// x(0) = DualNumber_T(1.0, { 1.0 }); +// double T = 1.0; +// DualNumber_T t(0.0); +// DualNumber_T dt(1.0 / 10.0); +// DualNumber_T prev = x(0); +// for (int i = 0; i < 10; ++i) { +// x = integrator.implicitMidpoint_AD(x, t, dt, stiffDecay, 10, DualNumber_T(1e-6, { 0.0 })); +// t += dt; +// ASSERT_TRUE(std::abs(x(0).real) < std::abs(prev.real), "Not monotone"); +// ASSERT_TRUE(std::abs(x(0).dual[0]) < std::abs(prev.dual[0]), "Not strictly decaying"); +// prev = x(0); +// } +//} - //// AD GLRK2 Tests - //// Test case for the GLRK2 method on the exponenital decay ODE. - //TEST("AD GLRK2 Method", GLRK2_ExponentialDecay) { - // VecX_T> x(1); - // x(0) = DualNumber_T(1.0, { 0.0 }); - // double T = 1.0; - // DualNumber_T dt(T / 1000.0, { 0.0 }); - // DualNumber_T t(0.0, { 0.0 }); - // for (int i = 0; i < 1000; ++i) { - // x = integrator.GLRK2(x, t, dt, expDecay, 50, 1e-6); - // t += dt; - // } - // ASSERT_TRUE(std::abs(x(0).real - std::exp(-1.0)) < tol_low, "GLRK3 exponential decay error too large"); - //} - //// Test case for the GLRK2 method on the exponential decay ODE. - //TEST("AD GLRK2 Method", GLRK2_ExponentialDecaySensitivity) { - // auto stepFn = [&]( - // const VecX_T>& x, - // DualNumber_T t, - // DualNumber_T dt - // ) { - // return integrator.GLRK2(x, t, dt, expDecay); - // }; - // verifyExpDecaySensitivity(stepFn, 1e-4, 1000); - //} - //// Test case for the GLRK2 method on the harmonic oscillator ODE. - //TEST("AD GLRK2 Method", GLRK2_HarmonicOscillator_EnergyPreservation) { - // VecX_T> x(2); - // x(0) = DualNumber_T(1.0, { 0.0, 0.0 }); - // x(1) = DualNumber_T(0.0, { 0.0, 0.0 }); - // double E0 = 0.5 * (x(0).real * x(0).real + x(1).real * x(1).real); - // double T = 5.0; - // DualNumber_T dt(T / 2500.0); - // DualNumber_T t(0.0); - // for (int i = 0; i < 2500; ++i) { - // x = integrator.GLRK2(x, t, dt, harmonicOsc, 50, 1e-6); - // t += dt; - // } - // double Ef = 0.5 * (x(0).real * x(0).real + x(1).real * x(1).real); - // double drift = std::abs(Ef - E0); - // ASSERT_TRUE(drift < tol_high * (1.0 + E0), "GLRK2 energy drift too large for SHO"); - //} - //// Test case for the GLRK2 method on the harmonic oscillator ODE. - //TEST("AD GLRK2 Method", GLRK2_HarmonicOscillatorSensitivity) { - // auto stepFn = [&]( - // const VecX_T>& x, - // DualNumber_T t, - // DualNumber_T dt - // ) { - // return integrator.GLRK2(x, t, dt, harmonicOsc); - // }; - // verifyHarmonicOscillatorSensitivity(stepFn, 1e-4, 1000); - //} - //// Test case for the GLRK2 method on the linear decay ODE. - //TEST("AD GLRK2 Method", GLRK2_LinearDecay) { - // VecX_T> x(1); - // x(0) = DualNumber_T(1.0, { 0.0 }); - // double T = 0.5; - // DualNumber_T dt(T / 500.0); - // DualNumber_T t(0.0); - // for (int i = 0; i < 500; ++i) { - // x = integrator.GLRK2(x, t, dt, linearDecay, 50, 1e-6); - // t += dt; - // } - // ASSERT_TRUE(std::abs(x(0).real - std::exp(-1.0)) < tol_low, "GLRK2 linear decay error too large"); - //} - //// Test case for the GLRK2 method on the linear decay ODE. - //TEST("AD GLRK2 Method", GLRK2_LinearDecaySensitivity) { - // auto stepFn = [&]( - // const VecX_T>& x, - // DualNumber_T t, - // DualNumber_T dt - // ) { - // return integrator.GLRK2(x, t, dt, linearDecay); - // }; - // verifyExpDecaySensitivity(stepFn, 1e-4, 1000); - //} - //// Test case for the GLRK2 method on the stiff decay ODE. - //TEST("AD GLRK2 Method", GLRK2_StiffDecay) { - // VecX_T> x(1); - // x(0) = DualNumber_T(1.0, { 0.0 }); - // double T = 0.5; - // DualNumber_T dt(T / 500.0); - // DualNumber_T t(0.0); - // double expected = std::exp(-50.0); - // auto stiff_f = [](auto t, const auto& x) { - // auto dx = x; - // dx(0) = -100.0 * x(0); - // return dx; - // }; - // for (int i = 0; i < 500; ++i) { - // x = integrator.GLRK2(x, t, dt, stiff_f, 50, 1e-10); - // t += dt; - // } - // ASSERT_TRUE(std::isfinite(x(0).real), "GLRK2 produced a non-finite result."); - // ASSERT_TRUE(x(0).real < 1e-12, "GLRK2 did not sufficiently damp stiff mode"); - //} - //// Test case for the GLRK2 method on the stiff decay ODE. - //TEST("AD GLRK2 Method", GLRK2_StiffDecaySensitivity) { - // auto stepFn = [&]( - // const VecX_T>& x, - // DualNumber_T t, - // DualNumber_T dt - // ) { - // return integrator.GLRK2(x, t, dt, stiffDecay); - // }; - // verifyExpDecaySensitivity(stepFn, 1e-4, 1000); - //} - //// Test case for the GLRK2 method on the nonlinear decay ODE. - //TEST("AD GLRK2 Method", GLRK2_NonlinearDecay) { - // VecX_T> x(1); - // x(0) = DualNumber_T(1.0, { 0.0 }); - // double T = 0.5; - // DualNumber_T dt(T / 500.0); - // DualNumber_T t(0.0); - // double expected = 1.0 / (1.0 + T); - // auto nl_f = [](auto, const auto& x) { - // auto dx = x; - // dx(0) = -x(0) * x(0); - // return dx; - // }; - // for (int i = 0; i < 500; ++i) { - // x = integrator.GLRK2(x, t, dt, nl_f, 50, 1e-6); - // t += dt; - // } - // double rel_err = std::abs(x(0).real - expected) / expected; - // ASSERT_TRUE(rel_err < tol_low, "GLRK2 nonlinear decay error too large"); - //} - //// Test case for the GLRK2 method on the stiff nonlinear decay ODE. - //TEST("AD GLRK2 Method", GLRK2_StiffNonlinearDecay) { - // VecX_T> x(1); - // x(0) = DualNumber_T(1.0, { 0.0 }); - // double T = 0.5; - // DualNumber_T dt(T / 500.0); - // DualNumber_T t(0.0); - // double expected = 1.0 / 51.0; - // auto stiff_nl_f = [](auto t, const auto& x) { - // auto dx = x; - // dx(0) = -100.0 * x(0) - x(0) * x(0); - // return dx; - // }; - // for (int i = 0; i < 500; ++i) { - // x = integrator.GLRK2(x, t, dt, stiff_nl_f, 50, 1e-6); - // t += dt; - // } - // double rel_err = std::abs(x(0).real - expected) / expected; - // ASSERT_TRUE(rel_err < tol_low, "GLRK2 stiff nonlinear decay error too large"); - //} - //// Test large step performance of GLRK2 - //TEST("AD GLRK2 Method", GLRK2_Stability_LargeStep) { - // VecX_T> x(1); - // x(0) = DualNumber_T(1.0, { 1.0 }); - // double T = 1.0; - // DualNumber_T t(0.0); - // DualNumber_T dt(1.0 / 10.0); - // DualNumber_T prev = x(0); - // for (int i = 0; i < 10; ++i) { - // x = integrator.GLRK2(x, t, dt, stiffDecay); - // t += dt; - // ASSERT_TRUE(std::abs(x(0).real) < std::abs(prev.real), "Not monotone"); - // ASSERT_TRUE(std::abs(x(0).dual[0]) < std::abs(prev.dual[0]), "Not strictly decaying"); - // prev = x(0); - // } - //} +//// AD GLRK2 Tests +//// Test case for the GLRK2 method on the exponenital decay ODE. +//TEST("AD GLRK2 Method", GLRK2_ExponentialDecay) { +// VecX_T> x(1); +// x(0) = DualNumber_T(1.0, { 0.0 }); +// double T = 1.0; +// DualNumber_T dt(T / 1000.0, { 0.0 }); +// DualNumber_T t(0.0, { 0.0 }); +// for (int i = 0; i < 1000; ++i) { +// x = integrator.GLRK2(x, t, dt, expDecay, 50, 1e-6); +// t += dt; +// } +// ASSERT_TRUE(std::abs(x(0).real - std::exp(-1.0)) < tol_low, "GLRK3 exponential decay error too large"); +//} +//// Test case for the GLRK2 method on the exponential decay ODE. +//TEST("AD GLRK2 Method", GLRK2_ExponentialDecaySensitivity) { +// auto stepFn = [&]( +// const VecX_T>& x, +// DualNumber_T t, +// DualNumber_T dt +// ) { +// return integrator.GLRK2(x, t, dt, expDecay); +// }; +// verifyExpDecaySensitivity(stepFn, 1e-4, 1000); +//} +//// Test case for the GLRK2 method on the harmonic oscillator ODE. +//TEST("AD GLRK2 Method", GLRK2_HarmonicOscillator_EnergyPreservation) { +// VecX_T> x(2); +// x(0) = DualNumber_T(1.0, { 0.0, 0.0 }); +// x(1) = DualNumber_T(0.0, { 0.0, 0.0 }); +// double E0 = 0.5 * (x(0).real * x(0).real + x(1).real * x(1).real); +// double T = 5.0; +// DualNumber_T dt(T / 2500.0); +// DualNumber_T t(0.0); +// for (int i = 0; i < 2500; ++i) { +// x = integrator.GLRK2(x, t, dt, harmonicOsc, 50, 1e-6); +// t += dt; +// } +// double Ef = 0.5 * (x(0).real * x(0).real + x(1).real * x(1).real); +// double drift = std::abs(Ef - E0); +// ASSERT_TRUE(drift < tol_high * (1.0 + E0), "GLRK2 energy drift too large for SHO"); +//} +//// Test case for the GLRK2 method on the harmonic oscillator ODE. +//TEST("AD GLRK2 Method", GLRK2_HarmonicOscillatorSensitivity) { +// auto stepFn = [&]( +// const VecX_T>& x, +// DualNumber_T t, +// DualNumber_T dt +// ) { +// return integrator.GLRK2(x, t, dt, harmonicOsc); +// }; +// verifyHarmonicOscillatorSensitivity(stepFn, 1e-4, 1000); +//} +//// Test case for the GLRK2 method on the linear decay ODE. +//TEST("AD GLRK2 Method", GLRK2_LinearDecay) { +// VecX_T> x(1); +// x(0) = DualNumber_T(1.0, { 0.0 }); +// double T = 0.5; +// DualNumber_T dt(T / 500.0); +// DualNumber_T t(0.0); +// for (int i = 0; i < 500; ++i) { +// x = integrator.GLRK2(x, t, dt, linearDecay, 50, 1e-6); +// t += dt; +// } +// ASSERT_TRUE(std::abs(x(0).real - std::exp(-1.0)) < tol_low, "GLRK2 linear decay error too large"); +//} +//// Test case for the GLRK2 method on the linear decay ODE. +//TEST("AD GLRK2 Method", GLRK2_LinearDecaySensitivity) { +// auto stepFn = [&]( +// const VecX_T>& x, +// DualNumber_T t, +// DualNumber_T dt +// ) { +// return integrator.GLRK2(x, t, dt, linearDecay); +// }; +// verifyExpDecaySensitivity(stepFn, 1e-4, 1000); +//} +//// Test case for the GLRK2 method on the stiff decay ODE. +//TEST("AD GLRK2 Method", GLRK2_StiffDecay) { +// VecX_T> x(1); +// x(0) = DualNumber_T(1.0, { 0.0 }); +// double T = 0.5; +// DualNumber_T dt(T / 500.0); +// DualNumber_T t(0.0); +// double expected = std::exp(-50.0); +// auto stiff_f = [](auto t, const auto& x) { +// auto dx = x; +// dx(0) = -100.0 * x(0); +// return dx; +// }; +// for (int i = 0; i < 500; ++i) { +// x = integrator.GLRK2(x, t, dt, stiff_f, 50, 1e-10); +// t += dt; +// } +// ASSERT_TRUE(std::isfinite(x(0).real), "GLRK2 produced a non-finite result."); +// ASSERT_TRUE(x(0).real < 1e-12, "GLRK2 did not sufficiently damp stiff mode"); +//} +//// Test case for the GLRK2 method on the stiff decay ODE. +//TEST("AD GLRK2 Method", GLRK2_StiffDecaySensitivity) { +// auto stepFn = [&]( +// const VecX_T>& x, +// DualNumber_T t, +// DualNumber_T dt +// ) { +// return integrator.GLRK2(x, t, dt, stiffDecay); +// }; +// verifyExpDecaySensitivity(stepFn, 1e-4, 1000); +//} +//// Test case for the GLRK2 method on the nonlinear decay ODE. +//TEST("AD GLRK2 Method", GLRK2_NonlinearDecay) { +// VecX_T> x(1); +// x(0) = DualNumber_T(1.0, { 0.0 }); +// double T = 0.5; +// DualNumber_T dt(T / 500.0); +// DualNumber_T t(0.0); +// double expected = 1.0 / (1.0 + T); +// auto nl_f = [](auto, const auto& x) { +// auto dx = x; +// dx(0) = -x(0) * x(0); +// return dx; +// }; +// for (int i = 0; i < 500; ++i) { +// x = integrator.GLRK2(x, t, dt, nl_f, 50, 1e-6); +// t += dt; +// } +// double rel_err = std::abs(x(0).real - expected) / expected; +// ASSERT_TRUE(rel_err < tol_low, "GLRK2 nonlinear decay error too large"); +//} +//// Test case for the GLRK2 method on the stiff nonlinear decay ODE. +//TEST("AD GLRK2 Method", GLRK2_StiffNonlinearDecay) { +// VecX_T> x(1); +// x(0) = DualNumber_T(1.0, { 0.0 }); +// double T = 0.5; +// DualNumber_T dt(T / 500.0); +// DualNumber_T t(0.0); +// double expected = 1.0 / 51.0; +// auto stiff_nl_f = [](auto t, const auto& x) { +// auto dx = x; +// dx(0) = -100.0 * x(0) - x(0) * x(0); +// return dx; +// }; +// for (int i = 0; i < 500; ++i) { +// x = integrator.GLRK2(x, t, dt, stiff_nl_f, 50, 1e-6); +// t += dt; +// } +// double rel_err = std::abs(x(0).real - expected) / expected; +// ASSERT_TRUE(rel_err < tol_low, "GLRK2 stiff nonlinear decay error too large"); +//} +//// Test large step performance of GLRK2 +//TEST("AD GLRK2 Method", GLRK2_Stability_LargeStep) { +// VecX_T> x(1); +// x(0) = DualNumber_T(1.0, { 1.0 }); +// double T = 1.0; +// DualNumber_T t(0.0); +// DualNumber_T dt(1.0 / 10.0); +// DualNumber_T prev = x(0); +// for (int i = 0; i < 10; ++i) { +// x = integrator.GLRK2(x, t, dt, stiffDecay); +// t += dt; +// ASSERT_TRUE(std::abs(x(0).real) < std::abs(prev.real), "Not monotone"); +// ASSERT_TRUE(std::abs(x(0).dual[0]) < std::abs(prev.dual[0]), "Not strictly decaying"); +// prev = x(0); +// } +//} - //// AD GLRK3 Tests - //// Test case for the GLRK3 method on the exponenital decay ODE. - //TEST("AD GLRK3 Method", GLRK3_ExponentialDecay) { - // VecX_T> x(1); - // x(0) = DualNumber_T(1.0, { 0.0 }); - // double T = 1.0; - // DualNumber_T dt(T / 1000.0, { 0.0 }); - // DualNumber_T t(0.0, { 0.0 }); - // for (int i = 0; i < 1000; ++i) { - // x = integrator.GLRK3(x, t, dt, expDecay, 80, 1e-7); - // t += dt; - // } - // ASSERT_TRUE(std::abs(x(0).real - std::exp(-1.0)) < tol_low, "GLRK3 exponential decay error too large"); - //} - //// Test case for the GLRK3 method on the exponential decay ODE sensitivity. - //TEST("AD GLRK3 Method", GLRK3_ExponentialDecaySensitivity) { - // auto stepFn = [&]( - // const VecX_T>& x, - // DualNumber_T t, - // DualNumber_T dt - // ) { - // return integrator.GLRK3(x, t, dt, expDecay); - // }; - // verifyExpDecaySensitivity(stepFn, 1e-4, 1000); - //} - //// Test case for the GLRK3 method on the harmonic oscillator ODE. - //TEST("AD GLRK3 Method", GLRK3_HarmonicOscillator_EnergyPreservation) { - // VecX_T> x(2); - // x(0) = DualNumber_T(1.0, { 0.0, 0.0 }); - // x(1) = DualNumber_T(0.0, { 0.0, 0.0 }); - // double E0 = 0.5 * (x(0).real * x(0).real + x(1).real * x(1).real); - // double T = 5.0; - // DualNumber_T dt(T / 2500.0); - // DualNumber_T t(0.0); - // for (int i = 0; i < 2500; ++i) { - // x = integrator.GLRK3(x, t, dt, harmonicOsc, 80, 1e-7); - // t += dt; - // } - // double Ef = 0.5 * (x(0).real * x(0).real + x(1).real * x(1).real); - // double drift = std::abs(Ef - E0); - // ASSERT_TRUE(drift < tol_high * (1.0 + E0), "GLRK3 energy drift too large for SHO"); - //} - //// Test case for the GLRK3 method on the harmonic oscillator ODE. - //TEST("AD GLRK3 Method", GLRK3_HarmonicOscillatorSensitivity) { - // auto stepFn = [&]( - // const VecX_T>& x, - // DualNumber_T t, - // DualNumber_T dt - // ) { - // return integrator.GLRK3(x, t, dt, harmonicOsc); - // }; - // verifyHarmonicOscillatorSensitivity(stepFn, 1e-4, 1000); - //} - //// Test case for the GLRK3 method on the linear decay ODE. - //TEST("AD GLRK3 Method", GLRK3_LinearDecay) { - // VecX_T> x(1); - // x(0) = DualNumber_T(1.0, { 0.0 }); - // double T = 0.5; - // DualNumber_T dt(T / 500.0); - // DualNumber_T t(0.0); - // for (int i = 0; i < 500; ++i) { - // x = integrator.GLRK3(x, t, dt, linearDecay, 80, 1e-7); - // t += dt; - // } - // ASSERT_TRUE(std::abs(x(0).real - std::exp(-1.0)) < tol_low, "GLRK3 linear decay error too large"); - //} - //// Test case for the GLRK3 method on the linear decay ODE. - //TEST("AD GLRK3 Method", GLRK3_LinearDecaySensitivity) { - // auto stepFn = [&]( - // const VecX_T>& x, - // DualNumber_T t, - // DualNumber_T dt - // ) { - // return integrator.GLRK3(x, t, dt, linearDecay); - // }; - // verifyExpDecaySensitivity(stepFn, 1e-12, 1000); - //} - //// Test case for the GLRK3 method on the stiff decay ODE. - //TEST("AD GLRK3 Method", GLRK3_StiffDecay) { - // VecX_T> x(1); - // x(0) = DualNumber_T(1.0, { 0.0 }); - // double T = 0.5; - // DualNumber_T dt(T / 500.0); - // DualNumber_T t(0.0); - // double expected = std::exp(-50.0); - // auto stiff_f = [](auto t, const auto& x) { - // auto dx = x; - // dx(0) = -100.0 * x(0); - // return dx; - // }; - // for (int i = 0; i < 500; ++i) { - // x = integrator.GLRK3(x, t, dt, stiff_f, 150, 1e-12); - // t += dt; - // } - // ASSERT_TRUE(std::isfinite(x(0).real), "GLRK3 produced a non-finite result."); - // ASSERT_TRUE(x(0).real < 1e-14, "GLRK3 did not sufficiently damp stiff mode"); - //} - //// Test case for the GLRK3 method on the stiff decay ODE. - //TEST("AD GLRK3 Method", GLRK3_StiffDecaySensitivity) { - // auto stepFn = [&]( - // const VecX_T>& x, - // DualNumber_T t, - // DualNumber_T dt - // ) { - // return integrator.GLRK3(x, t, dt, stiffDecay); - // }; - // verifyExpDecaySensitivity(stepFn, 1e-12, 1000); - //} - //// Test case for the GLRK3 method on the nonlinear decay ODE. - //TEST("AD GLRK3 Method", GLRK3_NonlinearDecay) { - // VecX_T> x(1); - // x(0) = DualNumber_T(1.0, { 0.0 }); - // double T = 0.5; - // DualNumber_T dt(T / 500.0); - // DualNumber_T t(0.0); - // double expected = 1.0 / (1.0 + T); - // auto nl_f = [](auto t, const auto& x) { - // auto dx = x; - // dx(0) = -x(0) * x(0); - // return dx; - // }; - // for (int i = 0; i < 500; ++i) { - // x = integrator.GLRK3(x, t, dt, nl_f, 80, 1e-7); - // t += dt; - // } - // double rel_err = std::abs(x(0).real -expected) / expected; - // ASSERT_TRUE(rel_err < tol_low, "GLRK3 nonlinear decay error too large"); - //} - //// Test case for the GLRK3 method on the stiff nonlinear decay ODE. - //TEST("AD GLRK3 Method", GLRK3_StiffNonlinearDecay) { - // VecX_T> x(1); - // x(0) = DualNumber_T(1.0, { 0.0 }); - // double T = 0.5; - // DualNumber_T dt(T / 500.0); - // DualNumber_T t(0.0); - // double expected = 1.0 / 51.0; - // auto stiff_nl_f = [](auto t, const auto& x) { - // auto dx = x.eval(); - // dx(0) = -100.0 * x(0) - x(0) * x(0); - // return dx; - // }; - // for (int i = 0; i < 500; ++i) { - // x = integrator.GLRK3(x, t, dt, stiff_nl_f, 80, 1e-7); - // t += dt; - // } - // double rel_err = std::abs(x(0).real - expected) / expected; - // ASSERT_TRUE(rel_err < tol_low, "GLRK3 stiff nonlinear decay error too large"); - //} - //// Test case for the GLRK3 method on the stiff decay ODE. - //TEST("AD GLRK3 Method", GLRK3_Stability_LargeStep) { - // VecX_T> x(1); - // x(0) = DualNumber_T(1.0, { 1.0 }); - // double T = 1.0; - // DualNumber_T t(0.0); - // DualNumber_T dt(1.0 / 10.0); - // DualNumber_T prev = x(0); - // for (int i = 0; i < 10; ++i) { - // x = integrator.GLRK3(x, t, dt, stiffDecay); - // t += dt; - // ASSERT_TRUE(std::abs(x(0).real) < std::abs(prev.real), "Not monotone"); - // ASSERT_TRUE(std::abs(x(0).dual[0]) < std::abs(prev.dual[0]), "Not strictly decaying"); - // prev = x(0); - // } - //} -} \ No newline at end of file +//// AD GLRK3 Tests +//// Test case for the GLRK3 method on the exponenital decay ODE. +//TEST("AD GLRK3 Method", GLRK3_ExponentialDecay) { +// VecX_T> x(1); +// x(0) = DualNumber_T(1.0, { 0.0 }); +// double T = 1.0; +// DualNumber_T dt(T / 1000.0, { 0.0 }); +// DualNumber_T t(0.0, { 0.0 }); +// for (int i = 0; i < 1000; ++i) { +// x = integrator.GLRK3(x, t, dt, expDecay, 80, 1e-7); +// t += dt; +// } +// ASSERT_TRUE(std::abs(x(0).real - std::exp(-1.0)) < tol_low, "GLRK3 exponential decay error too large"); +//} +//// Test case for the GLRK3 method on the exponential decay ODE sensitivity. +//TEST("AD GLRK3 Method", GLRK3_ExponentialDecaySensitivity) { +// auto stepFn = [&]( +// const VecX_T>& x, +// DualNumber_T t, +// DualNumber_T dt +// ) { +// return integrator.GLRK3(x, t, dt, expDecay); +// }; +// verifyExpDecaySensitivity(stepFn, 1e-4, 1000); +//} +//// Test case for the GLRK3 method on the harmonic oscillator ODE. +//TEST("AD GLRK3 Method", GLRK3_HarmonicOscillator_EnergyPreservation) { +// VecX_T> x(2); +// x(0) = DualNumber_T(1.0, { 0.0, 0.0 }); +// x(1) = DualNumber_T(0.0, { 0.0, 0.0 }); +// double E0 = 0.5 * (x(0).real * x(0).real + x(1).real * x(1).real); +// double T = 5.0; +// DualNumber_T dt(T / 2500.0); +// DualNumber_T t(0.0); +// for (int i = 0; i < 2500; ++i) { +// x = integrator.GLRK3(x, t, dt, harmonicOsc, 80, 1e-7); +// t += dt; +// } +// double Ef = 0.5 * (x(0).real * x(0).real + x(1).real * x(1).real); +// double drift = std::abs(Ef - E0); +// ASSERT_TRUE(drift < tol_high * (1.0 + E0), "GLRK3 energy drift too large for SHO"); +//} +//// Test case for the GLRK3 method on the harmonic oscillator ODE. +//TEST("AD GLRK3 Method", GLRK3_HarmonicOscillatorSensitivity) { +// auto stepFn = [&]( +// const VecX_T>& x, +// DualNumber_T t, +// DualNumber_T dt +// ) { +// return integrator.GLRK3(x, t, dt, harmonicOsc); +// }; +// verifyHarmonicOscillatorSensitivity(stepFn, 1e-4, 1000); +//} +//// Test case for the GLRK3 method on the linear decay ODE. +//TEST("AD GLRK3 Method", GLRK3_LinearDecay) { +// VecX_T> x(1); +// x(0) = DualNumber_T(1.0, { 0.0 }); +// double T = 0.5; +// DualNumber_T dt(T / 500.0); +// DualNumber_T t(0.0); +// for (int i = 0; i < 500; ++i) { +// x = integrator.GLRK3(x, t, dt, linearDecay, 80, 1e-7); +// t += dt; +// } +// ASSERT_TRUE(std::abs(x(0).real - std::exp(-1.0)) < tol_low, "GLRK3 linear decay error too large"); +//} +//// Test case for the GLRK3 method on the linear decay ODE. +//TEST("AD GLRK3 Method", GLRK3_LinearDecaySensitivity) { +// auto stepFn = [&]( +// const VecX_T>& x, +// DualNumber_T t, +// DualNumber_T dt +// ) { +// return integrator.GLRK3(x, t, dt, linearDecay); +// }; +// verifyExpDecaySensitivity(stepFn, 1e-12, 1000); +//} +//// Test case for the GLRK3 method on the stiff decay ODE. +//TEST("AD GLRK3 Method", GLRK3_StiffDecay) { +// VecX_T> x(1); +// x(0) = DualNumber_T(1.0, { 0.0 }); +// double T = 0.5; +// DualNumber_T dt(T / 500.0); +// DualNumber_T t(0.0); +// double expected = std::exp(-50.0); +// auto stiff_f = [](auto t, const auto& x) { +// auto dx = x; +// dx(0) = -100.0 * x(0); +// return dx; +// }; +// for (int i = 0; i < 500; ++i) { +// x = integrator.GLRK3(x, t, dt, stiff_f, 150, 1e-12); +// t += dt; +// } +// ASSERT_TRUE(std::isfinite(x(0).real), "GLRK3 produced a non-finite result."); +// ASSERT_TRUE(x(0).real < 1e-14, "GLRK3 did not sufficiently damp stiff mode"); +//} +//// Test case for the GLRK3 method on the stiff decay ODE. +//TEST("AD GLRK3 Method", GLRK3_StiffDecaySensitivity) { +// auto stepFn = [&]( +// const VecX_T>& x, +// DualNumber_T t, +// DualNumber_T dt +// ) { +// return integrator.GLRK3(x, t, dt, stiffDecay); +// }; +// verifyExpDecaySensitivity(stepFn, 1e-12, 1000); +//} +//// Test case for the GLRK3 method on the nonlinear decay ODE. +//TEST("AD GLRK3 Method", GLRK3_NonlinearDecay) { +// VecX_T> x(1); +// x(0) = DualNumber_T(1.0, { 0.0 }); +// double T = 0.5; +// DualNumber_T dt(T / 500.0); +// DualNumber_T t(0.0); +// double expected = 1.0 / (1.0 + T); +// auto nl_f = [](auto t, const auto& x) { +// auto dx = x; +// dx(0) = -x(0) * x(0); +// return dx; +// }; +// for (int i = 0; i < 500; ++i) { +// x = integrator.GLRK3(x, t, dt, nl_f, 80, 1e-7); +// t += dt; +// } +// double rel_err = std::abs(x(0).real -expected) / expected; +// ASSERT_TRUE(rel_err < tol_low, "GLRK3 nonlinear decay error too large"); +//} +//// Test case for the GLRK3 method on the stiff nonlinear decay ODE. +//TEST("AD GLRK3 Method", GLRK3_StiffNonlinearDecay) { +// VecX_T> x(1); +// x(0) = DualNumber_T(1.0, { 0.0 }); +// double T = 0.5; +// DualNumber_T dt(T / 500.0); +// DualNumber_T t(0.0); +// double expected = 1.0 / 51.0; +// auto stiff_nl_f = [](auto t, const auto& x) { +// auto dx = x.eval(); +// dx(0) = -100.0 * x(0) - x(0) * x(0); +// return dx; +// }; +// for (int i = 0; i < 500; ++i) { +// x = integrator.GLRK3(x, t, dt, stiff_nl_f, 80, 1e-7); +// t += dt; +// } +// double rel_err = std::abs(x(0).real - expected) / expected; +// ASSERT_TRUE(rel_err < tol_low, "GLRK3 stiff nonlinear decay error too large"); +//} +//// Test case for the GLRK3 method on the stiff decay ODE. +//TEST("AD GLRK3 Method", GLRK3_Stability_LargeStep) { +// VecX_T> x(1); +// x(0) = DualNumber_T(1.0, { 1.0 }); +// double T = 1.0; +// DualNumber_T t(0.0); +// DualNumber_T dt(1.0 / 10.0); +// DualNumber_T prev = x(0); +// for (int i = 0; i < 10; ++i) { +// x = integrator.GLRK3(x, t, dt, stiffDecay); +// t += dt; +// ASSERT_TRUE(std::abs(x(0).real) < std::abs(prev.real), "Not monotone"); +// ASSERT_TRUE(std::abs(x(0).dual[0]) < std::abs(prev.dual[0]), "Not strictly decaying"); +// prev = x(0); +// } +//} \ No newline at end of file diff --git a/PxMLib/MathLib_Unit/AutomaticDifferentiationTests/ADJacobianTests.cpp b/PxMLib/MathLib_Unit/AutomaticDifferentiationTests/ADJacobianTests.cpp index 42a275b9..fe61d703 100644 --- a/PxMLib/MathLib_Unit/AutomaticDifferentiationTests/ADJacobianTests.cpp +++ b/PxMLib/MathLib_Unit/AutomaticDifferentiationTests/ADJacobianTests.cpp @@ -75,131 +75,131 @@ namespace { } } } +} - // Test that the Jacobian computed using dual numbers matches the analytical Jacobian for a simple vector-valued function. - TEST("AD Jacobian", ADJacobian_anaylticalMatch) { - VecX_T> x(2); - x(0) = DualNumber_T(1.0, { 1.0, 0.0 }); // x = 1.0 with dual part for df/dx - x(1) = DualNumber_T(2.0, { 0.0, 1.0 }); // y = 2.0 with dual part for df/dy - MatX_T J_ad(2, 2); - MatX_T J_analytical(2, 2); - analyticalJacobian(x, J_analytical); - constructADJacobian(x, J_ad); - for (int i = 0; i < J_analytical.rows(); ++i) { - for (int j = 0; j < J_analytical.cols(); ++j) { - std::string errorMsg = "Jacobian mismatch at (" + std::to_string(i) - + ", " + std::to_string(j) - + "): " + std::to_string(J_analytical(i, j)) - + " vs " + std::to_string(J_ad(i, j)); - ASSERT_TRUE(std::abs(J_analytical(i, j) - J_ad(i, j)) < 1e-6, errorMsg.c_str()); - ASSERT_TRUE(std::isfinite(J_ad(i, j)), "AD Jacobian contains non-finite value"); - } +// Test that the Jacobian computed using dual numbers matches the analytical Jacobian for a simple vector-valued function. +TEST("AD Jacobian", ADJacobian_anaylticalMatch) { + VecX_T> x(2); + x(0) = DualNumber_T(1.0, { 1.0, 0.0 }); // x = 1.0 with dual part for df/dx + x(1) = DualNumber_T(2.0, { 0.0, 1.0 }); // y = 2.0 with dual part for df/dy + MatX_T J_ad(2, 2); + MatX_T J_analytical(2, 2); + analyticalJacobian(x, J_analytical); + constructADJacobian(x, J_ad); + for (int i = 0; i < J_analytical.rows(); ++i) { + for (int j = 0; j < J_analytical.cols(); ++j) { + std::string errorMsg = "Jacobian mismatch at (" + std::to_string(i) + + ", " + std::to_string(j) + + "): " + std::to_string(J_analytical(i, j)) + + " vs " + std::to_string(J_ad(i, j)); + ASSERT_TRUE(std::abs(J_analytical(i, j) - J_ad(i, j)) < 1e-6, errorMsg.c_str()); + ASSERT_TRUE(std::isfinite(J_ad(i, j)), "AD Jacobian contains non-finite value"); } - ASSERT_TRUE(J_analytical.isApprox(J_ad, 1e-6), "Analytical and AD Jacobians do not match"); } + ASSERT_TRUE(J_analytical.isApprox(J_ad, 1e-6), "Analytical and AD Jacobians do not match"); +} - // Test that the Jacobian computed using dual numbers matches a finite difference approximation of the Jacobian for the same vector-valued function. - TEST("AD Jacobian", ADJacobian_finiteDifferenceMatch) { - VecX_T> x(2); - x(0) = DualNumber_T(1.0, { 1.0, 0.0 }); // x = 1.0 with dual part for df/dx - x(1) = DualNumber_T(2.0, { 0.0, 1.0 }); // y = 2.0 with dual part for df/dy - MatX_T J_ad(2, 2); - MatX_T J_fd(2, 2); - finiteDifferenceJacobian(x, J_fd); - constructADJacobian(x, J_ad); - for (int i = 0; i < J_fd.rows(); ++i) { - for (int j = 0; j < J_fd.cols(); ++j) { - std::string errorMsg = "Jacobian mismatch at (" + std::to_string(i) - + ", " + std::to_string(j) - + "): " + std::to_string(J_fd(i, j)) - + " vs " + std::to_string(J_ad(i, j)); - ASSERT_TRUE(std::abs(J_fd(i, j) - J_ad(i, j)) < 1e-5, errorMsg.c_str()); - ASSERT_TRUE(std::isfinite(J_ad(i, j)), "AD Jacobian contains non-finite value"); - } +// Test that the Jacobian computed using dual numbers matches a finite difference approximation of the Jacobian for the same vector-valued function. +TEST("AD Jacobian", ADJacobian_finiteDifferenceMatch) { + VecX_T> x(2); + x(0) = DualNumber_T(1.0, { 1.0, 0.0 }); // x = 1.0 with dual part for df/dx + x(1) = DualNumber_T(2.0, { 0.0, 1.0 }); // y = 2.0 with dual part for df/dy + MatX_T J_ad(2, 2); + MatX_T J_fd(2, 2); + finiteDifferenceJacobian(x, J_fd); + constructADJacobian(x, J_ad); + for (int i = 0; i < J_fd.rows(); ++i) { + for (int j = 0; j < J_fd.cols(); ++j) { + std::string errorMsg = "Jacobian mismatch at (" + std::to_string(i) + + ", " + std::to_string(j) + + "): " + std::to_string(J_fd(i, j)) + + " vs " + std::to_string(J_ad(i, j)); + ASSERT_TRUE(std::abs(J_fd(i, j) - J_ad(i, j)) < 1e-5, errorMsg.c_str()); + ASSERT_TRUE(std::isfinite(J_ad(i, j)), "AD Jacobian contains non-finite value"); } - ASSERT_TRUE(J_fd.isApprox(J_ad, 1e-6), "Finite difference and AD Jacobians do not match"); } + ASSERT_TRUE(J_fd.isApprox(J_ad, 1e-6), "Finite difference and AD Jacobians do not match"); +} - // Tests the cross-variable propagation of derivatives in a coupled nonlinear system, ensuring that the Jacobian captures the interactions between variables correctly. - TEST("AD Jacobian", ADJacobian_coupledNonlinearSystem) { - VecX_T> x(2); - x(0) = DualNumber_T(1.0, { 1.0, 0.0 }); // x = 1.0 with dual part for df/dx - x(1) = DualNumber_T(2.0, { 0.0, 1.0 }); // y = 2.0 with dual part for df/dy - VecX_T> F = coupledNonlinearSystem(x); - MatX_T J_ad(2, 2); - for (int i = 0; i < F.size(); ++i) { - for (int j = 0; j < 2; ++j) { - J_ad(i, j) = F(i).dual[j]; - } +// Tests the cross-variable propagation of derivatives in a coupled nonlinear system, ensuring that the Jacobian captures the interactions between variables correctly. +TEST("AD Jacobian", ADJacobian_coupledNonlinearSystem) { + VecX_T> x(2); + x(0) = DualNumber_T(1.0, { 1.0, 0.0 }); // x = 1.0 with dual part for df/dx + x(1) = DualNumber_T(2.0, { 0.0, 1.0 }); // y = 2.0 with dual part for df/dy + VecX_T> F = coupledNonlinearSystem(x); + MatX_T J_ad(2, 2); + for (int i = 0; i < F.size(); ++i) { + for (int j = 0; j < 2; ++j) { + J_ad(i, j) = F(i).dual[j]; } - MatX_T J_expected(2, 2); - J_expected(0, 0) = 2.0 * x(0).real + x(1).real; // dF1/dx - J_expected(0, 1) = x(0).real - 1.0; // dF1/dy - J_expected(1, 0) = std::cos(x(0).real * x(1).real) * x(1).real; // dF2/dx - J_expected(1, 1) = std::cos(x(0).real * x(1).real) * x(0).real + 3.0 * pow(x(1).real, 2.0); // dF2/dy - for (int i = 0; i < J_ad.rows(); ++i) { - for (int j = 0; j < J_ad.cols(); ++j) { - std::string errorMsg = "Coupled system Jacobian mismatch at (" + std::to_string(i) - + ", " + std::to_string(j) - + "): " + std::to_string(J_expected(i, j)) - + " vs " + std::to_string(J_ad(i, j)); - ASSERT_TRUE(std::abs(J_expected(i, j) - J_ad(i, j)) < 1e-6, errorMsg.c_str()); - ASSERT_TRUE(std::isfinite(J_ad(i, j)), "AD Jacobian contains non-finite value"); - } + } + MatX_T J_expected(2, 2); + J_expected(0, 0) = 2.0 * x(0).real + x(1).real; // dF1/dx + J_expected(0, 1) = x(0).real - 1.0; // dF1/dy + J_expected(1, 0) = std::cos(x(0).real * x(1).real) * x(1).real; // dF2/dx + J_expected(1, 1) = std::cos(x(0).real * x(1).real) * x(0).real + 3.0 * pow(x(1).real, 2.0); // dF2/dy + for (int i = 0; i < J_ad.rows(); ++i) { + for (int j = 0; j < J_ad.cols(); ++j) { + std::string errorMsg = "Coupled system Jacobian mismatch at (" + std::to_string(i) + + ", " + std::to_string(j) + + "): " + std::to_string(J_expected(i, j)) + + " vs " + std::to_string(J_ad(i, j)); + ASSERT_TRUE(std::abs(J_expected(i, j) - J_ad(i, j)) < 1e-6, errorMsg.c_str()); + ASSERT_TRUE(std::isfinite(J_ad(i, j)), "AD Jacobian contains non-finite value"); } - ASSERT_TRUE(J_expected.isApprox(J_ad, 1e-6), "Expected and AD Jacobians do not match for coupled nonlinear system"); } + ASSERT_TRUE(J_expected.isApprox(J_ad, 1e-6), "Expected and AD Jacobians do not match for coupled nonlinear system"); +} - // Tests the conditioning of the Jacobian matrix computed via automatic differentiation, ensuring that it does not contain excessively large or small values that could indicate numerical instability. - TEST("AD Jacobian", ADJacobian_numericalStability) { - VecX_T> x(2); - x(0) = DualNumber_T(1.0, { 1.0, 0.0 }); // x = 1.0 with dual part for df/dx - x(1) = DualNumber_T(2.0, { 0.0, 1.0 }); // y = 2.0 with dual part for df/dy - VecX_T> f = vectorFunction(x); - MatX_T J_ad(2, 2); - J_ad(0, 0) = f(0).dual[0]; // df1/dx - J_ad(0, 1) = f(0).dual[1]; // df1/dy - J_ad(1, 0) = f(1).dual[0]; // df2/dx - J_ad(1, 1) = f(1).dual[1]; // df2/dy - for (int i = 0; i < J_ad.rows(); ++i) { - for (int j = 0; j < J_ad.cols(); ++j) { - std::string errorMsg = "AD Jacobian contains non-finite value at (" + std::to_string(i) - + ", " + std::to_string(j) - + "): " + std::to_string(J_ad(i, j)); - ASSERT_TRUE(std::isfinite(J_ad(i, j)), errorMsg.c_str()); - ASSERT_TRUE(std::abs(J_ad(i, j)) < 1e6, "AD Jacobian contains excessively large value"); - } +// Tests the conditioning of the Jacobian matrix computed via automatic differentiation, ensuring that it does not contain excessively large or small values that could indicate numerical instability. +TEST("AD Jacobian", ADJacobian_numericalStability) { + VecX_T> x(2); + x(0) = DualNumber_T(1.0, { 1.0, 0.0 }); // x = 1.0 with dual part for df/dx + x(1) = DualNumber_T(2.0, { 0.0, 1.0 }); // y = 2.0 with dual part for df/dy + VecX_T> f = vectorFunction(x); + MatX_T J_ad(2, 2); + J_ad(0, 0) = f(0).dual[0]; // df1/dx + J_ad(0, 1) = f(0).dual[1]; // df1/dy + J_ad(1, 0) = f(1).dual[0]; // df2/dx + J_ad(1, 1) = f(1).dual[1]; // df2/dy + for (int i = 0; i < J_ad.rows(); ++i) { + for (int j = 0; j < J_ad.cols(); ++j) { + std::string errorMsg = "AD Jacobian contains non-finite value at (" + std::to_string(i) + + ", " + std::to_string(j) + + "): " + std::to_string(J_ad(i, j)); + ASSERT_TRUE(std::isfinite(J_ad(i, j)), errorMsg.c_str()); + ASSERT_TRUE(std::abs(J_ad(i, j)) < 1e6, "AD Jacobian contains excessively large value"); } - ASSERT_TRUE(J_ad.norm() < 1e6, "AD Jacobian norm is excessively large, indicating potential numerical instability"); } + ASSERT_TRUE(J_ad.norm() < 1e6, "AD Jacobian norm is excessively large, indicating potential numerical instability"); +} - // Tests the consistency of the Jacobian computed via automatic differentiation across multiple evaluations, ensuring that repeated computations yield the same results. - TEST("AD Jacobian", ADJacobian_consistency) { - VecX_T> x(2); - x(0) = DualNumber_T(1.0, { 1.0, 0.0 }); // x = 1.0 with dual part for df/dx - x(1) = DualNumber_T(2.0, { 0.0, 1.0 }); // y = 2.0 with dual part for df/dy - MatX_T J_first(2, 2); - MatX_T J_second(2, 2); - VecX_T> f_first = vectorFunction(x); - J_first(0, 0) = f_first(0).dual[0]; // df1/dx - J_first(0, 1) = f_first(0).dual[1]; // df1/dy - J_first(1, 0) = f_first(1).dual[0]; // df2/dx - J_first(1, 1) = f_first(1).dual[1]; // df2/dy - VecX_T> f_second = vectorFunction(x); - J_second(0, 0) = f_second(0).dual[0]; // df1/dx - J_second(0, 1) = f_second(0).dual[1]; // df1/dy - J_second(1, 0) = f_second(1).dual[0]; // df2/dx - J_second(1, 1) = f_second(1).dual[1]; // df2/dy - for (int i = 0; i < J_first.rows(); ++i) { - for (int j = 0; j < J_first.cols(); ++j) { - std::string errorMsg = "Inconsistent AD Jacobian at (" + std::to_string(i) - + ", " + std::to_string(j) - + "): " + std::to_string(J_first(i, j)) - + " vs " + std::to_string(J_second(i, j)); - ASSERT_TRUE(std::abs(J_first(i, j) - J_second(i, j)) < 1e-6, errorMsg.c_str()); - ASSERT_TRUE(std::isfinite(J_first(i, j)) && std::isfinite(J_second(i, j)), "AD Jacobian contains non-finite value"); - } +// Tests the consistency of the Jacobian computed via automatic differentiation across multiple evaluations, ensuring that repeated computations yield the same results. +TEST("AD Jacobian", ADJacobian_consistency) { + VecX_T> x(2); + x(0) = DualNumber_T(1.0, { 1.0, 0.0 }); // x = 1.0 with dual part for df/dx + x(1) = DualNumber_T(2.0, { 0.0, 1.0 }); // y = 2.0 with dual part for df/dy + MatX_T J_first(2, 2); + MatX_T J_second(2, 2); + VecX_T> f_first = vectorFunction(x); + J_first(0, 0) = f_first(0).dual[0]; // df1/dx + J_first(0, 1) = f_first(0).dual[1]; // df1/dy + J_first(1, 0) = f_first(1).dual[0]; // df2/dx + J_first(1, 1) = f_first(1).dual[1]; // df2/dy + VecX_T> f_second = vectorFunction(x); + J_second(0, 0) = f_second(0).dual[0]; // df1/dx + J_second(0, 1) = f_second(0).dual[1]; // df1/dy + J_second(1, 0) = f_second(1).dual[0]; // df2/dx + J_second(1, 1) = f_second(1).dual[1]; // df2/dy + for (int i = 0; i < J_first.rows(); ++i) { + for (int j = 0; j < J_first.cols(); ++j) { + std::string errorMsg = "Inconsistent AD Jacobian at (" + std::to_string(i) + + ", " + std::to_string(j) + + "): " + std::to_string(J_first(i, j)) + + " vs " + std::to_string(J_second(i, j)); + ASSERT_TRUE(std::abs(J_first(i, j) - J_second(i, j)) < 1e-6, errorMsg.c_str()); + ASSERT_TRUE(std::isfinite(J_first(i, j)) && std::isfinite(J_second(i, j)), "AD Jacobian contains non-finite value"); } - ASSERT_TRUE(J_first.isApprox(J_second, 1e-6), "AD Jacobians from repeated evaluations do not match"); } + ASSERT_TRUE(J_first.isApprox(J_second, 1e-6), "AD Jacobians from repeated evaluations do not match"); } \ No newline at end of file diff --git a/PxMLib/MathLib_Unit/AutomaticDifferentiationTests/ADNewtonTests.cpp b/PxMLib/MathLib_Unit/AutomaticDifferentiationTests/ADNewtonTests.cpp index 7132f988..2679d0d8 100644 --- a/PxMLib/MathLib_Unit/AutomaticDifferentiationTests/ADNewtonTests.cpp +++ b/PxMLib/MathLib_Unit/AutomaticDifferentiationTests/ADNewtonTests.cpp @@ -29,38 +29,38 @@ namespace { auto J = nonlinearFuncDerivative(x); return x - g / J; } +} - // Test that the Newton-Raphson step using dual numbers correctly finds the root of the nonlinear function and that the Jacobian is computed correctly. - TEST("AD Newton-Raphson", ADNewton_RootFinding) { - DualNumber_T x(1.0, { 1.0 }); +// Test that the Newton-Raphson step using dual numbers correctly finds the root of the nonlinear function and that the Jacobian is computed correctly. +TEST("AD Newton-Raphson", ADNewton_RootFinding) { + DualNumber_T x(1.0, { 1.0 }); + for (int iter = 0; iter < 10; ++iter) { + x = newtonRaphsonStep(x); + } + double root = x.real; + ASSERT_TRUE(std::abs(root - std::sqrt(2.0)) < 1e-6, "Root finding error too large"); +} +// Test that the Newton-Raphson step using dual numbers converges to the root of the nonlinear function from different initial guesses. +TEST("AD Newton-Raphson", ADNewton_Convergence) { + std::vector x0 = { 0.5, 1.0, 1.5, 2.0 }; + for (double x_guess : x0) { + DualNumber_T x(x_guess, { 1.0 }); for (int iter = 0; iter < 10; ++iter) { - x = newtonRaphsonStep(x); + x = newtonRaphsonStep(x); } double root = x.real; - ASSERT_TRUE(std::abs(root - std::sqrt(2.0)) < 1e-6, "Root finding error too large"); - } - // Test that the Newton-Raphson step using dual numbers converges to the root of the nonlinear function from different initial guesses. - TEST("AD Newton-Raphson", ADNewton_Convergence) { - std::vector x0 = { 0.5, 1.0, 1.5, 2.0 }; - for (double x_guess : x0) { - DualNumber_T x(x_guess, { 1.0 }); - for (int iter = 0; iter < 10; ++iter) { - x = newtonRaphsonStep(x); - } - double root = x.real; - double residual = std::abs(nonlinearFunc(x).real); + double residual = std::abs(nonlinearFunc(x).real); - std::string errorMsg = "Root finding error too large for initial guess " + std::to_string(x_guess); - ASSERT_TRUE(std::abs(root - std::sqrt(2.0)) < 1e-6, errorMsg.c_str()); - ASSERT_TRUE(residual < 1e-10, "Residual is too large after Newton solve"); - } - } - // Test that the Jacobian computed using dual numbers matches the analytical derivative of the nonlinear function. - TEST("AD Newton-Raphson", ADNewton_JacobianAccuracy) { - DualNumber_T x(1.0, { 1.0 }); - DualNumber_T g = nonlinearFunc(x); - double computedJacobian = g.dual[0]; - double expectedJacobian = nonlinearFuncDerivative(x).real; - ASSERT_TRUE(std::abs(computedJacobian - expectedJacobian) < 1e-6, "Jacobian accuracy error too large"); + std::string errorMsg = "Root finding error too large for initial guess " + std::to_string(x_guess); + ASSERT_TRUE(std::abs(root - std::sqrt(2.0)) < 1e-6, errorMsg.c_str()); + ASSERT_TRUE(residual < 1e-10, "Residual is too large after Newton solve"); } +} +// Test that the Jacobian computed using dual numbers matches the analytical derivative of the nonlinear function. +TEST("AD Newton-Raphson", ADNewton_JacobianAccuracy) { + DualNumber_T x(1.0, { 1.0 }); + DualNumber_T g = nonlinearFunc(x); + double computedJacobian = g.dual[0]; + double expectedJacobian = nonlinearFuncDerivative(x).real; + ASSERT_TRUE(std::abs(computedJacobian - expectedJacobian) < 1e-6, "Jacobian accuracy error too large"); } \ No newline at end of file From 32c0ba5bedbbdbfc90b4d4ef821f09a7141f060e Mon Sep 17 00:00:00 2001 From: saltyjoss Date: Tue, 19 May 2026 18:00:25 +0100 Subject: [PATCH 039/210] fixes: Corrected debugging leftover in `Step` (Removed) --- DSFE_App/DSFE_Core/src/Robots/RobotSystem.cpp | 34 +++---------------- 1 file changed, 4 insertions(+), 30 deletions(-) diff --git a/DSFE_App/DSFE_Core/src/Robots/RobotSystem.cpp b/DSFE_App/DSFE_Core/src/Robots/RobotSystem.cpp index 279b6d0e..a20a53db 100644 --- a/DSFE_App/DSFE_Core/src/Robots/RobotSystem.cpp +++ b/DSFE_App/DSFE_Core/src/Robots/RobotSystem.cpp @@ -348,37 +348,11 @@ namespace robots { // Define the derivative function for integration, capturing necessary variables by reference auto f_deriv = [&, kp_frozen, kd_frozen](auto t, const auto& xIn) { - using Scalar = std::decay_t; - - DynamicsScratch scratch; - DynamicsResult result; - - SpatialModel spatialModel_s; - - spatialModel_s.linkNameToIndex = _spatialModel.linkNameToIndex; - spatialModel_s.joints.resize(_spatialModel.joints.size()); - - for (size_t i = 0; i < _spatialModel.joints.size(); ++i) { - - const auto& src = _spatialModel.joints[i]; - auto& dst = spatialModel_s.joints[i]; - - dst.parent = src.parent; - dst.type = src.type; - dst.name = src.name; - - dst.Xtree = src.Xtree.template cast(); - dst.inertia = src.inertia.template cast(); - dst.S.v = src.S.v.template cast(); - } - - auto snap_s = robots::castSnapshot(snap); - - return _dynamics->template derivative_spatial( - spatialModel_s, + return _dynamics->derivative_spatial( + _spatialModel, t, xIn, - snap_s, - scratch, result + snap, + _dynScratch, _dynResult ); }; // Define the Jacobian function for integration, capturing necessary variables by reference From 8ea3f9a1f7d952d51a161bd4d43d57e914d1f4ef Mon Sep 17 00:00:00 2001 From: saltyjoss Date: Tue, 19 May 2026 19:18:58 +0100 Subject: [PATCH 040/210] feat: Added new solver tests --- PxMLib/MathLib/include/core/DualNumbers.h | 145 ++++++++++++------ .../DualNumber_Eigen_ADTests.cpp | 79 +++++++++- .../DualNumber_Eigen_SolverTests.cpp | 59 ++++--- PxMLib/MathLib_Unit/Common/TestHarness.h | 5 +- 4 files changed, 218 insertions(+), 70 deletions(-) diff --git a/PxMLib/MathLib/include/core/DualNumbers.h b/PxMLib/MathLib/include/core/DualNumbers.h index ef5fcc22..a9b92a57 100644 --- a/PxMLib/MathLib/include/core/DualNumbers.h +++ b/PxMLib/MathLib/include/core/DualNumbers.h @@ -66,6 +66,7 @@ namespace mathlib { // Comparison Operators (compare only the real part) // ----- + // Comparison between dual numbers (compare only the real part) template inline bool operator==(const DualNumber_T& a, const DualNumber_T& b) { return a.real == b.real; } template @@ -73,12 +74,26 @@ namespace mathlib { template inline bool operator<(const DualNumber_T& a, const DualNumber_T& b) { return a.real < b.real; } template - inline bool operator<=(const DualNumber_T& a, const DualNumber_T& b) { return a.real <= b.real; } - template inline bool operator>(const DualNumber_T& a, const DualNumber_T& b) { return a.real > b.real; } template + inline bool operator<=(const DualNumber_T& a, const DualNumber_T& b) { return a.real <= b.real; } + template inline bool operator>=(const DualNumber_T& a, const DualNumber_T& b) { return a.real >= b.real; } + // Comparison with scalar (compare only the real part) + template + inline bool operator==(const DualNumber_T& a, Scalar b) { return a.real == b; } + template + inline bool operator!=(const DualNumber_T& a, Scalar b) { return !(a == b); } + template + inline bool operator<(const DualNumber_T& a, Scalar b) { return a.real < b; } + template + inline bool operator>(const DualNumber_T& a, Scalar b) { return a.real > b; } + template + inline bool operator<=(const DualNumber_T& a, Scalar b) { return a.real <= b; } + template + inline bool operator>=(const DualNumber_T& a, Scalar b) { return a.real >= b; } + // ----- // Scalar Interactions // ----- @@ -269,6 +284,15 @@ namespace mathlib { for (size_t i = 0; i < NVar; ++i) { out.dual[i] = derivative * x.dual[i]; } return out; } + // Logarithm function + template + inline DualNumber_T log(const DualNumber_T& a) { + if (a.real <= Scalar(0)) { throw std::runtime_error("log() domain error for DualNumber_T"); } + DualNumber_T out; + out.real = std::log(a.real); + for (size_t i = 0; i < NVar; ++i) { out.dual[i] = a.dual[i] / a.real; } + return out; + } // ----- // Assignment Operators @@ -521,55 +545,78 @@ namespace Eigen { static inline Real highest() { return std::numeric_limits::max(); } static inline Real lowest() { return std::numeric_limits::lowest(); } }; -} // namespace Eigen -// Specialisation of Eigen's internal traits to ensure that DualNumber_T is treated as a non-arithmetic type -namespace Eigen::internal { - template - struct is_arithmetic> : std::false_type {}; - template - struct scalar_abs_op> { - using Dual = mathlib::DualNumber_T; + // Specialisation of Eigen's internal traits to ensure that DualNumber_T is treated as a non-arithmetic type + namespace internal { + template + struct is_arithmetic> : std::false_type {}; - EIGEN_DEVICE_FUNC Scalar operator()(const Dual& x) const { return std::abs(x.real); } - }; + template + struct scalar_abs_op> { + using Dual = mathlib::DualNumber_T; - // Custom scoring function for DualNumber_T that compares based on the absolute value of the real part - template - struct scalar_score_coeff_op> { - // The result_type is a simple wrapper around the score (which is the absolute value of the real part of the dual number) - struct result_type { - Scalar score; - result_type(int i = 0) : score(i) {} // Default constructor for compatibility with Eigen's internal mechanisms - result_type(const mathlib::DualNumber_T& x) // Constructor to compute score from DualNumber_T - : score(std::abs(x.real)) {} + EIGEN_DEVICE_FUNC Scalar operator()(const Dual& x) const { return std::abs(x.real); } + }; - friend bool operator <(const result_type& a, const result_type& b) { return a.score < b.score; } - friend bool operator >(const result_type& a, const result_type& b) { return a.score > b.score; } - friend bool operator ==(const result_type& a, const result_type& b) { return a.score == b.score; } + // Custom scoring function for DualNumber_T that compares based on the absolute value of the real part + template + struct scalar_score_coeff_op> { + // The result_type is a simple wrapper around the score (which is the absolute value of the real part of the dual number) + struct result_type { + Scalar score; + result_type(int i = 0) : score(i) {} // Default constructor for compatibility with Eigen's internal mechanisms + result_type(const mathlib::DualNumber_T& x) // Constructor to compute score from DualNumber_T + : score(std::abs(x.real)) {} + + friend bool operator <(const result_type& a, const result_type& b) { return a.score < b.score; } + friend bool operator >(const result_type& a, const result_type& b) { return a.score > b.score; } + friend bool operator ==(const result_type& a, const result_type& b) { return a.score == b.score; } + }; + // The operator() computes the score for a given DualNumber_T by taking the absolute value of its real part (this allows Eigen's internal sorting and selection algorithms to work correctly with dual numbers) + EIGEN_DEVICE_FUNC result_type operator()(const mathlib::DualNumber_T& x) const { return result_type(x); } }; - // The operator() computes the score for a given DualNumber_T by taking the absolute value of its real part (this allows Eigen's internal sorting and selection algorithms to work correctly with dual numbers) - EIGEN_DEVICE_FUNC result_type operator()(const mathlib::DualNumber_T& x) const { return result_type(x); } - }; -} // namespace Eigen::internal -// Specialisation of Eigen's numext functions for DualNumber_T to allow Eigen's algorithms that rely on these functions (like abs, real, imag, conj) to work correctly with dual numbers -namespace Eigen::numext { - // Returns the real part of the dual number - template - inline Scalar real(const mathlib::DualNumber_T& x) { return x.real; } - // Returns the imaginary part of the dual number - template - inline Scalar imag(const mathlib::DualNumber_T&) { return Scalar(0); } - // Returns the absolute value of the dual number - template - inline Scalar abs(const mathlib::DualNumber_T& x) { return std::abs(x.real); } - // Returns the squared absolute value of the dual number - template - inline Scalar abs2(const mathlib::DualNumber_T& x) { return x.real * x.real; } - // Returns the 1-norm of the dual number - template - inline Scalar norm1(const mathlib::DualNumber_T& x) { return std::abs(x.real); } - // Returns the 2-norm of the dual number - template - inline mathlib::DualNumber_T conj(const mathlib::DualNumber_T& x) { return x; } -}// namespace Eigen::numext \ No newline at end of file + + // Specialisation of Eigen's real_impl + template + struct real_impl> { + typedef Scalar return_type; + EIGEN_DEVICE_FUNC static return_type run(const mathlib::DualNumber_T& x) { return x.real; } + }; + + // Specialisation of Eigen's imag_impl + template + struct imag_impl> { + typedef Scalar return_type; + EIGEN_DEVICE_FUNC static return_type run(const mathlib::DualNumber_T&) { return Scalar(0); } + }; + + // Specialisation of Eigen's abs2_impl + template + struct abs2_impl> { + typedef Scalar return_type; + EIGEN_DEVICE_FUNC static return_type run(const mathlib::DualNumber_T& x) { return x.real * x.real; } + }; + } // namespace internal + + // Specialisation of Eigen's numext functions for DualNumber_T to allow Eigen's algorithms that rely on these functions (like abs, real, imag, conj) to work correctly with dual numbers + namespace numext { + // Returns the real part of the dual number + template + inline Scalar real(const mathlib::DualNumber_T& x) { return x.real; } + // Returns the imaginary part of the dual number + template + inline Scalar imag(const mathlib::DualNumber_T&) { return Scalar(0); } + // Returns the absolute value of the dual number + template + inline Scalar abs(const mathlib::DualNumber_T& x) { return std::abs(x.real); } + // Returns the squared absolute value of the dual number + template + inline Scalar abs2(const mathlib::DualNumber_T& x) { return x.real * x.real; } + // Returns the 1-norm of the dual number + template + inline Scalar norm1(const mathlib::DualNumber_T& x) { return std::abs(x.real); } + // Returns the 2-norm of the dual number + template + inline mathlib::DualNumber_T conj(const mathlib::DualNumber_T& x) { return x; } + }// namespace numext +} // namespace Eigen \ No newline at end of file diff --git a/PxMLib/MathLib_Unit/AutomaticDifferentiationTests/DualNumber_Eigen_ADTests.cpp b/PxMLib/MathLib_Unit/AutomaticDifferentiationTests/DualNumber_Eigen_ADTests.cpp index 101fdec1..0c1e71de 100644 --- a/PxMLib/MathLib_Unit/AutomaticDifferentiationTests/DualNumber_Eigen_ADTests.cpp +++ b/PxMLib/MathLib_Unit/AutomaticDifferentiationTests/DualNumber_Eigen_ADTests.cpp @@ -1,9 +1,86 @@ #include "TestHarness.h" #include +#include +#include using namespace mathlib; +using namespace constants; namespace { using Dual = DualNumber_T; constexpr double EPS = 1e-9; -} \ No newline at end of file + + integration::NumericalIntegrator integrator; +} + +TEST("DualNumber_T Eigen AD", PolynomialDerivative) { + Dual x(2.0); + x.dual[0] = 1.0; + Dual y = x * x + 3.0 * x + 1.0; + ASSERT_TRUE(y.real == 11.0, "Polynomial evaluation real part incorrect"); + ASSERT_TRUE(y.dual[0] == 7.0, "Polynomial derivative dual part incorrect"); +} +TEST("DualNumber_T Eigen AD", SinDerivative) { + Dual x(PI_d / 4); // π/4 + x.dual[0] = 1.0; + Dual y = sin(x); + ASSERT_TRUE(std::abs(y.real - std::sin(PI_d / 4)) < EPS, "Sine evaluation real part incorrect"); + ASSERT_TRUE(std::abs(y.dual[0] - std::cos(PI_d / 4)) < EPS, "Sine derivative dual part incorrect"); +} +TEST("DualNumber_T Eigen AD", CosDerivative) { + Dual x(PI_d / 4); // π/4 + x.dual[0] = 1.0; + Dual y = cos(x); + ASSERT_TRUE(std::abs(y.real - std::cos(PI_d / 4)) < EPS, "Cosine evaluation real part incorrect"); + ASSERT_TRUE(std::abs(y.dual[0] + std::sin(PI_d / 4)) < EPS, "Cosine derivative dual part incorrect"); +} +TEST("DualNumber_T Eigen AD", TanDerivative) { + Dual x(PI_d / 4); // π/4 + x.dual[0] = 1.0; + Dual y = tan(x); + double expectedReal = std::tan(PI_d / 4); + double expectedDual = 1.0 / (std::cos(PI_d / 4) * std::cos(PI_d / 4)); // sec^2(x) + ASSERT_TRUE(std::abs(y.real - expectedReal) < EPS, "Tangent evaluation real part incorrect"); + ASSERT_TRUE(std::abs(y.dual[0] - expectedDual) < EPS, "Tangent derivative dual part incorrect"); +} +TEST("DualNumber_T Eigen AD", ExpDerivative) { + Dual x(1.0); + x.dual[0] = 1.0; + Dual y = exp(x); + ASSERT_TRUE(std::abs(y.real - std::exp(1.0)) < EPS, "Exponential evaluation real part incorrect"); + ASSERT_TRUE(std::abs(y.dual[0] - std::exp(1.0)) < EPS, "Exponential derivative dual part incorrect"); +} +TEST("DualNumber_T Eigen AD", LogDerivative) { + Dual x(2.0); + x.dual[0] = 1.0; + Dual y = log(x); + ASSERT_TRUE(std::abs(y.real - std::log(2.0)) < EPS, "Logarithm evaluation real part incorrect"); + ASSERT_TRUE(std::abs(y.dual[0] - 0.5) < EPS, "Logarithm derivative dual part incorrect"); +} + +//TEST("DualNumber_T Eigen AD", AutomaticDifferenceJacobian) { +// // Define a simple function f: R^2 -> R^2 for testing +// auto f = [](Dual t, const VecX_T& x) -> VecX_T { +// VecX_T y(2); +// y(0) = x(0) * x(0) + t; // f1(x, t) = x1^2 + t +// y(1) = x(0) * x(1); // f2(x, t) = x1 * x2 +// return y; +// }; +// Dual t = Dual(1.0, { 0.5 }); // Perturbation in time +// VecX_T x(2); +// x << Dual(2.0, { 0.5 }), Dual(3.0, { 0.5 }); // Perturbations in state +// // Compute the Jacobian using automatic differentiation +// mathlib::MatX_T J_ad = integration::NumericalIntegrator::automaticDifferenceJacobian(f, t, x); +// // Compute the expected Jacobian manually +// mathlib::MatX_T J_expected(2, 2); +// J_expected << 4.0, 0.0, +// 3.0, 2.0; +// // Check that the computed Jacobian matches the expected Jacobian within a tolerance +// for (int i = 0; i < J_ad.rows(); ++i) { +// for (int j = 0; j < J_ad.cols(); ++j) { +// +// ASSERT_TRUE(std::abs(J_ad(i, j) - J_expected(i, j)) < EPS, +// "AD Jacobian entry (" + std::to_string(i) + ", " + std::to_string(j) + ") is incorrect"); +// } +// } +//} \ No newline at end of file diff --git a/PxMLib/MathLib_Unit/AutomaticDifferentiationTests/DualNumber_Eigen_SolverTests.cpp b/PxMLib/MathLib_Unit/AutomaticDifferentiationTests/DualNumber_Eigen_SolverTests.cpp index 10ce3604..e9fa4f08 100644 --- a/PxMLib/MathLib_Unit/AutomaticDifferentiationTests/DualNumber_Eigen_SolverTests.cpp +++ b/PxMLib/MathLib_Unit/AutomaticDifferentiationTests/DualNumber_Eigen_SolverTests.cpp @@ -8,24 +8,6 @@ namespace { constexpr double EPS = 1e-9; } -TEST("DualNumber_T Eigen Solvers", PartialPivLU) { - Eigen::Matrix A; - Eigen::Matrix b; - - A(0, 0) = Dual(4.0, { 0.5 }); - A(0, 1) = Dual(2.0, { 0.5 }); - A(1, 0) = Dual(1.0, { 0.5 }); - A(1, 1) = Dual(3.0, { 0.5 }); - b << Dual(10.0, { 1.0 }), Dual(11.0, { 1.0 }); - - auto solver = A.partialPivLu(); - - auto x = solver.solve(b); - - ASSERT_TRUE(std::isfinite(x(0).real), "Real part of solution should be finite"); - ASSERT_TRUE(std::isfinite(x(0).dual[0]), "Dual part of solution should be finite"); -} - TEST("DualNumber_T Eigen Solvers", MaxCoeff) { Eigen::Matrix A; @@ -38,7 +20,6 @@ TEST("DualNumber_T Eigen Solvers", MaxCoeff) { ASSERT_TRUE(std::isfinite(m.real), "Max coefficient real part should be finite"); } - TEST("DualNumber_T Eigen Solvers", CwiseAbsMaxCoeff) { Eigen::Matrix A; @@ -50,4 +31,44 @@ TEST("DualNumber_T Eigen Solvers", CwiseAbsMaxCoeff) { auto m = A.cwiseAbs().maxCoeff(); ASSERT_TRUE(std::isfinite(m), "CwiseAbsMaxCoeff real part should be finite"); +} +TEST("DualNumber_T Eigen Solvers", PartialPivLU) { + Eigen::Matrix A; + Eigen::Matrix b; + A(0, 0) = Dual(4.0, { 0.5 }); + A(0, 1) = Dual(2.0, { 0.5 }); + A(1, 0) = Dual(1.0, { 0.5 }); + A(1, 1) = Dual(3.0, { 0.5 }); + b << Dual(10.0, { 1.0 }), Dual(11.0, { 1.0 }); + auto solver = A.partialPivLu(); + auto x = solver.solve(b); + ASSERT_TRUE(std::isfinite(x(0).real), "Real part of solution should be finite"); + ASSERT_TRUE(std::isfinite(x(0).dual[0]), "Dual part of solution should be finite"); +} +TEST("DualNumber_T Eigen Solvers", LDLT) { + Eigen::Matrix A; + Eigen::Matrix b; + A(0, 0) = Dual(4.0, { 0.5 }); + A(0, 1) = Dual(2.0, { 0.5 }); + A(1, 0) = Dual(1.0, { 0.5 }); + A(1, 1) = Dual(3.0, { 0.5 }); + b << Dual(10.0, { 1.0 }), Dual(11.0, { 1.0 }); + auto solver = A.ldlt(); + auto x = solver.solve(b); + ASSERT_TRUE(std::isfinite(x(0).real), "Real part of solution should be finite"); + ASSERT_TRUE(std::isfinite(x(0).dual[0]), "Dual part of solution should be finite"); +} + +TEST("DualNumber_T Eigen Solvers", LDLTComputeOnly) { + Eigen::Matrix A; + + A << + Dual(4.0), Dual(1.0), + Dual(1.0), Dual(3.0); + + Eigen::LDLT> ldlt; + + ldlt.compute(A); + + SUCCEED(); } \ No newline at end of file diff --git a/PxMLib/MathLib_Unit/Common/TestHarness.h b/PxMLib/MathLib_Unit/Common/TestHarness.h index 218ed2f1..f8d44875 100644 --- a/PxMLib/MathLib_Unit/Common/TestHarness.h +++ b/PxMLib/MathLib_Unit/Common/TestHarness.h @@ -93,4 +93,7 @@ namespace test { groupName, #testName, testName##_impl); } \ static void testName##_impl() -#define ASSERT_TRUE(cond, msg) test::assertTrue((cond), (msg)) \ No newline at end of file +#define ASSERT_TRUE(cond, msg) test::assertTrue((cond), (msg)) +#define ASSERT_EQ(a, b, tol, msg) test::assertTrue(std::abs((a) - (b)) < (tol), (msg) +#define SUCCEED() return +#define EXPECT_NEAR(a, b, tol) test::assertTrue(std::abs((a) - (b)) < (tol), "Expected " #a " and " #b " to be within " #tol) \ No newline at end of file From 45228981795b8c86204bc1828fe637311671c3c4 Mon Sep 17 00:00:00 2001 From: saltyjoss Date: Tue, 19 May 2026 23:17:44 +0100 Subject: [PATCH 041/210] chore: Fixed location of DualNumber UT --- .../ADImplicitIntegratorTests.cpp | 28 +++++++++---------- .../CMakeLists.txt | 4 --- .../DualNumberTests/CMakeLists.txt | 4 +++ .../DualNumber_Eigen_ADTests.cpp | 0 .../DualNumber_Eigen_ArithmeticTests.cpp | 0 .../DualNumber_Eigen_BasicTests.cpp | 0 .../DualNumber_Eigen_SolverTests.cpp | 0 7 files changed, 18 insertions(+), 18 deletions(-) rename PxMLib/MathLib_Unit/{AutomaticDifferentiationTests => DualNumberTests}/DualNumber_Eigen_ADTests.cpp (100%) rename PxMLib/MathLib_Unit/{AutomaticDifferentiationTests => DualNumberTests}/DualNumber_Eigen_ArithmeticTests.cpp (100%) rename PxMLib/MathLib_Unit/{AutomaticDifferentiationTests => DualNumberTests}/DualNumber_Eigen_BasicTests.cpp (100%) rename PxMLib/MathLib_Unit/{AutomaticDifferentiationTests => DualNumberTests}/DualNumber_Eigen_SolverTests.cpp (100%) diff --git a/PxMLib/MathLib_Unit/AutomaticDifferentiationTests/ADImplicitIntegratorTests.cpp b/PxMLib/MathLib_Unit/AutomaticDifferentiationTests/ADImplicitIntegratorTests.cpp index 8abbe6ae..74aad5f7 100644 --- a/PxMLib/MathLib_Unit/AutomaticDifferentiationTests/ADImplicitIntegratorTests.cpp +++ b/PxMLib/MathLib_Unit/AutomaticDifferentiationTests/ADImplicitIntegratorTests.cpp @@ -126,20 +126,20 @@ namespace { } } -//// AD Implicit Euler Tests -//// Test case for the GLRK2 method on the exponenital decay ODE. -//TEST("AD Implicit Euler Method", ImplicitEuler_ExponentialDecay) { -// VecX_T> x(1); -// x(0) = DualNumber_T(1.0, { 0.0 }); -// double T = 1.0; -// DualNumber_T dt(T / 1000.0, { 0.0 }); -// DualNumber_T t(0.0, { 0.0 }); -// for (int i = 0; i < 1000; ++i) { -// x = integrator.implicitEuler_AD(x, t, dt, expDecay, 8, DualNumber_T(1e-6, { 0.0 })); -// t += dt; -// } -// ASSERT_TRUE(std::abs(x(0).real - std::exp(-1.0)) < tol_low, "Implicit Euler exponential decay error too large"); -//} +// AD Implicit Euler Tests +// Test case for the GLRK2 method on the exponenital decay ODE. +TEST("AD Implicit Euler Method", ImplicitEuler_ExponentialDecay) { + VecX_T> x(1); + x(0) = DualNumber_T(1.0, { 0.0 }); + double T = 1.0; + DualNumber_T dt(T / 1000.0, { 0.0 }); + DualNumber_T t(0.0, { 0.0 }); + for (int i = 0; i < 1000; ++i) { + x = integrator.implicitEuler_AD(x, t, dt, expDecay, 8, DualNumber_T(1e-6, { 0.0 })); + t += dt; + } + ASSERT_TRUE(std::abs(x(0).real - std::exp(-1.0)) < tol_low, "Implicit Euler exponential decay error too large"); +} //// Test case for the implicit Euler method on the exponential decay ODE. //TEST("AD Implicit Euler Method", ImplicitEuler_ExponentialDecaySensitivity) { // auto stepFn = [&]( diff --git a/PxMLib/MathLib_Unit/AutomaticDifferentiationTests/CMakeLists.txt b/PxMLib/MathLib_Unit/AutomaticDifferentiationTests/CMakeLists.txt index be6c8bf9..4652f2d8 100644 --- a/PxMLib/MathLib_Unit/AutomaticDifferentiationTests/CMakeLists.txt +++ b/PxMLib/MathLib_Unit/AutomaticDifferentiationTests/CMakeLists.txt @@ -10,10 +10,6 @@ add_executable(MathLib_ADTests ADJacobianTests.cpp ADExplicitIntegratorTests.cpp ADImplicitIntegratorTests.cpp - DualNumber_Eigen_BasicTests.cpp - DualNumber_Eigen_ArithmeticTests.cpp - DualNumber_Eigen_SolverTests.cpp - DualNumber_Eigen_ADTests.cpp ) target_link_libraries(MathLib_ADTests PRIVATE diff --git a/PxMLib/MathLib_Unit/DualNumberTests/CMakeLists.txt b/PxMLib/MathLib_Unit/DualNumberTests/CMakeLists.txt index f70db1f1..08c8e969 100644 --- a/PxMLib/MathLib_Unit/DualNumberTests/CMakeLists.txt +++ b/PxMLib/MathLib_Unit/DualNumberTests/CMakeLists.txt @@ -11,6 +11,10 @@ add_executable(MathLib_DualNumberTests AssignmentOperatorTests.cpp DifferentiationTests.cpp TranscendentalTests.cpp + DualNumber_Eigen_BasicTests.cpp + DualNumber_Eigen_ArithmeticTests.cpp + DualNumber_Eigen_SolverTests.cpp + DualNumber_Eigen_ADTests.cpp ) target_link_libraries(MathLib_DualNumberTests PRIVATE diff --git a/PxMLib/MathLib_Unit/AutomaticDifferentiationTests/DualNumber_Eigen_ADTests.cpp b/PxMLib/MathLib_Unit/DualNumberTests/DualNumber_Eigen_ADTests.cpp similarity index 100% rename from PxMLib/MathLib_Unit/AutomaticDifferentiationTests/DualNumber_Eigen_ADTests.cpp rename to PxMLib/MathLib_Unit/DualNumberTests/DualNumber_Eigen_ADTests.cpp diff --git a/PxMLib/MathLib_Unit/AutomaticDifferentiationTests/DualNumber_Eigen_ArithmeticTests.cpp b/PxMLib/MathLib_Unit/DualNumberTests/DualNumber_Eigen_ArithmeticTests.cpp similarity index 100% rename from PxMLib/MathLib_Unit/AutomaticDifferentiationTests/DualNumber_Eigen_ArithmeticTests.cpp rename to PxMLib/MathLib_Unit/DualNumberTests/DualNumber_Eigen_ArithmeticTests.cpp diff --git a/PxMLib/MathLib_Unit/AutomaticDifferentiationTests/DualNumber_Eigen_BasicTests.cpp b/PxMLib/MathLib_Unit/DualNumberTests/DualNumber_Eigen_BasicTests.cpp similarity index 100% rename from PxMLib/MathLib_Unit/AutomaticDifferentiationTests/DualNumber_Eigen_BasicTests.cpp rename to PxMLib/MathLib_Unit/DualNumberTests/DualNumber_Eigen_BasicTests.cpp diff --git a/PxMLib/MathLib_Unit/AutomaticDifferentiationTests/DualNumber_Eigen_SolverTests.cpp b/PxMLib/MathLib_Unit/DualNumberTests/DualNumber_Eigen_SolverTests.cpp similarity index 100% rename from PxMLib/MathLib_Unit/AutomaticDifferentiationTests/DualNumber_Eigen_SolverTests.cpp rename to PxMLib/MathLib_Unit/DualNumberTests/DualNumber_Eigen_SolverTests.cpp From 6f51acedc9fbc2f953f624a53cf67e0acad75b2d Mon Sep 17 00:00:00 2001 From: saltyjoss Date: Tue, 19 May 2026 23:18:32 +0100 Subject: [PATCH 042/210] fixes: Updated `DualNumber` to include structs for `BaseScalar` template type --- PxMLib/MathLib/include/core/DualNumbers.h | 16 ++++++++++++++++ 1 file changed, 16 insertions(+) diff --git a/PxMLib/MathLib/include/core/DualNumbers.h b/PxMLib/MathLib/include/core/DualNumbers.h index a9b92a57..5900eb46 100644 --- a/PxMLib/MathLib/include/core/DualNumbers.h +++ b/PxMLib/MathLib/include/core/DualNumbers.h @@ -15,6 +15,22 @@ #include namespace mathlib { + // Forward declaration of the DualNumber_T template class + template + class DualNumber_T; + + // Helper struct to extract the underlying scalar type from a dual number + template + struct BaseScalar { + using type = T; + }; + + // Specialisation for dual numbers to extract the underlying scalar type + template + struct BaseScalar> { + using type = Scalar; + }; + // Template version of dual number template class DualNumber_T { From 3f001eed6391c23759a6909313d578a6eeddf391 Mon Sep 17 00:00:00 2001 From: saltyjoss Date: Tue, 19 May 2026 23:18:50 +0100 Subject: [PATCH 043/210] fixes: Updated the return type of `automaticDifferentationJacobian` --- .../include/integrators/numerical_integrators.h | 2 +- .../include/integrators/numerical_integrators.inl | 13 +++++++------ 2 files changed, 8 insertions(+), 7 deletions(-) diff --git a/PxMLib/MathLib/include/integrators/numerical_integrators.h b/PxMLib/MathLib/include/integrators/numerical_integrators.h index 2a3804c0..e0331c54 100644 --- a/PxMLib/MathLib/include/integrators/numerical_integrators.h +++ b/PxMLib/MathLib/include/integrators/numerical_integrators.h @@ -190,7 +190,7 @@ namespace integration { ); template - mathlib::MatX_T automaticDifferenceJacobian( + mathlib::MatX_T::BaseScalar> automaticDifferenceJacobian( Func&& f, Scalar t, const mathlib::VecX_T& x diff --git a/PxMLib/MathLib/include/integrators/numerical_integrators.inl b/PxMLib/MathLib/include/integrators/numerical_integrators.inl index b5ddea61..238b01e7 100644 --- a/PxMLib/MathLib/include/integrators/numerical_integrators.inl +++ b/PxMLib/MathLib/include/integrators/numerical_integrators.inl @@ -995,26 +995,27 @@ namespace integration { // Automatic Difference Jacobian template - mathlib::MatX_T NumericalIntegrator::automaticDifferenceJacobian( + mathlib::MatX_T::BaseScalar> NumericalIntegrator::automaticDifferenceJacobian( Func&& f, Scalar t, const mathlib::VecX_T& x ) { + using RealScalar = typename mathlib::DualTraits::BaseScalar; + const int n = static_cast(x.size()); auto f0 = f(t, x); const int m = static_cast(f0.size()); - mathlib::MatX_T J(m, n); + mathlib::MatX_T J(m, n); for (int i = 0; i < n; ++i) { - using BaseScalar = typename mathlib::DualTraits::BaseScalar; - using Dual_T = mathlib::DualNumber_T; + using Dual_T = mathlib::DualNumber_T; mathlib::VecX_T x_dual(n); for (int k = 0; k < n; ++k) { x_dual(k) = Dual_T( mathlib::real(x(k)), - std::array{ - (k == i) ? BaseScalar(1) : BaseScalar(0) + std::array{ + (k == i) ? RealScalar(1) : RealScalar(0) } ); } From 1425823da11cbfc492e999062f8194ddfaf30589 Mon Sep 17 00:00:00 2001 From: saltyjoss Date: Tue, 19 May 2026 23:18:50 +0100 Subject: [PATCH 044/210] fixes: Updated the return type of `automaticDifferentationJacobian` --- .../integrators/numerical_integrators.h | 2 +- .../integrators/numerical_integrators.inl | 13 +++++----- .../ADImplicitIntegratorTests.cpp | 24 +++++++++---------- 3 files changed, 20 insertions(+), 19 deletions(-) diff --git a/PxMLib/MathLib/include/integrators/numerical_integrators.h b/PxMLib/MathLib/include/integrators/numerical_integrators.h index 2a3804c0..e0331c54 100644 --- a/PxMLib/MathLib/include/integrators/numerical_integrators.h +++ b/PxMLib/MathLib/include/integrators/numerical_integrators.h @@ -190,7 +190,7 @@ namespace integration { ); template - mathlib::MatX_T automaticDifferenceJacobian( + mathlib::MatX_T::BaseScalar> automaticDifferenceJacobian( Func&& f, Scalar t, const mathlib::VecX_T& x diff --git a/PxMLib/MathLib/include/integrators/numerical_integrators.inl b/PxMLib/MathLib/include/integrators/numerical_integrators.inl index b5ddea61..238b01e7 100644 --- a/PxMLib/MathLib/include/integrators/numerical_integrators.inl +++ b/PxMLib/MathLib/include/integrators/numerical_integrators.inl @@ -995,26 +995,27 @@ namespace integration { // Automatic Difference Jacobian template - mathlib::MatX_T NumericalIntegrator::automaticDifferenceJacobian( + mathlib::MatX_T::BaseScalar> NumericalIntegrator::automaticDifferenceJacobian( Func&& f, Scalar t, const mathlib::VecX_T& x ) { + using RealScalar = typename mathlib::DualTraits::BaseScalar; + const int n = static_cast(x.size()); auto f0 = f(t, x); const int m = static_cast(f0.size()); - mathlib::MatX_T J(m, n); + mathlib::MatX_T J(m, n); for (int i = 0; i < n; ++i) { - using BaseScalar = typename mathlib::DualTraits::BaseScalar; - using Dual_T = mathlib::DualNumber_T; + using Dual_T = mathlib::DualNumber_T; mathlib::VecX_T x_dual(n); for (int k = 0; k < n; ++k) { x_dual(k) = Dual_T( mathlib::real(x(k)), - std::array{ - (k == i) ? BaseScalar(1) : BaseScalar(0) + std::array{ + (k == i) ? RealScalar(1) : RealScalar(0) } ); } diff --git a/PxMLib/MathLib_Unit/AutomaticDifferentiationTests/ADImplicitIntegratorTests.cpp b/PxMLib/MathLib_Unit/AutomaticDifferentiationTests/ADImplicitIntegratorTests.cpp index 74aad5f7..856a6cf6 100644 --- a/PxMLib/MathLib_Unit/AutomaticDifferentiationTests/ADImplicitIntegratorTests.cpp +++ b/PxMLib/MathLib_Unit/AutomaticDifferentiationTests/ADImplicitIntegratorTests.cpp @@ -128,18 +128,18 @@ namespace { // AD Implicit Euler Tests // Test case for the GLRK2 method on the exponenital decay ODE. -TEST("AD Implicit Euler Method", ImplicitEuler_ExponentialDecay) { - VecX_T> x(1); - x(0) = DualNumber_T(1.0, { 0.0 }); - double T = 1.0; - DualNumber_T dt(T / 1000.0, { 0.0 }); - DualNumber_T t(0.0, { 0.0 }); - for (int i = 0; i < 1000; ++i) { - x = integrator.implicitEuler_AD(x, t, dt, expDecay, 8, DualNumber_T(1e-6, { 0.0 })); - t += dt; - } - ASSERT_TRUE(std::abs(x(0).real - std::exp(-1.0)) < tol_low, "Implicit Euler exponential decay error too large"); -} +//TEST("AD Implicit Euler Method", ImplicitEuler_ExponentialDecay) { +// VecX_T> x(1); +// x(0) = DualNumber_T(1.0, { 0.0 }); +// double T = 1.0; +// DualNumber_T dt(T / 1000.0, { 0.0 }); +// DualNumber_T t(0.0, { 0.0 }); +// for (int i = 0; i < 1000; ++i) { +// x = integrator.implicitEuler_AD(x, t, dt, expDecay, 8, DualNumber_T(1e-6, { 0.0 })); +// t += dt; +// } +// ASSERT_TRUE(std::abs(x(0).real - std::exp(-1.0)) < tol_low, "Implicit Euler exponential decay error too large"); +//} //// Test case for the implicit Euler method on the exponential decay ODE. //TEST("AD Implicit Euler Method", ImplicitEuler_ExponentialDecaySensitivity) { // auto stepFn = [&]( From 50c9513452bebe6ab81ffd8abbaad3354ba67ddc Mon Sep 17 00:00:00 2001 From: saltyjoss Date: Thu, 21 May 2026 22:00:00 +0100 Subject: [PATCH 045/210] chore: Further separate AD, Implicit, and Explicit integration methods with different .inl files * Still trying to configure the AD correctly, struggling with the DualNumber typedef config, maybe revert back (though I feel I am close) --- .../integrators/ad_implicit_integrators.inl | 372 ++++++ .../integrators/explicit_integrators.inl | 187 +++ .../integrators/implicit_integrators.inl | 424 +++++++ .../integrators/numerical_integrators.h | 4 +- .../integrators/numerical_integrators.inl | 1031 ----------------- .../ADImplicitIntegratorTests.cpp | 24 +- 6 files changed, 998 insertions(+), 1044 deletions(-) create mode 100644 PxMLib/MathLib/include/integrators/ad_implicit_integrators.inl create mode 100644 PxMLib/MathLib/include/integrators/explicit_integrators.inl create mode 100644 PxMLib/MathLib/include/integrators/implicit_integrators.inl delete mode 100644 PxMLib/MathLib/include/integrators/numerical_integrators.inl diff --git a/PxMLib/MathLib/include/integrators/ad_implicit_integrators.inl b/PxMLib/MathLib/include/integrators/ad_implicit_integrators.inl new file mode 100644 index 00000000..9dd8fd91 --- /dev/null +++ b/PxMLib/MathLib/include/integrators/ad_implicit_integrators.inl @@ -0,0 +1,372 @@ +// PxM/MathLib ad_implicit_integrators.inl +#pragma once + +namespace integration { + // AD version of Implicit Euler method + template + mathlib::VecX_T NumericalIntegrator::implicitEuler_AD( + const mathlib::VecX_T& x, + Scalar t, + Scalar dt, + Func&& f, + int maxIter, + Scalar tol + ) { + using Real = typename mathlib::DualTraits::BaseScalar; + + // The function g(x_guess) = 0 that we want to solve for the implicit Euler step + auto g = [&](const mathlib::VecX_T& x_guess, mathlib::VecX_T& g_out) { g_out = x_guess - x - dt * f(t + dt, x_guess); }; + // Numerical Jacobian for Newton-Raphson + auto J = [&](const mathlib::VecX_T& x_guess, mathlib::MatX_T& J_out) { + int n = (int)x_guess.size(); + mathlib::MatX_T F; + F = automaticDifferenceJacobian( + [&](auto t_pert, const auto& x_pert) { + using Dual = std::decay_t; + Dual t_eval = Dual(t_pert) + Dual(dt); + return f(t_eval, x_pert); + }, + t + dt, + x_guess + ); + + J_out = mathlib::MatX_T::Identity(n, n) - mathlib::real(dt) * F; // J = I - dt * df/dx + }; + + mathlib::VecX_T x0 = x + dt * f(t + dt, x); // Initial guess for Newton-Raphson + return newtonRaphson_AD(g, J, x0, maxIter, tol); + } + + // AD version of Implicit Midpoint method + template + mathlib::VecX_T NumericalIntegrator::implicitMidpoint_AD( + const mathlib::VecX_T& x, + Scalar t, + Scalar dt, + Func&& f, + int maxIter, + Scalar tol + ) { + // The function g(x_guess) = 0 that we want to solve for the implicit midpoint step + auto g = [&](const mathlib::VecX_T& x_guess, mathlib::VecX_T& g_out) { g_out = x_guess - x - dt * f((t + dt) / Scalar(2), (x + x_guess) / Scalar(2)); }; + + // Numerical Jacobian for Newton-Raphson + auto J = [&](const mathlib::VecX_T& x_guess, mathlib::MatX_T& J_out) { + int n = (int)x_guess.size(); + mathlib::MatX_T F; + bool analytical_success = false; + + F = automaticDifferenceJacobian( + [&](auto t_pert, const auto& x_pert) { + using Dual = std::decay_t; + mathlib::VecX_T x_mid = (x.template cast() + x_pert) / Dual(2); + Dual t_mid = (t_pert + dt) / Dual(2); + return f(t_mid, x_mid); + }, + t + dt / Scalar(2), + x_guess + ); + J_out = mathlib::MatX_T::Identity(n, n) - Scalar(0.5) * dt * F; // J = I - dt * df/dx + }; + + mathlib::VecX_T x0 = x + dt * f((t + dt) / Scalar(2), x); // Initial guess for Newton-Raphson + return newtonRaphson_AD(g, J, x0, maxIter, tol); + } + + // AD version of Gauss-Legendre Runge-Kutta method (2 stages, 4th order) + template + mathlib::VecX_T NumericalIntegrator::GLRK2_AD( + const mathlib::VecX_T& x, + Scalar t, + Scalar dt, + Func&& f, + int maxIter, + Scalar tol + ) { + using Real = typename mathlib::DualTraits::BaseScalar; + using std::sqrt; + + const Eigen::Index n = x.size(); + + // Coefficients for the 2-stage Gauss-Legendre method (4th order) + mathlib::VecX_T c(2); + c << + Real(0.5) - sqrt(Real(3)) / Real(6), + Real(0.5) + sqrt(Real(3)) / Real(6); // Stage time fractions + + mathlib::MatX_T A(2, 2); + A << + Real(0.25), Real(0.25) - sqrt(Real(3)) / Real(6), + Real(0.25) + sqrt(Real(3)) / Real(6), Real(0.25); + + const Real b = Real(0.5); // Weights for final update + + // Initial guess for the stage values k1, k2, k3 + mathlib::VecX_T k(2 * n); // 2 stages + mathlib::VecX_T f0 = f(t, x); + k.segment(0, n) = f0; + k.segment(n, n) = f0; + + auto eval_g = [&](const mathlib::VecX_T& k_guess, mathlib::VecX_T& g) { + mathlib::VecX_T k1 = k_guess.segment(0, n); + mathlib::VecX_T k2 = k_guess.segment(n, n); + + mathlib::VecX_T x1 = x + dt * (A(0, 0) * k1 + A(0, 1) * k2); + mathlib::VecX_T x2 = x + dt * (A(1, 0) * k1 + A(1, 1) * k2); + + mathlib::VecX_T f1 = f(t + c(0) * dt, x1); + mathlib::VecX_T f2 = f(t + c(1) * dt, x2); + + g.resize(2 * n); + g.segment(0, n) = k1 - f1; + g.segment(n, n) = k2 - f2; + }; + + auto eval_j = [&](const mathlib::VecX_T& k_guess, mathlib::MatX_T& J) { + mathlib::VecX_T k1 = k_guess.segment(0, n); + mathlib::VecX_T k2 = k_guess.segment(n, n); + mathlib::VecX_T x1 = x + dt * (A(0, 0) * k1 + A(0, 1) * k2); + mathlib::VecX_T x2 = x + dt * (A(1, 0) * k1 + A(1, 1) * k2); + mathlib::MatX_T F1(n, n), F2(n, n); + bool analytical_success = false; + + F1 = automaticDifferenceJacobian( + [&](auto t_pert, const auto& x_pert) { + using Dual = std::decay_t; + Dual t_eval = Dual(t_pert) + Dual(c(0)) * Dual(dt); + return f(t_eval, x_pert); + }, + t + c(0) * dt, + x1 + ); + F2 = automaticDifferenceJacobian( + [&](auto t_pert, const auto& x_pert) { + using Dual = std::decay_t; + Dual t_eval = Dual(t_pert) + Dual(c(1)) * Dual(dt); + return f(t_eval, x_pert); + }, + t + c(1) * dt, + x2 + ); + + J.setZero(2 * n, 2 * n); + J.block(0, 0, n, n) = mathlib::MatX_T::Identity(n, n) - dt * A(0, 0) * F1; + J.block(0, n, n, n) = -dt * A(0, 1) * F1; + J.block(n, 0, n, n) = -dt * A(1, 0) * F2; + J.block(n, n, n, n) = mathlib::MatX_T::Identity(n, n) - dt * A(1, 1) * F2; + }; + + // Solve the nonlinear system for the stage values using Newton-Raphson + k = newtonRaphson_AD(eval_g, eval_j, k, maxIter, tol); + + // Compute the final update for x using the stage values + mathlib::VecX_T k1 = k.segment(0, n); + mathlib::VecX_T k2 = k.segment(n, n); + mathlib::VecX_T x_f = x + dt * (b * k1 + b * k2); + + return x_f; + + } + + // AD version of Gauss-Legendre Runge-Kutta method (3 stages, 6th order) + template + mathlib::VecX_T NumericalIntegrator::GLRK3_AD( + const mathlib::VecX_T& x, + Scalar t, + Scalar dt, + Func&& f, + int maxIter, + Scalar tol + ) { + using Real = typename mathlib::DualTraits::BaseScalar; + using std::sqrt; + using std::pow; + using std::abs; + using std::max; + + if (mathlib::real(tol) < Real(0)) { + Real dt_r = mathlib::real(dt); + Real tol_r = max(Real(1e-12), Real(1e-2) * pow(dt_r, Real(7))); + tol = Real(tol_r); + } + const Eigen::Index n = x.size(); + + // Coefficients for the 3-stage Gauss-Legendre method (6th order) + mathlib::VecX_T c(3); + c << + Real(0.5) - sqrt(Real(15)) / Real(10), + Real(0.5), + Real(0.5) + sqrt(Real(15)) / Real(10); // Stage time fractions + + mathlib::Mat3_T A(3, 3); + A << + Real(5) / Real(36), Real(2) / Real(9) - sqrt(Real(15)) / Real(15), Real(5) / Real(36) - sqrt(Real(15)) / Real(30), + Real(5) / Real(36) + sqrt(Real(15)) / Real(24), Real(2) / Real(9), Real(5) / Real(36) - sqrt(Real(15)) / Real(24), + Real(5) / Real(36) + sqrt(Real(15)) / Real(30), Real(2) / Real(9) + sqrt(Real(15)) / Real(15), Real(5) / Real(36); + + mathlib::VecX_T b(3); + b << Real(5) / Real(18), Real(4) / Real(9), Real(5) / Real(18); // Weights for final update + + // Initial guess for the stage values k1, k2, k3 + mathlib::VecX_T k(3 * n); // 3 stages + mathlib::VecX_T f0 = f(t, x); + k.segment(0, n) = f0; + k.segment(n, n) = f0; + k.segment(2 * n, n) = f0; + + auto eval_g = [&](const mathlib::VecX_T& k_guess, mathlib::VecX_T& g) { + // Extract the stage values k1, k2, k3 from the input guess vector + mathlib::VecX_T k1 = k_guess.segment(0, n); + mathlib::VecX_T k2 = k_guess.segment(n, n); + mathlib::VecX_T k3 = k_guess.segment(2 * n, n); + // Compute the stage points x1, x2, x3 based on the current guess for k1, k2, k3 + mathlib::VecX_T x1 = x + dt * (A(0, 0) * k1 + A(0, 1) * k2 + A(0, 2) * k3); + mathlib::VecX_T x2 = x + dt * (A(1, 0) * k1 + A(1, 1) * k2 + A(1, 2) * k3); + mathlib::VecX_T x3 = x + dt * (A(2, 0) * k1 + A(2, 1) * k2 + A(2, 2) * k3); + // Compute f at the stage points + mathlib::VecX_T f1 = f(t + c(0) * dt, x1); + mathlib::VecX_T f2 = f(t + c(1) * dt, x2); + mathlib::VecX_T f3 = f(t + c(2) * dt, x3); + // Compute the residuals for the nonlinear system + g.resize(3 * n); + g.segment(0, n) = k1 - f1; + g.segment(n, n) = k2 - f2; + g.segment(2 * n, n) = k3 - f3; + }; + + auto eval_j = [&](const mathlib::VecX_T& k_guess, mathlib::MatX_T& J) { + // Extract the stage values k1, k2, k3 from the input guess vector + mathlib::VecX_T k1 = k_guess.segment(0, n); + mathlib::VecX_T k2 = k_guess.segment(n, n); + mathlib::VecX_T k3 = k_guess.segment(2 * n, n); + // Compute the stage points x1, x2, x3 based on the current guess for k1, k2, k3 + mathlib::VecX_T x1 = x + dt * (A(0, 0) * k1 + A(0, 1) * k2 + A(0, 2) * k3); + mathlib::VecX_T x2 = x + dt * (A(1, 0) * k1 + A(1, 1) * k2 + A(1, 2) * k3); + mathlib::VecX_T x3 = x + dt * (A(2, 0) * k1 + A(2, 1) * k2 + A(2, 2) * k3); + + mathlib::MatX_T F1(n, n), F2(n, n), F3(n, n); + // Compute Jacobians of f at the stage points using automatic differentiation + F1 = automaticDifferenceJacobian( + [&](auto t_pert, const auto& x_pert) { + using Dual = std::decay_t; + Dual t_eval = Dual(t_pert) + Dual(c(0)) * Dual(dt); + return f(t_eval, x_pert); + }, + t + c(0) * dt, + x1 + ); + // Compute the Jacobian of f at the second stage using automatic differentiation + F2 = automaticDifferenceJacobian( + [&](auto t_pert, const auto& x_pert) { + using Dual = std::decay_t; + Dual t_eval = Dual(t_pert) + Dual(c(1)) * Dual(dt); + return f(t_eval, x_pert); + }, + t + c(1) * dt, + x2 + ); + // Compute the Jacobian of f at the third stage using automatic differentiation + F3 = automaticDifferenceJacobian( + [&](auto t_pert, const auto& x_pert) { + using Dual = std::decay_t; + Dual t_eval = Dual(t_pert) + Dual(c(2)) * Dual(dt); + return f(t_eval, x_pert); + }, + t + c(2) * dt, + x3 + ); + J.setZero(3 * n, 3 * n); + J.block(0, 0, n, n) = mathlib::MatX_T::Identity(n, n) - dt * A(0, 0) * F1; + J.block(0, n, n, n) = -dt * A(0, 1) * F1; + J.block(0, 2 * n, n, n) = -dt * A(0, 2) * F1; + J.block(n, 0, n, n) = -dt * A(1, 0) * F2; + J.block(n, n, n, n) = mathlib::MatX_T::Identity(n, n) - dt * A(1, 1) * F2; + J.block(n, 2 * n, n, n) = -dt * A(1, 2) * F2; + J.block(2 * n, 0, n, n) = -dt * A(2, 0) * F3; + J.block(2 * n, n, n, n) = -dt * A(2, 1) * F3; + J.block(2 * n, 2 * n, n, n) = mathlib::MatX_T::Identity(n, n) - dt * A(2, 2) * F3; + }; + + // Solve the nonlinear system for the stage values using Newton-Raphson + k = newtonRaphson_AD(eval_g, eval_j, k, maxIter, tol); + mathlib::VecX_T g_check; + eval_g(k, g_check); + + //if (residual > 1e-8) { std::cout << "[GLRK3] Large final residual: " << residual << std::endl; } + + // Compute the final update for x using the stage values + mathlib::VecX_T k1 = k.segment(0, n); + mathlib::VecX_T k2 = k.segment(n, n); + mathlib::VecX_T k3 = k.segment(2 * n, n); + mathlib::VecX_T x_f = x + dt * (b(0) * k1 + b(1) * k2 + b(2) * k3); + + return x_f; + } + + // AD version of Newton-Raphson solver for systems of nonlinear equations g(x) = 0 (DOES NOT USE NEWTON RAPHSON CALL) + template + mathlib::VecX_T NumericalIntegrator::newtonRaphson_AD( + EvalG&& eval_g, + EvalJ&& eval_j, + mathlib::VecX_T x0, + int maxIter, + Scalar tol + ) { + mathlib::VecX_T x = x0, g, x_trial; + using BaseScalar = typename mathlib::DualTraits::BaseScalar; + using Real = typename mathlib::DualTraits::BaseScalar; + mathlib::VecX_T delta = mathlib::VecX_T::Constant(x0.size(), Real(std::numeric_limits::infinity())); + mathlib::MatX_T J; + Eigen::PartialPivLU> solver; + for (int iter = 0; iter < maxIter; ++iter) { + eval_g(x, g); + if (!g.allFinite()) { throw std::runtime_error("Newton received non-finite residual at iter = " + std::to_string(iter) + ", residual norm = " + std::to_string(g.norm())); } + if (g.norm() < tol) { return x; } + eval_j(x, J); + if (!J.allFinite()) { throw std::runtime_error("Newton received non-finite Jacobian at iter = " + std::to_string(iter)); } + solver.compute(J); + delta = solver.solve(-g); + if (!delta.allFinite()) { throw std::runtime_error("Newton produced non-finite step at iter = " + std::to_string(iter)); } + x += delta; + if (delta.norm() < tol * (Scalar(1) + x.norm())) { return x; } + if (!x.allFinite()) { throw std::runtime_error("Newton state became non-finite at iter = " + std::to_string(iter)); } + } + throw std::runtime_error("Newton-Raphson failed to converge after " + std::to_string(maxIter) + " iterations. Final residual norm = " + std::to_string(g.norm()) + ", final step norm = " + std::to_string(delta.norm())); + } + + // Automatic Difference Jacobian + template + mathlib::MatX_T::BaseScalar> NumericalIntegrator::automaticDifferenceJacobian( + Func&& f, + Scalar t, + const mathlib::VecX_T& x + ) { + using RealScalar = typename mathlib::DualTraits::BaseScalar; + + const int n = static_cast(x.size()); + auto f0 = f(t, x); + const int m = static_cast(f0.size()); + + mathlib::MatX_T J(m, n); + for (int i = 0; i < n; ++i) { + using Dual_T = mathlib::DualNumber_T; + + mathlib::VecX_T x_dual(n); + for (int k = 0; k < n; ++k) { + x_dual(k) = Dual_T( + mathlib::real(x(k)), + std::array{ + (k == i) ? RealScalar(1) : RealScalar(0) + } + ); + } + + Dual_T t_dual(mathlib::real(t)); + auto f_dual = f(t_dual, x_dual); + for (int j = 0; j < m; ++j) { + J(j, i) = mathlib::dualPart(f_dual(j)); + } + } + return J; + } +} \ No newline at end of file diff --git a/PxMLib/MathLib/include/integrators/explicit_integrators.inl b/PxMLib/MathLib/include/integrators/explicit_integrators.inl new file mode 100644 index 00000000..f3eac1d3 --- /dev/null +++ b/PxMLib/MathLib/include/integrators/explicit_integrators.inl @@ -0,0 +1,187 @@ +// PxM/MathLib explicit_integrators.inl +#pragma once + +namespace integration { + // Euler method + template + mathlib::VecX_T NumericalIntegrator::eulerStep( + const mathlib::VecX_T& x, + Scalar t, + Scalar dt, + Func&& f + ) { + return x + dt * f(t, x); + } + + // Second-order Runge-Kutta method (Midpoint) + template + mathlib::VecX_T NumericalIntegrator::midpointStep( + const mathlib::VecX_T& x, + Scalar t, + Scalar dt, + Func&& f + ) { + mathlib::VecX_T k1 = dt * f(t, x); + mathlib::VecX_T k2 = dt * f(t + dt / Scalar(2), x + k1 / Scalar(2)); + return x + k2; + } + + // Second-order Runge-Kutta method (Heun) + template + mathlib::VecX_T NumericalIntegrator::heunStep( + const mathlib::VecX_T& x, + Scalar t, + Scalar dt, + Func&& f + ) { + mathlib::VecX_T k1 = dt * f(t, x); + mathlib::VecX_T k2 = dt * f(t + dt, x + k1); + return x + (k1 + k2) / Scalar(2); + } + + // Second-order Runge-Kutta method (Ralston) + template + mathlib::VecX_T NumericalIntegrator::ralstonStep( + const mathlib::VecX_T& x, + Scalar t, + Scalar dt, + Func&& f + ) { + mathlib::VecX_T k1 = dt * f(t, x); + mathlib::VecX_T k2 = dt * f(t + (Scalar(2) / Scalar(3)) * dt, x + (Scalar(2) / Scalar(3)) * k1); + return x + (k1 + Scalar(3) * k2) / Scalar(4); + } + + // Fourth-order Runge-Kutta method + template + mathlib::VecX_T NumericalIntegrator::rk4Step( + const mathlib::VecX_T& x, + Scalar t, + Scalar dt, + Func&& f + ) { + mathlib::VecX_T k1 = dt * f(t, x); + mathlib::VecX_T k2 = dt * f(t + dt / Scalar(2), x + k1 / Scalar(2)); + mathlib::VecX_T k3 = dt * f(t + dt / Scalar(2), x + k2 / Scalar(2)); + mathlib::VecX_T k4 = dt * f(t + dt, x + k3); + return x + (k1 + Scalar(2) * k2 + Scalar(2) * k3 + k4) / Scalar(6); + } + + // RK45 method with adaptive step size (Dormand-Prince) + template + mathlib::VecX_T NumericalIntegrator::rk45Step( + const mathlib::VecX_T& x, + Scalar t, + Scalar& dt, + Scalar& dt_used, + Func&& f, + Scalar rtol, + Scalar atol + ) { + const Scalar safety = 0.9; // safety factor to prevent aggressive step size changes + const Scalar fac_min = 0.2; // minimum factor for reducing step size + const Scalar fac_max = 5.0; // maximum factor for increasing step size + const Scalar h_min = 1e-10; // minimum allowed step size + const Scalar h_max = 1.0; // maximum allowed step size + + dt = std::clamp(dt, h_min, h_max); + + // Try up to 25 attempts to find an acceptable step size + for (int attempt = 0; attempt < 25; ++attempt) { + // Butcher tableau for Dormand-Prince method (7 stages, 5th order, SHOULD probably be precomputed as static constants, in a matrix or equiv) + + // Coefficients for error estimation + const Scalar c2 = 1.0 / 5.0; + const Scalar c3 = 3.0 / 10.0; + const Scalar c4 = 4.0 / 5.0; + const Scalar c5 = 8.0 / 9.0; + const Scalar c6 = 1.0; + const Scalar c7 = 1.0; + + // Dormand-Prince coefficients + const Scalar a21 = 1.0 / 5.0; + const Scalar a31 = 3.0 / 40.0; + const Scalar a32 = 9.0 / 40.0; + const Scalar a41 = 44.0 / 45.0; + const Scalar a42 = -56.0 / 15.0; + const Scalar a43 = 32.0 / 9.0; + const Scalar a51 = 19372.0 / 6561.0; + const Scalar a52 = -25360.0 / 2187.0; + const Scalar a53 = 64448.0 / 6561.0; + const Scalar a54 = -212.0 / 729.0; + const Scalar a61 = 9017.0 / 3168.0; + const Scalar a62 = -355.0 / 33.0; + const Scalar a63 = 46732.0 / 5247.0; + const Scalar a64 = 49.0 / 176.0; + const Scalar a65 = -5103.0 / 18656.0; + const Scalar a71 = 35.0 / 384.0; + const Scalar a72 = 0.0; + const Scalar a73 = 500.0 / 1113.0; + const Scalar a74 = 125.0 / 192.0; + const Scalar a75 = -2187.0 / 6784.0; + const Scalar a76 = 11.0 / 84.0; + + // Weights for 4th and 5th order estimates + const Scalar b1 = 35.0 / 384.0; + const Scalar b2 = 0.0; + const Scalar b3 = 500.0 / 1113.0; + const Scalar b4 = 125.0 / 192.0; + const Scalar b5 = -2187.0 / 6784.0; + const Scalar b6 = 11.0 / 84.0; + const Scalar b1s = 5179.0 / 57600.0; + const Scalar b2s = 0.0; + const Scalar b3s = 7571.0 / 16695.0; + const Scalar b4s = 393.0 / 640.0; + const Scalar b5s = -92097.0 / 339200.0; + const Scalar b6s = 187.0 / 2100.0; + const Scalar b7s = 1.0 / 40.0; + + // Compute the Runge-Kutta stages (DP -> 7 stages) + const mathlib::VecX_T k1 = f(t, x); + const mathlib::VecX_T k2 = f(t + c2 * dt, x + dt * (a21 * k1)); + const mathlib::VecX_T k3 = f(t + c3 * dt, x + dt * (a31 * k1 + a32 * k2)); + const mathlib::VecX_T k4 = f(t + c4 * dt, x + dt * (a41 * k1 + a42 * k2 + a43 * k3)); + const mathlib::VecX_T k5 = f(t + c5 * dt, x + dt * (a51 * k1 + a52 * k2 + a53 * k3 + a54 * k4)); + const mathlib::VecX_T k6 = f(t + c6 * dt, x + dt * (a61 * k1 + a62 * k2 + a63 * k3 + a64 * k4 + a65 * k5)); + const mathlib::VecX_T k7 = f(t + c7 * dt, x + dt * (a71 * k1 + a72 * k2 + a73 * k3 + a74 * k4 + a75 * k5 + a76 * k6)); + + // Compute 4th and 5th order estimates + const mathlib::VecX_T y5 = x + dt * ((b1 * k1) + (b3 * k3) + (b4 * k4) + (b5 * k5) + (b6 * k6)); // 5th order estimate + const mathlib::VecX_T y4 = x + dt * ((b1s * k1) + (b3s * k3) + (b4s * k4) + (b5s * k5) + (b6s * k6) + (b7s * k7)); // 4th order estimate + + const mathlib::VecX_T e = y5 - y4; // Error estimate + + // Compute the error norm + Scalar errNorm = 0.0; + for (int i = 0; i < e.size(); ++i) { + Scalar sc = atol + rtol * std::max(std::abs(x(i)), std::abs(y5(i))); + const Scalar r = e(i) / sc; + errNorm += r * r; + } + Scalar err = std::sqrt(errNorm / e.size()); + + // Adaptive step size control + + // Accept + if (err <= 1.0 && std::isfinite(err)) { + dt_used = dt; // Store the actual step size used for this step + + // Update step size for next iteration + const Scalar denom = std::max(err, 1e-10); // prevent division by zero + Scalar fac = safety * std::pow(denom, -0.2); // exponent for 5th order method + fac = std::clamp(fac, fac_min, fac_max); // limit step size change + dt = std::clamp(dt * fac, h_min, h_max); // update step size + return y5; + } + // Reject + else { + Scalar denom = (std::isfinite(err) + ? std::max(err, 1e-16) : 1e16); // prevent division by zero & NaN + Scalar fac = safety * std::pow(denom, -0.2); // exponent for 4th order method + fac = std::clamp(fac, fac_min, fac_max); + dt = std::clamp(dt * fac, h_min, h_max); + } + } + throw std::runtime_error("RK45 failed to converge after maximum attempts"); + } +} \ No newline at end of file diff --git a/PxMLib/MathLib/include/integrators/implicit_integrators.inl b/PxMLib/MathLib/include/integrators/implicit_integrators.inl new file mode 100644 index 00000000..160e4a5e --- /dev/null +++ b/PxMLib/MathLib/include/integrators/implicit_integrators.inl @@ -0,0 +1,424 @@ +// PxM/MathLib implicit_integrators.inl +#pragma once + +namespace integration { + // Implicit Euler method + template + mathlib::VecX_T NumericalIntegrator::implicitEuler( + const mathlib::VecX_T& x, + Scalar t, + Scalar dt, + Func&& f, + JacFunc&& jac, + int maxIter, + Scalar tol + ) { + // The function g(x_guess) = 0 that we want to solve for the implicit Euler step + auto g = [&](const mathlib::VecX_T& x_guess, mathlib::VecX_T& g_out) { g_out = x_guess - x - dt * f(t + dt, x_guess); }; + // Numerical Jacobian for Newton-Raphson + auto J = [&](const mathlib::VecX_T& x_guess, mathlib::MatX_T& J_out) { + int n = (int)x_guess.size(); + + mathlib::MatX_T F; + bool analytical_success = false; + + if constexpr (!std::is_same_v, std::nullptr_t>) { + if constexpr (std::is_pointer_v> || requires { bool(jac); }) { + if (jac) { + jac(x_guess, F); // User-provided Jacobian + analytical_success = true; + } + } + else { + jac(x_guess, F); // User-provided Jacobian + analytical_success = true; + } + } + if (!analytical_success) { + F = finiteDifferenceJacobian( + [&](Scalar, const mathlib::VecX_T& x_pert) { + return f(t + dt, x_pert); + }, + t + dt, + x_guess + ); + } + + J_out = mathlib::MatX_T::Identity(n, n) - dt * F; // J = I - dt * df/dx + }; + + mathlib::VecX_T x0 = x + dt * f(t + dt, x); // Initial guess for Newton-Raphson + return newtonRaphson(g, J, x0, maxIter, tol); + } + + // Implicit Midpoint method + template + mathlib::VecX_T NumericalIntegrator::implicitMidpoint( + const mathlib::VecX_T& x, + Scalar t, + Scalar dt, + Func&& f, + JacFunc&& jac, + int maxIter, + Scalar tol + ) { + // The function g(x_guess) = 0 that we want to solve for the implicit midpoint step + auto g = [&](const mathlib::VecX_T& x_guess, mathlib::VecX_T& g_out) { g_out = x_guess - x - dt * f((t + dt) / Scalar(2), (x + x_guess) / Scalar(2)); }; + + // Numerical Jacobian for Newton-Raphson + auto J = [&](const mathlib::VecX_T& x_guess, mathlib::MatX_T& J_out) { + int n = (int)x_guess.size(); + mathlib::MatX_T F; + bool analytical_success = false; + + if constexpr (!std::is_same_v, std::nullptr_t>) { + if constexpr (std::is_pointer_v> || requires { bool(jac); }) { + if (jac) { + jac((x + x_guess) / Scalar(2), F); // User-provided Jacobian + analytical_success = true; + } + } + else { + jac((x + x_guess) / Scalar(2), F); // User-provided Jacobian + analytical_success = true; + } + } + if (!analytical_success) { + F = finiteDifferenceJacobian( + [&](Scalar, const mathlib::VecX_T& x_pert) { + return f((t + dt) / Scalar(2), (x + x_pert) / Scalar(2)); + }, + t + dt / Scalar(2), + x_guess + ); + } + J_out = mathlib::MatX_T::Identity(n, n) - Scalar(0.5) * dt * F; // J = I - dt * df/dx + }; + + mathlib::VecX_T x0 = x + dt * f((t + dt) / Scalar(2), x); // Initial guess for Newton-Raphson + return newtonRaphson(g, J, x0, maxIter, tol); + } + + // Gauss-Legendre Runge-Kutta method (2 stages, 4th order) + template + mathlib::VecX_T NumericalIntegrator::GLRK2( + const mathlib::VecX_T& x, + Scalar t, + Scalar dt, + Func&& f, + JacFunc&& jac, + int maxIter, + Scalar tol + ) { + using std::sqrt; + + const Eigen::Index n = x.size(); + + // Coefficients for the 2-stage Gauss-Legendre method (4th order) + mathlib::VecX_T c(2); + c << + Scalar(0.5) - sqrt(Scalar(3)) / Scalar(6), + Scalar(0.5) + sqrt(Scalar(3)) / Scalar(6); // Stage time fractions + + mathlib::MatX_T A(2, 2); + A << + Scalar(0.25), Scalar(0.25) - sqrt(Scalar(3)) / Scalar(6), + Scalar(0.25) + sqrt(Scalar(3)) / Scalar(6), Scalar(0.25); + + const Scalar b = Scalar(0.5); // Weights for final update + + // Initial guess for the stage values k1, k2 + mathlib::VecX_T k(2 * n); // 2 stages + mathlib::VecX_T f0 = f(t, x); + k.segment(0, n) = f0; + k.segment(n, n) = f0; + + auto eval_g = [&](const mathlib::VecX_T& k_guess, mathlib::VecX_T& g) { + mathlib::VecX_T k1 = k_guess.segment(0, n); + mathlib::VecX_T k2 = k_guess.segment(n, n); + + mathlib::VecX_T x1 = x + dt * (A(0, 0) * k1 + A(0, 1) * k2); + mathlib::VecX_T x2 = x + dt * (A(1, 0) * k1 + A(1, 1) * k2); + + mathlib::VecX_T f1 = f(t + c(0) * dt, x1); + mathlib::VecX_T f2 = f(t + c(1) * dt, x2); + + g.resize(2 * n); + g.segment(0, n) = k1 - f1; + g.segment(n, n) = k2 - f2; + }; + + auto eval_j = [&](const mathlib::VecX_T& k_guess, mathlib::MatX_T& J) { + mathlib::VecX_T k1 = k_guess.segment(0, n); + mathlib::VecX_T k2 = k_guess.segment(n, n); + mathlib::VecX_T x1 = x + dt * (A(0, 0) * k1 + A(0, 1) * k2); + mathlib::VecX_T x2 = x + dt * (A(1, 0) * k1 + A(1, 1) * k2); + mathlib::MatX_T F1(n, n), F2(n, n); + bool analytical_success = false; + + // Attempt to use the provided Jacobian function if it's not a nullptr and is callable + if constexpr (!std::is_same_v, std::nullptr_t>) { + if constexpr (std::is_pointer_v> || requires { bool(jac); }) { + if (jac) { + jac(x1, F1); + jac(x2, F2); + analytical_success = true; + } + } + else { + // Pure lambda type execution path + jac(x1, F1); + jac(x2, F2); + analytical_success = true; + } + } + + if (!analytical_success) { + F1 = finiteDifferenceJacobian( + [&](Scalar, const mathlib::VecX_T& x_pert) { + return f(t + c(0) * dt, x_pert); + }, + t + c(0) * dt, + x1 + ); + F2 = finiteDifferenceJacobian( + [&](Scalar, const mathlib::VecX_T& x_pert) { + return f(t + c(1) * dt, x_pert); + }, + t + c(1) * dt, + x2 + ); + } + + J.setZero(2 * n, 2 * n); + J.block(0, 0, n, n) = mathlib::MatX_T::Identity(n, n) - dt * A(0, 0) * F1; + J.block(0, n, n, n) = -dt * A(0, 1) * F1; + J.block(n, 0, n, n) = -dt * A(1, 0) * F2; + J.block(n, n, n, n) = mathlib::MatX_T::Identity(n, n) - dt * A(1, 1) * F2; + }; + + // Solve the nonlinear system for the stage values using Newton-Raphson + k = newtonRaphson(eval_g, eval_j, k, maxIter, tol); + + // Compute the final update for x using the stage values + mathlib::VecX_T k1 = k.segment(0, n); + mathlib::VecX_T k2 = k.segment(n, n); + mathlib::VecX_T x_f = x + dt * (b * k1 + b * k2); + + // Precautionary check for divergence (NaN or Inf) + if (!x_f.allFinite()) { throw std::runtime_error("GLRK2 diverged"); } + + return x_f; + } + + // Gauss-Legendre Runge-Kutta method (3 stages, 6th order) + template + mathlib::VecX_T NumericalIntegrator::GLRK3( + const mathlib::VecX_T& x, + Scalar t, + Scalar dt, + Func&& f, + JacFunc&& jac, + int maxIter, + Scalar tol + ) { + using Real = typename mathlib::DualTraits::BaseScalar; + using std::sqrt; + using std::pow; + using std::abs; + using std::max; + + if (mathlib::real(tol) < Real(0)) { + Real dt_r = mathlib::real(dt); + Real tol_r = max(Real(1e-15), Real(1e-2) * pow(dt_r, Real(7))); + tol = Scalar(tol_r); + } + const Eigen::Index n = x.size(); + + // Coefficients for the 3-stage Gauss-Legendre method (6th order) + mathlib::VecX_T c(3); + c << + Scalar(0.5) - sqrt(Scalar(15)) / Scalar(10), + Scalar(0.5), + Scalar(0.5) + sqrt(Scalar(15)) / Scalar(10); // Stage time fractions + + mathlib::Mat3_T A(3, 3); + A << + Scalar(5) / Scalar(36), Scalar(2) / Scalar(9) - sqrt(Scalar(15)) / Scalar(15), Scalar(5) / Scalar(36) - sqrt(Scalar(15)) / Scalar(30), + Scalar(5) / Scalar(36) + sqrt(Scalar(15)) / Scalar(24), Scalar(2) / Scalar(9), Scalar(5) / Scalar(36) - sqrt(Scalar(15)) / Scalar(24), + Scalar(5) / Scalar(36) + sqrt(Scalar(15)) / Scalar(30), Scalar(2) / Scalar(9) + sqrt(Scalar(15)) / Scalar(15), Scalar(5) / Scalar(36); + + mathlib::VecX_T b(3); + b << Scalar(5) / Scalar(18), Scalar(4) / Scalar(9), Scalar(5) / Scalar(18); // Weights for final update + + // Initial guess for the stage values k1, k2, k3 + mathlib::VecX_T k(3 * n); // 3 stages + mathlib::VecX_T f0 = f(t, x); + k.segment(0, n) = f0; + k.segment(n, n) = f0; + k.segment(2 * n, n) = f0; + + auto eval_g = [&](const mathlib::VecX_T& k_guess, mathlib::VecX_T& g) { + mathlib::VecX_T k1 = k_guess.segment(0, n); + mathlib::VecX_T k2 = k_guess.segment(n, n); + mathlib::VecX_T k3 = k_guess.segment(2 * n, n); + + mathlib::VecX_T x1 = x + dt * (A(0, 0) * k1 + A(0, 1) * k2 + A(0, 2) * k3); + mathlib::VecX_T x2 = x + dt * (A(1, 0) * k1 + A(1, 1) * k2 + A(1, 2) * k3); + mathlib::VecX_T x3 = x + dt * (A(2, 0) * k1 + A(2, 1) * k2 + A(2, 2) * k3); + + mathlib::VecX_T f1 = f(t + c(0) * dt, x1); + mathlib::VecX_T f2 = f(t + c(1) * dt, x2); + mathlib::VecX_T f3 = f(t + c(2) * dt, x3); + + g.resize(3 * n); + + g.segment(0, n) = k1 - f1; + g.segment(n, n) = k2 - f2; + g.segment(2 * n, n) = k3 - f3; + }; + + auto eval_j = [&](const mathlib::VecX_T& k_guess, mathlib::MatX_T& J) { + mathlib::VecX_T k1 = k_guess.segment(0, n); + mathlib::VecX_T k2 = k_guess.segment(n, n); + mathlib::VecX_T k3 = k_guess.segment(2 * n, n); + + mathlib::VecX_T x1 = x + dt * (A(0, 0) * k1 + A(0, 1) * k2 + A(0, 2) * k3); + mathlib::VecX_T x2 = x + dt * (A(1, 0) * k1 + A(1, 1) * k2 + A(1, 2) * k3); + mathlib::VecX_T x3 = x + dt * (A(2, 0) * k1 + A(2, 1) * k2 + A(2, 2) * k3); + + mathlib::MatX_T F1(n, n), F2(n, n), F3(n, n); + bool analytical_success = false; + + // Attempt to use the provided Jacobian function if it's not a nullptr and is callable + if constexpr (!std::is_same_v, std::nullptr_t>) { + if constexpr (std::is_pointer_v> || requires { bool(jac); }) { + if (jac) { + jac(x1, F1); + jac(x2, F2); + jac(x3, F3); + analytical_success = true; + } + } + else { + // Pure lambda type execution path + jac(x1, F1); + jac(x2, F2); + jac(x3, F3); + analytical_success = true; + } + } + + if (!analytical_success) { + F1 = finiteDifferenceJacobian( + [&](Scalar, const mathlib::VecX_T& x_pert) { + return f(t + c(0) * dt, x_pert); + }, + t + c(0) * dt, + x1 + ); + + F2 = finiteDifferenceJacobian( + [&](Scalar, const mathlib::VecX_T& x_pert) { + return f(t + c(1) * dt, x_pert); + }, + t + c(1) * dt, + x2 + ); + + F3 = finiteDifferenceJacobian( + [&](Scalar, const mathlib::VecX_T& x_pert) { + return f(t + c(2) * dt, x_pert); + }, + t + c(2) * dt, + x3 + ); + } + + J.setZero(3 * n, 3 * n); + J.block(0, 0, n, n) = mathlib::MatX_T::Identity(n, n) - dt * A(0, 0) * F1; + J.block(0, n, n, n) = -dt * A(0, 1) * F1; + J.block(0, 2 * n, n, n) = -dt * A(0, 2) * F1; + J.block(n, 0, n, n) = -dt * A(1, 0) * F2; + J.block(n, n, n, n) = mathlib::MatX_T::Identity(n, n) - dt * A(1, 1) * F2; + J.block(n, 2 * n, n, n) = -dt * A(1, 2) * F2; + J.block(2 * n, 0, n, n) = -dt * A(2, 0) * F3; + J.block(2 * n, n, n, n) = -dt * A(2, 1) * F3; + J.block(2 * n, 2 * n, n, n) = mathlib::MatX_T::Identity(n, n) - dt * A(2, 2) * F3; + }; + + // Solve the nonlinear system for the stage values using Newton-Raphson + k = newtonRaphson(eval_g, eval_j, k, maxIter, tol); + + mathlib::VecX_T g_check; + eval_g(k, g_check); + + auto residual = mathlib::real(g_check.norm()); + //if (residual > 1e-8) { std::cout << "[GLRK3] Large final residual: " << residual << std::endl; } + + // Compute the final update for x using the stage values + mathlib::VecX_T k1 = k.segment(0, n); + mathlib::VecX_T k2 = k.segment(n, n); + mathlib::VecX_T k3 = k.segment(2 * n, n); + mathlib::VecX_T x_f = x + dt * (b(0) * k1 + b(1) * k2 + b(2) * k3); + + // Precautionary check for divergence (NaN or Inf) + if (!x_f.allFinite()) { throw std::runtime_error("GLRK3 diverged"); } + + return x_f; + } + + // Newton-Raphson solver for systems of nonlinear equations g(x) = 0 + template + mathlib::VecX_T NumericalIntegrator::newtonRaphson( + EvalG&& eval_g, + EvalJ&& eval_j, + mathlib::VecX_T x0, + int maxIter, + Scalar tol + ) { + mathlib::VecX_T x = x0, g, x_trial; + mathlib::VecX_T delta = mathlib::VecX_T::Constant(x0.size(), std::numeric_limits::infinity()); + mathlib::MatX_T J; + Eigen::FullPivLU> solver; + for (int iter = 0; iter < maxIter; ++iter) { + eval_g(x, g); + if (!g.allFinite()) { throw std::runtime_error("Newton received non-finite residual at iter = " + std::to_string(iter) + ", residual norm = " + std::to_string(g.norm())); } + if (g.norm() < tol) { return x; } + eval_j(x, J); + if (!J.allFinite()) { throw std::runtime_error("Newton received non-finite Jacobian at iter = " + std::to_string(iter)); } + solver.compute(J); + delta = solver.solve(-g); + if (!delta.allFinite()) { throw std::runtime_error("Newton produced non-finite step at iter = " + std::to_string(iter)); } + x += delta; + if (delta.norm() < tol * (Scalar(1) + x.norm())) { return x; } + if (!x.allFinite()) { throw std::runtime_error("Newton state became non-finite at iter = " + std::to_string(iter)); } + } + throw std::runtime_error("Newton-Raphson failed to converge after " + std::to_string(maxIter) + " iterations. Final residual norm = " + std::to_string(g.norm()) + ", final step norm = " + std::to_string(delta.norm())); + } + + // Finite difference approximation of the Jacobian matrix df/dx for a vector-valued function f: R^n -> R^m at a point x + template + mathlib::MatX_T NumericalIntegrator::finiteDifferenceJacobian( + Func&& f, + Scalar t, + const mathlib::VecX_T& x + ) { + const Scalar eps_rel = Scalar(1e-8); + const int n = static_cast(x.size()); + mathlib::VecX_T f_0 = f(t, x); + mathlib::MatX_T J(f_0.size(), n); + // Compute the Jacobian column by column using central differences + for (int i = 0; i < n; ++i) { + mathlib::VecX_T x_fwd = x; + mathlib::VecX_T x_bwd = x; + Scalar h = eps_rel * std::max(Scalar(1), Scalar(std::abs(mathlib::real(x(i))))); + x_fwd(i) += h; + x_bwd(i) -= h; + mathlib::VecX_T f_fwd = f(t, x_fwd); + mathlib::VecX_T f_bwd = f(t, x_bwd); + J.col(i) = (f_fwd - f_bwd) / (Scalar(2) * h); + } + return J; + } +} \ No newline at end of file diff --git a/PxMLib/MathLib/include/integrators/numerical_integrators.h b/PxMLib/MathLib/include/integrators/numerical_integrators.h index e0331c54..7ba54139 100644 --- a/PxMLib/MathLib/include/integrators/numerical_integrators.h +++ b/PxMLib/MathLib/include/integrators/numerical_integrators.h @@ -198,4 +198,6 @@ namespace integration { }; }// namespace integration -#include "numerical_integrators.inl" \ No newline at end of file +#include "explicit_integrators.inl" +#include "implicit_integrators.inl" +#include "ad_implicit_integrators.inl" \ No newline at end of file diff --git a/PxMLib/MathLib/include/integrators/numerical_integrators.inl b/PxMLib/MathLib/include/integrators/numerical_integrators.inl deleted file mode 100644 index 238b01e7..00000000 --- a/PxMLib/MathLib/include/integrators/numerical_integrators.inl +++ /dev/null @@ -1,1031 +0,0 @@ -// PxM/MathLib numerical_integrators.inl -#pragma once - -namespace integration { - // Euler method - template - mathlib::VecX_T NumericalIntegrator::eulerStep( - const mathlib::VecX_T& x, - Scalar t, - Scalar dt, - Func&& f - ) { - return x + dt * f(t, x); - } - - // Second-order Runge-Kutta method (Midpoint) - template - mathlib::VecX_T NumericalIntegrator::midpointStep( - const mathlib::VecX_T& x, - Scalar t, - Scalar dt, - Func&& f - ) { - mathlib::VecX_T k1 = dt * f(t, x); - mathlib::VecX_T k2 = dt * f(t + dt / Scalar(2), x + k1 / Scalar(2)); - return x + k2; - } - - // Second-order Runge-Kutta method (Heun) - template - mathlib::VecX_T NumericalIntegrator::heunStep( - const mathlib::VecX_T& x, - Scalar t, - Scalar dt, - Func&& f - ) { - mathlib::VecX_T k1 = dt * f(t, x); - mathlib::VecX_T k2 = dt * f(t + dt, x + k1); - return x + (k1 + k2) / Scalar(2); - } - - // Second-order Runge-Kutta method (Ralston) - template - mathlib::VecX_T NumericalIntegrator::ralstonStep( - const mathlib::VecX_T& x, - Scalar t, - Scalar dt, - Func&& f - ) { - mathlib::VecX_T k1 = dt * f(t, x); - mathlib::VecX_T k2 = dt * f(t + (Scalar(2) / Scalar(3)) * dt, x + (Scalar(2) / Scalar(3)) * k1); - return x + (k1 + Scalar(3) * k2) / Scalar(4); - } - - // Fourth-order Runge-Kutta method - template - mathlib::VecX_T NumericalIntegrator::rk4Step( - const mathlib::VecX_T& x, - Scalar t, - Scalar dt, - Func&& f - ) { - mathlib::VecX_T k1 = dt * f(t, x); - mathlib::VecX_T k2 = dt * f(t + dt / Scalar(2), x + k1 / Scalar(2)); - mathlib::VecX_T k3 = dt * f(t + dt / Scalar(2), x + k2 / Scalar(2)); - mathlib::VecX_T k4 = dt * f(t + dt, x + k3); - return x + (k1 + Scalar(2) * k2 + Scalar(2) * k3 + k4) / Scalar(6); - } - - // RK45 method with adaptive step size (Dormand-Prince) - template - mathlib::VecX_T NumericalIntegrator::rk45Step( - const mathlib::VecX_T& x, - Scalar t, - Scalar& dt, - Scalar& dt_used, - Func&& f, - Scalar rtol, - Scalar atol - ) { - const Scalar safety = 0.9; // safety factor to prevent aggressive step size changes - const Scalar fac_min = 0.2; // minimum factor for reducing step size - const Scalar fac_max = 5.0; // maximum factor for increasing step size - const Scalar h_min = 1e-10; // minimum allowed step size - const Scalar h_max = 1.0; // maximum allowed step size - - dt = std::clamp(dt, h_min, h_max); - - // Try up to 25 attempts to find an acceptable step size - for (int attempt = 0; attempt < 25; ++attempt) { - // Butcher tableau for Dormand-Prince method (7 stages, 5th order, SHOULD probably be precomputed as static constants, in a matrix or equiv) - - // Coefficients for error estimation - const Scalar c2 = 1.0 / 5.0; - const Scalar c3 = 3.0 / 10.0; - const Scalar c4 = 4.0 / 5.0; - const Scalar c5 = 8.0 / 9.0; - const Scalar c6 = 1.0; - const Scalar c7 = 1.0; - - // Dormand-Prince coefficients - const Scalar a21 = 1.0 / 5.0; - const Scalar a31 = 3.0 / 40.0; - const Scalar a32 = 9.0 / 40.0; - const Scalar a41 = 44.0 / 45.0; - const Scalar a42 = -56.0 / 15.0; - const Scalar a43 = 32.0 / 9.0; - const Scalar a51 = 19372.0 / 6561.0; - const Scalar a52 = -25360.0 / 2187.0; - const Scalar a53 = 64448.0 / 6561.0; - const Scalar a54 = -212.0 / 729.0; - const Scalar a61 = 9017.0 / 3168.0; - const Scalar a62 = -355.0 / 33.0; - const Scalar a63 = 46732.0 / 5247.0; - const Scalar a64 = 49.0 / 176.0; - const Scalar a65 = -5103.0 / 18656.0; - const Scalar a71 = 35.0 / 384.0; - const Scalar a72 = 0.0; - const Scalar a73 = 500.0 / 1113.0; - const Scalar a74 = 125.0 / 192.0; - const Scalar a75 = -2187.0 / 6784.0; - const Scalar a76 = 11.0 / 84.0; - - // Weights for 4th and 5th order estimates - const Scalar b1 = 35.0 / 384.0; - const Scalar b2 = 0.0; - const Scalar b3 = 500.0 / 1113.0; - const Scalar b4 = 125.0 / 192.0; - const Scalar b5 = -2187.0 / 6784.0; - const Scalar b6 = 11.0 / 84.0; - const Scalar b1s = 5179.0 / 57600.0; - const Scalar b2s = 0.0; - const Scalar b3s = 7571.0 / 16695.0; - const Scalar b4s = 393.0 / 640.0; - const Scalar b5s = -92097.0 / 339200.0; - const Scalar b6s = 187.0 / 2100.0; - const Scalar b7s = 1.0 / 40.0; - - // Compute the Runge-Kutta stages (DP -> 7 stages) - const mathlib::VecX_T k1 = f(t, x); - const mathlib::VecX_T k2 = f(t + c2 * dt, x + dt * (a21 * k1)); - const mathlib::VecX_T k3 = f(t + c3 * dt, x + dt * (a31 * k1 + a32 * k2)); - const mathlib::VecX_T k4 = f(t + c4 * dt, x + dt * (a41 * k1 + a42 * k2 + a43 * k3)); - const mathlib::VecX_T k5 = f(t + c5 * dt, x + dt * (a51 * k1 + a52 * k2 + a53 * k3 + a54 * k4)); - const mathlib::VecX_T k6 = f(t + c6 * dt, x + dt * (a61 * k1 + a62 * k2 + a63 * k3 + a64 * k4 + a65 * k5)); - const mathlib::VecX_T k7 = f(t + c7 * dt, x + dt * (a71 * k1 + a72 * k2 + a73 * k3 + a74 * k4 + a75 * k5 + a76 * k6)); - - // Compute 4th and 5th order estimates - const mathlib::VecX_T y5 = x + dt * ((b1 * k1) + (b3 * k3) + (b4 * k4) + (b5 * k5) + (b6 * k6)); // 5th order estimate - const mathlib::VecX_T y4 = x + dt * ((b1s * k1) + (b3s * k3) + (b4s * k4) + (b5s * k5) + (b6s * k6) + (b7s * k7)); // 4th order estimate - - const mathlib::VecX_T e = y5 - y4; // Error estimate - - // Compute the error norm - Scalar errNorm = 0.0; - for (int i = 0; i < e.size(); ++i) { - Scalar sc = atol + rtol * std::max(std::abs(x(i)), std::abs(y5(i))); - const Scalar r = e(i) / sc; - errNorm += r * r; - } - Scalar err = std::sqrt(errNorm / e.size()); - - // Adaptive step size control - - // Accept - if (err <= 1.0 && std::isfinite(err)) { - dt_used = dt; // Store the actual step size used for this step - - // Update step size for next iteration - const Scalar denom = std::max(err, 1e-10); // prevent division by zero - Scalar fac = safety * std::pow(denom, -0.2); // exponent for 5th order method - fac = std::clamp(fac, fac_min, fac_max); // limit step size change - dt = std::clamp(dt * fac, h_min, h_max); // update step size - return y5; - } - // Reject - else { - Scalar denom = (std::isfinite(err) - ? std::max(err, 1e-16) : 1e16); // prevent division by zero & NaN - Scalar fac = safety * std::pow(denom, -0.2); // exponent for 4th order method - fac = std::clamp(fac, fac_min, fac_max); - dt = std::clamp(dt * fac, h_min, h_max); - } - } - throw std::runtime_error("RK45 failed to converge after maximum attempts"); - } - - // Implicit Euler method - template - mathlib::VecX_T NumericalIntegrator::implicitEuler( - const mathlib::VecX_T& x, - Scalar t, - Scalar dt, - Func&& f, - JacFunc&& jac, - int maxIter, - Scalar tol - ) { - // The function g(x_guess) = 0 that we want to solve for the implicit Euler step - auto g = [&](const mathlib::VecX_T& x_guess, mathlib::VecX_T& g_out) { g_out = x_guess - x - dt * f(t + dt, x_guess); }; - // Numerical Jacobian for Newton-Raphson - auto J = [&](const mathlib::VecX_T& x_guess, mathlib::MatX_T& J_out) { - int n = (int)x_guess.size(); - - mathlib::MatX_T F; - bool analytical_success = false; - - if constexpr (!std::is_same_v, std::nullptr_t>) { - if constexpr (std::is_pointer_v> || requires { bool(jac); }) { - if (jac) { - jac(x_guess, F); // User-provided Jacobian - analytical_success = true; - } - } - else { - jac(x_guess, F); // User-provided Jacobian - analytical_success = true; - } - } - if (!analytical_success) { - printf("Using finite difference Jacobian for Implicit Euler\n"); - F = finiteDifferenceJacobian( - [&](Scalar, const mathlib::VecX_T& x_pert) { - return f(t + dt, x_pert); - }, - t + dt, - x_guess - ); - } - - J_out = mathlib::MatX_T::Identity(n, n) - dt * F; // J = I - dt * df/dx - }; - - mathlib::VecX_T x0 = x + dt * f(t + dt, x); // Initial guess for Newton-Raphson - return newtonRaphson(g, J, x0, maxIter, tol); - } - // AD version of Implicit Euler method - template - mathlib::VecX_T NumericalIntegrator::implicitEuler_AD( - const mathlib::VecX_T& x, - Scalar t, - Scalar dt, - Func&& f, - int maxIter, - Scalar tol - ) { - // The function g(x_guess) = 0 that we want to solve for the implicit Euler step - auto g = [&](const mathlib::VecX_T& x_guess, mathlib::VecX_T& g_out) { g_out = x_guess - x - dt * f(t + dt, x_guess); }; - // Numerical Jacobian for Newton-Raphson - auto J = [&](const mathlib::VecX_T& x_guess, mathlib::MatX_T& J_out) { - int n = (int)x_guess.size(); - mathlib::MatX_T F; - F = automaticDifferenceJacobian( - [&](auto t_pert, const auto& x_pert) { - using Dual = std::decay_t; - Dual t_eval = Dual(t_pert) + Dual(dt); - return f(t_eval, x_pert); - }, - t + dt, - x_guess - ); - - J_out = mathlib::MatX_T::Identity(n, n) - dt * F; // J = I - dt * df/dx - }; - - mathlib::VecX_T x0 = x + dt * f(t + dt, x); // Initial guess for Newton-Raphson - return newtonRaphson_AD(g, J, x0, maxIter, tol); - } - - // Implicit Midpoint method - template - mathlib::VecX_T NumericalIntegrator::implicitMidpoint( - const mathlib::VecX_T& x, - Scalar t, - Scalar dt, - Func&& f, - JacFunc&& jac, - int maxIter, - Scalar tol - ) { - // The function g(x_guess) = 0 that we want to solve for the implicit midpoint step - auto g = [&](const mathlib::VecX_T& x_guess, mathlib::VecX_T& g_out) { g_out = x_guess - x - dt * f((t + dt) / Scalar(2), (x + x_guess) / Scalar(2)); }; - - // Numerical Jacobian for Newton-Raphson - auto J = [&](const mathlib::VecX_T& x_guess, mathlib::MatX_T& J_out) { - int n = (int)x_guess.size(); - mathlib::MatX_T F; - bool analytical_success = false; - - if constexpr (!std::is_same_v, std::nullptr_t>) { - if constexpr (std::is_pointer_v> || requires { bool(jac); }) { - if (jac) { - jac((x + x_guess) / Scalar(2), F); // User-provided Jacobian - analytical_success = true; - } - } - else { - jac((x + x_guess) / Scalar(2), F); // User-provided Jacobian - analytical_success = true; - } - } - if (!analytical_success) { - F = finiteDifferenceJacobian( - [&](Scalar, const mathlib::VecX_T& x_pert) { - return f((t + dt) / Scalar(2), (x + x_pert) / Scalar(2)); - }, - t + dt / Scalar(2), - x_guess - ); - } - J_out = mathlib::MatX_T::Identity(n, n) - Scalar(0.5) * dt * F; // J = I - dt * df/dx - }; - - mathlib::VecX_T x0 = x + dt * f((t + dt) / Scalar(2), x); // Initial guess for Newton-Raphson - return newtonRaphson(g, J, x0, maxIter, tol); - } - // AD version of Implicit Midpoint method - template - mathlib::VecX_T NumericalIntegrator::implicitMidpoint_AD( - const mathlib::VecX_T& x, - Scalar t, - Scalar dt, - Func&& f, - int maxIter, - Scalar tol - ) { - // The function g(x_guess) = 0 that we want to solve for the implicit midpoint step - auto g = [&](const mathlib::VecX_T& x_guess, mathlib::VecX_T& g_out) { g_out = x_guess - x - dt * f((t + dt) / Scalar(2), (x + x_guess) / Scalar(2)); }; - - // Numerical Jacobian for Newton-Raphson - auto J = [&](const mathlib::VecX_T& x_guess, mathlib::MatX_T& J_out) { - int n = (int)x_guess.size(); - mathlib::MatX_T F; - bool analytical_success = false; - - F = automaticDifferenceJacobian( - [&](auto t_pert, const auto& x_pert) { - using Dual = std::decay_t; - mathlib::VecX_T x_mid = (x.template cast() + x_pert) / Dual(2); - Dual t_mid = (t_pert + dt) / Dual(2); - return f(t_mid, x_mid); - }, - t + dt / Scalar(2), - x_guess - ); - J_out = mathlib::MatX_T::Identity(n, n) - Scalar(0.5) * dt * F; // J = I - dt * df/dx - }; - - mathlib::VecX_T x0 = x + dt * f((t + dt) / Scalar(2), x); // Initial guess for Newton-Raphson - return newtonRaphson_AD(g, J, x0, maxIter, tol); - } - - // Gauss-Legendre Runge-Kutta method (2 stages, 4th order) - template - mathlib::VecX_T NumericalIntegrator::GLRK2( - const mathlib::VecX_T& x, - Scalar t, - Scalar dt, - Func&& f, - JacFunc&& jac, - int maxIter, - Scalar tol - ) { - using std::sqrt; - - const Eigen::Index n = x.size(); - - // Coefficients for the 2-stage Gauss-Legendre method (4th order) - mathlib::VecX_T c(2); - c << - Scalar(0.5) - sqrt(Scalar(3)) / Scalar(6), - Scalar(0.5) + sqrt(Scalar(3)) / Scalar(6); // Stage time fractions - - mathlib::MatX_T A(2, 2); - A << - Scalar(0.25), Scalar(0.25) - sqrt(Scalar(3)) / Scalar(6), - Scalar(0.25) + sqrt(Scalar(3)) / Scalar(6), Scalar(0.25); - - const Scalar b = Scalar(0.5); // Weights for final update - - // Initial guess for the stage values k1, k2 - mathlib::VecX_T k(2 * n); // 2 stages - mathlib::VecX_T f0 = f(t, x); - k.segment(0, n) = f0; - k.segment(n, n) = f0; - - auto eval_g = [&](const mathlib::VecX_T& k_guess, mathlib::VecX_T& g) { - mathlib::VecX_T k1 = k_guess.segment(0, n); - mathlib::VecX_T k2 = k_guess.segment(n, n); - - mathlib::VecX_T x1 = x + dt * (A(0, 0) * k1 + A(0, 1) * k2); - mathlib::VecX_T x2 = x + dt * (A(1, 0) * k1 + A(1, 1) * k2); - - mathlib::VecX_T f1 = f(t + c(0) * dt, x1); - mathlib::VecX_T f2 = f(t + c(1) * dt, x2); - - g.resize(2 * n); - g.segment(0, n) = k1 - f1; - g.segment(n, n) = k2 - f2; - }; - - auto eval_j = [&](const mathlib::VecX_T& k_guess, mathlib::MatX_T& J) { - mathlib::VecX_T k1 = k_guess.segment(0, n); - mathlib::VecX_T k2 = k_guess.segment(n, n); - mathlib::VecX_T x1 = x + dt * (A(0, 0) * k1 + A(0, 1) * k2); - mathlib::VecX_T x2 = x + dt * (A(1, 0) * k1 + A(1, 1) * k2); - mathlib::MatX_T F1(n, n), F2(n, n); - bool analytical_success = false; - - // Attempt to use the provided Jacobian function if it's not a nullptr and is callable - if constexpr (!std::is_same_v, std::nullptr_t>) { - if constexpr (std::is_pointer_v> || requires { bool(jac); }) { - if (jac) { - jac(x1, F1); - jac(x2, F2); - analytical_success = true; - } - } - else { - // Pure lambda type execution path - jac(x1, F1); - jac(x2, F2); - analytical_success = true; - } - } - - if (!analytical_success) { - F1 = finiteDifferenceJacobian( - [&](Scalar, const mathlib::VecX_T& x_pert) { - return f(t + c(0) * dt, x_pert); - }, - t + c(0) * dt, - x1 - ); - F2 = finiteDifferenceJacobian( - [&](Scalar, const mathlib::VecX_T& x_pert) { - return f(t + c(1) * dt, x_pert); - }, - t + c(1) * dt, - x2 - ); - } - - J.setZero(2 * n, 2 * n); - J.block(0, 0, n, n) = mathlib::MatX_T::Identity(n, n) - dt * A(0, 0) * F1; - J.block(0, n, n, n) = -dt * A(0, 1) * F1; - J.block(n, 0, n, n) = -dt * A(1, 0) * F2; - J.block(n, n, n, n) = mathlib::MatX_T::Identity(n, n) - dt * A(1, 1) * F2; - }; - - // Solve the nonlinear system for the stage values using Newton-Raphson - k = newtonRaphson(eval_g, eval_j, k, maxIter, tol); - - // Compute the final update for x using the stage values - mathlib::VecX_T k1 = k.segment(0, n); - mathlib::VecX_T k2 = k.segment(n, n); - mathlib::VecX_T x_f = x + dt * (b * k1 + b * k2); - - // Precautionary check for divergence (NaN or Inf) - if (!x_f.allFinite()) { throw std::runtime_error("GLRK2 diverged"); } - - return x_f; - } - // AD version of Gauss-Legendre Runge-Kutta method (2 stages, 4th order) - template - mathlib::VecX_T NumericalIntegrator::GLRK2_AD( - const mathlib::VecX_T& x, - Scalar t, - Scalar dt, - Func&& f, - int maxIter, - Scalar tol - ) { - using std::sqrt; - - const Eigen::Index n = x.size(); - - // Coefficients for the 2-stage Gauss-Legendre method (4th order) - mathlib::VecX_T c(2); - c << - Scalar(0.5) - sqrt(Scalar(3)) / Scalar(6), - Scalar(0.5) + sqrt(Scalar(3)) / Scalar(6); // Stage time fractions - - mathlib::MatX_T A(2, 2); - A << - Scalar(0.25), Scalar(0.25) - sqrt(Scalar(3)) / Scalar(6), - Scalar(0.25) + sqrt(Scalar(3)) / Scalar(6), Scalar(0.25); - - const Scalar b = Scalar(0.5); // Weights for final update - - // Initial guess for the stage values k1, k2, k3 - mathlib::VecX_T k(2 * n); // 2 stages - mathlib::VecX_T f0 = f(t, x); - k.segment(0, n) = f0; - k.segment(n, n) = f0; - - auto eval_g = [&](const mathlib::VecX_T& k_guess, mathlib::VecX_T& g) { - mathlib::VecX_T k1 = k_guess.segment(0, n); - mathlib::VecX_T k2 = k_guess.segment(n, n); - - mathlib::VecX_T x1 = x + dt * (A(0, 0) * k1 + A(0, 1) * k2); - mathlib::VecX_T x2 = x + dt * (A(1, 0) * k1 + A(1, 1) * k2); - - mathlib::VecX_T f1 = f(t + c(0) * dt, x1); - mathlib::VecX_T f2 = f(t + c(1) * dt, x2); - - g.resize(2 * n); - g.segment(0, n) = k1 - f1; - g.segment(n, n) = k2 - f2; - }; - - auto eval_j = [&](const mathlib::VecX_T& k_guess, mathlib::MatX_T& J) { - mathlib::VecX_T k1 = k_guess.segment(0, n); - mathlib::VecX_T k2 = k_guess.segment(n, n); - mathlib::VecX_T x1 = x + dt * (A(0, 0) * k1 + A(0, 1) * k2); - mathlib::VecX_T x2 = x + dt * (A(1, 0) * k1 + A(1, 1) * k2); - mathlib::MatX_T F1(n, n), F2(n, n); - bool analytical_success = false; - - F1 = automaticDifferenceJacobian( - [&](auto t_pert, const auto& x_pert) { - using Dual = std::decay_t; - Dual t_eval = Dual(t_pert) + Dual(c(0)) * Dual(dt); - return f(t_eval, x_pert); - }, - t + c(0) * dt, - x1 - ); - F2 = automaticDifferenceJacobian( - [&](auto t_pert, const auto& x_pert) { - using Dual = std::decay_t; - Dual t_eval = Dual(t_pert) + Dual(c(1)) * Dual(dt); - return f(t_eval, x_pert); - }, - t + c(1) * dt, - x2 - ); - - J.setZero(2 * n, 2 * n); - J.block(0, 0, n, n) = mathlib::MatX_T::Identity(n, n) - dt * A(0, 0) * F1; - J.block(0, n, n, n) = -dt * A(0, 1) * F1; - J.block(n, 0, n, n) = -dt * A(1, 0) * F2; - J.block(n, n, n, n) = mathlib::MatX_T::Identity(n, n) - dt * A(1, 1) * F2; - }; - - // Solve the nonlinear system for the stage values using Newton-Raphson - k = newtonRaphson_AD(eval_g, eval_j, k, maxIter, tol); - - // Compute the final update for x using the stage values - mathlib::VecX_T k1 = k.segment(0, n); - mathlib::VecX_T k2 = k.segment(n, n); - mathlib::VecX_T x_f = x + dt * (b * k1 + b * k2); - - return x_f; - - } - - // Gauss-Legendre Runge-Kutta method (3 stages, 6th order) - template - mathlib::VecX_T NumericalIntegrator::GLRK3( - const mathlib::VecX_T& x, - Scalar t, - Scalar dt, - Func&& f, - JacFunc&& jac, - int maxIter, - Scalar tol - ) { - using Real = typename mathlib::DualTraits::BaseScalar; - using std::sqrt; - using std::pow; - using std::abs; - using std::max; - - if (mathlib::real(tol) < Real(0)) { - Real dt_r = mathlib::real(dt); - Real tol_r = max(Real(1e-12), Real(1e-2) * pow(dt_r, Real(7))); - tol = Scalar(tol_r); - } - const Eigen::Index n = x.size(); - - // Coefficients for the 3-stage Gauss-Legendre method (6th order) - mathlib::VecX_T c(3); - c << - Scalar(0.5) - sqrt(Scalar(15)) / Scalar(10), - Scalar(0.5), - Scalar(0.5) + sqrt(Scalar(15)) / Scalar(10); // Stage time fractions - - mathlib::Mat3_T A(3, 3); - A << - Scalar(5) / Scalar(36), Scalar(2) / Scalar(9) - sqrt(Scalar(15)) / Scalar(15), Scalar(5) / Scalar(36) - sqrt(Scalar(15)) / Scalar(30), - Scalar(5) / Scalar(36) + sqrt(Scalar(15)) / Scalar(24), Scalar(2) / Scalar(9), Scalar(5) / Scalar(36) - sqrt(Scalar(15)) / Scalar(24), - Scalar(5) / Scalar(36) + sqrt(Scalar(15)) / Scalar(30), Scalar(2) / Scalar(9) - sqrt(Scalar(15)) / Scalar(15), Scalar(5) / Scalar(36); - - mathlib::VecX_T b(3); - b << 5.0 / 18.0, 4.0 / 9.0, 5.0 / 18.0; // Weights for final update - - // Initial guess for the stage values k1, k2, k3 - mathlib::VecX_T k(3 * n); // 3 stages - mathlib::VecX_T f0 = f(t, x); - k.segment(0, n) = f0; - k.segment(n, n) = f0; - k.segment(2 * n, n) = f0; - - auto eval_g = [&](const mathlib::VecX_T& k_guess, mathlib::VecX_T& g) { - mathlib::VecX_T k1 = k_guess.segment(0, n); - mathlib::VecX_T k2 = k_guess.segment(n, n); - mathlib::VecX_T k3 = k_guess.segment(2 * n, n); - - mathlib::VecX_T x1 = x + dt * (A(0, 0) * k1 + A(0, 1) * k2 + A(0, 2) * k3); - mathlib::VecX_T x2 = x + dt * (A(1, 0) * k1 + A(1, 1) * k2 + A(1, 2) * k3); - mathlib::VecX_T x3 = x + dt * (A(2, 0) * k1 + A(2, 1) * k2 + A(2, 2) * k3); - - mathlib::VecX_T f1 = f(t + c(0) * dt, x1); - mathlib::VecX_T f2 = f(t + c(1) * dt, x2); - mathlib::VecX_T f3 = f(t + c(2) * dt, x3); - - g.resize(3 * n); - - g.segment(0, n) = k1 - f1; - g.segment(n, n) = k2 - f2; - g.segment(2 * n, n) = k3 - f3; - }; - - auto eval_j = [&](const mathlib::VecX_T& k_guess, mathlib::MatX_T& J) { - mathlib::VecX_T k1 = k_guess.segment(0, n); - mathlib::VecX_T k2 = k_guess.segment(n, n); - mathlib::VecX_T k3 = k_guess.segment(2 * n, n); - - mathlib::VecX_T x1 = x + dt * (A(0, 0) * k1 + A(0, 1) * k2 + A(0, 2) * k3); - mathlib::VecX_T x2 = x + dt * (A(1, 0) * k1 + A(1, 1) * k2 + A(1, 2) * k3); - mathlib::VecX_T x3 = x + dt * (A(2, 0) * k1 + A(2, 1) * k2 + A(2, 2) * k3); - - mathlib::MatX_T F1(n, n), F2(n, n), F3(n, n); - bool analytical_success = false; - - // Attempt to use the provided Jacobian function if it's not a nullptr and is callable - if constexpr (!std::is_same_v, std::nullptr_t>) { - if constexpr (std::is_pointer_v> || requires { bool(jac); }) { - if (jac) { - jac(x1, F1); - jac(x2, F2); - jac(x3, F3); - analytical_success = true; - } - } - else { - // Pure lambda type execution path - jac(x1, F1); - jac(x2, F2); - jac(x3, F3); - analytical_success = true; - } - } - - if (!analytical_success) { - F1 = finiteDifferenceJacobian( - [&](Scalar, const mathlib::VecX_T& x_pert) { - return f(t + c(0) * dt, x_pert); - }, - t + c(0) * dt, - x1 - ); - - F2 = finiteDifferenceJacobian( - [&](Scalar, const mathlib::VecX_T& x_pert) { - return f(t + c(1) * dt, x_pert); - }, - t + c(1) * dt, - x2 - ); - - F3 = finiteDifferenceJacobian( - [&](Scalar, const mathlib::VecX_T& x_pert) { - return f(t + c(2) * dt, x_pert); - }, - t + c(2) * dt, - x3 - ); - } - - J.setZero(3 * n, 3 * n); - J.block(0, 0, n, n) = mathlib::MatX_T::Identity(n, n) - dt * A(0, 0) * F1; - J.block(0, n, n, n) = -dt * A(0, 1) * F1; - J.block(0, 2 * n, n, n) = -dt * A(0, 2) * F1; - J.block(n, 0, n, n) = -dt * A(1, 0) * F2; - J.block(n, n, n, n) = mathlib::MatX_T::Identity(n, n) - dt * A(1, 1) * F2; - J.block(n, 2 * n, n, n) = -dt * A(1, 2) * F2; - J.block(2 * n, 0, n, n) = -dt * A(2, 0) * F3; - J.block(2 * n, n, n, n) = -dt * A(2, 1) * F3; - J.block(2 * n, 2 * n, n, n) = mathlib::MatX_T::Identity(n, n) - dt * A(2, 2) * F3; - }; - - // Solve the nonlinear system for the stage values using Newton-Raphson - k = newtonRaphson(eval_g, eval_j, k, maxIter, tol); - - mathlib::VecX_T g_check; - eval_g(k, g_check); - - auto residual = mathlib::real(g_check.norm()); - //if (residual > 1e-8) { std::cout << "[GLRK3] Large final residual: " << residual << std::endl; } - - // Compute the final update for x using the stage values - mathlib::VecX_T k1 = k.segment(0, n); - mathlib::VecX_T k2 = k.segment(n, n); - mathlib::VecX_T k3 = k.segment(2 * n, n); - mathlib::VecX_T x_f = x + dt * (b(0) * k1 + b(1) * k2 + b(2) * k3); - - // Precautionary check for divergence (NaN or Inf) - if (!x_f.allFinite()) { throw std::runtime_error("GLRK3 diverged"); } - - return x_f; - } - // AD version of Gauss-Legendre Runge-Kutta method (3 stages, 6th order) - template - mathlib::VecX_T NumericalIntegrator::GLRK3_AD( - const mathlib::VecX_T& x, - Scalar t, - Scalar dt, - Func&& f, - int maxIter, - Scalar tol - ) { - using Real = typename mathlib::DualTraits::BaseScalar; - using std::sqrt; - using std::pow; - using std::abs; - using std::max; - - if (mathlib::real(tol) < Real(0)) { - Real dt_r = mathlib::real(dt); - Real tol_r = max(Real(1e-12), Real(1e-2) * pow(dt_r, Real(7))); - tol = Scalar(tol_r); - } - const Eigen::Index n = x.size(); - - // Coefficients for the 3-stage Gauss-Legendre method (6th order) - mathlib::VecX_T c(3); - c << - Scalar(0.5) - sqrt(Scalar(15)) / Scalar(10), - Scalar(0.5), - Scalar(0.5) + sqrt(Scalar(15)) / Scalar(10); // Stage time fractions - - mathlib::Mat3_T A(3, 3); - A << - Scalar(5) / Scalar(36), Scalar(2) / Scalar(9) - sqrt(Scalar(15)) / Scalar(15), Scalar(5) / Scalar(36) - sqrt(Scalar(15)) / Scalar(30), - Scalar(5) / Scalar(36) + sqrt(Scalar(15)) / Scalar(24), Scalar(2) / Scalar(9), Scalar(5) / Scalar(36) - sqrt(Scalar(15)) / Scalar(24), - Scalar(5) / Scalar(36) + sqrt(Scalar(15)) / Scalar(30), Scalar(2) / Scalar(9) - sqrt(Scalar(15)) / Scalar(15), Scalar(5) / Scalar(36); - - mathlib::VecX_T b(3); - b << 5.0 / 18.0, 4.0 / 9.0, 5.0 / 18.0; // Weights for final update - - // Initial guess for the stage values k1, k2, k3 - mathlib::VecX_T k(3 * n); // 3 stages - mathlib::VecX_T f0 = f(t, x); - k.segment(0, n) = f0; - k.segment(n, n) = f0; - k.segment(2 * n, n) = f0; - - auto eval_g = [&](const mathlib::VecX_T& k_guess, mathlib::VecX_T& g) { - mathlib::VecX_T k1 = k_guess.segment(0, n); - mathlib::VecX_T k2 = k_guess.segment(n, n); - mathlib::VecX_T k3 = k_guess.segment(2 * n, n); - - mathlib::VecX_T x1 = x + dt * (A(0, 0) * k1 + A(0, 1) * k2 + A(0, 2) * k3); - mathlib::VecX_T x2 = x + dt * (A(1, 0) * k1 + A(1, 1) * k2 + A(1, 2) * k3); - mathlib::VecX_T x3 = x + dt * (A(2, 0) * k1 + A(2, 1) * k2 + A(2, 2) * k3); - - mathlib::VecX_T f1 = f(t + c(0) * dt, x1); - mathlib::VecX_T f2 = f(t + c(1) * dt, x2); - mathlib::VecX_T f3 = f(t + c(2) * dt, x3); - - g.resize(3 * n); - - g.segment(0, n) = k1 - f1; - g.segment(n, n) = k2 - f2; - g.segment(2 * n, n) = k3 - f3; - }; - - auto eval_j = [&](const mathlib::VecX_T& k_guess, mathlib::MatX_T& J) { - mathlib::VecX_T k1 = k_guess.segment(0, n); - mathlib::VecX_T k2 = k_guess.segment(n, n); - mathlib::VecX_T k3 = k_guess.segment(2 * n, n); - - mathlib::VecX_T x1 = x + dt * (A(0, 0) * k1 + A(0, 1) * k2 + A(0, 2) * k3); - mathlib::VecX_T x2 = x + dt * (A(1, 0) * k1 + A(1, 1) * k2 + A(1, 2) * k3); - mathlib::VecX_T x3 = x + dt * (A(2, 0) * k1 + A(2, 1) * k2 + A(2, 2) * k3); - - mathlib::MatX_T F1(n, n), F2(n, n), F3(n, n); - - F1 = automaticDifferenceJacobian( - [&](auto t_pert, const auto& x_pert) { - using Dual = std::decay_t; - Dual t_eval = Dual(t_pert) + Dual(c(0)) * Dual(dt); - return f(t_eval, x_pert); - }, - t + c(0) * dt, - x1 - ); - F2 = automaticDifferenceJacobian( - [&](auto t_pert, const auto& x_pert) { - using Dual = std::decay_t; - Dual t_eval = Dual(t_pert) + Dual(c(1)) * Dual(dt); - return f(t_eval, x_pert); - }, - t + c(1) * dt, - x2 - ); - F3 = automaticDifferenceJacobian( - [&](auto t_pert, const auto& x_pert) { - using Dual = std::decay_t; - Dual t_eval = Dual(t_pert) + Dual(c(2)) * Dual(dt); - return f(t_eval, x_pert); - }, - t + c(2) * dt, - x3 - ); - - - J.setZero(3 * n, 3 * n); - J.block(0, 0, n, n) = mathlib::MatX_T::Identity(n, n) - dt * A(0, 0) * F1; - J.block(0, n, n, n) = -dt * A(0, 1) * F1; - J.block(0, 2 * n, n, n) = -dt * A(0, 2) * F1; - J.block(n, 0, n, n) = -dt * A(1, 0) * F2; - J.block(n, n, n, n) = mathlib::MatX_T::Identity(n, n) - dt * A(1, 1) * F2; - J.block(n, 2 * n, n, n) = -dt * A(1, 2) * F2; - J.block(2 * n, 0, n, n) = -dt * A(2, 0) * F3; - J.block(2 * n, n, n, n) = -dt * A(2, 1) * F3; - J.block(2 * n, 2 * n, n, n) = mathlib::MatX_T::Identity(n, n) - dt * A(2, 2) * F3; - }; - - // Solve the nonlinear system for the stage values using Newton-Raphson - k = newtonRaphson_AD(eval_g, eval_j, k, maxIter, tol); - - mathlib::VecX_T g_check; - eval_g(k, g_check); - - //if (residual > 1e-8) { std::cout << "[GLRK3] Large final residual: " << residual << std::endl; } - - // Compute the final update for x using the stage values - mathlib::VecX_T k1 = k.segment(0, n); - mathlib::VecX_T k2 = k.segment(n, n); - mathlib::VecX_T k3 = k.segment(2 * n, n); - mathlib::VecX_T x_f = x + dt * (b(0) * k1 + b(1) * k2 + b(2) * k3); - - return x_f; - } - - // Newton-Raphson solver for systems of nonlinear equations g(x) = 0 - template - mathlib::VecX_T NumericalIntegrator::newtonRaphson( - EvalG&& eval_g, - EvalJ&& eval_j, - mathlib::VecX_T x0, - int maxIter, - Scalar tol - ) { - mathlib::VecX_T x = x0, g, x_trial; - mathlib::VecX_T delta = mathlib::VecX_T::Constant(x0.size(), std::numeric_limits::infinity()); - mathlib::MatX_T J; - Eigen::PartialPivLU> solver; - - for (int iter = 0; iter < maxIter; ++iter) { - eval_g(x, g); - if (!g.allFinite()) { - throw std::runtime_error( - "Newton received non-finite residual at iter = " - + std::to_string(iter) - + ", residual norm = " - + std::to_string(g.norm()) - ); - } - if (g.norm() < tol) { return x; } - - eval_j(x, J); - if (!J.allFinite()) { - throw std::runtime_error( - "Newton received non-finite Jacobian at iter = " - + std::to_string(iter) - ); - } - - solver.compute(J); - delta = solver.solve(-g); - - if (!delta.allFinite()) { - throw std::runtime_error( - "Newton produced non-finite step at iter = " - + std::to_string(iter) - ); - } - - x += delta; - if (delta.norm() < tol * (Scalar(1) + x.norm())) { return x; } - - if (!x.allFinite()) { - throw std::runtime_error( - "Newton state became non-finite at iter = " - + std::to_string(iter) - ); - } - } - - throw std::runtime_error( - "Newton-Raphson failed to converge after " - + std::to_string(maxIter) - + " iterations. Final residual norm = " - + std::to_string(g.norm()) - + ", final step norm = " - + std::to_string(delta.norm()) - ); - } - // AD version of Newton-Raphson solver for systems of nonlinear equations g(x) = 0 (DOES NOT USE NEWTON RAPHSON CALL) - template - mathlib::VecX_T NumericalIntegrator::newtonRaphson_AD( - EvalG&& eval_g, - EvalJ&& eval_j, - mathlib::VecX_T x0, - int maxIter, - Scalar tol - ) { - mathlib::VecX_T x = x0, g, x_trial; - using BaseScalar = typename mathlib::DualTraits::BaseScalar; - mathlib::VecX_T delta = mathlib::VecX_T::Constant(x0.size(), Scalar(std::numeric_limits::infinity())); - mathlib::MatX_T J; - Eigen::PartialPivLU> solver; - for (int iter = 0; iter < maxIter; ++iter) { - eval_g(x, g); - if (!g.allFinite()) { - throw std::runtime_error( - "Newton received non-finite residual at iter = " - + std::to_string(iter) - + ", residual norm = " - + std::to_string(mathlib::real(g.norm())) - ); - } - eval_j(x, J); - if (!J.allFinite()) { - throw std::runtime_error( - "Newton received non-finite Jacobian at iter = " - + std::to_string(iter) - ); - } - solver.compute(J); - delta = solver.solve(-g); - if (!delta.allFinite()) { - throw std::runtime_error( - "Newton produced non-finite step at iter = " - + std::to_string(iter) - ); - } - x += delta; - if (mathlib::real(g.norm()) < mathlib::real(tol)) { return x; } - if (!x.allFinite()) { - throw std::runtime_error( - "Newton state became non-finite at iter = " - + std::to_string(iter) - ); - } - } - throw std::runtime_error( - "Newton-Raphson failed to converge after " - + std::to_string(maxIter) - + " iterations. Final residual norm = " - + std::to_string(mathlib::real(g.norm())) - + ", final step norm = " - + std::to_string(mathlib::real(delta.norm())) - ); - } - - // Finite difference approximation of the Jacobian matrix df/dx for a vector-valued function f: R^n -> R^m at a point x - template - mathlib::MatX_T NumericalIntegrator::finiteDifferenceJacobian( - Func&& f, - Scalar t, - const mathlib::VecX_T& x - ) { - const Scalar eps_rel = Scalar(1e-8); - const int n = static_cast(x.size()); - mathlib::VecX_T f_0 = f(t, x); - mathlib::MatX_T J(f_0.size(), n); - // Compute the Jacobian column by column using central differences - for (int i = 0; i < n; ++i) { - mathlib::VecX_T x_fwd = x; - mathlib::VecX_T x_bwd = x; - Scalar h = eps_rel * std::max(Scalar(1), Scalar(std::abs(mathlib::real(x(i))))); - x_fwd(i) += h; - x_bwd(i) -= h; - mathlib::VecX_T f_fwd = f(t, x_fwd); - mathlib::VecX_T f_bwd = f(t, x_bwd); - J.col(i) = (f_fwd - f_bwd) / (Scalar(2) * h); - } - return J; - } - - // Automatic Difference Jacobian - template - mathlib::MatX_T::BaseScalar> NumericalIntegrator::automaticDifferenceJacobian( - Func&& f, - Scalar t, - const mathlib::VecX_T& x - ) { - using RealScalar = typename mathlib::DualTraits::BaseScalar; - - const int n = static_cast(x.size()); - auto f0 = f(t, x); - const int m = static_cast(f0.size()); - - mathlib::MatX_T J(m, n); - for (int i = 0; i < n; ++i) { - using Dual_T = mathlib::DualNumber_T; - - mathlib::VecX_T x_dual(n); - for (int k = 0; k < n; ++k) { - x_dual(k) = Dual_T( - mathlib::real(x(k)), - std::array{ - (k == i) ? RealScalar(1) : RealScalar(0) - } - ); - } - - Dual_T t_dual(mathlib::real(t)); - auto f_dual = f(t_dual, x_dual); - for (int j = 0; j < m; ++j) { - J(j, i) = mathlib::dualPart(f_dual(j)); - } - } - return J; - } -} \ No newline at end of file diff --git a/PxMLib/MathLib_Unit/AutomaticDifferentiationTests/ADImplicitIntegratorTests.cpp b/PxMLib/MathLib_Unit/AutomaticDifferentiationTests/ADImplicitIntegratorTests.cpp index 856a6cf6..74aad5f7 100644 --- a/PxMLib/MathLib_Unit/AutomaticDifferentiationTests/ADImplicitIntegratorTests.cpp +++ b/PxMLib/MathLib_Unit/AutomaticDifferentiationTests/ADImplicitIntegratorTests.cpp @@ -128,18 +128,18 @@ namespace { // AD Implicit Euler Tests // Test case for the GLRK2 method on the exponenital decay ODE. -//TEST("AD Implicit Euler Method", ImplicitEuler_ExponentialDecay) { -// VecX_T> x(1); -// x(0) = DualNumber_T(1.0, { 0.0 }); -// double T = 1.0; -// DualNumber_T dt(T / 1000.0, { 0.0 }); -// DualNumber_T t(0.0, { 0.0 }); -// for (int i = 0; i < 1000; ++i) { -// x = integrator.implicitEuler_AD(x, t, dt, expDecay, 8, DualNumber_T(1e-6, { 0.0 })); -// t += dt; -// } -// ASSERT_TRUE(std::abs(x(0).real - std::exp(-1.0)) < tol_low, "Implicit Euler exponential decay error too large"); -//} +TEST("AD Implicit Euler Method", ImplicitEuler_ExponentialDecay) { + VecX_T> x(1); + x(0) = DualNumber_T(1.0, { 0.0 }); + double T = 1.0; + DualNumber_T dt(T / 1000.0, { 0.0 }); + DualNumber_T t(0.0, { 0.0 }); + for (int i = 0; i < 1000; ++i) { + x = integrator.implicitEuler_AD(x, t, dt, expDecay, 8, DualNumber_T(1e-6, { 0.0 })); + t += dt; + } + ASSERT_TRUE(std::abs(x(0).real - std::exp(-1.0)) < tol_low, "Implicit Euler exponential decay error too large"); +} //// Test case for the implicit Euler method on the exponential decay ODE. //TEST("AD Implicit Euler Method", ImplicitEuler_ExponentialDecaySensitivity) { // auto stepFn = [&]( From 38a7ff46136939736deb7c724108f427fe6ac87d Mon Sep 17 00:00:00 2001 From: saltyjoss Date: Fri, 22 May 2026 09:35:05 +0100 Subject: [PATCH 046/210] fixes: Corrected DualNumber_T scalar interop and Eigen support * Added explicit Scalar conversion to DualNumber_T and extended comparison operators for scalar on LHS. * Introduced Eigen specializations for scalar_product_op and ScalarBinaryOpTraits to improve dual-scalar operations. * Updated AD Newton-Raphson to handle type casting between Real and Scalar. * THIS DOESNT NOT FIX THE OTHER CONVERSION ERRORS, THAT IS NEXT. --- PxMLib/MathLib/include/core/DualNumbers.h | 57 +++++++++++++++---- .../integrators/ad_implicit_integrators.inl | 6 +- 2 files changed, 49 insertions(+), 14 deletions(-) diff --git a/PxMLib/MathLib/include/core/DualNumbers.h b/PxMLib/MathLib/include/core/DualNumbers.h index 5900eb46..2ee86e9e 100644 --- a/PxMLib/MathLib/include/core/DualNumbers.h +++ b/PxMLib/MathLib/include/core/DualNumbers.h @@ -35,14 +35,10 @@ namespace mathlib { template class DualNumber_T { public: - using ScalarT = Scalar; - DualNumber_T(const Scalar& real = Scalar(0)) : real(real) { dual.fill(Scalar(0)); } - DualNumber_T(const Scalar& real, const std::array& dual) : real(real), dual(dual) {} - DualNumber_T(Scalar real, std::initializer_list duals) : real(real) { dual.fill(Scalar(0)); @@ -53,6 +49,8 @@ namespace mathlib { ); } + explicit operator Scalar() const { return real; } + Scalar real; std::array dual; }; @@ -95,7 +93,6 @@ namespace mathlib { inline bool operator<=(const DualNumber_T& a, const DualNumber_T& b) { return a.real <= b.real; } template inline bool operator>=(const DualNumber_T& a, const DualNumber_T& b) { return a.real >= b.real; } - // Comparison with scalar (compare only the real part) template inline bool operator==(const DualNumber_T& a, Scalar b) { return a.real == b; } @@ -109,6 +106,19 @@ namespace mathlib { inline bool operator<=(const DualNumber_T& a, Scalar b) { return a.real <= b; } template inline bool operator>=(const DualNumber_T& a, Scalar b) { return a.real >= b; } + // Comparison with scalar (scalar on left) + template + inline bool operator==(Scalar a, const DualNumber_T& b) { return b == a; } + template + inline bool operator!=(Scalar a, const DualNumber_T& b) { return !(b == a); } + template + inline bool operator<(Scalar a, const DualNumber_T& b) { return a < b.real; } + template + inline bool operator>(Scalar a, const DualNumber_T& b) { return a > b.real; } + template + inline bool operator<=(Scalar a, const DualNumber_T& b) { return a <= b.real; } + template + inline bool operator>=(Scalar a, const DualNumber_T& b) { return a >= b.real; } // ----- // Scalar Interactions @@ -382,7 +392,6 @@ namespace mathlib { inline DualNumber_T numeric_limits_infinity() { return DualNumber_T(std::numeric_limits::infinity(), std::array()); } - // Alternative absolute value function that simply negates the dual number if the real part is negative (this is less smooth but can be more efficient and avoids issues with zero) template inline DualNumber_T abs(const DualNumber_T& a) { @@ -441,7 +450,6 @@ namespace mathlib { } return out; } - // Since dual numbers are not complex, the real part is just the real part of the dual number (for non-dual types, this is just the value itself) template inline T real(const T& x) { return x; } @@ -475,7 +483,6 @@ namespace mathlib { using BaseScalar = Scalar; static constexpr bool is_dual = true; }; - // Dual Part template Scalar dualPart(const DualNumber_T& x) { return x.dual[0]; } @@ -570,7 +577,6 @@ namespace Eigen { template struct scalar_abs_op> { using Dual = mathlib::DualNumber_T; - EIGEN_DEVICE_FUNC Scalar operator()(const Dual& x) const { return std::abs(x.real); } }; @@ -598,22 +604,51 @@ namespace Eigen { typedef Scalar return_type; EIGEN_DEVICE_FUNC static return_type run(const mathlib::DualNumber_T& x) { return x.real; } }; - // Specialisation of Eigen's imag_impl template struct imag_impl> { typedef Scalar return_type; EIGEN_DEVICE_FUNC static return_type run(const mathlib::DualNumber_T&) { return Scalar(0); } }; - // Specialisation of Eigen's abs2_impl template struct abs2_impl> { typedef Scalar return_type; EIGEN_DEVICE_FUNC static return_type run(const mathlib::DualNumber_T& x) { return x.real * x.real; } }; + + // Definition of scalar_product_op for DualNumber_T + template + struct scalar_product_op, Scalar> { + typedef mathlib::DualNumber_T result_type; + }; + template + struct scalar_product_op> { + typedef mathlib::DualNumber_T result_type; + }; } // namespace internal + // Specialisation for dual + scalar (the result is still a dual number) + template + struct ScalarBinaryOpTraits, Scalar, internal::add_assign_op, Scalar>> { + typedef mathlib::DualNumber_T ReturnType; + }; + // Specialisation for scalar + dual (the order of the operands is reversed, but the result is still a dual number) + template + struct ScalarBinaryOpTraits, internal::add_assign_op>> { + typedef mathlib::DualNumber_T ReturnType; + }; + // Specialisation for dual - scalar (the result is still a dual number) + template + struct ScalarBinaryOpTraits, Scalar, internal::sub_assign_op, Scalar>> { + typedef mathlib::DualNumber_T ReturnType; + }; + // Specialisation for scalar - dual (the order of the operands is reversed, but the result is still a dual number) + template + struct ScalarBinaryOpTraits, internal::sub_assign_op>> { + typedef mathlib::DualNumber_T ReturnType; + }; + // Specialisation of Eigen's numext functions for DualNumber_T to allow Eigen's algorithms that rely on these functions (like abs, real, imag, conj) to work correctly with dual numbers namespace numext { // Returns the real part of the dual number diff --git a/PxMLib/MathLib/include/integrators/ad_implicit_integrators.inl b/PxMLib/MathLib/include/integrators/ad_implicit_integrators.inl index 9dd8fd91..fc11a45b 100644 --- a/PxMLib/MathLib/include/integrators/ad_implicit_integrators.inl +++ b/PxMLib/MathLib/include/integrators/ad_implicit_integrators.inl @@ -303,7 +303,7 @@ namespace integration { return x_f; } - // AD version of Newton-Raphson solver for systems of nonlinear equations g(x) = 0 (DOES NOT USE NEWTON RAPHSON CALL) + // AD version of Newton-Raphson solver for systems of nonlinear equations g(x) = 0 template mathlib::VecX_T NumericalIntegrator::newtonRaphson_AD( EvalG&& eval_g, @@ -325,9 +325,9 @@ namespace integration { eval_j(x, J); if (!J.allFinite()) { throw std::runtime_error("Newton received non-finite Jacobian at iter = " + std::to_string(iter)); } solver.compute(J); - delta = solver.solve(-g); + delta = solver.solve((-g).template cast()); if (!delta.allFinite()) { throw std::runtime_error("Newton produced non-finite step at iter = " + std::to_string(iter)); } - x += delta; + x += delta.template cast(); if (delta.norm() < tol * (Scalar(1) + x.norm())) { return x; } if (!x.allFinite()) { throw std::runtime_error("Newton state became non-finite at iter = " + std::to_string(iter)); } } From e38c083cae9cbb71140c77dfce138904ab4a39e0 Mon Sep 17 00:00:00 2001 From: saltyjoss Date: Fri, 22 May 2026 09:51:51 +0100 Subject: [PATCH 047/210] fixes: Updated Eigen library compatability logic in `DualNumber` header --- PxMLib/MathLib/include/core/DualNumbers.h | 75 +++++++++++++++-------- 1 file changed, 50 insertions(+), 25 deletions(-) diff --git a/PxMLib/MathLib/include/core/DualNumbers.h b/PxMLib/MathLib/include/core/DualNumbers.h index 2ee86e9e..23c54595 100644 --- a/PxMLib/MathLib/include/core/DualNumbers.h +++ b/PxMLib/MathLib/include/core/DualNumbers.h @@ -544,10 +544,8 @@ namespace mathlib { // Specialisation of Eigen's NumTraits for DualNumber_T to allow Eigen to work with dual numbers in its expressions and algorithms namespace Eigen { template - struct NumTraits> - : GenericNumTraits> { + struct NumTraits> : GenericNumTraits> { using Dual = mathlib::DualNumber_T; - // Define the types that Eigen uses for various purposes when working with DualNumber_T typedef Scalar Real; typedef Dual NonInteger; typedef Dual Nested; @@ -577,25 +575,24 @@ namespace Eigen { template struct scalar_abs_op> { using Dual = mathlib::DualNumber_T; - EIGEN_DEVICE_FUNC Scalar operator()(const Dual& x) const { return std::abs(x.real); } + typedef Scalar result_type; + EIGEN_DEVICE_FUNC inline Scalar operator()(const Dual& x) const { return std::abs(x.real); } }; - // Custom scoring function for DualNumber_T that compares based on the absolute value of the real part + // Specialisation of scalar_score_coeff_op for DualNumber_T to allow Eigen's internal sorting and selection algorithms to work correctly with dual numbers template struct scalar_score_coeff_op> { - // The result_type is a simple wrapper around the score (which is the absolute value of the real part of the dual number) struct result_type { Scalar score; result_type(int i = 0) : score(i) {} // Default constructor for compatibility with Eigen's internal mechanisms - result_type(const mathlib::DualNumber_T& x) // Constructor to compute score from DualNumber_T - : score(std::abs(x.real)) {} + result_type(const mathlib::DualNumber_T& x) : score(std::abs(x.real)) {} friend bool operator <(const result_type& a, const result_type& b) { return a.score < b.score; } friend bool operator >(const result_type& a, const result_type& b) { return a.score > b.score; } friend bool operator ==(const result_type& a, const result_type& b) { return a.score == b.score; } }; - // The operator() computes the score for a given DualNumber_T by taking the absolute value of its real part (this allows Eigen's internal sorting and selection algorithms to work correctly with dual numbers) - EIGEN_DEVICE_FUNC result_type operator()(const mathlib::DualNumber_T& x) const { return result_type(x); } + typedef result_type result_type; + EIGEN_DEVICE_FUNC inline result_type operator()(const mathlib::DualNumber_T& x) const { return result_type(x); } }; // Specialisation of Eigen's real_impl @@ -617,56 +614,84 @@ namespace Eigen { EIGEN_DEVICE_FUNC static return_type run(const mathlib::DualNumber_T& x) { return x.real * x.real; } }; - // Definition of scalar_product_op for DualNumber_T + // Specialisation of Eigen's scalar_product_op for DualNumber_T (LHS is dual, RHS is scalar) template struct scalar_product_op, Scalar> { typedef mathlib::DualNumber_T result_type; }; + // Same as above but with the order of the operands reversed (LHS is scalar, RHS is dual) template struct scalar_product_op> { typedef mathlib::DualNumber_T result_type; }; } // namespace internal - // Specialisation for dual + scalar (the result is still a dual number) + // Standard binary operation traits for DualNumber_T to ensure that operations between dual numbers and scalars yield the correct result type + // Specialisation for dual + scalar (the result is a dual number) template - struct ScalarBinaryOpTraits, Scalar, internal::add_assign_op, Scalar>> { + struct ScalarBinaryOpTraits, Scalar, internal::scalar_sum_op, Scalar>> { typedef mathlib::DualNumber_T ReturnType; }; - // Specialisation for scalar + dual (the order of the operands is reversed, but the result is still a dual number) + // Specialisation for scalar + dual (result is a dual number) template - struct ScalarBinaryOpTraits, internal::add_assign_op>> { + struct ScalarBinaryOpTraits, internal::scalar_sum_op>> { typedef mathlib::DualNumber_T ReturnType; }; - // Specialisation for dual - scalar (the result is still a dual number) + // Specialisation for dual - scalar (result is a dual number) template - struct ScalarBinaryOpTraits, Scalar, internal::sub_assign_op, Scalar>> { + struct ScalarBinaryOpTraits, Scalar, internal::scalar_difference_op, Scalar>> { typedef mathlib::DualNumber_T ReturnType; }; - // Specialisation for scalar - dual (the order of the operands is reversed, but the result is still a dual number) + // Specialisation for scalar - dual (result is a dual number) template - struct ScalarBinaryOpTraits, internal::sub_assign_op>> { + struct ScalarBinaryOpTraits, internal::scalar_difference_op>> { + typedef mathlib::DualNumber_T ReturnType; + }; + // Specialisation for dual * scalar (result is a dual number) + template + struct ScalarBinaryOpTraits, Scalar, internal::scalar_product_op, Scalar>> { + typedef mathlib::DualNumber_T ReturnType; + }; + // Specialisation for scalar * dual (result is a dual number) + template + struct ScalarBinaryOpTraits, internal::scalar_product_op>> { typedef mathlib::DualNumber_T ReturnType; }; + // Assignment Operator Traits for DualNumber_T to ensure that compound assignment operations between dual numbers and scalars yield the correct result type + // Specialisation for dual += scalar (result is a dual number) + template + struct ScalarBinaryOpTraits, Scalar, internal::add_assign_op, Scalar>> { + typedef mathlib::DualNumber_T& ReturnType; + }; + // Specialisation for Scalar += dual (result is a dual number) + template + struct ScalarBinaryOpTraits, internal::add_assign_op>> { + typedef mathlib::DualNumber_T& ReturnType; + }; + // Specialisation for dual -= scalar (result is a dual number) + template + struct ScalarBinaryOpTraits, Scalar, internal::sub_assign_op, Scalar>> { + typedef mathlib::DualNumber_T& ReturnType; + }; + // Specialisation for Scalar -= dual (result is a dual number) + template + struct ScalarBinaryOpTraits, internal::sub_assign_op>> { + typedef mathlib::DualNumber_T& ReturnType; + }; + // Specialisation of Eigen's numext functions for DualNumber_T to allow Eigen's algorithms that rely on these functions (like abs, real, imag, conj) to work correctly with dual numbers namespace numext { - // Returns the real part of the dual number template inline Scalar real(const mathlib::DualNumber_T& x) { return x.real; } - // Returns the imaginary part of the dual number template inline Scalar imag(const mathlib::DualNumber_T&) { return Scalar(0); } - // Returns the absolute value of the dual number template inline Scalar abs(const mathlib::DualNumber_T& x) { return std::abs(x.real); } - // Returns the squared absolute value of the dual number template inline Scalar abs2(const mathlib::DualNumber_T& x) { return x.real * x.real; } - // Returns the 1-norm of the dual number template inline Scalar norm1(const mathlib::DualNumber_T& x) { return std::abs(x.real); } - // Returns the 2-norm of the dual number template inline mathlib::DualNumber_T conj(const mathlib::DualNumber_T& x) { return x; } }// namespace numext From 0eb80eddb6aee6feaa7285970709a099952ed212 Mon Sep 17 00:00:00 2001 From: saltyjoss Date: Fri, 22 May 2026 10:31:16 +0100 Subject: [PATCH 048/210] fixes(WIP): Updated type usage in autodiff implicit integrators --- PxMLib/MathLib/include/core/DualNumbers.h | 6 + .../integrators/ad_implicit_integrators.inl | 76 +++---- .../ADImplicitIntegratorTests.cpp | 192 +++++++++--------- .../DualNumber_Eigen_ArithmeticTests.cpp | 12 +- 4 files changed, 141 insertions(+), 145 deletions(-) diff --git a/PxMLib/MathLib/include/core/DualNumbers.h b/PxMLib/MathLib/include/core/DualNumbers.h index 23c54595..8be62c54 100644 --- a/PxMLib/MathLib/include/core/DualNumbers.h +++ b/PxMLib/MathLib/include/core/DualNumbers.h @@ -618,11 +618,17 @@ namespace Eigen { template struct scalar_product_op, Scalar> { typedef mathlib::DualNumber_T result_type; + EIGEN_DEVICE_FUNC inline result_type operator()(const mathlib::DualNumber_T& a, Scalar b) const { + return a * b; // Use the previously defined operator* for DualNumber_T and scalar + } }; // Same as above but with the order of the operands reversed (LHS is scalar, RHS is dual) template struct scalar_product_op> { typedef mathlib::DualNumber_T result_type; + EIGEN_DEVICE_FUNC inline result_type operator()(Scalar a, const mathlib::DualNumber_T& b) const { + return b * a; // Use the previously defined operator* for DualNumber_T and scalar + } }; } // namespace internal diff --git a/PxMLib/MathLib/include/integrators/ad_implicit_integrators.inl b/PxMLib/MathLib/include/integrators/ad_implicit_integrators.inl index fc11a45b..09210f84 100644 --- a/PxMLib/MathLib/include/integrators/ad_implicit_integrators.inl +++ b/PxMLib/MathLib/include/integrators/ad_implicit_integrators.inl @@ -47,13 +47,15 @@ namespace integration { int maxIter, Scalar tol ) { + using Real = typename mathlib::DualTraits::BaseScalar; + // The function g(x_guess) = 0 that we want to solve for the implicit midpoint step auto g = [&](const mathlib::VecX_T& x_guess, mathlib::VecX_T& g_out) { g_out = x_guess - x - dt * f((t + dt) / Scalar(2), (x + x_guess) / Scalar(2)); }; // Numerical Jacobian for Newton-Raphson - auto J = [&](const mathlib::VecX_T& x_guess, mathlib::MatX_T& J_out) { + auto J = [&](const mathlib::VecX_T& x_guess, mathlib::MatX_T& J_out) { int n = (int)x_guess.size(); - mathlib::MatX_T F; + mathlib::MatX_T F; bool analytical_success = false; F = automaticDifferenceJacobian( @@ -63,10 +65,10 @@ namespace integration { Dual t_mid = (t_pert + dt) / Dual(2); return f(t_mid, x_mid); }, - t + dt / Scalar(2), + t + dt / Real(2), x_guess ); - J_out = mathlib::MatX_T::Identity(n, n) - Scalar(0.5) * dt * F; // J = I - dt * df/dx + J_out = (mathlib::MatX_T::Identity(n, n) - Scalar(0.5) * dt * F).template cast(); // J = I - dt * df/dx }; mathlib::VecX_T x0 = x + dt * f((t + dt) / Scalar(2), x); // Initial guess for Newton-Raphson @@ -89,17 +91,17 @@ namespace integration { const Eigen::Index n = x.size(); // Coefficients for the 2-stage Gauss-Legendre method (4th order) - mathlib::VecX_T c(2); + mathlib::VecX_T c(2); c << - Real(0.5) - sqrt(Real(3)) / Real(6), - Real(0.5) + sqrt(Real(3)) / Real(6); // Stage time fractions + Scalar(0.5) - sqrt(Scalar(3)) / Scalar(6), + Scalar(0.5) + sqrt(Scalar(3)) / Scalar(6); // Stage time fractions - mathlib::MatX_T A(2, 2); + mathlib::MatX_T A(2, 2); A << - Real(0.25), Real(0.25) - sqrt(Real(3)) / Real(6), - Real(0.25) + sqrt(Real(3)) / Real(6), Real(0.25); + Scalar(0.25), Scalar(0.25) - sqrt(Scalar(3)) / Scalar(6), + Scalar(0.25) + sqrt(Scalar(3)) / Scalar(6), Scalar(0.25); - const Real b = Real(0.5); // Weights for final update + const Scalar b = Scalar(0.5); // Weights for final update // Initial guess for the stage values k1, k2, k3 mathlib::VecX_T k(2 * n); // 2 stages @@ -122,7 +124,7 @@ namespace integration { g.segment(n, n) = k2 - f2; }; - auto eval_j = [&](const mathlib::VecX_T& k_guess, mathlib::MatX_T& J) { + auto eval_j = [&](const mathlib::VecX_T& k_guess, mathlib::MatX_T& J) { mathlib::VecX_T k1 = k_guess.segment(0, n); mathlib::VecX_T k2 = k_guess.segment(n, n); mathlib::VecX_T x1 = x + dt * (A(0, 0) * k1 + A(0, 1) * k2); @@ -150,10 +152,10 @@ namespace integration { ); J.setZero(2 * n, 2 * n); - J.block(0, 0, n, n) = mathlib::MatX_T::Identity(n, n) - dt * A(0, 0) * F1; - J.block(0, n, n, n) = -dt * A(0, 1) * F1; - J.block(n, 0, n, n) = -dt * A(1, 0) * F2; - J.block(n, n, n, n) = mathlib::MatX_T::Identity(n, n) - dt * A(1, 1) * F2; + J.block(0, 0, n, n) = (mathlib::MatX_T::Identity(n, n) - dt * A(0, 0) * F1).template cast; + J.block(0, n, n, n) = (- dt * A(0, 1) * F1).template cast; + J.block(n, 0, n, n) = (- dt * A(1, 0) * F2).template cast; + J.block(n, n, n, n) = (mathlib::MatX_T::Identity(n, n) - dt * A(1, 1) * F2).template cast; }; // Solve the nonlinear system for the stage values using Newton-Raphson @@ -192,20 +194,20 @@ namespace integration { const Eigen::Index n = x.size(); // Coefficients for the 3-stage Gauss-Legendre method (6th order) - mathlib::VecX_T c(3); + mathlib::VecX_T c(3); c << - Real(0.5) - sqrt(Real(15)) / Real(10), - Real(0.5), - Real(0.5) + sqrt(Real(15)) / Real(10); // Stage time fractions + Scalar(0.5) - sqrt(Scalar(15)) / Scalar(10), + Scalar(0.5), + Scalar(0.5) + sqrt(Scalar(15)) / Scalar(10); // Stage time fractions - mathlib::Mat3_T A(3, 3); + mathlib::Mat3_T A(3, 3); A << - Real(5) / Real(36), Real(2) / Real(9) - sqrt(Real(15)) / Real(15), Real(5) / Real(36) - sqrt(Real(15)) / Real(30), - Real(5) / Real(36) + sqrt(Real(15)) / Real(24), Real(2) / Real(9), Real(5) / Real(36) - sqrt(Real(15)) / Real(24), - Real(5) / Real(36) + sqrt(Real(15)) / Real(30), Real(2) / Real(9) + sqrt(Real(15)) / Real(15), Real(5) / Real(36); + Scalar(5) / Scalar(36), Scalar(2) / Scalar(9) - sqrt(Scalar(15)) / Scalar(15), Scalar(5) / Scalar(36) - sqrt(Scalar(15)) / Scalar(30), + Scalar(5) / Scalar(36) + sqrt(Scalar(15)) / Scalar(24), Scalar(2) / Scalar(9), Scalar(5) / Scalar(36) - sqrt(Scalar(15)) / Scalar(24), + Scalar(5) / Scalar(36) + sqrt(Scalar(15)) / Scalar(30), Scalar(2) / Scalar(9) + sqrt(Scalar(15)) / Scalar(15), Scalar(5) / Scalar(36); - mathlib::VecX_T b(3); - b << Real(5) / Real(18), Real(4) / Real(9), Real(5) / Real(18); // Weights for final update + mathlib::VecX_T b(3); + b << Scalar(5) / Scalar(18), Scalar(4) / Scalar(9), Scalar(5) / Scalar(18); // Weights for final update // Initial guess for the stage values k1, k2, k3 mathlib::VecX_T k(3 * n); // 3 stages @@ -234,7 +236,7 @@ namespace integration { g.segment(2 * n, n) = k3 - f3; }; - auto eval_j = [&](const mathlib::VecX_T& k_guess, mathlib::MatX_T& J) { + auto eval_j = [&](const mathlib::VecX_T& k_guess, mathlib::MatX_T& J) { // Extract the stage values k1, k2, k3 from the input guess vector mathlib::VecX_T k1 = k_guess.segment(0, n); mathlib::VecX_T k2 = k_guess.segment(n, n); @@ -276,15 +278,15 @@ namespace integration { x3 ); J.setZero(3 * n, 3 * n); - J.block(0, 0, n, n) = mathlib::MatX_T::Identity(n, n) - dt * A(0, 0) * F1; - J.block(0, n, n, n) = -dt * A(0, 1) * F1; - J.block(0, 2 * n, n, n) = -dt * A(0, 2) * F1; - J.block(n, 0, n, n) = -dt * A(1, 0) * F2; - J.block(n, n, n, n) = mathlib::MatX_T::Identity(n, n) - dt * A(1, 1) * F2; - J.block(n, 2 * n, n, n) = -dt * A(1, 2) * F2; - J.block(2 * n, 0, n, n) = -dt * A(2, 0) * F3; - J.block(2 * n, n, n, n) = -dt * A(2, 1) * F3; - J.block(2 * n, 2 * n, n, n) = mathlib::MatX_T::Identity(n, n) - dt * A(2, 2) * F3; + J.block(0, 0, n, n) = (mathlib::MatX_T::Identity(n, n) - dt * A(0, 0) * F1).template cast; + J.block(0, n, n, n) = (- dt * A(0, 1) * F1).template cast; + J.block(0, 2 * n, n, n) = (- dt * A(0, 2) * F1).template cast; + J.block(n, 0, n, n) = (- dt * A(1, 0) * F2).template cast; + J.block(n, n, n, n) = (mathlib::MatX_T::Identity(n, n) - dt * A(1, 1) * F2).template cast; + J.block(n, 2 * n, n, n) = (- dt * A(1, 2) * F2).template cast; + J.block(2 * n, 0, n, n) = (- dt * A(2, 0) * F3).template cast; + J.block(2 * n, n, n, n) = (- dt * A(2, 1) * F3).template cast; + J.block(2 * n, 2 * n, n, n) = (mathlib::MatX_T::Identity(n, n) - dt * A(2, 2) * F3).template cast; }; // Solve the nonlinear system for the stage values using Newton-Raphson @@ -292,8 +294,6 @@ namespace integration { mathlib::VecX_T g_check; eval_g(k, g_check); - //if (residual > 1e-8) { std::cout << "[GLRK3] Large final residual: " << residual << std::endl; } - // Compute the final update for x using the stage values mathlib::VecX_T k1 = k.segment(0, n); mathlib::VecX_T k2 = k.segment(n, n); diff --git a/PxMLib/MathLib_Unit/AutomaticDifferentiationTests/ADImplicitIntegratorTests.cpp b/PxMLib/MathLib_Unit/AutomaticDifferentiationTests/ADImplicitIntegratorTests.cpp index 74aad5f7..d6e8457b 100644 --- a/PxMLib/MathLib_Unit/AutomaticDifferentiationTests/ADImplicitIntegratorTests.cpp +++ b/PxMLib/MathLib_Unit/AutomaticDifferentiationTests/ADImplicitIntegratorTests.cpp @@ -140,103 +140,103 @@ TEST("AD Implicit Euler Method", ImplicitEuler_ExponentialDecay) { } ASSERT_TRUE(std::abs(x(0).real - std::exp(-1.0)) < tol_low, "Implicit Euler exponential decay error too large"); } -//// Test case for the implicit Euler method on the exponential decay ODE. -//TEST("AD Implicit Euler Method", ImplicitEuler_ExponentialDecaySensitivity) { -// auto stepFn = [&]( -// const VecX_T>& x, -// DualNumber_T t, -// DualNumber_T dt -// ) { -// return integrator.implicitEuler_AD(x, t, dt, expDecay, 8, DualNumber_T(1e-6, { 0.0 })); -// }; -// verifyExpDecaySensitivity(stepFn, 1e-2, 1000); -//} -//// Test case for the Implicit Midpoint method on the linear decay ODE. -//TEST("AD Implicit Euler Method", ImplicitEuler_LinearDecay) { -// VecX_T> x(1); -// x(0) = DualNumber_T(1.0, { 0.0 }); -// double T = 0.5; -// DualNumber_T dt(T / 500.0); -// DualNumber_T t(0.0); -// for (int i = 0; i < 500; ++i) { -// x = integrator.implicitEuler_AD(x, t, dt, linearDecay, 50, DualNumber_T(1e-6, { 0.0 })); -// t += dt; -// } -// ASSERT_TRUE(std::abs(x(0).real - std::exp(-1.0)) < tol_low, "Implicit Euler linear decay error too large"); -//} -//// Test case for the implicit Euler method on the linear decay ODE. -//TEST("AD Implicit Euler Method", ImplicitEuler_LinearDecaySensitivity) { -// auto stepFn = [&]( -// const VecX_T>& x, -// DualNumber_T t, -// DualNumber_T dt -// ) { -// return integrator.implicitEuler_AD(x, t, dt, expDecay, 8, DualNumber_T(1e-6, { 0.0 })); -// }; -// verifyExpDecaySensitivity(stepFn, 1e-2, 1000); -//} -//// Test case for the Implicit Euler method on the stiff decay ODE. -//TEST("AD Implicit Euler Method", ImplicitEuler_StiffDecay) { -// VecX_T> x(1); -// x(0) = DualNumber_T(1.0, { 0.0 }); -// double T = 0.5; -// DualNumber_T dt(T / 500.0); -// DualNumber_T t(0.0); -// double expected = std::exp(-50.0); -// auto stiff_f = [](auto t, const auto& x) { -// auto dx = x; -// dx(0) = -100.0 * x(0); -// return dx; -// }; -// for (int i = 0; i < 500; ++i) { -// x = integrator.implicitEuler_AD(x, t, dt, stiff_f, 50, DualNumber_T(1e-6, { 0.0 })); -// t += dt; -// } -// ASSERT_TRUE(std::isfinite(x(0).real), "Implicit Euler produced a non-finite result."); -// ASSERT_TRUE(x(0).real < 1e-12, "Implicit Euler did not sufficiently damp stiff mode"); -//} -//// Test case for the implicit Euler method on the stiff decay ODE. -//TEST("AD Implicit Euler Method", ImplicitEuler_StiffDecaySensitivity) { -// auto stepFn = [&]( -// const VecX_T>& x, -// DualNumber_T t, -// DualNumber_T dt -// ) { -// return integrator.implicitEuler_AD(x, t, dt, stiffDecay, 50, DualNumber_T(1e-6, { 0.0 })); -// }; -// verifyExpDecaySensitivity(stepFn, 1e-2, 1000); -//} -//// Test large step performance of implicit midpoint -//TEST("AD Implicit Euler Method", ImplicitEuler_Stability_LargeStep) { -// VecX_T> x(1); -// x(0) = DualNumber_T(1.0, { 1.0 }); -// double T = 1.0; -// DualNumber_T t(0.0); -// DualNumber_T dt(1.0 / 10.0); -// DualNumber_T prev = x(0); -// for (int i = 0; i < 10; ++i) { -// x = integrator.implicitEuler_AD(x, t, dt, stiffDecay, 50, DualNumber_T(1e-6, { 0.0 })); -// t += dt; -// ASSERT_TRUE(std::abs(x(0).real) < std::abs(prev.real), "Not monotone"); -// ASSERT_TRUE(std::abs(x(0).dual[0]) < std::abs(prev.dual[0]), "Not strictly decaying"); -// prev = x(0); -// } -//} +// Test case for the implicit Euler method on the exponential decay ODE. +TEST("AD Implicit Euler Method", ImplicitEuler_ExponentialDecaySensitivity) { + auto stepFn = [&]( + const VecX_T>& x, + DualNumber_T t, + DualNumber_T dt + ) { + return integrator.implicitEuler_AD(x, t, dt, expDecay, 8, DualNumber_T(1e-6, { 0.0 })); + }; + verifyExpDecaySensitivity(stepFn, 1e-2, 1000); +} +// Test case for the Implicit Midpoint method on the linear decay ODE. +TEST("AD Implicit Euler Method", ImplicitEuler_LinearDecay) { + VecX_T> x(1); + x(0) = DualNumber_T(1.0, { 0.0 }); + double T = 0.5; + DualNumber_T dt(T / 500.0); + DualNumber_T t(0.0); + for (int i = 0; i < 500; ++i) { + x = integrator.implicitEuler_AD(x, t, dt, linearDecay, 50, DualNumber_T(1e-6, { 0.0 })); + t += dt; + } + ASSERT_TRUE(std::abs(x(0).real - std::exp(-1.0)) < tol_low, "Implicit Euler linear decay error too large"); +} +// Test case for the implicit Euler method on the linear decay ODE. +TEST("AD Implicit Euler Method", ImplicitEuler_LinearDecaySensitivity) { + auto stepFn = [&]( + const VecX_T>& x, + DualNumber_T t, + DualNumber_T dt + ) { + return integrator.implicitEuler_AD(x, t, dt, expDecay, 8, DualNumber_T(1e-6, { 0.0 })); + }; + verifyExpDecaySensitivity(stepFn, 1e-2, 1000); +} +// Test case for the Implicit Euler method on the stiff decay ODE. +TEST("AD Implicit Euler Method", ImplicitEuler_StiffDecay) { + VecX_T> x(1); + x(0) = DualNumber_T(1.0, { 0.0 }); + double T = 0.5; + DualNumber_T dt(T / 500.0); + DualNumber_T t(0.0); + double expected = std::exp(-50.0); + auto stiff_f = [](auto t, const auto& x) { + auto dx = x; + dx(0) = -100.0 * x(0); + return dx; + }; + for (int i = 0; i < 500; ++i) { + x = integrator.implicitEuler_AD(x, t, dt, stiff_f, 50, DualNumber_T(1e-6, { 0.0 })); + t += dt; + } + ASSERT_TRUE(std::isfinite(x(0).real), "Implicit Euler produced a non-finite result."); + ASSERT_TRUE(x(0).real < 1e-12, "Implicit Euler did not sufficiently damp stiff mode"); +} +// Test case for the implicit Euler method on the stiff decay ODE. +TEST("AD Implicit Euler Method", ImplicitEuler_StiffDecaySensitivity) { + auto stepFn = [&]( + const VecX_T>& x, + DualNumber_T t, + DualNumber_T dt + ) { + return integrator.implicitEuler_AD(x, t, dt, stiffDecay, 50, DualNumber_T(1e-6, { 0.0 })); + }; + verifyExpDecaySensitivity(stepFn, 1e-2, 1000); +} +// Test large step performance of implicit midpoint +TEST("AD Implicit Euler Method", ImplicitEuler_Stability_LargeStep) { + VecX_T> x(1); + x(0) = DualNumber_T(1.0, { 1.0 }); + double T = 1.0; + DualNumber_T t(0.0); + DualNumber_T dt(1.0 / 10.0); + DualNumber_T prev = x(0); + for (int i = 0; i < 10; ++i) { + x = integrator.implicitEuler_AD(x, t, dt, stiffDecay, 50, DualNumber_T(1e-6, { 0.0 })); + t += dt; + ASSERT_TRUE(std::abs(x(0).real) < std::abs(prev.real), "Not monotone"); + ASSERT_TRUE(std::abs(x(0).dual[0]) < std::abs(prev.dual[0]), "Not strictly decaying"); + prev = x(0); + } +} -//// AD Implicit Midpoint Tests -//// Test case for the Implicit Midpoint method on the exponenital decay ODE. -//TEST("AD Implicit Midpoint Method", ImplicitMidpoint_ExponentialDecay) { -// VecX_T> x(1); -// x(0) = DualNumber_T(1.0, { 0.0 }); -// double T = 1.0; -// DualNumber_T dt(T / 1000.0, { 0.0 }); -// DualNumber_T t(0.0, { 0.0 }); -// for (int i = 0; i < 1000; ++i) { -// x = integrator.implicitMidpoint_AD(x, t, dt, expDecay, 8, DualNumber_T(1e-6, { 0.0 })); -// t += dt; -// } -// ASSERT_TRUE(std::abs(x(0).real - std::exp(-1.0)) < tol_low, "Implicit Midpoint exponential decay error too large"); -//} +// AD Implicit Midpoint Tests +// Test case for the Implicit Midpoint method on the exponenital decay ODE. +TEST("AD Implicit Midpoint Method", ImplicitMidpoint_ExponentialDecay) { + VecX_T> x(1); + x(0) = DualNumber_T(1.0, { 0.0 }); + double T = 1.0; + DualNumber_T dt(T / 1000.0, { 0.0 }); + DualNumber_T t(0.0, { 0.0 }); + for (int i = 0; i < 1000; ++i) { + x = integrator.implicitMidpoint_AD(x, t, dt, expDecay, 8, DualNumber_T(1e-6, { 0.0 })); + t += dt; + } + ASSERT_TRUE(std::abs(x(0).real - std::exp(-1.0)) < tol_low, "Implicit Midpoint exponential decay error too large"); +} //// Test case for the implicit midpoint method on the exponential decay ODE. //TEST("AD Implicit Midpoint Method", ImplicitMidpoint_ExponentialDecaySensitivity) { // auto stepFn = [&]( diff --git a/PxMLib/MathLib_Unit/DualNumberTests/DualNumber_Eigen_ArithmeticTests.cpp b/PxMLib/MathLib_Unit/DualNumberTests/DualNumber_Eigen_ArithmeticTests.cpp index 44f943c6..bb4ea197 100644 --- a/PxMLib/MathLib_Unit/DualNumberTests/DualNumber_Eigen_ArithmeticTests.cpp +++ b/PxMLib/MathLib_Unit/DualNumberTests/DualNumber_Eigen_ArithmeticTests.cpp @@ -60,14 +60,4 @@ TEST("DualNumber_T Eigen Arithmetic", DotProduct) { Dual dotProduct = a.dot(b); ASSERT_TRUE(dotProduct.real == 11.0 && dotProduct.dual[0] == 17.0, "The dot product should be the sum of the products of corresponding elements"); -} - -//TEST("DualNumber_T Eigen Arithmetic", MatrixScalarMultiplication) { -// Eigen::Matrix A; -// A(0, 0) = Dual(1.0, { 0.5 }); -// A(1, 1) = Dual(2.0, { 1.5 }); -// double scalar = 2.0; -// Eigen::Matrix B = A * scalar; -// ASSERT_TRUE(B(0, 0).real == 2.0 && B(0, 0).dual[0] == 1.0, "B(0, 0) should be the product of A and scalar"); -// ASSERT_TRUE(B(1, 1).real == 4.0 && B(1, 1).dual[0] == 3.0, "B(1, 1) should be the product of A and scalar"); -//} \ No newline at end of file +} \ No newline at end of file From 6ce33662d23bd4684f6f7e9123454653c5badb7f Mon Sep 17 00:00:00 2001 From: saltyjoss Date: Sat, 23 May 2026 08:27:22 +0100 Subject: [PATCH 049/210] refactor(WIP): Updated AD Jacobian and enable full implicit midpoint tests * Refactored the AD Jacobian calculation for implicit integrators, improving type safety and correctness by templating on scalar type and variable count. TODO: * Need to move to fullPivLu now as I believe that will better compute and decompose the matrix of Reals --- .../integrators/ad_implicit_integrators.inl | 48 ++-- .../integrators/numerical_integrators.h | 9 +- .../ADImplicitIntegratorTests.cpp | 220 +++++++++--------- 3 files changed, 141 insertions(+), 136 deletions(-) diff --git a/PxMLib/MathLib/include/integrators/ad_implicit_integrators.inl b/PxMLib/MathLib/include/integrators/ad_implicit_integrators.inl index 09210f84..9362e99b 100644 --- a/PxMLib/MathLib/include/integrators/ad_implicit_integrators.inl +++ b/PxMLib/MathLib/include/integrators/ad_implicit_integrators.inl @@ -58,17 +58,24 @@ namespace integration { mathlib::MatX_T F; bool analytical_success = false; - F = automaticDifferenceJacobian( - [&](auto t_pert, const auto& x_pert) { + auto perturbation_func = [&](auto t_pert, const auto& x_pert) { using Dual = std::decay_t; - mathlib::VecX_T x_mid = (x.template cast() + x_pert) / Dual(2); - Dual t_mid = (t_pert + dt) / Dual(2); - return f(t_mid, x_mid); - }, - t + dt / Real(2), + auto x_real = x.template cast(); + mathlib::VecX_T x_cast = x_real.template cast(); + mathlib::VecX_T x_mid = (x_cast + x_pert) / Dual(2); + Dual t_mid = (t_pert + Dual(static_cast(dt))) / Dual(2); + auto f_eval = f(t_mid, x_mid); + return f_eval.template cast(); + }; + + F = automaticDifferenceJacobian( + perturbation_func, + t, x_guess ); - J_out = (mathlib::MatX_T::Identity(n, n) - Scalar(0.5) * dt * F).template cast(); // J = I - dt * df/dx + + Real dt_real = static_cast(dt); + J_out = mathlib::MatX_T::Identity(n, n) - Real(0.5) * dt_real * F; // J = I - dt * df/dx }; mathlib::VecX_T x0 = x + dt * f((t + dt) / Scalar(2), x); // Initial guess for Newton-Raphson @@ -102,7 +109,7 @@ namespace integration { Scalar(0.25) + sqrt(Scalar(3)) / Scalar(6), Scalar(0.25); const Scalar b = Scalar(0.5); // Weights for final update - + // Initial guess for the stage values k1, k2, k3 mathlib::VecX_T k(2 * n); // 2 stages mathlib::VecX_T f0 = f(t, x); @@ -335,30 +342,27 @@ namespace integration { } // Automatic Difference Jacobian - template - mathlib::MatX_T::BaseScalar> NumericalIntegrator::automaticDifferenceJacobian( + template + mathlib::MatX_T NumericalIntegrator::automaticDifferenceJacobian( Func&& f, - Scalar t, - const mathlib::VecX_T& x + mathlib::DualNumber_T t, + const mathlib::VecX_T>& x ) { - using RealScalar = typename mathlib::DualTraits::BaseScalar; - const int n = static_cast(x.size()); + + using SysScalar = mathlib::DualNumber_T; auto f0 = f(t, x); const int m = static_cast(f0.size()); mathlib::MatX_T J(m, n); for (int i = 0; i < n; ++i) { - using Dual_T = mathlib::DualNumber_T; + using Dual_T = mathlib::DualNumber_T; mathlib::VecX_T x_dual(n); for (int k = 0; k < n; ++k) { - x_dual(k) = Dual_T( - mathlib::real(x(k)), - std::array{ - (k == i) ? RealScalar(1) : RealScalar(0) - } - ); + std::array seed_array{}; + if (k == 1) { seed_array.fill(RealScalar(0)); seed_array[0] = RealScalar(1); } + x_dual(k) = Dual_T(mathlib::real(x(k)), seed_array); } Dual_T t_dual(mathlib::real(t)); diff --git a/PxMLib/MathLib/include/integrators/numerical_integrators.h b/PxMLib/MathLib/include/integrators/numerical_integrators.h index 7ba54139..46c23b82 100644 --- a/PxMLib/MathLib/include/integrators/numerical_integrators.h +++ b/PxMLib/MathLib/include/integrators/numerical_integrators.h @@ -9,6 +9,7 @@ #include #include #include +#include // Numerical integration methods namespace integration { @@ -189,11 +190,11 @@ namespace integration { const mathlib::VecX_T& x ); - template - mathlib::MatX_T::BaseScalar> automaticDifferenceJacobian( + template + mathlib::MatX_T automaticDifferenceJacobian( Func&& f, - Scalar t, - const mathlib::VecX_T& x + mathlib::DualNumber_T t, + const mathlib::VecX_T>& x ); }; }// namespace integration diff --git a/PxMLib/MathLib_Unit/AutomaticDifferentiationTests/ADImplicitIntegratorTests.cpp b/PxMLib/MathLib_Unit/AutomaticDifferentiationTests/ADImplicitIntegratorTests.cpp index d6e8457b..70dbf3f2 100644 --- a/PxMLib/MathLib_Unit/AutomaticDifferentiationTests/ADImplicitIntegratorTests.cpp +++ b/PxMLib/MathLib_Unit/AutomaticDifferentiationTests/ADImplicitIntegratorTests.cpp @@ -237,116 +237,116 @@ TEST("AD Implicit Midpoint Method", ImplicitMidpoint_ExponentialDecay) { } ASSERT_TRUE(std::abs(x(0).real - std::exp(-1.0)) < tol_low, "Implicit Midpoint exponential decay error too large"); } -//// Test case for the implicit midpoint method on the exponential decay ODE. -//TEST("AD Implicit Midpoint Method", ImplicitMidpoint_ExponentialDecaySensitivity) { -// auto stepFn = [&]( -// const VecX_T>& x, -// DualNumber_T t, -// DualNumber_T dt -// ) { -// return integrator.implicitMidpoint_AD(x, t, dt, expDecay, 8, DualNumber_T(1e-6, { 0.0 })); -// }; -// verifyExpDecaySensitivity(stepFn, 1e-3, 1000); -//} -//// Test case for the Implicit Midpoint method on the harmonic oscillator ODE. -//TEST("AD Implicit Midpoint Method", ImplicitMidpoint_HarmonicOscillator_EnergyPreservation) { -// VecX_T> x(2); -// x(0) = DualNumber_T(1.0, { 0.0, 0.0 }); -// x(1) = DualNumber_T(0.0, { 0.0, 0.0 }); -// double E0 = 0.5 * (x(0).real * x(0).real + x(1).real * x(1).real); -// double T = 5.0; -// DualNumber_T dt(T / 2500.0); -// DualNumber_T t(0.0); -// for (int i = 0; i < 2500; ++i) { -// x = integrator.implicitMidpoint_AD(x, t, dt, harmonicOsc, 10, DualNumber_T(1e-7, { 0.0, 0.0 })); -// t += dt; -// } -// double Ef = 0.5 * (x(0).real * x(0).real + x(1).real * x(1).real); -// double drift = std::abs(Ef - E0); -// ASSERT_TRUE(drift < tol_high * (1.0 + E0), "Implicit Midpoint energy drift too large for SHO"); -//} -//// Test case for the implicit midpoint method on the harmonic oscillator ODE. -//TEST("AD Implicit Midpoint Method", ImplicitMidpoint_HarmonicOscillatorSensitivity) { -// auto stepFn = [&]( -// const VecX_T>& x, -// DualNumber_T t, -// DualNumber_T dt -// ) { -// return integrator.implicitMidpoint_AD(x, t, dt, harmonicOsc, 10, DualNumber_T(1e-7, { 0.0, 0.0 })); -// }; -// verifyHarmonicOscillatorSensitivity(stepFn, 1e-3, 1000); -//} -//// Test case for the Implicit Midpoint method on the linear decay ODE. -//TEST("AD Implicit Midpoint Method", ImplicitMidpoint_LinearDecay) { -// VecX_T> x(1); -// x(0) = DualNumber_T(1.0, { 0.0 }); -// double T = 0.5; -// DualNumber_T dt(T / 500.0); -// DualNumber_T t(0.0); -// for (int i = 0; i < 500; ++i) { -// x = integrator.implicitMidpoint_AD(x, t, dt, linearDecay, 10, DualNumber_T(1e-6, { 0.0 })); -// t += dt; -// } -// ASSERT_TRUE(std::abs(x(0).real - std::exp(-1.0)) < tol_low, "Implicit Midpoint linear decay error too large"); -//} -//// Test case for the implicit midpoint method on the linear decay ODE. -//TEST("AD Implicit Midpoint Method", ImplicitMidpoint_LinearDecaySensitivity) { -// auto stepFn = [&]( -// const VecX_T>& x, -// DualNumber_T t, -// DualNumber_T dt -// ) { -// return integrator.implicitMidpoint_AD(x, t, dt, linearDecay, 10, DualNumber_T(1e-6, { 0.0 })); -// }; -// verifyExpDecaySensitivity(stepFn, 1e-3, 1000); -//} -//// Test case for the Implicit Midpoint method on the stiff decay ODE. -//TEST("AD Implicit Midpoint Method", ImplicitMidpoint_StiffDecay) { -// VecX_T> x(1); -// x(0) = DualNumber_T(1.0, { 0.0 }); -// double T = 0.5; -// DualNumber_T dt(T / 500.0); -// DualNumber_T t(0.0); -// double expected = std::exp(-50.0); -// auto stiff_f = [](auto t, const auto& x) { // why is this necessary? Why can't I just pass stiffDecay directly? the answer is that the template parameters can't be deduced from the function pointer, but they can be deduced from the -// auto dx = x; -// dx(0) = -100.0 * x(0); -// return dx; -// }; -// for (int i = 0; i < 500; ++i) { -// x = integrator.implicitMidpoint_AD(x, t, dt, stiffDecay, 10, DualNumber_T(1e-6, { 0.0 })); -// t += dt; -// } -// ASSERT_TRUE(std::isfinite(x(0).real), "Implicit Midpoint produced a non-finite result."); -// ASSERT_TRUE(x(0).real < 1e-12, "Implicit Midpoint did not sufficiently damp stiff mode"); -//} -//// Test case for the implicit midpoint method on the stiff decay ODE. -//TEST("AD Implicit Midpoint Method", ImplicitMidpoint_StiffDecaySensitivity) { -// auto stepFn = [&]( -// const VecX_T>& x, -// DualNumber_T t, -// DualNumber_T dt -// ) { -// return integrator.implicitMidpoint_AD(x, t, dt, stiffDecay, 10, DualNumber_T(1e-6, { 0.0 })); -// }; -// verifyExpDecaySensitivity(stepFn, 1e-3, 1000); -//} -//// Test large step performance of implicit midpoint -//TEST("AD Implicit Midpoint Method", ImplicitMidpoint_Stability_LargeStep) { -// VecX_T> x(1); -// x(0) = DualNumber_T(1.0, { 1.0 }); -// double T = 1.0; -// DualNumber_T t(0.0); -// DualNumber_T dt(1.0 / 10.0); -// DualNumber_T prev = x(0); -// for (int i = 0; i < 10; ++i) { -// x = integrator.implicitMidpoint_AD(x, t, dt, stiffDecay, 10, DualNumber_T(1e-6, { 0.0 })); -// t += dt; -// ASSERT_TRUE(std::abs(x(0).real) < std::abs(prev.real), "Not monotone"); -// ASSERT_TRUE(std::abs(x(0).dual[0]) < std::abs(prev.dual[0]), "Not strictly decaying"); -// prev = x(0); -// } -//} +// Test case for the implicit midpoint method on the exponential decay ODE. +TEST("AD Implicit Midpoint Method", ImplicitMidpoint_ExponentialDecaySensitivity) { + auto stepFn = [&]( + const VecX_T>& x, + DualNumber_T t, + DualNumber_T dt + ) { + return integrator.implicitMidpoint_AD(x, t, dt, expDecay, 8, DualNumber_T(1e-6, { 0.0 })); + }; + verifyExpDecaySensitivity(stepFn, 1e-3, 1000); +} +// Test case for the Implicit Midpoint method on the harmonic oscillator ODE. +TEST("AD Implicit Midpoint Method", ImplicitMidpoint_HarmonicOscillator_EnergyPreservation) { + VecX_T> x(2); + x(0) = DualNumber_T(1.0, { 0.0, 0.0 }); + x(1) = DualNumber_T(0.0, { 0.0, 0.0 }); + double E0 = 0.5 * (x(0).real * x(0).real + x(1).real * x(1).real); + double T = 5.0; + DualNumber_T dt(T / 2500.0); + DualNumber_T t(0.0); + for (int i = 0; i < 2500; ++i) { + x = integrator.implicitMidpoint_AD(x, t, dt, harmonicOsc, 10, DualNumber_T(1e-7, { 0.0, 0.0 })); + t += dt; + } + double Ef = 0.5 * (x(0).real * x(0).real + x(1).real * x(1).real); + double drift = std::abs(Ef - E0); + ASSERT_TRUE(drift < tol_high * (1.0 + E0), "Implicit Midpoint energy drift too large for SHO"); +} +// Test case for the implicit midpoint method on the harmonic oscillator ODE. +TEST("AD Implicit Midpoint Method", ImplicitMidpoint_HarmonicOscillatorSensitivity) { + auto stepFn = [&]( + const VecX_T>& x, + DualNumber_T t, + DualNumber_T dt + ) { + return integrator.implicitMidpoint_AD(x, t, dt, harmonicOsc, 10, DualNumber_T(1e-7, { 0.0, 0.0 })); + }; + verifyHarmonicOscillatorSensitivity(stepFn, 1e-3, 1000); +} +// Test case for the Implicit Midpoint method on the linear decay ODE. +TEST("AD Implicit Midpoint Method", ImplicitMidpoint_LinearDecay) { + VecX_T> x(1); + x(0) = DualNumber_T(1.0, { 0.0 }); + double T = 0.5; + DualNumber_T dt(T / 500.0); + DualNumber_T t(0.0); + for (int i = 0; i < 500; ++i) { + x = integrator.implicitMidpoint_AD(x, t, dt, linearDecay, 10, DualNumber_T(1e-6, { 0.0 })); + t += dt; + } + ASSERT_TRUE(std::abs(x(0).real - std::exp(-1.0)) < tol_low, "Implicit Midpoint linear decay error too large"); +} +// Test case for the implicit midpoint method on the linear decay ODE. +TEST("AD Implicit Midpoint Method", ImplicitMidpoint_LinearDecaySensitivity) { + auto stepFn = [&]( + const VecX_T>& x, + DualNumber_T t, + DualNumber_T dt + ) { + return integrator.implicitMidpoint_AD(x, t, dt, linearDecay, 10, DualNumber_T(1e-6, { 0.0 })); + }; + verifyExpDecaySensitivity(stepFn, 1e-3, 1000); +} +// Test case for the Implicit Midpoint method on the stiff decay ODE. +TEST("AD Implicit Midpoint Method", ImplicitMidpoint_StiffDecay) { + VecX_T> x(1); + x(0) = DualNumber_T(1.0, { 0.0 }); + double T = 0.5; + DualNumber_T dt(T / 500.0); + DualNumber_T t(0.0); + double expected = std::exp(-50.0); + auto stiff_f = [](auto t, const auto& x) { // why is this necessary? Why can't I just pass stiffDecay directly? the answer is that the template parameters can't be deduced from the function pointer, but they can be deduced from the + auto dx = x; + dx(0) = -100.0 * x(0); + return dx; + }; + for (int i = 0; i < 500; ++i) { + x = integrator.implicitMidpoint_AD(x, t, dt, stiffDecay, 10, DualNumber_T(1e-6, { 0.0 })); + t += dt; + } + ASSERT_TRUE(std::isfinite(x(0).real), "Implicit Midpoint produced a non-finite result."); + ASSERT_TRUE(x(0).real < 1e-12, "Implicit Midpoint did not sufficiently damp stiff mode"); +} +// Test case for the implicit midpoint method on the stiff decay ODE. +TEST("AD Implicit Midpoint Method", ImplicitMidpoint_StiffDecaySensitivity) { + auto stepFn = [&]( + const VecX_T>& x, + DualNumber_T t, + DualNumber_T dt + ) { + return integrator.implicitMidpoint_AD(x, t, dt, stiffDecay, 10, DualNumber_T(1e-6, { 0.0 })); + }; + verifyExpDecaySensitivity(stepFn, 1e-3, 1000); +} +// Test large step performance of implicit midpoint +TEST("AD Implicit Midpoint Method", ImplicitMidpoint_Stability_LargeStep) { + VecX_T> x(1); + x(0) = DualNumber_T(1.0, { 1.0 }); + double T = 1.0; + DualNumber_T t(0.0); + DualNumber_T dt(1.0 / 10.0); + DualNumber_T prev = x(0); + for (int i = 0; i < 10; ++i) { + x = integrator.implicitMidpoint_AD(x, t, dt, stiffDecay, 10, DualNumber_T(1e-6, { 0.0 })); + t += dt; + ASSERT_TRUE(std::abs(x(0).real) < std::abs(prev.real), "Not monotone"); + ASSERT_TRUE(std::abs(x(0).dual[0]) < std::abs(prev.dual[0]), "Not strictly decaying"); + prev = x(0); + } +} //// AD GLRK2 Tests //// Test case for the GLRK2 method on the exponenital decay ODE. From c835ebe9c448de6827ad03251c82095cd7c9b7ec Mon Sep 17 00:00:00 2001 From: saltyjoss Date: Sat, 23 May 2026 08:39:55 +0100 Subject: [PATCH 050/210] feat: Added FullPivLU unit test to `DualNumber_Eigen_SolverTests` --- PxMLib/MathLib_Unit/Common/TestHarness.h | 2 +- .../DualNumber_Eigen_SolverTests.cpp | 43 +++++++++++++++---- 2 files changed, 35 insertions(+), 10 deletions(-) diff --git a/PxMLib/MathLib_Unit/Common/TestHarness.h b/PxMLib/MathLib_Unit/Common/TestHarness.h index f8d44875..d3de4c46 100644 --- a/PxMLib/MathLib_Unit/Common/TestHarness.h +++ b/PxMLib/MathLib_Unit/Common/TestHarness.h @@ -96,4 +96,4 @@ namespace test { #define ASSERT_TRUE(cond, msg) test::assertTrue((cond), (msg)) #define ASSERT_EQ(a, b, tol, msg) test::assertTrue(std::abs((a) - (b)) < (tol), (msg) #define SUCCEED() return -#define EXPECT_NEAR(a, b, tol) test::assertTrue(std::abs((a) - (b)) < (tol), "Expected " #a " and " #b " to be within " #tol) \ No newline at end of file +#define ASSERT_NEAR(a, b, tol) test::assertTrue(std::abs((a) - (b)) < (tol), "Expected " #a " and " #b " to be within " #tol) \ No newline at end of file diff --git a/PxMLib/MathLib_Unit/DualNumberTests/DualNumber_Eigen_SolverTests.cpp b/PxMLib/MathLib_Unit/DualNumberTests/DualNumber_Eigen_SolverTests.cpp index e9fa4f08..0076132f 100644 --- a/PxMLib/MathLib_Unit/DualNumberTests/DualNumber_Eigen_SolverTests.cpp +++ b/PxMLib/MathLib_Unit/DualNumberTests/DualNumber_Eigen_SolverTests.cpp @@ -45,6 +45,18 @@ TEST("DualNumber_T Eigen Solvers", PartialPivLU) { ASSERT_TRUE(std::isfinite(x(0).real), "Real part of solution should be finite"); ASSERT_TRUE(std::isfinite(x(0).dual[0]), "Dual part of solution should be finite"); } + +TEST("DualNumber_T Eigen Solvers", LDLTComputeOnly) { + Eigen::Matrix A; + A << + Dual(4.0), Dual(1.0), + Dual(1.0), Dual(3.0); + + Eigen::LDLT> ldlt; + ldlt.compute(A); + SUCCEED(); +} + TEST("DualNumber_T Eigen Solvers", LDLT) { Eigen::Matrix A; Eigen::Matrix b; @@ -59,16 +71,29 @@ TEST("DualNumber_T Eigen Solvers", LDLT) { ASSERT_TRUE(std::isfinite(x(0).dual[0]), "Dual part of solution should be finite"); } -TEST("DualNumber_T Eigen Solvers", LDLTComputeOnly) { - Eigen::Matrix A; +TEST("DualNumber_T Eigen Solvers", FullPivLUComputeAndSolve_DecompWithDualCasts) { + using Dual = DualNumber_T; - A << - Dual(4.0), Dual(1.0), - Dual(1.0), Dual(3.0); + mathlib::MatX_T J(2, 2); + J << + 1.0, 1000.0, + 0.0, 1.0; + mathlib::VecX_T b(2); + b << -5.0, 2.0; - Eigen::LDLT> ldlt; + Eigen::FullPivLU> solver; + solver.compute(J); + ASSERT_TRUE(solver.isInvertible(), "FullPivLU failed to invert the system matrix."); - ldlt.compute(A); + mathlib::VecX_T delta = solver.solve(b); + // x + 1000y = -5 + ASSERT_NEAR(delta(0), -2005.0, 1e-7); // x = -2005 + ASSERT_NEAR(delta(1), 2.0, 1e-7); // y = 2 - SUCCEED(); -} \ No newline at end of file + mathlib::VecX_T x(2); + x(0) = Dual(10.0, { 1.0, 0.0 }); + x(1) = Dual(5.0, { 0.0, 1.0 }); + x += delta.template cast(); + ASSERT_NEAR(x(0).real , -1995.0, 1e-7); + ASSERT_NEAR(x(1).real, 7.0, 1e-7); +} From f739c592de32fc749558c796c582b772bc411808 Mon Sep 17 00:00:00 2001 From: saltyjoss Date: Sat, 23 May 2026 09:09:47 +0100 Subject: [PATCH 051/210] refactor: Moved `newtonRaphson_AD` over to a FullPivLU solver, isolating the root finding part to the type Real --- PxMLib/MathLib/include/core/DualNumbers.h | 5 +- .../integrators/ad_implicit_integrators.inl | 52 ++++++++++++++----- 2 files changed, 41 insertions(+), 16 deletions(-) diff --git a/PxMLib/MathLib/include/core/DualNumbers.h b/PxMLib/MathLib/include/core/DualNumbers.h index 8be62c54..4dad9a62 100644 --- a/PxMLib/MathLib/include/core/DualNumbers.h +++ b/PxMLib/MathLib/include/core/DualNumbers.h @@ -48,9 +48,8 @@ namespace mathlib { dual.begin() ); } - explicit operator Scalar() const { return real; } - + static constexpr size_t Dimension = NVar; Scalar real; std::array dual; }; @@ -476,12 +475,14 @@ namespace mathlib { struct DualTraits { using BaseScalar = T; static constexpr bool is_dual = false; + static constexpr size_t Dimension = 1; }; // Specialization of DualTraits for DualNumber_T template struct DualTraits> { using BaseScalar = Scalar; static constexpr bool is_dual = true; + static constexpr size_t Dimension = NVar; }; // Dual Part template diff --git a/PxMLib/MathLib/include/integrators/ad_implicit_integrators.inl b/PxMLib/MathLib/include/integrators/ad_implicit_integrators.inl index 9362e99b..94530706 100644 --- a/PxMLib/MathLib/include/integrators/ad_implicit_integrators.inl +++ b/PxMLib/MathLib/include/integrators/ad_implicit_integrators.inl @@ -319,26 +319,50 @@ namespace integration { int maxIter, Scalar tol ) { - mathlib::VecX_T x = x0, g, x_trial; - using BaseScalar = typename mathlib::DualTraits::BaseScalar; using Real = typename mathlib::DualTraits::BaseScalar; - mathlib::VecX_T delta = mathlib::VecX_T::Constant(x0.size(), Real(std::numeric_limits::infinity())); - mathlib::MatX_T J; - Eigen::PartialPivLU> solver; + const Eigen::Index n = x0.size(); + + mathlib::VecX_T x_real = x0.template cast(); + mathlib::VecX_T g_real(n); + mathlib::VecX_T delta(n); + mathlib::MatX_T J(n, n); + Eigen::FullPivLU> solver; + bool converged = false; + for (int iter = 0; iter < maxIter; ++iter) { - eval_g(x, g); - if (!g.allFinite()) { throw std::runtime_error("Newton received non-finite residual at iter = " + std::to_string(iter) + ", residual norm = " + std::to_string(g.norm())); } - if (g.norm() < tol) { return x; } - eval_j(x, J); + mathlib::VecX_T g_dual; + eval_g(x_real.template cast(), g_dual); + g_real = g_dual.template cast(); + if (!g_real.allFinite()) { throw std::runtime_error("Newton received non-finite residual at iter = " + std::to_string(iter)); } + if (g_real.norm() < static_cast(tol)) { converged = true; break; } + eval_j(x_real.template cast(), J); if (!J.allFinite()) { throw std::runtime_error("Newton received non-finite Jacobian at iter = " + std::to_string(iter)); } solver.compute(J); - delta = solver.solve((-g).template cast()); + delta = solver.solve(-g_real); if (!delta.allFinite()) { throw std::runtime_error("Newton produced non-finite step at iter = " + std::to_string(iter)); } - x += delta.template cast(); - if (delta.norm() < tol * (Scalar(1) + x.norm())) { return x; } - if (!x.allFinite()) { throw std::runtime_error("Newton state became non-finite at iter = " + std::to_string(iter)); } + x_real += delta; + Real x_norm = x_real.norm(); + if (delta.norm() < static_cast(tol) * (Real(1) + x_norm)) { converged = true; break; } + } + + if (!converged) { throw std::runtime_error("Newton-Raphson failed to converge after " + std::to_string(maxIter) + " iterations."); } + + mathlib::VecX_T x_final = x_real.template cast(); + eval_j(x_final, J); + solver.compute(J); + mathlib::VecX_T g_final; + eval_g(x0, g_final); + + constexpr size_t NVar = mathlib::DualTraits::Dimension; + for (Eigen::Index i = 0; i < n; ++i) { + for (size_t d = 0; d < NVar; ++d) { + mathlib::VecX_T rhs_seed(n); + for (Eigen::Index j = 0; j < n; ++j) { rhs_seed(j) = g_final(j).dual[d]; } + mathlib::VecX_T corrected_sensitivies = solver.solve(-rhs_seed); + x_final(i).dual[d] = corrected_sensitivies(i); + } } - throw std::runtime_error("Newton-Raphson failed to converge after " + std::to_string(maxIter) + " iterations. Final residual norm = " + std::to_string(g.norm()) + ", final step norm = " + std::to_string(delta.norm())); + return x_final; } // Automatic Difference Jacobian From b63c6a98c1e93ca862c74479e113265907ca8828 Mon Sep 17 00:00:00 2001 From: saltyjoss Date: Sat, 23 May 2026 10:19:33 +0100 Subject: [PATCH 052/210] Refactor AD implicit integrators for real-valued tolerance * Updated AD implicit integrator methods and AD Newton-Raphson solver to use real-valued (BaseScalar) tolerance parameters, ensuring convergence checks are performed on real parts only. * Updated Jacobian computation for clarity and optimised the sensitivity update loop for efficiency. NOTE: * These changes, whilst correcting small bugs, are not solvign the current problem of AD implicit unit tests for sensitivity unconditionally failing. * I am going to move onto getting AD GLRK2/3 ready for unit testing so I can determine whether its a universal problem OR its a low-order implicit problem with AD. --- .../integrators/ad_implicit_integrators.inl | 57 +++------ .../integrators/numerical_integrators.h | 10 +- .../ADImplicitIntegratorTests.cpp | 108 +++++++++++------- 3 files changed, 92 insertions(+), 83 deletions(-) diff --git a/PxMLib/MathLib/include/integrators/ad_implicit_integrators.inl b/PxMLib/MathLib/include/integrators/ad_implicit_integrators.inl index 94530706..afab0dae 100644 --- a/PxMLib/MathLib/include/integrators/ad_implicit_integrators.inl +++ b/PxMLib/MathLib/include/integrators/ad_implicit_integrators.inl @@ -10,7 +10,7 @@ namespace integration { Scalar dt, Func&& f, int maxIter, - Scalar tol + typename mathlib::DualTraits::BaseScalar tol ) { using Real = typename mathlib::DualTraits::BaseScalar; @@ -20,19 +20,15 @@ namespace integration { auto J = [&](const mathlib::VecX_T& x_guess, mathlib::MatX_T& J_out) { int n = (int)x_guess.size(); mathlib::MatX_T F; - F = automaticDifferenceJacobian( - [&](auto t_pert, const auto& x_pert) { + auto perturbation_func = [&](auto t_pert, const auto& x_pert) { using Dual = std::decay_t; Dual t_eval = Dual(t_pert) + Dual(dt); - return f(t_eval, x_pert); - }, - t + dt, - x_guess - ); - - J_out = mathlib::MatX_T::Identity(n, n) - mathlib::real(dt) * F; // J = I - dt * df/dx + return f(t_eval, x_pert); + }; + F = automaticDifferenceJacobian(perturbation_func, t + dt, x_guess); + Real dt_real = static_cast(dt); + J_out = mathlib::MatX_T::Identity(n, n) - dt_real * F; // J = I - dt * df/dx }; - mathlib::VecX_T x0 = x + dt * f(t + dt, x); // Initial guess for Newton-Raphson return newtonRaphson_AD(g, J, x0, maxIter, tol); } @@ -45,19 +41,16 @@ namespace integration { Scalar dt, Func&& f, int maxIter, - Scalar tol + typename mathlib::DualTraits::BaseScalar tol ) { using Real = typename mathlib::DualTraits::BaseScalar; // The function g(x_guess) = 0 that we want to solve for the implicit midpoint step auto g = [&](const mathlib::VecX_T& x_guess, mathlib::VecX_T& g_out) { g_out = x_guess - x - dt * f((t + dt) / Scalar(2), (x + x_guess) / Scalar(2)); }; - // Numerical Jacobian for Newton-Raphson auto J = [&](const mathlib::VecX_T& x_guess, mathlib::MatX_T& J_out) { int n = (int)x_guess.size(); mathlib::MatX_T F; - bool analytical_success = false; - auto perturbation_func = [&](auto t_pert, const auto& x_pert) { using Dual = std::decay_t; auto x_real = x.template cast(); @@ -67,17 +60,10 @@ namespace integration { auto f_eval = f(t_mid, x_mid); return f_eval.template cast(); }; - - F = automaticDifferenceJacobian( - perturbation_func, - t, - x_guess - ); - + F = automaticDifferenceJacobian(perturbation_func, t, x_guess); Real dt_real = static_cast(dt); J_out = mathlib::MatX_T::Identity(n, n) - Real(0.5) * dt_real * F; // J = I - dt * df/dx }; - mathlib::VecX_T x0 = x + dt * f((t + dt) / Scalar(2), x); // Initial guess for Newton-Raphson return newtonRaphson_AD(g, J, x0, maxIter, tol); } @@ -90,11 +76,10 @@ namespace integration { Scalar dt, Func&& f, int maxIter, - Scalar tol + typename mathlib::DualTraits::BaseScalar tol ) { using Real = typename mathlib::DualTraits::BaseScalar; using std::sqrt; - const Eigen::Index n = x.size(); // Coefficients for the 2-stage Gauss-Legendre method (4th order) @@ -102,12 +87,10 @@ namespace integration { c << Scalar(0.5) - sqrt(Scalar(3)) / Scalar(6), Scalar(0.5) + sqrt(Scalar(3)) / Scalar(6); // Stage time fractions - mathlib::MatX_T A(2, 2); A << Scalar(0.25), Scalar(0.25) - sqrt(Scalar(3)) / Scalar(6), Scalar(0.25) + sqrt(Scalar(3)) / Scalar(6), Scalar(0.25); - const Scalar b = Scalar(0.5); // Weights for final update // Initial guess for the stage values k1, k2, k3 @@ -185,7 +168,7 @@ namespace integration { Scalar dt, Func&& f, int maxIter, - Scalar tol + typename mathlib::DualTraits::BaseScalar tol ) { using Real = typename mathlib::DualTraits::BaseScalar; using std::sqrt; @@ -317,7 +300,7 @@ namespace integration { EvalJ&& eval_j, mathlib::VecX_T x0, int maxIter, - Scalar tol + typename mathlib::DualTraits::BaseScalar tol ) { using Real = typename mathlib::DualTraits::BaseScalar; const Eigen::Index n = x0.size(); @@ -334,7 +317,7 @@ namespace integration { eval_g(x_real.template cast(), g_dual); g_real = g_dual.template cast(); if (!g_real.allFinite()) { throw std::runtime_error("Newton received non-finite residual at iter = " + std::to_string(iter)); } - if (g_real.norm() < static_cast(tol)) { converged = true; break; } + if (g_real.norm() < tol) { converged = true; break; } eval_j(x_real.template cast(), J); if (!J.allFinite()) { throw std::runtime_error("Newton received non-finite Jacobian at iter = " + std::to_string(iter)); } solver.compute(J); @@ -342,7 +325,7 @@ namespace integration { if (!delta.allFinite()) { throw std::runtime_error("Newton produced non-finite step at iter = " + std::to_string(iter)); } x_real += delta; Real x_norm = x_real.norm(); - if (delta.norm() < static_cast(tol) * (Real(1) + x_norm)) { converged = true; break; } + if (delta.norm() < tol * (Real(1) + x_norm)) { converged = true; break; } } if (!converged) { throw std::runtime_error("Newton-Raphson failed to converge after " + std::to_string(maxIter) + " iterations."); } @@ -354,13 +337,11 @@ namespace integration { eval_g(x0, g_final); constexpr size_t NVar = mathlib::DualTraits::Dimension; - for (Eigen::Index i = 0; i < n; ++i) { - for (size_t d = 0; d < NVar; ++d) { - mathlib::VecX_T rhs_seed(n); - for (Eigen::Index j = 0; j < n; ++j) { rhs_seed(j) = g_final(j).dual[d]; } - mathlib::VecX_T corrected_sensitivies = solver.solve(-rhs_seed); - x_final(i).dual[d] = corrected_sensitivies(i); - } + for (size_t d = 0; d < NVar; ++d) { + mathlib::VecX_T rhs_seed(n); + for (Eigen::Index j = 0; j < n; ++j) { rhs_seed(j) = g_final(j).dual[d]; } + mathlib::VecX_T corrected_sensitivies = solver.solve(-rhs_seed); + for (Eigen::Index i = 0; i < n; ++i) { x_final(i).dual[d] = corrected_sensitivies(i); } } return x_final; } diff --git a/PxMLib/MathLib/include/integrators/numerical_integrators.h b/PxMLib/MathLib/include/integrators/numerical_integrators.h index 46c23b82..5f351c68 100644 --- a/PxMLib/MathLib/include/integrators/numerical_integrators.h +++ b/PxMLib/MathLib/include/integrators/numerical_integrators.h @@ -92,7 +92,7 @@ namespace integration { Scalar dt, Func&& f, int maxIter, - Scalar tol + typename mathlib::DualTraits::BaseScalar tol ); // Implicit Midpoint method @@ -114,7 +114,7 @@ namespace integration { Scalar dt, Func&& f, int maxIter, - Scalar tol + typename mathlib::DualTraits::BaseScalar tol ); // Gauss-Legendre Runge-Kutta method (2 stages, 4th order) @@ -136,7 +136,7 @@ namespace integration { Scalar dt, Func&& f, int maxIter, - Scalar tol + typename mathlib::DualTraits::BaseScalar tol ); // Gauss-Legendre Runge-Kutta method (3 stages, 6th order) @@ -158,7 +158,7 @@ namespace integration { Scalar dt, Func&& f, int maxIter, - Scalar tol + typename mathlib::DualTraits::BaseScalar tol ); private: @@ -179,7 +179,7 @@ namespace integration { EvalJ&& eval_j, mathlib::VecX_T x0, int maxIter, - Scalar tol + typename mathlib::DualTraits::BaseScalar tol ); // Finite difference approximation of the Jacobian matrix df/dx for a vector-valued function f: R^n -> R^m at a point x diff --git a/PxMLib/MathLib_Unit/AutomaticDifferentiationTests/ADImplicitIntegratorTests.cpp b/PxMLib/MathLib_Unit/AutomaticDifferentiationTests/ADImplicitIntegratorTests.cpp index 70dbf3f2..23a4647a 100644 --- a/PxMLib/MathLib_Unit/AutomaticDifferentiationTests/ADImplicitIntegratorTests.cpp +++ b/PxMLib/MathLib_Unit/AutomaticDifferentiationTests/ADImplicitIntegratorTests.cpp @@ -94,12 +94,20 @@ namespace { double tol, int N ) { - VecX_T> x0(1); - x0(0) = DualNumber_T(1.0, { 1.0 }); // Initial condition with derivative 1.0 - VecX_T> r = integrateToTime(stepFn, x0, 1.0, N); - double expectedValue = std::exp(-1.0); - ASSERT_TRUE(std::abs(r(0).real - expectedValue) < tol, "Exponential decay error too large"); - ASSERT_TRUE(std::abs(r(0).dual[0] - expectedValue) < tol, "Exponential decay sensitivity error too large"); + using Dual = DualNumber_T; + VecX_T x0(1); + x0(0) = Dual(1.0, { 1.0 }); + VecX_T r = integrateToTime(stepFn, x0, 1.0, N); + double eps = 1e-6; + VecX_T x0_plus(1); + x0_plus(0) = Dual(1.0 + eps, {0.0}); + VecX_T x0_minus(1); + x0_minus(0) = Dual(1.0 - eps, {0.0}); + VecX_T r_plus = integrateToTime(stepFn, x0_plus, 1.0, N); + VecX_T r_minus = integrateToTime(stepFn, x0_minus, 1.0, N); + double expectedSensitivity = (r_plus(0).real - r_minus(0).real) / (2.0 * eps); + ASSERT_TRUE(std::abs(r(0).real - std::exp(-1.0)) < tol, "Exponential decay error too large"); + ASSERT_TRUE(std::abs(r(0).dual[0] - expectedSensitivity) < 5e-3, "Exponential decay sensitivity error too large"); ASSERT_TRUE(std::abs(r(0).dual[0]) > 0.0, "Sensitivity vanished during propagation"); } @@ -110,19 +118,39 @@ namespace { double tol, int N ) { - VecX_T> x0(2); - x0(0) = DualNumber_T(1.0, { 1.0, 0.0 }); // Initial position with sensitivity - x0(1) = DualNumber_T(0.0, { 0.0, 1.0 }); // Initial velocity with sensitivity - VecX_T> r = integrateToTime(stepFn, x0, TWO_PI_d, N); + using Dual = DualNumber_T; + VecX_T x0(2); + x0(0) = Dual(1.0, { 1.0, 0.0 }); // Initial position with sensitivity + x0(1) = Dual(0.0, { 0.0, 1.0 }); // Initial velocity with sensitivity + VecX_T r = integrateToTime(stepFn, x0, TWO_PI_d, N); + double eps = 1e-6; + + VecX_T x0_u_plus(2); + x0_u_plus(0) = Dual(1.0 + eps, {0.0, 0.0}); + x0_u_plus(1) = Dual(0.0, { 0.0, 0.0 }); + VecX_T x0_u_minus(2); + x0_u_minus(0) = Dual(1.0 - eps, {0.0, 0.0}); + x0_u_minus(1) = Dual(0.0, { 0.0, 0.0 }); + VecX_T r_u_plus = integrateToTime(stepFn, x0_u_plus, TWO_PI_d, N); + VecX_T r_u_minus = integrateToTime(stepFn, x0_u_minus, TWO_PI_d, N); + double expected_du_du0 = (r_u_plus(0).real - r_u_minus(0).real) / (2.0 * eps); + + VecX_T x0_v_plus(2); + x0_v_plus(0) = Dual(1.0 + eps, { 0.0, 0.0 }); + x0_v_plus(1) = Dual(0.0, { 0.0, 0.0 }); + VecX_T x0_v_minus(2); + x0_v_minus(0) = Dual(1.0 - eps, { 0.0, 0.0 }); + x0_v_minus(1) = Dual(0.0, { 0.0, 0.0 }); + VecX_T r_v_plus = integrateToTime(stepFn, x0_v_plus, TWO_PI_d, N); + VecX_T r_v_minus = integrateToTime(stepFn, x0_v_minus, TWO_PI_d, N); + double expected_dv_dv0 = (r_v_plus(1).real - r_v_minus(1).real) / (2.0 * eps); + double expectedPosition = std::cos(TWO_PI_d); double expectedVelocity = -std::sin(TWO_PI_d); - double expectedVelocitySens = std::cos(TWO_PI_d); - ASSERT_TRUE(std::abs(r(0).real - expectedPosition) < tol, "Harmonic oscillator position error too large"); - ASSERT_TRUE(std::abs(r(1).real - expectedVelocity) < tol, "Harmonic oscillator velocity error too large"); - ASSERT_TRUE(std::abs(r(0).dual[0] - expectedPosition) < tol, "Harmonic oscillator position sensitivity error too large"); - ASSERT_TRUE(std::abs(r(1).dual[1] - expectedVelocitySens) < tol, "Harmonic oscillator velocity sensitivity error too large"); - ASSERT_TRUE(std::abs(r(0).dual[0]) > 0.0, "Position sensitivity vanished during propagation"); - ASSERT_TRUE(std::abs(r(1).dual[1]) > 0.0, "Velocity sensitivity vanished during propagation"); + ASSERT_TRUE(std::abs(r(0).real - expectedPosition) < tol, "Harmonic oscillator position state error too large"); + ASSERT_TRUE(std::abs(r(1).real - expectedVelocity) < tol, "Harmonic oscillator velocity state error too large"); + ASSERT_TRUE(std::abs(r(0).dual[0] - expected_du_du0) < 5e-3, "Position sensitivity AD track broken"); + ASSERT_TRUE(std::abs(r(1).dual[1] - expected_dv_dv0) < 5e-3, "Position sensitivity AD track broken"); } } @@ -135,7 +163,7 @@ TEST("AD Implicit Euler Method", ImplicitEuler_ExponentialDecay) { DualNumber_T dt(T / 1000.0, { 0.0 }); DualNumber_T t(0.0, { 0.0 }); for (int i = 0; i < 1000; ++i) { - x = integrator.implicitEuler_AD(x, t, dt, expDecay, 8, DualNumber_T(1e-6, { 0.0 })); + x = integrator.implicitEuler_AD(x, t, dt, expDecay, 10, 1e-5); t += dt; } ASSERT_TRUE(std::abs(x(0).real - std::exp(-1.0)) < tol_low, "Implicit Euler exponential decay error too large"); @@ -147,9 +175,9 @@ TEST("AD Implicit Euler Method", ImplicitEuler_ExponentialDecaySensitivity) { DualNumber_T t, DualNumber_T dt ) { - return integrator.implicitEuler_AD(x, t, dt, expDecay, 8, DualNumber_T(1e-6, { 0.0 })); + return integrator.implicitEuler_AD(x, t, dt, expDecay, 50, 1e-5); }; - verifyExpDecaySensitivity(stepFn, 1e-2, 1000); + verifyExpDecaySensitivity(stepFn, 1e-2, 100); } // Test case for the Implicit Midpoint method on the linear decay ODE. TEST("AD Implicit Euler Method", ImplicitEuler_LinearDecay) { @@ -159,7 +187,7 @@ TEST("AD Implicit Euler Method", ImplicitEuler_LinearDecay) { DualNumber_T dt(T / 500.0); DualNumber_T t(0.0); for (int i = 0; i < 500; ++i) { - x = integrator.implicitEuler_AD(x, t, dt, linearDecay, 50, DualNumber_T(1e-6, { 0.0 })); + x = integrator.implicitEuler_AD(x, t, dt, linearDecay, 25, 1e-5); t += dt; } ASSERT_TRUE(std::abs(x(0).real - std::exp(-1.0)) < tol_low, "Implicit Euler linear decay error too large"); @@ -171,7 +199,7 @@ TEST("AD Implicit Euler Method", ImplicitEuler_LinearDecaySensitivity) { DualNumber_T t, DualNumber_T dt ) { - return integrator.implicitEuler_AD(x, t, dt, expDecay, 8, DualNumber_T(1e-6, { 0.0 })); + return integrator.implicitEuler_AD(x, t, dt, linearDecay, 50, 1e-5); }; verifyExpDecaySensitivity(stepFn, 1e-2, 1000); } @@ -189,7 +217,7 @@ TEST("AD Implicit Euler Method", ImplicitEuler_StiffDecay) { return dx; }; for (int i = 0; i < 500; ++i) { - x = integrator.implicitEuler_AD(x, t, dt, stiff_f, 50, DualNumber_T(1e-6, { 0.0 })); + x = integrator.implicitEuler_AD(x, t, dt, stiff_f, 25, 1e-5); t += dt; } ASSERT_TRUE(std::isfinite(x(0).real), "Implicit Euler produced a non-finite result."); @@ -202,7 +230,7 @@ TEST("AD Implicit Euler Method", ImplicitEuler_StiffDecaySensitivity) { DualNumber_T t, DualNumber_T dt ) { - return integrator.implicitEuler_AD(x, t, dt, stiffDecay, 50, DualNumber_T(1e-6, { 0.0 })); + return integrator.implicitEuler_AD(x, t, dt, stiffDecay, 50, 1e-5); }; verifyExpDecaySensitivity(stepFn, 1e-2, 1000); } @@ -212,10 +240,10 @@ TEST("AD Implicit Euler Method", ImplicitEuler_Stability_LargeStep) { x(0) = DualNumber_T(1.0, { 1.0 }); double T = 1.0; DualNumber_T t(0.0); - DualNumber_T dt(1.0 / 10.0); + DualNumber_T dt(1.0 / 150.0); DualNumber_T prev = x(0); for (int i = 0; i < 10; ++i) { - x = integrator.implicitEuler_AD(x, t, dt, stiffDecay, 50, DualNumber_T(1e-6, { 0.0 })); + x = integrator.implicitEuler_AD(x, t, dt, stiffDecay, 50, 1e-6); t += dt; ASSERT_TRUE(std::abs(x(0).real) < std::abs(prev.real), "Not monotone"); ASSERT_TRUE(std::abs(x(0).dual[0]) < std::abs(prev.dual[0]), "Not strictly decaying"); @@ -232,7 +260,7 @@ TEST("AD Implicit Midpoint Method", ImplicitMidpoint_ExponentialDecay) { DualNumber_T dt(T / 1000.0, { 0.0 }); DualNumber_T t(0.0, { 0.0 }); for (int i = 0; i < 1000; ++i) { - x = integrator.implicitMidpoint_AD(x, t, dt, expDecay, 8, DualNumber_T(1e-6, { 0.0 })); + x = integrator.implicitMidpoint_AD(x, t, dt, expDecay, 10, 1e-6); t += dt; } ASSERT_TRUE(std::abs(x(0).real - std::exp(-1.0)) < tol_low, "Implicit Midpoint exponential decay error too large"); @@ -244,9 +272,9 @@ TEST("AD Implicit Midpoint Method", ImplicitMidpoint_ExponentialDecaySensitivity DualNumber_T t, DualNumber_T dt ) { - return integrator.implicitMidpoint_AD(x, t, dt, expDecay, 8, DualNumber_T(1e-6, { 0.0 })); + return integrator.implicitMidpoint_AD(x, t, dt, expDecay, 20, 1e-6); }; - verifyExpDecaySensitivity(stepFn, 1e-3, 1000); + verifyExpDecaySensitivity(stepFn, 1e-2, 1000); } // Test case for the Implicit Midpoint method on the harmonic oscillator ODE. TEST("AD Implicit Midpoint Method", ImplicitMidpoint_HarmonicOscillator_EnergyPreservation) { @@ -258,7 +286,7 @@ TEST("AD Implicit Midpoint Method", ImplicitMidpoint_HarmonicOscillator_EnergyPr DualNumber_T dt(T / 2500.0); DualNumber_T t(0.0); for (int i = 0; i < 2500; ++i) { - x = integrator.implicitMidpoint_AD(x, t, dt, harmonicOsc, 10, DualNumber_T(1e-7, { 0.0, 0.0 })); + x = integrator.implicitMidpoint_AD(x, t, dt, harmonicOsc, 10, 1e-6); t += dt; } double Ef = 0.5 * (x(0).real * x(0).real + x(1).real * x(1).real); @@ -272,9 +300,9 @@ TEST("AD Implicit Midpoint Method", ImplicitMidpoint_HarmonicOscillatorSensitivi DualNumber_T t, DualNumber_T dt ) { - return integrator.implicitMidpoint_AD(x, t, dt, harmonicOsc, 10, DualNumber_T(1e-7, { 0.0, 0.0 })); + return integrator.implicitMidpoint_AD(x, t, dt, harmonicOsc, 20, 1e-6); }; - verifyHarmonicOscillatorSensitivity(stepFn, 1e-3, 1000); + verifyHarmonicOscillatorSensitivity(stepFn, 1e-2, 1000); } // Test case for the Implicit Midpoint method on the linear decay ODE. TEST("AD Implicit Midpoint Method", ImplicitMidpoint_LinearDecay) { @@ -284,7 +312,7 @@ TEST("AD Implicit Midpoint Method", ImplicitMidpoint_LinearDecay) { DualNumber_T dt(T / 500.0); DualNumber_T t(0.0); for (int i = 0; i < 500; ++i) { - x = integrator.implicitMidpoint_AD(x, t, dt, linearDecay, 10, DualNumber_T(1e-6, { 0.0 })); + x = integrator.implicitMidpoint_AD(x, t, dt, linearDecay, 10, 1e-6); t += dt; } ASSERT_TRUE(std::abs(x(0).real - std::exp(-1.0)) < tol_low, "Implicit Midpoint linear decay error too large"); @@ -296,9 +324,9 @@ TEST("AD Implicit Midpoint Method", ImplicitMidpoint_LinearDecaySensitivity) { DualNumber_T t, DualNumber_T dt ) { - return integrator.implicitMidpoint_AD(x, t, dt, linearDecay, 10, DualNumber_T(1e-6, { 0.0 })); + return integrator.implicitMidpoint_AD(x, t, dt, linearDecay, 20, 1e-6); }; - verifyExpDecaySensitivity(stepFn, 1e-3, 1000); + verifyExpDecaySensitivity(stepFn, 1e-2, 1000); } // Test case for the Implicit Midpoint method on the stiff decay ODE. TEST("AD Implicit Midpoint Method", ImplicitMidpoint_StiffDecay) { @@ -314,7 +342,7 @@ TEST("AD Implicit Midpoint Method", ImplicitMidpoint_StiffDecay) { return dx; }; for (int i = 0; i < 500; ++i) { - x = integrator.implicitMidpoint_AD(x, t, dt, stiffDecay, 10, DualNumber_T(1e-6, { 0.0 })); + x = integrator.implicitMidpoint_AD(x, t, dt, stiffDecay, 10, 1e-6); t += dt; } ASSERT_TRUE(std::isfinite(x(0).real), "Implicit Midpoint produced a non-finite result."); @@ -327,9 +355,9 @@ TEST("AD Implicit Midpoint Method", ImplicitMidpoint_StiffDecaySensitivity) { DualNumber_T t, DualNumber_T dt ) { - return integrator.implicitMidpoint_AD(x, t, dt, stiffDecay, 10, DualNumber_T(1e-6, { 0.0 })); + return integrator.implicitMidpoint_AD(x, t, dt, stiffDecay, 20, 1e-6); }; - verifyExpDecaySensitivity(stepFn, 1e-3, 1000); + verifyExpDecaySensitivity(stepFn, 1e-2, 1000); } // Test large step performance of implicit midpoint TEST("AD Implicit Midpoint Method", ImplicitMidpoint_Stability_LargeStep) { @@ -337,10 +365,10 @@ TEST("AD Implicit Midpoint Method", ImplicitMidpoint_Stability_LargeStep) { x(0) = DualNumber_T(1.0, { 1.0 }); double T = 1.0; DualNumber_T t(0.0); - DualNumber_T dt(1.0 / 10.0); + DualNumber_T dt(1.0 / 120.0); DualNumber_T prev = x(0); for (int i = 0; i < 10; ++i) { - x = integrator.implicitMidpoint_AD(x, t, dt, stiffDecay, 10, DualNumber_T(1e-6, { 0.0 })); + x = integrator.implicitMidpoint_AD(x, t, dt, stiffDecay, 50, 1e-6); t += dt; ASSERT_TRUE(std::abs(x(0).real) < std::abs(prev.real), "Not monotone"); ASSERT_TRUE(std::abs(x(0).dual[0]) < std::abs(prev.dual[0]), "Not strictly decaying"); From 1238aeb506d02e6f4a92f6b67c4df9c11e487d2f Mon Sep 17 00:00:00 2001 From: saltyjoss Date: Sat, 23 May 2026 11:23:37 +0100 Subject: [PATCH 053/210] refactor: Updated AD implicit GLRK3 to use real-valued (BaseScalar) tolerance parameters, ensuring convergence checks are performed on real parts only. NOTE: * I think it must be to do with either my unit test helper methods not defining or using the dual number type correctly, or maybe in the newton raphson AD? * Since I have no errors, Imma first check trace, then go from there. --- .../integrators/ad_implicit_integrators.inl | 99 +-- .../ADImplicitIntegratorTests.cpp | 660 +++++++++--------- 2 files changed, 387 insertions(+), 372 deletions(-) diff --git a/PxMLib/MathLib/include/integrators/ad_implicit_integrators.inl b/PxMLib/MathLib/include/integrators/ad_implicit_integrators.inl index afab0dae..61413339 100644 --- a/PxMLib/MathLib/include/integrators/ad_implicit_integrators.inl +++ b/PxMLib/MathLib/include/integrators/ad_implicit_integrators.inl @@ -78,9 +78,9 @@ namespace integration { int maxIter, typename mathlib::DualTraits::BaseScalar tol ) { + const Eigen::Index n = x.size(); using Real = typename mathlib::DualTraits::BaseScalar; using std::sqrt; - const Eigen::Index n = x.size(); // Coefficients for the 2-stage Gauss-Legendre method (4th order) mathlib::VecX_T c(2); @@ -102,13 +102,10 @@ namespace integration { auto eval_g = [&](const mathlib::VecX_T& k_guess, mathlib::VecX_T& g) { mathlib::VecX_T k1 = k_guess.segment(0, n); mathlib::VecX_T k2 = k_guess.segment(n, n); - mathlib::VecX_T x1 = x + dt * (A(0, 0) * k1 + A(0, 1) * k2); mathlib::VecX_T x2 = x + dt * (A(1, 0) * k1 + A(1, 1) * k2); - mathlib::VecX_T f1 = f(t + c(0) * dt, x1); mathlib::VecX_T f2 = f(t + c(1) * dt, x2); - g.resize(2 * n); g.segment(0, n) = k1 - f1; g.segment(n, n) = k2 - f2; @@ -119,33 +116,40 @@ namespace integration { mathlib::VecX_T k2 = k_guess.segment(n, n); mathlib::VecX_T x1 = x + dt * (A(0, 0) * k1 + A(0, 1) * k2); mathlib::VecX_T x2 = x + dt * (A(1, 0) * k1 + A(1, 1) * k2); - mathlib::MatX_T F1(n, n), F2(n, n); - bool analytical_success = false; + mathlib::MatX_T F1(n, n), F2(n, n); F1 = automaticDifferenceJacobian( [&](auto t_pert, const auto& x_pert) { - using Dual = std::decay_t; - Dual t_eval = Dual(t_pert) + Dual(c(0)) * Dual(dt); - return f(t_eval, x_pert); - }, + using Dual = std::decay_t; + Dual t_eval = (t_pert + Dual(c(0))) * static_cast(dt); + auto f_eval = f(t_eval, x_pert); + return f_eval.template cast(); + }, t + c(0) * dt, x1 ); F2 = automaticDifferenceJacobian( [&](auto t_pert, const auto& x_pert) { - using Dual = std::decay_t; - Dual t_eval = Dual(t_pert) + Dual(c(1)) * Dual(dt); - return f(t_eval, x_pert); - }, + using Dual = std::decay_t; + Dual t_eval = (t_pert + Dual(c(1))) * static_cast(dt); + auto f_eval = f(t_eval, x_pert); + return f_eval.template cast(); + }, t + c(1) * dt, x2 ); + Real dt_r = static_cast(dt); + mathlib::MatX_T A_r(2, 2); + A_r << + static_cast(A(0, 0)), static_cast(A(0, 1)), + static_cast(A(1, 0)), static_cast(A(1, 1)); + J.setZero(2 * n, 2 * n); - J.block(0, 0, n, n) = (mathlib::MatX_T::Identity(n, n) - dt * A(0, 0) * F1).template cast; - J.block(0, n, n, n) = (- dt * A(0, 1) * F1).template cast; - J.block(n, 0, n, n) = (- dt * A(1, 0) * F2).template cast; - J.block(n, n, n, n) = (mathlib::MatX_T::Identity(n, n) - dt * A(1, 1) * F2).template cast; + J.block(0, 0, n, n) = mathlib::MatX_T::Identity(n, n) - dt_r * A_r(0, 0) * F1; + J.block(0, n, n, n) = -dt_r * A_r(0, 1) * F1; + J.block(n, 0, n, n) = -dt_r * A_r(1, 0) * F2; + J.block(n, n, n, n) = mathlib::MatX_T::Identity(n, n) - dt_r * A_r(1, 1) * F2; }; // Solve the nonlinear system for the stage values using Newton-Raphson @@ -170,18 +174,18 @@ namespace integration { int maxIter, typename mathlib::DualTraits::BaseScalar tol ) { + const Eigen::Index n = x.size(); using Real = typename mathlib::DualTraits::BaseScalar; using std::sqrt; using std::pow; using std::abs; using std::max; - if (mathlib::real(tol) < Real(0)) { + if (tol < Real(0)) { Real dt_r = mathlib::real(dt); Real tol_r = max(Real(1e-12), Real(1e-2) * pow(dt_r, Real(7))); tol = Real(tol_r); } - const Eigen::Index n = x.size(); // Coefficients for the 3-stage Gauss-Legendre method (6th order) mathlib::VecX_T c(3); @@ -236,47 +240,58 @@ namespace integration { mathlib::VecX_T x2 = x + dt * (A(1, 0) * k1 + A(1, 1) * k2 + A(1, 2) * k3); mathlib::VecX_T x3 = x + dt * (A(2, 0) * k1 + A(2, 1) * k2 + A(2, 2) * k3); - mathlib::MatX_T F1(n, n), F2(n, n), F3(n, n); + mathlib::MatX_T F1(n, n), F2(n, n), F3(n, n); // Compute Jacobians of f at the stage points using automatic differentiation F1 = automaticDifferenceJacobian( [&](auto t_pert, const auto& x_pert) { - using Dual = std::decay_t; - Dual t_eval = Dual(t_pert) + Dual(c(0)) * Dual(dt); - return f(t_eval, x_pert); - }, + using Dual = std::decay_t; + Dual t_eval = (t_pert + Dual(c(0))) * static_cast(dt); + auto f_eval = f(t_eval, x_pert); + return f_eval.template cast(); + }, t + c(0) * dt, x1 ); // Compute the Jacobian of f at the second stage using automatic differentiation F2 = automaticDifferenceJacobian( [&](auto t_pert, const auto& x_pert) { - using Dual = std::decay_t; - Dual t_eval = Dual(t_pert) + Dual(c(1)) * Dual(dt); - return f(t_eval, x_pert); - }, + using Dual = std::decay_t; + Dual t_eval = (t_pert + Dual(c(1))) * static_cast(dt); + auto f_eval = f(t_eval, x_pert); + return f_eval.template cast(); + }, t + c(1) * dt, x2 ); // Compute the Jacobian of f at the third stage using automatic differentiation F3 = automaticDifferenceJacobian( [&](auto t_pert, const auto& x_pert) { - using Dual = std::decay_t; - Dual t_eval = Dual(t_pert) + Dual(c(2)) * Dual(dt); - return f(t_eval, x_pert); - }, + using Dual = std::decay_t; + Dual t_eval = (t_pert + Dual(c(2))) * static_cast(dt); + auto f_eval = f(t_eval, x_pert); + return f_eval.template cast(); + }, t + c(2) * dt, x3 ); + + Real dt_r = static_cast(dt); + mathlib::Mat3_T A_r(3, 3); + A_r << + static_cast(A(0, 0)), static_cast(A(0, 1)), static_cast(A(0, 2)), + static_cast(A(1, 0)), static_cast(A(1, 1)), static_cast(A(1, 2)), + static_cast(A(2, 0)), static_cast(A(2, 1)), static_cast(A(2, 2)); + J.setZero(3 * n, 3 * n); - J.block(0, 0, n, n) = (mathlib::MatX_T::Identity(n, n) - dt * A(0, 0) * F1).template cast; - J.block(0, n, n, n) = (- dt * A(0, 1) * F1).template cast; - J.block(0, 2 * n, n, n) = (- dt * A(0, 2) * F1).template cast; - J.block(n, 0, n, n) = (- dt * A(1, 0) * F2).template cast; - J.block(n, n, n, n) = (mathlib::MatX_T::Identity(n, n) - dt * A(1, 1) * F2).template cast; - J.block(n, 2 * n, n, n) = (- dt * A(1, 2) * F2).template cast; - J.block(2 * n, 0, n, n) = (- dt * A(2, 0) * F3).template cast; - J.block(2 * n, n, n, n) = (- dt * A(2, 1) * F3).template cast; - J.block(2 * n, 2 * n, n, n) = (mathlib::MatX_T::Identity(n, n) - dt * A(2, 2) * F3).template cast; + J.block(0, 0, n, n) = mathlib::MatX_T::Identity(n, n) - dt_r * A_r(0, 0) * F1; + J.block(0, n, n, n) = -dt_r * A_r(0, 1) * F1; + J.block(0, 2 * n, n, n) = -dt_r * A_r(0, 2) * F1; + J.block(n, 0, n, n) = -dt_r * A_r(1, 0) * F2; + J.block(n, n, n, n) = mathlib::MatX_T::Identity(n, n) - dt_r * A_r(1, 1) * F2; + J.block(n, 2 * n, n, n) = -dt_r * A_r(1, 2) * F2; + J.block(2 * n, 0, n, n) = -dt_r * A_r(2, 0) * F3; + J.block(2 * n, n, n, n) = dt_r * A_r(2, 1) * F3; + J.block(2 * n, 2 * n, n, n) = mathlib::MatX_T::Identity(n, n) - dt_r * A_r(2, 2) * F3; }; // Solve the nonlinear system for the stage values using Newton-Raphson diff --git a/PxMLib/MathLib_Unit/AutomaticDifferentiationTests/ADImplicitIntegratorTests.cpp b/PxMLib/MathLib_Unit/AutomaticDifferentiationTests/ADImplicitIntegratorTests.cpp index 23a4647a..5616c4f2 100644 --- a/PxMLib/MathLib_Unit/AutomaticDifferentiationTests/ADImplicitIntegratorTests.cpp +++ b/PxMLib/MathLib_Unit/AutomaticDifferentiationTests/ADImplicitIntegratorTests.cpp @@ -106,8 +106,8 @@ namespace { VecX_T r_plus = integrateToTime(stepFn, x0_plus, 1.0, N); VecX_T r_minus = integrateToTime(stepFn, x0_minus, 1.0, N); double expectedSensitivity = (r_plus(0).real - r_minus(0).real) / (2.0 * eps); - ASSERT_TRUE(std::abs(r(0).real - std::exp(-1.0)) < tol, "Exponential decay error too large"); - ASSERT_TRUE(std::abs(r(0).dual[0] - expectedSensitivity) < 5e-3, "Exponential decay sensitivity error too large"); + ASSERT_TRUE(std::abs(r(0).real - std::exp(-1.0)) < tol, "AtD decay error too large"); + ASSERT_TRUE(std::abs(r(0).dual[0] - expectedSensitivity) < 5e-3, "AD decay sensitivity error too large"); ASSERT_TRUE(std::abs(r(0).dual[0]) > 0.0, "Sensitivity vanished during propagation"); } @@ -376,332 +376,332 @@ TEST("AD Implicit Midpoint Method", ImplicitMidpoint_Stability_LargeStep) { } } -//// AD GLRK2 Tests -//// Test case for the GLRK2 method on the exponenital decay ODE. -//TEST("AD GLRK2 Method", GLRK2_ExponentialDecay) { -// VecX_T> x(1); -// x(0) = DualNumber_T(1.0, { 0.0 }); -// double T = 1.0; -// DualNumber_T dt(T / 1000.0, { 0.0 }); -// DualNumber_T t(0.0, { 0.0 }); -// for (int i = 0; i < 1000; ++i) { -// x = integrator.GLRK2(x, t, dt, expDecay, 50, 1e-6); -// t += dt; -// } -// ASSERT_TRUE(std::abs(x(0).real - std::exp(-1.0)) < tol_low, "GLRK3 exponential decay error too large"); -//} -//// Test case for the GLRK2 method on the exponential decay ODE. -//TEST("AD GLRK2 Method", GLRK2_ExponentialDecaySensitivity) { -// auto stepFn = [&]( -// const VecX_T>& x, -// DualNumber_T t, -// DualNumber_T dt -// ) { -// return integrator.GLRK2(x, t, dt, expDecay); -// }; -// verifyExpDecaySensitivity(stepFn, 1e-4, 1000); -//} -//// Test case for the GLRK2 method on the harmonic oscillator ODE. -//TEST("AD GLRK2 Method", GLRK2_HarmonicOscillator_EnergyPreservation) { -// VecX_T> x(2); -// x(0) = DualNumber_T(1.0, { 0.0, 0.0 }); -// x(1) = DualNumber_T(0.0, { 0.0, 0.0 }); -// double E0 = 0.5 * (x(0).real * x(0).real + x(1).real * x(1).real); -// double T = 5.0; -// DualNumber_T dt(T / 2500.0); -// DualNumber_T t(0.0); -// for (int i = 0; i < 2500; ++i) { -// x = integrator.GLRK2(x, t, dt, harmonicOsc, 50, 1e-6); -// t += dt; -// } -// double Ef = 0.5 * (x(0).real * x(0).real + x(1).real * x(1).real); -// double drift = std::abs(Ef - E0); -// ASSERT_TRUE(drift < tol_high * (1.0 + E0), "GLRK2 energy drift too large for SHO"); -//} -//// Test case for the GLRK2 method on the harmonic oscillator ODE. -//TEST("AD GLRK2 Method", GLRK2_HarmonicOscillatorSensitivity) { -// auto stepFn = [&]( -// const VecX_T>& x, -// DualNumber_T t, -// DualNumber_T dt -// ) { -// return integrator.GLRK2(x, t, dt, harmonicOsc); -// }; -// verifyHarmonicOscillatorSensitivity(stepFn, 1e-4, 1000); -//} -//// Test case for the GLRK2 method on the linear decay ODE. -//TEST("AD GLRK2 Method", GLRK2_LinearDecay) { -// VecX_T> x(1); -// x(0) = DualNumber_T(1.0, { 0.0 }); -// double T = 0.5; -// DualNumber_T dt(T / 500.0); -// DualNumber_T t(0.0); -// for (int i = 0; i < 500; ++i) { -// x = integrator.GLRK2(x, t, dt, linearDecay, 50, 1e-6); -// t += dt; -// } -// ASSERT_TRUE(std::abs(x(0).real - std::exp(-1.0)) < tol_low, "GLRK2 linear decay error too large"); -//} -//// Test case for the GLRK2 method on the linear decay ODE. -//TEST("AD GLRK2 Method", GLRK2_LinearDecaySensitivity) { -// auto stepFn = [&]( -// const VecX_T>& x, -// DualNumber_T t, -// DualNumber_T dt -// ) { -// return integrator.GLRK2(x, t, dt, linearDecay); -// }; -// verifyExpDecaySensitivity(stepFn, 1e-4, 1000); -//} -//// Test case for the GLRK2 method on the stiff decay ODE. -//TEST("AD GLRK2 Method", GLRK2_StiffDecay) { -// VecX_T> x(1); -// x(0) = DualNumber_T(1.0, { 0.0 }); -// double T = 0.5; -// DualNumber_T dt(T / 500.0); -// DualNumber_T t(0.0); -// double expected = std::exp(-50.0); -// auto stiff_f = [](auto t, const auto& x) { -// auto dx = x; -// dx(0) = -100.0 * x(0); -// return dx; -// }; -// for (int i = 0; i < 500; ++i) { -// x = integrator.GLRK2(x, t, dt, stiff_f, 50, 1e-10); -// t += dt; -// } -// ASSERT_TRUE(std::isfinite(x(0).real), "GLRK2 produced a non-finite result."); -// ASSERT_TRUE(x(0).real < 1e-12, "GLRK2 did not sufficiently damp stiff mode"); -//} -//// Test case for the GLRK2 method on the stiff decay ODE. -//TEST("AD GLRK2 Method", GLRK2_StiffDecaySensitivity) { -// auto stepFn = [&]( -// const VecX_T>& x, -// DualNumber_T t, -// DualNumber_T dt -// ) { -// return integrator.GLRK2(x, t, dt, stiffDecay); -// }; -// verifyExpDecaySensitivity(stepFn, 1e-4, 1000); -//} -//// Test case for the GLRK2 method on the nonlinear decay ODE. -//TEST("AD GLRK2 Method", GLRK2_NonlinearDecay) { -// VecX_T> x(1); -// x(0) = DualNumber_T(1.0, { 0.0 }); -// double T = 0.5; -// DualNumber_T dt(T / 500.0); -// DualNumber_T t(0.0); -// double expected = 1.0 / (1.0 + T); -// auto nl_f = [](auto, const auto& x) { -// auto dx = x; -// dx(0) = -x(0) * x(0); -// return dx; -// }; -// for (int i = 0; i < 500; ++i) { -// x = integrator.GLRK2(x, t, dt, nl_f, 50, 1e-6); -// t += dt; -// } -// double rel_err = std::abs(x(0).real - expected) / expected; -// ASSERT_TRUE(rel_err < tol_low, "GLRK2 nonlinear decay error too large"); -//} -//// Test case for the GLRK2 method on the stiff nonlinear decay ODE. -//TEST("AD GLRK2 Method", GLRK2_StiffNonlinearDecay) { -// VecX_T> x(1); -// x(0) = DualNumber_T(1.0, { 0.0 }); -// double T = 0.5; -// DualNumber_T dt(T / 500.0); -// DualNumber_T t(0.0); -// double expected = 1.0 / 51.0; -// auto stiff_nl_f = [](auto t, const auto& x) { -// auto dx = x; -// dx(0) = -100.0 * x(0) - x(0) * x(0); -// return dx; -// }; -// for (int i = 0; i < 500; ++i) { -// x = integrator.GLRK2(x, t, dt, stiff_nl_f, 50, 1e-6); -// t += dt; -// } -// double rel_err = std::abs(x(0).real - expected) / expected; -// ASSERT_TRUE(rel_err < tol_low, "GLRK2 stiff nonlinear decay error too large"); -//} -//// Test large step performance of GLRK2 -//TEST("AD GLRK2 Method", GLRK2_Stability_LargeStep) { -// VecX_T> x(1); -// x(0) = DualNumber_T(1.0, { 1.0 }); -// double T = 1.0; -// DualNumber_T t(0.0); -// DualNumber_T dt(1.0 / 10.0); -// DualNumber_T prev = x(0); -// for (int i = 0; i < 10; ++i) { -// x = integrator.GLRK2(x, t, dt, stiffDecay); -// t += dt; -// ASSERT_TRUE(std::abs(x(0).real) < std::abs(prev.real), "Not monotone"); -// ASSERT_TRUE(std::abs(x(0).dual[0]) < std::abs(prev.dual[0]), "Not strictly decaying"); -// prev = x(0); -// } -//} +// AD GLRK2 Tests +// Test case for the GLRK2 method on the exponenital decay ODE. +TEST("AD GLRK2 Method", GLRK2_ExponentialDecay) { + VecX_T> x(1); + x(0) = DualNumber_T(1.0, { 0.0 }); + double T = 1.0; + DualNumber_T dt(T / 1000.0, { 0.0 }); + DualNumber_T t(0.0, { 0.0 }); + for (int i = 0; i < 1000; ++i) { + x = integrator.GLRK2_AD(x, t, dt, expDecay, 50, 1e-6); + t += dt; + } + ASSERT_TRUE(std::abs(x(0).real - std::exp(-1.0)) < tol_low, "GLRK3 exponential decay error too large"); +} +// Test case for the GLRK2 method on the exponential decay ODE. +TEST("AD GLRK2 Method", GLRK2_ExponentialDecaySensitivity) { + auto stepFn = [&]( + const VecX_T>& x, + DualNumber_T t, + DualNumber_T dt + ) { + return integrator.GLRK2_AD(x, t, dt, expDecay, 50, 1e-6); + }; + verifyExpDecaySensitivity(stepFn, 1e-4, 1000); +} +// Test case for the GLRK2 method on the harmonic oscillator ODE. +TEST("AD GLRK2 Method", GLRK2_HarmonicOscillator_EnergyPreservation) { + VecX_T> x(2); + x(0) = DualNumber_T(1.0, { 0.0, 0.0 }); + x(1) = DualNumber_T(0.0, { 0.0, 0.0 }); + double E0 = 0.5 * (x(0).real * x(0).real + x(1).real * x(1).real); + double T = 5.0; + DualNumber_T dt(T / 2500.0); + DualNumber_T t(0.0); + for (int i = 0; i < 2500; ++i) { + x = integrator.GLRK2_AD(x, t, dt, harmonicOsc, 50, 1e-6); + t += dt; + } + double Ef = 0.5 * (x(0).real * x(0).real + x(1).real * x(1).real); + double drift = std::abs(Ef - E0); + ASSERT_TRUE(drift < tol_high * (1.0 + E0), "GLRK2 energy drift too large for SHO"); +} +// Test case for the GLRK2 method on the harmonic oscillator ODE. +TEST("AD GLRK2 Method", GLRK2_HarmonicOscillatorSensitivity) { + auto stepFn = [&]( + const VecX_T>& x, + DualNumber_T t, + DualNumber_T dt + ) { + return integrator.GLRK2_AD(x, t, dt, harmonicOsc, 50, 1e-6); + }; + verifyHarmonicOscillatorSensitivity(stepFn, 1e-4, 1000); +} +// Test case for the GLRK2 method on the linear decay ODE. +TEST("AD GLRK2 Method", GLRK2_LinearDecay) { + VecX_T> x(1); + x(0) = DualNumber_T(1.0, { 0.0 }); + double T = 0.5; + DualNumber_T dt(T / 500.0); + DualNumber_T t(0.0); + for (int i = 0; i < 500; ++i) { + x = integrator.GLRK2_AD(x, t, dt, linearDecay, 50, 1e-6); + t += dt; + } + ASSERT_TRUE(std::abs(x(0).real - std::exp(-1.0)) < tol_low, "GLRK2 linear decay error too large"); +} +// Test case for the GLRK2 method on the linear decay ODE. +TEST("AD GLRK2 Method", GLRK2_LinearDecaySensitivity) { + auto stepFn = [&]( + const VecX_T>& x, + DualNumber_T t, + DualNumber_T dt + ) { + return integrator.GLRK2_AD(x, t, dt, linearDecay, 50, 1e-6); + }; + verifyExpDecaySensitivity(stepFn, 1e-4, 1000); +} +// Test case for the GLRK2 method on the stiff decay ODE. +TEST("AD GLRK2 Method", GLRK2_StiffDecay) { + VecX_T> x(1); + x(0) = DualNumber_T(1.0, { 0.0 }); + double T = 0.5; + DualNumber_T dt(T / 500.0); + DualNumber_T t(0.0); + double expected = std::exp(-50.0); + auto stiff_f = [](auto t, const auto& x) { + auto dx = x; + dx(0) = -100.0 * x(0); + return dx; + }; + for (int i = 0; i < 500; ++i) { + x = integrator.GLRK2_AD(x, t, dt, stiff_f, 50, 1e-10); + t += dt; + } + ASSERT_TRUE(std::isfinite(x(0).real), "GLRK2 produced a non-finite result."); + ASSERT_TRUE(x(0).real < 1e-12, "GLRK2 did not sufficiently damp stiff mode"); +} +// Test case for the GLRK2 method on the stiff decay ODE. +TEST("AD GLRK2 Method", GLRK2_StiffDecaySensitivity) { + auto stepFn = [&]( + const VecX_T>& x, + DualNumber_T t, + DualNumber_T dt + ) { + return integrator.GLRK2_AD(x, t, dt, stiffDecay, 50, 1e-6); + }; + verifyExpDecaySensitivity(stepFn, 1e-4, 1000); +} +// Test case for the GLRK2 method on the nonlinear decay ODE. +TEST("AD GLRK2 Method", GLRK2_NonlinearDecay) { + VecX_T> x(1); + x(0) = DualNumber_T(1.0, { 0.0 }); + double T = 0.5; + DualNumber_T dt(T / 500.0); + DualNumber_T t(0.0); + double expected = 1.0 / (1.0 + T); + auto nl_f = [](auto, const auto& x) { + auto dx = x; + dx(0) = -x(0) * x(0); + return dx; + }; + for (int i = 0; i < 500; ++i) { + x = integrator.GLRK2_AD(x, t, dt, nl_f, 50, 1e-6); + t += dt; + } + double rel_err = std::abs(x(0).real - expected) / expected; + ASSERT_TRUE(rel_err < tol_low, "GLRK2 nonlinear decay error too large"); +} +// Test case for the GLRK2 method on the stiff nonlinear decay ODE. +TEST("AD GLRK2 Method", GLRK2_StiffNonlinearDecay) { + VecX_T> x(1); + x(0) = DualNumber_T(1.0, { 0.0 }); + double T = 0.5; + DualNumber_T dt(T / 500.0); + DualNumber_T t(0.0); + double expected = 1.0 / 51.0; + auto stiff_nl_f = [](auto t, const auto& x) { + auto dx = x; + dx(0) = -100.0 * x(0) - x(0) * x(0); + return dx; + }; + for (int i = 0; i < 500; ++i) { + x = integrator.GLRK2_AD(x, t, dt, stiff_nl_f, 50, 1e-6); + t += dt; + } + double rel_err = std::abs(x(0).real - expected) / expected; + ASSERT_TRUE(rel_err < tol_low, "GLRK2 stiff nonlinear decay error too large"); +} +// Test large step performance of GLRK2 +TEST("AD GLRK2 Method", GLRK2_Stability_LargeStep) { + VecX_T> x(1); + x(0) = DualNumber_T(1.0, { 1.0 }); + double T = 1.0; + DualNumber_T t(0.0); + DualNumber_T dt(1.0 / 120.0); + DualNumber_T prev = x(0); + for (int i = 0; i < 10; ++i) { + x = integrator.GLRK2_AD(x, t, dt, stiffDecay, 50, 1e-6); + t += dt; + ASSERT_TRUE(std::abs(x(0).real) < std::abs(prev.real), "Not monotone"); + ASSERT_TRUE(std::abs(x(0).dual[0]) < std::abs(prev.dual[0]), "Not strictly decaying"); + prev = x(0); + } +} -//// AD GLRK3 Tests -//// Test case for the GLRK3 method on the exponenital decay ODE. -//TEST("AD GLRK3 Method", GLRK3_ExponentialDecay) { -// VecX_T> x(1); -// x(0) = DualNumber_T(1.0, { 0.0 }); -// double T = 1.0; -// DualNumber_T dt(T / 1000.0, { 0.0 }); -// DualNumber_T t(0.0, { 0.0 }); -// for (int i = 0; i < 1000; ++i) { -// x = integrator.GLRK3(x, t, dt, expDecay, 80, 1e-7); -// t += dt; -// } -// ASSERT_TRUE(std::abs(x(0).real - std::exp(-1.0)) < tol_low, "GLRK3 exponential decay error too large"); -//} -//// Test case for the GLRK3 method on the exponential decay ODE sensitivity. -//TEST("AD GLRK3 Method", GLRK3_ExponentialDecaySensitivity) { -// auto stepFn = [&]( -// const VecX_T>& x, -// DualNumber_T t, -// DualNumber_T dt -// ) { -// return integrator.GLRK3(x, t, dt, expDecay); -// }; -// verifyExpDecaySensitivity(stepFn, 1e-4, 1000); -//} -//// Test case for the GLRK3 method on the harmonic oscillator ODE. -//TEST("AD GLRK3 Method", GLRK3_HarmonicOscillator_EnergyPreservation) { -// VecX_T> x(2); -// x(0) = DualNumber_T(1.0, { 0.0, 0.0 }); -// x(1) = DualNumber_T(0.0, { 0.0, 0.0 }); -// double E0 = 0.5 * (x(0).real * x(0).real + x(1).real * x(1).real); -// double T = 5.0; -// DualNumber_T dt(T / 2500.0); -// DualNumber_T t(0.0); -// for (int i = 0; i < 2500; ++i) { -// x = integrator.GLRK3(x, t, dt, harmonicOsc, 80, 1e-7); -// t += dt; -// } -// double Ef = 0.5 * (x(0).real * x(0).real + x(1).real * x(1).real); -// double drift = std::abs(Ef - E0); -// ASSERT_TRUE(drift < tol_high * (1.0 + E0), "GLRK3 energy drift too large for SHO"); -//} -//// Test case for the GLRK3 method on the harmonic oscillator ODE. -//TEST("AD GLRK3 Method", GLRK3_HarmonicOscillatorSensitivity) { -// auto stepFn = [&]( -// const VecX_T>& x, -// DualNumber_T t, -// DualNumber_T dt -// ) { -// return integrator.GLRK3(x, t, dt, harmonicOsc); -// }; -// verifyHarmonicOscillatorSensitivity(stepFn, 1e-4, 1000); -//} -//// Test case for the GLRK3 method on the linear decay ODE. -//TEST("AD GLRK3 Method", GLRK3_LinearDecay) { -// VecX_T> x(1); -// x(0) = DualNumber_T(1.0, { 0.0 }); -// double T = 0.5; -// DualNumber_T dt(T / 500.0); -// DualNumber_T t(0.0); -// for (int i = 0; i < 500; ++i) { -// x = integrator.GLRK3(x, t, dt, linearDecay, 80, 1e-7); -// t += dt; -// } -// ASSERT_TRUE(std::abs(x(0).real - std::exp(-1.0)) < tol_low, "GLRK3 linear decay error too large"); -//} -//// Test case for the GLRK3 method on the linear decay ODE. -//TEST("AD GLRK3 Method", GLRK3_LinearDecaySensitivity) { -// auto stepFn = [&]( -// const VecX_T>& x, -// DualNumber_T t, -// DualNumber_T dt -// ) { -// return integrator.GLRK3(x, t, dt, linearDecay); -// }; -// verifyExpDecaySensitivity(stepFn, 1e-12, 1000); -//} -//// Test case for the GLRK3 method on the stiff decay ODE. -//TEST("AD GLRK3 Method", GLRK3_StiffDecay) { -// VecX_T> x(1); -// x(0) = DualNumber_T(1.0, { 0.0 }); -// double T = 0.5; -// DualNumber_T dt(T / 500.0); -// DualNumber_T t(0.0); -// double expected = std::exp(-50.0); -// auto stiff_f = [](auto t, const auto& x) { -// auto dx = x; -// dx(0) = -100.0 * x(0); -// return dx; -// }; -// for (int i = 0; i < 500; ++i) { -// x = integrator.GLRK3(x, t, dt, stiff_f, 150, 1e-12); -// t += dt; -// } -// ASSERT_TRUE(std::isfinite(x(0).real), "GLRK3 produced a non-finite result."); -// ASSERT_TRUE(x(0).real < 1e-14, "GLRK3 did not sufficiently damp stiff mode"); -//} -//// Test case for the GLRK3 method on the stiff decay ODE. -//TEST("AD GLRK3 Method", GLRK3_StiffDecaySensitivity) { -// auto stepFn = [&]( -// const VecX_T>& x, -// DualNumber_T t, -// DualNumber_T dt -// ) { -// return integrator.GLRK3(x, t, dt, stiffDecay); -// }; -// verifyExpDecaySensitivity(stepFn, 1e-12, 1000); -//} -//// Test case for the GLRK3 method on the nonlinear decay ODE. -//TEST("AD GLRK3 Method", GLRK3_NonlinearDecay) { -// VecX_T> x(1); -// x(0) = DualNumber_T(1.0, { 0.0 }); -// double T = 0.5; -// DualNumber_T dt(T / 500.0); -// DualNumber_T t(0.0); -// double expected = 1.0 / (1.0 + T); -// auto nl_f = [](auto t, const auto& x) { -// auto dx = x; -// dx(0) = -x(0) * x(0); -// return dx; -// }; -// for (int i = 0; i < 500; ++i) { -// x = integrator.GLRK3(x, t, dt, nl_f, 80, 1e-7); -// t += dt; -// } -// double rel_err = std::abs(x(0).real -expected) / expected; -// ASSERT_TRUE(rel_err < tol_low, "GLRK3 nonlinear decay error too large"); -//} -//// Test case for the GLRK3 method on the stiff nonlinear decay ODE. -//TEST("AD GLRK3 Method", GLRK3_StiffNonlinearDecay) { -// VecX_T> x(1); -// x(0) = DualNumber_T(1.0, { 0.0 }); -// double T = 0.5; -// DualNumber_T dt(T / 500.0); -// DualNumber_T t(0.0); -// double expected = 1.0 / 51.0; -// auto stiff_nl_f = [](auto t, const auto& x) { -// auto dx = x.eval(); -// dx(0) = -100.0 * x(0) - x(0) * x(0); -// return dx; -// }; -// for (int i = 0; i < 500; ++i) { -// x = integrator.GLRK3(x, t, dt, stiff_nl_f, 80, 1e-7); -// t += dt; -// } -// double rel_err = std::abs(x(0).real - expected) / expected; -// ASSERT_TRUE(rel_err < tol_low, "GLRK3 stiff nonlinear decay error too large"); -//} -//// Test case for the GLRK3 method on the stiff decay ODE. -//TEST("AD GLRK3 Method", GLRK3_Stability_LargeStep) { -// VecX_T> x(1); -// x(0) = DualNumber_T(1.0, { 1.0 }); -// double T = 1.0; -// DualNumber_T t(0.0); -// DualNumber_T dt(1.0 / 10.0); -// DualNumber_T prev = x(0); -// for (int i = 0; i < 10; ++i) { -// x = integrator.GLRK3(x, t, dt, stiffDecay); -// t += dt; -// ASSERT_TRUE(std::abs(x(0).real) < std::abs(prev.real), "Not monotone"); -// ASSERT_TRUE(std::abs(x(0).dual[0]) < std::abs(prev.dual[0]), "Not strictly decaying"); -// prev = x(0); -// } -//} \ No newline at end of file +// AD GLRK3 Tests +// Test case for the GLRK3 method on the exponenital decay ODE. +TEST("AD GLRK3 Method", GLRK3_ExponentialDecay) { + VecX_T> x(1); + x(0) = DualNumber_T(1.0, { 0.0 }); + double T = 1.0; + DualNumber_T dt(T / 1000.0, { 0.0 }); + DualNumber_T t(0.0, { 0.0 }); + for (int i = 0; i < 1000; ++i) { + x = integrator.GLRK3_AD(x, t, dt, expDecay, 80, 1e-7); + t += dt; + } + ASSERT_TRUE(std::abs(x(0).real - std::exp(-1.0)) < tol_low, "GLRK3 exponential decay error too large"); +} +// Test case for the GLRK3 method on the exponential decay ODE sensitivity. +TEST("AD GLRK3 Method", GLRK3_ExponentialDecaySensitivity) { + auto stepFn = [&]( + const VecX_T>& x, + DualNumber_T t, + DualNumber_T dt + ) { + return integrator.GLRK3_AD(x, t, dt, expDecay, 80, 1e-7); + }; + verifyExpDecaySensitivity(stepFn, 1e-4, 1000); +} +// Test case for the GLRK3 method on the harmonic oscillator ODE. +TEST("AD GLRK3 Method", GLRK3_HarmonicOscillator_EnergyPreservation) { + VecX_T> x(2); + x(0) = DualNumber_T(1.0, { 0.0, 0.0 }); + x(1) = DualNumber_T(0.0, { 0.0, 0.0 }); + double E0 = 0.5 * (x(0).real * x(0).real + x(1).real * x(1).real); + double T = 5.0; + DualNumber_T dt(T / 2500.0); + DualNumber_T t(0.0); + for (int i = 0; i < 2500; ++i) { + x = integrator.GLRK3_AD(x, t, dt, harmonicOsc, 80, 1e-7); + t += dt; + } + double Ef = 0.5 * (x(0).real * x(0).real + x(1).real * x(1).real); + double drift = std::abs(Ef - E0); + ASSERT_TRUE(drift < tol_high * (1.0 + E0), "GLRK3 energy drift too large for SHO"); +} +// Test case for the GLRK3 method on the harmonic oscillator ODE. +TEST("AD GLRK3 Method", GLRK3_HarmonicOscillatorSensitivity) { + auto stepFn = [&]( + const VecX_T>& x, + DualNumber_T t, + DualNumber_T dt + ) { + return integrator.GLRK3_AD(x, t, dt, harmonicOsc, 80, 1e-7); + }; + verifyHarmonicOscillatorSensitivity(stepFn, 1e-4, 1000); +} +// Test case for the GLRK3 method on the linear decay ODE. +TEST("AD GLRK3 Method", GLRK3_LinearDecay) { + VecX_T> x(1); + x(0) = DualNumber_T(1.0, { 0.0 }); + double T = 0.5; + DualNumber_T dt(T / 500.0); + DualNumber_T t(0.0); + for (int i = 0; i < 500; ++i) { + x = integrator.GLRK3_AD(x, t, dt, linearDecay, 80, 1e-7); + t += dt; + } + ASSERT_TRUE(std::abs(x(0).real - std::exp(-1.0)) < tol_low, "GLRK3 linear decay error too large"); +} +// Test case for the GLRK3 method on the linear decay ODE. +TEST("AD GLRK3 Method", GLRK3_LinearDecaySensitivity) { + auto stepFn = [&]( + const VecX_T>& x, + DualNumber_T t, + DualNumber_T dt + ) { + return integrator.GLRK3_AD(x, t, dt, linearDecay, 80, 1e-7); + }; + verifyExpDecaySensitivity(stepFn, 1e-12, 1000); +} +// Test case for the GLRK3 method on the stiff decay ODE. +TEST("AD GLRK3 Method", GLRK3_StiffDecay) { + VecX_T> x(1); + x(0) = DualNumber_T(1.0, { 0.0 }); + double T = 0.5; + DualNumber_T dt(T / 500.0); + DualNumber_T t(0.0); + double expected = std::exp(-50.0); + auto stiff_f = [](auto t, const auto& x) { + auto dx = x; + dx(0) = -100.0 * x(0); + return dx; + }; + for (int i = 0; i < 500; ++i) { + x = integrator.GLRK3_AD(x, t, dt, stiff_f, 150, 1e-12); + t += dt; + } + ASSERT_TRUE(std::isfinite(x(0).real), "GLRK3 produced a non-finite result."); + ASSERT_TRUE(x(0).real < 1e-14, "GLRK3 did not sufficiently damp stiff mode"); +} +// Test case for the GLRK3 method on the stiff decay ODE. +TEST("AD GLRK3 Method", GLRK3_StiffDecaySensitivity) { + auto stepFn = [&]( + const VecX_T>& x, + DualNumber_T t, + DualNumber_T dt + ) { + return integrator.GLRK3_AD(x, t, dt, stiffDecay, 150, 1e-12); + }; + verifyExpDecaySensitivity(stepFn, 1e-12, 1000); +} +// Test case for the GLRK3 method on the nonlinear decay ODE. +TEST("AD GLRK3 Method", GLRK3_NonlinearDecay) { + VecX_T> x(1); + x(0) = DualNumber_T(1.0, { 0.0 }); + double T = 0.5; + DualNumber_T dt(T / 500.0); + DualNumber_T t(0.0); + double expected = 1.0 / (1.0 + T); + auto nl_f = [](auto t, const auto& x) { + auto dx = x; + dx(0) = -x(0) * x(0); + return dx; + }; + for (int i = 0; i < 500; ++i) { + x = integrator.GLRK3_AD(x, t, dt, nl_f, 80, 1e-7); + t += dt; + } + double rel_err = std::abs(x(0).real -expected) / expected; + ASSERT_TRUE(rel_err < tol_low, "GLRK3 nonlinear decay error too large"); +} +// Test case for the GLRK3 method on the stiff nonlinear decay ODE. +TEST("AD GLRK3 Method", GLRK3_StiffNonlinearDecay) { + VecX_T> x(1); + x(0) = DualNumber_T(1.0, { 0.0 }); + double T = 0.5; + DualNumber_T dt(T / 500.0); + DualNumber_T t(0.0); + double expected = 1.0 / 51.0; + auto stiff_nl_f = [](auto t, const auto& x) { + auto dx = x.eval(); + dx(0) = -100.0 * x(0) - x(0) * x(0); + return dx; + }; + for (int i = 0; i < 500; ++i) { + x = integrator.GLRK3_AD(x, t, dt, stiff_nl_f, 80, 1e-7); + t += dt; + } + double rel_err = std::abs(x(0).real - expected) / expected; + ASSERT_TRUE(rel_err < tol_low, "GLRK3 stiff nonlinear decay error too large"); +} +// Test case for the GLRK3 method on the stiff decay ODE. +TEST("AD GLRK3 Method", GLRK3_Stability_LargeStep) { + VecX_T> x(1); + x(0) = DualNumber_T(1.0, { 1.0 }); + double T = 1.0; + DualNumber_T t(0.0); + DualNumber_T dt(1.0 / 10.0); + DualNumber_T prev = x(0); + for (int i = 0; i < 10; ++i) { + x = integrator.GLRK3_AD(x, t, dt, stiffDecay, 150, 1e-12); + t += dt; + ASSERT_TRUE(std::abs(x(0).real) < std::abs(prev.real), "Not monotone"); + ASSERT_TRUE(std::abs(x(0).dual[0]) < std::abs(prev.dual[0]), "Not strictly decaying"); + prev = x(0); + } +} \ No newline at end of file From 3c903a9dd62556eab69613e5ee57b7e1e39950db Mon Sep 17 00:00:00 2001 From: saltyjoss Date: Sat, 23 May 2026 11:23:37 +0100 Subject: [PATCH 054/210] refactor: Updated AD implicit GLRK3 to use real-valued (BaseScalar) tolerance parameters, ensuring convergence checks are performed on real parts only. NOTE: * I think it must be to do with either my unit test helper methods not defining or using the dual number type correctly, or maybe in the newton raphson AD? * Since I have no errors, Imma first check trace, then go from there. * Could be that I need to embed a lambda function given the new AutoDiffJacobian logic? --- .../integrators/ad_implicit_integrators.inl | 99 +-- .../ADImplicitIntegratorTests.cpp | 662 +++++++++--------- 2 files changed, 388 insertions(+), 373 deletions(-) diff --git a/PxMLib/MathLib/include/integrators/ad_implicit_integrators.inl b/PxMLib/MathLib/include/integrators/ad_implicit_integrators.inl index afab0dae..61413339 100644 --- a/PxMLib/MathLib/include/integrators/ad_implicit_integrators.inl +++ b/PxMLib/MathLib/include/integrators/ad_implicit_integrators.inl @@ -78,9 +78,9 @@ namespace integration { int maxIter, typename mathlib::DualTraits::BaseScalar tol ) { + const Eigen::Index n = x.size(); using Real = typename mathlib::DualTraits::BaseScalar; using std::sqrt; - const Eigen::Index n = x.size(); // Coefficients for the 2-stage Gauss-Legendre method (4th order) mathlib::VecX_T c(2); @@ -102,13 +102,10 @@ namespace integration { auto eval_g = [&](const mathlib::VecX_T& k_guess, mathlib::VecX_T& g) { mathlib::VecX_T k1 = k_guess.segment(0, n); mathlib::VecX_T k2 = k_guess.segment(n, n); - mathlib::VecX_T x1 = x + dt * (A(0, 0) * k1 + A(0, 1) * k2); mathlib::VecX_T x2 = x + dt * (A(1, 0) * k1 + A(1, 1) * k2); - mathlib::VecX_T f1 = f(t + c(0) * dt, x1); mathlib::VecX_T f2 = f(t + c(1) * dt, x2); - g.resize(2 * n); g.segment(0, n) = k1 - f1; g.segment(n, n) = k2 - f2; @@ -119,33 +116,40 @@ namespace integration { mathlib::VecX_T k2 = k_guess.segment(n, n); mathlib::VecX_T x1 = x + dt * (A(0, 0) * k1 + A(0, 1) * k2); mathlib::VecX_T x2 = x + dt * (A(1, 0) * k1 + A(1, 1) * k2); - mathlib::MatX_T F1(n, n), F2(n, n); - bool analytical_success = false; + mathlib::MatX_T F1(n, n), F2(n, n); F1 = automaticDifferenceJacobian( [&](auto t_pert, const auto& x_pert) { - using Dual = std::decay_t; - Dual t_eval = Dual(t_pert) + Dual(c(0)) * Dual(dt); - return f(t_eval, x_pert); - }, + using Dual = std::decay_t; + Dual t_eval = (t_pert + Dual(c(0))) * static_cast(dt); + auto f_eval = f(t_eval, x_pert); + return f_eval.template cast(); + }, t + c(0) * dt, x1 ); F2 = automaticDifferenceJacobian( [&](auto t_pert, const auto& x_pert) { - using Dual = std::decay_t; - Dual t_eval = Dual(t_pert) + Dual(c(1)) * Dual(dt); - return f(t_eval, x_pert); - }, + using Dual = std::decay_t; + Dual t_eval = (t_pert + Dual(c(1))) * static_cast(dt); + auto f_eval = f(t_eval, x_pert); + return f_eval.template cast(); + }, t + c(1) * dt, x2 ); + Real dt_r = static_cast(dt); + mathlib::MatX_T A_r(2, 2); + A_r << + static_cast(A(0, 0)), static_cast(A(0, 1)), + static_cast(A(1, 0)), static_cast(A(1, 1)); + J.setZero(2 * n, 2 * n); - J.block(0, 0, n, n) = (mathlib::MatX_T::Identity(n, n) - dt * A(0, 0) * F1).template cast; - J.block(0, n, n, n) = (- dt * A(0, 1) * F1).template cast; - J.block(n, 0, n, n) = (- dt * A(1, 0) * F2).template cast; - J.block(n, n, n, n) = (mathlib::MatX_T::Identity(n, n) - dt * A(1, 1) * F2).template cast; + J.block(0, 0, n, n) = mathlib::MatX_T::Identity(n, n) - dt_r * A_r(0, 0) * F1; + J.block(0, n, n, n) = -dt_r * A_r(0, 1) * F1; + J.block(n, 0, n, n) = -dt_r * A_r(1, 0) * F2; + J.block(n, n, n, n) = mathlib::MatX_T::Identity(n, n) - dt_r * A_r(1, 1) * F2; }; // Solve the nonlinear system for the stage values using Newton-Raphson @@ -170,18 +174,18 @@ namespace integration { int maxIter, typename mathlib::DualTraits::BaseScalar tol ) { + const Eigen::Index n = x.size(); using Real = typename mathlib::DualTraits::BaseScalar; using std::sqrt; using std::pow; using std::abs; using std::max; - if (mathlib::real(tol) < Real(0)) { + if (tol < Real(0)) { Real dt_r = mathlib::real(dt); Real tol_r = max(Real(1e-12), Real(1e-2) * pow(dt_r, Real(7))); tol = Real(tol_r); } - const Eigen::Index n = x.size(); // Coefficients for the 3-stage Gauss-Legendre method (6th order) mathlib::VecX_T c(3); @@ -236,47 +240,58 @@ namespace integration { mathlib::VecX_T x2 = x + dt * (A(1, 0) * k1 + A(1, 1) * k2 + A(1, 2) * k3); mathlib::VecX_T x3 = x + dt * (A(2, 0) * k1 + A(2, 1) * k2 + A(2, 2) * k3); - mathlib::MatX_T F1(n, n), F2(n, n), F3(n, n); + mathlib::MatX_T F1(n, n), F2(n, n), F3(n, n); // Compute Jacobians of f at the stage points using automatic differentiation F1 = automaticDifferenceJacobian( [&](auto t_pert, const auto& x_pert) { - using Dual = std::decay_t; - Dual t_eval = Dual(t_pert) + Dual(c(0)) * Dual(dt); - return f(t_eval, x_pert); - }, + using Dual = std::decay_t; + Dual t_eval = (t_pert + Dual(c(0))) * static_cast(dt); + auto f_eval = f(t_eval, x_pert); + return f_eval.template cast(); + }, t + c(0) * dt, x1 ); // Compute the Jacobian of f at the second stage using automatic differentiation F2 = automaticDifferenceJacobian( [&](auto t_pert, const auto& x_pert) { - using Dual = std::decay_t; - Dual t_eval = Dual(t_pert) + Dual(c(1)) * Dual(dt); - return f(t_eval, x_pert); - }, + using Dual = std::decay_t; + Dual t_eval = (t_pert + Dual(c(1))) * static_cast(dt); + auto f_eval = f(t_eval, x_pert); + return f_eval.template cast(); + }, t + c(1) * dt, x2 ); // Compute the Jacobian of f at the third stage using automatic differentiation F3 = automaticDifferenceJacobian( [&](auto t_pert, const auto& x_pert) { - using Dual = std::decay_t; - Dual t_eval = Dual(t_pert) + Dual(c(2)) * Dual(dt); - return f(t_eval, x_pert); - }, + using Dual = std::decay_t; + Dual t_eval = (t_pert + Dual(c(2))) * static_cast(dt); + auto f_eval = f(t_eval, x_pert); + return f_eval.template cast(); + }, t + c(2) * dt, x3 ); + + Real dt_r = static_cast(dt); + mathlib::Mat3_T A_r(3, 3); + A_r << + static_cast(A(0, 0)), static_cast(A(0, 1)), static_cast(A(0, 2)), + static_cast(A(1, 0)), static_cast(A(1, 1)), static_cast(A(1, 2)), + static_cast(A(2, 0)), static_cast(A(2, 1)), static_cast(A(2, 2)); + J.setZero(3 * n, 3 * n); - J.block(0, 0, n, n) = (mathlib::MatX_T::Identity(n, n) - dt * A(0, 0) * F1).template cast; - J.block(0, n, n, n) = (- dt * A(0, 1) * F1).template cast; - J.block(0, 2 * n, n, n) = (- dt * A(0, 2) * F1).template cast; - J.block(n, 0, n, n) = (- dt * A(1, 0) * F2).template cast; - J.block(n, n, n, n) = (mathlib::MatX_T::Identity(n, n) - dt * A(1, 1) * F2).template cast; - J.block(n, 2 * n, n, n) = (- dt * A(1, 2) * F2).template cast; - J.block(2 * n, 0, n, n) = (- dt * A(2, 0) * F3).template cast; - J.block(2 * n, n, n, n) = (- dt * A(2, 1) * F3).template cast; - J.block(2 * n, 2 * n, n, n) = (mathlib::MatX_T::Identity(n, n) - dt * A(2, 2) * F3).template cast; + J.block(0, 0, n, n) = mathlib::MatX_T::Identity(n, n) - dt_r * A_r(0, 0) * F1; + J.block(0, n, n, n) = -dt_r * A_r(0, 1) * F1; + J.block(0, 2 * n, n, n) = -dt_r * A_r(0, 2) * F1; + J.block(n, 0, n, n) = -dt_r * A_r(1, 0) * F2; + J.block(n, n, n, n) = mathlib::MatX_T::Identity(n, n) - dt_r * A_r(1, 1) * F2; + J.block(n, 2 * n, n, n) = -dt_r * A_r(1, 2) * F2; + J.block(2 * n, 0, n, n) = -dt_r * A_r(2, 0) * F3; + J.block(2 * n, n, n, n) = dt_r * A_r(2, 1) * F3; + J.block(2 * n, 2 * n, n, n) = mathlib::MatX_T::Identity(n, n) - dt_r * A_r(2, 2) * F3; }; // Solve the nonlinear system for the stage values using Newton-Raphson diff --git a/PxMLib/MathLib_Unit/AutomaticDifferentiationTests/ADImplicitIntegratorTests.cpp b/PxMLib/MathLib_Unit/AutomaticDifferentiationTests/ADImplicitIntegratorTests.cpp index 23a4647a..a9b4bb0a 100644 --- a/PxMLib/MathLib_Unit/AutomaticDifferentiationTests/ADImplicitIntegratorTests.cpp +++ b/PxMLib/MathLib_Unit/AutomaticDifferentiationTests/ADImplicitIntegratorTests.cpp @@ -80,7 +80,7 @@ namespace { Scalar T, int N ) { - DualNumber_T dt(T / Scalar(N)); + DualNumber_T dt(T / static_cast(N)); DualNumber_T t(0.0); VecX_T> x = x0; for (int i = 0; i < N; ++i) { x = stepFn(x, t, dt); t += dt; } @@ -106,8 +106,8 @@ namespace { VecX_T r_plus = integrateToTime(stepFn, x0_plus, 1.0, N); VecX_T r_minus = integrateToTime(stepFn, x0_minus, 1.0, N); double expectedSensitivity = (r_plus(0).real - r_minus(0).real) / (2.0 * eps); - ASSERT_TRUE(std::abs(r(0).real - std::exp(-1.0)) < tol, "Exponential decay error too large"); - ASSERT_TRUE(std::abs(r(0).dual[0] - expectedSensitivity) < 5e-3, "Exponential decay sensitivity error too large"); + ASSERT_TRUE(std::abs(r(0).real - std::exp(-1.0)) < tol, "AtD decay error too large"); + ASSERT_TRUE(std::abs(r(0).dual[0] - expectedSensitivity) < 5e-3, "AD decay sensitivity error too large"); ASSERT_TRUE(std::abs(r(0).dual[0]) > 0.0, "Sensitivity vanished during propagation"); } @@ -376,332 +376,332 @@ TEST("AD Implicit Midpoint Method", ImplicitMidpoint_Stability_LargeStep) { } } -//// AD GLRK2 Tests -//// Test case for the GLRK2 method on the exponenital decay ODE. -//TEST("AD GLRK2 Method", GLRK2_ExponentialDecay) { -// VecX_T> x(1); -// x(0) = DualNumber_T(1.0, { 0.0 }); -// double T = 1.0; -// DualNumber_T dt(T / 1000.0, { 0.0 }); -// DualNumber_T t(0.0, { 0.0 }); -// for (int i = 0; i < 1000; ++i) { -// x = integrator.GLRK2(x, t, dt, expDecay, 50, 1e-6); -// t += dt; -// } -// ASSERT_TRUE(std::abs(x(0).real - std::exp(-1.0)) < tol_low, "GLRK3 exponential decay error too large"); -//} -//// Test case for the GLRK2 method on the exponential decay ODE. -//TEST("AD GLRK2 Method", GLRK2_ExponentialDecaySensitivity) { -// auto stepFn = [&]( -// const VecX_T>& x, -// DualNumber_T t, -// DualNumber_T dt -// ) { -// return integrator.GLRK2(x, t, dt, expDecay); -// }; -// verifyExpDecaySensitivity(stepFn, 1e-4, 1000); -//} -//// Test case for the GLRK2 method on the harmonic oscillator ODE. -//TEST("AD GLRK2 Method", GLRK2_HarmonicOscillator_EnergyPreservation) { -// VecX_T> x(2); -// x(0) = DualNumber_T(1.0, { 0.0, 0.0 }); -// x(1) = DualNumber_T(0.0, { 0.0, 0.0 }); -// double E0 = 0.5 * (x(0).real * x(0).real + x(1).real * x(1).real); -// double T = 5.0; -// DualNumber_T dt(T / 2500.0); -// DualNumber_T t(0.0); -// for (int i = 0; i < 2500; ++i) { -// x = integrator.GLRK2(x, t, dt, harmonicOsc, 50, 1e-6); -// t += dt; -// } -// double Ef = 0.5 * (x(0).real * x(0).real + x(1).real * x(1).real); -// double drift = std::abs(Ef - E0); -// ASSERT_TRUE(drift < tol_high * (1.0 + E0), "GLRK2 energy drift too large for SHO"); -//} -//// Test case for the GLRK2 method on the harmonic oscillator ODE. -//TEST("AD GLRK2 Method", GLRK2_HarmonicOscillatorSensitivity) { -// auto stepFn = [&]( -// const VecX_T>& x, -// DualNumber_T t, -// DualNumber_T dt -// ) { -// return integrator.GLRK2(x, t, dt, harmonicOsc); -// }; -// verifyHarmonicOscillatorSensitivity(stepFn, 1e-4, 1000); -//} -//// Test case for the GLRK2 method on the linear decay ODE. -//TEST("AD GLRK2 Method", GLRK2_LinearDecay) { -// VecX_T> x(1); -// x(0) = DualNumber_T(1.0, { 0.0 }); -// double T = 0.5; -// DualNumber_T dt(T / 500.0); -// DualNumber_T t(0.0); -// for (int i = 0; i < 500; ++i) { -// x = integrator.GLRK2(x, t, dt, linearDecay, 50, 1e-6); -// t += dt; -// } -// ASSERT_TRUE(std::abs(x(0).real - std::exp(-1.0)) < tol_low, "GLRK2 linear decay error too large"); -//} -//// Test case for the GLRK2 method on the linear decay ODE. -//TEST("AD GLRK2 Method", GLRK2_LinearDecaySensitivity) { -// auto stepFn = [&]( -// const VecX_T>& x, -// DualNumber_T t, -// DualNumber_T dt -// ) { -// return integrator.GLRK2(x, t, dt, linearDecay); -// }; -// verifyExpDecaySensitivity(stepFn, 1e-4, 1000); -//} -//// Test case for the GLRK2 method on the stiff decay ODE. -//TEST("AD GLRK2 Method", GLRK2_StiffDecay) { -// VecX_T> x(1); -// x(0) = DualNumber_T(1.0, { 0.0 }); -// double T = 0.5; -// DualNumber_T dt(T / 500.0); -// DualNumber_T t(0.0); -// double expected = std::exp(-50.0); -// auto stiff_f = [](auto t, const auto& x) { -// auto dx = x; -// dx(0) = -100.0 * x(0); -// return dx; -// }; -// for (int i = 0; i < 500; ++i) { -// x = integrator.GLRK2(x, t, dt, stiff_f, 50, 1e-10); -// t += dt; -// } -// ASSERT_TRUE(std::isfinite(x(0).real), "GLRK2 produced a non-finite result."); -// ASSERT_TRUE(x(0).real < 1e-12, "GLRK2 did not sufficiently damp stiff mode"); -//} -//// Test case for the GLRK2 method on the stiff decay ODE. -//TEST("AD GLRK2 Method", GLRK2_StiffDecaySensitivity) { -// auto stepFn = [&]( -// const VecX_T>& x, -// DualNumber_T t, -// DualNumber_T dt -// ) { -// return integrator.GLRK2(x, t, dt, stiffDecay); -// }; -// verifyExpDecaySensitivity(stepFn, 1e-4, 1000); -//} -//// Test case for the GLRK2 method on the nonlinear decay ODE. -//TEST("AD GLRK2 Method", GLRK2_NonlinearDecay) { -// VecX_T> x(1); -// x(0) = DualNumber_T(1.0, { 0.0 }); -// double T = 0.5; -// DualNumber_T dt(T / 500.0); -// DualNumber_T t(0.0); -// double expected = 1.0 / (1.0 + T); -// auto nl_f = [](auto, const auto& x) { -// auto dx = x; -// dx(0) = -x(0) * x(0); -// return dx; -// }; -// for (int i = 0; i < 500; ++i) { -// x = integrator.GLRK2(x, t, dt, nl_f, 50, 1e-6); -// t += dt; -// } -// double rel_err = std::abs(x(0).real - expected) / expected; -// ASSERT_TRUE(rel_err < tol_low, "GLRK2 nonlinear decay error too large"); -//} -//// Test case for the GLRK2 method on the stiff nonlinear decay ODE. -//TEST("AD GLRK2 Method", GLRK2_StiffNonlinearDecay) { -// VecX_T> x(1); -// x(0) = DualNumber_T(1.0, { 0.0 }); -// double T = 0.5; -// DualNumber_T dt(T / 500.0); -// DualNumber_T t(0.0); -// double expected = 1.0 / 51.0; -// auto stiff_nl_f = [](auto t, const auto& x) { -// auto dx = x; -// dx(0) = -100.0 * x(0) - x(0) * x(0); -// return dx; -// }; -// for (int i = 0; i < 500; ++i) { -// x = integrator.GLRK2(x, t, dt, stiff_nl_f, 50, 1e-6); -// t += dt; -// } -// double rel_err = std::abs(x(0).real - expected) / expected; -// ASSERT_TRUE(rel_err < tol_low, "GLRK2 stiff nonlinear decay error too large"); -//} -//// Test large step performance of GLRK2 -//TEST("AD GLRK2 Method", GLRK2_Stability_LargeStep) { -// VecX_T> x(1); -// x(0) = DualNumber_T(1.0, { 1.0 }); -// double T = 1.0; -// DualNumber_T t(0.0); -// DualNumber_T dt(1.0 / 10.0); -// DualNumber_T prev = x(0); -// for (int i = 0; i < 10; ++i) { -// x = integrator.GLRK2(x, t, dt, stiffDecay); -// t += dt; -// ASSERT_TRUE(std::abs(x(0).real) < std::abs(prev.real), "Not monotone"); -// ASSERT_TRUE(std::abs(x(0).dual[0]) < std::abs(prev.dual[0]), "Not strictly decaying"); -// prev = x(0); -// } -//} +// AD GLRK2 Tests +// Test case for the GLRK2 method on the exponenital decay ODE. +TEST("AD GLRK2 Method", GLRK2_ExponentialDecay) { + VecX_T> x(1); + x(0) = DualNumber_T(1.0, { 0.0 }); + double T = 1.0; + DualNumber_T dt(T / 1000.0, { 0.0 }); + DualNumber_T t(0.0, { 0.0 }); + for (int i = 0; i < 1000; ++i) { + x = integrator.GLRK2_AD(x, t, dt, expDecay, 50, 1e-6); + t += dt; + } + ASSERT_TRUE(std::abs(x(0).real - std::exp(-1.0)) < tol_low, "GLRK3 exponential decay error too large"); +} +// Test case for the GLRK2 method on the exponential decay ODE. +TEST("AD GLRK2 Method", GLRK2_ExponentialDecaySensitivity) { + auto stepFn = [&]( + const VecX_T>& x, + DualNumber_T t, + DualNumber_T dt + ) { + return integrator.GLRK2_AD(x, t, dt, expDecay, 50, 1e-6); + }; + verifyExpDecaySensitivity(stepFn, 1e-4, 1000); +} +// Test case for the GLRK2 method on the harmonic oscillator ODE. +TEST("AD GLRK2 Method", GLRK2_HarmonicOscillator_EnergyPreservation) { + VecX_T> x(2); + x(0) = DualNumber_T(1.0, { 0.0, 0.0 }); + x(1) = DualNumber_T(0.0, { 0.0, 0.0 }); + double E0 = 0.5 * (x(0).real * x(0).real + x(1).real * x(1).real); + double T = 5.0; + DualNumber_T dt(T / 2500.0); + DualNumber_T t(0.0); + for (int i = 0; i < 2500; ++i) { + x = integrator.GLRK2_AD(x, t, dt, harmonicOsc, 50, 1e-6); + t += dt; + } + double Ef = 0.5 * (x(0).real * x(0).real + x(1).real * x(1).real); + double drift = std::abs(Ef - E0); + ASSERT_TRUE(drift < tol_high * (1.0 + E0), "GLRK2 energy drift too large for SHO"); +} +// Test case for the GLRK2 method on the harmonic oscillator ODE. +TEST("AD GLRK2 Method", GLRK2_HarmonicOscillatorSensitivity) { + auto stepFn = [&]( + const VecX_T>& x, + DualNumber_T t, + DualNumber_T dt + ) { + return integrator.GLRK2_AD(x, t, dt, harmonicOsc, 50, 1e-6); + }; + verifyHarmonicOscillatorSensitivity(stepFn, 1e-4, 1000); +} +// Test case for the GLRK2 method on the linear decay ODE. +TEST("AD GLRK2 Method", GLRK2_LinearDecay) { + VecX_T> x(1); + x(0) = DualNumber_T(1.0, { 0.0 }); + double T = 0.5; + DualNumber_T dt(T / 500.0); + DualNumber_T t(0.0); + for (int i = 0; i < 500; ++i) { + x = integrator.GLRK2_AD(x, t, dt, linearDecay, 50, 1e-6); + t += dt; + } + ASSERT_TRUE(std::abs(x(0).real - std::exp(-1.0)) < tol_low, "GLRK2 linear decay error too large"); +} +// Test case for the GLRK2 method on the linear decay ODE. +TEST("AD GLRK2 Method", GLRK2_LinearDecaySensitivity) { + auto stepFn = [&]( + const VecX_T>& x, + DualNumber_T t, + DualNumber_T dt + ) { + return integrator.GLRK2_AD(x, t, dt, linearDecay, 50, 1e-6); + }; + verifyExpDecaySensitivity(stepFn, 1e-4, 1000); +} +// Test case for the GLRK2 method on the stiff decay ODE. +TEST("AD GLRK2 Method", GLRK2_StiffDecay) { + VecX_T> x(1); + x(0) = DualNumber_T(1.0, { 0.0 }); + double T = 0.5; + DualNumber_T dt(T / 500.0); + DualNumber_T t(0.0); + double expected = std::exp(-50.0); + auto stiff_f = [](auto t, const auto& x) { + auto dx = x; + dx(0) = -100.0 * x(0); + return dx; + }; + for (int i = 0; i < 500; ++i) { + x = integrator.GLRK2_AD(x, t, dt, stiff_f, 50, 1e-10); + t += dt; + } + ASSERT_TRUE(std::isfinite(x(0).real), "GLRK2 produced a non-finite result."); + ASSERT_TRUE(x(0).real < 1e-12, "GLRK2 did not sufficiently damp stiff mode"); +} +// Test case for the GLRK2 method on the stiff decay ODE. +TEST("AD GLRK2 Method", GLRK2_StiffDecaySensitivity) { + auto stepFn = [&]( + const VecX_T>& x, + DualNumber_T t, + DualNumber_T dt + ) { + return integrator.GLRK2_AD(x, t, dt, stiffDecay, 50, 1e-6); + }; + verifyExpDecaySensitivity(stepFn, 1e-4, 1000); +} +// Test case for the GLRK2 method on the nonlinear decay ODE. +TEST("AD GLRK2 Method", GLRK2_NonlinearDecay) { + VecX_T> x(1); + x(0) = DualNumber_T(1.0, { 0.0 }); + double T = 0.5; + DualNumber_T dt(T / 500.0); + DualNumber_T t(0.0); + double expected = 1.0 / (1.0 + T); + auto nl_f = [](auto, const auto& x) { + auto dx = x; + dx(0) = -x(0) * x(0); + return dx; + }; + for (int i = 0; i < 500; ++i) { + x = integrator.GLRK2_AD(x, t, dt, nl_f, 50, 1e-6); + t += dt; + } + double rel_err = std::abs(x(0).real - expected) / expected; + ASSERT_TRUE(rel_err < tol_low, "GLRK2 nonlinear decay error too large"); +} +// Test case for the GLRK2 method on the stiff nonlinear decay ODE. +TEST("AD GLRK2 Method", GLRK2_StiffNonlinearDecay) { + VecX_T> x(1); + x(0) = DualNumber_T(1.0, { 0.0 }); + double T = 0.5; + DualNumber_T dt(T / 500.0); + DualNumber_T t(0.0); + double expected = 1.0 / 51.0; + auto stiff_nl_f = [](auto t, const auto& x) { + auto dx = x; + dx(0) = -100.0 * x(0) - x(0) * x(0); + return dx; + }; + for (int i = 0; i < 500; ++i) { + x = integrator.GLRK2_AD(x, t, dt, stiff_nl_f, 50, 1e-6); + t += dt; + } + double rel_err = std::abs(x(0).real - expected) / expected; + ASSERT_TRUE(rel_err < tol_low, "GLRK2 stiff nonlinear decay error too large"); +} +// Test large step performance of GLRK2 +TEST("AD GLRK2 Method", GLRK2_Stability_LargeStep) { + VecX_T> x(1); + x(0) = DualNumber_T(1.0, { 1.0 }); + double T = 1.0; + DualNumber_T t(0.0); + DualNumber_T dt(1.0 / 120.0); + DualNumber_T prev = x(0); + for (int i = 0; i < 10; ++i) { + x = integrator.GLRK2_AD(x, t, dt, stiffDecay, 50, 1e-6); + t += dt; + ASSERT_TRUE(std::abs(x(0).real) < std::abs(prev.real), "Not monotone"); + ASSERT_TRUE(std::abs(x(0).dual[0]) < std::abs(prev.dual[0]), "Not strictly decaying"); + prev = x(0); + } +} -//// AD GLRK3 Tests -//// Test case for the GLRK3 method on the exponenital decay ODE. -//TEST("AD GLRK3 Method", GLRK3_ExponentialDecay) { -// VecX_T> x(1); -// x(0) = DualNumber_T(1.0, { 0.0 }); -// double T = 1.0; -// DualNumber_T dt(T / 1000.0, { 0.0 }); -// DualNumber_T t(0.0, { 0.0 }); -// for (int i = 0; i < 1000; ++i) { -// x = integrator.GLRK3(x, t, dt, expDecay, 80, 1e-7); -// t += dt; -// } -// ASSERT_TRUE(std::abs(x(0).real - std::exp(-1.0)) < tol_low, "GLRK3 exponential decay error too large"); -//} -//// Test case for the GLRK3 method on the exponential decay ODE sensitivity. -//TEST("AD GLRK3 Method", GLRK3_ExponentialDecaySensitivity) { -// auto stepFn = [&]( -// const VecX_T>& x, -// DualNumber_T t, -// DualNumber_T dt -// ) { -// return integrator.GLRK3(x, t, dt, expDecay); -// }; -// verifyExpDecaySensitivity(stepFn, 1e-4, 1000); -//} -//// Test case for the GLRK3 method on the harmonic oscillator ODE. -//TEST("AD GLRK3 Method", GLRK3_HarmonicOscillator_EnergyPreservation) { -// VecX_T> x(2); -// x(0) = DualNumber_T(1.0, { 0.0, 0.0 }); -// x(1) = DualNumber_T(0.0, { 0.0, 0.0 }); -// double E0 = 0.5 * (x(0).real * x(0).real + x(1).real * x(1).real); -// double T = 5.0; -// DualNumber_T dt(T / 2500.0); -// DualNumber_T t(0.0); -// for (int i = 0; i < 2500; ++i) { -// x = integrator.GLRK3(x, t, dt, harmonicOsc, 80, 1e-7); -// t += dt; -// } -// double Ef = 0.5 * (x(0).real * x(0).real + x(1).real * x(1).real); -// double drift = std::abs(Ef - E0); -// ASSERT_TRUE(drift < tol_high * (1.0 + E0), "GLRK3 energy drift too large for SHO"); -//} -//// Test case for the GLRK3 method on the harmonic oscillator ODE. -//TEST("AD GLRK3 Method", GLRK3_HarmonicOscillatorSensitivity) { -// auto stepFn = [&]( -// const VecX_T>& x, -// DualNumber_T t, -// DualNumber_T dt -// ) { -// return integrator.GLRK3(x, t, dt, harmonicOsc); -// }; -// verifyHarmonicOscillatorSensitivity(stepFn, 1e-4, 1000); -//} -//// Test case for the GLRK3 method on the linear decay ODE. -//TEST("AD GLRK3 Method", GLRK3_LinearDecay) { -// VecX_T> x(1); -// x(0) = DualNumber_T(1.0, { 0.0 }); -// double T = 0.5; -// DualNumber_T dt(T / 500.0); -// DualNumber_T t(0.0); -// for (int i = 0; i < 500; ++i) { -// x = integrator.GLRK3(x, t, dt, linearDecay, 80, 1e-7); -// t += dt; -// } -// ASSERT_TRUE(std::abs(x(0).real - std::exp(-1.0)) < tol_low, "GLRK3 linear decay error too large"); -//} -//// Test case for the GLRK3 method on the linear decay ODE. -//TEST("AD GLRK3 Method", GLRK3_LinearDecaySensitivity) { -// auto stepFn = [&]( -// const VecX_T>& x, -// DualNumber_T t, -// DualNumber_T dt -// ) { -// return integrator.GLRK3(x, t, dt, linearDecay); -// }; -// verifyExpDecaySensitivity(stepFn, 1e-12, 1000); -//} -//// Test case for the GLRK3 method on the stiff decay ODE. -//TEST("AD GLRK3 Method", GLRK3_StiffDecay) { -// VecX_T> x(1); -// x(0) = DualNumber_T(1.0, { 0.0 }); -// double T = 0.5; -// DualNumber_T dt(T / 500.0); -// DualNumber_T t(0.0); -// double expected = std::exp(-50.0); -// auto stiff_f = [](auto t, const auto& x) { -// auto dx = x; -// dx(0) = -100.0 * x(0); -// return dx; -// }; -// for (int i = 0; i < 500; ++i) { -// x = integrator.GLRK3(x, t, dt, stiff_f, 150, 1e-12); -// t += dt; -// } -// ASSERT_TRUE(std::isfinite(x(0).real), "GLRK3 produced a non-finite result."); -// ASSERT_TRUE(x(0).real < 1e-14, "GLRK3 did not sufficiently damp stiff mode"); -//} -//// Test case for the GLRK3 method on the stiff decay ODE. -//TEST("AD GLRK3 Method", GLRK3_StiffDecaySensitivity) { -// auto stepFn = [&]( -// const VecX_T>& x, -// DualNumber_T t, -// DualNumber_T dt -// ) { -// return integrator.GLRK3(x, t, dt, stiffDecay); -// }; -// verifyExpDecaySensitivity(stepFn, 1e-12, 1000); -//} -//// Test case for the GLRK3 method on the nonlinear decay ODE. -//TEST("AD GLRK3 Method", GLRK3_NonlinearDecay) { -// VecX_T> x(1); -// x(0) = DualNumber_T(1.0, { 0.0 }); -// double T = 0.5; -// DualNumber_T dt(T / 500.0); -// DualNumber_T t(0.0); -// double expected = 1.0 / (1.0 + T); -// auto nl_f = [](auto t, const auto& x) { -// auto dx = x; -// dx(0) = -x(0) * x(0); -// return dx; -// }; -// for (int i = 0; i < 500; ++i) { -// x = integrator.GLRK3(x, t, dt, nl_f, 80, 1e-7); -// t += dt; -// } -// double rel_err = std::abs(x(0).real -expected) / expected; -// ASSERT_TRUE(rel_err < tol_low, "GLRK3 nonlinear decay error too large"); -//} -//// Test case for the GLRK3 method on the stiff nonlinear decay ODE. -//TEST("AD GLRK3 Method", GLRK3_StiffNonlinearDecay) { -// VecX_T> x(1); -// x(0) = DualNumber_T(1.0, { 0.0 }); -// double T = 0.5; -// DualNumber_T dt(T / 500.0); -// DualNumber_T t(0.0); -// double expected = 1.0 / 51.0; -// auto stiff_nl_f = [](auto t, const auto& x) { -// auto dx = x.eval(); -// dx(0) = -100.0 * x(0) - x(0) * x(0); -// return dx; -// }; -// for (int i = 0; i < 500; ++i) { -// x = integrator.GLRK3(x, t, dt, stiff_nl_f, 80, 1e-7); -// t += dt; -// } -// double rel_err = std::abs(x(0).real - expected) / expected; -// ASSERT_TRUE(rel_err < tol_low, "GLRK3 stiff nonlinear decay error too large"); -//} -//// Test case for the GLRK3 method on the stiff decay ODE. -//TEST("AD GLRK3 Method", GLRK3_Stability_LargeStep) { -// VecX_T> x(1); -// x(0) = DualNumber_T(1.0, { 1.0 }); -// double T = 1.0; -// DualNumber_T t(0.0); -// DualNumber_T dt(1.0 / 10.0); -// DualNumber_T prev = x(0); -// for (int i = 0; i < 10; ++i) { -// x = integrator.GLRK3(x, t, dt, stiffDecay); -// t += dt; -// ASSERT_TRUE(std::abs(x(0).real) < std::abs(prev.real), "Not monotone"); -// ASSERT_TRUE(std::abs(x(0).dual[0]) < std::abs(prev.dual[0]), "Not strictly decaying"); -// prev = x(0); -// } -//} \ No newline at end of file +// AD GLRK3 Tests +// Test case for the GLRK3 method on the exponenital decay ODE. +TEST("AD GLRK3 Method", GLRK3_ExponentialDecay) { + VecX_T> x(1); + x(0) = DualNumber_T(1.0, { 0.0 }); + double T = 1.0; + DualNumber_T dt(T / 1000.0, { 0.0 }); + DualNumber_T t(0.0, { 0.0 }); + for (int i = 0; i < 1000; ++i) { + x = integrator.GLRK3_AD(x, t, dt, expDecay, 80, 1e-7); + t += dt; + } + ASSERT_TRUE(std::abs(x(0).real - std::exp(-1.0)) < tol_low, "GLRK3 exponential decay error too large"); +} +// Test case for the GLRK3 method on the exponential decay ODE sensitivity. +TEST("AD GLRK3 Method", GLRK3_ExponentialDecaySensitivity) { + auto stepFn = [&]( + const VecX_T>& x, + DualNumber_T t, + DualNumber_T dt + ) { + return integrator.GLRK3_AD(x, t, dt, expDecay, 80, 1e-7); + }; + verifyExpDecaySensitivity(stepFn, 1e-4, 1000); +} +// Test case for the GLRK3 method on the harmonic oscillator ODE. +TEST("AD GLRK3 Method", GLRK3_HarmonicOscillator_EnergyPreservation) { + VecX_T> x(2); + x(0) = DualNumber_T(1.0, { 0.0, 0.0 }); + x(1) = DualNumber_T(0.0, { 0.0, 0.0 }); + double E0 = 0.5 * (x(0).real * x(0).real + x(1).real * x(1).real); + double T = 5.0; + DualNumber_T dt(T / 2500.0); + DualNumber_T t(0.0); + for (int i = 0; i < 2500; ++i) { + x = integrator.GLRK3_AD(x, t, dt, harmonicOsc, 80, 1e-7); + t += dt; + } + double Ef = 0.5 * (x(0).real * x(0).real + x(1).real * x(1).real); + double drift = std::abs(Ef - E0); + ASSERT_TRUE(drift < tol_high * (1.0 + E0), "GLRK3 energy drift too large for SHO"); +} +// Test case for the GLRK3 method on the harmonic oscillator ODE. +TEST("AD GLRK3 Method", GLRK3_HarmonicOscillatorSensitivity) { + auto stepFn = [&]( + const VecX_T>& x, + DualNumber_T t, + DualNumber_T dt + ) { + return integrator.GLRK3_AD(x, t, dt, harmonicOsc, 80, 1e-7); + }; + verifyHarmonicOscillatorSensitivity(stepFn, 1e-4, 1000); +} +// Test case for the GLRK3 method on the linear decay ODE. +TEST("AD GLRK3 Method", GLRK3_LinearDecay) { + VecX_T> x(1); + x(0) = DualNumber_T(1.0, { 0.0 }); + double T = 0.5; + DualNumber_T dt(T / 500.0); + DualNumber_T t(0.0); + for (int i = 0; i < 500; ++i) { + x = integrator.GLRK3_AD(x, t, dt, linearDecay, 80, 1e-7); + t += dt; + } + ASSERT_TRUE(std::abs(x(0).real - std::exp(-1.0)) < tol_low, "GLRK3 linear decay error too large"); +} +// Test case for the GLRK3 method on the linear decay ODE. +TEST("AD GLRK3 Method", GLRK3_LinearDecaySensitivity) { + auto stepFn = [&]( + const VecX_T>& x, + DualNumber_T t, + DualNumber_T dt + ) { + return integrator.GLRK3_AD(x, t, dt, linearDecay, 80, 1e-7); + }; + verifyExpDecaySensitivity(stepFn, 1e-12, 1000); +} +// Test case for the GLRK3 method on the stiff decay ODE. +TEST("AD GLRK3 Method", GLRK3_StiffDecay) { + VecX_T> x(1); + x(0) = DualNumber_T(1.0, { 0.0 }); + double T = 0.5; + DualNumber_T dt(T / 500.0); + DualNumber_T t(0.0); + double expected = std::exp(-50.0); + auto stiff_f = [](auto t, const auto& x) { + auto dx = x; + dx(0) = -100.0 * x(0); + return dx; + }; + for (int i = 0; i < 500; ++i) { + x = integrator.GLRK3_AD(x, t, dt, stiff_f, 150, 1e-12); + t += dt; + } + ASSERT_TRUE(std::isfinite(x(0).real), "GLRK3 produced a non-finite result."); + ASSERT_TRUE(x(0).real < 1e-14, "GLRK3 did not sufficiently damp stiff mode"); +} +// Test case for the GLRK3 method on the stiff decay ODE. +TEST("AD GLRK3 Method", GLRK3_StiffDecaySensitivity) { + auto stepFn = [&]( + const VecX_T>& x, + DualNumber_T t, + DualNumber_T dt + ) { + return integrator.GLRK3_AD(x, t, dt, stiffDecay, 150, 1e-12); + }; + verifyExpDecaySensitivity(stepFn, 1e-12, 1000); +} +// Test case for the GLRK3 method on the nonlinear decay ODE. +TEST("AD GLRK3 Method", GLRK3_NonlinearDecay) { + VecX_T> x(1); + x(0) = DualNumber_T(1.0, { 0.0 }); + double T = 0.5; + DualNumber_T dt(T / 500.0); + DualNumber_T t(0.0); + double expected = 1.0 / (1.0 + T); + auto nl_f = [](auto t, const auto& x) { + auto dx = x; + dx(0) = -x(0) * x(0); + return dx; + }; + for (int i = 0; i < 500; ++i) { + x = integrator.GLRK3_AD(x, t, dt, nl_f, 80, 1e-7); + t += dt; + } + double rel_err = std::abs(x(0).real -expected) / expected; + ASSERT_TRUE(rel_err < tol_low, "GLRK3 nonlinear decay error too large"); +} +// Test case for the GLRK3 method on the stiff nonlinear decay ODE. +TEST("AD GLRK3 Method", GLRK3_StiffNonlinearDecay) { + VecX_T> x(1); + x(0) = DualNumber_T(1.0, { 0.0 }); + double T = 0.5; + DualNumber_T dt(T / 500.0); + DualNumber_T t(0.0); + double expected = 1.0 / 51.0; + auto stiff_nl_f = [](auto t, const auto& x) { + auto dx = x.eval(); + dx(0) = -100.0 * x(0) - x(0) * x(0); + return dx; + }; + for (int i = 0; i < 500; ++i) { + x = integrator.GLRK3_AD(x, t, dt, stiff_nl_f, 80, 1e-7); + t += dt; + } + double rel_err = std::abs(x(0).real - expected) / expected; + ASSERT_TRUE(rel_err < tol_low, "GLRK3 stiff nonlinear decay error too large"); +} +// Test case for the GLRK3 method on the stiff decay ODE. +TEST("AD GLRK3 Method", GLRK3_Stability_LargeStep) { + VecX_T> x(1); + x(0) = DualNumber_T(1.0, { 1.0 }); + double T = 1.0; + DualNumber_T t(0.0); + DualNumber_T dt(1.0 / 10.0); + DualNumber_T prev = x(0); + for (int i = 0; i < 10; ++i) { + x = integrator.GLRK3_AD(x, t, dt, stiffDecay, 150, 1e-12); + t += dt; + ASSERT_TRUE(std::abs(x(0).real) < std::abs(prev.real), "Not monotone"); + ASSERT_TRUE(std::abs(x(0).dual[0]) < std::abs(prev.dual[0]), "Not strictly decaying"); + prev = x(0); + } +} \ No newline at end of file From d0cdada336d8d4119ae7b72bcee09156de541d8b Mon Sep 17 00:00:00 2001 From: saltyjoss Date: Sat, 23 May 2026 12:41:22 +0100 Subject: [PATCH 055/210] fixes: Corrected incorrect index in `automaticDifferenceJacobian`, use of initial state instead of final state in `newtonRaphsonAD`, and removal of bracket in stage time computation in `GLRK2_AD` and `GLRK3_AD` DISCLAIMER: * I used AI tools to help identify where exactly in the code these were occuring, glad I did as I think I would have kept missing them. * Still need to fix non-linear stiff decay test and Implicit Midpoint (AD) sensitiviity unit tests. --- .../integrators/ad_implicit_integrators.inl | 16 ++--- .../ADImplicitIntegratorTests.cpp | 68 ++++++++++--------- 2 files changed, 43 insertions(+), 41 deletions(-) diff --git a/PxMLib/MathLib/include/integrators/ad_implicit_integrators.inl b/PxMLib/MathLib/include/integrators/ad_implicit_integrators.inl index 61413339..835c15d5 100644 --- a/PxMLib/MathLib/include/integrators/ad_implicit_integrators.inl +++ b/PxMLib/MathLib/include/integrators/ad_implicit_integrators.inl @@ -121,7 +121,7 @@ namespace integration { F1 = automaticDifferenceJacobian( [&](auto t_pert, const auto& x_pert) { using Dual = std::decay_t; - Dual t_eval = (t_pert + Dual(c(0))) * static_cast(dt); + Dual t_eval = t_pert + Dual(c(0)) * static_cast(dt); auto f_eval = f(t_eval, x_pert); return f_eval.template cast(); }, @@ -131,7 +131,7 @@ namespace integration { F2 = automaticDifferenceJacobian( [&](auto t_pert, const auto& x_pert) { using Dual = std::decay_t; - Dual t_eval = (t_pert + Dual(c(1))) * static_cast(dt); + Dual t_eval = t_pert + Dual(c(1)) * static_cast(dt); auto f_eval = f(t_eval, x_pert); return f_eval.template cast(); }, @@ -245,7 +245,7 @@ namespace integration { F1 = automaticDifferenceJacobian( [&](auto t_pert, const auto& x_pert) { using Dual = std::decay_t; - Dual t_eval = (t_pert + Dual(c(0))) * static_cast(dt); + Dual t_eval = t_pert + Dual(c(0)) * static_cast(dt); auto f_eval = f(t_eval, x_pert); return f_eval.template cast(); }, @@ -256,7 +256,7 @@ namespace integration { F2 = automaticDifferenceJacobian( [&](auto t_pert, const auto& x_pert) { using Dual = std::decay_t; - Dual t_eval = (t_pert + Dual(c(1))) * static_cast(dt); + Dual t_eval = t_pert + Dual(c(1)) * static_cast(dt); auto f_eval = f(t_eval, x_pert); return f_eval.template cast(); }, @@ -267,7 +267,7 @@ namespace integration { F3 = automaticDifferenceJacobian( [&](auto t_pert, const auto& x_pert) { using Dual = std::decay_t; - Dual t_eval = (t_pert + Dual(c(2))) * static_cast(dt); + Dual t_eval = t_pert + Dual(c(2)) * static_cast(dt); auto f_eval = f(t_eval, x_pert); return f_eval.template cast(); }, @@ -290,7 +290,7 @@ namespace integration { J.block(n, n, n, n) = mathlib::MatX_T::Identity(n, n) - dt_r * A_r(1, 1) * F2; J.block(n, 2 * n, n, n) = -dt_r * A_r(1, 2) * F2; J.block(2 * n, 0, n, n) = -dt_r * A_r(2, 0) * F3; - J.block(2 * n, n, n, n) = dt_r * A_r(2, 1) * F3; + J.block(2 * n, n, n, n) = -dt_r * A_r(2, 1) * F3; J.block(2 * n, 2 * n, n, n) = mathlib::MatX_T::Identity(n, n) - dt_r * A_r(2, 2) * F3; }; @@ -349,7 +349,7 @@ namespace integration { eval_j(x_final, J); solver.compute(J); mathlib::VecX_T g_final; - eval_g(x0, g_final); + eval_g(x_final, g_final); constexpr size_t NVar = mathlib::DualTraits::Dimension; for (size_t d = 0; d < NVar; ++d) { @@ -381,7 +381,7 @@ namespace integration { mathlib::VecX_T x_dual(n); for (int k = 0; k < n; ++k) { std::array seed_array{}; - if (k == 1) { seed_array.fill(RealScalar(0)); seed_array[0] = RealScalar(1); } + if (k == i) { seed_array.fill(RealScalar(0)); seed_array[0] = RealScalar(1); } x_dual(k) = Dual_T(mathlib::real(x(k)), seed_array); } diff --git a/PxMLib/MathLib_Unit/AutomaticDifferentiationTests/ADImplicitIntegratorTests.cpp b/PxMLib/MathLib_Unit/AutomaticDifferentiationTests/ADImplicitIntegratorTests.cpp index a9b4bb0a..4d24011a 100644 --- a/PxMLib/MathLib_Unit/AutomaticDifferentiationTests/ADImplicitIntegratorTests.cpp +++ b/PxMLib/MathLib_Unit/AutomaticDifferentiationTests/ADImplicitIntegratorTests.cpp @@ -92,6 +92,7 @@ namespace { void verifyExpDecaySensitivity( StepFn stepFn, double tol, + double exactFinal, int N ) { using Dual = DualNumber_T; @@ -99,14 +100,13 @@ namespace { x0(0) = Dual(1.0, { 1.0 }); VecX_T r = integrateToTime(stepFn, x0, 1.0, N); double eps = 1e-6; - VecX_T x0_plus(1); + VecX_T x0_plus(1), x0_minus(1); x0_plus(0) = Dual(1.0 + eps, {0.0}); - VecX_T x0_minus(1); x0_minus(0) = Dual(1.0 - eps, {0.0}); VecX_T r_plus = integrateToTime(stepFn, x0_plus, 1.0, N); VecX_T r_minus = integrateToTime(stepFn, x0_minus, 1.0, N); double expectedSensitivity = (r_plus(0).real - r_minus(0).real) / (2.0 * eps); - ASSERT_TRUE(std::abs(r(0).real - std::exp(-1.0)) < tol, "AtD decay error too large"); + ASSERT_TRUE(std::abs(r(0).real - exactFinal) < tol, "State error too large"); ASSERT_TRUE(std::abs(r(0).dual[0] - expectedSensitivity) < 5e-3, "AD decay sensitivity error too large"); ASSERT_TRUE(std::abs(r(0).dual[0]) > 0.0, "Sensitivity vanished during propagation"); } @@ -125,22 +125,20 @@ namespace { VecX_T r = integrateToTime(stepFn, x0, TWO_PI_d, N); double eps = 1e-6; - VecX_T x0_u_plus(2); + VecX_T x0_u_plus(2), x0_u_minus(2); x0_u_plus(0) = Dual(1.0 + eps, {0.0, 0.0}); x0_u_plus(1) = Dual(0.0, { 0.0, 0.0 }); - VecX_T x0_u_minus(2); x0_u_minus(0) = Dual(1.0 - eps, {0.0, 0.0}); x0_u_minus(1) = Dual(0.0, { 0.0, 0.0 }); VecX_T r_u_plus = integrateToTime(stepFn, x0_u_plus, TWO_PI_d, N); VecX_T r_u_minus = integrateToTime(stepFn, x0_u_minus, TWO_PI_d, N); double expected_du_du0 = (r_u_plus(0).real - r_u_minus(0).real) / (2.0 * eps); - VecX_T x0_v_plus(2); - x0_v_plus(0) = Dual(1.0 + eps, { 0.0, 0.0 }); - x0_v_plus(1) = Dual(0.0, { 0.0, 0.0 }); - VecX_T x0_v_minus(2); - x0_v_minus(0) = Dual(1.0 - eps, { 0.0, 0.0 }); - x0_v_minus(1) = Dual(0.0, { 0.0, 0.0 }); + VecX_T x0_v_plus(2), x0_v_minus(2); + x0_v_plus(0) = Dual(1.0, { 0.0, 0.0 }); + x0_v_plus(1) = Dual(0.0 + eps, { 0.0, 0.0 }); + x0_v_minus(0) = Dual(1.0, { 0.0, 0.0 }); + x0_v_minus(1) = Dual(0.0 - eps, { 0.0, 0.0 }); VecX_T r_v_plus = integrateToTime(stepFn, x0_v_plus, TWO_PI_d, N); VecX_T r_v_minus = integrateToTime(stepFn, x0_v_minus, TWO_PI_d, N); double expected_dv_dv0 = (r_v_plus(1).real - r_v_minus(1).real) / (2.0 * eps); @@ -150,7 +148,7 @@ namespace { ASSERT_TRUE(std::abs(r(0).real - expectedPosition) < tol, "Harmonic oscillator position state error too large"); ASSERT_TRUE(std::abs(r(1).real - expectedVelocity) < tol, "Harmonic oscillator velocity state error too large"); ASSERT_TRUE(std::abs(r(0).dual[0] - expected_du_du0) < 5e-3, "Position sensitivity AD track broken"); - ASSERT_TRUE(std::abs(r(1).dual[1] - expected_dv_dv0) < 5e-3, "Position sensitivity AD track broken"); + ASSERT_TRUE(std::abs(r(1).dual[1] - expected_dv_dv0) < 5e-3, "Velocity sensitivity AD track broken"); } } @@ -177,7 +175,7 @@ TEST("AD Implicit Euler Method", ImplicitEuler_ExponentialDecaySensitivity) { ) { return integrator.implicitEuler_AD(x, t, dt, expDecay, 50, 1e-5); }; - verifyExpDecaySensitivity(stepFn, 1e-2, 100); + verifyExpDecaySensitivity(stepFn, 1e-2, std::exp(-1.0), 100); } // Test case for the Implicit Midpoint method on the linear decay ODE. TEST("AD Implicit Euler Method", ImplicitEuler_LinearDecay) { @@ -201,7 +199,7 @@ TEST("AD Implicit Euler Method", ImplicitEuler_LinearDecaySensitivity) { ) { return integrator.implicitEuler_AD(x, t, dt, linearDecay, 50, 1e-5); }; - verifyExpDecaySensitivity(stepFn, 1e-2, 1000); + verifyExpDecaySensitivity(stepFn, 1e-2, std::exp(-2.0), 1000); } // Test case for the Implicit Euler method on the stiff decay ODE. TEST("AD Implicit Euler Method", ImplicitEuler_StiffDecay) { @@ -232,7 +230,7 @@ TEST("AD Implicit Euler Method", ImplicitEuler_StiffDecaySensitivity) { ) { return integrator.implicitEuler_AD(x, t, dt, stiffDecay, 50, 1e-5); }; - verifyExpDecaySensitivity(stepFn, 1e-2, 1000); + verifyExpDecaySensitivity(stepFn, 1e-2, std::exp(-100.0), 1000); } // Test large step performance of implicit midpoint TEST("AD Implicit Euler Method", ImplicitEuler_Stability_LargeStep) { @@ -274,7 +272,7 @@ TEST("AD Implicit Midpoint Method", ImplicitMidpoint_ExponentialDecaySensitivity ) { return integrator.implicitMidpoint_AD(x, t, dt, expDecay, 20, 1e-6); }; - verifyExpDecaySensitivity(stepFn, 1e-2, 1000); + verifyExpDecaySensitivity(stepFn, 1e-2, std::exp(-1.0), 1000); } // Test case for the Implicit Midpoint method on the harmonic oscillator ODE. TEST("AD Implicit Midpoint Method", ImplicitMidpoint_HarmonicOscillator_EnergyPreservation) { @@ -326,7 +324,7 @@ TEST("AD Implicit Midpoint Method", ImplicitMidpoint_LinearDecaySensitivity) { ) { return integrator.implicitMidpoint_AD(x, t, dt, linearDecay, 20, 1e-6); }; - verifyExpDecaySensitivity(stepFn, 1e-2, 1000); + verifyExpDecaySensitivity(stepFn, 1e-2, std::exp(-2.0), 1000); } // Test case for the Implicit Midpoint method on the stiff decay ODE. TEST("AD Implicit Midpoint Method", ImplicitMidpoint_StiffDecay) { @@ -357,7 +355,7 @@ TEST("AD Implicit Midpoint Method", ImplicitMidpoint_StiffDecaySensitivity) { ) { return integrator.implicitMidpoint_AD(x, t, dt, stiffDecay, 20, 1e-6); }; - verifyExpDecaySensitivity(stepFn, 1e-2, 1000); + verifyExpDecaySensitivity(stepFn, 1e-2, std::exp(-100.0), 1000); } // Test large step performance of implicit midpoint TEST("AD Implicit Midpoint Method", ImplicitMidpoint_Stability_LargeStep) { @@ -399,7 +397,7 @@ TEST("AD GLRK2 Method", GLRK2_ExponentialDecaySensitivity) { ) { return integrator.GLRK2_AD(x, t, dt, expDecay, 50, 1e-6); }; - verifyExpDecaySensitivity(stepFn, 1e-4, 1000); + verifyExpDecaySensitivity(stepFn, 1e-4, std::exp(-1.0), 1000); } // Test case for the GLRK2 method on the harmonic oscillator ODE. TEST("AD GLRK2 Method", GLRK2_HarmonicOscillator_EnergyPreservation) { @@ -451,7 +449,7 @@ TEST("AD GLRK2 Method", GLRK2_LinearDecaySensitivity) { ) { return integrator.GLRK2_AD(x, t, dt, linearDecay, 50, 1e-6); }; - verifyExpDecaySensitivity(stepFn, 1e-4, 1000); + verifyExpDecaySensitivity(stepFn, 1e-4, std::exp(-2.0), 1000); } // Test case for the GLRK2 method on the stiff decay ODE. TEST("AD GLRK2 Method", GLRK2_StiffDecay) { @@ -482,7 +480,7 @@ TEST("AD GLRK2 Method", GLRK2_StiffDecaySensitivity) { ) { return integrator.GLRK2_AD(x, t, dt, stiffDecay, 50, 1e-6); }; - verifyExpDecaySensitivity(stepFn, 1e-4, 1000); + verifyExpDecaySensitivity(stepFn, 1e-4, std::exp(-100.0), 1000); } // Test case for the GLRK2 method on the nonlinear decay ODE. TEST("AD GLRK2 Method", GLRK2_NonlinearDecay) { @@ -511,16 +509,16 @@ TEST("AD GLRK2 Method", GLRK2_StiffNonlinearDecay) { double T = 0.5; DualNumber_T dt(T / 500.0); DualNumber_T t(0.0); - double expected = 1.0 / 51.0; auto stiff_nl_f = [](auto t, const auto& x) { auto dx = x; - dx(0) = -100.0 * x(0) - x(0) * x(0); + dx(0) = -x(0) * (100.0 + x(0)); return dx; }; for (int i = 0; i < 500; ++i) { x = integrator.GLRK2_AD(x, t, dt, stiff_nl_f, 50, 1e-6); t += dt; } + double expected = 100.0 / (101.0 * std::exp(100.0 * T) - 1.0); double rel_err = std::abs(x(0).real - expected) / expected; ASSERT_TRUE(rel_err < tol_low, "GLRK2 stiff nonlinear decay error too large"); } @@ -562,9 +560,10 @@ TEST("AD GLRK3 Method", GLRK3_ExponentialDecaySensitivity) { DualNumber_T t, DualNumber_T dt ) { - return integrator.GLRK3_AD(x, t, dt, expDecay, 80, 1e-7); + auto f = [](auto t, const auto& x) { return expDecay(t, x); }; + return integrator.GLRK3_AD(x, t, dt, f, 80, 1e-7); }; - verifyExpDecaySensitivity(stepFn, 1e-4, 1000); + verifyExpDecaySensitivity(stepFn, 1e-4, std::exp(-1.0), 1000); } // Test case for the GLRK3 method on the harmonic oscillator ODE. TEST("AD GLRK3 Method", GLRK3_HarmonicOscillator_EnergyPreservation) { @@ -590,7 +589,8 @@ TEST("AD GLRK3 Method", GLRK3_HarmonicOscillatorSensitivity) { DualNumber_T t, DualNumber_T dt ) { - return integrator.GLRK3_AD(x, t, dt, harmonicOsc, 80, 1e-7); + auto f = [](auto t, const auto& x) { return harmonicOsc(t, x); }; + return integrator.GLRK3_AD(x, t, dt, f, 80, 1e-7); }; verifyHarmonicOscillatorSensitivity(stepFn, 1e-4, 1000); } @@ -613,10 +613,11 @@ TEST("AD GLRK3 Method", GLRK3_LinearDecaySensitivity) { const VecX_T>& x, DualNumber_T t, DualNumber_T dt - ) { - return integrator.GLRK3_AD(x, t, dt, linearDecay, 80, 1e-7); + ) { + auto f = [](auto t, const auto& x) { return linearDecay(t, x); }; + return integrator.GLRK3_AD(x, t, dt, f, 80, 1e-7); }; - verifyExpDecaySensitivity(stepFn, 1e-12, 1000); + verifyExpDecaySensitivity(stepFn, 1e-12, std::exp(-2.0), 1000); } // Test case for the GLRK3 method on the stiff decay ODE. TEST("AD GLRK3 Method", GLRK3_StiffDecay) { @@ -645,9 +646,10 @@ TEST("AD GLRK3 Method", GLRK3_StiffDecaySensitivity) { DualNumber_T t, DualNumber_T dt ) { - return integrator.GLRK3_AD(x, t, dt, stiffDecay, 150, 1e-12); + auto f = [](auto t, const auto& x) { return stiffDecay(t, x); }; + return integrator.GLRK3_AD(x, t, dt, f, 150, 1e-12); }; - verifyExpDecaySensitivity(stepFn, 1e-12, 1000); + verifyExpDecaySensitivity(stepFn, 1e-12, std::exp(-100.0), 1000); } // Test case for the GLRK3 method on the nonlinear decay ODE. TEST("AD GLRK3 Method", GLRK3_NonlinearDecay) { @@ -676,16 +678,16 @@ TEST("AD GLRK3 Method", GLRK3_StiffNonlinearDecay) { double T = 0.5; DualNumber_T dt(T / 500.0); DualNumber_T t(0.0); - double expected = 1.0 / 51.0; auto stiff_nl_f = [](auto t, const auto& x) { auto dx = x.eval(); - dx(0) = -100.0 * x(0) - x(0) * x(0); + dx(0) = -x(0) * (100.0 + x(0)); return dx; }; for (int i = 0; i < 500; ++i) { x = integrator.GLRK3_AD(x, t, dt, stiff_nl_f, 80, 1e-7); t += dt; } + double expected = 100.0 / (101.0 * std::exp(100.0 * T) - 1.0); double rel_err = std::abs(x(0).real - expected) / expected; ASSERT_TRUE(rel_err < tol_low, "GLRK3 stiff nonlinear decay error too large"); } From 6563c066dc5ca04c08025b42ccc972337d172a31 Mon Sep 17 00:00:00 2001 From: saltyjoss Date: Sat, 23 May 2026 13:15:09 +0100 Subject: [PATCH 056/210] fixes: Removed incorrected inclusion of midpoint chain factor in `Midpoint_AD`, specifically in the use of `J_out`, and adding midpoint specific right-side factor to fullfil the variontal equation --- .../integrators/ad_implicit_integrators.inl | 46 +++++++++++-------- .../ADImplicitIntegratorTests.cpp | 12 ++--- 2 files changed, 34 insertions(+), 24 deletions(-) diff --git a/PxMLib/MathLib/include/integrators/ad_implicit_integrators.inl b/PxMLib/MathLib/include/integrators/ad_implicit_integrators.inl index 835c15d5..b7d098f6 100644 --- a/PxMLib/MathLib/include/integrators/ad_implicit_integrators.inl +++ b/PxMLib/MathLib/include/integrators/ad_implicit_integrators.inl @@ -53,19 +53,34 @@ namespace integration { mathlib::MatX_T F; auto perturbation_func = [&](auto t_pert, const auto& x_pert) { using Dual = std::decay_t; - auto x_real = x.template cast(); - mathlib::VecX_T x_cast = x_real.template cast(); + mathlib::VecX_T x_cast = x.template cast(); mathlib::VecX_T x_mid = (x_cast + x_pert) / Dual(2); - Dual t_mid = (t_pert + Dual(static_cast(dt))) / Dual(2); - auto f_eval = f(t_mid, x_mid); - return f_eval.template cast(); + Dual t_mid = t_pert + Dual(static_cast(dt)) / Dual(2); + return f(t_mid, x_mid); }; F = automaticDifferenceJacobian(perturbation_func, t, x_guess); Real dt_real = static_cast(dt); - J_out = mathlib::MatX_T::Identity(n, n) - Real(0.5) * dt_real * F; // J = I - dt * df/dx + J_out = mathlib::MatX_T::Identity(n, n) - dt_real * F; // J = I - dt * df/dx }; mathlib::VecX_T x0 = x + dt * f((t + dt) / Scalar(2), x); // Initial guess for Newton-Raphson - return newtonRaphson_AD(g, J, x0, maxIter, tol); + mathlib::VecX_T res = newtonRaphson_AD(g, J, x0, maxIter, tol); + + // Right-Side Factof Implicit Midpoint (missing from the generic `newtonRaphson_AD` method + constexpr size_t n = mathlib::DualTraits::Dimension; + mathlib::MatX_T Fx; + auto midpoint_func = [&](auto /*t_pert*/, const auto& x_pert) { return f(t + Scalar(dt) / Scalar(2), x_pert); }; + mathlib::VecX_T x_mid = (x + res) / Scalar(2); + Fx = automaticDifferenceJacobian(midpoint_func, t, x_mid); + mathlib::MatX_T A = mathlib::MatX_T::Identity(x.size(), x.size()) - Real(0.5) * static_cast(dt) * Fx; + mathlib::MatX_T B = mathlib::MatX_T::Identity(x.size(), x.size()) + Real(0.5) * static_cast(dt) * Fx; + Eigen::FullPivLU> solver(A); + for (size_t d = 0; d < n; ++d) { + mathlib::VecX_T s_old(x.size()); + for (Eigen::Index i = 0; i < x.size(); ++i) { s_old(i) = x(i).dual[d]; } + mathlib::VecX_T s_new = solver.solve(B * s_old); + for (Eigen::Index i = 0; i < x.size(); ++i) { res(i).dual[d] = s_new(i); } + } + return res; } // AD version of Gauss-Legendre Runge-Kutta method (2 stages, 4th order) @@ -121,9 +136,8 @@ namespace integration { F1 = automaticDifferenceJacobian( [&](auto t_pert, const auto& x_pert) { using Dual = std::decay_t; - Dual t_eval = t_pert + Dual(c(0)) * static_cast(dt); - auto f_eval = f(t_eval, x_pert); - return f_eval.template cast(); + Dual t_eval = t_pert + Dual(c(0)) * static_cast(dt); + return f(t_eval, x_pert); }, t + c(0) * dt, x1 @@ -132,8 +146,7 @@ namespace integration { [&](auto t_pert, const auto& x_pert) { using Dual = std::decay_t; Dual t_eval = t_pert + Dual(c(1)) * static_cast(dt); - auto f_eval = f(t_eval, x_pert); - return f_eval.template cast(); + return f(t_eval, x_pert); }, t + c(1) * dt, x2 @@ -246,8 +259,7 @@ namespace integration { [&](auto t_pert, const auto& x_pert) { using Dual = std::decay_t; Dual t_eval = t_pert + Dual(c(0)) * static_cast(dt); - auto f_eval = f(t_eval, x_pert); - return f_eval.template cast(); + return f(t_eval, x_pert); }, t + c(0) * dt, x1 @@ -257,8 +269,7 @@ namespace integration { [&](auto t_pert, const auto& x_pert) { using Dual = std::decay_t; Dual t_eval = t_pert + Dual(c(1)) * static_cast(dt); - auto f_eval = f(t_eval, x_pert); - return f_eval.template cast(); + return f(t_eval, x_pert); }, t + c(1) * dt, x2 @@ -268,8 +279,7 @@ namespace integration { [&](auto t_pert, const auto& x_pert) { using Dual = std::decay_t; Dual t_eval = t_pert + Dual(c(2)) * static_cast(dt); - auto f_eval = f(t_eval, x_pert); - return f_eval.template cast(); + return f(t_eval, x_pert); }, t + c(2) * dt, x3 diff --git a/PxMLib/MathLib_Unit/AutomaticDifferentiationTests/ADImplicitIntegratorTests.cpp b/PxMLib/MathLib_Unit/AutomaticDifferentiationTests/ADImplicitIntegratorTests.cpp index 4d24011a..64a042da 100644 --- a/PxMLib/MathLib_Unit/AutomaticDifferentiationTests/ADImplicitIntegratorTests.cpp +++ b/PxMLib/MathLib_Unit/AutomaticDifferentiationTests/ADImplicitIntegratorTests.cpp @@ -363,7 +363,7 @@ TEST("AD Implicit Midpoint Method", ImplicitMidpoint_Stability_LargeStep) { x(0) = DualNumber_T(1.0, { 1.0 }); double T = 1.0; DualNumber_T t(0.0); - DualNumber_T dt(1.0 / 120.0); + DualNumber_T dt(1.0 / 10.0); DualNumber_T prev = x(0); for (int i = 0; i < 10; ++i) { x = integrator.implicitMidpoint_AD(x, t, dt, stiffDecay, 50, 1e-6); @@ -519,8 +519,8 @@ TEST("AD GLRK2 Method", GLRK2_StiffNonlinearDecay) { t += dt; } double expected = 100.0 / (101.0 * std::exp(100.0 * T) - 1.0); - double rel_err = std::abs(x(0).real - expected) / expected; - ASSERT_TRUE(rel_err < tol_low, "GLRK2 stiff nonlinear decay error too large"); + double rel_err = std::abs(x(0).real - expected); + ASSERT_TRUE(rel_err < 1e-12, "GLRK2 stiff nonlinear decay error too large"); } // Test large step performance of GLRK2 TEST("AD GLRK2 Method", GLRK2_Stability_LargeStep) { @@ -528,7 +528,7 @@ TEST("AD GLRK2 Method", GLRK2_Stability_LargeStep) { x(0) = DualNumber_T(1.0, { 1.0 }); double T = 1.0; DualNumber_T t(0.0); - DualNumber_T dt(1.0 / 120.0); + DualNumber_T dt(1.0 / 10.0); DualNumber_T prev = x(0); for (int i = 0; i < 10; ++i) { x = integrator.GLRK2_AD(x, t, dt, stiffDecay, 50, 1e-6); @@ -688,8 +688,8 @@ TEST("AD GLRK3 Method", GLRK3_StiffNonlinearDecay) { t += dt; } double expected = 100.0 / (101.0 * std::exp(100.0 * T) - 1.0); - double rel_err = std::abs(x(0).real - expected) / expected; - ASSERT_TRUE(rel_err < tol_low, "GLRK3 stiff nonlinear decay error too large"); + double rel_err = std::abs(x(0).real - expected); + ASSERT_TRUE(rel_err < 1e-12, "GLRK3 stiff nonlinear decay error too large"); } // Test case for the GLRK3 method on the stiff decay ODE. TEST("AD GLRK3 Method", GLRK3_Stability_LargeStep) { From e22668e2958fa6439c5d04dcbf361c754c575f75 Mon Sep 17 00:00:00 2001 From: saltyjoss Date: Sat, 23 May 2026 13:22:46 +0100 Subject: [PATCH 057/210] fixes TODO * I now need to find a way to properly integrate this into the central (core) dsfe lib, so I think I may start with putting AD back into the specific method rather than having it separated (I.e. no _AD method) --- .../AutomaticDifferentiationTests/ADImplicitIntegratorTests.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/PxMLib/MathLib_Unit/AutomaticDifferentiationTests/ADImplicitIntegratorTests.cpp b/PxMLib/MathLib_Unit/AutomaticDifferentiationTests/ADImplicitIntegratorTests.cpp index 64a042da..f228b053 100644 --- a/PxMLib/MathLib_Unit/AutomaticDifferentiationTests/ADImplicitIntegratorTests.cpp +++ b/PxMLib/MathLib_Unit/AutomaticDifferentiationTests/ADImplicitIntegratorTests.cpp @@ -363,7 +363,7 @@ TEST("AD Implicit Midpoint Method", ImplicitMidpoint_Stability_LargeStep) { x(0) = DualNumber_T(1.0, { 1.0 }); double T = 1.0; DualNumber_T t(0.0); - DualNumber_T dt(1.0 / 10.0); + DualNumber_T dt(1.0 / 100.0); DualNumber_T prev = x(0); for (int i = 0; i < 10; ++i) { x = integrator.implicitMidpoint_AD(x, t, dt, stiffDecay, 50, 1e-6); From 7c3958182f22642adbe4a6042438672b18f0e9d7 Mon Sep 17 00:00:00 2001 From: saltyjoss Date: Sat, 23 May 2026 13:37:39 +0100 Subject: [PATCH 058/210] refactor: Updated naming and inline files for auto diff integrators and integration stepping methods. --- .../include/Numerics/IntegrationService.h | 90 +----------------- .../include/Numerics/IntegrationStep.inl | 94 +++++++++++++++++++ ...egrators.inl => auto_diff_integrators.inl} | 2 +- .../integrators/numerical_integrators.h | 2 +- 4 files changed, 100 insertions(+), 88 deletions(-) create mode 100644 DSFE_App/DSFE_Core/include/Numerics/IntegrationStep.inl rename PxMLib/MathLib/include/integrators/{ad_implicit_integrators.inl => auto_diff_integrators.inl} (99%) diff --git a/DSFE_App/DSFE_Core/include/Numerics/IntegrationService.h b/DSFE_App/DSFE_Core/include/Numerics/IntegrationService.h index 77fcf24e..5f4d5b2d 100644 --- a/DSFE_App/DSFE_Core/include/Numerics/IntegrationService.h +++ b/DSFE_App/DSFE_Core/include/Numerics/IntegrationService.h @@ -28,94 +28,10 @@ namespace integration { IntegrationService(); template - StepOut step(eIntegrationMethod m, mathlib::VecX& x, double t, double dt, Func&& f, JacFunc&& jac) { - if constexpr (std::is_pointer_v> || requires { f == nullptr; }) { - if (f == nullptr) { - D_WARN_ONCE("No derivative function provided for integration - Assuming constant derivative (Euler step)"); - return { _integrator->eulerStep(x, t, dt, std::forward(f)), dt, dt }; - } - } - - switch (m) { - // Explicit methods - // * currently all explicit methods use fixed step size, apart from RK45 as it is an adaptive method - case eIntegrationMethod::Euler: return { _integrator->eulerStep(x, t, dt, std::forward(f)), dt, dt }; - case eIntegrationMethod::Midpoint: return { _integrator->midpointStep(x, t, dt, std::forward(f)), dt, dt }; - case eIntegrationMethod::Heun: return { _integrator->heunStep(x, t, dt, std::forward(f)), dt, dt }; - case eIntegrationMethod::Ralston: return { _integrator->ralstonStep(x, t, dt, std::forward(f)), dt, dt }; - case eIntegrationMethod::RK4: return { _integrator->rk4Step(x, t, dt, std::forward(f)), dt, dt }; - case eIntegrationMethod::RK45: return step_adaptive(eIntegrationMethod::RK45, x, t, dt, std::forward(f), _rtol, _atol); - // Implicit methods - // * currently use fixed step size (no error estimation), but are likely to support adaptive stepping in the future - case eIntegrationMethod::ImplicitEuler: return { _integrator->implicitEuler(x, t, dt, std::forward(f), std::forward(jac)), dt, dt }; - case eIntegrationMethod::ImplicitMidpoint: return { _integrator->implicitMidpoint(x, t, dt, std::forward(f), std::forward(jac)), dt, dt }; - case eIntegrationMethod::GLRK2: return { _integrator->GLRK2(x, t, dt, std::forward(f), std::forward(jac), 50, 1e-10), dt, dt }; - case eIntegrationMethod::GLRK3: return { _integrator->GLRK3(x, t, dt, std::forward(f), std::forward(jac), 50, 1e-10), dt, dt }; - default: - LOG_WARN("Unknown integration method: %s. Defaulting to RK4.", toString(m)); - return { _integrator->rk4Step(x, t, dt, std::forward(f)), dt, dt }; - } - } + StepOut step(eIntegrationMethod m, mathlib::VecX& x, double t, double dt, Func&& f, JacFunc&& jac); template - StepOut step_adaptive(eIntegrationMethod m, mathlib::VecX& x, double t, double dt_try, Func&& f, double rtol, double atol) { - if constexpr (std::is_pointer_v> || requires { f == nullptr; }) { - if (f == nullptr) { - LOG_WARN("No derivative function provided for adaptive integration - returning state unchanged"); - return { x, dt_try, dt_try }; - } - } - if (m != eIntegrationMethod::RK45) { - LOG_WARN("Adaptive step size integration is only implemented for RK45 method. Defaulting to RK45 Method", toString(m)); - } - - // Start with the last successful step size or the initial guess - double h_init = (_dt_last > 0.0) ? _dt_last : dt_try; - - // Enforce maximum step size if set - if (_dt_max > 0.0) { - h_init = std::min(h_init, _dt_max); - } - double h = std::min(h_init, dt_try); - - // Target end time for this adaptive step - const double t_end = t + dt_try; // target end time for this step - const double eps = 1e-12 * dt_try; // small epsilon to prevent division by zero - - // Initialise current state and time for the adaptive stepping loop - mathlib::VecX x_curr = x; // current state during the adaptive step - double t_curr = t; // current time during the adaptive step - double t_total = 0.0; // total time taken for the step - - // Limit the number of substeps to prevent infinite loop - int substeps = 0; - const int max_substeps = 500; // safety limit - - // Loop until we reach the target end time or exceed the maximum number of substeps - while (t_curr < t_end && substeps < max_substeps) { - double h_try = std::min(h, t_end - t_curr); - double dt_used = 0.0; - - mathlib::VecX x_next = _integrator->rk45Step(x_curr, t_curr, h_try, dt_used, std::forward(f), rtol, atol); - - // Update rk45step - t_curr += dt_used; - t_total += dt_used; - x_curr = x_next; - h = h_try; - - ++substeps; - } - - // If substep limit was reached, a warning is logged - if (substeps >= max_substeps) { - LOG_WARN("Adaptive integration exceeded maximum substeps (%d) at time %f. Returning last computed state.", max_substeps, t_curr); - } - - // Persist the last good step size for next frame - _dt_last = h; - return { x_curr, t_total, h }; - } + StepOut step_adaptive(eIntegrationMethod m, mathlib::VecX& x, double t, double dt_try, Func&& f, double rtol, double atol); void setIntegrationMethod(eIntegrationMethod m) { method = m; } eIntegrationMethod getIntegrationMethod() const { return method; } @@ -143,3 +59,5 @@ namespace integration { double _dt_max = 0.0; // maximum allowed step size }; } // namespace integration + +#include "IntegrationStep.inl" diff --git a/DSFE_App/DSFE_Core/include/Numerics/IntegrationStep.inl b/DSFE_App/DSFE_Core/include/Numerics/IntegrationStep.inl new file mode 100644 index 00000000..b9343fa1 --- /dev/null +++ b/DSFE_App/DSFE_Core/include/Numerics/IntegrationStep.inl @@ -0,0 +1,94 @@ +// DSFE_Core IntegrationStep.inl +#pragma once + +namespace integration { + template + StepOut IntegrationService::step(eIntegrationMethod m, mathlib::VecX& x, double t, double dt, Func&& f, JacFunc&& jac) { + if constexpr (std::is_pointer_v> || requires { f == nullptr; }) { + if (f == nullptr) { + D_WARN_ONCE("No derivative function provided for integration - Assuming constant derivative (Euler step)"); + return { _integrator->eulerStep(x, t, dt, std::forward(f)), dt, dt }; + } + } + + switch (m) { + // Explicit methods + // * currently all explicit methods use fixed step size, apart from RK45 as it is an adaptive method + case eIntegrationMethod::Euler: return { _integrator->eulerStep(x, t, dt, std::forward(f)), dt, dt }; + case eIntegrationMethod::Midpoint: return { _integrator->midpointStep(x, t, dt, std::forward(f)), dt, dt }; + case eIntegrationMethod::Heun: return { _integrator->heunStep(x, t, dt, std::forward(f)), dt, dt }; + case eIntegrationMethod::Ralston: return { _integrator->ralstonStep(x, t, dt, std::forward(f)), dt, dt }; + case eIntegrationMethod::RK4: return { _integrator->rk4Step(x, t, dt, std::forward(f)), dt, dt }; + case eIntegrationMethod::RK45: return step_adaptive(eIntegrationMethod::RK45, x, t, dt, std::forward(f), _rtol, _atol); + // Implicit methods + // * currently use fixed step size (no error estimation), but are likely to support adaptive stepping in the future + case eIntegrationMethod::ImplicitEuler: return { _integrator->implicitEuler(x, t, dt, std::forward(f), std::forward(jac)), dt, dt }; + case eIntegrationMethod::ImplicitMidpoint: return { _integrator->implicitMidpoint(x, t, dt, std::forward(f), std::forward(jac)), dt, dt }; + case eIntegrationMethod::GLRK2: return { _integrator->GLRK2(x, t, dt, std::forward(f), std::forward(jac), 50, 1e-10), dt, dt }; + case eIntegrationMethod::GLRK3: return { _integrator->GLRK3(x, t, dt, std::forward(f), std::forward(jac), 50, 1e-10), dt, dt }; + default: + LOG_WARN("Unknown integration method: %s. Defaulting to RK4.", toString(m)); + return { _integrator->rk4Step(x, t, dt, std::forward(f)), dt, dt }; + } + } + + template + StepOut IntegrationService::step_adaptive(eIntegrationMethod m, mathlib::VecX& x, double t, double dt_try, Func&& f, double rtol, double atol) { + if constexpr (std::is_pointer_v> || requires { f == nullptr; }) { + if (f == nullptr) { + LOG_WARN("No derivative function provided for adaptive integration - returning state unchanged"); + return { x, dt_try, dt_try }; + } + } + if (m != eIntegrationMethod::RK45) { + LOG_WARN("Adaptive step size integration is only implemented for RK45 method. Defaulting to RK45 Method", toString(m)); + } + + // Start with the last successful step size or the initial guess + double h_init = (_dt_last > 0.0) ? _dt_last : dt_try; + + // Enforce maximum step size if set + if (_dt_max > 0.0) { + h_init = std::min(h_init, _dt_max); + } + double h = std::min(h_init, dt_try); + + // Target end time for this adaptive step + const double t_end = t + dt_try; // target end time for this step + const double eps = 1e-12 * dt_try; // small epsilon to prevent division by zero + + // Initialise current state and time for the adaptive stepping loop + mathlib::VecX x_curr = x; // current state during the adaptive step + double t_curr = t; // current time during the adaptive step + double t_total = 0.0; // total time taken for the step + + // Limit the number of substeps to prevent infinite loop + int substeps = 0; + const int max_substeps = 500; // safety limit + + // Loop until we reach the target end time or exceed the maximum number of substeps + while (t_curr < t_end && substeps < max_substeps) { + double h_try = std::min(h, t_end - t_curr); + double dt_used = 0.0; + + mathlib::VecX x_next = _integrator->rk45Step(x_curr, t_curr, h_try, dt_used, std::forward(f), rtol, atol); + + // Update rk45step + t_curr += dt_used; + t_total += dt_used; + x_curr = x_next; + h = h_try; + + ++substeps; + } + + // If substep limit was reached, a warning is logged + if (substeps >= max_substeps) { + LOG_WARN("Adaptive integration exceeded maximum substeps (%d) at time %f. Returning last computed state.", max_substeps, t_curr); + } + + // Persist the last good step size for next frame + _dt_last = h; + return { x_curr, t_total, h }; + } +} // namespace integration \ No newline at end of file diff --git a/PxMLib/MathLib/include/integrators/ad_implicit_integrators.inl b/PxMLib/MathLib/include/integrators/auto_diff_integrators.inl similarity index 99% rename from PxMLib/MathLib/include/integrators/ad_implicit_integrators.inl rename to PxMLib/MathLib/include/integrators/auto_diff_integrators.inl index b7d098f6..d4a1f43d 100644 --- a/PxMLib/MathLib/include/integrators/ad_implicit_integrators.inl +++ b/PxMLib/MathLib/include/integrators/auto_diff_integrators.inl @@ -1,4 +1,4 @@ -// PxM/MathLib ad_implicit_integrators.inl +// PxM/MathLib auto_diff_integrators.inl #pragma once namespace integration { diff --git a/PxMLib/MathLib/include/integrators/numerical_integrators.h b/PxMLib/MathLib/include/integrators/numerical_integrators.h index 5f351c68..e8bd1e9d 100644 --- a/PxMLib/MathLib/include/integrators/numerical_integrators.h +++ b/PxMLib/MathLib/include/integrators/numerical_integrators.h @@ -201,4 +201,4 @@ namespace integration { #include "explicit_integrators.inl" #include "implicit_integrators.inl" -#include "ad_implicit_integrators.inl" \ No newline at end of file +#include "auto_diff_integrators.inl" \ No newline at end of file From fb8239dd47a884f2412746e2f6bea255d25150eb Mon Sep 17 00:00:00 2001 From: saltyjoss Date: Sat, 23 May 2026 13:47:47 +0100 Subject: [PATCH 059/210] feat(WIP): Added new `DifferentiableIntegrator` class, with a `step` method * I realised that merging the methods just is not the correct thing right now, so I have decided to first getting the AD methods working with the core logic, then improving staiblity both mathemaitcally and computationally, then I will slowly merge them once I am happy with their performance, optimisation, and integration relative to the codebase. * This allows me to change my approach a little as I integrate them, because given the fact it has taken nearly 2-3 weeks to actually add them (SUCCESSFULLY) - thats with debugging help from AI - I am acutely aware that I need to slow down a little and process what I am doing more. * That being said, once integrated successfully (not merged just added) I will delete this branch so I can start adding some newer components of DSFE (did someone say very basic particle dynamics?) --- DSFE_App/DSFE_Core/include/Numerics/IntegrationMethods.h | 7 +++++++ DSFE_App/DSFE_Core/include/Numerics/IntegrationService.h | 8 ++++++++ 2 files changed, 15 insertions(+) diff --git a/DSFE_App/DSFE_Core/include/Numerics/IntegrationMethods.h b/DSFE_App/DSFE_Core/include/Numerics/IntegrationMethods.h index 75182e1c..90914fc5 100644 --- a/DSFE_App/DSFE_Core/include/Numerics/IntegrationMethods.h +++ b/DSFE_App/DSFE_Core/include/Numerics/IntegrationMethods.h @@ -13,4 +13,11 @@ namespace integration { GLRK2 = 8, // Gauss-Legendre Runge-Kutta Method (2 stages, 4th order, implicit) GLRK3 = 9 // Gauss-Legendre Runge-Kutta Method (3 stages, 6th order, implicit) }; + + enum class eAutoDiffIntegrationMethod { + AD_ImplicitEuler = 0, + AD_ImplicitMidpoint = 1, + AD_GLRK2 = 2, + AD_GLRK3 = 3 + }; } \ No newline at end of file diff --git a/DSFE_App/DSFE_Core/include/Numerics/IntegrationService.h b/DSFE_App/DSFE_Core/include/Numerics/IntegrationService.h index 5f4d5b2d..a800a146 100644 --- a/DSFE_App/DSFE_Core/include/Numerics/IntegrationService.h +++ b/DSFE_App/DSFE_Core/include/Numerics/IntegrationService.h @@ -58,6 +58,14 @@ namespace integration { double _dt_last = 0.0; // last successful step double _dt_max = 0.0; // maximum allowed step size }; + + template + class DSFE_API DifferentiableIntegrator { + public: + template + mathlib::VecX_T step(eAutoDiffIntegrationMethod m, mathlib::VecX_T < Scalar> x, Scalar t, Scalar dt, Func&& f); + }; + } // namespace integration #include "IntegrationStep.inl" From 19276712ce5867429c0150f71de3307ba3c7158d Mon Sep 17 00:00:00 2001 From: saltyjoss Date: Sat, 23 May 2026 14:43:48 +0100 Subject: [PATCH 060/210] feat(WIP): Updated `DifferentiableIntegrator` `step` method in the inline fle * Added DifferentiableIntegrator subsystem * Added templated StepOut_T for differentiable integration * Isolated AD implicit integrators from runtime integration path * Preserved runtime double-only integration architecture --- .../include/Numerics/AutoDiffStep.inl | 27 ++++++++++++++++ .../include/Numerics/IntegrationService.h | 31 ++++++++++--------- .../include/Numerics/IntegrationStep.inl | 8 ++--- .../src/Numerics/IntegrationService.cpp | 7 ++++- .../integrators/numerical_integrators.h | 4 +-- 5 files changed, 55 insertions(+), 22 deletions(-) create mode 100644 DSFE_App/DSFE_Core/include/Numerics/AutoDiffStep.inl diff --git a/DSFE_App/DSFE_Core/include/Numerics/AutoDiffStep.inl b/DSFE_App/DSFE_Core/include/Numerics/AutoDiffStep.inl new file mode 100644 index 00000000..7a0a1e60 --- /dev/null +++ b/DSFE_App/DSFE_Core/include/Numerics/AutoDiffStep.inl @@ -0,0 +1,27 @@ +// DSFE_Core AutoDiffStep.inl +#pragma once + +namespace integration { + template + StepOut_T DifferentiableIntegrator::step(eAutoDiffIntegrationMethod m, mathlib::VecX_T& x, Scalar t, Scalar dt, Func&& f) { + using Real = typename mathlib::DualTraits::BaseScalar; + if constexpr (std::is_pointer_v> || requires { f == nullptr; }) { + if (f == nullptr) { + D_WARN_ONCE("No derivative function provided for integration - Assuming constant derivative."); + return { _integrator->implicitEuler_AD(x, t, dt, std::forward(f), 8, 1e-6), dt, dt }; + } + } + + Real dt_r = mathlib::real(dt); + + switch (m) { + case eAutoDiffIntegrationMethod::AD_ImplicitEuler: return { _integrator->implicitEuler_AD(x, t, dt, std::forward(f), 8, 1e-6), dt_r, dt_r }; + case eAutoDiffIntegrationMethod::AD_ImplicitMidpoint: return { _integrator->implicitMidpoint_AD(x, t, dt, std::forward(f), 10, 1e-7), dt_r, dt_r }; + case eAutoDiffIntegrationMethod::AD_GLRK2: return { _integrator->GLRK2_AD(x, t, dt, std::forward(f), 80, 1e-10), dt_r, dt_r }; + case eAutoDiffIntegrationMethod::AD_GLRK3: return { _integrator->GLRK3_AD(x, t, dt, std::forward(f), 150, 1e-14), dt_r, dt_r }; + default: + LOG_WARN("Unknown Integrator, defaulting to ImplicitEuler (AutoDiff)."); + return { _integrator->implicitEuler_AD(x, t, dt, std::forward(f), 8, 1e-6), dt_r, dt_r }; + } + } +} // namespace integration \ No newline at end of file diff --git a/DSFE_App/DSFE_Core/include/Numerics/IntegrationService.h b/DSFE_App/DSFE_Core/include/Numerics/IntegrationService.h index a800a146..2dc8f382 100644 --- a/DSFE_App/DSFE_Core/include/Numerics/IntegrationService.h +++ b/DSFE_App/DSFE_Core/include/Numerics/IntegrationService.h @@ -15,33 +15,29 @@ namespace integration { // Struct representing the result of a single integration step - struct DSFE_API StepOut { - mathlib::VecX x_next; // next state vector - double dt_taken = 0.0; // actual step size taken - double dt_sug = 0.0; // suggested next step size + template + struct DSFE_API StepOut_T { + mathlib::VecX_T x_next; // next state vector + typename mathlib::DualTraits::BaseScalar dt_taken = 0.0; // actual step size taken + typename mathlib::DualTraits::BaseScalar dt_sug = 0.0; // suggested next step size }; // Class representing the integration service class DSFE_API IntegrationService { public: - // Constructor + using StepOut = StepOut_T; IntegrationService(); template StepOut step(eIntegrationMethod m, mathlib::VecX& x, double t, double dt, Func&& f, JacFunc&& jac); - template StepOut step_adaptive(eIntegrationMethod m, mathlib::VecX& x, double t, double dt_try, Func&& f, double rtol, double atol); void setIntegrationMethod(eIntegrationMethod m) { method = m; } eIntegrationMethod getIntegrationMethod() const { return method; } - const std::string IntegratorName(eIntegrationMethod m); - void setAdaptiveTolerances(double rtol, double atol) { _rtol = rtol; _atol = atol; } void setMaxStep(double max_dt) { _dt_max = max_dt; } - - // Reset the cached adaptive step size (call on robot load/reset) void resetAdaptiveState() { _dt_last = 0.0; } private: @@ -49,23 +45,28 @@ namespace integration { integration::eIntegrationMethod method; std::unique_ptr _integrator; - std::string _methodStr = "RK4"; double _rtol; double _atol; - double _dt_last = 0.0; // last successful step double _dt_max = 0.0; // maximum allowed step size }; - template class DSFE_API DifferentiableIntegrator { public: - template - mathlib::VecX_T step(eAutoDiffIntegrationMethod m, mathlib::VecX_T < Scalar> x, Scalar t, Scalar dt, Func&& f); + DifferentiableIntegrator(); + + template + StepOut_T step(eAutoDiffIntegrationMethod m, mathlib::VecX_T& x, Scalar t, Scalar dt, Func&& f); + + private: + integration::eAutoDiffIntegrationMethod _m; + std::unique_ptr _integrator; + std::string _mStr = "AD_ImplicitEuler"; }; } // namespace integration #include "IntegrationStep.inl" +#include "AutoDiffStep.inl" diff --git a/DSFE_App/DSFE_Core/include/Numerics/IntegrationStep.inl b/DSFE_App/DSFE_Core/include/Numerics/IntegrationStep.inl index b9343fa1..1ccd9f2d 100644 --- a/DSFE_App/DSFE_Core/include/Numerics/IntegrationStep.inl +++ b/DSFE_App/DSFE_Core/include/Numerics/IntegrationStep.inl @@ -20,12 +20,12 @@ namespace integration { case eIntegrationMethod::Ralston: return { _integrator->ralstonStep(x, t, dt, std::forward(f)), dt, dt }; case eIntegrationMethod::RK4: return { _integrator->rk4Step(x, t, dt, std::forward(f)), dt, dt }; case eIntegrationMethod::RK45: return step_adaptive(eIntegrationMethod::RK45, x, t, dt, std::forward(f), _rtol, _atol); - // Implicit methods - // * currently use fixed step size (no error estimation), but are likely to support adaptive stepping in the future + // Implicit methods + // * currently use fixed step size (no error estimation), but are likely to support adaptive stepping in the future case eIntegrationMethod::ImplicitEuler: return { _integrator->implicitEuler(x, t, dt, std::forward(f), std::forward(jac)), dt, dt }; case eIntegrationMethod::ImplicitMidpoint: return { _integrator->implicitMidpoint(x, t, dt, std::forward(f), std::forward(jac)), dt, dt }; - case eIntegrationMethod::GLRK2: return { _integrator->GLRK2(x, t, dt, std::forward(f), std::forward(jac), 50, 1e-10), dt, dt }; - case eIntegrationMethod::GLRK3: return { _integrator->GLRK3(x, t, dt, std::forward(f), std::forward(jac), 50, 1e-10), dt, dt }; + case eIntegrationMethod::GLRK2: return { _integrator->GLRK2(x, t, dt, std::forward(f), std::forward(jac), 80, 1e-10), dt, dt }; + case eIntegrationMethod::GLRK3: return { _integrator->GLRK3(x, t, dt, std::forward(f), std::forward(jac), 150, 1e-14), dt, dt }; default: LOG_WARN("Unknown integration method: %s. Defaulting to RK4.", toString(m)); return { _integrator->rk4Step(x, t, dt, std::forward(f)), dt, dt }; diff --git a/DSFE_App/DSFE_Core/src/Numerics/IntegrationService.cpp b/DSFE_App/DSFE_Core/src/Numerics/IntegrationService.cpp index 01d4b792..4f3abe9c 100644 --- a/DSFE_App/DSFE_Core/src/Numerics/IntegrationService.cpp +++ b/DSFE_App/DSFE_Core/src/Numerics/IntegrationService.cpp @@ -60,7 +60,12 @@ namespace integration { const std::string IntegrationService::IntegratorName(eIntegrationMethod m) { return std::string(toString(m)); } // Constructor - IntegrationService::IntegrationService() + IntegrationService::IntegrationService() : _integrator(std::make_unique()), method(eIntegrationMethod::RK4), _rtol(1e-3), _atol(1e-6), _dt_last(), _dt_max() { } + + // Constructor for autodiff + DifferentiableIntegrator::DifferentiableIntegrator() + : _integrator(std::make_unique()), _m(eAutoDiffIntegrationMethod::AD_ImplicitEuler) { + } } // namespace integration \ No newline at end of file diff --git a/PxMLib/MathLib/include/integrators/numerical_integrators.h b/PxMLib/MathLib/include/integrators/numerical_integrators.h index e8bd1e9d..c57eb065 100644 --- a/PxMLib/MathLib/include/integrators/numerical_integrators.h +++ b/PxMLib/MathLib/include/integrators/numerical_integrators.h @@ -125,8 +125,8 @@ namespace integration { Scalar dt, Func&& f, JacFunc&& jac = nullptr, - int maxIter = 50, - Scalar tol = Scalar(1e-6) + int maxIter = 80, + Scalar tol = Scalar(1e-10) ); // AD version of GLRK2 template From ad61f352ed09b0b371ba95099e1d57e7b5d3a1aa Mon Sep 17 00:00:00 2001 From: saltyjoss Date: Sat, 23 May 2026 14:43:48 +0100 Subject: [PATCH 061/210] feat(WIP): Updated `DifferentiableIntegrator` `step` method in the inline fle * Added DifferentiableIntegrator subsystem * Added templated StepOut_T for differentiable integration * Isolated AD implicit integrators from runtime integration path * Preserved runtime double-only integration architecture --- .../include/Numerics/AutoDiffStep.inl | 27 ++++++++ .../include/Numerics/IntegrationService.h | 31 +++++----- .../include/Numerics/IntegrationStep.inl | 12 ++-- .../include/Robots/RobotDynamics.inl | 61 ++++++++++--------- .../src/Numerics/IntegrationService.cpp | 7 ++- .../integrators/numerical_integrators.h | 4 +- 6 files changed, 88 insertions(+), 54 deletions(-) create mode 100644 DSFE_App/DSFE_Core/include/Numerics/AutoDiffStep.inl diff --git a/DSFE_App/DSFE_Core/include/Numerics/AutoDiffStep.inl b/DSFE_App/DSFE_Core/include/Numerics/AutoDiffStep.inl new file mode 100644 index 00000000..7a0a1e60 --- /dev/null +++ b/DSFE_App/DSFE_Core/include/Numerics/AutoDiffStep.inl @@ -0,0 +1,27 @@ +// DSFE_Core AutoDiffStep.inl +#pragma once + +namespace integration { + template + StepOut_T DifferentiableIntegrator::step(eAutoDiffIntegrationMethod m, mathlib::VecX_T& x, Scalar t, Scalar dt, Func&& f) { + using Real = typename mathlib::DualTraits::BaseScalar; + if constexpr (std::is_pointer_v> || requires { f == nullptr; }) { + if (f == nullptr) { + D_WARN_ONCE("No derivative function provided for integration - Assuming constant derivative."); + return { _integrator->implicitEuler_AD(x, t, dt, std::forward(f), 8, 1e-6), dt, dt }; + } + } + + Real dt_r = mathlib::real(dt); + + switch (m) { + case eAutoDiffIntegrationMethod::AD_ImplicitEuler: return { _integrator->implicitEuler_AD(x, t, dt, std::forward(f), 8, 1e-6), dt_r, dt_r }; + case eAutoDiffIntegrationMethod::AD_ImplicitMidpoint: return { _integrator->implicitMidpoint_AD(x, t, dt, std::forward(f), 10, 1e-7), dt_r, dt_r }; + case eAutoDiffIntegrationMethod::AD_GLRK2: return { _integrator->GLRK2_AD(x, t, dt, std::forward(f), 80, 1e-10), dt_r, dt_r }; + case eAutoDiffIntegrationMethod::AD_GLRK3: return { _integrator->GLRK3_AD(x, t, dt, std::forward(f), 150, 1e-14), dt_r, dt_r }; + default: + LOG_WARN("Unknown Integrator, defaulting to ImplicitEuler (AutoDiff)."); + return { _integrator->implicitEuler_AD(x, t, dt, std::forward(f), 8, 1e-6), dt_r, dt_r }; + } + } +} // namespace integration \ No newline at end of file diff --git a/DSFE_App/DSFE_Core/include/Numerics/IntegrationService.h b/DSFE_App/DSFE_Core/include/Numerics/IntegrationService.h index a800a146..2dc8f382 100644 --- a/DSFE_App/DSFE_Core/include/Numerics/IntegrationService.h +++ b/DSFE_App/DSFE_Core/include/Numerics/IntegrationService.h @@ -15,33 +15,29 @@ namespace integration { // Struct representing the result of a single integration step - struct DSFE_API StepOut { - mathlib::VecX x_next; // next state vector - double dt_taken = 0.0; // actual step size taken - double dt_sug = 0.0; // suggested next step size + template + struct DSFE_API StepOut_T { + mathlib::VecX_T x_next; // next state vector + typename mathlib::DualTraits::BaseScalar dt_taken = 0.0; // actual step size taken + typename mathlib::DualTraits::BaseScalar dt_sug = 0.0; // suggested next step size }; // Class representing the integration service class DSFE_API IntegrationService { public: - // Constructor + using StepOut = StepOut_T; IntegrationService(); template StepOut step(eIntegrationMethod m, mathlib::VecX& x, double t, double dt, Func&& f, JacFunc&& jac); - template StepOut step_adaptive(eIntegrationMethod m, mathlib::VecX& x, double t, double dt_try, Func&& f, double rtol, double atol); void setIntegrationMethod(eIntegrationMethod m) { method = m; } eIntegrationMethod getIntegrationMethod() const { return method; } - const std::string IntegratorName(eIntegrationMethod m); - void setAdaptiveTolerances(double rtol, double atol) { _rtol = rtol; _atol = atol; } void setMaxStep(double max_dt) { _dt_max = max_dt; } - - // Reset the cached adaptive step size (call on robot load/reset) void resetAdaptiveState() { _dt_last = 0.0; } private: @@ -49,23 +45,28 @@ namespace integration { integration::eIntegrationMethod method; std::unique_ptr _integrator; - std::string _methodStr = "RK4"; double _rtol; double _atol; - double _dt_last = 0.0; // last successful step double _dt_max = 0.0; // maximum allowed step size }; - template class DSFE_API DifferentiableIntegrator { public: - template - mathlib::VecX_T step(eAutoDiffIntegrationMethod m, mathlib::VecX_T < Scalar> x, Scalar t, Scalar dt, Func&& f); + DifferentiableIntegrator(); + + template + StepOut_T step(eAutoDiffIntegrationMethod m, mathlib::VecX_T& x, Scalar t, Scalar dt, Func&& f); + + private: + integration::eAutoDiffIntegrationMethod _m; + std::unique_ptr _integrator; + std::string _mStr = "AD_ImplicitEuler"; }; } // namespace integration #include "IntegrationStep.inl" +#include "AutoDiffStep.inl" diff --git a/DSFE_App/DSFE_Core/include/Numerics/IntegrationStep.inl b/DSFE_App/DSFE_Core/include/Numerics/IntegrationStep.inl index b9343fa1..fa0f9733 100644 --- a/DSFE_App/DSFE_Core/include/Numerics/IntegrationStep.inl +++ b/DSFE_App/DSFE_Core/include/Numerics/IntegrationStep.inl @@ -3,7 +3,7 @@ namespace integration { template - StepOut IntegrationService::step(eIntegrationMethod m, mathlib::VecX& x, double t, double dt, Func&& f, JacFunc&& jac) { + StepOut_T IntegrationService::step(eIntegrationMethod m, mathlib::VecX& x, double t, double dt, Func&& f, JacFunc&& jac) { if constexpr (std::is_pointer_v> || requires { f == nullptr; }) { if (f == nullptr) { D_WARN_ONCE("No derivative function provided for integration - Assuming constant derivative (Euler step)"); @@ -20,12 +20,12 @@ namespace integration { case eIntegrationMethod::Ralston: return { _integrator->ralstonStep(x, t, dt, std::forward(f)), dt, dt }; case eIntegrationMethod::RK4: return { _integrator->rk4Step(x, t, dt, std::forward(f)), dt, dt }; case eIntegrationMethod::RK45: return step_adaptive(eIntegrationMethod::RK45, x, t, dt, std::forward(f), _rtol, _atol); - // Implicit methods - // * currently use fixed step size (no error estimation), but are likely to support adaptive stepping in the future + // Implicit methods + // * currently use fixed step size (no error estimation), but are likely to support adaptive stepping in the future case eIntegrationMethod::ImplicitEuler: return { _integrator->implicitEuler(x, t, dt, std::forward(f), std::forward(jac)), dt, dt }; case eIntegrationMethod::ImplicitMidpoint: return { _integrator->implicitMidpoint(x, t, dt, std::forward(f), std::forward(jac)), dt, dt }; - case eIntegrationMethod::GLRK2: return { _integrator->GLRK2(x, t, dt, std::forward(f), std::forward(jac), 50, 1e-10), dt, dt }; - case eIntegrationMethod::GLRK3: return { _integrator->GLRK3(x, t, dt, std::forward(f), std::forward(jac), 50, 1e-10), dt, dt }; + case eIntegrationMethod::GLRK2: return { _integrator->GLRK2(x, t, dt, std::forward(f), std::forward(jac), 80, 1e-10), dt, dt }; + case eIntegrationMethod::GLRK3: return { _integrator->GLRK3(x, t, dt, std::forward(f), std::forward(jac), 150, 1e-14), dt, dt }; default: LOG_WARN("Unknown integration method: %s. Defaulting to RK4.", toString(m)); return { _integrator->rk4Step(x, t, dt, std::forward(f)), dt, dt }; @@ -33,7 +33,7 @@ namespace integration { } template - StepOut IntegrationService::step_adaptive(eIntegrationMethod m, mathlib::VecX& x, double t, double dt_try, Func&& f, double rtol, double atol) { + StepOut_T IntegrationService::step_adaptive(eIntegrationMethod m, mathlib::VecX& x, double t, double dt_try, Func&& f, double rtol, double atol) { if constexpr (std::is_pointer_v> || requires { f == nullptr; }) { if (f == nullptr) { LOG_WARN("No derivative function provided for adaptive integration - returning state unchanged"); diff --git a/DSFE_App/DSFE_Core/include/Robots/RobotDynamics.inl b/DSFE_App/DSFE_Core/include/Robots/RobotDynamics.inl index 8371ae58..a0f0fac8 100644 --- a/DSFE_App/DSFE_Core/include/Robots/RobotDynamics.inl +++ b/DSFE_App/DSFE_Core/include/Robots/RobotDynamics.inl @@ -254,17 +254,18 @@ namespace robots { const RobotJoint& joint = robot.joints[i]; if (joint.type == eJointType::FIXED) { continue; } - const Scalar wn = (Scalar)joint.wn_target; - const Scalar z = (Scalar)joint.zeta_target; - const Scalar eps = (Scalar)1e-6; + const Scalar wn = static_cast(joint.wn_target); // [rad/s], natural frequency + const Scalar z = static_cast(joint.zeta_target); // damping ratio + const Scalar eps = static_cast(1e-6); + const Scalar I_eff = std::max(scratch.M(i, i), eps); const Scalar k_p = I_eff * wn * wn; const Scalar k_d = Scalar(2.0) * z * I_eff * wn; - const Scalar b = Scalar(0.2); // viscous damping coefficient - const Scalar c = Scalar(0.05); // Coulomb friction coefficient - const Scalar eps_f = (Scalar)1e-2; + const Scalar b = static_cast(0.2); // viscous damping coefficient + const Scalar c = static_cast(0.05); // Coulomb friction coefficient + const Scalar eps_f = static_cast(1e-2); dTau_dq(i, i) = -k_p; @@ -322,21 +323,21 @@ namespace robots { continue; } - const Scalar wn = (Scalar)joint.wn_target; // [rad/s], natural frequency - const Scalar z = (Scalar)joint.zeta_target; // damping ratio + const Scalar wn = static_cast(joint.wn_target); // [rad/s], natural frequency + const Scalar z = static_cast(joint.zeta_target); // damping ratio const Scalar err = snap.q_ref[i] - q[i]; // [rad], position error const Scalar err_d = snap.qd_ref[i] - qd[i]; // [rad/s], velocity error - const Scalar eps = (Scalar)1e-6; + const Scalar eps = static_cast(1e-6); const Scalar I_eff = std::max(scratch.dense.M(i, i), eps); // [kg*m^2], effective inertia for joint i with floor to prevent singularities const Scalar k_p = I_eff * wn * wn; // [Nm/rad], proportional gain const Scalar k_d = Scalar(2.0) * z * I_eff * wn; // [Nm/(rad/s)], derivative gain - const Scalar b = Scalar(0.2); // viscous damping coefficient - const Scalar c = Scalar(0.05); // Coulomb friction coefficient - const Scalar eps_f = (Scalar)1e-2; + const Scalar b = static_cast(0.2); // viscous damping coefficient + const Scalar c = static_cast(0.05); // Coulomb friction coefficient + const Scalar eps_f = static_cast(1e-2); Scalar tau_i = k_p * err + k_d * err_d + I_eff * snap.qdd_ref[i]; // [Nm], control torque for joint i tau_i += scratch.g[i]; // Gravity compensation @@ -345,7 +346,7 @@ namespace robots { tau_i -= c * std::tanh(qd[i] / eps_f); // subtract Coulomb friction - scratch.dense.tau[i] = mathlib::real(tau_i); + scratch.dense.tau[i] = tau_i; out.metrics.q[i] = mathlib::real(q[i]); out.metrics.qd[i] = mathlib::real(qd[i]); @@ -410,27 +411,27 @@ namespace robots { continue; } - const Scalar wn = Scalar(5); - const Scalar z = Scalar(0.7); + const Scalar wn = static_cast(5); + const Scalar z = static_cast(0.7); const Scalar err = snap.q_ref[i] - q[i]; const Scalar err_d = snap.qd_ref[i] - qd[i]; - const Scalar eps = Scalar(1e-6); + const Scalar eps = static_cast(1e-6); const Scalar I_eff = std::max(M(i, i), eps); const Scalar k_p = I_eff * wn * wn; const Scalar k_d = Scalar(2) * z * I_eff * wn; - const Scalar b = Scalar(0.2); // viscous damping coefficient - const Scalar c = Scalar(0.05); // Coulomb friction coefficient - const Scalar eps_f = Scalar(1e-2); + const Scalar b = static_cast(0.2); // viscous damping coefficient + const Scalar c = static_cast(0.05); // Coulomb friction coefficient + const Scalar eps_f = static_cast(1e-2); Scalar tau_i = k_p * err + k_d * err_d + I_eff * snap.qdd_ref[i]; tau_i -= b * qd[i]; //tau_i -= b * std::tanh(qd[i] / eps_f); - scratch.dense.tau[i] = mathlib::real(tau_i); + scratch.dense.tau[i] = tau_i; out.metrics.q[i] = mathlib::real(q[i]); out.metrics.qd[i] = mathlib::real(qd[i]); @@ -483,9 +484,9 @@ namespace robots { const SpatialJoint& joint = model.joints[i]; if (!isControlledJoint(joint.type)) { continue; } dTau_dq(i, i) = -kp[i]; - const Scalar eps_f = Scalar(1e-2); - const Scalar b = Scalar(0.2); - const Scalar c = Scalar(0.05); + const Scalar b = static_cast(0.2); // viscous damping coefficient + const Scalar c = static_cast(0.05); // Coulomb friction coefficient + const Scalar eps_f = static_cast(1e-2); Scalar tanh_term = std::tanh(qd[i] / eps_f); Scalar stiff_friction_slope = -c * (Scalar(1.0) - tanh_term * tanh_term) / eps_f; @@ -532,10 +533,10 @@ namespace robots { for (size_t i = 0; i < n; ++i) { if (snap.model->joints[i].type == eJointType::FIXED) continue; - const Scalar eps = (Scalar)1e-6; - const Scalar b = Scalar(0.2); // viscous damping coefficient - const Scalar c = Scalar(0.05); // Coulomb friction coefficient - const Scalar eps_f = (Scalar)1e-2; + const Scalar eps = static_cast < Scalar>(1e-6); + const Scalar b = static_cast(0.2); // viscous damping coefficient + const Scalar c = static_cast(0.05); // Coulomb friction coefficient + const Scalar eps_f = static_cast(1e-2); Scalar tau_i = kp[i] * (snap.q_ref[i] - q[i]) + kd[i] * (snap.qd_ref[i] - qd[i]) + std::max(scratch.dense.M(i, i), eps) * snap.qdd_ref[i]; tau_i += scratch.g[i] + scratch.dense.h[i]; @@ -585,9 +586,9 @@ namespace robots { dTau_dq(i, i) = -kp[i]; - const Scalar b = Scalar(0.2); // viscous damping coefficient - const Scalar c = Scalar(0.05); // Coulomb friction coefficient - const Scalar eps_f = (Scalar)1e-2; + const Scalar b = static_cast(0.2); // viscous damping coefficient + const Scalar c = static_cast(0.05); // Coulomb friction coefficient + const Scalar eps_f = static_cast(1e-2); Scalar tanh_term = std::tanh(qd[i] / eps_f); Scalar stiff_friction = -c * (Scalar(1.0) - tanh_term * tanh_term) / eps_f; diff --git a/DSFE_App/DSFE_Core/src/Numerics/IntegrationService.cpp b/DSFE_App/DSFE_Core/src/Numerics/IntegrationService.cpp index 01d4b792..4f3abe9c 100644 --- a/DSFE_App/DSFE_Core/src/Numerics/IntegrationService.cpp +++ b/DSFE_App/DSFE_Core/src/Numerics/IntegrationService.cpp @@ -60,7 +60,12 @@ namespace integration { const std::string IntegrationService::IntegratorName(eIntegrationMethod m) { return std::string(toString(m)); } // Constructor - IntegrationService::IntegrationService() + IntegrationService::IntegrationService() : _integrator(std::make_unique()), method(eIntegrationMethod::RK4), _rtol(1e-3), _atol(1e-6), _dt_last(), _dt_max() { } + + // Constructor for autodiff + DifferentiableIntegrator::DifferentiableIntegrator() + : _integrator(std::make_unique()), _m(eAutoDiffIntegrationMethod::AD_ImplicitEuler) { + } } // namespace integration \ No newline at end of file diff --git a/PxMLib/MathLib/include/integrators/numerical_integrators.h b/PxMLib/MathLib/include/integrators/numerical_integrators.h index e8bd1e9d..c57eb065 100644 --- a/PxMLib/MathLib/include/integrators/numerical_integrators.h +++ b/PxMLib/MathLib/include/integrators/numerical_integrators.h @@ -125,8 +125,8 @@ namespace integration { Scalar dt, Func&& f, JacFunc&& jac = nullptr, - int maxIter = 50, - Scalar tol = Scalar(1e-6) + int maxIter = 80, + Scalar tol = Scalar(1e-10) ); // AD version of GLRK2 template From 8e96613f9f219510778da1c30ef74bcae4297956 Mon Sep 17 00:00:00 2001 From: saltyjoss Date: Sat, 23 May 2026 15:39:19 +0100 Subject: [PATCH 062/210] feat: Added LogSumExp smooth maximum method for the dual number type * Will likely also add a SoftPlus smooth max method too, but only when neeeded. --- PxMLib/MathLib/include/core/DualNumbers.h | 7 +++++++ 1 file changed, 7 insertions(+) diff --git a/PxMLib/MathLib/include/core/DualNumbers.h b/PxMLib/MathLib/include/core/DualNumbers.h index 4dad9a62..d26dcda3 100644 --- a/PxMLib/MathLib/include/core/DualNumbers.h +++ b/PxMLib/MathLib/include/core/DualNumbers.h @@ -319,6 +319,13 @@ namespace mathlib { return out; } + // LogSumExp Smooth Max + template + inline Scalar LSE_smoothMax(const Scalar& a, const Scalar& b, const Scalar& k = Scalar(10)) { + Scalar m = (a > b) ? a : b; + return m + log(exp(k * (a - m)) + exp(k * (b - m))) / k; + } + // ----- // Assignment Operators // ----- From be45efd5b879431d99b2c36ab00fae18cea96ddb Mon Sep 17 00:00:00 2001 From: saltyjoss Date: Sat, 23 May 2026 15:48:24 +0100 Subject: [PATCH 063/210] feat: Integrated LSE smooth maximum into the current robot dynamics methods previously using `std::max()` --- DSFE_App/DSFE_Core/include/Robots/RobotDynamics.inl | 8 ++++---- PxMLib/MathLib/include/core/DualNumbers.h | 2 +- 2 files changed, 5 insertions(+), 5 deletions(-) diff --git a/DSFE_App/DSFE_Core/include/Robots/RobotDynamics.inl b/DSFE_App/DSFE_Core/include/Robots/RobotDynamics.inl index a0f0fac8..2d290650 100644 --- a/DSFE_App/DSFE_Core/include/Robots/RobotDynamics.inl +++ b/DSFE_App/DSFE_Core/include/Robots/RobotDynamics.inl @@ -258,7 +258,7 @@ namespace robots { const Scalar z = static_cast(joint.zeta_target); // damping ratio const Scalar eps = static_cast(1e-6); - const Scalar I_eff = std::max(scratch.M(i, i), eps); + const Scalar I_eff = mathlib::LSE_smoothMax(scratch.M(i, i), eps); const Scalar k_p = I_eff * wn * wn; const Scalar k_d = Scalar(2.0) * z * I_eff * wn; @@ -331,7 +331,7 @@ namespace robots { const Scalar eps = static_cast(1e-6); - const Scalar I_eff = std::max(scratch.dense.M(i, i), eps); // [kg*m^2], effective inertia for joint i with floor to prevent singularities + const Scalar I_eff = mathlib::LSE_smoothMax(scratch.dense.M(i, i), eps); // [kg*m^2], effective inertia for joint i with floor to prevent singularities const Scalar k_p = I_eff * wn * wn; // [Nm/rad], proportional gain const Scalar k_d = Scalar(2.0) * z * I_eff * wn; // [Nm/(rad/s)], derivative gain @@ -419,7 +419,7 @@ namespace robots { const Scalar eps = static_cast(1e-6); - const Scalar I_eff = std::max(M(i, i), eps); + const Scalar I_eff = mathlib::LSE_smoothMax(M(i, i), eps); const Scalar k_p = I_eff * wn * wn; const Scalar k_d = Scalar(2) * z * I_eff * wn; @@ -538,7 +538,7 @@ namespace robots { const Scalar c = static_cast(0.05); // Coulomb friction coefficient const Scalar eps_f = static_cast(1e-2); - Scalar tau_i = kp[i] * (snap.q_ref[i] - q[i]) + kd[i] * (snap.qd_ref[i] - qd[i]) + std::max(scratch.dense.M(i, i), eps) * snap.qdd_ref[i]; + Scalar tau_i = kp[i] * (snap.q_ref[i] - q[i]) + kd[i] * (snap.qd_ref[i] - qd[i]) + mathlib::LSE_smoothMax(scratch.dense.M(i, i), eps) * snap.qdd_ref[i]; tau_i += scratch.g[i] + scratch.dense.h[i]; tau_i -= b * qd[i]; tau_i -= c * std::tanh(qd[i] / eps_f); diff --git a/PxMLib/MathLib/include/core/DualNumbers.h b/PxMLib/MathLib/include/core/DualNumbers.h index d26dcda3..e5de3ea8 100644 --- a/PxMLib/MathLib/include/core/DualNumbers.h +++ b/PxMLib/MathLib/include/core/DualNumbers.h @@ -321,7 +321,7 @@ namespace mathlib { // LogSumExp Smooth Max template - inline Scalar LSE_smoothMax(const Scalar& a, const Scalar& b, const Scalar& k = Scalar(10)) { + inline Scalar LSE_smoothMax(const Scalar& a, const Scalar& b, const Scalar& k = Scalar(15)) { Scalar m = (a > b) ? a : b; return m + log(exp(k * (a - m)) + exp(k * (b - m))) / k; } From 4bf3674d05a37e51c2db5c47cc6343825561a4a4 Mon Sep 17 00:00:00 2001 From: saltyjoss Date: Sat, 23 May 2026 16:01:38 +0100 Subject: [PATCH 064/210] feat: Updated smooth max LSE to include a scalar overload, thereby requiring transcendental overloads (will be reworked soon to be used in all of the header) --- PxMLib/MathLib/include/core/DualNumbers.h | 32 +++++++++++++++++++++-- 1 file changed, 30 insertions(+), 2 deletions(-) diff --git a/PxMLib/MathLib/include/core/DualNumbers.h b/PxMLib/MathLib/include/core/DualNumbers.h index e5de3ea8..6ba96425 100644 --- a/PxMLib/MathLib/include/core/DualNumbers.h +++ b/PxMLib/MathLib/include/core/DualNumbers.h @@ -297,6 +297,27 @@ namespace mathlib { for (size_t i = 0; i < NVar; ++i) { out.dual[i] = out.real * a.dual[i]; } return out; } + + // Exponential function (Scalar) + template + inline Scalar exp(const Scalar& x) { return std::exp(x); } + // Logarithm function (Scalar) + template + inline Scalar log(const Scalar& x) { return std::log(x); } + // Sine function (Scalar) + template + inline Scalar sin(const Scalar& x) { return std::sin(x); } + // Cosine function (Scalar) + template + inline Scalar cos(const Scalar& x) { return std::cos(x); } + // Tangent function (Scalar) + template + inline Scalar tan(const Scalar& x) { return std::tan(x); } + // Hyperbolic Tangnet (Scalar) + template + inline Scalar tanh(const Scalar& x) { return std::tanh(x); } + + // Smooth step function for smooth interpolation between 0 and 1 template inline Scalar smoothStep(const Scalar& x) { return x * x * (Scalar(3) - Scalar(2) * x); } @@ -319,9 +340,16 @@ namespace mathlib { return out; } - // LogSumExp Smooth Max + // LogSumExp Smooth Max (DualNumber) + template + inline Scalar LSE_smoothMax(const DualNumber_T& a, const DualNumber_T& b, const Scalar& k = Scalar(15)) { + Scalar m = (a.real > b.real) ? a.real : b.real; + return DualNumber_T(m) + log(exp(k * (a - m)) + exp(k * (b - m))) / k; + } + + // LogSumExp Smooth Max (Scalar) template - inline Scalar LSE_smoothMax(const Scalar& a, const Scalar& b, const Scalar& k = Scalar(15)) { + inline Scalar LSE_smoothMax(const Scalar& a, const Scalar& b, const Scalar& k = Scalar(10)) { Scalar m = (a > b) ? a : b; return m + log(exp(k * (a - m)) + exp(k * (b - m))) / k; } From d1733bcc05105f1fb7d00bb2be2ff1846e82a824 Mon Sep 17 00:00:00 2001 From: saltyjoss Date: Sat, 23 May 2026 17:29:05 +0100 Subject: [PATCH 065/210] feat: Updated scalar overloads definition and their use --- PxMLib/MathLib/include/core/DualNumbers.h | 168 +++++++++++++--------- 1 file changed, 97 insertions(+), 71 deletions(-) diff --git a/PxMLib/MathLib/include/core/DualNumbers.h b/PxMLib/MathLib/include/core/DualNumbers.h index 6ba96425..d20223b7 100644 --- a/PxMLib/MathLib/include/core/DualNumbers.h +++ b/PxMLib/MathLib/include/core/DualNumbers.h @@ -92,7 +92,7 @@ namespace mathlib { inline bool operator<=(const DualNumber_T& a, const DualNumber_T& b) { return a.real <= b.real; } template inline bool operator>=(const DualNumber_T& a, const DualNumber_T& b) { return a.real >= b.real; } - // Comparison with scalar (compare only the real part) + // Comparison with scalar (scalar on the right) template inline bool operator==(const DualNumber_T& a, Scalar b) { return a.real == b; } template @@ -178,6 +178,38 @@ namespace mathlib { return out; } + // ----- + // Scalar Interactions + // ----- + + // Power function (Scalar) + template + inline Scalar pow(const Scalar& x, const Scalar& n) { return std::pow(x, n); } + // Sqrt function (Scalar) + template + inline Scalar sqrt(const Scalar& x) { return std::sqrt(x); } + // Sine function (Scalar) + template + inline Scalar sin(const Scalar& x) { return std::sin(x); } + // Cosine function (Scalar) + template + inline Scalar cos(const Scalar& x) { return std::cos(x); } + // Tangent function (Scalar) + template + inline Scalar tan(const Scalar& x) { return std::tan(x); } + // Arctangent function (Scalar) + template + inline Scalar atan(const Scalar& x) { return std::atan(x); } + // Hyperbolic Tangnet (Scalar) + template + inline Scalar tanh(const Scalar& x) { return std::tanh(x); } + // Exponential function (Scalar) + template + inline Scalar exp(const Scalar& x) { return std::exp(x); } + // Logarithm function (Scalar) + template + inline Scalar log(const Scalar& x) { return std::log(x); } + // ----- // Dual Number Interactions // ----- @@ -230,8 +262,8 @@ namespace mathlib { for (size_t i = 0; i < NVar; ++i) { out.dual[i] = Scalar(0); } return out; } - out.real = std::pow(a.real, n); - for (size_t i = 0; i < NVar; ++i) { out.dual[i] = n * std::pow(a.real, n - Scalar(1)) * a.dual[i]; } + out.real = pow(a.real, n); + for (size_t i = 0; i < NVar; ++i) { out.dual[i] = n * pow(a.real, n - Scalar(1)) * a.dual[i]; } return out; } // Square root function @@ -239,7 +271,7 @@ namespace mathlib { inline DualNumber_T sqrt(const DualNumber_T& a) { if (a.real < Scalar(0)) { throw std::runtime_error("sqrt() domain error for DualNumber_T"); } DualNumber_T out; - const Scalar sqrtReal = std::sqrt(a.real); + const Scalar sqrtReal = sqrt(a.real); out.real = sqrtReal; if (sqrtReal == Scalar(0)) { for (size_t i = 0; i < NVar; ++i) { out.dual[i] = Scalar(0); } @@ -253,31 +285,31 @@ namespace mathlib { template inline DualNumber_T sin(const DualNumber_T& a) { DualNumber_T out; - out.real = std::sin(a.real); - for (size_t i = 0; i < NVar; ++i) { out.dual[i] = std::cos(a.real) * a.dual[i]; } + out.real = sin(a.real); + for (size_t i = 0; i < NVar; ++i) { out.dual[i] = cos(a.real) * a.dual[i]; } return out; } // Cosine function template inline DualNumber_T cos(const DualNumber_T& a) { DualNumber_T out; - out.real = std::cos(a.real); - for (size_t i = 0; i < NVar; ++i) { out.dual[i] = -std::sin(a.real) * a.dual[i]; } + out.real = cos(a.real); + for (size_t i = 0; i < NVar; ++i) { out.dual[i] = -sin(a.real) * a.dual[i]; } return out; } // Tangent function template inline DualNumber_T tan(const DualNumber_T& a) { DualNumber_T out; - out.real = std::tan(a.real); - for (size_t i = 0; i < NVar; ++i) { out.dual[i] = a.dual[i] / (std::cos(a.real) * std::cos(a.real)); } + out.real = tan(a.real); + for (size_t i = 0; i < NVar; ++i) { out.dual[i] = a.dual[i] / (cos(a.real) * cos(a.real)); } return out; } // Arctangent function template inline DualNumber_T atan(const DualNumber_T& a) { DualNumber_T out; - out.real = std::atan(a.real); + out.real = atan(a.real); for (size_t i = 0; i < NVar; ++i) { out.dual[i] = a.dual[i] / (Scalar(1) + a.real * a.real); } return out; } @@ -285,7 +317,7 @@ namespace mathlib { template inline DualNumber_T tanh(const DualNumber_T& a) { DualNumber_T out; - out.real = std::tanh(a.real); + out.real = tanh(a.real); for (size_t i = 0; i < NVar; ++i) { out.dual[i] = a.dual[i] * (Scalar(1) - out.real * out.real); } return out; } @@ -293,30 +325,23 @@ namespace mathlib { template inline DualNumber_T exp(const DualNumber_T& a) { DualNumber_T out; - out.real = std::exp(a.real); + out.real = exp(a.real); for (size_t i = 0; i < NVar; ++i) { out.dual[i] = out.real * a.dual[i]; } return out; } + // Logarithm function + template + inline DualNumber_T log(const DualNumber_T& a) { + if (a.real <= Scalar(0)) { throw std::runtime_error("log() domain error for DualNumber_T"); } + DualNumber_T out; + out.real = log(a.real); + for (size_t i = 0; i < NVar; ++i) { out.dual[i] = a.dual[i] / a.real; } + return out; + } - // Exponential function (Scalar) - template - inline Scalar exp(const Scalar& x) { return std::exp(x); } - // Logarithm function (Scalar) - template - inline Scalar log(const Scalar& x) { return std::log(x); } - // Sine function (Scalar) - template - inline Scalar sin(const Scalar& x) { return std::sin(x); } - // Cosine function (Scalar) - template - inline Scalar cos(const Scalar& x) { return std::cos(x); } - // Tangent function (Scalar) - template - inline Scalar tan(const Scalar& x) { return std::tan(x); } - // Hyperbolic Tangnet (Scalar) - template - inline Scalar tanh(const Scalar& x) { return std::tanh(x); } - + // ----- + // Smoothing + // ----- // Smooth step function for smooth interpolation between 0 and 1 template @@ -330,16 +355,12 @@ namespace mathlib { for (size_t i = 0; i < NVar; ++i) { out.dual[i] = derivative * x.dual[i]; } return out; } - // Logarithm function - template - inline DualNumber_T log(const DualNumber_T& a) { - if (a.real <= Scalar(0)) { throw std::runtime_error("log() domain error for DualNumber_T"); } - DualNumber_T out; - out.real = std::log(a.real); - for (size_t i = 0; i < NVar; ++i) { out.dual[i] = a.dual[i] / a.real; } - return out; + // LogSumExp Smooth Max (Scalar) + template + inline Scalar LSE_smoothMax(const Scalar& a, const Scalar& b, const Scalar& k = Scalar(10)) { + Scalar m = (a > b) ? a : b; + return m + log(exp(k * (a - m)) + exp(k * (b - m))) / k; } - // LogSumExp Smooth Max (DualNumber) template inline Scalar LSE_smoothMax(const DualNumber_T& a, const DualNumber_T& b, const Scalar& k = Scalar(15)) { @@ -347,13 +368,6 @@ namespace mathlib { return DualNumber_T(m) + log(exp(k * (a - m)) + exp(k * (b - m))) / k; } - // LogSumExp Smooth Max (Scalar) - template - inline Scalar LSE_smoothMax(const Scalar& a, const Scalar& b, const Scalar& k = Scalar(10)) { - Scalar m = (a > b) ? a : b; - return m + log(exp(k * (a - m)) + exp(k * (b - m))) / k; - } - // ----- // Assignment Operators // ----- @@ -421,38 +435,50 @@ namespace mathlib { // Numeric Limits Specialisation for DualNumber_T // ----- + // Absolute value function (Scalar) + template + inline Scalar abs(const Scalar& a) { return (a < Scalar(0)) ? -a : a; } + // Maximum value function (Scalar) + template + inline Scalar max(const Scalar& a, const Scalar& b) { return (a > b) ? a : b; } + // Minimum value function (Scalar) + template + inline Scalar min(const Scalar& a, const Scalar& b) { return (a < b) ? a : b; } + // Sign function (Scalar) + template + inline Scalar sgn(const Scalar& a) { return (a > Scalar(0)) - (a < Scalar(0)); } + // Is finite function (Scalar) + template + inline Scalar isfinite(const Scalar& a) { return std::isfinite(a); } + // Function to return a DualNumber_T representing infinity (real part is infinity, dual parts are zero) - template - inline DualNumber_T numeric_limits_infinity() { - return DualNumber_T(std::numeric_limits::infinity(), std::array()); - } - // Alternative absolute value function that simply negates the dual number if the real part is negative (this is less smooth but can be more efficient and avoids issues with zero) template - inline DualNumber_T abs(const DualNumber_T& a) { - DualNumber_T out; - out = (a.real < Scalar(0)) ? -a : a; - return out; + inline DualNumber_T numeric_limits_infinity() { + return DualNumber_T(std::numeric_limits::infinity(), std::array()); } + // Absolute value function that simply negates the dual number if the real part is negative (this is less smooth but can be more efficient and avoids issues with zero) + template + inline DualNumber_T abs(const DualNumber_T& a) { return abs(a.real); } // Absolute value function with epsilon threshold to avoid non-differentiability at zero (scales dual part by the sign of the real part, but treats values within epsilon of zero as zero) template inline DualNumber_T abs(const DualNumber_T& a, Scalar epsilon) { DualNumber_T out; - if (std::abs(a.real) < epsilon) { + if (abs(a.real) < epsilon) { out.real = Scalar(0); for (size_t i = 0; i < NVar; ++i) { out.dual[i] = Scalar(0); } } else { - out.real = std::abs(a.real); - Scalar sign = (a.real > Scalar(0)) - (a.real < Scalar(0)); // Sign of the real part + out.real = abs(a.real); + Scalar sign = sgn(a.real); for (size_t i = 0; i < NVar; ++i) { out.dual[i] = sign * a.dual[i]; } } return out; } - // Sign function for DualNumber_T (returns the sign of the real part, dual parts are zero) + // Signum function for DualNumber_T (returns the sign of the real part, dual parts are zero) template - inline DualNumber_T sign(const DualNumber_T& a) { + inline DualNumber_T sgn(const DualNumber_T& a) { DualNumber_T out; - out.real = (a.real > Scalar(0)) - (a.real < Scalar(0)); // Sign of the real part + out.real = sgn(a.real); // Signum of the real part for (size_t i = 0; i < NVar; ++i) { out.dual[i] = Scalar(0); } return out; } @@ -498,7 +524,7 @@ namespace mathlib { inline DualNumber_T conj(const DualNumber_T& a) { return a; } // Since dual numbers are not complex, the norm is just the absolute value of the real part template - inline Scalar norm(const DualNumber_T& a) { return std::abs(a.real); } + inline Scalar norm(const DualNumber_T& a) { return abs(a.real); } // Since dual numbers are not complex, the squared norm is just the square of the absolute value of the real part template inline Scalar abs2(const DualNumber_T& a) { return a.real * a.real; } @@ -540,11 +566,11 @@ namespace mathlib { } // Check if the real part and all dual parts are finite template - inline bool isFinite(const DualNumber_T& x) { - if (!std::isfinite(x.real)) { return false; } + inline bool isfinite(const DualNumber_T& x) { + if (!isfinite(x.real)) { return false; } for (size_t i = 0; i < NVar; ++i) { - if (!std::isfinite(x.dual[i])) { return false; } + if (!isfinite(x.dual[i])) { return false; } } return true; @@ -612,7 +638,7 @@ namespace Eigen { struct scalar_abs_op> { using Dual = mathlib::DualNumber_T; typedef Scalar result_type; - EIGEN_DEVICE_FUNC inline Scalar operator()(const Dual& x) const { return std::abs(x.real); } + EIGEN_DEVICE_FUNC inline Scalar operator()(const Dual& x) const { return mathlib::abs(x.real); } }; // Specialisation of scalar_score_coeff_op for DualNumber_T to allow Eigen's internal sorting and selection algorithms to work correctly with dual numbers @@ -621,7 +647,7 @@ namespace Eigen { struct result_type { Scalar score; result_type(int i = 0) : score(i) {} // Default constructor for compatibility with Eigen's internal mechanisms - result_type(const mathlib::DualNumber_T& x) : score(std::abs(x.real)) {} + result_type(const mathlib::DualNumber_T& x) : score(mathlib::abs(x.real)) {} friend bool operator <(const result_type& a, const result_type& b) { return a.score < b.score; } friend bool operator >(const result_type& a, const result_type& b) { return a.score > b.score; } @@ -729,11 +755,11 @@ namespace Eigen { template inline Scalar imag(const mathlib::DualNumber_T&) { return Scalar(0); } template - inline Scalar abs(const mathlib::DualNumber_T& x) { return std::abs(x.real); } + inline Scalar abs(const mathlib::DualNumber_T& x) { return mathlib::abs(x.real); } template inline Scalar abs2(const mathlib::DualNumber_T& x) { return x.real * x.real; } template - inline Scalar norm1(const mathlib::DualNumber_T& x) { return std::abs(x.real); } + inline Scalar norm1(const mathlib::DualNumber_T& x) { return mathlib::abs(x.real); } template inline mathlib::DualNumber_T conj(const mathlib::DualNumber_T& x) { return x; } }// namespace numext From 95339ca6c681da507a11f9f321711dd4eb001e2c Mon Sep 17 00:00:00 2001 From: saltyjoss Date: Sat, 23 May 2026 17:55:44 +0100 Subject: [PATCH 066/210] feat: Added internal `step_impl` template method for allowing scalar step methods (such as double OR DualNumber_T) to be called * Added new `RobotStepResult_T` which allows dynamic movement of snap and other info previously used in step only. --- .../DSFE_Core/include/Robots/RobotSystem.h | 20 +++- DSFE_App/DSFE_Core/src/Robots/RobotSystem.cpp | 105 ++++++++++-------- 2 files changed, 76 insertions(+), 49 deletions(-) diff --git a/DSFE_App/DSFE_Core/include/Robots/RobotSystem.h b/DSFE_App/DSFE_Core/include/Robots/RobotSystem.h index d886eff6..31902d66 100644 --- a/DSFE_App/DSFE_Core/include/Robots/RobotSystem.h +++ b/DSFE_App/DSFE_Core/include/Robots/RobotSystem.h @@ -32,6 +32,20 @@ namespace robots { Baseline }; + // Step Result struct + template + struct RobotStepResult_T { + integration::StepOut_T integration; + RobotSimSnapshot_T snap; + std::vector> T_world; + std::vector> jointWorldPoses; + mathlib::MatX_T M; + mathlib::VecX_T tau_rnea; + mathlib::VecX_T tau_g; + Scalar dt_taken; + Scalar dt_sug; + }; + class DSFE_API RobotSystem { public: RobotSystem(); @@ -112,7 +126,8 @@ namespace robots { // --- SIMULATION STEP METHOD --- - RobotSimSnapshot takeSnapshot(double simTime) const; + template + RobotSimSnapshot takeSnapshot(Scalar simTime) const; void step(double dt, double simTime); void updateTrajectoryInputs(control::TrajectoryManager& traj, double t); @@ -164,6 +179,9 @@ namespace robots { void buildLinkIndex(); void buildSpatialModel(); + template + RobotStepResult_T step_impl(Scalar dt, Scalar t, IntegratorT& integrator); + std::unique_ptr _kinematics; std::unique_ptr _dynamics; diff --git a/DSFE_App/DSFE_Core/src/Robots/RobotSystem.cpp b/DSFE_App/DSFE_Core/src/Robots/RobotSystem.cpp index a20a53db..a41a59f0 100644 --- a/DSFE_App/DSFE_Core/src/Robots/RobotSystem.cpp +++ b/DSFE_App/DSFE_Core/src/Robots/RobotSystem.cpp @@ -256,7 +256,8 @@ namespace robots { } // Method to take a snapshot of the current robot state - RobotSimSnapshot RobotSystem::takeSnapshot(double simTime) const { + template + RobotSimSnapshot RobotSystem::takeSnapshot(Scalar simTime) const { RobotSimSnapshot snap; snap.model = &_constModel; const size_t n = (size_t)_robot.joints.size(); @@ -292,26 +293,26 @@ namespace robots { return snap; } - // Method to advance the robot state by dt using the selected integrator - void RobotSystem::step(double dt, double simTime) { - if (!_hasRobot) { return; } - _simTime = simTime; + template + RobotStepResult_T RobotSystem::step_impl(Scalar dt, Scalar t, IntegratorT& integrator) { + _simTime = t; mathlib::VecX x = packState(); - - RobotSimSnapshot_T snap = takeSnapshot(simTime); + RobotStepResult_T result; + result.snap = takeSnapshot(t); + using snap = result.snap; const size_t n = snap.model->joints.size(); - Eigen::Map> q(x.data(), n); - Eigen::Map> qd(x.data() + n, n); + Eigen::Map> q(x.data(), n); + Eigen::Map> qd(x.data() + n, n); - mathlib::VecX_T qdd(n); + mathlib::VecX_T qdd(n); for (size_t i = 0; i < n; ++i) { qdd[i] = _robot.joints[i].qdd_ref; } std::vector T_start(snap.model->links.size()); _kinematics->computeForwardKinematics_fromState(*snap.model, x, T_start); std::vector jointWorldPoses_start = _kinematics->calcJointWorldPoses(T_start, *snap.model); - SpatialDynamics::computeSpatialKinematicsAndBias( + SpatialDynamics::computeSpatialKinematicsAndBias( _spatialModel, q, qd, _dynScratch.spatial.Xup, @@ -319,36 +320,36 @@ namespace robots { ); // CRBA only for controller inertia scaling - mathlib::MatX_T M_start = SpatialDynamics::CRBA( + mathlib::MatX_T M_start = SpatialDynamics::CRBA( _spatialModel, _dynScratch.spatial.Xup, _dynScratch ); // Cache frozen joint gains for this step - mathlib::VecX_T kp_frozen(n), kd_frozen(n); + mathlib::VecX_T kp_frozen(n), kd_frozen(n); for (size_t i = 0; i < n; ++i) { const auto& joint = snap.model->joints[i]; if (joint.type == eJointType::FIXED) { continue; } - _dynScratch.dense.I_eff_controller[i] = std::max(M_start(i, i), 1e-6); - const double I_eff = _dynScratch.dense.I_eff_controller[i]; + _dynScratch.dense.I_eff_controller[i] = mathlib::max(M_start(i, i), 1e-6); + const Scalar I_eff = _dynScratch.dense.I_eff_controller[i]; kp_frozen[i] = I_eff * joint.wn_target * joint.wn_target; kd_frozen[i] = 2.0 * joint.zeta_target * I_eff * joint.wn_target; } // Compute RNEA torques for feedforward control - mathlib::VecX_T tau_rnea = SpatialDynamics::RNEA( + mathlib::VecX_T tau_rnea = SpatialDynamics::RNEA( _spatialModel, q, qd, qdd, _dynScratch ); // [Nm] - LOG_INFO_ONCE("tau_rnea size = %lld", (long long)tau_rnea.size()); + LOG_INFO_ONCE("tau_rnea size = %d", (double)tau_rnea.size()); // Define the derivative function for integration, capturing necessary variables by reference auto f_deriv = [&, kp_frozen, kd_frozen](auto t, const auto& xIn) { - return _dynamics->derivative_spatial( + return _dynamics->derivative_spatial( _spatialModel, t, xIn, snap, @@ -356,49 +357,62 @@ namespace robots { ); }; // Define the Jacobian function for integration, capturing necessary variables by reference - auto f_J = [&, kp_frozen, kd_frozen](const mathlib::VecX_T& xIn, mathlib::MatX_T& J_out) { - _dynamics->jacobian_spatial( + auto f_J = [&, kp_frozen, kd_frozen](const mathlib::VecX_T& xIn, mathlib::MatX_T& J_out) { + _dynamics->jacobian_spatial( _spatialModel, xIn, snap, kp_frozen, kd_frozen, J_out, _dynScratch ); }; - - auto step = _integrator->step(_curIntMethod, x, simTime, dt, f_deriv, f_J); + + auto step = _integrator->step(_curIntMethod, x, t, dt, f_deriv, f_J); unpackState(step.x_next); _dynamics->setDt(step.dt_taken); - Eigen::Map> q_next(step.x_next.data(), n); - Eigen::Map> qd_next(step.x_next.data() + n, n); + Eigen::Map> q_next(step.x_next.data(), n); + Eigen::Map> qd_next(step.x_next.data() + n, n); // Enforce joint limits for (auto& j : _robot.joints) { enforceJointLimits(j); } // Recompute kinematics and dynamics at the new state for logging and control purposes - std::vector> T_world(snap.model->links.size()); - _kinematics->computeForwardKinematics_fromState(*snap.model, step.x_next, T_world); - std::vector> jointWorldPoses = _kinematics->calcJointWorldPoses(T_world, *snap.model); + std::vector> T_world(snap.model->links.size()); + _kinematics->computeForwardKinematics_fromState(*snap.model, step.x_next, T_world); + std::vector> jointWorldPoses = _kinematics->calcJointWorldPoses(T_world, *snap.model); // Compute spatial kinematics and bias terms for the new state - SpatialDynamics::computeSpatialKinematicsAndBias( + SpatialDynamics::computeSpatialKinematicsAndBias( _spatialModel, q_next, qd_next, _dynScratch.spatial.Xup, _dynScratch.spatial.v, _dynScratch.spatial.c ); // Compute mass matrix at the new state - mathlib::MatX_T M_full = SpatialDynamics::CRBA( - _spatialModel, - _dynScratch.spatial.Xup, - _dynScratch - ); + mathlib::MatX_T M = SpatialDynamics::CRBA(_spatialModel, _dynScratch.spatial.Xup, _dynScratch); + mathlib::VecX_T tau_g = _dynamics->computeGravityTorque(*snap.model, T_world, jointWorldPoses); + + result.integration = integrator; + result.T_world = T_world; + result.jointWorldPoses = jointWorldPoses; + result.M = M; + result.tau_rnea = tau_rnea; + result.tau_g = tau_g; + result.dt_taken = step.dt_taken; + result.dt_sug = step.dt_sug; + + return result; + } - mathlib::VecX_T tau_g = _dynamics->computeGravityTorque(*snap.model, T_world, jointWorldPoses); + // Method to advance the robot state by dt using the selected integrator + void RobotSystem::step(double dt, double simTime) { + if (!_hasRobot) { return; } + const size_t n = _robot.joints.size(); + auto result = step_impl(dt, simTime, *_integrator); // Compute system kinetic energy: E_kin = 0.5 * qd^T * M(q) * qd - double sys_KE = 0.5 * qd_next.transpose() * M_full * qd_next; // [J], kinetic energy of the robot at configuration q and velocity qd + double sys_KE = 0.5 * result.snap.qd.transpose() * result.M * result.snap.qd; // [J], kinetic energy of the robot at configuration q and velocity qd // Compute system potential energy at configuration q (relative to gravity) double sys_PE = 0.0; @@ -407,13 +421,8 @@ namespace robots { for (size_t k = 0; k < _robot.links.size(); ++k) { const RobotLink& link = _robot.links[k]; const double m = link.inertial.mass; - if (m <= 0.0) { continue; } - - Vec3 com_world = - (T_world[k].block<3, 3>(0, 0) * link.inertial.com_xyz) + - T_world[k].block<3, 1>(0, 3); - + Vec3 com_world = (result.T_world[k].block<3, 3>(0, 0) * link.inertial.com_xyz) + result.T_world[k].block<3, 1>(0, 3); sys_PE += m * g * com_world.z(); } @@ -432,21 +441,21 @@ namespace robots { if (buf) { for (size_t i = 0; i < n; ++i) { - const RobotJoint& j = snap.model->joints[i]; + const RobotJoint& j = _robot.joints[i]; const double I_eff = (j.type == eJointType::FIXED) ? 1.0 : _dynResult.metrics.I_eff[i]; - const double err = snap.q_ref[i] - q_next[i]; - const double err_d = snap.qd_ref[i] - qd_next[i]; + const double err = result.snap.q_ref[i] - result.snap.q[i]; + const double err_d = result.snap.qd_ref[i] - result.snap.qd[i]; JointLogBuffer::JointLogEntry e{}; e.sim_time = simTime; - e.dt_taken = step.dt_taken; - e.dt_sug = step.dt_sug; - e.theta = q_next[i]; e.omega = qd_next[i]; e.alpha = _dynResult.metrics.qdd[i]; + e.dt_taken = result.dt_taken; + e.dt_sug = result.dt_sug; + e.theta = result.snap.q[i]; e.omega = result.snap.qd[i]; e.alpha = _dynResult.metrics.qdd[i]; e.err = err; e.err_d = err_d; e.I_eff = I_eff; - e.tau = _dynResult.metrics.tau[i]; e.tau_ff = tau_rnea[i]; e.tau_gravity = tau_g[i]; + e.tau = _dynResult.metrics.tau[i]; e.tau_ff = result.tau_rnea[i]; e.tau_gravity = result.tau_g[i]; e.tau_sat = _dynResult.metrics.tau_sat[i]; e.KE = sys_KE; e.PE = sys_PE; e.E_total = sys_E; e.clamp_theta = (double)_clampTheta[i]; e.clamp_omega = (double)_clampOmega[i]; From d6ac84e3a461ae4bb0dc36d57fee82eaaae2a8c1 Mon Sep 17 00:00:00 2001 From: saltyjoss Date: Sat, 23 May 2026 20:46:40 +0100 Subject: [PATCH 067/210] feat(WIP): Integrating DualNumbers and AutoDiff into RobotSystem and SimulationCore --- .../include/Numerics/IntegrationService.h | 3 + .../DSFE_Core/include/Robots/RobotSystem.h | 23 ++- .../src/Numerics/IntegrationService.cpp | 46 ++--- DSFE_App/DSFE_Core/src/Robots/RobotSystem.cpp | 195 ++++++++++++++---- 4 files changed, 189 insertions(+), 78 deletions(-) diff --git a/DSFE_App/DSFE_Core/include/Numerics/IntegrationService.h b/DSFE_App/DSFE_Core/include/Numerics/IntegrationService.h index 2dc8f382..fb88ba92 100644 --- a/DSFE_App/DSFE_Core/include/Numerics/IntegrationService.h +++ b/DSFE_App/DSFE_Core/include/Numerics/IntegrationService.h @@ -59,8 +59,11 @@ namespace integration { template StepOut_T step(eAutoDiffIntegrationMethod m, mathlib::VecX_T& x, Scalar t, Scalar dt, Func&& f); + const std::string IntegratorName(eAutoDiffIntegrationMethod m); private: + const char* toString(eAutoDiffIntegrationMethod m); + integration::eAutoDiffIntegrationMethod _m; std::unique_ptr _integrator; std::string _mStr = "AD_ImplicitEuler"; diff --git a/DSFE_App/DSFE_Core/include/Robots/RobotSystem.h b/DSFE_App/DSFE_Core/include/Robots/RobotSystem.h index 31902d66..7a3ccf76 100644 --- a/DSFE_App/DSFE_Core/include/Robots/RobotSystem.h +++ b/DSFE_App/DSFE_Core/include/Robots/RobotSystem.h @@ -35,15 +35,9 @@ namespace robots { // Step Result struct template struct RobotStepResult_T { - integration::StepOut_T integration; + integration::StepOut_T stepOut; RobotSimSnapshot_T snap; - std::vector> T_world; - std::vector> jointWorldPoses; - mathlib::MatX_T M; mathlib::VecX_T tau_rnea; - mathlib::VecX_T tau_g; - Scalar dt_taken; - Scalar dt_sug; }; class DSFE_API RobotSystem { @@ -130,6 +124,8 @@ namespace robots { RobotSimSnapshot takeSnapshot(Scalar simTime) const; void step(double dt, double simTime); + template + void step_AD(double dt, double simTime); void updateTrajectoryInputs(control::TrajectoryManager& traj, double t); // --- ROBOT LOADING AND RESET METHODS --- @@ -155,6 +151,10 @@ namespace robots { void setIntegrationMethod(integration::eIntegrationMethod method) { _curIntMethod = method; } std::string getIntegratorName() const { return _integrator->IntegratorName(_curIntMethod); } + integration::eAutoDiffIntegrationMethod AD_IntegrationMethod() const { return _curIntMethod_AD; } + void setIntegrationMethod(integration::eAutoDiffIntegrationMethod method) { _curIntMethod_AD = method; } + std::string AD_integratorName() const { return _AD_integrator->IntegratorName(_curIntMethod_AD); } + integration::IntegrationService* getIntegrator(); const integration::IntegrationService* getIntegrator() const; @@ -188,6 +188,9 @@ namespace robots { std::unique_ptr _integrator; integration::eIntegrationMethod _curIntMethod{}; + std::unique_ptr _AD_integrator; + integration::eAutoDiffIntegrationMethod _curIntMethod_AD{}; + eRole _role = eRole::Simulation; double _wn = 0.0; // configurable natural frequency for PD control (rad/s) @@ -269,9 +272,9 @@ namespace robots { double _baseYawAcc = 0.0; // Tunables - double _baseMass = 62.0; // kg (H1 ~60–65) - double _baseLinearDamping = 6.0; // Ns/m - double _baseYawDamping = 2.0; // Nms/rad + double _baseMass = 62.0; // kg (H1 ~60–65) + double _baseLinearDamping = 6.0; // Ns/m + double _baseYawDamping = 2.0; // Nms/rad double _lastBaseForwardForce = 0.0; // Double-buffer design diff --git a/DSFE_App/DSFE_Core/src/Numerics/IntegrationService.cpp b/DSFE_App/DSFE_Core/src/Numerics/IntegrationService.cpp index 4f3abe9c..7e2e240e 100644 --- a/DSFE_App/DSFE_Core/src/Numerics/IntegrationService.cpp +++ b/DSFE_App/DSFE_Core/src/Numerics/IntegrationService.cpp @@ -31,34 +31,34 @@ namespace integration { // Helper to convert integration method enum to string const char* IntegrationService::toString(eIntegrationMethod m) { switch (m) { - case eIntegrationMethod::Euler: - return "euler"; - case eIntegrationMethod::Midpoint: - return "midpoint"; - case eIntegrationMethod::Heun: - return "heun"; - case eIntegrationMethod::Ralston: - return "ralston"; - case eIntegrationMethod::RK4: - return "rk4"; - case eIntegrationMethod::RK45: - return "rk45"; - case eIntegrationMethod::ImplicitEuler: - return "implicit_euler"; - case eIntegrationMethod::ImplicitMidpoint: - return "implicit_midpoint"; - case eIntegrationMethod::GLRK2: - return "glrk2"; - case eIntegrationMethod::GLRK3: - return "glrk3"; - default: - return "Unknown"; + case eIntegrationMethod::Euler: return "euler"; + case eIntegrationMethod::Midpoint: return "midpoint"; + case eIntegrationMethod::Heun: return "heun"; + case eIntegrationMethod::Ralston: return "ralston"; + case eIntegrationMethod::RK4: return "rk4"; + case eIntegrationMethod::RK45: return "rk45"; + case eIntegrationMethod::ImplicitEuler: return "implicit_euler"; + case eIntegrationMethod::ImplicitMidpoint: return "implicit_midpoint"; + case eIntegrationMethod::GLRK2: return "glrk2"; + case eIntegrationMethod::GLRK3: return "glrk3"; + default: return "Unknown method"; + } + } + + const char* DifferentiableIntegrator::toString(eAutoDiffIntegrationMethod m) { + switch (m) { + case eAutoDiffIntegrationMethod::AD_ImplicitEuler: return "ad_implicit_euler"; + case eAutoDiffIntegrationMethod::AD_ImplicitMidpoint: return "ad_implicit_midpoint"; + case eAutoDiffIntegrationMethod::AD_GLRK2: return "ad_glrk2"; + case eAutoDiffIntegrationMethod::AD_GLRK3: return "ad_glrk3"; + default: return "Unknown method"; } } // Get integrator name const std::string IntegrationService::IntegratorName(eIntegrationMethod m) { return std::string(toString(m)); } - + const std::string DifferentiableIntegrator::IntegratorName(eAutoDiffIntegrationMethod m) { return std::string(toString(m)); } + // Constructor IntegrationService::IntegrationService() : _integrator(std::make_unique()), method(eIntegrationMethod::RK4), _rtol(1e-3), _atol(1e-6), _dt_last(), _dt_max() { diff --git a/DSFE_App/DSFE_Core/src/Robots/RobotSystem.cpp b/DSFE_App/DSFE_Core/src/Robots/RobotSystem.cpp index a41a59f0..0e48ae2c 100644 --- a/DSFE_App/DSFE_Core/src/Robots/RobotSystem.cpp +++ b/DSFE_App/DSFE_Core/src/Robots/RobotSystem.cpp @@ -28,9 +28,10 @@ namespace robots { // Constructor RobotSystem::RobotSystem() : _integrator(std::make_unique()), _curIntMethod(integration::eIntegrationMethod::RK4), + _AD_integrator(std::make_unique()), _curIntMethod_AD(integration::eAutoDiffIntegrationMethod::AD_ImplicitEuler), _kinematics(std::make_unique()), _dynamics(std::make_unique()), _torqueMode(eTorqueMode::CONTROLLED) { - if (!_integrator) { LOG_WARN("RobotSystem got null IntegrationService*"); } + if (!_integrator ) { LOG_WARN("RobotSystem got null IntegrationService*"); } } // Destructor RobotSystem::~RobotSystem() = default; @@ -204,7 +205,6 @@ namespace robots { if (theta_out != theta_in) { const double upperLimit = j.limits.maxAngle; const double lowerLimit = j.limits.minAngle; - if (theta_out >= upperLimit && omega_in > 0.0f) { omega_out = 0.0f; } if (theta_out <= lowerLimit && omega_in < 0.0f) { omega_out = 0.0f; } } @@ -212,7 +212,6 @@ namespace robots { // Record clamping _clampTheta[i] = (theta_in != theta_out) ? 1 : 0; _clampOmega[i] = (omega_in != omega_out) ? 1 : 0; - // Update joint states j.q = theta_out; j.qd = omega_out; @@ -295,11 +294,10 @@ namespace robots { template RobotStepResult_T RobotSystem::step_impl(Scalar dt, Scalar t, IntegratorT& integrator) { - _simTime = t; - mathlib::VecX x = packState(); + mathlib::VecX_T x = packState().template cast(); RobotStepResult_T result; result.snap = takeSnapshot(t); - using snap = result.snap; + auto& snap = result.snap; const size_t n = snap.model->joints.size(); Eigen::Map> q(x.data(), n); @@ -308,9 +306,9 @@ namespace robots { mathlib::VecX_T qdd(n); for (size_t i = 0; i < n; ++i) { qdd[i] = _robot.joints[i].qdd_ref; } - std::vector T_start(snap.model->links.size()); + std::vector> T_start(snap.model->links.size()); _kinematics->computeForwardKinematics_fromState(*snap.model, x, T_start); - std::vector jointWorldPoses_start = _kinematics->calcJointWorldPoses(T_start, *snap.model); + std::vector> jointWorldPoses_start = _kinematics->calcJointWorldPoses(T_start, *snap.model); SpatialDynamics::computeSpatialKinematicsAndBias( _spatialModel, @@ -332,11 +330,11 @@ namespace robots { const auto& joint = snap.model->joints[i]; if (joint.type == eJointType::FIXED) { continue; } - _dynScratch.dense.I_eff_controller[i] = mathlib::max(M_start(i, i), 1e-6); + _dynScratch.dense.I_eff_controller[i] = mathlib::max(M_start(i, i), Scalar(1e-6)); const Scalar I_eff = _dynScratch.dense.I_eff_controller[i]; kp_frozen[i] = I_eff * joint.wn_target * joint.wn_target; - kd_frozen[i] = 2.0 * joint.zeta_target * I_eff * joint.wn_target; + kd_frozen[i] = Scalar(2) * joint.zeta_target * I_eff * joint.wn_target; } // Compute RNEA torques for feedforward control @@ -345,6 +343,7 @@ namespace robots { q, qd, qdd, _dynScratch ); // [Nm] + result.tau_rnea = tau_rnea; LOG_INFO_ONCE("tau_rnea size = %d", (double)tau_rnea.size()); // Define the derivative function for integration, capturing necessary variables by reference @@ -366,53 +365,58 @@ namespace robots { ); }; - auto step = _integrator->step(_curIntMethod, x, t, dt, f_deriv, f_J); + if constexpr (std::is_same_v, integration::IntegrationService>) { + mathlib::VecX x_real = x.template cast(); + auto step = integrator.step(_curIntMethod, x_real, static_cast(t), static_cast(dt), f_deriv, f_J); + result.stepOut.x_next = step.x_next.template cast(); + result.stepOut.dt_taken = step.dt_taken; + result.stepOut.dt_sug = step.dt_sug; + } + else if constexpr (std::is_same_v, integration::DifferentiableIntegrator>) { + result.stepOut = integrator.step(_curIntMethod_AD, x, t, dt, f_deriv); + } + + for (int i = 0; i < result.stepOut.x_next.size(); ++i) { + const auto v = mathlib::real(result.stepOut.x_next[i]); + if (std::isnan(v) || std::isinf(v)) { LOG_ERROR("Non-finite x_next[%d] = %f", i, (double)v); } + } + return result; + } + + // Method to advance the robot state by dt using the selected integrator + void RobotSystem::step(double dt, double simTime) { + if (!_hasRobot) { return; } + _simTime = simTime; + const size_t n = _robot.joints.size(); + auto result = step_impl(dt, simTime, *_integrator); - unpackState(step.x_next); - _dynamics->setDt(step.dt_taken); + unpackState(result.stepOut.x_next); + _dynamics->setDt(result.stepOut.dt_taken); - Eigen::Map> q_next(step.x_next.data(), n); - Eigen::Map> qd_next(step.x_next.data() + n, n); + Eigen::Map q_next(result.stepOut.x_next.data(), n); + Eigen::Map qd_next(result.stepOut.x_next.data() + n, n); // Enforce joint limits for (auto& j : _robot.joints) { enforceJointLimits(j); } // Recompute kinematics and dynamics at the new state for logging and control purposes - std::vector> T_world(snap.model->links.size()); - _kinematics->computeForwardKinematics_fromState(*snap.model, step.x_next, T_world); - std::vector> jointWorldPoses = _kinematics->calcJointWorldPoses(T_world, *snap.model); + std::vector T_world(result.snap.model->links.size()); + _kinematics->computeForwardKinematics_fromState(*result.snap.model, result.stepOut.x_next, T_world); + std::vector jointWorldPoses = _kinematics->calcJointWorldPoses(T_world, *result.snap.model); // Compute spatial kinematics and bias terms for the new state - SpatialDynamics::computeSpatialKinematicsAndBias( + SpatialDynamics::computeSpatialKinematicsAndBias( _spatialModel, q_next, qd_next, _dynScratch.spatial.Xup, _dynScratch.spatial.v, _dynScratch.spatial.c ); // Compute mass matrix at the new state - mathlib::MatX_T M = SpatialDynamics::CRBA(_spatialModel, _dynScratch.spatial.Xup, _dynScratch); - mathlib::VecX_T tau_g = _dynamics->computeGravityTorque(*snap.model, T_world, jointWorldPoses); - - result.integration = integrator; - result.T_world = T_world; - result.jointWorldPoses = jointWorldPoses; - result.M = M; - result.tau_rnea = tau_rnea; - result.tau_g = tau_g; - result.dt_taken = step.dt_taken; - result.dt_sug = step.dt_sug; - - return result; - } - - // Method to advance the robot state by dt using the selected integrator - void RobotSystem::step(double dt, double simTime) { - if (!_hasRobot) { return; } - const size_t n = _robot.joints.size(); - auto result = step_impl(dt, simTime, *_integrator); + mathlib::MatX M = SpatialDynamics::CRBA(_spatialModel, _dynScratch.spatial.Xup, _dynScratch); + mathlib::VecX tau_g = _dynamics->computeGravityTorque(*result.snap.model, T_world, jointWorldPoses); // Compute system kinetic energy: E_kin = 0.5 * qd^T * M(q) * qd - double sys_KE = 0.5 * result.snap.qd.transpose() * result.M * result.snap.qd; // [J], kinetic energy of the robot at configuration q and velocity qd + double sys_KE = 0.5 * result.snap.qd.transpose() * M * result.snap.qd; // [J], kinetic energy of the robot at configuration q and velocity qd // Compute system potential energy at configuration q (relative to gravity) double sys_PE = 0.0; @@ -422,7 +426,7 @@ namespace robots { const RobotLink& link = _robot.links[k]; const double m = link.inertial.mass; if (m <= 0.0) { continue; } - Vec3 com_world = (result.T_world[k].block<3, 3>(0, 0) * link.inertial.com_xyz) + result.T_world[k].block<3, 1>(0, 3); + Vec3 com_world = (T_world[k].block<3, 3>(0, 0) * link.inertial.com_xyz) + T_world[k].block<3, 1>(0, 3); sys_PE += m * g * com_world.z(); } @@ -450,15 +454,15 @@ namespace robots { JointLogBuffer::JointLogEntry e{}; e.sim_time = simTime; - e.dt_taken = result.dt_taken; - e.dt_sug = result.dt_sug; + e.dt_taken = result.stepOut.dt_taken; + e.dt_sug = result.stepOut.dt_sug; e.theta = result.snap.q[i]; e.omega = result.snap.qd[i]; e.alpha = _dynResult.metrics.qdd[i]; e.err = err; e.err_d = err_d; e.I_eff = I_eff; - e.tau = _dynResult.metrics.tau[i]; e.tau_ff = result.tau_rnea[i]; e.tau_gravity = result.tau_g[i]; + e.tau = _dynResult.metrics.tau[i]; e.tau_ff = result.tau_rnea[i]; e.tau_gravity = tau_g[i]; e.tau_sat = _dynResult.metrics.tau_sat[i]; e.KE = sys_KE; e.PE = sys_PE; e.E_total = sys_E; - e.clamp_theta = (double)_clampTheta[i]; e.clamp_omega = (double)_clampOmega[i]; + e.clamp_theta = static_cast(_clampTheta[i]); e.clamp_omega = static_cast(_clampOmega[i]); e.sat_flag = _dynResult.metrics.sat_flag[i]; e.joint_index = (int)i; buf->push_entry(e); } @@ -474,6 +478,107 @@ namespace robots { computeRobotKinematics(_worldTransforms); } + template + void RobotSystem::step_AD(double dt, double simTime) { + if (!hasRobot()) { return; } + using Dual = mathlib::DualNumber_T(); + _simTime = simTime; + const size_t n = _robot.joints.size(); + auto result = step_impl(Dual(dt), Dual(simTime), *_AD_integrator); + mathlib::VecX x_real = result.stepOut.x_next.template cast(); + + unpackState(x_real); + _dynamics->setDt(result.stepOut.dt_taken); + + Eigen::Map q_next(x_real.data(), n); + Eigen::Map qd_next(x_real.data() + n, n); + + // Enforce joint limits + for (auto& j : _robot.joints) { enforceJointLimits(j); } + + // Recompute kinematics and dynamics at the new state for logging and control purposes + std::vector T_world(result.snap.model->links.size()); + _kinematics->computeForwardKinematics_fromState(*result.snap.model, x_real, T_world); + std::vector jointWorldPoses = _kinematics->calcJointWorldPoses(T_world, *result.snap.model); + + // Compute spatial kinematics and bias terms for the new state + SpatialDynamics::computeSpatialKinematicsAndBias( + _spatialModel, + q_next, qd_next, + _dynScratch.spatial.Xup, + _dynScratch.spatial.v, _dynScratch.spatial.c + ); + // Compute mass matrix at the new state + mathlib::MatX M = SpatialDynamics::CRBA(_spatialModel, _dynScratch.spatial.Xup, _dynScratch); + mathlib::VecX tau_g = _dynamics->computeGravityTorque(*result.snap.model, T_world, jointWorldPoses); + + // Compute system kinetic energy: E_kin = 0.5 * qd^T * M(q) * qd + double sys_KE = 0.5 * result.snap.qd.transpose() * M * result.snap.qd; // [J], kinetic energy of the robot at configuration q and velocity qd + + // Compute system potential energy at configuration q (relative to gravity) + double sys_PE = 0.0; + double g = _dynamics->getGravity(); + + for (size_t k = 0; k < _robot.links.size(); ++k) { + const RobotLink& link = _robot.links[k]; + const double m = link.inertial.mass; + if (m <= 0.0) { continue; } + Vec3 com_world = (T_world[k].block<3, 3>(0, 0) * link.inertial.com_xyz) + T_world[k].block<3, 1>(0, 3); + sys_PE += m * g * com_world.z(); + } + + const double sys_E = sys_KE + sys_PE; // total mechanical energy of the system + + // Log metrics to buffer if logging is enabled + robots::JointLogBuffer* buf = nullptr; + + // If using internal logging, get the active buffer + if (_useInternalLogging) { + int idx = _activeLogBufIdx.load(std::memory_order_acquire); + buf = &_logBuffers[idx]; + } + else { + buf = _logBuffer; + } + + if (buf) { + auto q_real = result.snap.q.template cast(); + auto qd_real = result.snap.qd.template cast(); + auto q_ref_real = result.snap.q_ref.template cast(); + auto qd_ref_real = result.snap.qd_ref.template cast(); + auto tau_rnea_real = result.tau_rnea.template cast(); + + for (size_t i = 0; i < n; ++i) { + const RobotJoint& j = _robot.joints[i]; + + const double I_eff = (j.type == eJointType::FIXED) ? 1.0 : _dynResult.metrics.I_eff[i]; + const double err = q_ref_real[i] - q_real[i]; + const double err_d = qd_ref_real[i] - qd_real[i] ; + + JointLogBuffer::JointLogEntry e{}; + + e.sim_time = simTime; + e.dt_taken = result.stepOut.dt_taken; + e.dt_sug = result.stepOut.dt_sug; + e.theta = q_real[i]; e.omega = qd_real[i]; e.alpha = _dynResult.metrics.qdd[i]; + e.err = err; e.err_d = err_d; + e.I_eff = I_eff; + e.tau = _dynResult.metrics.tau[i]; e.tau_ff = tau_rnea_real[i]; e.tau_gravity = tau_g[i]; + e.tau_sat = _dynResult.metrics.tau_sat[i]; + e.KE = sys_KE; e.PE = sys_PE; e.E_total = sys_E; + e.clamp_theta = static_cast(_clampTheta[i]); e.clamp_omega = static_cast(_clampOmega[i]); + e.sat_flag = _dynResult.metrics.sat_flag[i]; e.joint_index = (int)i; + buf->push_entry(e); + } + } + + // Update base pose if free-floating + if (_baseIsFree) { + integrateBaseTranslation(dt); + updateBaseRootPose(); + } + } + // Method to step the reference trajectory and update joint reference states void RobotSystem::updateTrajectoryInputs(control::TrajectoryManager& traj, double t) { if (!_hasRobot) { return; } From 4630c6781b0b5c219c642e44701227a1581639f1 Mon Sep 17 00:00:00 2001 From: saltyjoss Date: Sun, 24 May 2026 07:27:07 +0100 Subject: [PATCH 068/210] refactor: Updated step_impl to take explicit state vector `x` --- .../DSFE_Core/include/Robots/RobotSystem.h | 2 +- DSFE_App/DSFE_Core/src/Robots/RobotSystem.cpp | 28 +++++++++++++------ 2 files changed, 20 insertions(+), 10 deletions(-) diff --git a/DSFE_App/DSFE_Core/include/Robots/RobotSystem.h b/DSFE_App/DSFE_Core/include/Robots/RobotSystem.h index 7a3ccf76..49bf62da 100644 --- a/DSFE_App/DSFE_Core/include/Robots/RobotSystem.h +++ b/DSFE_App/DSFE_Core/include/Robots/RobotSystem.h @@ -180,7 +180,7 @@ namespace robots { void buildSpatialModel(); template - RobotStepResult_T step_impl(Scalar dt, Scalar t, IntegratorT& integrator); + RobotStepResult_T step_impl(const mathlib::VecX_T& x, Scalar dt, Scalar t, IntegratorT& integrator); std::unique_ptr _kinematics; std::unique_ptr _dynamics; diff --git a/DSFE_App/DSFE_Core/src/Robots/RobotSystem.cpp b/DSFE_App/DSFE_Core/src/Robots/RobotSystem.cpp index 0e48ae2c..607ac819 100644 --- a/DSFE_App/DSFE_Core/src/Robots/RobotSystem.cpp +++ b/DSFE_App/DSFE_Core/src/Robots/RobotSystem.cpp @@ -293,8 +293,7 @@ namespace robots { } template - RobotStepResult_T RobotSystem::step_impl(Scalar dt, Scalar t, IntegratorT& integrator) { - mathlib::VecX_T x = packState().template cast(); + RobotStepResult_T RobotSystem::step_impl(const mathlib::VecX_T& x, Scalar dt, Scalar t, IntegratorT& integrator) { RobotStepResult_T result; result.snap = takeSnapshot(t); auto& snap = result.snap; @@ -388,7 +387,8 @@ namespace robots { if (!_hasRobot) { return; } _simTime = simTime; const size_t n = _robot.joints.size(); - auto result = step_impl(dt, simTime, *_integrator); + mathlib::VecX x = packState(); + auto result = step_impl(x, dt, simTime, *_integrator); unpackState(result.stepOut.x_next); _dynamics->setDt(result.stepOut.dt_taken); @@ -444,6 +444,12 @@ namespace robots { } if (buf) { + auto q_real = result.snap.q.template cast(); + auto qd_real = result.snap.qd.template cast(); + auto q_ref_real = result.snap.q_ref.template cast(); + auto qd_ref_real = result.snap.qd_ref.template cast(); + auto tau_rnea_real = result.tau_rnea.template cast(); + for (size_t i = 0; i < n; ++i) { const RobotJoint& j = _robot.joints[i]; @@ -456,11 +462,11 @@ namespace robots { e.sim_time = simTime; e.dt_taken = result.stepOut.dt_taken; e.dt_sug = result.stepOut.dt_sug; - e.theta = result.snap.q[i]; e.omega = result.snap.qd[i]; e.alpha = _dynResult.metrics.qdd[i]; + e.theta = q_real[i]; e.omega = qd_real[i]; e.alpha = _dynResult.metrics.qdd[i]; e.err = err; e.err_d = err_d; e.I_eff = I_eff; - e.tau = _dynResult.metrics.tau[i]; e.tau_ff = result.tau_rnea[i]; e.tau_gravity = tau_g[i]; - e.tau_sat = _dynResult.metrics.tau_sat[i]; + e.tau = _dynResult.metrics.tau[i]; e.tau_ff = tau_rnea_real[i]; e.tau_gravity = tau_g[i]; + e.tau_sat = _dynResult.metrics.tau_sat[i]; e.KE = sys_KE; e.PE = sys_PE; e.E_total = sys_E; e.clamp_theta = static_cast(_clampTheta[i]); e.clamp_omega = static_cast(_clampOmega[i]); e.sat_flag = _dynResult.metrics.sat_flag[i]; e.joint_index = (int)i; @@ -481,11 +487,15 @@ namespace robots { template void RobotSystem::step_AD(double dt, double simTime) { if (!hasRobot()) { return; } - using Dual = mathlib::DualNumber_T(); + using Dual = mathlib::DualNumber_T; _simTime = simTime; const size_t n = _robot.joints.size(); - auto result = step_impl(Dual(dt), Dual(simTime), *_AD_integrator); - mathlib::VecX x_real = result.stepOut.x_next.template cast(); + mathlib::VecX_T x = packState().template cast(); + + for (size_t i = 0; i < x.size(); ++i) { x[i].dual[i] = 1.0; } + + auto result = step_impl(x, Dual(dt), Dual(simTime), *_AD_integrator); + mathlib::VecX x_real = result.stepOut.x_next.unaryExpr([](const auto& v) { return mathlib::real(v); }); unpackState(x_real); _dynamics->setDt(result.stepOut.dt_taken); From 4f7ed071d8d61d662cf81f47369090886301a29d Mon Sep 17 00:00:00 2001 From: saltyjoss Date: Sun, 24 May 2026 09:23:43 +0100 Subject: [PATCH 069/210] refactor(WIP): Updated AD robot stepping pipeline and isolated runtime post-step evaluation * Moved templated RobotSystem step implementations into .inl files * Added scalar-generic `step_impl()` execution kernel * Added DifferentiableIntegrator execution path for DualNumber-based integration * Added `step_AD()` runtime path for autodiff simulation stepping * Introduced execution-local SpatialModel casting in `step_impl()` * Added relevant cast methods in `SpatialModel` and `SpatialVec_T` * Separated execution scratch from runtime post-step telemetry evaluation * Added `postStepUpdate() `pipeline for authoritative accepted-state recomputation * Replaced shared _dynScratch usage with local runtime scratch in `postStepUpdate()` * Move telemetry/logging/FK recomputation outside integration execution path * Add RobotStepResult_T propagation of dynamics outputs and integration results * Remove shared mutable execution state coupling between AD and runtime paths * Add Dual-state sensitivity seeding for propagated state derivatives * Improve scalar-generic integration flow consistency across runtime backends * Improve separation between execution workspace and semantic simulation outputs * Updated `IntegrationService` and `DifferentiableIntegrator`'s step methods to take a constant system state (x) * Fixed smaller syntax and logic errors causing built failure --- .../include/Numerics/AutoDiffStep.inl | 2 +- .../include/Numerics/IntegrationService.h | 8 +- .../include/Numerics/IntegrationStep.inl | 4 +- .../include/Platform/ISimulationCore.h | 8 +- .../include/Platform/SimulationState.h | 6 + .../DSFE_Core/include/Robots/DynamicsTypes.h | 2 - .../DSFE_Core/include/Robots/RobotSystem.h | 22 +- .../include/Robots/RobotSystemStep.inl | 248 ++++++++++++++ .../DSFE_Core/include/Robots/SpatialModel.h | 6 +- .../include/Robots/SpatialModelCast.inl | 22 ++ .../DSFE_Core/include/Scene/SimulationCore.h | 13 +- DSFE_App/DSFE_Core/src/Robots/RobotSystem.cpp | 323 +----------------- .../DSFE_Core/src/Scene/SimulationCore.cpp | 15 +- PxMLib/MathLib/include/core/SpatialMath.h | 8 +- PxMLib/MathLib/include/core/SpatialVec.inl | 12 + 15 files changed, 356 insertions(+), 343 deletions(-) create mode 100644 DSFE_App/DSFE_Core/include/Robots/RobotSystemStep.inl create mode 100644 DSFE_App/DSFE_Core/include/Robots/SpatialModelCast.inl create mode 100644 PxMLib/MathLib/include/core/SpatialVec.inl diff --git a/DSFE_App/DSFE_Core/include/Numerics/AutoDiffStep.inl b/DSFE_App/DSFE_Core/include/Numerics/AutoDiffStep.inl index 7a0a1e60..a76e49cb 100644 --- a/DSFE_App/DSFE_Core/include/Numerics/AutoDiffStep.inl +++ b/DSFE_App/DSFE_Core/include/Numerics/AutoDiffStep.inl @@ -3,7 +3,7 @@ namespace integration { template - StepOut_T DifferentiableIntegrator::step(eAutoDiffIntegrationMethod m, mathlib::VecX_T& x, Scalar t, Scalar dt, Func&& f) { + StepOut_T DifferentiableIntegrator::step(eAutoDiffIntegrationMethod m, const mathlib::VecX_T& x, Scalar t, Scalar dt, Func&& f) { using Real = typename mathlib::DualTraits::BaseScalar; if constexpr (std::is_pointer_v> || requires { f == nullptr; }) { if (f == nullptr) { diff --git a/DSFE_App/DSFE_Core/include/Numerics/IntegrationService.h b/DSFE_App/DSFE_Core/include/Numerics/IntegrationService.h index fb88ba92..68de60d9 100644 --- a/DSFE_App/DSFE_Core/include/Numerics/IntegrationService.h +++ b/DSFE_App/DSFE_Core/include/Numerics/IntegrationService.h @@ -29,9 +29,9 @@ namespace integration { IntegrationService(); template - StepOut step(eIntegrationMethod m, mathlib::VecX& x, double t, double dt, Func&& f, JacFunc&& jac); + StepOut step(eIntegrationMethod m, const mathlib::VecX& x, double t, double dt, Func&& f, JacFunc&& jac); template - StepOut step_adaptive(eIntegrationMethod m, mathlib::VecX& x, double t, double dt_try, Func&& f, double rtol, double atol); + StepOut step_adaptive(eIntegrationMethod m, const mathlib::VecX& x, double t, double dt_try, Func&& f, double rtol, double atol); void setIntegrationMethod(eIntegrationMethod m) { method = m; } eIntegrationMethod getIntegrationMethod() const { return method; } @@ -58,8 +58,10 @@ namespace integration { DifferentiableIntegrator(); template - StepOut_T step(eAutoDiffIntegrationMethod m, mathlib::VecX_T& x, Scalar t, Scalar dt, Func&& f); + StepOut_T step(eAutoDiffIntegrationMethod m, const mathlib::VecX_T& x, Scalar t, Scalar dt, Func&& f); const std::string IntegratorName(eAutoDiffIntegrationMethod m); + void setIntegrationMethod(eAutoDiffIntegrationMethod m) { _m = m; } + eAutoDiffIntegrationMethod integrationMethod() const { return _m; } private: const char* toString(eAutoDiffIntegrationMethod m); diff --git a/DSFE_App/DSFE_Core/include/Numerics/IntegrationStep.inl b/DSFE_App/DSFE_Core/include/Numerics/IntegrationStep.inl index fa0f9733..16dfcb3d 100644 --- a/DSFE_App/DSFE_Core/include/Numerics/IntegrationStep.inl +++ b/DSFE_App/DSFE_Core/include/Numerics/IntegrationStep.inl @@ -3,7 +3,7 @@ namespace integration { template - StepOut_T IntegrationService::step(eIntegrationMethod m, mathlib::VecX& x, double t, double dt, Func&& f, JacFunc&& jac) { + StepOut_T IntegrationService::step(eIntegrationMethod m, const mathlib::VecX& x, double t, double dt, Func&& f, JacFunc&& jac) { if constexpr (std::is_pointer_v> || requires { f == nullptr; }) { if (f == nullptr) { D_WARN_ONCE("No derivative function provided for integration - Assuming constant derivative (Euler step)"); @@ -33,7 +33,7 @@ namespace integration { } template - StepOut_T IntegrationService::step_adaptive(eIntegrationMethod m, mathlib::VecX& x, double t, double dt_try, Func&& f, double rtol, double atol) { + StepOut_T IntegrationService::step_adaptive(eIntegrationMethod m, const mathlib::VecX& x, double t, double dt_try, Func&& f, double rtol, double atol) { if constexpr (std::is_pointer_v> || requires { f == nullptr; }) { if (f == nullptr) { LOG_WARN("No derivative function provided for adaptive integration - returning state unchanged"); diff --git a/DSFE_App/DSFE_Core/include/Platform/ISimulationCore.h b/DSFE_App/DSFE_Core/include/Platform/ISimulationCore.h index d4e5c4d3..16b75fed 100644 --- a/DSFE_App/DSFE_Core/include/Platform/ISimulationCore.h +++ b/DSFE_App/DSFE_Core/include/Platform/ISimulationCore.h @@ -7,12 +7,14 @@ #include // Forward Declarations -namespace integration { enum class eIntegrationMethod; } +namespace integration { enum class eIntegrationMethod; enum class eAutoDiffIntegrationMethod; } namespace robots { class RobotSystem; } namespace control { class TrajectoryManager; } namespace diagnostics { class TelemetryRecorder; } namespace interpreter { class IStoredProgram; } +enum class eSimulationBackend; + namespace core { struct DSFE_API SimulationSnapshot { double simTime; @@ -29,6 +31,8 @@ namespace core { virtual void startSimulation() = 0; virtual void stopSimulation() = 0; virtual bool isSimRunning() const = 0; + virtual void setSimulationBackend(eSimulationBackend backend) = 0; + virtual eSimulationBackend simulationBackend() const = 0; // Time stepping virtual void setFixedDt(double dt) = 0; virtual double fixedDt() const = 0; @@ -36,8 +40,10 @@ namespace core { virtual SimulationSnapshot snapshot() const = 0; // Integrator virtual void setIntegrationMethod(integration::eIntegrationMethod method) = 0; + virtual void setIntegrationMethod(integration::eAutoDiffIntegrationMethod method) = 0; virtual std::string integrationMethodName() const = 0; virtual integration::eIntegrationMethod integrationMethod() const = 0; + virtual integration::eAutoDiffIntegrationMethod autoDiffIntegrationMethod() const = 0; // Setter for run tag name of current script virtual void setRunTag(const std::string& tag) = 0; // Subsystems diff --git a/DSFE_App/DSFE_Core/include/Platform/SimulationState.h b/DSFE_App/DSFE_Core/include/Platform/SimulationState.h index 50173444..77bdd2ec 100644 --- a/DSFE_App/DSFE_Core/include/Platform/SimulationState.h +++ b/DSFE_App/DSFE_Core/include/Platform/SimulationState.h @@ -32,10 +32,16 @@ enum class eRunMode { Interactive, Synchronous }; +// Current simulation backend integration method (e.g., standard numerical integration[explicit, implicit] vs. auto-differentiation for gradients) +enum class eSimulationBackend { + Standard, + AutoDiff +}; // Struct to hold the current simulation mode and related settings struct DSFE_API modes { eRunMode _runMode = eRunMode::Interactive; + eSimulationBackend _simBackend = eSimulationBackend::Standard; }; diff --git a/DSFE_App/DSFE_Core/include/Robots/DynamicsTypes.h b/DSFE_App/DSFE_Core/include/Robots/DynamicsTypes.h index 350f516b..5534a636 100644 --- a/DSFE_App/DSFE_Core/include/Robots/DynamicsTypes.h +++ b/DSFE_App/DSFE_Core/include/Robots/DynamicsTypes.h @@ -164,8 +164,6 @@ namespace robots { g.resize(nJoints); } - - // Clears all scratch buffers void clear() { dense.clear(); diff --git a/DSFE_App/DSFE_Core/include/Robots/RobotSystem.h b/DSFE_App/DSFE_Core/include/Robots/RobotSystem.h index 49bf62da..86372932 100644 --- a/DSFE_App/DSFE_Core/include/Robots/RobotSystem.h +++ b/DSFE_App/DSFE_Core/include/Robots/RobotSystem.h @@ -8,6 +8,11 @@ #include "Robots/RobotSimSnapshot.h" #include "Robots/DynamicsTypes.h" +#include +#include "Robots/RobotKinematics.h" +#include "Robots/RobotDynamics.h" +#include "Robots/SpatialDynamics.h" + #include "Analysis/MetricLogger.h" #include "Numerics/IntegrationService.h" @@ -16,8 +21,6 @@ namespace control { class TrajectoryManager; } namespace robots { // Forward declarations - class RobotKinematics; - class RobotDynamics; enum class eTorqueMode; // Joint state structure @@ -37,6 +40,7 @@ namespace robots { struct RobotStepResult_T { integration::StepOut_T stepOut; RobotSimSnapshot_T snap; + DynamicsResult dynamics; mathlib::VecX_T tau_rnea; }; @@ -121,11 +125,11 @@ namespace robots { // --- SIMULATION STEP METHOD --- template - RobotSimSnapshot takeSnapshot(Scalar simTime) const; - - void step(double dt, double simTime); + RobotSimSnapshot_T takeSnapshot(Scalar simTime) const; template void step_AD(double dt, double simTime); + + void step(double dt, double simTime); void updateTrajectoryInputs(control::TrajectoryManager& traj, double t); // --- ROBOT LOADING AND RESET METHODS --- @@ -142,7 +146,6 @@ namespace robots { void setRobotRootHome(const mathlib::Vec3& pos, const mathlib::Quat& rot); bool setDefaultPoseDeg(); - void setCurrentJointIndex(int index) { _currentJointIndex = index; } // --- GET AND SET INTEGRATION METHOD --- @@ -158,6 +161,9 @@ namespace robots { integration::IntegrationService* getIntegrator(); const integration::IntegrationService* getIntegrator() const; + integration::DifferentiableIntegrator* getADIntegrator(); + const integration::DifferentiableIntegrator* getADIntegrator() const; + void setRefBuffer(robots::TrajRefBuffer* buf) { _refBuffer = buf; } void setLogBuffer(robots::JointLogBuffer* buf) { _logBuffer = buf; } void setRole(eRole role) { _role = role; } @@ -182,6 +188,9 @@ namespace robots { template RobotStepResult_T step_impl(const mathlib::VecX_T& x, Scalar dt, Scalar t, IntegratorT& integrator); + template + void postStepUpdate(const RobotStepResult_T& result, const mathlib::VecX& x); + std::unique_ptr _kinematics; std::unique_ptr _dynamics; @@ -289,6 +298,7 @@ namespace robots { }; } // namespace robot +#include "RobotSystemStep.inl" // --- Logging macros for robot syste debugging --- diff --git a/DSFE_App/DSFE_Core/include/Robots/RobotSystemStep.inl b/DSFE_App/DSFE_Core/include/Robots/RobotSystemStep.inl new file mode 100644 index 00000000..27201e56 --- /dev/null +++ b/DSFE_App/DSFE_Core/include/Robots/RobotSystemStep.inl @@ -0,0 +1,248 @@ +// DSFE_Core RobotSystemStep.inl +#pragma once + +namespace robots { + // Method to take a snapshot of the current robot state + template + RobotSimSnapshot_T RobotSystem::takeSnapshot(Scalar simTime) const { + RobotSimSnapshot_T snap; + snap.model = &_constModel; + const size_t n = (size_t)_robot.joints.size(); + + snap.q.resize(n); + snap.qd.resize(n); + + snap.q_ref.resize(n); + snap.qd_ref.resize(n); + snap.qdd_ref.resize(n); + + for (size_t i = 0; i < n; ++i) { + const auto& j = _robot.joints[i]; + + snap.q[i] = j.q; + snap.qd[i] = j.qd; + + snap.q_ref[i] = j.q_ref; + snap.qd_ref[i] = j.qd_ref; + snap.qdd_ref[i] = j.qdd_ref; + } + + snap.robotRootPose = _robotRootPose; + snap.baseIsFree = _baseIsFree; + snap.lastBaseForwardForce = _lastBaseForwardForce; + snap.gravity = _gravity; + + snap.torqueMode = _robot.torqueMode; + + snap.dt = _dynamics->dt(); + snap.simTime = simTime; + + return snap; + } + + template + RobotStepResult_T RobotSystem::step_impl(const mathlib::VecX_T& x, Scalar dt, Scalar t, IntegratorT& integrator) { + RobotStepResult_T result; + result.snap = takeSnapshot(t); + auto& snap = result.snap; + const size_t n = snap.model->joints.size(); + + Eigen::Map> q(x.data(), n); + Eigen::Map> qd(x.data() + n, n); + + mathlib::VecX_T qdd(n); + for (size_t i = 0; i < n; ++i) { qdd[i] = _robot.joints[i].qdd_ref; } + + std::vector> T_start(snap.model->links.size()); + _kinematics->computeForwardKinematics_fromState(*snap.model, x, T_start); + std::vector> jointWorldPoses_start = _kinematics->calcJointWorldPoses(T_start, *snap.model); + + SpatialModel spatialModel = _spatialModel.template cast(); + DynamicsScratch dynScratch; + DynamicsResult dynResult; + + SpatialDynamics::computeSpatialKinematicsAndBias( + spatialModel, + q, qd, + dynScratch.spatial.Xup, + dynScratch.spatial.v, dynScratch.spatial.c + ); + + // CRBA only for controller inertia scaling + mathlib::MatX_T M_start = SpatialDynamics::CRBA( + spatialModel, + dynScratch.spatial.Xup, + dynScratch + ); + + // Cache frozen joint gains for this step + mathlib::VecX_T kp_frozen(n), kd_frozen(n); + for (size_t i = 0; i < n; ++i) { + const auto& joint = snap.model->joints[i]; + if (joint.type == eJointType::FIXED) { continue; } + + dynScratch.dense.I_eff_controller[i] = mathlib::max(M_start(i, i), Scalar(1e-6)); + const Scalar I_eff = dynScratch.dense.I_eff_controller[i]; + + kp_frozen[i] = I_eff * joint.wn_target * joint.wn_target; + kd_frozen[i] = Scalar(2) * joint.zeta_target * I_eff * joint.wn_target; + } + + // Compute RNEA torques for feedforward control + mathlib::VecX_T tau_rnea = SpatialDynamics::RNEA( + spatialModel, + q, qd, qdd, + dynScratch + ); // [Nm] + result.tau_rnea = tau_rnea; + LOG_INFO_ONCE("tau_rnea size = %d", (double)tau_rnea.size()); + + // Define the derivative function for integration, capturing necessary variables by reference + auto f_deriv = [&, kp_frozen, kd_frozen](auto t, const auto& xIn) { + return _dynamics->derivative_spatial( + spatialModel, + t, xIn, + snap, + dynScratch, dynResult + ); + }; + // Define the Jacobian function for integration, capturing necessary variables by reference + auto f_J = [&, kp_frozen, kd_frozen](const mathlib::VecX_T& xIn, mathlib::MatX_T& J_out) { + _dynamics->jacobian_spatial( + spatialModel, + xIn, snap, + kp_frozen, kd_frozen, + J_out, dynScratch + ); + }; + + if constexpr (std::is_same_v, integration::IntegrationService>) { + mathlib::VecX x_real = x.template cast(); + auto step = integrator.step(_curIntMethod, x_real, static_cast(t), static_cast(dt), f_deriv, f_J); + result.stepOut.x_next = step.x_next.template cast(); + result.stepOut.dt_taken = step.dt_taken; + result.stepOut.dt_sug = step.dt_sug; + } + else if constexpr (std::is_same_v, integration::DifferentiableIntegrator>) { + result.stepOut = integrator.step(_curIntMethod_AD, x, t, dt, f_deriv); + } + + for (int i = 0; i < result.stepOut.x_next.size(); ++i) { + const auto v = mathlib::real(result.stepOut.x_next[i]); + if (std::isnan(v) || std::isinf(v)) { LOG_ERROR("Non-finite x_next[%d] = %f", i, (double)v); } // TODO add Scalar isnan and isinf checks to mathlib and use those instead (need to handle both float and double cases) + } + result.dynamics = dynResult; + return result; + } + + template + void RobotSystem::postStepUpdate(const RobotStepResult_T& result, const mathlib::VecX& x) { + const size_t n = result.snap.model->joints.size(); + + Eigen::Map q_next(x.data(), n); + Eigen::Map qd_next(x.data() + n, n); + + // Enforce joint limits + for (auto& j : _robot.joints) { enforceJointLimits(j); } + + // Recompute kinematics and dynamics at the new state for logging and control purposes + std::vector T_world(result.snap.model->links.size()); + _kinematics->computeForwardKinematics_fromState(*result.snap.model, x, T_world); + std::vector jointWorldPoses = _kinematics->calcJointWorldPoses(T_world, *result.snap.model); + + DynamicsScratch dynScratch; + + // Compute spatial kinematics and bias terms for the new state + SpatialDynamics::computeSpatialKinematicsAndBias( + _spatialModel, + q_next, qd_next, + dynScratch.spatial.Xup, + dynScratch.spatial.v, dynScratch.spatial.c + ); + // Compute mass matrix at the new state + mathlib::MatX M = SpatialDynamics::CRBA(_spatialModel, dynScratch.spatial.Xup, dynScratch); + mathlib::VecX tau_g = _dynamics->computeGravityTorque(*result.snap.model, T_world, jointWorldPoses); + + // Extract real parts of relevant variables for logging and control + mathlib::VecX q_real = result.snap.q.unaryExpr([](const auto& v) { return mathlib::real(v); }); + mathlib::VecX qd_real = result.snap.qd.unaryExpr([](const auto& v) { return mathlib::real(v); }); + mathlib::VecX q_ref_real = result.snap.q_ref.unaryExpr([](const auto& v) { return mathlib::real(v); }); + mathlib::VecX qd_ref_real = result.snap.qd_ref.unaryExpr([](const auto& v) { return mathlib::real(v); }); + mathlib::VecX tau_rnea_real = result.tau_rnea.unaryExpr([](const auto& v) { return mathlib::real(v); }); + + // Compute system kinetic energy: E_kin = 0.5 * qd^T * M(q) * qd + double sys_KE = 0.5 * qd_real.transpose() * M * qd_real; // [J], kinetic energy of the robot at configuration q and velocity qd + + // Compute system potential energy at configuration q (relative to gravity) + double sys_PE = 0.0; + double g = _dynamics->getGravity(); + + for (size_t k = 0; k < _robot.links.size(); ++k) { + const RobotLink& link = _robot.links[k]; + const double m = link.inertial.mass; + if (m <= 0.0) { continue; } + Vec3 com_world = (T_world[k].block<3, 3>(0, 0) * link.inertial.com_xyz) + T_world[k].block<3, 1>(0, 3); + sys_PE += m * g * com_world.z(); + } + + const double sys_E = sys_KE + sys_PE; // total mechanical energy of the system + + // Log metrics to buffer if logging is enabled + robots::JointLogBuffer* buf = nullptr; + if (_useInternalLogging) { int idx = _activeLogBufIdx.load(std::memory_order_acquire); buf = &_logBuffers[idx]; } + else { buf = _logBuffer; } + + if (buf) { + auto dynResult = result.dynamics; + for (size_t i = 0; i < n; ++i) { + const RobotJoint& j = _robot.joints[i]; + + const double I_eff = (j.type == eJointType::FIXED) ? 1.0 : dynResult.metrics.I_eff[i]; + const double err = q_ref_real[i] - q_real[i]; + const double err_d = qd_ref_real[i] - qd_real[i]; + + JointLogBuffer::JointLogEntry e{}; + + e.sim_time = _simTime; + e.dt_taken = mathlib::real(result.stepOut.dt_taken); + e.dt_sug = mathlib::real(result.stepOut.dt_sug); + e.theta = q_real[i]; e.omega = qd_real[i]; e.alpha = mathlib::real(dynResult.metrics.qdd[i]); + e.err = err; e.err_d = err_d; + e.I_eff = I_eff; + e.tau = mathlib::real(dynResult.metrics.tau[i]); e.tau_ff = tau_rnea_real[i]; e.tau_gravity = tau_g[i]; + e.tau_sat = mathlib::real(dynResult.metrics.tau_sat[i]); + e.KE = sys_KE; e.PE = sys_PE; e.E_total = sys_E; + e.clamp_theta = mathlib::real(_clampTheta[i]); e.clamp_omega = mathlib::real(_clampOmega[i]); + e.sat_flag = mathlib::real(dynResult.metrics.sat_flag[i]); e.joint_index = (int)i; + buf->push_entry(e); + } + } + } + + template + void RobotSystem::step_AD(double dt, double simTime) { + if (!hasRobot()) { return; } + using Dual = mathlib::DualNumber_T; + _simTime = simTime; + const size_t n = _robot.joints.size(); + mathlib::VecX_T x = packState().template cast(); + + assert((size_t)x.size() <= NVar && "State size exceeds the number of dual variables."); // Checks state vector size is within the dual variable limit + for (size_t i = 0; i < (size_t)x.size(); ++i) { x[i].dual[i] = 1.0; } + + auto result = step_impl(x, Dual(dt), Dual(simTime), *_AD_integrator); + mathlib::VecX x_real = result.stepOut.x_next.unaryExpr([](const auto& v) { return mathlib::real(v); }); + unpackState(x_real); + _dynamics->setDt(result.stepOut.dt_taken); + + postStepUpdate(result, x_real); + + // Update base pose if free-floating + if (_baseIsFree) { + integrateBaseTranslation(dt); + updateBaseRootPose(); + } + // Update kinematics + computeRobotKinematics(_worldTransforms); + } +} \ No newline at end of file diff --git a/DSFE_App/DSFE_Core/include/Robots/SpatialModel.h b/DSFE_App/DSFE_Core/include/Robots/SpatialModel.h index 210fe9af..e9d9a7ee 100644 --- a/DSFE_App/DSFE_Core/include/Robots/SpatialModel.h +++ b/DSFE_App/DSFE_Core/include/Robots/SpatialModel.h @@ -27,5 +27,9 @@ namespace robots { struct SpatialModel { std::vector> joints; std::unordered_map linkNameToIndex; + + template + SpatialModel cast() const; }; -} \ No newline at end of file +} // namespace robots +#include "SpatialModelCast.inl" \ No newline at end of file diff --git a/DSFE_App/DSFE_Core/include/Robots/SpatialModelCast.inl b/DSFE_App/DSFE_Core/include/Robots/SpatialModelCast.inl new file mode 100644 index 00000000..8ceb6c06 --- /dev/null +++ b/DSFE_App/DSFE_Core/include/Robots/SpatialModelCast.inl @@ -0,0 +1,22 @@ +// DSFE_Core SpatialModelCast.inl +#pragma once + +namespace robots { + template + template + SpatialModel SpatialModel::cast() const { + SpatialModel out; + out.joints.resize(joints.size()); + for (size_t i = 0; i < joints.size(); ++i) { + const auto& j = joints[i]; + auto& out_j = out.joints[i]; + out_j.parent = j.parent; + out_j.type = j.type; + out_j.Xtree = j.Xtree.template cast(); + out_j.inertia = j.inertia.template cast(); + out_j.S = j.S.template cast(); + out_j.name = j.name; + } + return out; + } +} \ No newline at end of file diff --git a/DSFE_App/DSFE_Core/include/Scene/SimulationCore.h b/DSFE_App/DSFE_Core/include/Scene/SimulationCore.h index dce60c5e..742e1c5d 100644 --- a/DSFE_App/DSFE_Core/include/Scene/SimulationCore.h +++ b/DSFE_App/DSFE_Core/include/Scene/SimulationCore.h @@ -22,16 +22,16 @@ #include "Platform/Logger.h" // Forward Declarations -namespace integration { enum class eIntegrationMethod; } namespace control { class TrajectoryManager; } namespace robots { class RobotSystem; } namespace interpreter { class IStoredProgram; } namespace core { // configurable defaults (not part of class to allow tuning without recompilation) - constexpr double DEFAULT_INTERACTIVE_MINUTES = 60.0; // long runs for interactive mode - constexpr double DEFAULT_SYNC_MINUTES = 10.0; // short runs for synchronous mode - constexpr size_t MAX_LOG_ENTRIES = 50'000'000; // hard cap to avoid OutOfMemory crashes + inline constexpr double DEFAULT_INTERACTIVE_MINUTES = 60.0; // long runs for interactive mode + inline constexpr double DEFAULT_SYNC_MINUTES = 10.0; // short runs for synchronous mode + inline constexpr size_t MAX_LOG_ENTRIES = 50'000'000; // hard cap to avoid OutOfMemory crashes + inline constexpr size_t DSFE_AD_VARS = 64; // number of independent variables for autodiff (used for pre-allocating AD integrator buffers) class DSFE_API SimulationCore : public ISimulationCore { public: @@ -50,6 +50,8 @@ namespace core { void startSimulation() override; void stopSimulation() override; bool isSimRunning() const override { return _simRunning.load(); } + void setSimulationBackend(eSimulationBackend backend) override; + eSimulationBackend simulationBackend() const override; // Time stepping void setFixedDt(double dt) override; @@ -69,8 +71,10 @@ namespace core { // Integrator void setupSimulationIntegrator(); void setIntegrationMethod(integration::eIntegrationMethod method) override; + void setIntegrationMethod(integration::eAutoDiffIntegrationMethod method) override; std::string integrationMethodName() const override; integration::eIntegrationMethod integrationMethod() const override; + integration::eAutoDiffIntegrationMethod autoDiffIntegrationMethod() const override; void setRunTag(const std::string& tag) override { _runTag = tag; } // Subsystems access @@ -165,6 +169,7 @@ namespace core { // Run mode eRunMode _runMode = eRunMode::Interactive; + eSimulationBackend _simBackend = eSimulationBackend::Standard; // Last script text for comparison re-use std::string _lastScriptText; diff --git a/DSFE_App/DSFE_Core/src/Robots/RobotSystem.cpp b/DSFE_App/DSFE_Core/src/Robots/RobotSystem.cpp index 607ac819..6017e81e 100644 --- a/DSFE_App/DSFE_Core/src/Robots/RobotSystem.cpp +++ b/DSFE_App/DSFE_Core/src/Robots/RobotSystem.cpp @@ -2,14 +2,8 @@ #include "pch.h" #include "Robots/RobotSystem.h" - -#include -#include "Robots/RobotKinematics.h" -#include "Robots/RobotDynamics.h" #include "Robots/RobotLoader.h" -#include - #include #include #include @@ -254,134 +248,6 @@ namespace robots { if (j.q > hi) { j.q = hi; if (j.qd > 0.0f) { j.qd = 0.0f; }} } - // Method to take a snapshot of the current robot state - template - RobotSimSnapshot RobotSystem::takeSnapshot(Scalar simTime) const { - RobotSimSnapshot snap; - snap.model = &_constModel; - const size_t n = (size_t)_robot.joints.size(); - - snap.q.resize(n); - snap.qd.resize(n); - - snap.q_ref.resize(n); - snap.qd_ref.resize(n); - snap.qdd_ref.resize(n); - - for (size_t i = 0; i < n; ++i) { - const auto& j = _robot.joints[i]; - - snap.q[i] = j.q; - snap.qd[i] = j.qd; - - snap.q_ref[i] = j.q_ref; - snap.qd_ref[i] = j.qd_ref; - snap.qdd_ref[i] = j.qdd_ref; - } - - snap.robotRootPose = _robotRootPose; - snap.baseIsFree = _baseIsFree; - snap.lastBaseForwardForce = _lastBaseForwardForce; - snap.gravity = _gravity; - - snap.torqueMode = _robot.torqueMode; - - snap.dt = _dynamics->dt(); - snap.simTime = simTime; - - return snap; - } - - template - RobotStepResult_T RobotSystem::step_impl(const mathlib::VecX_T& x, Scalar dt, Scalar t, IntegratorT& integrator) { - RobotStepResult_T result; - result.snap = takeSnapshot(t); - auto& snap = result.snap; - const size_t n = snap.model->joints.size(); - - Eigen::Map> q(x.data(), n); - Eigen::Map> qd(x.data() + n, n); - - mathlib::VecX_T qdd(n); - for (size_t i = 0; i < n; ++i) { qdd[i] = _robot.joints[i].qdd_ref; } - - std::vector> T_start(snap.model->links.size()); - _kinematics->computeForwardKinematics_fromState(*snap.model, x, T_start); - std::vector> jointWorldPoses_start = _kinematics->calcJointWorldPoses(T_start, *snap.model); - - SpatialDynamics::computeSpatialKinematicsAndBias( - _spatialModel, - q, qd, - _dynScratch.spatial.Xup, - _dynScratch.spatial.v, _dynScratch.spatial.c - ); - - // CRBA only for controller inertia scaling - mathlib::MatX_T M_start = SpatialDynamics::CRBA( - _spatialModel, - _dynScratch.spatial.Xup, - _dynScratch - ); - - // Cache frozen joint gains for this step - mathlib::VecX_T kp_frozen(n), kd_frozen(n); - for (size_t i = 0; i < n; ++i) { - const auto& joint = snap.model->joints[i]; - if (joint.type == eJointType::FIXED) { continue; } - - _dynScratch.dense.I_eff_controller[i] = mathlib::max(M_start(i, i), Scalar(1e-6)); - const Scalar I_eff = _dynScratch.dense.I_eff_controller[i]; - - kp_frozen[i] = I_eff * joint.wn_target * joint.wn_target; - kd_frozen[i] = Scalar(2) * joint.zeta_target * I_eff * joint.wn_target; - } - - // Compute RNEA torques for feedforward control - mathlib::VecX_T tau_rnea = SpatialDynamics::RNEA( - _spatialModel, - q, qd, qdd, - _dynScratch - ); // [Nm] - result.tau_rnea = tau_rnea; - LOG_INFO_ONCE("tau_rnea size = %d", (double)tau_rnea.size()); - - // Define the derivative function for integration, capturing necessary variables by reference - auto f_deriv = [&, kp_frozen, kd_frozen](auto t, const auto& xIn) { - return _dynamics->derivative_spatial( - _spatialModel, - t, xIn, - snap, - _dynScratch, _dynResult - ); - }; - // Define the Jacobian function for integration, capturing necessary variables by reference - auto f_J = [&, kp_frozen, kd_frozen](const mathlib::VecX_T& xIn, mathlib::MatX_T& J_out) { - _dynamics->jacobian_spatial( - _spatialModel, - xIn, snap, - kp_frozen, kd_frozen, - J_out, _dynScratch - ); - }; - - if constexpr (std::is_same_v, integration::IntegrationService>) { - mathlib::VecX x_real = x.template cast(); - auto step = integrator.step(_curIntMethod, x_real, static_cast(t), static_cast(dt), f_deriv, f_J); - result.stepOut.x_next = step.x_next.template cast(); - result.stepOut.dt_taken = step.dt_taken; - result.stepOut.dt_sug = step.dt_sug; - } - else if constexpr (std::is_same_v, integration::DifferentiableIntegrator>) { - result.stepOut = integrator.step(_curIntMethod_AD, x, t, dt, f_deriv); - } - - for (int i = 0; i < result.stepOut.x_next.size(); ++i) { - const auto v = mathlib::real(result.stepOut.x_next[i]); - if (std::isnan(v) || std::isinf(v)) { LOG_ERROR("Non-finite x_next[%d] = %f", i, (double)v); } - } - return result; - } - // Method to advance the robot state by dt using the selected integrator void RobotSystem::step(double dt, double simTime) { if (!_hasRobot) { return; } @@ -393,86 +259,7 @@ namespace robots { unpackState(result.stepOut.x_next); _dynamics->setDt(result.stepOut.dt_taken); - Eigen::Map q_next(result.stepOut.x_next.data(), n); - Eigen::Map qd_next(result.stepOut.x_next.data() + n, n); - - // Enforce joint limits - for (auto& j : _robot.joints) { enforceJointLimits(j); } - - // Recompute kinematics and dynamics at the new state for logging and control purposes - std::vector T_world(result.snap.model->links.size()); - _kinematics->computeForwardKinematics_fromState(*result.snap.model, result.stepOut.x_next, T_world); - std::vector jointWorldPoses = _kinematics->calcJointWorldPoses(T_world, *result.snap.model); - - // Compute spatial kinematics and bias terms for the new state - SpatialDynamics::computeSpatialKinematicsAndBias( - _spatialModel, - q_next, qd_next, - _dynScratch.spatial.Xup, - _dynScratch.spatial.v, _dynScratch.spatial.c - ); - // Compute mass matrix at the new state - mathlib::MatX M = SpatialDynamics::CRBA(_spatialModel, _dynScratch.spatial.Xup, _dynScratch); - mathlib::VecX tau_g = _dynamics->computeGravityTorque(*result.snap.model, T_world, jointWorldPoses); - - // Compute system kinetic energy: E_kin = 0.5 * qd^T * M(q) * qd - double sys_KE = 0.5 * result.snap.qd.transpose() * M * result.snap.qd; // [J], kinetic energy of the robot at configuration q and velocity qd - - // Compute system potential energy at configuration q (relative to gravity) - double sys_PE = 0.0; - double g = _dynamics->getGravity(); - - for (size_t k = 0; k < _robot.links.size(); ++k) { - const RobotLink& link = _robot.links[k]; - const double m = link.inertial.mass; - if (m <= 0.0) { continue; } - Vec3 com_world = (T_world[k].block<3, 3>(0, 0) * link.inertial.com_xyz) + T_world[k].block<3, 1>(0, 3); - sys_PE += m * g * com_world.z(); - } - - const double sys_E = sys_KE + sys_PE; // total mechanical energy of the system - - // Log metrics to buffer if logging is enabled - robots::JointLogBuffer* buf = nullptr; - - // If using internal logging, get the active buffer - if (_useInternalLogging) { - int idx = _activeLogBufIdx.load(std::memory_order_acquire); - buf = &_logBuffers[idx]; - } else { - buf = _logBuffer; - } - - if (buf) { - auto q_real = result.snap.q.template cast(); - auto qd_real = result.snap.qd.template cast(); - auto q_ref_real = result.snap.q_ref.template cast(); - auto qd_ref_real = result.snap.qd_ref.template cast(); - auto tau_rnea_real = result.tau_rnea.template cast(); - - for (size_t i = 0; i < n; ++i) { - const RobotJoint& j = _robot.joints[i]; - - const double I_eff = (j.type == eJointType::FIXED) ? 1.0 : _dynResult.metrics.I_eff[i]; - const double err = result.snap.q_ref[i] - result.snap.q[i]; - const double err_d = result.snap.qd_ref[i] - result.snap.qd[i]; - - JointLogBuffer::JointLogEntry e{}; - - e.sim_time = simTime; - e.dt_taken = result.stepOut.dt_taken; - e.dt_sug = result.stepOut.dt_sug; - e.theta = q_real[i]; e.omega = qd_real[i]; e.alpha = _dynResult.metrics.qdd[i]; - e.err = err; e.err_d = err_d; - e.I_eff = I_eff; - e.tau = _dynResult.metrics.tau[i]; e.tau_ff = tau_rnea_real[i]; e.tau_gravity = tau_g[i]; - e.tau_sat = _dynResult.metrics.tau_sat[i]; - e.KE = sys_KE; e.PE = sys_PE; e.E_total = sys_E; - e.clamp_theta = static_cast(_clampTheta[i]); e.clamp_omega = static_cast(_clampOmega[i]); - e.sat_flag = _dynResult.metrics.sat_flag[i]; e.joint_index = (int)i; - buf->push_entry(e); - } - } + postStepUpdate(result, result.stepOut.x_next); // Update base pose if free-floating if (_baseIsFree) { @@ -484,111 +271,6 @@ namespace robots { computeRobotKinematics(_worldTransforms); } - template - void RobotSystem::step_AD(double dt, double simTime) { - if (!hasRobot()) { return; } - using Dual = mathlib::DualNumber_T; - _simTime = simTime; - const size_t n = _robot.joints.size(); - mathlib::VecX_T x = packState().template cast(); - - for (size_t i = 0; i < x.size(); ++i) { x[i].dual[i] = 1.0; } - - auto result = step_impl(x, Dual(dt), Dual(simTime), *_AD_integrator); - mathlib::VecX x_real = result.stepOut.x_next.unaryExpr([](const auto& v) { return mathlib::real(v); }); - - unpackState(x_real); - _dynamics->setDt(result.stepOut.dt_taken); - - Eigen::Map q_next(x_real.data(), n); - Eigen::Map qd_next(x_real.data() + n, n); - - // Enforce joint limits - for (auto& j : _robot.joints) { enforceJointLimits(j); } - - // Recompute kinematics and dynamics at the new state for logging and control purposes - std::vector T_world(result.snap.model->links.size()); - _kinematics->computeForwardKinematics_fromState(*result.snap.model, x_real, T_world); - std::vector jointWorldPoses = _kinematics->calcJointWorldPoses(T_world, *result.snap.model); - - // Compute spatial kinematics and bias terms for the new state - SpatialDynamics::computeSpatialKinematicsAndBias( - _spatialModel, - q_next, qd_next, - _dynScratch.spatial.Xup, - _dynScratch.spatial.v, _dynScratch.spatial.c - ); - // Compute mass matrix at the new state - mathlib::MatX M = SpatialDynamics::CRBA(_spatialModel, _dynScratch.spatial.Xup, _dynScratch); - mathlib::VecX tau_g = _dynamics->computeGravityTorque(*result.snap.model, T_world, jointWorldPoses); - - // Compute system kinetic energy: E_kin = 0.5 * qd^T * M(q) * qd - double sys_KE = 0.5 * result.snap.qd.transpose() * M * result.snap.qd; // [J], kinetic energy of the robot at configuration q and velocity qd - - // Compute system potential energy at configuration q (relative to gravity) - double sys_PE = 0.0; - double g = _dynamics->getGravity(); - - for (size_t k = 0; k < _robot.links.size(); ++k) { - const RobotLink& link = _robot.links[k]; - const double m = link.inertial.mass; - if (m <= 0.0) { continue; } - Vec3 com_world = (T_world[k].block<3, 3>(0, 0) * link.inertial.com_xyz) + T_world[k].block<3, 1>(0, 3); - sys_PE += m * g * com_world.z(); - } - - const double sys_E = sys_KE + sys_PE; // total mechanical energy of the system - - // Log metrics to buffer if logging is enabled - robots::JointLogBuffer* buf = nullptr; - - // If using internal logging, get the active buffer - if (_useInternalLogging) { - int idx = _activeLogBufIdx.load(std::memory_order_acquire); - buf = &_logBuffers[idx]; - } - else { - buf = _logBuffer; - } - - if (buf) { - auto q_real = result.snap.q.template cast(); - auto qd_real = result.snap.qd.template cast(); - auto q_ref_real = result.snap.q_ref.template cast(); - auto qd_ref_real = result.snap.qd_ref.template cast(); - auto tau_rnea_real = result.tau_rnea.template cast(); - - for (size_t i = 0; i < n; ++i) { - const RobotJoint& j = _robot.joints[i]; - - const double I_eff = (j.type == eJointType::FIXED) ? 1.0 : _dynResult.metrics.I_eff[i]; - const double err = q_ref_real[i] - q_real[i]; - const double err_d = qd_ref_real[i] - qd_real[i] ; - - JointLogBuffer::JointLogEntry e{}; - - e.sim_time = simTime; - e.dt_taken = result.stepOut.dt_taken; - e.dt_sug = result.stepOut.dt_sug; - e.theta = q_real[i]; e.omega = qd_real[i]; e.alpha = _dynResult.metrics.qdd[i]; - e.err = err; e.err_d = err_d; - e.I_eff = I_eff; - e.tau = _dynResult.metrics.tau[i]; e.tau_ff = tau_rnea_real[i]; e.tau_gravity = tau_g[i]; - e.tau_sat = _dynResult.metrics.tau_sat[i]; - e.KE = sys_KE; e.PE = sys_PE; e.E_total = sys_E; - e.clamp_theta = static_cast(_clampTheta[i]); e.clamp_omega = static_cast(_clampOmega[i]); - e.sat_flag = _dynResult.metrics.sat_flag[i]; e.joint_index = (int)i; - buf->push_entry(e); - } - } - - // Update base pose if free-floating - if (_baseIsFree) { - integrateBaseTranslation(dt); - updateBaseRootPose(); - } - } - // Method to step the reference trajectory and update joint reference states void RobotSystem::updateTrajectoryInputs(control::TrajectoryManager& traj, double t) { if (!_hasRobot) { return; } @@ -748,6 +430,9 @@ namespace robots { integration::IntegrationService* RobotSystem::getIntegrator() { return _integrator.get(); } const integration::IntegrationService* RobotSystem::getIntegrator() const { return _integrator.get(); } + integration::DifferentiableIntegrator* RobotSystem::getADIntegrator() { return _AD_integrator.get(); } + const integration::DifferentiableIntegrator* RobotSystem::getADIntegrator() const { return _AD_integrator.get(); } + // --- ROBOT KINEMATICS AND JOINT STATE METHODS --- std::string RobotSystem::findRootLink() const { diff --git a/DSFE_App/DSFE_Core/src/Scene/SimulationCore.cpp b/DSFE_App/DSFE_Core/src/Scene/SimulationCore.cpp index 91c7817a..9c01e59a 100644 --- a/DSFE_App/DSFE_Core/src/Scene/SimulationCore.cpp +++ b/DSFE_App/DSFE_Core/src/Scene/SimulationCore.cpp @@ -47,6 +47,11 @@ namespace core { if (!_robot) { return; } _robot->getIntegrator()->setIntegrationMethod(method); } + // Set the auto-diff integration method for the simulation (also updates the robot's AD integrator if it exists) + void SimulationCore::setIntegrationMethod(integration::eAutoDiffIntegrationMethod method) { + if (!_robot) { return; } + _robot->getADIntegrator()->setIntegrationMethod(method); + } // Get the name of the current integration method (returns "no_robot" if no robot is loaded) std::string SimulationCore::integrationMethodName() const { if (!_robot) { return "no_robot"; } @@ -57,6 +62,14 @@ namespace core { if (!_robot) { return integration::eIntegrationMethod::RK4; } return _robot->getIntegrator()->getIntegrationMethod(); } + // Get the current auto-diff integration method + integration::eAutoDiffIntegrationMethod SimulationCore::autoDiffIntegrationMethod() const { + if (!_robot) { return integration::eAutoDiffIntegrationMethod::AD_ImplicitEuler; } + return _robot->getADIntegrator()->integrationMethod(); + } + + void SimulationCore::setSimulationBackend(eSimulationBackend backend) { _simBackend = backend; } + eSimulationBackend SimulationCore::simulationBackend() const { return _simBackend; } // Fixed timestep loop for physics and robot updates, called from the main render loop with the frame delta time void SimulationCore::stepFixed(double frame_dt) { @@ -98,7 +111,7 @@ namespace core { // Update robot trajectory inputs and step the robot forward in time if (hasRobot()) { _robot->updateTrajectoryInputs(*_traj, simTime); - _robot->step(_dt, simTime); + _robot->step_AD(_dt, simTime); // Telemetry update if (!_telemetryBegun) { _telemetry.beginRun(simTime, _telHz, 300.0); diff --git a/PxMLib/MathLib/include/core/SpatialMath.h b/PxMLib/MathLib/include/core/SpatialMath.h index d71f60dc..9fe608ec 100644 --- a/PxMLib/MathLib/include/core/SpatialMath.h +++ b/PxMLib/MathLib/include/core/SpatialMath.h @@ -12,7 +12,6 @@ namespace mathlib { // Template version of spatial vector template struct SpatialVec_T { - using ScalarT = Scalar; using Vec3S = Vec3_T; using Vec6S = Vec6_T; @@ -25,7 +24,6 @@ namespace mathlib { Vec6S v; SpatialVec_T() { v.setZero(); } - SpatialVec_T(const Vec3S& angular, const Vec3S& linear) { v.template segment<3>(0) = angular; v.template segment<3>(3) = linear; @@ -68,6 +66,9 @@ namespace mathlib { Scalar dot(const SpatialVec_T& sv) const { return this->v.dot(sv.v); } + + template + SpatialVec_T cast() const; }; using SpatialVec = SpatialVec_T; @@ -212,4 +213,5 @@ namespace mathlib { out.v = forceCrossMatrix(lhs) * rhs.v; return out; } -} // namespace mathlib \ No newline at end of file +} // namespace mathlib +#include "SpatialVec.inl" \ No newline at end of file diff --git a/PxMLib/MathLib/include/core/SpatialVec.inl b/PxMLib/MathLib/include/core/SpatialVec.inl new file mode 100644 index 00000000..aae1218f --- /dev/null +++ b/PxMLib/MathLib/include/core/SpatialVec.inl @@ -0,0 +1,12 @@ +// PxM/MathLib SpatialVec.inl +#pragma once + +namespace mathlib { + template + template + SpatialVec_T SpatialVec_T::cast() const { + SpatialVec_T out; + out.v = this->v.template cast(); + return out; + } +} // namespace mathlib \ No newline at end of file From c39044d7fc8c1740890859680dd90627a3ca8f33 Mon Sep 17 00:00:00 2001 From: saltyjoss Date: Sun, 24 May 2026 11:12:51 +0100 Subject: [PATCH 070/210] refactor(WIP): Updated scalar-generic math layer and AD-compatible robotics pipeline * Split templated Eigen aliases into `Types_tpl.h` and concrete aliases into Types.h * Added centralised scalar-generic math dispatch layer (sin/cos/tanh/exp/sqrt/log/etc.) * Added `ScalarStdFunc.h` for scalar and Dual-compatible transcendental/math operations * Added `ScalarScaling.h` for scalar utility operations and Eigen-safe normalization helpers * Added `ScalarTransforms.h` with scalar-generic Rodrigues `AngleAxis` rotation construction * Removed Eigen::AngleAxis dependency from scalar-generic robotics execution paths * Added `safeNorm()` and `safeNormalised()` helpers for AD-safe vector normalisation * Refactored robotics pipeline to consistently use mathlib scalar dispatch functions * Improved DualNumber scalar interoperability with Eigen expressions and robotics math * Added scalar-generic SpatialModel casting support for AD execution * Refactored templated robot stepping implementation into .inl structure * Added scalar-generic step_impl() execution kernel and step_AD() integration path * Separated runtime telemetry recomputation from integration execution state * Added runtime scalar-collapse boundary using mathlib::real() for logging/export systems * Improved scalar consistency across spatial dynamics, kinematics, and integration code * Reduced hidden Eigen geometry/scalar promotion dependencies in differentiable paths * Improved architecture for future symbolic, interval, and higher-order scalar support TODO: * Convert other MathLib-specific headers to template. --- .../include/Numerics/IntegrationService.h | 4 +- .../DSFE_Core/include/Robots/RobotDynamics.h | 5 +- .../include/Robots/RobotDynamics.inl | 12 +- .../include/Robots/RobotKinematics.h | 3 +- .../include/Robots/RobotKinematics.inl | 4 +- .../DSFE_Core/include/Robots/RobotModel.h | 3 +- .../include/Robots/SpatialDynamics.h | 1 - .../include/Robots/SpatialDynamics.inl | 6 +- .../DSFE_Core/include/Robots/SpatialModel.h | 2 - .../DSFE_Core/include/Scene/SimulationCore.h | 3 - DSFE_App/DSFE_Core/src/Robots/RobotLoader.cpp | 7 +- PxMLib/MathLib/include/core/DualNumbers.h | 57 +---- PxMLib/MathLib/include/core/MathLib.h | 10 + PxMLib/MathLib/include/core/ScalarScaling.h | 39 ++++ PxMLib/MathLib/include/core/ScalarStdFunc.h | 57 +++++ .../MathLib/include/core/ScalarTransforms.h | 15 ++ PxMLib/MathLib/include/core/SpatialMath.h | 12 +- PxMLib/MathLib/include/core/Types.h | 40 ++-- PxMLib/MathLib/include/core/Types_tpl.h | 17 +- PxMLib/MathLib/include/core/Utils.h | 194 ++++++++++-------- 20 files changed, 285 insertions(+), 206 deletions(-) create mode 100644 PxMLib/MathLib/include/core/MathLib.h create mode 100644 PxMLib/MathLib/include/core/ScalarScaling.h create mode 100644 PxMLib/MathLib/include/core/ScalarStdFunc.h create mode 100644 PxMLib/MathLib/include/core/ScalarTransforms.h diff --git a/DSFE_App/DSFE_Core/include/Numerics/IntegrationService.h b/DSFE_App/DSFE_Core/include/Numerics/IntegrationService.h index 68de60d9..9a1f7dd3 100644 --- a/DSFE_App/DSFE_Core/include/Numerics/IntegrationService.h +++ b/DSFE_App/DSFE_Core/include/Numerics/IntegrationService.h @@ -4,9 +4,7 @@ #pragma warning(disable : 4251) #include "EngineCore.h" -#include -#include -#include +#include #include #include "Numerics/IntegrationMethods.h" diff --git a/DSFE_App/DSFE_Core/include/Robots/RobotDynamics.h b/DSFE_App/DSFE_Core/include/Robots/RobotDynamics.h index 90235787..aee14433 100644 --- a/DSFE_App/DSFE_Core/include/Robots/RobotDynamics.h +++ b/DSFE_App/DSFE_Core/include/Robots/RobotDynamics.h @@ -2,9 +2,9 @@ #pragma once #include "EngineCore.h" -#include "MathLibAPI.h" -#include "core/Types.h" +#include #include + #include "Robots/DynamicsTypes.h" #include "Robots/RobotMetrics.h" @@ -16,7 +16,6 @@ #include "Robots/TrajectoryManager.h" #include -#include #include #include "EngineLib/LogMacros.h" diff --git a/DSFE_App/DSFE_Core/include/Robots/RobotDynamics.inl b/DSFE_App/DSFE_Core/include/Robots/RobotDynamics.inl index 2d290650..30fd6f44 100644 --- a/DSFE_App/DSFE_Core/include/Robots/RobotDynamics.inl +++ b/DSFE_App/DSFE_Core/include/Robots/RobotDynamics.inl @@ -270,7 +270,7 @@ namespace robots { dTau_dq(i, i) = -k_p; Scalar qd_i = qd_local[i]; - Scalar tanh_term = std::tanh(qd_i / eps_f); + Scalar tanh_term = mathlib::tanh(qd_i / eps_f); Scalar stiff_friction_slope = c * (1.0 - tanh_term * tanh_term) / eps_f; dTau_dv(i, i) = -k_d - b + stiff_friction_slope; } @@ -343,7 +343,7 @@ namespace robots { tau_i += scratch.g[i]; // Gravity compensation tau_i += scratch.dense.h[i]; // add Coriolis and centrifugal bias tau_i -= b * qd[i]; // subtract viscous damping - tau_i -= c * std::tanh(qd[i] / eps_f); // subtract Coulomb friction + tau_i -= c * mathlib::tanh(qd[i] / eps_f); // subtract Coulomb friction scratch.dense.tau[i] = tau_i; @@ -429,7 +429,7 @@ namespace robots { Scalar tau_i = k_p * err + k_d * err_d + I_eff * snap.qdd_ref[i]; tau_i -= b * qd[i]; - //tau_i -= b * std::tanh(qd[i] / eps_f); + //tau_i -= b * mathlib::tanh(qd[i] / eps_f); scratch.dense.tau[i] = tau_i; @@ -488,7 +488,7 @@ namespace robots { const Scalar c = static_cast(0.05); // Coulomb friction coefficient const Scalar eps_f = static_cast(1e-2); - Scalar tanh_term = std::tanh(qd[i] / eps_f); + Scalar tanh_term = mathlib::tanh(qd[i] / eps_f); Scalar stiff_friction_slope = -c * (Scalar(1.0) - tanh_term * tanh_term) / eps_f; dTau_dv(i, i) = -kd[i] - b; } @@ -541,7 +541,7 @@ namespace robots { Scalar tau_i = kp[i] * (snap.q_ref[i] - q[i]) + kd[i] * (snap.qd_ref[i] - qd[i]) + mathlib::LSE_smoothMax(scratch.dense.M(i, i), eps) * snap.qdd_ref[i]; tau_i += scratch.g[i] + scratch.dense.h[i]; tau_i -= b * qd[i]; - tau_i -= c * std::tanh(qd[i] / eps_f); + tau_i -= c * mathlib::tanh(qd[i] / eps_f); scratch.dense.tau[i] = tau_i; } @@ -590,7 +590,7 @@ namespace robots { const Scalar c = static_cast(0.05); // Coulomb friction coefficient const Scalar eps_f = static_cast(1e-2); - Scalar tanh_term = std::tanh(qd[i] / eps_f); + Scalar tanh_term = mathlib::tanh(qd[i] / eps_f); Scalar stiff_friction = -c * (Scalar(1.0) - tanh_term * tanh_term) / eps_f; dTau_dv(i, i) = -kd[i] - b + stiff_friction; } diff --git a/DSFE_App/DSFE_Core/include/Robots/RobotKinematics.h b/DSFE_App/DSFE_Core/include/Robots/RobotKinematics.h index c6c56d72..12bf2e5f 100644 --- a/DSFE_App/DSFE_Core/include/Robots/RobotKinematics.h +++ b/DSFE_App/DSFE_Core/include/Robots/RobotKinematics.h @@ -5,7 +5,8 @@ #include "MathLibAPI.h" #include #include -#include +#include +#include #include #include "Robots/RobotSimSnapshot.h" diff --git a/DSFE_App/DSFE_Core/include/Robots/RobotKinematics.inl b/DSFE_App/DSFE_Core/include/Robots/RobotKinematics.inl index a621df12..d9c61b7e 100644 --- a/DSFE_App/DSFE_Core/include/Robots/RobotKinematics.inl +++ b/DSFE_App/DSFE_Core/include/Robots/RobotKinematics.inl @@ -35,10 +35,10 @@ namespace robots { // Compute joint motion transform based on joint axis and angle Pose_T T_motion = mathlib::Pose_T::Identity(); if (joint.type == eJointType::REVOLUTE) { - T_motion = jointMotionTransform(joint.axis, q); // rotation about joint axis + T_motion = jointMotionTransform(joint.axis.template cast(), q); // rotation about joint axis } else if (joint.type == eJointType::PRISMATIC) { - T_motion.template block<3, 1>(0, 3) = joint.axis.normalized() * q; // translation along joint axis + T_motion.template block<3, 1>(0, 3) = mathlib::safeNormalised(joint.axis) * q; // translation along joint axis } // compose transforms diff --git a/DSFE_App/DSFE_Core/include/Robots/RobotModel.h b/DSFE_App/DSFE_Core/include/Robots/RobotModel.h index 9ed9e8d8..fa692430 100644 --- a/DSFE_App/DSFE_Core/include/Robots/RobotModel.h +++ b/DSFE_App/DSFE_Core/include/Robots/RobotModel.h @@ -2,8 +2,7 @@ #pragma once #include "EngineCore.h" -#include -#include +#include #include #include diff --git a/DSFE_App/DSFE_Core/include/Robots/SpatialDynamics.h b/DSFE_App/DSFE_Core/include/Robots/SpatialDynamics.h index 5a5c7893..9963d617 100644 --- a/DSFE_App/DSFE_Core/include/Robots/SpatialDynamics.h +++ b/DSFE_App/DSFE_Core/include/Robots/SpatialDynamics.h @@ -4,7 +4,6 @@ #include "EngineCore.h" #include "Robots/SpatialModel.h" #include "Robots/DynamicsTypes.h" -#include namespace robots { class DSFE_API SpatialDynamics { diff --git a/DSFE_App/DSFE_Core/include/Robots/SpatialDynamics.inl b/DSFE_App/DSFE_Core/include/Robots/SpatialDynamics.inl index 4691e1f6..62d36157 100644 --- a/DSFE_App/DSFE_Core/include/Robots/SpatialDynamics.inl +++ b/DSFE_App/DSFE_Core/include/Robots/SpatialDynamics.inl @@ -23,13 +23,15 @@ namespace robots { mathlib::SpatialMat_T XJ = mathlib::SpatialMat_T::Identity(); if (j.type == eJointType::REVOLUTE) { - Eigen::AngleAxis aa(q[i], j.S.angular().normalized()); + mathlib::Vec3_T axis = mathlib::safeNormalised(j.S.angular()); + Eigen::AngleAxis aa(q[i], axis); mathlib::Mat3_T R = aa.toRotationMatrix(); mathlib::Vec3_T r = mathlib::Vec3_T::Zero(); XJ = mathlib::spatialTransform(R, r); } else if (j.type == eJointType::PRISMATIC) { - mathlib::Vec3_T r = q[i] * j.S.linear().normalized(); + mathlib::Vec3_T axis = mathlib::safeNormalised(j.S.linear()); + mathlib::Vec3_T r = q[i] * axis; mathlib::Mat3_T R = mathlib::Mat3_T::Identity(); XJ = mathlib::spatialTransform(R, r); } diff --git a/DSFE_App/DSFE_Core/include/Robots/SpatialModel.h b/DSFE_App/DSFE_Core/include/Robots/SpatialModel.h index e9d9a7ee..90c1a31c 100644 --- a/DSFE_App/DSFE_Core/include/Robots/SpatialModel.h +++ b/DSFE_App/DSFE_Core/include/Robots/SpatialModel.h @@ -1,8 +1,6 @@ // DSFE_Core SpatialModel.h #pragma once -#include "EngineCore.h" - #include #include #include "Robots/RobotModel.h" diff --git a/DSFE_App/DSFE_Core/include/Scene/SimulationCore.h b/DSFE_App/DSFE_Core/include/Scene/SimulationCore.h index 742e1c5d..46bff490 100644 --- a/DSFE_App/DSFE_Core/include/Scene/SimulationCore.h +++ b/DSFE_App/DSFE_Core/include/Scene/SimulationCore.h @@ -12,9 +12,6 @@ #include "Platform/ISimulationCore.h" #include "Platform/SimulationState.h" -#include -#include - #include "Analysis/Telemetry.h" #include "Platform/DataManager.h" diff --git a/DSFE_App/DSFE_Core/src/Robots/RobotLoader.cpp b/DSFE_App/DSFE_Core/src/Robots/RobotLoader.cpp index 7426b063..1d614b1a 100644 --- a/DSFE_App/DSFE_Core/src/Robots/RobotLoader.cpp +++ b/DSFE_App/DSFE_Core/src/Robots/RobotLoader.cpp @@ -2,9 +2,7 @@ #include "pch.h" #include "Robots/RobotLoader.h" - -#include -#include +#include #include "EngineLib/LogMacros.h" #include @@ -19,8 +17,7 @@ namespace robots { // --- Static Helper Functions --- // tf2::Quaternion::setRPY(roll,pitch,yaw) corresponds to q = qz * qy * qx. - static Quat rpyRadToQuat(const Vec3& rpyRad) - { + static Quat rpyRadToQuat(const Vec3& rpyRad) { const double roll = rpyRad.x(); const double pitch = rpyRad.y(); const double yaw = rpyRad.z(); diff --git a/PxMLib/MathLib/include/core/DualNumbers.h b/PxMLib/MathLib/include/core/DualNumbers.h index d20223b7..f1d454c3 100644 --- a/PxMLib/MathLib/include/core/DualNumbers.h +++ b/PxMLib/MathLib/include/core/DualNumbers.h @@ -1,8 +1,7 @@ // PxM/MathLib M_DualNumbers.h #pragma once -#include "MathLibAPI.h" -#include "core/Types_tpl.h" +#include #define EIGEN_DONT_VECTORIZE #define EIGEN_DISABLE_UNALIGNED_ARRAY_ASSERT @@ -178,38 +177,6 @@ namespace mathlib { return out; } - // ----- - // Scalar Interactions - // ----- - - // Power function (Scalar) - template - inline Scalar pow(const Scalar& x, const Scalar& n) { return std::pow(x, n); } - // Sqrt function (Scalar) - template - inline Scalar sqrt(const Scalar& x) { return std::sqrt(x); } - // Sine function (Scalar) - template - inline Scalar sin(const Scalar& x) { return std::sin(x); } - // Cosine function (Scalar) - template - inline Scalar cos(const Scalar& x) { return std::cos(x); } - // Tangent function (Scalar) - template - inline Scalar tan(const Scalar& x) { return std::tan(x); } - // Arctangent function (Scalar) - template - inline Scalar atan(const Scalar& x) { return std::atan(x); } - // Hyperbolic Tangnet (Scalar) - template - inline Scalar tanh(const Scalar& x) { return std::tanh(x); } - // Exponential function (Scalar) - template - inline Scalar exp(const Scalar& x) { return std::exp(x); } - // Logarithm function (Scalar) - template - inline Scalar log(const Scalar& x) { return std::log(x); } - // ----- // Dual Number Interactions // ----- @@ -355,12 +322,6 @@ namespace mathlib { for (size_t i = 0; i < NVar; ++i) { out.dual[i] = derivative * x.dual[i]; } return out; } - // LogSumExp Smooth Max (Scalar) - template - inline Scalar LSE_smoothMax(const Scalar& a, const Scalar& b, const Scalar& k = Scalar(10)) { - Scalar m = (a > b) ? a : b; - return m + log(exp(k * (a - m)) + exp(k * (b - m))) / k; - } // LogSumExp Smooth Max (DualNumber) template inline Scalar LSE_smoothMax(const DualNumber_T& a, const DualNumber_T& b, const Scalar& k = Scalar(15)) { @@ -435,22 +396,6 @@ namespace mathlib { // Numeric Limits Specialisation for DualNumber_T // ----- - // Absolute value function (Scalar) - template - inline Scalar abs(const Scalar& a) { return (a < Scalar(0)) ? -a : a; } - // Maximum value function (Scalar) - template - inline Scalar max(const Scalar& a, const Scalar& b) { return (a > b) ? a : b; } - // Minimum value function (Scalar) - template - inline Scalar min(const Scalar& a, const Scalar& b) { return (a < b) ? a : b; } - // Sign function (Scalar) - template - inline Scalar sgn(const Scalar& a) { return (a > Scalar(0)) - (a < Scalar(0)); } - // Is finite function (Scalar) - template - inline Scalar isfinite(const Scalar& a) { return std::isfinite(a); } - // Function to return a DualNumber_T representing infinity (real part is infinity, dual parts are zero) template inline DualNumber_T numeric_limits_infinity() { diff --git a/PxMLib/MathLib/include/core/MathLib.h b/PxMLib/MathLib/include/core/MathLib.h new file mode 100644 index 00000000..1a15311d --- /dev/null +++ b/PxMLib/MathLib/include/core/MathLib.h @@ -0,0 +1,10 @@ +// PxM/MathLib MathLib.h +#pragma once + +#include "MathLibAPI.h" +#include "core/Types.h" +#include "core/ScalarStdFunc.h" +#include "core/ScalarScaling.h" +#include "core/ScalarTransforms.h" +#include "core/constants.h" +#include "Core/Utils.h" \ No newline at end of file diff --git a/PxMLib/MathLib/include/core/ScalarScaling.h b/PxMLib/MathLib/include/core/ScalarScaling.h new file mode 100644 index 00000000..1cf1861e --- /dev/null +++ b/PxMLib/MathLib/include/core/ScalarScaling.h @@ -0,0 +1,39 @@ +// PxM/MathLib ScalarScaling.h +#pragma once + +#include "MathLibAPI.h" +#include "core/Types_tpl.h" + +namespace mathlib { + // LogSumExp Smooth Max (Scalar) + template + inline Scalar LSE_smoothMax(const Scalar& a, const Scalar& b, const Scalar& k = Scalar(10)) { + Scalar m = (a > b) ? a : b; + return m + log(exp(k * (a - m)) + exp(k * (b - m))) / k; + } + + // Maximum value function (Scalar) + template + inline Scalar max(const Scalar& a, const Scalar& b) { return (a > b) ? a : b; } + // Minimum value function (Scalar) + template + inline Scalar min(const Scalar& a, const Scalar& b) { return (a < b) ? a : b; } + + // Is finite function (Scalar) + template + inline bool isfinite(const Scalar& a) { return std::isfinite(a); } + + template + inline auto safeNorm(const Eigen::MatrixBase& v) { + using Scalar = typename Derived::Scalar; + return mathlib::sqrt(v.dot(v)); + } + template + inline typename Derived::PlainObject safeNormalised(const Eigen::MatrixBase& v) { + using Scalar = typename Derived::Scalar; + using Plain = typename Derived::PlainObject; + Scalar n = safeNorm(v); + if (n > Scalar(0)) { return (v / n).eval(); } + return Plain::Zero(v.rows(), v.cols()); + } +} \ No newline at end of file diff --git a/PxMLib/MathLib/include/core/ScalarStdFunc.h b/PxMLib/MathLib/include/core/ScalarStdFunc.h new file mode 100644 index 00000000..598bfa63 --- /dev/null +++ b/PxMLib/MathLib/include/core/ScalarStdFunc.h @@ -0,0 +1,57 @@ +// PxM/MathLib ScalarStdFunc.h +#pragma once + +#include "MathLibAPI.h" +#include "core/Types_tpl.h" +#include + +namespace mathlib { + // Power function (Scalar) + template + inline Scalar pow(const Scalar& x, const Scalar& n) { return std::pow(x, n); } + // Sqrt function (Scalar) + template + inline Scalar sqrt(const Scalar& x) { return std::sqrt(x); } + // Sine function (Scalar) + template + inline Scalar sin(const Scalar& x) { return std::sin(x); } + // Arcsine function (Scalar) + template + inline Scalar asin(const Scalar& x) { return std::asin(x); } + // Cosine function (Scalar) + template + inline Scalar cos(const Scalar& x) { return std::cos(x); } + // Arccosine function (Scalar) + template + inline Scalar acos(const Scalar& x) { return std::acos(x); } + // Tangent function (Scalar) + template + inline Scalar tan(const Scalar& x) { return std::tan(x); } + // Arctangent function (Scalar) + template + inline Scalar atan(const Scalar& x) { return std::atan(x); } + // Arctangent sqaured function (Scalar) + template + inline Scalar atan2(const Scalar& y, const Scalar& x) { return std::atan2(y, x); } + // Hyperbolic Tangnet (Scalar) + template + inline Scalar tanh(const Scalar& x) { return std::tanh(x); } + // Exponential function (Scalar) + template + inline Scalar exp(const Scalar& x) { return std::exp(x); } + // Logarithm function (Scalar) + template + inline Scalar log(const Scalar& x) { return std::log(x); } + // Absolute value function (Scalar) + template + inline Scalar abs(const Scalar& a) { return (a < Scalar(0)) ? -a : a; } + // Absolute value function squared (Scalar) + template + inline Scalar abs2(const Scalar& a) { return a * a; } + // Absolute value with tolerance (Scalar, overload) + template + inline Scalar abs(const Scalar& a, const Scalar& tol) { return (abs(a) < tol) ? Scalar(0) : a; } + // Signum function (Scalar) + template + inline Scalar sgn(const Scalar& a) { return (a > Scalar(0)) - (a < Scalar(0)); } +} // namespace mathlib \ No newline at end of file diff --git a/PxMLib/MathLib/include/core/ScalarTransforms.h b/PxMLib/MathLib/include/core/ScalarTransforms.h new file mode 100644 index 00000000..9140adfa --- /dev/null +++ b/PxMLib/MathLib/include/core/ScalarTransforms.h @@ -0,0 +1,15 @@ +// PxM/MathLib ScalarTransforms.h +#pragma once + +#include "MathLibAPI.h" +#include "core/Types_tpl.h" +#include "core/SpatialMath.h" + +namespace mathlib { + template + inline Mat3_T AngleAxis(const Scalar& theta, const Vec3_T& axis) { + Vec3_T axis_n = safeNormalised(axis); + Mat3_T K = skewSymmetric(axis_n); + return Mat3_T::Identity() + sin(theta) * K + (Scalar(1) - cos(theta)) * K * K; + } +} \ No newline at end of file diff --git a/PxMLib/MathLib/include/core/SpatialMath.h b/PxMLib/MathLib/include/core/SpatialMath.h index 9fe608ec..cb92231b 100644 --- a/PxMLib/MathLib/include/core/SpatialMath.h +++ b/PxMLib/MathLib/include/core/SpatialMath.h @@ -1,14 +1,9 @@ // PxMLib SpatialMath.h #pragma once -#include "MathLibAPI.h" -#include "core/Types.h" -#include "core/Types_tpl.h" +#include namespace mathlib { - using Vec3 = Vec3_T; - using Mat3 = Mat3_T; - // Template version of spatial vector template struct SpatialVec_T { @@ -46,7 +41,6 @@ namespace mathlib { out.v = this->v - rhs.v; return out; } - SpatialVec_T operator*(Scalar rhs) const { SpatialVec_T out; out.v = this->v * rhs; @@ -63,9 +57,7 @@ namespace mathlib { return *this; } - Scalar dot(const SpatialVec_T& sv) const { - return this->v.dot(sv.v); - } + Scalar dot(const SpatialVec_T& sv) const { return this->v.dot(sv.v); } template SpatialVec_T cast() const; diff --git a/PxMLib/MathLib/include/core/Types.h b/PxMLib/MathLib/include/core/Types.h index f41dda0b..ca52c473 100644 --- a/PxMLib/MathLib/include/core/Types.h +++ b/PxMLib/MathLib/include/core/Types.h @@ -1,7 +1,6 @@ #pragma once -#include "MathLibAPI.h" -#include +#include "Types_tpl.h" namespace mathlib { // Basic type definitions @@ -11,21 +10,26 @@ namespace mathlib { using ulong = unsigned long; using ullong = unsigned long long; - // Eigen type aliases - using Vec2 = Eigen::Vector2d; // A 2D vector, often used to represent positions, directions, or other 2D quantities in space - using Vec3 = Eigen::Vector3d; // A 3D vector, often used to represent positions, directions, or other 3D quantities in space - using Vec4 = Eigen::Vector4d; // A 4D vector, often used to represent homogeneous coordinates (x, y, z, w) in 3D graphics and transformations - using VecX = Eigen::VectorXd; // A dynamic-size vector, where the number of elements can be determined at runtime - using Mat2 = Eigen::Matrix2d; // A 2x2 matrix, often used to represent linear transformations in 2D space - using Mat3 = Eigen::Matrix3d; // A 3x3 matrix, often used to represent rotations in 3D space - using Mat4 = Eigen::Matrix4d; // A 4x4 matrix, often used to represent transformations (rotation + translation) in 3D space - using MatX = Eigen::MatrixXd; // A dynamic-size matrix, where the number of rows and columns can be determined at runtime + // Double and float Eigen types using the template aliases defined in Types_tpl.h + using Vec2 = Vec2_T; + using Vec2f = Vec2_T; + using Vec3 = Vec3_T; + using Vec3f = Vec3_T; + using Vec4 = Vec4_T; + using Vec4f = Vec4_T; + using VecX = VecX_T; + using VecXf = VecX_T; + using Mat2 = Mat2_T; + using Mat2f = Mat2_T; + using Mat3 = Mat3_T; + using Mat3f = Mat3_T; + using Mat4 = Mat4_T; + using Mat4f = Mat4_T; + using MatX = MatX_T; + using MatXf = MatX_T; + using Pose = Pose_T; + using Posef = Pose_T; + using Quat = Quat_T; + using Quatf = Quat_T; - using Vec6 = Eigen::Matrix; // A 6D vector, often used to represent spatial velocities (linear and angular) or twists in robotics and kinematics - using Vec7 = Eigen::Matrix; // A 7D vector, commonly used to represent a pose in 3D space (position and orientation) using a combination of translation (3D) and rotation (4D quaternion) - using Mat6 = Eigen::Matrix; // A 6x6 matrix, often used to represent spatial inertia tensors or Jacobians in robotics and kinematics - - using Pose = Eigen::Matrix4d; // A 4x4 transformation matrix that combines rotation and translation and represents the pose of an object in 3D space - using Quat = Eigen::Quaterniond; // A quaternion representing rotation in 3D space; Eigen uses semantic order (w, x, y, z) with scalar part w and vector part (x, y, z), while coeffs() is stored as (x, y, z, w) - using Quatf = Eigen::Quaternionf; // A single-precision quaternion representing rotation in 3D space; same Eigen convention applies: semantic order (w, x, y, z), coeffs() storage (x, y, z, w) } \ No newline at end of file diff --git a/PxMLib/MathLib/include/core/Types_tpl.h b/PxMLib/MathLib/include/core/Types_tpl.h index 0682044e..60de60dc 100644 --- a/PxMLib/MathLib/include/core/Types_tpl.h +++ b/PxMLib/MathLib/include/core/Types_tpl.h @@ -5,38 +5,39 @@ #include namespace mathlib { + // Template version of 2D vector + template + using Vec2_T = Eigen::Matrix; // Template version of 3D vector template using Vec3_T = Eigen::Matrix; - + // Template version of 4D vector (e.g., for homogeneous coordinates) + template + using Vec4_T = Eigen::Matrix; + // Template version of 3x3 matrix + template + using Mat2_T = Eigen::Matrix; // Template version of 3x3 matrix template using Mat3_T = Eigen::Matrix; - // Template version of 4x4 matrix for homogeneous transformations template using Mat4_T = Eigen::Matrix; - // Template version of 6D vector for spatial algebra template using Vec6_T = Eigen::Matrix; - // Template version of 6x6 matrix for spatial algebra template using Mat6_T = Eigen::Matrix; - // Template version of dynamic-size vector and matrix template using VecX_T = Eigen::Matrix; - // Template version of dynamic-size matrix template using MatX_T = Eigen::Matrix; - // Template version of pose (4x4 homogeneous transformation matrix) template using Pose_T = Eigen::Matrix; - // Template version of quaternion template using Quat_T = Eigen::Quaternion; diff --git a/PxMLib/MathLib/include/core/Utils.h b/PxMLib/MathLib/include/core/Utils.h index e354da64..a9face88 100644 --- a/PxMLib/MathLib/include/core/Utils.h +++ b/PxMLib/MathLib/include/core/Utils.h @@ -8,113 +8,104 @@ using namespace mathlib; using namespace constants; namespace mathlib { - // Converts a non-eigen vector to Vec3 (if it has 3 elements) - template - inline Vec3 toVec3(const T& vec) { - static_assert(std::tuple_size::value == 3, "Input vector must have exactly 3 elements"); - return Vec3(vec[0], vec[1], vec[2]); - } - - // Converts a non-eigen vector to Vec4 (if it has 4 elements) - template - inline Vec4 toVec4(const T& vec) { - static_assert(std::tuple_size::value == 4, "Input vector must have exactly 4 elements"); - return Vec4(vec[0], vec[1], vec[2], vec[3]); - } - - // Converts a non-eigen 3x3 matrix to Mat3 (if it has 3 rows and 3 columns) - template - inline Mat3 toMat3(const T& mat) { - static_assert(std::tuple_size::value == 3 && std::tuple_size::value == 3, "Input matrix must be 3x3"); - return Mat3(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] - ); - } - - // Converts a non-eigen 4x4 matrix to Mat4 (if it has 4 rows and 4 columns) - template - inline Mat4 toMat4(const T& mat) { - static_assert(std::tuple_size::value == 4 && std::tuple_size::value == 4, "Input matrix must be 4x4"); - return Mat4(mat[0][0], mat[0][1], mat[0][2], mat[0][3], - mat[1][0], mat[1][1], mat[1][2], mat[1][3], - mat[2][0], mat[2][1], mat[2][2], mat[2][3], - mat[3][0], mat[3][1], mat[3][2], mat[3][3] - ); - } - - // Convert degrees to radians - inline double deg2rad(double degrees) { return degrees * (PI_d / 180.0); } + // Convert degrees to radians (Scalar) + template + inline Scalar radians(Scalar degrees) { return degrees * (Scalar(PI) / Scalar(180)); } + // Convert degrees to radians (double overload) + inline double radians(double degrees) { return radians(degrees); } // Convert radians to degrees - inline double rad2deg(double radians) { return radians * (180.0 / PI_d); } - - // Portable radian conversion - inline double radians(double degrees) { - return degrees * std::numbers::pi / 180.0; - } - // Portable degree conversion - inline double degrees(double radians) { - return radians * 180.0 / std::numbers::pi; - } + template + inline Scalar degrees(Scalar radians) { return radians * (Scalar(180) / Scalar(PI)); } + // Convert radians to degrees (double overload) + inline double degrees(double radians) { return degrees(radians); } // Clamp a value between min and max - inline double clamp(double value, double minVal, double maxVal) { + template + inline Scalar clamp(Scalar value, Scalar minVal, Scalar maxVal) { if (value < minVal) { return minVal; } if (value > maxVal) { return maxVal; } return value; } + // Clamp a value between min and max (double overload) + inline double clamp(double value, double minVal, double maxVal) { return clamp(value, minVal, maxVal); } // Method to wrap an angle in radians to the range [-pi, pi] - inline double wrapToPi(double angleRad) { - angleRad = std::fmod(angleRad + PI_d, TWO_PI_d); - if (angleRad < 0.0f) { angleRad += TWO_PI_d; } - return angleRad - PI_d; // [rad] + template + inline Scalar wrapToPi(Scalar angleRad) { + angleRad = std::fmod(angleRad + Scalar(PI), Scalar(TWO_PI)); + if (angleRad < Scalar(0)) { angleRad += Scalar(TWO_PI); } + return angleRad - Scalar(PI); // [rad] } + // Method to wrap an angle in radians to the range [-pi, pi] (double overload) + inline double wrapToPi(double angleRad) { return wrapToPi(angleRad); } // Method to wrap an angle in radians to the range [0, 2pi] - inline double wrapRad(double angleRad) { - angleRad = fmod(angleRad, TWO_PI_d); - if (angleRad < 0.0f) { angleRad += TWO_PI_d; } + template + inline Scalar wrapRad(Scalar angleRad) { + angleRad = fmod(angleRad, Scalar(TWO_PI)); + if (angleRad < Scalar(0)) { angleRad += Scalar(TWO_PI); } return angleRad; // [rad] } + // Method to wrap an angle in radians to the range [0, 2pi] (double overload) + inline double wrapRad(double angleRad) { return wrapRad(angleRad); } // Linear interpolation between a and b by factor t (0 <= t <= 1) + template inline double lerp(double a, double b, double t) { return a + t * (b - a); } - // Check if two doubles are approximately equal within a tolerance + // Check if two doubles are approximately equal within a tolerance + template inline bool approximatelyEqual(double a, double b, double tolerance = 1e-9) { return std::fabs(a - b) <= tolerance; } // Check if a value is within a specified range [minVal, maxVal] + template inline bool isInRange(double value, double minVal, double maxVal) { return (value >= minVal) && (value <= maxVal); } - // Sign function: returns -1 for negative, 1 for positive, and 0 for zero - inline double sgn(double val) { return (val > 0) - (val < 0); } - - // Returns a quadratic function f(x) = a*x^2 + b*x + c - inline std::function quadratic(double a, double b, double c) { return [a, b, c](double x) { return a * x * x + b * x + c; }; } - - // Returns a cubic function f(x) = a*x^3 + b*x^2 + c*x + d - inline std::function cubic(double a, double b, double c, double d) { return [a, b, c, d](double x) { return a * x * x * x + b * x * x + c * x + d; }; } - - // Dot product of two Vec3 - inline double dot(const Vec3& v1, const Vec3& v2) { return v1.x() * v2.x() + v1.y() * v2.y() + v1.z() * v2.z(); } + // Dot product of two 3D vectors + template + inline Scalar dot(const Vec3_T& v1, const Vec3_T& v2) { return v1.x() * v2.x() + v1.y() * v2.y() + v1.z() * v2.z(); } + // Dot product of two 3D vectors (double overload) + inline double dot(const Vec3& v1, const Vec3& v2) { return dot(v1, v2); } + // Cross product of two 3D vectors + template + inline Vec3_T cross(const Vec3_T& v1, const Vec3_T& v2) { + return Vec3_T( + v1.y() * v2.z() - v1.z() * v2.y(), + v1.z() * v2.x() - v1.x() * v2.z(), + v1.x() * v2.y() - v1.y() * v2.x() + ); + } + // Cross product of two 3D vectors (double overload) + inline Vec3 cross(const Vec3& v1, const Vec3& v2) { return cross(v1, v2); } // Natural frequency of a mass-spring system - inline double natural_freq(double k_p, double I) { - if (!std::isfinite(k_p) || !std::isfinite(I)) return std::numeric_limits::quiet_NaN(); - if (k_p < 0.0 || I <= 0.0) return std::numeric_limits::quiet_NaN(); - return std::sqrt(k_p / I); + template + inline Scalar natural_freq(Scalar k_p, Scalar I) { + if (!isfinite(k_p) || !isfinite(I)) { return std::numeric_limits::quiet_NaN(); } + if (k_p < Scalar(0) || I <= Scalar(0)) { return std::numeric_limits::quiet_NaN(); } + return sqrt(k_p / I); } + // Natural frequency of a mass-spring system (double overload) + inline double natural_freq(double k_p, double I) { return natural_freq(k_p, I); } // Damping ratio of a mass-spring-damper system - inline double damping_ratio(double k_d, double I, double k_p) { - if (!std::isfinite(k_p) || !std::isfinite(k_d) || !std::isfinite(I)) { return std::numeric_limits::quiet_NaN(); } - if (k_p <= 0.0 || I <= 0.0) { return std::numeric_limits::quiet_NaN(); } - double w_n = natural_freq(k_p, I); - return k_d / (2.0 * I * w_n); // damping ratio - zeta + template + inline Scalar damping_ratio(Scalar k_d, Scalar I, Scalar k_p) { + if (!isfinite(k_p) || !isfinite(k_d) || !isfinite(I)) { return std::numeric_limits::quiet_NaN(); } + if (k_p < Scalar(0) || I <= Scalar(0)) { return std::numeric_limits::quiet_NaN(); } + Scalar w_n = natural_freq(k_p, I); + return k_d / (Scalar(2) * I * w_n); // damping ratio - zeta } + // Damping ratio of a mass-spring-damper system (double overload) + inline double damping_ratio(double k_d, double I, double k_p) { return damping_ratio(k_d, I, k_p); } + // Converts a non-eigen vector to Vec3 (if it has 3 elements) + template + inline Vec3 toVec3(const T& vec) { + static_assert(std::tuple_size::value == 3, "Input vector must have exactly 3 elements"); + return Vec3(vec[0], vec[1], vec[2]); + } // Overloads for std::array and C arrays for Vec3 conversion template inline std::enable_if_t toVec3(const std::array& arr) { @@ -125,6 +116,12 @@ namespace mathlib { return Vec3(arr[0], arr[1], arr[2]); } + // Converts a non-eigen vector to Vec4 (if it has 4 elements) + template + inline Vec4 toVec4(const T& vec) { + static_assert(std::tuple_size::value == 4, "Input vector must have exactly 4 elements"); + return Vec4(vec[0], vec[1], vec[2], vec[3]); + } // Overloads for std::array and C arrays for Vec4 conversion template inline std::enable_if_t toVec4(const std::array& arr) { @@ -135,33 +132,62 @@ namespace mathlib { return Vec4(arr[0], arr[1], arr[2], arr[3]); } + // Converts a non-eigen 3x3 matrix to Mat3 (if it has 3 rows and 3 columns) + template + inline Mat3 toMat3(const T& mat) { + static_assert(std::tuple_size::value == 3 && std::tuple_size::value == 3, "Input matrix must be 3x3"); + return Mat3( + 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] + ); + } // Overloads for std::array and C arrays for Mat3 conversion template inline std::enable_if_t toMat3(const std::array, N>& mat) { - return Mat3(mat[0][0], mat[0][1], mat[0][2], + return Mat3( + 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]); + mat[2][0], mat[2][1], mat[2][2] + ); } template inline Mat3 toMat3(const T mat[3][3]) { - return Mat3(mat[0][0], mat[0][1], mat[0][2], + return Mat3( + 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]); + mat[2][0], mat[2][1], mat[2][2] + ); } + // Converts a non-eigen 4x4 matrix to Mat4 (if it has 4 rows and 4 columns) + template + inline Mat4 toMat4(const T& mat) { + static_assert(std::tuple_size::value == 4 && std::tuple_size::value == 4, "Input matrix must be 4x4"); + return Mat4( + mat[0][0], mat[0][1], mat[0][2], mat[0][3], + mat[1][0], mat[1][1], mat[1][2], mat[1][3], + mat[2][0], mat[2][1], mat[2][2], mat[2][3], + mat[3][0], mat[3][1], mat[3][2], mat[3][3] + ); + } // Overloads for std::array and C arrays for Mat4 conversion template inline std::enable_if_t toMat4(const std::array, N>& mat) { - return Mat4(mat[0][0], mat[0][1], mat[0][2], mat[0][3], + return Mat4( + mat[0][0], mat[0][1], mat[0][2], mat[0][3], mat[1][0], mat[1][1], mat[1][2], mat[1][3], mat[2][0], mat[2][1], mat[2][2], mat[2][3], - mat[3][0], mat[3][1], mat[3][2], mat[3][3]); + mat[3][0], mat[3][1], mat[3][2], mat[3][3] + ); } template inline Mat4 toMat4(const T mat[4][4]) { - return Mat4(mat[0][0], mat[0][1], mat[0][2], mat[0][3], + return Mat4( + mat[0][0], mat[0][1], mat[0][2], mat[0][3], mat[1][0], mat[1][1], mat[1][2], mat[1][3], mat[2][0], mat[2][1], mat[2][2], mat[2][3], - mat[3][0], mat[3][1], mat[3][2], mat[3][3]); + mat[3][0], mat[3][1], mat[3][2], mat[3][3] + ); } } \ No newline at end of file From 642044951c3757dee217a422c4346f7d8e22508c Mon Sep 17 00:00:00 2001 From: saltyjoss Date: Sun, 24 May 2026 18:39:45 +0100 Subject: [PATCH 071/210] refactor(WIP): Updated MathLib and DSFE scalar pipeline for full DualNumber/Eigen compatibility * Add scalar-generic math wrappers and transform utilities for AD-safe numerical operations * Introduce VectorUtils helpers for scalar-consistent vector normalisation and norm evaluation * Propagate templated scalar support through robot dynamics, kinematics, metrics, telemetry, and integration systems * Fix mixed Eigen scalar assignment failures across Pose, Quaternion, Matrix, and Vector paths * Add explicit scalar casting boundaries for runtime telemetry and logging extraction * Refactor RobotSystem step/update pipeline for scalar-generic dynamics execution * Rework spatial dynamics and kinematics transforms for AD-safe quaternion and AngleAxis usage * Update implicit and explicit AD integrators for unified scalar propagation * Consolidate templated MathLib type aliases and scalar utility infrastructure * Remove legacy non-templated dynamics/integration source files superseded by header/inl implementations * Expand DualNumber and AD unit test coverage for Eigen, Newton, Jacobian, and integrator systems * Clean up include ordering and dependency visibility issues across MathLib and DSFE * Stabilise full automatic differentiation simulation pipeline and restore successful project-wide compilaty TODO: * Sort out actual runtime crashing --- .../DSFE_Core/include/Analysis/Telemetry.h | 33 ++-- .../DSFE_Core/include/Robots/DynamicsTypes.h | 2 +- .../DSFE_Core/include/Robots/RobotDynamics.h | 1 - .../include/Robots/RobotDynamics.inl | 4 +- .../include/Robots/RobotKinematics.h | 7 +- .../include/Robots/RobotKinematics.inl | 23 ++- .../DSFE_Core/include/Robots/RobotMetrics.h | 28 ++-- .../DSFE_Core/include/Robots/RobotModel.h | 7 +- .../include/Robots/RobotSystemStep.inl | 10 +- .../include/Robots/SpatialDynamics.inl | 3 +- .../include/Robots/TrajectoryManager.h | 5 +- DSFE_App/DSFE_Core/src/Analysis/Telemetry.cpp | 6 +- .../src/Interpreter/Commands/TrajSetCmd.cpp | 10 +- DSFE_App/DSFE_Core/src/Robots/RobotLoader.cpp | 4 +- DSFE_App/DSFE_Core/src/Robots/RobotSystem.cpp | 6 +- .../src/Robots/TrajectoryManager.cpp | 2 +- PxMLib/MathLib/CMakeLists.txt | 10 -- PxMLib/MathLib/include/control/Error.h | 40 ++++- .../include/control/IJointTrajectory.h | 8 +- PxMLib/MathLib/include/control/Metrics.h | 51 ++---- .../include/control/MultisineTrajectory.h | 80 +++++---- PxMLib/MathLib/include/control/PID.h | 44 +++-- .../include/control/SinusoidalTrajectory.h | 56 ++++--- .../MathLib/include/control/TrajectoryTypes.h | 16 +- .../include/control/TrapezoidTrajectory.h | 88 +++++----- PxMLib/MathLib/include/core/DualNumbers.h | 91 +++++++---- PxMLib/MathLib/include/core/MathLib.h | 7 +- PxMLib/MathLib/include/core/ScalarScaling.h | 25 +-- PxMLib/MathLib/include/core/ScalarStdFunc.h | 44 ++--- PxMLib/MathLib/include/core/Utils.h | 8 +- PxMLib/MathLib/include/core/VectorUtils.h | 22 +++ PxMLib/MathLib/include/dynamics/MassMatrix.h | 23 --- .../include/dynamics/Nonlinear_Terms.h | 34 ---- PxMLib/MathLib/include/dynamics/RigidBody.h | 12 +- PxMLib/MathLib/include/dynamics/StateSpace.h | 26 --- .../MathLib/include/integrators/Integration.h | 31 ---- .../integrators/auto_diff_integrators.inl | 7 +- .../integrators/explicit_integrators.inl | 115 +++++++------ .../integrators/numerical_integrators.h | 6 +- PxMLib/MathLib/include/kinematics/DH_Params.h | 41 +++-- .../include/kinematics/Forward_Kinematics.h | 153 +++++++++--------- PxMLib/MathLib/include/kinematics/Jacobian.h | 64 +++++++- .../include/kinematics/Transform_Utils.h | 26 +-- .../MathLib/include/kinematics/URDF_Types.h | 16 +- PxMLib/MathLib/pch.cpp | 5 +- PxMLib/MathLib/src/control/Error.cpp | 35 ---- PxMLib/MathLib/src/control/PID.cpp | 27 ---- PxMLib/MathLib/src/dynamics/MassMatrix.cpp | 17 -- .../MathLib/src/dynamics/Nonlinear_Terms.cpp | 28 ---- PxMLib/MathLib/src/dynamics/StateSpace.cpp | 19 --- .../MathLib/src/integrators/Integration.cpp | 54 ------- .../src/integrators/IntegrationAnalysis.cpp | 85 ---------- PxMLib/MathLib/src/kinematics/DH_Params.cpp | 27 ---- PxMLib/MathLib/src/kinematics/Jacobian.cpp | 67 -------- .../src/kinematics/Transform_Utils.cpp | 42 ----- .../ADExplicitIntegratorTests.cpp | 2 +- .../ADImplicitIntegratorTests.cpp | 2 +- .../ADJacobianTests.cpp | 2 +- .../ADNewtonTests.cpp | 10 +- .../DualNumberTests/ArithmeticTests.cpp | 100 ++++++------ .../AssignmentOperatorTests.cpp | 44 ++--- .../DualNumberTests/DifferentiationTests.cpp | 36 ++--- .../DualNumberTests/DivisionTests.cpp | 38 ++--- .../DualNumber_Eigen_ADTests.cpp | 57 ++----- .../DualNumber_Eigen_ArithmeticTests.cpp | 3 +- .../DualNumber_Eigen_BasicTests.cpp | 2 +- .../DualNumber_Eigen_SolverTests.cpp | 14 +- .../DualNumberTests/TranscendentalTests.cpp | 2 +- 68 files changed, 813 insertions(+), 1200 deletions(-) create mode 100644 PxMLib/MathLib/include/core/VectorUtils.h delete mode 100644 PxMLib/MathLib/include/dynamics/MassMatrix.h delete mode 100644 PxMLib/MathLib/include/dynamics/Nonlinear_Terms.h delete mode 100644 PxMLib/MathLib/include/dynamics/StateSpace.h delete mode 100644 PxMLib/MathLib/include/integrators/Integration.h delete mode 100644 PxMLib/MathLib/src/control/Error.cpp delete mode 100644 PxMLib/MathLib/src/control/PID.cpp delete mode 100644 PxMLib/MathLib/src/dynamics/MassMatrix.cpp delete mode 100644 PxMLib/MathLib/src/dynamics/Nonlinear_Terms.cpp delete mode 100644 PxMLib/MathLib/src/dynamics/StateSpace.cpp delete mode 100644 PxMLib/MathLib/src/integrators/Integration.cpp delete mode 100644 PxMLib/MathLib/src/integrators/IntegrationAnalysis.cpp delete mode 100644 PxMLib/MathLib/src/kinematics/DH_Params.cpp delete mode 100644 PxMLib/MathLib/src/kinematics/Jacobian.cpp delete mode 100644 PxMLib/MathLib/src/kinematics/Transform_Utils.cpp diff --git a/DSFE_App/DSFE_Core/include/Analysis/Telemetry.h b/DSFE_App/DSFE_Core/include/Analysis/Telemetry.h index 2db3b7ce..dd80ed2b 100644 --- a/DSFE_App/DSFE_Core/include/Analysis/Telemetry.h +++ b/DSFE_App/DSFE_Core/include/Analysis/Telemetry.h @@ -2,6 +2,9 @@ #pragma once #include "EngineCore.h" + +#include + #include #include @@ -22,24 +25,24 @@ namespace diagnostics { // Struct for joint telemetry data struct DSFE_API JointTelemetry{ // Actual data - double q = 0.0f; // Joint angle in radians - double qd = 0.0f; // Joint angular velocity in radians per second + double q = 0.0; // Joint angle in radians + double qd = 0.0; // Joint angular velocity in radians per second // Additional dynamics data - double torqueNm = 0.0f; // Joint torque (N·m) - double damping = 0.0f; // Joint damping coefficient (kg·m²/s) - double friction = 0.0f; // Joint friction coefficient (Coulomb friction - N·m) - double effort = 0.0f; // Normalized effort (0 to 1) + double torqueNm = 0.0; // Joint torque (N·m) + double damping = 0.0; // Joint damping coefficient (kg·m²/s) + double friction = 0.0; // Joint friction coefficient (Coulomb friction - N·m) + double effort = 0.0; // Normalized effort (0 to 1) // Reference data - double q_ref = 0.0f; // Reference joint angle in radians - double qd_ref = 0.0f; // Reference joint angular velocity in radians per second - double qdd_ref = 0.0f; // Reference joint angular acceleration in radians per second squared + double q_ref = 0.0; // Reference joint angle in radians + double qd_ref = 0.0; // Reference joint angular velocity in radians per second + double qdd_ref = 0.0; // Reference joint angular acceleration in radians per second squared // Trajectory data - double traj_q = 0.0f; // Trajectory joint position - double traj_qd = 0.0f; // Trajectory joint velocity - double traj_qdd = 0.0f; // Trajectory joint acceleration + double traj_q = 0.0; // Trajectory joint position + double traj_qd = 0.0; // Trajectory joint velocity + double traj_qdd = 0.0; // Trajectory joint acceleration bool traj_active = false; // Trajectory active state for a joint // Limit clamping flags @@ -54,8 +57,8 @@ namespace diagnostics { std::vector j; // Vector of joint telemetry data // Summary statistics (precomputed to relieve analysis load) - double err_rms = 0.0f; // RMS error across all joints - double err_max = 0.0f; // Maximum error across all joints + double err_rms = 0.0; // RMS error across all joints + double err_max = 0.0; // Maximum error across all joints int clamp_theta = 0; // Sum of angle clamping events across all joints int clamp_omega = 0; // Sum of velocity clamping events across all joints int clamp_sum = 0; // Sum of clamping events across all joints @@ -77,7 +80,7 @@ namespace diagnostics { // Reset the ring buffer with new capacity void reset(size_t cap) { - _cap = std::max(cap, 1); + _cap = mathlib::max(cap, 1); _buf.clear(); _buf.resize(_cap); clear(); diff --git a/DSFE_App/DSFE_Core/include/Robots/DynamicsTypes.h b/DSFE_App/DSFE_Core/include/Robots/DynamicsTypes.h index 5534a636..197d600f 100644 --- a/DSFE_App/DSFE_Core/include/Robots/DynamicsTypes.h +++ b/DSFE_App/DSFE_Core/include/Robots/DynamicsTypes.h @@ -138,7 +138,7 @@ namespace robots { mathlib::VecX_T dxdt; mathlib::VecX_T qdd; - RobotMetrics metrics; + RobotMetrics metrics; void resize(size_t n) { dxdt.resize(2 * n); diff --git a/DSFE_App/DSFE_Core/include/Robots/RobotDynamics.h b/DSFE_App/DSFE_Core/include/Robots/RobotDynamics.h index aee14433..c9a850b3 100644 --- a/DSFE_App/DSFE_Core/include/Robots/RobotDynamics.h +++ b/DSFE_App/DSFE_Core/include/Robots/RobotDynamics.h @@ -3,7 +3,6 @@ #include "EngineCore.h" #include -#include #include "Robots/DynamicsTypes.h" #include "Robots/RobotMetrics.h" diff --git a/DSFE_App/DSFE_Core/include/Robots/RobotDynamics.inl b/DSFE_App/DSFE_Core/include/Robots/RobotDynamics.inl index 30fd6f44..8e971286 100644 --- a/DSFE_App/DSFE_Core/include/Robots/RobotDynamics.inl +++ b/DSFE_App/DSFE_Core/include/Robots/RobotDynamics.inl @@ -110,7 +110,7 @@ namespace robots { const mathlib::Mat3_T R_j = T_joint_j.template block<3, 3>(0, 0); const mathlib::Vec3_T p_j = T_joint_j.template block<3, 1>(0, 3); - const mathlib::Vec3_T z_j = (R_j * j_j.axis).normalized(); + const mathlib::Vec3_T z_j = mathlib::safeNormalised(R_j * j_j.axis); mathlib::Vec3_T J_vj = z_j.cross(com - p_j); // linear velocity Jacobian column for joint j mathlib::Vec3_T J_wj = z_j; // angular velocity Jacobian column for joint j @@ -271,7 +271,7 @@ namespace robots { Scalar qd_i = qd_local[i]; Scalar tanh_term = mathlib::tanh(qd_i / eps_f); - Scalar stiff_friction_slope = c * (1.0 - tanh_term * tanh_term) / eps_f; + Scalar stiff_friction_slope = c * (Scalar(1) - tanh_term * tanh_term) / eps_f; dTau_dv(i, i) = -k_d - b + stiff_friction_slope; } diff --git a/DSFE_App/DSFE_Core/include/Robots/RobotKinematics.h b/DSFE_App/DSFE_Core/include/Robots/RobotKinematics.h index 12bf2e5f..581fe529 100644 --- a/DSFE_App/DSFE_Core/include/Robots/RobotKinematics.h +++ b/DSFE_App/DSFE_Core/include/Robots/RobotKinematics.h @@ -2,11 +2,7 @@ #pragma once #include "EngineCore.h" -#include "MathLibAPI.h" -#include -#include -#include -#include +#include #include #include "Robots/RobotSimSnapshot.h" @@ -16,7 +12,6 @@ namespace robots { // Forward declarations struct RobotLink; struct RobotJoint; - struct RobotMetrics; // Kinematics class responsible for computing forward kinematics and related transformations class DSFE_API RobotKinematics { diff --git a/DSFE_App/DSFE_Core/include/Robots/RobotKinematics.inl b/DSFE_App/DSFE_Core/include/Robots/RobotKinematics.inl index d9c61b7e..e44c60b8 100644 --- a/DSFE_App/DSFE_Core/include/Robots/RobotKinematics.inl +++ b/DSFE_App/DSFE_Core/include/Robots/RobotKinematics.inl @@ -29,8 +29,10 @@ namespace robots { const Scalar q = x[i]; // joint angle from state vector mathlib::Pose_T T_origin = mathlib::Pose_T::Identity(); // transform from parent link to joint frame (fixed) - T_origin.template block<3, 3>(0, 0) = joint.origin_q.toRotationMatrix(); // rotation from parent link frame to joint frame, derived from rpy in JSON - T_origin.template block<3, 1>(0, 3) = joint.origin_xyz; // translation from parent link to joint frame + mathlib::Quat_T q_origin = joint.origin_q.template cast(); // convert quaternion to correct scalar type + + T_origin.template block<3, 3>(0, 0) = q_origin.toRotationMatrix(); // rotation from parent link frame to joint frame, derived from rpy in JSON + T_origin.template block<3, 1>(0, 3) = joint.origin_xyz.template cast(); // translation from parent link to joint frame // Compute joint motion transform based on joint axis and angle Pose_T T_motion = mathlib::Pose_T::Identity(); @@ -83,23 +85,20 @@ namespace robots { Scalar q ) const { mathlib::Pose_T T = mathlib::Pose_T::Identity(); // homogeneous transformation matrix (4x4) - - Eigen::AngleAxis aa(q, axis_joint.normalized()); // create angle-axis rotation from joint angle and axis - T.template block<3, 3>(0, 0) = aa.toRotationMatrix(); // set upper-left 3x3 block to rotation matrix - + T.template block<3, 3>(0, 0) = mathlib::AngleAxis(q, mathlib::safeNormalised(axis_joint)); // set upper-left 3x3 block to rotation matrix return T; // (4x4) homogeneous transformation } // Converts roll-pitch-yaw angles (in radians) to a quaternion representation template mathlib::Quat_T RobotKinematics::rpyRadToQuat(const mathlib::Vec3_T& rpyRad) { - const double roll = rpyRad.x(); - const double pitch = rpyRad.y(); - const double yaw = rpyRad.z(); + const Scalar roll = rpyRad.x(); + const Scalar pitch = rpyRad.y(); + const Scalar yaw = rpyRad.z(); - const Quat_T qx(Eigen::AngleAxis(roll, mathlib::Vec3_T(Scalar(1), Scalar(0), Scalar(0)))); - const Quat_T qy(Eigen::AngleAxis(pitch, mathlib::Vec3_T(Scalar(0), Scalar(1), Scalar(0)))); - const Quat_T qz(Eigen::AngleAxis(yaw, mathlib::Vec3_T(Scalar(0), Scalar(0), Scalar(1)))); + const Quat_T qx(Eigen::AngleAxis(roll, mathlib::Vec3_T(Scalar(1), Scalar(0), Scalar(0)))); + const Quat_T qy(Eigen::AngleAxis(pitch, mathlib::Vec3_T(Scalar(0), Scalar(1), Scalar(0)))); + const Quat_T qz(Eigen::AngleAxis(yaw, mathlib::Vec3_T(Scalar(0), Scalar(0), Scalar(1)))); return (qz * qy * qx).normalized(); } diff --git a/DSFE_App/DSFE_Core/include/Robots/RobotMetrics.h b/DSFE_App/DSFE_Core/include/Robots/RobotMetrics.h index d412d704..74da3b6d 100644 --- a/DSFE_App/DSFE_Core/include/Robots/RobotMetrics.h +++ b/DSFE_App/DSFE_Core/include/Robots/RobotMetrics.h @@ -3,32 +3,32 @@ #include "EngineCore.h" #include -#include namespace robots { // Per-joint metrics + template struct RobotMetrics { // State - mathlib::VecX q; - mathlib::VecX qd; - mathlib::VecX qdd; + mathlib::VecX_T q; + mathlib::VecX_T qd; + mathlib::VecX_T qdd; - mathlib::VecX err; - mathlib::VecX errd; + mathlib::VecX_T err; + mathlib::VecX_T errd; // Dynamics - mathlib::VecX I_eff; - mathlib::VecX tau; + mathlib::VecX_T I_eff; + mathlib::VecX_T tau; // Constraints / realism - mathlib::VecX tau_barrier; - mathlib::VecX tau_sat; + mathlib::VecX_T tau_barrier; + mathlib::VecX_T tau_sat; // Energy, Work, & Power - mathlib::VecX KE; - mathlib::VecX PE; - mathlib::VecX E_total; - mathlib::VecX W_actuator; + mathlib::VecX_T KE; + mathlib::VecX_T PE; + mathlib::VecX_T E_total; + mathlib::VecX_T W_actuator; // Stability flags std::vector sat_flag; diff --git a/DSFE_App/DSFE_Core/include/Robots/RobotModel.h b/DSFE_App/DSFE_Core/include/Robots/RobotModel.h index fa692430..0d56061e 100644 --- a/DSFE_App/DSFE_Core/include/Robots/RobotModel.h +++ b/DSFE_App/DSFE_Core/include/Robots/RobotModel.h @@ -2,6 +2,7 @@ #pragma once #include "EngineCore.h" + #include #include #include @@ -11,8 +12,6 @@ #include "Platform/Logger.h" #include "EngineLib/LogMacros.h" -constexpr double DEG2RAD = std::numbers::pi / 180.0; - namespace robots { // --- Robot Model Kinematic Models --- enum class eKinematicsModel { @@ -105,7 +104,7 @@ namespace robots { bool continuous = false; double minAngle = 0.0; double maxAngle = 0.0; - double maxqd = 180.0 * DEG2RAD; + double maxqd = PI_d; double maxEffort = 0.0; // max torque/force // Soft limits double omegaRefMaxRad_s = 0.0; @@ -179,7 +178,7 @@ namespace robots { // Kinematics model (URDF or DH) eKinematicsModel kinematicsModel = eKinematicsModel::URDF; - std::vector dhParams; + std::vector> dhParams; // Visualization options eVisualFrame visualFrame = eVisualFrame::JOINT; diff --git a/DSFE_App/DSFE_Core/include/Robots/RobotSystemStep.inl b/DSFE_App/DSFE_Core/include/Robots/RobotSystemStep.inl index 27201e56..0d149206 100644 --- a/DSFE_App/DSFE_Core/include/Robots/RobotSystemStep.inl +++ b/DSFE_App/DSFE_Core/include/Robots/RobotSystemStep.inl @@ -27,14 +27,14 @@ namespace robots { snap.qdd_ref[i] = j.qdd_ref; } - snap.robotRootPose = _robotRootPose; + snap.robotRootPose = _robotRootPose.template cast(); snap.baseIsFree = _baseIsFree; - snap.lastBaseForwardForce = _lastBaseForwardForce; - snap.gravity = _gravity; + snap.lastBaseForwardForce = Scalar(_lastBaseForwardForce); + snap.gravity = Scalar(_gravity); snap.torqueMode = _robot.torqueMode; - snap.dt = _dynamics->dt(); + snap.dt = Scalar(_dynamics->dt()); snap.simTime = simTime; return snap; @@ -197,7 +197,7 @@ namespace robots { for (size_t i = 0; i < n; ++i) { const RobotJoint& j = _robot.joints[i]; - const double I_eff = (j.type == eJointType::FIXED) ? 1.0 : dynResult.metrics.I_eff[i]; + const double I_eff = (j.type == eJointType::FIXED) ? 1.0 : mathlib::real(dynResult.metrics.I_eff[i]); const double err = q_ref_real[i] - q_real[i]; const double err_d = qd_ref_real[i] - qd_real[i]; diff --git a/DSFE_App/DSFE_Core/include/Robots/SpatialDynamics.inl b/DSFE_App/DSFE_Core/include/Robots/SpatialDynamics.inl index 62d36157..01fe14e2 100644 --- a/DSFE_App/DSFE_Core/include/Robots/SpatialDynamics.inl +++ b/DSFE_App/DSFE_Core/include/Robots/SpatialDynamics.inl @@ -24,8 +24,7 @@ namespace robots { if (j.type == eJointType::REVOLUTE) { mathlib::Vec3_T axis = mathlib::safeNormalised(j.S.angular()); - Eigen::AngleAxis aa(q[i], axis); - mathlib::Mat3_T R = aa.toRotationMatrix(); + mathlib::Mat3_T R = mathlib::AngleAxis(q[i], axis); mathlib::Vec3_T r = mathlib::Vec3_T::Zero(); XJ = mathlib::spatialTransform(R, r); } diff --git a/DSFE_App/DSFE_Core/include/Robots/TrajectoryManager.h b/DSFE_App/DSFE_Core/include/Robots/TrajectoryManager.h index ac3e64b5..50cc1418 100644 --- a/DSFE_App/DSFE_Core/include/Robots/TrajectoryManager.h +++ b/DSFE_App/DSFE_Core/include/Robots/TrajectoryManager.h @@ -5,8 +5,7 @@ // Initially templated off a tutorial: // GitHub: jayanam/jgl_demos/JGL_MeshLoader #include "EngineCore.h" -#include -#include +#include #include namespace robots { class DSFE_API RobotSystem; } @@ -34,7 +33,7 @@ namespace control { bool empty() const { return _active.empty(); } // - bool tryEval(const std::string& link, double t, control::TrajState& out) const; + bool tryEval(const std::string& link, double t, control::TrajState& out) const; // Check if a trajectory is active for a specific robot link bool hasActive(const std::string& link) const; diff --git a/DSFE_App/DSFE_Core/src/Analysis/Telemetry.cpp b/DSFE_App/DSFE_Core/src/Analysis/Telemetry.cpp index 0deb9bd8..7824a98f 100644 --- a/DSFE_App/DSFE_Core/src/Analysis/Telemetry.cpp +++ b/DSFE_App/DSFE_Core/src/Analysis/Telemetry.cpp @@ -49,7 +49,7 @@ namespace diagnostics { // Clamping flags jt.clampTheta = (j.q <= j.limits.minAngle) || (j.q >= j.limits.maxAngle); - jt.clampOmega = (std::abs(j.qd) >= j.limits.maxqd); + jt.clampOmega = (mathlib::abs(j.qd) >= j.limits.maxqd); // Accumulate clamping events clampThetaSum += (int)jt.clampTheta; @@ -58,7 +58,7 @@ namespace diagnostics { // Trajectory data if (trajOpt) { - control::TrajState ts{}; + control::TrajState ts{}; if (trajOpt->tryEval(std::string(j.child), t, ts)) { jt.traj_q = ts.q; jt.traj_qd = ts.qd; @@ -79,7 +79,7 @@ namespace diagnostics { } // Error statistics - s.err_rms = (n > 0) ? std::sqrt(sum_e2 / (double)n) : 0.0f; // RMS error + s.err_rms = (n > 0) ? mathlib::sqrt(sum_e2 / (double)n) : 0.0f; // RMS error s.err_max = max_abs_e; // max error s.clamp_theta = clampThetaSum; // total angle clamping events s.clamp_omega = clampOmegaSum; // total velocity clamping events diff --git a/DSFE_App/DSFE_Core/src/Interpreter/Commands/TrajSetCmd.cpp b/DSFE_App/DSFE_Core/src/Interpreter/Commands/TrajSetCmd.cpp index 4380f64c..8c96e4ec 100644 --- a/DSFE_App/DSFE_Core/src/Interpreter/Commands/TrajSetCmd.cpp +++ b/DSFE_App/DSFE_Core/src/Interpreter/Commands/TrajSetCmd.cpp @@ -98,7 +98,7 @@ namespace commands { const double vmax = degToRad(_params[1]); const double amax = degToRad(_params[2]); - auto traj = std::make_unique(t0, q0, q1, vmax, amax); + auto traj = std::make_unique>(t0, q0, q1, vmax, amax); auto trajMgr = core->trajectoryManager(); trajMgr->set(_link, std::move(traj)); @@ -148,7 +148,7 @@ namespace commands { // Phase double phi = (_params.size() == 5) ? degToRad(_params[4]) : 0.0; // radians - auto traj = std::make_unique(t0, t0 + dur, centre, amp, fHz, phi); + auto traj = std::make_unique>(t0, t0 + dur, centre, amp, fHz, phi); auto trajMgr = core->trajectoryManager(); trajMgr->set(_link, std::move(traj)); @@ -186,11 +186,11 @@ namespace commands { return { CmdState::Failed, {}, "trajSet(MSINE): params after duration must be triples (amp,f,phase)." }; } - std::vector comps; + std::vector> comps; comps.reserve(rest / 3); for (size_t k = 2; k + 2 < _params.size(); k += 3) { - control::SineComponent comp; + control::SineComponent comp; comp.amp = degToRad(_params[k + 0]); // radians comp.freqHz = _params[k + 1]; // Hz comp.phaseRad = degToRad(_params[k + 2]); // radians @@ -218,7 +218,7 @@ namespace commands { } - auto traj = std::make_unique(t0, t0 + dur, centre, std::move(comps)); + auto traj = std::make_unique>(t0, t0 + dur, centre, std::move(comps)); auto trajMgr = core->trajectoryManager(); trajMgr->set(_link, std::move(traj)); diff --git a/DSFE_App/DSFE_Core/src/Robots/RobotLoader.cpp b/DSFE_App/DSFE_Core/src/Robots/RobotLoader.cpp index 1d614b1a..975182c4 100644 --- a/DSFE_App/DSFE_Core/src/Robots/RobotLoader.cpp +++ b/DSFE_App/DSFE_Core/src/Robots/RobotLoader.cpp @@ -333,7 +333,7 @@ namespace robots { } // Parse DH parameters if present - static bool parseDHParameters(const json& jointData, DH_Params& out) { + static bool parseDHParameters(const json& jointData, DH_Params& out) { // Accept "dh" ONLY (your JSON uses "dh") if (!jointData.contains("dh") || !jointData["dh"].is_object()) return false; @@ -463,7 +463,7 @@ namespace robots { // If robot is DH-mode, also parse DH table if (robot.kinematicsModel == eKinematicsModel::DH) { - DH_Params dh{}; + DH_Params dh{}; if (!parseDHParameters(jointData, dh)) { LOG_WARN("Joint %s missing 'dh' unexpectedly; forcing URDF mode.", joint.name.c_str()); robot.kinematicsModel = eKinematicsModel::URDF; diff --git a/DSFE_App/DSFE_Core/src/Robots/RobotSystem.cpp b/DSFE_App/DSFE_Core/src/Robots/RobotSystem.cpp index 6017e81e..b3e7a0cc 100644 --- a/DSFE_App/DSFE_Core/src/Robots/RobotSystem.cpp +++ b/DSFE_App/DSFE_Core/src/Robots/RobotSystem.cpp @@ -30,9 +30,7 @@ namespace robots { // Destructor RobotSystem::~RobotSystem() = default; - const robots::RobotModel& RobotSystem::model() const { - return _robot; - } + const robots::RobotModel& RobotSystem::model() const { return _robot; } // Helper function to convert std::vector to Eigen::VectorXd static VecX toVecX(const std::vector& a) { @@ -281,7 +279,7 @@ namespace robots { // Sample trajectories ("ground truth" inputs) for (size_t i = 0; i < n; ++i) { RobotJoint& j = _robot.joints[i]; - control::TrajState s{}; + control::TrajState s{}; // Try to evaluate trajectory if (traj.tryEval(std::string(j.child), t, s)) { j.q_ref = clampJointAngle(j, s.q); // set ref angle diff --git a/DSFE_App/DSFE_Core/src/Robots/TrajectoryManager.cpp b/DSFE_App/DSFE_Core/src/Robots/TrajectoryManager.cpp index 0a40036d..b699c5f6 100644 --- a/DSFE_App/DSFE_Core/src/Robots/TrajectoryManager.cpp +++ b/DSFE_App/DSFE_Core/src/Robots/TrajectoryManager.cpp @@ -17,7 +17,7 @@ namespace control { void TrajectoryManager::clearAll() { _active.clear(); } // Evaluate the trajectory for a specific robot link at time t, returning the desired state in out - bool TrajectoryManager::tryEval(const std::string& link, double t, control::TrajState& out) const { + bool TrajectoryManager::tryEval(const std::string& link, double t, control::TrajState& out) const { auto it = _active.find(link); if (it == _active.end()) { return false; } const auto r = it->second->eval(t); diff --git a/PxMLib/MathLib/CMakeLists.txt b/PxMLib/MathLib/CMakeLists.txt index d36da191..495cd98e 100644 --- a/PxMLib/MathLib/CMakeLists.txt +++ b/PxMLib/MathLib/CMakeLists.txt @@ -5,16 +5,6 @@ project(MathLib LANGUAGES CXX) add_library(MathLib STATIC ${CMAKE_CURRENT_SOURCE_DIR}/pch.cpp - ${CMAKE_CURRENT_SOURCE_DIR}/src/control/Error.cpp - ${CMAKE_CURRENT_SOURCE_DIR}/src/control/PID.cpp - ${CMAKE_CURRENT_SOURCE_DIR}/src/dynamics/MassMatrix.cpp - ${CMAKE_CURRENT_SOURCE_DIR}/src/dynamics/Nonlinear_Terms.cpp - ${CMAKE_CURRENT_SOURCE_DIR}/src/dynamics/StateSpace.cpp - ${CMAKE_CURRENT_SOURCE_DIR}/src/integrators/Integration.cpp - ${CMAKE_CURRENT_SOURCE_DIR}/src/integrators/IntegrationAnalysis.cpp - ${CMAKE_CURRENT_SOURCE_DIR}/src/kinematics/DH_Params.cpp - ${CMAKE_CURRENT_SOURCE_DIR}/src/kinematics/Jacobian.cpp - ${CMAKE_CURRENT_SOURCE_DIR}/src/kinematics/Transform_Utils.cpp ) target_precompile_headers(MathLib PRIVATE diff --git a/PxMLib/MathLib/include/control/Error.h b/PxMLib/MathLib/include/control/Error.h index 4c74af22..74e24d3b 100644 --- a/PxMLib/MathLib/include/control/Error.h +++ b/PxMLib/MathLib/include/control/Error.h @@ -1,7 +1,6 @@ #pragma once -#include "MathLibAPI.h" -#include "core/Types.h" +#include using namespace mathlib; @@ -9,9 +8,10 @@ namespace control { /// /// Structure to hold pose error information. /// - struct MATHLIB_API PoseError { - Vec3 position; // Positional error (x, y, z) - Vec3 orientation; // Orientational error (roll, pitch, yaw) + template + struct MATHLIB_API PoseError_T { + Vec3_T position; // Positional error (x, y, z) + Vec3_T orientation; // Orientational error (roll, pitch, yaw) }; /// @@ -20,7 +20,8 @@ namespace control { /// The desired joint configuration. /// The current joint configuration. /// The output joint space error. - void jointErr(const VecX& q_desired, const VecX& q_current, VecX& out_error); + template + void jointErr(const VecX_T& q_des, const VecX_T& q_cur, VecX_T& err) { err = q_des - q_cur; } /// /// Computes the pose error between the desired and current transformation matrices. @@ -28,5 +29,30 @@ namespace control { /// The desired transformation matrix. /// The current transformation matrix. /// A PoseError struct containing positional and orientational errors. - PoseError poseErr(const Mat4& T_desired, const Mat4& T_current); + template + PoseError_T poseErr(const Mat4_T& T_des, const Mat4_T& T_cur) { + PoseError error; + // Positional error + error.position = T_des.template block<3, 1>(0, 3) - T_cur.template block<3, 1>(0, 3); + // Orientational error (using rotation matrices) + Mat3_T R_des = T_des.template block<3, 3>(0, 0); + Mat3_T R_cur = T_cur.template block<3, 3>(0, 0); + Mat3_T R_err = R_des * R_cur.transpose(); + // Convert rotation matrix to roll-pitch-yaw angles + Scalar sy = sqrt(R_err(0, 0) * R_err(0, 0) + R_err(1, 0) * R_err(1, 0)); + bool singular = sy < Scalar(1e-6); + double x, y, z; + if (!singular) { + x = atan2(R_err(2, 1), R_err(2, 2)); + y = atan2(-R_err(2, 0), sy); + z = atan2(R_err(1, 0), R_err(0, 0)); + } + else { + x = atan2(-R_err(1, 2), R_err(1, 1)); + y = atan2(-R_err(2, 0), sy); + z = Scalar(0); + } + error.orientation = Vec3_T(x, y, z); + return error; + } } \ No newline at end of file diff --git a/PxMLib/MathLib/include/control/IJointTrajectory.h b/PxMLib/MathLib/include/control/IJointTrajectory.h index 3ceca94e..3665bffc 100644 --- a/PxMLib/MathLib/include/control/IJointTrajectory.h +++ b/PxMLib/MathLib/include/control/IJointTrajectory.h @@ -1,6 +1,6 @@ #pragma once -#include "MathLibAPI.h" +#include #include "TrajectoryTypes.h" using namespace mathlib; @@ -11,10 +11,10 @@ namespace control { public: virtual ~IJointTrajectory() = default; - // Get the desired trajectory state at time t - virtual TrajState eval(double t) const = 0; + // Get the desired trajectory state at time t (double overload) + virtual TrajState eval(double t) const = 0; // Get the time span of the trajectory - virtual TrajTimeSpan span() const = 0; + virtual TrajTimeSpan span() const = 0; // Check if the trajectory is finished at time t bool finished(double t) const { const auto s = span(); diff --git a/PxMLib/MathLib/include/control/Metrics.h b/PxMLib/MathLib/include/control/Metrics.h index 61f733f0..75bee739 100644 --- a/PxMLib/MathLib/include/control/Metrics.h +++ b/PxMLib/MathLib/include/control/Metrics.h @@ -1,8 +1,8 @@ +// PxM/MathLib Metrics.h #pragma once #pragma warning(disable : 4251) -#include "MathLibAPI.h" -#include "core/Types.h" +#include using namespace mathlib; @@ -14,13 +14,14 @@ namespace control { /// /// The errors. /// The MSE value. - double MSE(const std::vector& errors) { - if (errors.empty()) return 0.0; - double sum_sq = 0.0; - size_t n = 0; + template + inline Scalar MSE(const std::vector>& errors) { + if (errors.empty()) { return Scalar(0); } + Scalar sum_sq = Scalar(0); + size_t n = Scalar(0); for (const auto& err : errors) { - sum_sq += err.squaredNorm(); - n += err.size(); + sum_sq += norm2(err); + n += (size_t)err.size(); } return sum_sq / n; } @@ -29,41 +30,23 @@ namespace control { /// Calculates the Root Mean Square Error (RMSE) of the provided error signals. /// /// A vector of error vectors over time. - /// The RMSE value. - double RMSE(const std::vector& errors) { - return sqrt(MSE(errors)); - } + /// The RMSE value. + template + inline Scalar RMSE(const std::vector>& errors) { return sqrt(MSE(errors)); } /// /// Calculates the maximum norm of the provided error signals. /// /// A vector of error vectors over time. /// The maximum norm value. - double maxNorm(const std::vector& errors) { - double max_norm = 0.0; + template + Scalar maxNorm(const std::vector>& errors) { + Scalar max_norm = Scalar(0); for (const auto& err : errors) { - double norm = err.norm(); - if (norm > max_norm) { - max_norm = norm; - } + Scalar norm = norm(err); + max_norm = max(max_norm, norm); } return max_norm; } - - /// - /// Calculates the settling time of a system based on the provided error signals. - /// - /// A vector of error vectors over time. - /// The time step between each error measurement. - /// The error threshold to consider the system settled. - /// The settling time in seconds. - double settlingTime(const std::vector& errors, double dt, double threshold) { - for (size_t i = errors.size(); i-- > 0;) { - if (errors[i].norm() > threshold) { - return (i + 1) * dt; // Return time just after the last exceedance - } - } - return 0.0; // System is always within threshold - } }; } \ No newline at end of file diff --git a/PxMLib/MathLib/include/control/MultisineTrajectory.h b/PxMLib/MathLib/include/control/MultisineTrajectory.h index 79a93df3..7017c565 100644 --- a/PxMLib/MathLib/include/control/MultisineTrajectory.h +++ b/PxMLib/MathLib/include/control/MultisineTrajectory.h @@ -1,7 +1,6 @@ #pragma once -#include "MathLibAPI.h" -#include "core/constants.h" +#include #include "IJointTrajectory.h" #include @@ -12,83 +11,78 @@ using namespace constants; namespace control { // Structure to define a sine wave component + template struct SineComponent { - double amp = 0.0; // rad - double freqHz = 0.0; // Hz - double phaseRad = 0.0; // rad + Scalar amp = Scalar(0); // rad + Scalar freqHz = Scalar(0); // Hz + Scalar phaseRad = Scalar(0); // rad }; + template class MultisineTrajectory : public IJointTrajectory { public: - MultisineTrajectory(double t0, double tf, double q0, std::vector comps) + // Constructor + template + MultisineTrajectory(Scalar t0, Scalar tf, Scalar q0, std::vector> comps) : _t0(t0), _tf(tf), _q0(q0), _comps(std::move(comps)) {} // Get the trajectory state at time t - TrajState eval(double t) const override { - if (t <= _t0) { return { _q0, 0.0, 0.0 }; } // before start time + template + inline TrajState eval(double t) const { + if (t <= _t0) { return { _q0, Scalar(0), Scalar(0) }; } // before start time // after end time - const double tt = std::min(t, _tf); // clamp to end time - const double tau = tt - _t0; // time since start - const double blendTime = 0.5; // seconds + const Scalar tt = min(t, _tf); // clamp to end time + const Scalar tau = tt - _t0; // time since start + const Scalar blendTime = Scalar(0.5); // seconds // Blend in with a smooth polynomial ramp - double ramp = 1.0, rampd = 0.0, rampdd = 0.0; + Scalar ramp = Scalar(1), rampd = Scalar(0), rampdd = Scalar(0); if (tau < blendTime) { - const double u = tau / blendTime; // [0, 1] + const Scalar u = tau / blendTime; // [0, 1] // smooth cubic ramp - ramp = - 3.0 * u * u - - 2.0 * u * u * u; + ramp = Scalar(3) * u * u - Scalar(2) * u * u * u; // derivative of ramp - rampd = - (6.0 * u - 6.0 * u * u) / - blendTime; + rampd = (Scalar(6) * u - Scalar(6) * u * u) / blendTime; // second derivative of ramp - rampdd = - (6.0 - 12.0 * u) / - (blendTime * blendTime); + rampdd = (Scalar(6) - Scalar(12) * u) / (blendTime * blendTime); } else if (tau > (_tf - _t0 - blendTime)) { const double u = (_tf - _t0 - tau) / blendTime; // [0, 1] // smooth cubic ramp down - ramp = - 3.0 * u * u - - 2.0 * u * u * u; + ramp = Scalar(3) * u * u - Scalar(2) * u * u * u; // derivative of ramp - rampd = - -(6.0 * u - 6.0 * u * u) / - blendTime; + rampd = -(Scalar(6) * u - Scalar(6) * u * u) / blendTime; // second derivative of ramp - rampdd = - (-6.0+ 12.0 * u) / - (blendTime * blendTime); + rampdd = (-Scalar(6) + Scalar(12) * u) / (blendTime * blendTime); } // Sum contributions from all sine components - double q = _q0, qd = 0.0, qdd = 0.0; + Scalar q = _q0, qd = Scalar(0), qdd = Scalar(0); for (const auto& c : _comps) { - const double w = 2.0 * PI_d * c.freqHz; // angular frequency - const double arg = w * tau + c.phaseRad; - const double sins = std::sin(arg); // sine term - const double coss = std::cos(arg); // cosine term + const Scalar w = Scalar(2) * PI_d * c.freqHz; // angular frequency + const Scalar arg = w * tau + c.phaseRad; + const Scalar sins = sin(arg); // sine term + const Scalar coss = cos(arg); // cosine term // raw sinsuoidal - const double qs = c.amp * sins; // position - const double qds = c.amp * w * coss; // velocity - const double qdds = -c.amp * w * w * sins; // acceleration + const Scalar qs = c.amp * sins; // position + const Scalar qds = c.amp * w * coss; // velocity + const Scalar qdds = -c.amp * w * w * sins; // acceleration // blended sinusoidal q += ramp * qs; qd += ramp * qds + rampd * qs; - qdd += ramp * qdds + 2.0 * rampd * qds + rampdd * qs; + qdd += ramp * qdds + Scalar(2) * rampd * qds + rampdd * qs; } return { q, qd, qdd }; } + // Double overload + TrajState eval(double t) const override { return eval(t); } // Get the time span of the trajectory - TrajTimeSpan span() const override { return { _t0, _tf }; } + TrajTimeSpan span() const override { return { _t0, _tf }; } private: - double _t0{ 0 }, _tf{ 0 }, _q0{ 0 }; // start time, end time, position offset - std::vector _comps; // amplitudes of the sine components + Scalar _t0{ Scalar(0) }, _tf{ Scalar(0) }, _q0{ Scalar(0) }; // start time, end time, position offset + std::vector> _comps; // amplitudes of the sine components }; } \ No newline at end of file diff --git a/PxMLib/MathLib/include/control/PID.h b/PxMLib/MathLib/include/control/PID.h index 528c4a4d..e7fcc517 100644 --- a/PxMLib/MathLib/include/control/PID.h +++ b/PxMLib/MathLib/include/control/PID.h @@ -1,7 +1,6 @@ #pragma once -#include "MathLibAPI.h" -#include "core/Types.h" +#include using namespace mathlib; @@ -9,18 +8,20 @@ namespace control { /// /// Structure to hold PID controller gains. /// - struct MATHLIB_API PID_Gains { - VecX Kp; // Proportional gains - VecX Ki; // Integral gains - VecX Kd; // Derivative gains + template + struct MATHLIB_API PID_Gains_T { + VecX_T Kp; // Proportional gains + VecX_T Ki; // Integral gains + VecX_T Kd; // Derivative gains }; /// /// Structure to hold the state of the PID controller. /// - struct MATHLIB_API PID_State { - VecX integral; // Integral of the error - VecX prev_error; // Previous error for derivative calculation + template + struct MATHLIB_API PID_State_T { + VecX_T integral; // Integral of the error + VecX_T prev_error; // Previous error for derivative calculation bool first_update = true; // Flag to check if it's the first update }; @@ -32,5 +33,28 @@ namespace control { /// The current error signal. /// The time step since the last update. /// The computed control output. - void MATHLIB_API PID(const PID_Gains& gains, PID_State& state, const VecX& error, double dt, VecX& out_u); + template + void PID( + const PID_Gains_T& gains, PID_State_T& state, + const VecX_T& error, Scalar dt, VecX_T& out_u + ) { + out_u.resize(error.size()); // Ensure the output vector is the correct size + + VecX_T P = gains.Kp.cwiseProduct(error); // Proportional term + state.integral += error * dt; // Integral term + VecX_T I = gains.Ki.cwiseProduct(state.integral); // Compute integral term + VecX_T D; // Derivative term + + if (state.first_update) { + D = VecX_T::Zero(error.size()); + state.first_update = false; + } + else { + VecX_T deriv = (error - state.prev_error) / dt; + D = gains.Kd.cwiseProduct(deriv); + } + + state.prev_error = error; // Update previous error + out_u = P + I + D; // Compute total control output + } } // namespace control \ No newline at end of file diff --git a/PxMLib/MathLib/include/control/SinusoidalTrajectory.h b/PxMLib/MathLib/include/control/SinusoidalTrajectory.h index 439085f2..d2ff9635 100644 --- a/PxMLib/MathLib/include/control/SinusoidalTrajectory.h +++ b/PxMLib/MathLib/include/control/SinusoidalTrajectory.h @@ -1,49 +1,57 @@ #pragma once -#include "MathLibAPI.h" -#include "core/constants.h" +#include #include "IJointTrajectory.h" using namespace mathlib; using namespace constants; namespace control { + template class SinusoidalTrajectory : public IJointTrajectory { public: - SinusoidalTrajectory(double t0, double tf, double q0, double amp, double freqHz, double phaseRad = 0.0) - : _t0(t0), _tf(tf), _q0(q0), _A(amp), _f(freqHz), _phi(phaseRad) {} - - TrajState eval(double t) const override { - if (t <= _t0) { return { _q0, 0.0, 0.0 }; } // before start time + template + SinusoidalTrajectory( + Scalar t0, Scalar tf, Scalar q0, + Scalar amp, Scalar freqHz, Scalar phaseRad = Scalar(0) + ) : _t0(t0), _tf(tf), _q0(q0), _A(amp), _f(freqHz), _phi(phaseRad) {} + + // Evaluate the trajectory state at time t (Scale) + template + inline TrajState eval(Scalar t) const { + if (t <= _t0) { return { _q0, Scalar(0), Scalar(0) }; } // before start time // after end time if (t >= _tf) { - const double tau = _tf - _t0; - const double w = 2.0 * PI_d * _f; - const double s = std::sin(w * tau + _phi); // sine term - const double c = std::cos(w * tau + _phi); // cosine term + const Scalar tau = _tf - _t0; + const Scalar w = Scalar(2) * PI_d * _f; + const Scalar s = sin(w * tau + _phi); // sine term + const Scalar c = cos(w * tau + _phi); // cosine term - const double q = _q0 + _A * s; // position - const double qd = _A * w * c; // velocity - const double qdd = -_A * w * w * s; // acceleration + const Scalar q = _q0 + _A * s; // position + const Scalar qd = _A * w * c; // velocity + const Scalar qdd = -_A * w * w * s; // acceleration return { q, qd, qdd }; } - const double tau = t - _t0; - const double w = 2.0 * PI * _f; - const double s = std::sin(w * tau + _phi); // sine term - const double c = std::cos(w * tau + _phi); // cosine term + const Scalar tau = t - _t0; + const Scalar w = Scalar(2) * PI * _f; + const Scalar s = sin(w * tau + _phi); // sine term + const Scalar c = cos(w * tau + _phi); // cosine term - const double q = _q0 + _A * s; // position - const double qd = _A * w * c; // velocity - const double qdd = -_A * w * w * s; // acceleration + const Scalar q = _q0 + _A * s; // position + const Scalar qd = _A * w * c; // velocity + const Scalar qdd = -_A * w * w * s; // acceleration return { q, qd, qdd }; } + // Evaluate the trajectory state at time t (double overload, for current compatibility) + TrajState eval(double t) const override { return eval(t); } - TrajTimeSpan span() const override { return { _t0, _tf }; } + // Get the time span of the trajectory + TrajTimeSpan span() const override { return { _t0, _tf }; } private: - double _t0{ 0 }, _tf{ 0 }; // start and end times - double _q0{ 0 }, _A{ 0 }, _f{ 0 }, _phi{ 0 }; // position offset, amplitude, frequency (Hz), phase (rad) + Scalar _t0{ Scalar(0) }, _tf{ Scalar(0) }; // start and end times + Scalar _q0{ Scalar(0) }, _A{ Scalar(0) }, _f{ Scalar(0) }, _phi{ Scalar(0) }; // position offset, amplitude, frequency (Hz), phase (rad) }; } // namespace control \ No newline at end of file diff --git a/PxMLib/MathLib/include/control/TrajectoryTypes.h b/PxMLib/MathLib/include/control/TrajectoryTypes.h index 9bd6a0ff..d7ec2a7a 100644 --- a/PxMLib/MathLib/include/control/TrajectoryTypes.h +++ b/PxMLib/MathLib/include/control/TrajectoryTypes.h @@ -1,19 +1,21 @@ +// PxM/ #pragma once -#include "MathLibAPI.h" -#include "core/Types.h" +#include using namespace mathlib; namespace control { + template struct TrajState { - double q = 0.0; // Desired position at this trajectory point - double qd = 0.0; // Desired velocity at this trajectory point - double qdd = 0.0; // Desired acceleration at this trajectory point + Scalar q = Scalar(0); // Desired position at this trajectory point + Scalar qd = Scalar(0); // Desired velocity at this trajectory point + Scalar qdd = Scalar(0); // Desired acceleration at this trajectory point }; + template struct TrajTimeSpan { - double t0 = 0.0; // Start time of the trajectory segment - double tf = 0.0; // End time of the trajectory segment + Scalar t0 = Scalar(0); // Start time of the trajectory segment + Scalar tf = Scalar(0); // End time of the trajectory segment }; } // namespace control \ No newline at end of file diff --git a/PxMLib/MathLib/include/control/TrapezoidTrajectory.h b/PxMLib/MathLib/include/control/TrapezoidTrajectory.h index 38a8ae4b..9efb50f8 100644 --- a/PxMLib/MathLib/include/control/TrapezoidTrajectory.h +++ b/PxMLib/MathLib/include/control/TrapezoidTrajectory.h @@ -1,28 +1,30 @@ #pragma once -#include "MathLibAPI.h" +#include #include "IJointTrajectory.h" using namespace mathlib; namespace control { + template class TrapezoidTrajectory : public IJointTrajectory { public: - TrapezoidTrajectory(double t0, double q0, double q1, double vMax, double aMax) - : _t0(t0), _q0(q0), _q1(q1), _vMax(std::abs(vMax)), _aMax(std::abs(aMax)) { - const double dq = _q1 - _q0; // total displacement - _sgn = (dq >= 0) ? 1.0 : -1.0; // direction sign - _D = std::abs(dq); // total distance - - if (_D <= 0.0 || _vMax <= 0.0 || _aMax <= 0.0) { - _ta = _tc = _td = 0.0; + template + TrapezoidTrajectory(Scalar t0, Scalar q0, Scalar q1, Scalar vMax, Scalar aMax) + : _t0(t0), _q0(q0), _q1(q1), _vMax(mathlib::abs(vMax)), _aMax(mathlib::abs(aMax)) { + const Scalar dq = _q1 - _q0; // total displacement + _sgn = mathlib::sgn(dq); // direction signum + _D = abs(dq); // total distance + + if (_D <= Scalar(0) || _vMax <= Scalar(0) || _aMax <= Scalar(0)) { + _ta = _tc = _td = Scalar(0); _tf = _t0; _triangular = true; return; } - const double ta_full = _vMax / _aMax; // time to reach max velocity - const double D_min = (_vMax * _vMax) / _aMax; // distance during accel and decel + const Scalar ta_full = _vMax / _aMax; // time to reach max velocity + const Scalar D_min = (_vMax * _vMax) / _aMax; // distance during accel and decel if (_D >= D_min) { _triangular = false; // trapezoidal profile @@ -32,9 +34,9 @@ namespace control { } else { _triangular = true; // triangular profile - _ta = std::sqrt(_D / _aMax); + _ta = sqrt(_D / _aMax); _td = _ta; - _tc = 0.0; + _tc = Scalar(0); _vp = _aMax * _ta; // peak velocity } @@ -44,60 +46,64 @@ namespace control { _tf = _t3; // final time } - TrajState eval(double t) const override { - if (t <= _t0) { return { _q0, 0.0, 0.0 }; } - if (t >= _tf) { return { _q1, 0.0, 0.0 }; } + template + inline TrajState eval(Scalar t) const { + if (t <= _t0) { return { _q0, Scalar(0), Scalar(0) }; } + if (t >= _tf) { return { _q1, Scalar(0), Scalar(0) }; } - const double tau = t - _t0; // time since start - const double a = _aMax * _sgn; // acceleration - const double vp = _triangular ? _vp : _vMax * _sgn; // peak velocity + const Scalar tau = t - _t0; // time since start + const Scalar a = _aMax * _sgn; // acceleration + const Scalar vp = _triangular ? _vp : _vMax * _sgn; // peak velocity // Phase 1: Acceleration [_t0, _t1) if (t < _t1) { - const double q = _q0 + 0.5 * a * tau * tau; - const double qd = a * tau; - const double qdd = a; + const Scalar q = _q0 + Scalar(0.5) * a * tau * tau; + const Scalar qd = a * tau; + const Scalar qdd = a; return { q, qd, qdd }; } // Precompute distance - const double qa = _q0 + 0.5 * a * _ta * _ta; // position at end of accel phase + const Scalar qa = _q0 + Scalar(0.5) * a * _ta * _ta; // position at end of accel phase // Phase 2: Cruise [_t1, _t2) if (t < _t2) { - const double q = qa + vp * (tau - _ta); - const double qd = vp; - const double qdd = 0.0; + const Scalar q = qa + vp * (tau - _ta); + const Scalar qd = vp; + const Scalar qdd = Scalar(0); return { q, qd, qdd }; } // Phase 3: Deceleration [_t2, _t3] { - const double td = tau - _ta - _tc; - const double q = qa + vp * _tc + vp * td - 0.5 * a * td * td; - const double qd = vp - a * td; - const double qdd = -a; + const Scalar td = tau - _ta - _tc; + const Scalar q = qa + vp * _tc + vp * td - Scalar(0.5) * a * td * td; + const Scalar qd = vp - a * td; + const Scalar qdd = -a; return { q, qd, qdd }; } } - - TrajTimeSpan span() const override { return { _t0, _tf }; } + // Get the time span of the trajectory + inline TrajState eval(double t) const override { return eval(t); } + + // Get the time span of the trajectory + TrajTimeSpan span() const override { return { _t0, _tf }; } private: - double _t0{ 0 }, _tf{ 0 }; // start and end times - double _q0{ 0 }, _q1{ 0 }; // start and end positions - double _sgn{ 1 }; // direction sign + Scalar _t0{ Scalar(0) }, _tf{ Scalar(0) }; // start and end times + Scalar _q0{ Scalar(0) }, _q1{ Scalar(0) }; // start and end positions + Scalar _sgn{ Scalar(1) }; // direction sign - double _vMax{ 0 }, _aMax{ 0 }; // max velocity and acceleration - double _ta{ 0 }, _tc{ 0 }, _td{ 0 }; // accel, cruise, decel times - double _qa{ 0 }, _qc{ 0 }, _qd{ 0 }; // positions at phase transitions + Scalar _vMax{ Scalar(0) }, _aMax{ Scalar(0) }; // max velocity and acceleration + Scalar _ta{ Scalar(0) }, _tc{ Scalar(0) }, _td{ Scalar(0) }; // accel, cruise, decel times + Scalar _qa{ Scalar(0) }, _qc{ Scalar(0) }, _qd{ Scalar(0) }; // positions at phase transitions - double _D{ 0 }; // total distance + Scalar _D{ Scalar(0) }; // total distance bool _triangular{ false }; // flag for triangular profile // key times - double _t1{ 0 }, _t2{ 0 }, _t3{ 0 }; // phase transition times + Scalar _t1{ Scalar(0) }, _t2{ Scalar(0) }, _t3{ Scalar(0) }; // phase transition times - double _vp{ 0 }; // peak velocity for triangular profile + Scalar _vp{ Scalar(0) }; // peak velocity for triangular profile }; } // namespace control \ No newline at end of file diff --git a/PxMLib/MathLib/include/core/DualNumbers.h b/PxMLib/MathLib/include/core/DualNumbers.h index f1d454c3..0422ed4a 100644 --- a/PxMLib/MathLib/include/core/DualNumbers.h +++ b/PxMLib/MathLib/include/core/DualNumbers.h @@ -1,13 +1,16 @@ // PxM/MathLib M_DualNumbers.h #pragma once -#include +#include "core/Types_tpl.h" +#include "core/ScalarStdFunc.h" +#include "core/ScalarScaling.h" #define EIGEN_DONT_VECTORIZE #define EIGEN_DISABLE_UNALIGNED_ARRAY_ASSERT #include +#include #include #include #include @@ -229,8 +232,8 @@ namespace mathlib { for (size_t i = 0; i < NVar; ++i) { out.dual[i] = Scalar(0); } return out; } - out.real = pow(a.real, n); - for (size_t i = 0; i < NVar; ++i) { out.dual[i] = n * pow(a.real, n - Scalar(1)) * a.dual[i]; } + out.real = std::pow(a.real, n); + for (size_t i = 0; i < NVar; ++i) { out.dual[i] = n * std::pow(a.real, n - Scalar(1)) * a.dual[i]; } return out; } // Square root function @@ -238,7 +241,7 @@ namespace mathlib { inline DualNumber_T sqrt(const DualNumber_T& a) { if (a.real < Scalar(0)) { throw std::runtime_error("sqrt() domain error for DualNumber_T"); } DualNumber_T out; - const Scalar sqrtReal = sqrt(a.real); + const Scalar sqrtReal = std::sqrt(a.real); out.real = sqrtReal; if (sqrtReal == Scalar(0)) { for (size_t i = 0; i < NVar; ++i) { out.dual[i] = Scalar(0); } @@ -252,39 +255,68 @@ namespace mathlib { template inline DualNumber_T sin(const DualNumber_T& a) { DualNumber_T out; - out.real = sin(a.real); - for (size_t i = 0; i < NVar; ++i) { out.dual[i] = cos(a.real) * a.dual[i]; } + out.real = std::sin(a.real); + for (size_t i = 0; i < NVar; ++i) { out.dual[i] = std::cos(a.real) * a.dual[i]; } + return out; + } + // Arcsine function + template + inline DualNumber_T asin(const DualNumber_T& a) { + if (a.real < Scalar(-1) || a.real > Scalar(1)) { throw std::runtime_error("asin() domain error for DualNumber_T"); } + DualNumber_T out; + out.real = std::asin(a.real); + const Scalar invSqrt = Scalar(1) / std::sqrt(Scalar(1) - a.real * a.real); + for (size_t i = 0; i < NVar; ++i) { out.dual[i] = invSqrt * a.dual[i]; } return out; } // Cosine function template inline DualNumber_T cos(const DualNumber_T& a) { DualNumber_T out; - out.real = cos(a.real); - for (size_t i = 0; i < NVar; ++i) { out.dual[i] = -sin(a.real) * a.dual[i]; } + out.real = std::cos(a.real); + for (size_t i = 0; i < NVar; ++i) { out.dual[i] = -std::sin(a.real) * a.dual[i]; } + return out; + } + // Arccosine function + template + inline DualNumber_T acos(const DualNumber_T& a) { + if (a.real < Scalar(-1) || a.real > Scalar(1)) { throw std::runtime_error("acos() domain error for DualNumber_T"); } + DualNumber_T out; + out.real = std::acos(a.real); + const Scalar invSqrt = Scalar(1) / std::sqrt(Scalar(1) - a.real * a.real); + for (size_t i = 0; i < NVar; ++i) { out.dual[i] = -invSqrt * a.dual[i]; } return out; } // Tangent function template inline DualNumber_T tan(const DualNumber_T& a) { DualNumber_T out; - out.real = tan(a.real); - for (size_t i = 0; i < NVar; ++i) { out.dual[i] = a.dual[i] / (cos(a.real) * cos(a.real)); } + out.real = std::tan(a.real); + for (size_t i = 0; i < NVar; ++i) { out.dual[i] = a.dual[i] / (std::cos(a.real) * std::cos(a.real)); } return out; } // Arctangent function template inline DualNumber_T atan(const DualNumber_T& a) { DualNumber_T out; - out.real = atan(a.real); + out.real = std::atan(a.real); for (size_t i = 0; i < NVar; ++i) { out.dual[i] = a.dual[i] / (Scalar(1) + a.real * a.real); } return out; } + // Arctangent squared function + template + inline DualNumber_T atan2(const DualNumber_T& y, const DualNumber_T& x) { + DualNumber_T out; + out.real = std::atan2(y.real, x.real); + Scalar denom = x.real * x.real + y.real * y.real; + for (size_t i = 0; i < NVar; ++i) { out.dual[i] = (x.real * y.dual[i] - y.real * x.dual[i]) / denom; } + return out; + } // Hyperbolic tangent function (tanh) template inline DualNumber_T tanh(const DualNumber_T& a) { DualNumber_T out; - out.real = tanh(a.real); + out.real = std::tanh(a.real); for (size_t i = 0; i < NVar; ++i) { out.dual[i] = a.dual[i] * (Scalar(1) - out.real * out.real); } return out; } @@ -292,7 +324,7 @@ namespace mathlib { template inline DualNumber_T exp(const DualNumber_T& a) { DualNumber_T out; - out.real = exp(a.real); + out.real = std::exp(a.real); for (size_t i = 0; i < NVar; ++i) { out.dual[i] = out.real * a.dual[i]; } return out; } @@ -301,7 +333,7 @@ namespace mathlib { inline DualNumber_T log(const DualNumber_T& a) { if (a.real <= Scalar(0)) { throw std::runtime_error("log() domain error for DualNumber_T"); } DualNumber_T out; - out.real = log(a.real); + out.real = std::log(a.real); for (size_t i = 0; i < NVar; ++i) { out.dual[i] = a.dual[i] / a.real; } return out; } @@ -310,23 +342,20 @@ namespace mathlib { // Smoothing // ----- - // Smooth step function for smooth interpolation between 0 and 1 - template - inline Scalar smoothStep(const Scalar& x) { return x * x * (Scalar(3) - Scalar(2) * x); } // Smooth step function for dual numbers (applies smooth step to the real part, scales dual part by the derivative of the smooth step) template inline DualNumber_T smoothStep(const DualNumber_T& x) { DualNumber_T out; - out.real = smoothStep(x.real); + out.real = mathlib::smoothStep(x.real); Scalar derivative = Scalar(6) * x.real * (Scalar(1) - x.real); // Derivative of smooth step with respect to x for (size_t i = 0; i < NVar; ++i) { out.dual[i] = derivative * x.dual[i]; } return out; } // LogSumExp Smooth Max (DualNumber) template - inline Scalar LSE_smoothMax(const DualNumber_T& a, const DualNumber_T& b, const Scalar& k = Scalar(15)) { + inline DualNumber_TLSE_smoothMax(const DualNumber_T& a, const DualNumber_T& b, const Scalar& k = Scalar(15)) { Scalar m = (a.real > b.real) ? a.real : b.real; - return DualNumber_T(m) + log(exp(k * (a - m)) + exp(k * (b - m))) / k; + return DualNumber_T(m) + mathlib::log(mathlib::exp(k * (a - m)) + mathlib::exp(k * (b - m))) / k; } // ----- @@ -401,6 +430,13 @@ namespace mathlib { inline DualNumber_T numeric_limits_infinity() { return DualNumber_T(std::numeric_limits::infinity(), std::array()); } + // Function to check if a DualNumber_T is finite (real part is finite and all dual parts are finite) + template + inline bool isfinite(const DualNumber_T& a) { + if (!mathlib::isfinite(a.real)) { return false; } + for (size_t i = 0; i < NVar; ++i) { if (!mathlib::isfinite(a.dual[i])) { return false; } } + return true; + } // Absolute value function that simply negates the dual number if the real part is negative (this is less smooth but can be more efficient and avoids issues with zero) template inline DualNumber_T abs(const DualNumber_T& a) { return abs(a.real); } @@ -413,8 +449,8 @@ namespace mathlib { for (size_t i = 0; i < NVar; ++i) { out.dual[i] = Scalar(0); } } else { - out.real = abs(a.real); - Scalar sign = sgn(a.real); + out.real = mathlib::abs(a.real); + Scalar sign = mathlib::sgn(a.real); for (size_t i = 0; i < NVar; ++i) { out.dual[i] = sign * a.dual[i]; } } return out; @@ -509,17 +545,6 @@ namespace mathlib { os << "] }"; return os; } - // Check if the real part and all dual parts are finite - template - inline bool isfinite(const DualNumber_T& x) { - if (!isfinite(x.real)) { return false; } - - for (size_t i = 0; i < NVar; ++i) { - if (!isfinite(x.dual[i])) { return false; } - } - - return true; - } // ----- // Eigen Matrix Interactions diff --git a/PxMLib/MathLib/include/core/MathLib.h b/PxMLib/MathLib/include/core/MathLib.h index 1a15311d..ad68f67d 100644 --- a/PxMLib/MathLib/include/core/MathLib.h +++ b/PxMLib/MathLib/include/core/MathLib.h @@ -1,10 +1,13 @@ // PxM/MathLib MathLib.h #pragma once + #include "MathLibAPI.h" +#include "core/constants.h" #include "core/Types.h" #include "core/ScalarStdFunc.h" +#include "core/DualNumbers.h" #include "core/ScalarScaling.h" +#include "core/VectorUtils.h" #include "core/ScalarTransforms.h" -#include "core/constants.h" -#include "Core/Utils.h" \ No newline at end of file +#include "core/Utils.h" \ No newline at end of file diff --git a/PxMLib/MathLib/include/core/ScalarScaling.h b/PxMLib/MathLib/include/core/ScalarScaling.h index 1cf1861e..a93ba17e 100644 --- a/PxMLib/MathLib/include/core/ScalarScaling.h +++ b/PxMLib/MathLib/include/core/ScalarScaling.h @@ -5,11 +5,14 @@ #include "core/Types_tpl.h" namespace mathlib { + // Smooth step function for smooth interpolation between 0 and 1 + template + inline Scalar smoothStep(const Scalar& x) { return x * x * (Scalar(3) - Scalar(2) * x); } // LogSumExp Smooth Max (Scalar) template inline Scalar LSE_smoothMax(const Scalar& a, const Scalar& b, const Scalar& k = Scalar(10)) { Scalar m = (a > b) ? a : b; - return m + log(exp(k * (a - m)) + exp(k * (b - m))) / k; + return m + mathlib::log(mathlib::exp(k * (a - m)) + mathlib::exp(k * (b - m))) / k; } // Maximum value function (Scalar) @@ -19,21 +22,7 @@ namespace mathlib { template inline Scalar min(const Scalar& a, const Scalar& b) { return (a < b) ? a : b; } - // Is finite function (Scalar) - template - inline bool isfinite(const Scalar& a) { return std::isfinite(a); } - - template - inline auto safeNorm(const Eigen::MatrixBase& v) { - using Scalar = typename Derived::Scalar; - return mathlib::sqrt(v.dot(v)); - } - template - inline typename Derived::PlainObject safeNormalised(const Eigen::MatrixBase& v) { - using Scalar = typename Derived::Scalar; - using Plain = typename Derived::PlainObject; - Scalar n = safeNorm(v); - if (n > Scalar(0)) { return (v / n).eval(); } - return Plain::Zero(v.rows(), v.cols()); - } + // Check if a scalar is finite -> could use std::isfinite, but this avoids potential issues with non-standard types + inline bool isfinite(double x) { return std::isfinite(x); } + inline bool isfinite(float x) { return std::isfinite(x); } } \ No newline at end of file diff --git a/PxMLib/MathLib/include/core/ScalarStdFunc.h b/PxMLib/MathLib/include/core/ScalarStdFunc.h index 598bfa63..cb035222 100644 --- a/PxMLib/MathLib/include/core/ScalarStdFunc.h +++ b/PxMLib/MathLib/include/core/ScalarStdFunc.h @@ -3,55 +3,63 @@ #include "MathLibAPI.h" #include "core/Types_tpl.h" +#include "core/constants.h" +#include #include namespace mathlib { // Power function (Scalar) - template + template, int> = 0> inline Scalar pow(const Scalar& x, const Scalar& n) { return std::pow(x, n); } // Sqrt function (Scalar) - template + template, int> = 0> inline Scalar sqrt(const Scalar& x) { return std::sqrt(x); } // Sine function (Scalar) - template + template, int> = 0> inline Scalar sin(const Scalar& x) { return std::sin(x); } // Arcsine function (Scalar) - template + template, int> = 0> inline Scalar asin(const Scalar& x) { return std::asin(x); } // Cosine function (Scalar) - template + template, int> = 0> inline Scalar cos(const Scalar& x) { return std::cos(x); } // Arccosine function (Scalar) - template + template, int> = 0> inline Scalar acos(const Scalar& x) { return std::acos(x); } // Tangent function (Scalar) - template + template, int> = 0> inline Scalar tan(const Scalar& x) { return std::tan(x); } // Arctangent function (Scalar) - template + template, int> = 0> inline Scalar atan(const Scalar& x) { return std::atan(x); } // Arctangent sqaured function (Scalar) - template + template, int> = 0> inline Scalar atan2(const Scalar& y, const Scalar& x) { return std::atan2(y, x); } - // Hyperbolic Tangnet (Scalar) - template + // Hyperbolic Tangnet function (Scalar) + template, int> = 0> inline Scalar tanh(const Scalar& x) { return std::tanh(x); } // Exponential function (Scalar) - template + template, int> = 0> inline Scalar exp(const Scalar& x) { return std::exp(x); } // Logarithm function (Scalar) - template + template, int> = 0> inline Scalar log(const Scalar& x) { return std::log(x); } // Absolute value function (Scalar) - template + template, int> = 0> inline Scalar abs(const Scalar& a) { return (a < Scalar(0)) ? -a : a; } // Absolute value function squared (Scalar) - template + template, int> = 0> inline Scalar abs2(const Scalar& a) { return a * a; } // Absolute value with tolerance (Scalar, overload) - template - inline Scalar abs(const Scalar& a, const Scalar& tol) { return (abs(a) < tol) ? Scalar(0) : a; } + template, int> = 0> + inline Scalar abs(const Scalar& a, const Scalar& tol) { return (mathlib::abs(a) < tol) ? Scalar(0) : a; } // Signum function (Scalar) - template + template, int> = 0> inline Scalar sgn(const Scalar& a) { return (a > Scalar(0)) - (a < Scalar(0)); } + // Normalisation function (Scalar) + template, int> = 0> + inline Scalar norm(const Scalar& a) { return (a == Scalar(0)) ? Scalar(0) : mathlib::sgn(a); } + // Squared normalisation function (Scalar) + template, int> = 0> + inline Scalar norm2(const Scalar& a) { return (a == Scalar(0)) ? Scalar(0) : Scalar(1); } } // namespace mathlib \ No newline at end of file diff --git a/PxMLib/MathLib/include/core/Utils.h b/PxMLib/MathLib/include/core/Utils.h index a9face88..78b95564 100644 --- a/PxMLib/MathLib/include/core/Utils.h +++ b/PxMLib/MathLib/include/core/Utils.h @@ -2,6 +2,8 @@ #include "MathLibAPI.h" #include "core/Types.h" +#include "core/ScalarStdFunc.h" +#include "core/ScalarScaling.h" #include "core/constants.h" using namespace mathlib; @@ -82,9 +84,9 @@ namespace mathlib { // Natural frequency of a mass-spring system template inline Scalar natural_freq(Scalar k_p, Scalar I) { - if (!isfinite(k_p) || !isfinite(I)) { return std::numeric_limits::quiet_NaN(); } + if (!mathlib::isfinite(k_p) || !mathlib::isfinite(I)) { return std::numeric_limits::quiet_NaN(); } if (k_p < Scalar(0) || I <= Scalar(0)) { return std::numeric_limits::quiet_NaN(); } - return sqrt(k_p / I); + return mathlib::sqrt(k_p / I); } // Natural frequency of a mass-spring system (double overload) inline double natural_freq(double k_p, double I) { return natural_freq(k_p, I); } @@ -92,7 +94,7 @@ namespace mathlib { // Damping ratio of a mass-spring-damper system template inline Scalar damping_ratio(Scalar k_d, Scalar I, Scalar k_p) { - if (!isfinite(k_p) || !isfinite(k_d) || !isfinite(I)) { return std::numeric_limits::quiet_NaN(); } + if (!mathlib::isfinite(k_p) || !mathlib::isfinite(k_d) || !mathlib::isfinite(I)) { return std::numeric_limits::quiet_NaN(); } if (k_p < Scalar(0) || I <= Scalar(0)) { return std::numeric_limits::quiet_NaN(); } Scalar w_n = natural_freq(k_p, I); return k_d / (Scalar(2) * I * w_n); // damping ratio - zeta diff --git a/PxMLib/MathLib/include/core/VectorUtils.h b/PxMLib/MathLib/include/core/VectorUtils.h new file mode 100644 index 00000000..66361b03 --- /dev/null +++ b/PxMLib/MathLib/include/core/VectorUtils.h @@ -0,0 +1,22 @@ +// PxM/MathLib VectorUtils.h +#pragma once + +#include "core/ScalarScaling.h" +#include "core/DualNumbers.h" +#include + +namespace mathlib { + template + inline auto safeNorm(const Eigen::MatrixBase& v) { + using Scalar = typename Derived::Scalar; + return mathlib::sqrt(v.dot(v)); + } + template + inline typename Derived::PlainObject safeNormalised(const Eigen::MatrixBase& v) { + using Scalar = typename Derived::Scalar; + using Plain = typename Derived::PlainObject; + Scalar n = safeNorm(v); + if (n > Scalar(0)) { return (v / n).eval(); } + return Plain::Zero(v.rows(), v.cols()); + } +} \ No newline at end of file diff --git a/PxMLib/MathLib/include/dynamics/MassMatrix.h b/PxMLib/MathLib/include/dynamics/MassMatrix.h deleted file mode 100644 index 07282cc7..00000000 --- a/PxMLib/MathLib/include/dynamics/MassMatrix.h +++ /dev/null @@ -1,23 +0,0 @@ -#pragma once - -#include "MathLibAPI.h" -#include "core/Types.h" -#include "kinematics/DH_Params.h" -#include "dynamics/RigidBody.h" - -using namespace mathlib; -using namespace kinematics; - -namespace dynamics { - class MATHLIB_API MassMatrix { - public: - /// - /// Computes the mass matrix of a robotic manipulator using the provided DH parameters, link inertias, and joint angles. - /// - /// A vector of DH_Param structures representing the Denavit-Hartenberg parameters of each link. - /// A vector of LinkInertia structures representing the inertia properties of each link. - /// A vector of joint angles. - /// A MatX representing the mass matrix of the manipulator. - MatX massMatrix(const std::vector& dh_p, const std::vector& inertias, const VecX& q); - }; -} \ No newline at end of file diff --git a/PxMLib/MathLib/include/dynamics/Nonlinear_Terms.h b/PxMLib/MathLib/include/dynamics/Nonlinear_Terms.h deleted file mode 100644 index 18ae22de..00000000 --- a/PxMLib/MathLib/include/dynamics/Nonlinear_Terms.h +++ /dev/null @@ -1,34 +0,0 @@ -#pragma once - -#include "MathLibAPI.h" -#include "core/Types.h" -#include "kinematics/DH_Params.h" -#include "dynamics/RigidBody.h" - -using namespace mathlib; -using namespace kinematics; - -namespace dynamics { - class MATHLIB_API Nonlinear_Terms { - public: - /// - /// Computes the nonlinear terms (Coriolis and centrifugal forces) of a robotic manipulator. - /// - /// A vector of DH_Param structures representing the Denavit-Hartenberg parameters of each link. - /// A vector of LinkInertia structures representing the inertia properties of each link. - /// A vector of joint angles. - /// A vector of joint velocities. - /// A VecX representing the nonlinear terms of the manipulator. - VecX coriolisCentrifugal(const std::vector& dh_p, const std::vector& inertias, const VecX& q, const VecX& q_dot); - - /// - /// Computes the gravity torque vector of a robotic manipulator. - /// - /// A vector of DH_Param structures representing the Denavit-Hartenberg parameters of each link. - /// A vector of LinkInertia structures representing the inertia properties of each link. - /// A vector of joint angles. - /// A Vec3 representing the gravity vector. - /// A VecX representing the gravity torque vector of the manipulator. - VecX gravityTorque(const std::vector& dh_p, const std::vector& inertias, const VecX& q, const Vec3& gravity = Vec3{0,0,0}); - }; -} \ No newline at end of file diff --git a/PxMLib/MathLib/include/dynamics/RigidBody.h b/PxMLib/MathLib/include/dynamics/RigidBody.h index a2cf4ce2..a2c119ec 100644 --- a/PxMLib/MathLib/include/dynamics/RigidBody.h +++ b/PxMLib/MathLib/include/dynamics/RigidBody.h @@ -1,14 +1,14 @@ #pragma once -#include "MathLibAPI.h" -#include "core/Types.h" +#include using namespace mathlib; namespace dynamics { - struct MATHLIB_API LinkInertia { - double mass; - Vec3 com; // center of mass - Mat3 inertia; // inertia tensor + template + struct LinkInertia { + Scalar mass; + Vec3_T com; // center of mass + Mat3_T inertia; // inertia tensor }; } \ No newline at end of file diff --git a/PxMLib/MathLib/include/dynamics/StateSpace.h b/PxMLib/MathLib/include/dynamics/StateSpace.h deleted file mode 100644 index 1a0234c2..00000000 --- a/PxMLib/MathLib/include/dynamics/StateSpace.h +++ /dev/null @@ -1,26 +0,0 @@ -#pragma once - -#include "MathLibAPI.h" -#include "core/Types.h" -#include "kinematics/DH_Params.h" -#include "dynamics/RigidBody.h" - -using namespace mathlib; -using namespace kinematics; - -namespace dynamics { - class MATHLIB_API StateSpace { - public: - /// - /// Computes the dynamic right-hand side of the robotic manipulator's equations of motion. - /// - /// A vector of DH_Param structures representing the Denavit-Hartenberg parameters of each link. - /// A vector of LinkInertia structures representing the inertia properties of each link. - /// A vector of joint angles. - /// A vector of joint velocities. - /// A vector of joint torques. - /// A Vec3 representing the gravity vector. - /// A VecX representing the dynamic right-hand side of the equations of motion. - VecX dynamicRHS(const std::vector& dh_p, const std::vector& inertias, const VecX& q, const VecX& q_dot, const VecX& tau, const Vec3& gravity = Vec3{ 0,0,0 }); - }; -} \ No newline at end of file diff --git a/PxMLib/MathLib/include/integrators/Integration.h b/PxMLib/MathLib/include/integrators/Integration.h deleted file mode 100644 index 718a0527..00000000 --- a/PxMLib/MathLib/include/integrators/Integration.h +++ /dev/null @@ -1,31 +0,0 @@ -#pragma once - -#include "MathLibAPI.h" - -#include "core/Types.h" -#include - -using namespace mathlib; - -namespace integration { - class MATHLIB_API integrator { - public: - // General ODE integrator function - VecX integrate_ODE( - const std::function& f, - double y0, double t0, double t_final, double dt, - const std::function&, double, const double&, double)>& step_function - ); - - // General PDE integrator function - VecX integrate_PDE( - const std::function& f, - const VecX& u0, double t0, double t_final, double dt, - const std::function&, double, const VecX&, double)>& step_function - ); - - private: - // Helper function to append a value to an Eigen::VecX (originally tried to use push_back, but Eigen doesn't support it) - static void append(VecX& v, double value); - }; -} \ No newline at end of file diff --git a/PxMLib/MathLib/include/integrators/auto_diff_integrators.inl b/PxMLib/MathLib/include/integrators/auto_diff_integrators.inl index d4a1f43d..58cc3e88 100644 --- a/PxMLib/MathLib/include/integrators/auto_diff_integrators.inl +++ b/PxMLib/MathLib/include/integrators/auto_diff_integrators.inl @@ -95,7 +95,6 @@ namespace integration { ) { const Eigen::Index n = x.size(); using Real = typename mathlib::DualTraits::BaseScalar; - using std::sqrt; // Coefficients for the 2-stage Gauss-Legendre method (4th order) mathlib::VecX_T c(2); @@ -189,14 +188,10 @@ namespace integration { ) { const Eigen::Index n = x.size(); using Real = typename mathlib::DualTraits::BaseScalar; - using std::sqrt; - using std::pow; - using std::abs; - using std::max; if (tol < Real(0)) { Real dt_r = mathlib::real(dt); - Real tol_r = max(Real(1e-12), Real(1e-2) * pow(dt_r, Real(7))); + Real tol_r = max(Real(1e-12), Real(1e-2) * pow(dt_r, Real(7))); tol = Real(tol_r); } diff --git a/PxMLib/MathLib/include/integrators/explicit_integrators.inl b/PxMLib/MathLib/include/integrators/explicit_integrators.inl index f3eac1d3..8df06d4b 100644 --- a/PxMLib/MathLib/include/integrators/explicit_integrators.inl +++ b/PxMLib/MathLib/include/integrators/explicit_integrators.inl @@ -78,63 +78,63 @@ namespace integration { Scalar rtol, Scalar atol ) { - const Scalar safety = 0.9; // safety factor to prevent aggressive step size changes - const Scalar fac_min = 0.2; // minimum factor for reducing step size - const Scalar fac_max = 5.0; // maximum factor for increasing step size - const Scalar h_min = 1e-10; // minimum allowed step size - const Scalar h_max = 1.0; // maximum allowed step size + const Scalar safety = Scalar(0.9); // safety factor to prevent aggressive step size changes + const Scalar fac_min = Scalar(0.2); // minimum factor for reducing step size + const Scalar fac_max = Scalar(5.0); // maximum factor for increasing step size + const Scalar h_min = Scalar(1e-10); // minimum allowed step size + const Scalar h_max = Scalar(1.0); // maximum allowed step size - dt = std::clamp(dt, h_min, h_max); + dt = clamp(dt, h_min, h_max); // Try up to 25 attempts to find an acceptable step size for (int attempt = 0; attempt < 25; ++attempt) { // Butcher tableau for Dormand-Prince method (7 stages, 5th order, SHOULD probably be precomputed as static constants, in a matrix or equiv) // Coefficients for error estimation - const Scalar c2 = 1.0 / 5.0; - const Scalar c3 = 3.0 / 10.0; - const Scalar c4 = 4.0 / 5.0; - const Scalar c5 = 8.0 / 9.0; - const Scalar c6 = 1.0; - const Scalar c7 = 1.0; + const Scalar c2 = Scalar(1) / Scalar(5); + const Scalar c3 = Scalar(3) / Scalar(10); + const Scalar c4 = Scalar(4) / Scalar(5); + const Scalar c5 = Scalar(8) / Scalar(9); + const Scalar c6 = Scalar(1); + const Scalar c7 = Scalar(1); // Dormand-Prince coefficients - const Scalar a21 = 1.0 / 5.0; - const Scalar a31 = 3.0 / 40.0; - const Scalar a32 = 9.0 / 40.0; - const Scalar a41 = 44.0 / 45.0; - const Scalar a42 = -56.0 / 15.0; - const Scalar a43 = 32.0 / 9.0; - const Scalar a51 = 19372.0 / 6561.0; - const Scalar a52 = -25360.0 / 2187.0; - const Scalar a53 = 64448.0 / 6561.0; - const Scalar a54 = -212.0 / 729.0; - const Scalar a61 = 9017.0 / 3168.0; - const Scalar a62 = -355.0 / 33.0; - const Scalar a63 = 46732.0 / 5247.0; - const Scalar a64 = 49.0 / 176.0; - const Scalar a65 = -5103.0 / 18656.0; - const Scalar a71 = 35.0 / 384.0; - const Scalar a72 = 0.0; - const Scalar a73 = 500.0 / 1113.0; - const Scalar a74 = 125.0 / 192.0; - const Scalar a75 = -2187.0 / 6784.0; - const Scalar a76 = 11.0 / 84.0; + const Scalar a21 = Scalar(1) / Scalar(5); + const Scalar a31 = Scalar(3) / Scalar(40); + const Scalar a32 = Scalar(9) / Scalar(40); + const Scalar a41 = Scalar(44) / Scalar(45); + const Scalar a42 = Scalar(-56) / Scalar(15); + const Scalar a43 = Scalar(32) / Scalar(9); + const Scalar a51 = Scalar(19372) / Scalar(6561); + const Scalar a52 = Scalar(-25360) / Scalar(2187); + const Scalar a53 = Scalar(64448) / Scalar(6561); + const Scalar a54 = Scalar(-212) / Scalar(729); + const Scalar a61 = Scalar(9017) / Scalar(3168); + const Scalar a62 = Scalar(-355) / Scalar(33); + const Scalar a63 = Scalar(46732) / Scalar(5247.0); + const Scalar a64 = Scalar(49) / Scalar(176); + const Scalar a65 = Scalar(-5103) / Scalar(18656); + const Scalar a71 = Scalar(35) / Scalar(384); + const Scalar a72 = Scalar(0); + const Scalar a73 = Scalar(500) / Scalar(1113); + const Scalar a74 = Scalar(125) / Scalar(192); + const Scalar a75 = Scalar(-2187) / Scalar(6784); + const Scalar a76 = Scalar(11) / Scalar(84); // Weights for 4th and 5th order estimates - const Scalar b1 = 35.0 / 384.0; - const Scalar b2 = 0.0; - const Scalar b3 = 500.0 / 1113.0; - const Scalar b4 = 125.0 / 192.0; - const Scalar b5 = -2187.0 / 6784.0; - const Scalar b6 = 11.0 / 84.0; - const Scalar b1s = 5179.0 / 57600.0; - const Scalar b2s = 0.0; - const Scalar b3s = 7571.0 / 16695.0; - const Scalar b4s = 393.0 / 640.0; - const Scalar b5s = -92097.0 / 339200.0; - const Scalar b6s = 187.0 / 2100.0; - const Scalar b7s = 1.0 / 40.0; + const Scalar b1 = Scalar(35) / Scalar(384); + const Scalar b2 = Scalar(0); + const Scalar b3 = Scalar(500) / Scalar(1113); + const Scalar b4 = Scalar(125) / Scalar(192); + const Scalar b5 = Scalar(-2187) / Scalar(6784); + const Scalar b6 = Scalar(11) / Scalar(84); + const Scalar b1s = Scalar(5179) / Scalar(57600); + const Scalar b2s = Scalar(0); + const Scalar b3s = Scalar(7571) / Scalar(16695); + const Scalar b4s = Scalar(393) / Scalar(640); + const Scalar b5s = Scalar(-92097) / Scalar(339200); + const Scalar b6s = Scalar(187) / Scalar(2100); + const Scalar b7s = Scalar(1) / Scalar(40); // Compute the Runge-Kutta stages (DP -> 7 stages) const mathlib::VecX_T k1 = f(t, x); @@ -154,32 +154,31 @@ namespace integration { // Compute the error norm Scalar errNorm = 0.0; for (int i = 0; i < e.size(); ++i) { - Scalar sc = atol + rtol * std::max(std::abs(x(i)), std::abs(y5(i))); + Scalar sc = atol + rtol * max(abs(x(i)), abs(y5(i))); const Scalar r = e(i) / sc; errNorm += r * r; } - Scalar err = std::sqrt(errNorm / e.size()); + Scalar err = sqrt(errNorm / e.size()); // Adaptive step size control // Accept - if (err <= 1.0 && std::isfinite(err)) { + if (err <= Scalar(1) && isfinite(err)) { dt_used = dt; // Store the actual step size used for this step // Update step size for next iteration - const Scalar denom = std::max(err, 1e-10); // prevent division by zero - Scalar fac = safety * std::pow(denom, -0.2); // exponent for 5th order method - fac = std::clamp(fac, fac_min, fac_max); // limit step size change - dt = std::clamp(dt * fac, h_min, h_max); // update step size + const Scalar denom = max(err, Scalar(1e-10)); // prevent division by zero + Scalar fac = safety * pow(denom, Scalar(-0.2)); // exponent for 5th order method + fac = clamp(fac, fac_min, fac_max); // limit step size change + dt = clamp(dt * fac, h_min, h_max); // update step size return y5; } // Reject else { - Scalar denom = (std::isfinite(err) - ? std::max(err, 1e-16) : 1e16); // prevent division by zero & NaN - Scalar fac = safety * std::pow(denom, -0.2); // exponent for 4th order method - fac = std::clamp(fac, fac_min, fac_max); - dt = std::clamp(dt * fac, h_min, h_max); + Scalar denom = (isfinite(err) ? max(err, Scalar(1e-16)) : Scalar(1e16)); // prevent division by zero & NaN + Scalar fac = safety * pow(denom, Scalar(-0.2)); // exponent for 4th order method + fac = clamp(fac, fac_min, fac_max); + dt = clamp(dt * fac, h_min, h_max); } } throw std::runtime_error("RK45 failed to converge after maximum attempts"); diff --git a/PxMLib/MathLib/include/integrators/numerical_integrators.h b/PxMLib/MathLib/include/integrators/numerical_integrators.h index c57eb065..0b7037fb 100644 --- a/PxMLib/MathLib/include/integrators/numerical_integrators.h +++ b/PxMLib/MathLib/include/integrators/numerical_integrators.h @@ -1,11 +1,7 @@ #pragma once // File: numerical_integrators.h // GitHub: SaltyJoss -#include "MathLibAPI.h" - -#include -#include -#include +#include #include #include #include diff --git a/PxMLib/MathLib/include/kinematics/DH_Params.h b/PxMLib/MathLib/include/kinematics/DH_Params.h index 2b19c224..bac4183e 100644 --- a/PxMLib/MathLib/include/kinematics/DH_Params.h +++ b/PxMLib/MathLib/include/kinematics/DH_Params.h @@ -1,7 +1,6 @@ #pragma once -#include "MathLibAPI.h" -#include "core/Types.h" +#include using namespace mathlib; @@ -14,11 +13,12 @@ namespace kinematics { /// /// Denavit-Hartenberg parameters structure /// + template struct DH_Params { - double a; // Link length - double alpha; // Link twist - double d; // Link offset - double theta; // Joint angle + Scalar a; // Link length + Scalar alpha; // Link twist + Scalar d; // Link offset + Scalar theta; // Joint angle JointType_DH type; // Joint type (Revolute or Prismatic) }; @@ -30,7 +30,28 @@ namespace kinematics { /// DH parameters /// Joint variable (angle or displacement) /// Transformation matrix - Mat4 dhTransform(const DH_Params& p, double joint_val); + template + Mat4_T dhTransform(const DH_Params& p, Scalar joint_val) { + Scalar theta = p.theta + joint_val; // Update joint angle with provided joint value + Mat4_T transform = Mat4_T::Identity(); + transform(0, 0) = cos(theta); + transform(0, 1) = -sin(theta) * cos(p.alpha); + transform(0, 2) = sin(theta) * sin(p.alpha); + transform(0, 3) = p.a * cos(theta); + transform(1, 0) = sin(theta); + transform(1, 1) = cos(theta) * cos(p.alpha); + transform(1, 2) = -cos(theta) * sin(p.alpha); + transform(1, 3) = p.a * sin(theta); + transform(2, 0) = Scalar(0); + transform(2, 1) = sin(p.alpha); + transform(2, 2) = cos(p.alpha); + transform(2, 3) = p.d; + transform(3, 0) = Scalar(0); + transform(3, 1) = Scalar(0); + transform(3, 2) = Scalar(0); + transform(3, 3) = Scalar(1); + return transform; + } /// /// Determines whether the specified joint p is revolute. @@ -39,7 +60,8 @@ namespace kinematics { /// /// true if [is joint revolute] [the specified p]; otherwise, false. /// - bool isJointRevolute(const DH_Params& p) const { return p.type == JointType_DH::Revolute; } + template + bool isJointRevolute(const DH_Params& p) const { return p.type == JointType_DH::Revolute; } /// /// Determines whether the specified joint p is prismatic. @@ -48,6 +70,7 @@ namespace kinematics { /// /// true if [is joint prismatic] [the specified p]; otherwise, false. /// - bool isJointPrismatic(const DH_Params& p) const { return p.type == JointType_DH::Prismatic; } + template + bool isJointPrismatic(const DH_Params& p) const { return p.type == JointType_DH::Prismatic; } }; } \ No newline at end of file diff --git a/PxMLib/MathLib/include/kinematics/Forward_Kinematics.h b/PxMLib/MathLib/include/kinematics/Forward_Kinematics.h index 2b625480..c66b6004 100644 --- a/PxMLib/MathLib/include/kinematics/Forward_Kinematics.h +++ b/PxMLib/MathLib/include/kinematics/Forward_Kinematics.h @@ -1,7 +1,6 @@ #pragma once -#include "MathLibAPI.h" -#include "core/Types.h" +#include #include "kinematics/DH_Params.h" #include "kinematics/URDF_Types.h" #include @@ -19,40 +18,42 @@ namespace kinematics { /// Denavit-Hartenberg parameters for each joint /// Joint variables (angles for revolute joints, displacements for prismatic joints) /// End-effector pose as a 4x4 transformation matrix - Pose FK_DH(const std::vector& dh_p, const VecX& q) { - Pose T = Pose::Identity(); // Initialize as identity + template + inline Pose_T FK_DH(const std::vector>& dh_p, const VecX_T& q) { + Pose_T T = Pose_T::Identity(); // Initialise as identity - const std::size_t n = dh_p.size(); // number of joints + const size_t n = dh_p.size(); // number of joints assert(static_cast(q.size()) >= n); // basic safety for (std::size_t i = 0; i < n; ++i) { - const DH_Params& p = dh_p[i]; // current joint parameters - const double joint = q(static_cast(i)); // current joint variable - double theta = p.theta; // base angle - double d = p.d; // base offset + const DH_Params& p = dh_p[i]; // current joint parameters + const Scalar joint = q(static_cast(i)); // current joint variable + Scalar theta = p.theta; // base angle + Scalar d = p.d; // base offset if (p.type == JointType_DH::Revolute) { theta += joint; } else { d += joint; } - const double a = p.a; // link length - const double alpha = p.alpha; // link twist + const Scalar a = p.a; // link length + const Scalar alpha = p.alpha; // link twist - const double cth = std::cos(theta); - const double sth = std::sin(theta); - const double ca = std::cos(alpha); - const double sa = std::sin(alpha); + const Scalar cth = mathlib::cos(theta); + const Scalar sth = mathlib::sin(theta); + const Scalar ca = mathlib::cos(alpha); + const Scalar sa = mathlib::sin(alpha); - Pose A; // individual link transform - A << cth, -sth * ca, sth* sa, a* cth, // row 1 - rotation - sth, cth* ca, -cth * sa, a* sth, // row 2 - rotation - 0.0, sa, ca, d, // row 3 - translation - 0.0, 0.0, 0.0, 1.0; // row 4 - homogeneous + Pose_T A; // individual link transform + A << cth, -sth * ca, sth* sa, a* cth, // row 1 - rotation + sth, cth* ca, -cth * sa, a* sth, // row 2 - rotation + Scalar(0), sa, ca, d, // row 3 - translation + Scalar(0), Scalar(0), Scalar(0), Scalar(1); // row 4 - homogeneous T = T * A; // accumulate } - return T; // end-effector pose } + // Double overload + inline Pose FK_DH(const std::vector>& dh_p, const VecX& q) { return FK_DH(dh_p, q); } /// /// Compute the transformation matrices for each link in the kinematic chain @@ -60,37 +61,38 @@ namespace kinematics { /// Denavit-Hartenberg parameters for each joint /// Joint variables (angles for revolute joints, displacements for prismatic joints) /// Vector of transformation matrices for each link - std::vector linkTransforms_DH(const std::vector& dh_p, const VecX& q, const Pose& T_base = Pose::Identity()) { - std::vector transforms; + template + inline std::vector> linkTransforms_DH(const std::vector>& dh_p, const VecX_T& q, const Pose_T& T_base = Pose_T::Identity()) { + std::vector> transforms; transforms.reserve(dh_p.size()); // avoid reallocs - Pose T = T_base; // Initialize as identity + Pose_T T = T_base; // Initialise as identity const std::size_t n = dh_p.size(); assert(static_cast(q.size()) >= n); for (std::size_t i = 0; i < n; ++i) { - const DH_Params& p = dh_p[i]; - const double joint = q(static_cast(i)); - double theta = p.theta; - double d = p.d; + const DH_Params& p = dh_p[i]; + const Scalar joint = q(static_cast(i)); + Scalar theta = p.theta; + Scalar d = p.d; if (p.type == JointType_DH::Revolute) { theta += joint; } else { d += joint; } - const double a = p.a; - const double alpha = p.alpha; + const Scalar a = p.a; + const Scalar alpha = p.alpha; - const double cth = std::cos(theta); - const double sth = std::sin(theta); - const double ca = std::cos(alpha); - const double sa = std::sin(alpha); + const Scalar cth = mathlib::cos(theta); + const Scalar sth = mathlib::sin(theta); + const Scalar ca = mathlib::cos(alpha); + const Scalar sa = mathlib::sin(alpha); - Pose A; // individual link transform - A << cth, -sth * ca, sth* sa, a* cth, // row 1 - rotation - sth, cth* ca, -cth * sa, a* sth, // row 2 - rotation - 0.0, sa, ca, d, // row 3 - translation - 0.0, 0.0, 0.0, 1.0; // row 4 - homogeneous + Pose_T A; // individual link transform + A << cth, -sth * ca, sth* sa, a* cth, // row 1 - rotation + sth, cth* ca, -cth * sa, a* sth, // row 2 - rotation + Scalar(0), sa, ca, d, // row 3 - translation + Scalar(0), Scalar(0), Scalar(0), Scalar(1); // row 4 - homogeneous T = T * A; transforms.push_back(T); // store current link transform @@ -98,6 +100,10 @@ namespace kinematics { return transforms; } + // Double overload + inline std::vector linkTransforms_DH(const std::vector>& dh_p, const VecX& q, const Pose& T_base = Pose::Identity()) { + return linkTransforms_DH(dh_p, q, T_base); + } /// /// Computes Forward Kinematics using URDF joint definitions @@ -106,69 +112,64 @@ namespace kinematics { /// A vector of joint variables /// Optional base transformation matrix /// End-effector pose as a 4x4 transformation matrix - Pose FK_URDF(const std::vector& joints, const VecX& q, const Pose& T_base = Pose::Identity()) { - Pose T = T_base; - const std::size_t n = joints.size(); + template + inline Pose_T FK_URDF(const std::vector>& joints, const VecX_T& q, const Pose_T& T_base = Pose_T::Identity()) { + Pose_T T = T_base; + const size_t n = joints.size(); assert(static_cast(q.size()) >= n); for (std::size_t i = 0; i < n; ++i) { const auto& joint = joints[i]; - const double qi = q(static_cast(i)); // joint variable + const Scalar qi = q(static_cast(i)); // joint variable - Pose T_origin = Pose::Identity(); // Joint origin transform - T_origin.block<3, 3>(0, 0) = joint.origin_R; - T_origin.block<3, 1>(0, 3) = joint.origin_xyz; + Pose_T T_origin = Pose_T::Identity(); // Joint origin transform + T_origin.template block<3, 3>(0, 0) = joint.origin_R; + T_origin.template block<3, 1>(0, 3) = joint.origin_xyz; // Axis in parent frame - Vec3 axis = joint.axis.normalized(); - if (joint.axixInJointFrame) { axis = (joint.origin_R * axis).normalized(); } // Transform axis to joint frame if needed + Vec3_T axis = safeNormalised(joint.axis); + if (joint.axixInJointFrame) { axis = safeNormalised(joint.origin_R * axis); } // Compute joint motion transform - Pose T_motion = Pose::Identity(); - if (joint.type == JointType_URDF::REVOLUTE) { - Eigen::AngleAxisd aa(qi, axis); - T_motion.block<3, 3>(0, 0) = aa.toRotationMatrix(); - } - else if (joint.type == JointType_URDF::PRISMATIC) { - T_motion.block<3, 1>(0, 3) = axis * qi; - } + Pose_T T_motion = Pose_T::Identity(); + if (joint.type == JointType_URDF::REVOLUTE) { T_motion.template block<3, 3>(0, 0) = AngleAxis(qi, axis); } + else if (joint.type == JointType_URDF::PRISMATIC) { T_motion.template block<3, 1>(0, 3) = axis * qi; } // Update total transform T = T * T_origin * T_motion; } return T; // Placeholder implementation } + // Double overload + inline Pose FK_URDF(const std::vector>& joints, const VecX& q, const Pose& T_base = Pose::Identity()) { + return FK_URDF(joints, q, T_base); + } - - std::vector linkTransforms_URDF(const std::vector& joints, const VecX& q, const Pose& T_base = Pose::Identity()) { - std::vector out; + template + std::vector> linkTransforms_URDF(const std::vector>& joints, const VecX_T& q, const Pose_T& T_base = Pose_T::Identity()) { + std::vector> out; out.reserve(joints.size()); - Pose T = T_base; + Pose_T T = T_base; const std::size_t n = joints.size(); assert(static_cast(q.size()) >= n); for (std::size_t i = 0; i < n; ++i) { const auto& joint = joints[i]; - const double qi = q(static_cast(i)); // joint variable + const Scalar qi = q(static_cast(i)); // joint variable - Pose T_origin = Pose::Identity(); // Joint origin transform - T_origin.block<3, 3>(0, 0) = joint.origin_R; - T_origin.block<3, 1>(0, 3) = joint.origin_xyz; + Pose_T T_origin = Pose_T::Identity(); // Joint origin transform + T_origin.template block<3, 3>(0, 0) = joint.origin_R; + T_origin.template block<3, 1>(0, 3) = joint.origin_xyz; // Axis in parent frame - Vec3 axis = joint.axis.normalized(); - if (joint.axixInJointFrame) { axis = (joint.origin_R * axis).normalized(); } // Transform axis to joint frame if needed + Vec3_T axis = safeNormalised(joint.axis); + if (joint.axixInJointFrame) { axis = safeNormalised(joint.origin_R * axis); } // Compute joint motion transform - Pose T_motion = Pose::Identity(); - if (joint.type == JointType_URDF::REVOLUTE) { - Eigen::AngleAxisd aa(qi, axis); - T_motion.block<3, 3>(0, 0) = aa.toRotationMatrix(); - } - else if (joint.type == JointType_URDF::PRISMATIC) { - T_motion.block<3, 1>(0, 3) = axis * qi; - } + Pose_T T_motion = Pose_T::Identity(); + if (joint.type == JointType_URDF::REVOLUTE) { T_motion.template block<3, 3>(0, 0) = AngleAxis(qi, axis); } + else if (joint.type == JointType_URDF::PRISMATIC) { T_motion.template block<3, 1>(0, 3) = axis * qi; } // Update total transform T = T * T_origin * T_motion; @@ -176,5 +177,9 @@ namespace kinematics { } return out; } + // Double overload + inline std::vector linkTransforms_URDF(const std::vector>& joints, const VecX& q, const Pose& T_base = Pose::Identity()) { + return linkTransforms_URDF(joints, q, T_base); + } }; } \ No newline at end of file diff --git a/PxMLib/MathLib/include/kinematics/Jacobian.h b/PxMLib/MathLib/include/kinematics/Jacobian.h index 38df7d12..764417bc 100644 --- a/PxMLib/MathLib/include/kinematics/Jacobian.h +++ b/PxMLib/MathLib/include/kinematics/Jacobian.h @@ -1,8 +1,8 @@ #pragma once -#include "MathLibAPI.h" -#include "core/Types.h" +#include #include "kinematics/DH_Params.h" +#include "kinematics/Forward_Kinematics.h" using namespace mathlib; @@ -15,7 +15,40 @@ namespace kinematics { /// Denavit-Hartenberg parameters for each joint /// Joint variables (angles for revolute joints, displacements for prismatic joints) /// Geometric Jacobian matrix as a 6xN matrix, where N is the number of joints - MatX jacobianGeometric(const std::vector& dh_p, const VecX& q); + MatX jacobianGeometric(const std::vector>& dh_p, const VecX& q) { + size_t n = dh_p.size(); // Number of joints + MatX J = MatX::Zero(6, n); // Initialize Jacobian matrix + + auto T_list = Forward_Kinematics().linkTransforms_DH(dh_p, q); // Get link transformations + + Pose T_end = T_list.back(); // End-effector transformation + Vec3 p_end = T_end.block<3, 1>(0, 3); // End-effector position + + Vec3 z_base = Vec3::UnitZ(); // Initial z-axis + Vec3 p_base = Vec3::Zero(); // Initial position + + for (size_t i = 0; i < n; ++i) { + Vec3 z_i, p_i; + + if (i == 0) { + z_i = z_base; + p_i = p_base; + } + else { + z_i = T_list[i - 1].block<3, 1>(0, 2); // z_{i-1} + p_i = T_list[i - 1].block<3, 1>(0, 3); // p_{i-1} + } + if (dh_p[i].type == JointType_DH::Revolute) { + J.block<3, 1>(0, i) = z_i.cross(p_end - p_i); // Linear velocity part + J.block<3, 1>(3, i) = z_i; // Angular velocity part + } + else if (dh_p[i].type == JointType_DH::Prismatic) { + J.block<3, 1>(0, i) = z_i; // Linear velocity part + J.block<3, 1>(3, i) = Vec3::Zero(); // Angular velocity part + } + } + return J; // Return Geometric Jacobian matrix + } /// /// Compute the numerical Jacobian matrix using finite differences @@ -24,6 +57,29 @@ namespace kinematics { /// Joint variables at which to compute the Jacobian /// Small perturbation value for finite differences /// Numerical Jacobian matrix as a 6xN matrix, where N is the number of joints - MatX numericalJacobian(const std::function& f, const VecX& q, double h = 1e-6); + MatX numericalJacobian(const std::function& f, const VecX& q, double h = 1e-6) { + size_t n = q.size(); + MatX J = MatX::Zero(6, n); + + Pose T0 = f(q); // Compute pose at original joint variables + Vec3 p0 = T0.block<3, 1>(0, 3); // Position part + Eigen::Matrix3d R0 = T0.block<3, 3>(0, 0); // Rotation part + + for (size_t i = 0; i < n; ++i) { + VecX q_perturbed = q; + + q_perturbed(i) += h; // Perturb joint variable + Pose T_perturbed = f(q_perturbed); // Compute perturbed pose + Vec3 p_perturbed = T_perturbed.block<3, 1>(0, 3); // Perturbed position + Eigen::Matrix3d R_perturbed = T_perturbed.block<3, 3>(0, 0); // Perturbed rotation + + J.block<3, 1>(0, i) = (p_perturbed - p0) / h; // Numerical derivative for position + Eigen::Matrix3d R_diff = R0.transpose() * R_perturbed; // Rotation difference + Eigen::AngleAxisd angle_axis(R_diff); // Convert to angle-axis representation + + J.block<3, 1>(3, i) = (angle_axis.axis() * angle_axis.angle()) / h; // Numerical derivative for orientation + } + return J; // Return numerical Jacobian matrix + } }; } \ No newline at end of file diff --git a/PxMLib/MathLib/include/kinematics/Transform_Utils.h b/PxMLib/MathLib/include/kinematics/Transform_Utils.h index 0ffcf8c8..2bc431a2 100644 --- a/PxMLib/MathLib/include/kinematics/Transform_Utils.h +++ b/PxMLib/MathLib/include/kinematics/Transform_Utils.h @@ -1,7 +1,6 @@ #pragma once -#include "MathLibAPI.h" -#include "core/Types.h" +#include #include "kinematics/DH_Params.h" using namespace mathlib; @@ -15,42 +14,51 @@ namespace kinematics { /// Translation vector /// Rotation quaternion /// Transformation matrix - Pose makeTransform(const Vec3& translation, const Eigen::Quaterniond& rotation); + Pose makeTransform(const Vec3& translation, const Eigen::Quaterniond& rotation) { + Pose transform = Pose::Identity(); + transform.block<3, 1>(0, 3) = translation; // Set translation vector + transform.block<3, 3>(0, 0) = rotation.toRotationMatrix(); // Set rotation matrix + return transform; + } /// /// Extract translation vector from transformation matrix /// /// The transformation matrix /// Translation vector - Vec3 getTranslation(const Pose& transform); + Vec3 getTranslation(const Pose& transform) { return transform.block<3, 1>(0, 3); } /// /// Extract rotation quaternion from transformation matrix /// /// The transformation matrix /// Rotation quaternion - Quat getRotation(const Pose& transform); + Quat getRotation(const Pose& transform) { return (Quat)transform.block<3, 3>(0, 0); } /// /// Create transformation matrix from translation and rotation /// /// The transformation matrix /// Transformation matrix - Pose fromTranslationRotation(const Vec3& translation, const Quat& rotation); + Pose fromTranslationRotation(const Vec3& translation, const Quat& rotation) { + Pose transform = Pose::Identity(); + transform.block<3, 1>(0, 3) = translation; + transform.block<3, 3>(0, 0) = rotation.toRotationMatrix(); + return transform; + } /// /// Interpolate between two transformation matrices /// /// The transformation matrix /// Transformation matrix - Pose interpolateTransforms(const Pose& t1, const Pose& t2, double t); + Pose interpolateTransforms(const Pose& t1, const Pose& t2, double t) { return t1 * (1.0 - t) + t2 * t; } /// /// Invert a transformation matrix /// /// The transformation matrix /// Transformation matrix - Pose invertTransform(const Pose& transform); - + Pose invertTransform(const Pose& transform) { return transform.inverse(); } }; } \ No newline at end of file diff --git a/PxMLib/MathLib/include/kinematics/URDF_Types.h b/PxMLib/MathLib/include/kinematics/URDF_Types.h index e5604c5f..78642370 100644 --- a/PxMLib/MathLib/include/kinematics/URDF_Types.h +++ b/PxMLib/MathLib/include/kinematics/URDF_Types.h @@ -1,7 +1,6 @@ #pragma once -#include "MathLibAPI.h" -#include "core/Types.h" +#include using namespace mathlib; @@ -15,7 +14,7 @@ namespace kinematics { FLOATING, PLANAR }; - // URDF Link Collision Shape Types + // URDF Link Collision Shape Types (May remove now, but Ill leave until full refactor is done) enum class CollisionShapeType_URDF { BOX, CYLINDER, @@ -23,12 +22,13 @@ namespace kinematics { MESH }; // URDF Joint Structure + template struct JointURDF { - Vec3 origin_xyz; // joint origin translation - Mat3 origin_R; // joint origin rotation - Vec3 axis; // joint axis - JointType_URDF type; // Revolute / Prismatic / ... - bool axixInJointFrame; // true for "axis_frame":"joint" + Vec3_T origin_xyz; // joint origin translation + Mat3_T origin_R; // joint origin rotation + Vec3_T axis; // joint axis + JointType_URDF type; // Revolute / Prismatic / ... + bool axixInJointFrame; // true for "axis_frame":"joint" }; } \ No newline at end of file diff --git a/PxMLib/MathLib/pch.cpp b/PxMLib/MathLib/pch.cpp index 64b7eef6..c94d6873 100644 --- a/PxMLib/MathLib/pch.cpp +++ b/PxMLib/MathLib/pch.cpp @@ -1,5 +1,2 @@ // pch.cpp: source file corresponding to the pre-compiled header - -#include "pch.h" - -// When you are using pre-compiled headers, this source file is necessary for compilation to succeed. +#include "pch.h" \ No newline at end of file diff --git a/PxMLib/MathLib/src/control/Error.cpp b/PxMLib/MathLib/src/control/Error.cpp deleted file mode 100644 index 05e5fba2..00000000 --- a/PxMLib/MathLib/src/control/Error.cpp +++ /dev/null @@ -1,35 +0,0 @@ -#include "pch.h" -#include "control/Error.h" - -namespace control { - /// - void jointErr(const VecX& q_desired, const VecX& q_current, VecX& out_error) { - out_error = q_desired - q_current; - } - - /// - PoseError poseErr(const Mat4& T_desired, const Mat4& T_current) { - PoseError error; - // Positional error - error.position = T_desired.block<3, 1>(0, 3) - T_current.block<3, 1>(0, 3); - // Orientational error (using rotation matrices) - Mat3 R_desired = T_desired.block<3, 3>(0, 0); - Mat3 R_current = T_current.block<3, 3>(0, 0); - Mat3 R_error = R_desired * R_current.transpose(); - // Convert rotation matrix to roll-pitch-yaw angles - double sy = sqrt(R_error(0, 0) * R_error(0, 0) + R_error(1, 0) * R_error(1, 0)); - bool singular = sy < 1e-6; // If - double x, y, z; - if (!singular) { - x = atan2(R_error(2, 1), R_error(2, 2)); - y = atan2(-R_error(2, 0), sy); - z = atan2(R_error(1, 0), R_error(0, 0)); - } else { - x = atan2(-R_error(1, 2), R_error(1, 1)); - y = atan2(-R_error(2, 0), sy); - z = 0; - } - error.orientation = Vec3(x, y, z); - return error; - } -} \ No newline at end of file diff --git a/PxMLib/MathLib/src/control/PID.cpp b/PxMLib/MathLib/src/control/PID.cpp deleted file mode 100644 index 137c2643..00000000 --- a/PxMLib/MathLib/src/control/PID.cpp +++ /dev/null @@ -1,27 +0,0 @@ -#include "pch.h" -#include "control/PID.h" - -namespace control { - /// - void PID(const PID_Gains& gains, PID_State& state, const VecX& error, double dt, VecX& out_u) { - - out_u.resize(error.size()); // Ensure the output vector is the correct size - - VecX P = gains.Kp.cwiseProduct(error); // Proportional term - state.integral += error * dt; // Integral term - VecX I = gains.Ki.cwiseProduct(state.integral); // Compute integral term - VecX D; // Derivative term - - if (state.first_update) { - D = VecX::Zero(error.size()); - state.first_update = false; - } - else { - VecX derivative = (error - state.prev_error) / dt; - D = gains.Kd.cwiseProduct(derivative); - } - - state.prev_error = error; // Update previous error - out_u = P + I + D; // Compute total control output - } -} \ No newline at end of file diff --git a/PxMLib/MathLib/src/dynamics/MassMatrix.cpp b/PxMLib/MathLib/src/dynamics/MassMatrix.cpp deleted file mode 100644 index bf1d607d..00000000 --- a/PxMLib/MathLib/src/dynamics/MassMatrix.cpp +++ /dev/null @@ -1,17 +0,0 @@ -#include "pch.h" -#include "dynamics/MassMatrix.h" - -namespace dynamics { - MatX MassMatrix::massMatrix(const std::vector& dh_p, const std::vector& inertias, const VecX& q) { - size_t n = dh_p.size(); - MatX M = MatX::Zero(n, n); - - // Placeholder implementation: In a real scenario, this would involve complex calculations - // based on the robot's kinematics and dynamics. - - for (size_t i = 0; i < n; ++i) { - M(i, i) = inertias[i].mass; // Simplified diagonal mass matrix - } - return M; // Return the computed mass matrix - } -} \ No newline at end of file diff --git a/PxMLib/MathLib/src/dynamics/Nonlinear_Terms.cpp b/PxMLib/MathLib/src/dynamics/Nonlinear_Terms.cpp deleted file mode 100644 index 61b78f03..00000000 --- a/PxMLib/MathLib/src/dynamics/Nonlinear_Terms.cpp +++ /dev/null @@ -1,28 +0,0 @@ -#include "pch.h" -#include "dynamics/Nonlinear_Terms.h" - -namespace dynamics { - /// - VecX Nonlinear_Terms::coriolisCentrifugal(const std::vector& dh_p, const std::vector& inertias, const VecX& q, const VecX& q_dot) { - size_t n = dh_p.size(); - VecX C = VecX::Zero(n); - // Placeholder implementation: In a real scenario, this would involve complex calculations - // based on the robot's kinematics and dynamics. - for (size_t i = 0; i < n; ++i) { - C(i) = 0.1 * q_dot(i); // Simplified linear relation - } - return C; // Return the computed nonlinear terms - } - - /// - VecX Nonlinear_Terms::gravityTorque(const std::vector& dh_p, const std::vector& inertias, const VecX& q, const Vec3& gravity) { - size_t n = dh_p.size(); - VecX G = VecX::Zero(n); - // Placeholder implementation: In a real scenario, this would involve complex calculations - // based on the robot's kinematics and dynamics. - for (size_t i = 0; i < n; ++i) { - G(i) = inertias[i].mass * gravity(2) * 0.5; // Simplified gravity torque - } - return G; // Return the computed gravity torque vector - } -} \ No newline at end of file diff --git a/PxMLib/MathLib/src/dynamics/StateSpace.cpp b/PxMLib/MathLib/src/dynamics/StateSpace.cpp deleted file mode 100644 index d36d7dc0..00000000 --- a/PxMLib/MathLib/src/dynamics/StateSpace.cpp +++ /dev/null @@ -1,19 +0,0 @@ -#include "pch.h" -#include "dynamics/StateSpace.h" - -namespace dynamics { - /// - VecX StateSpace::dynamicRHS(const std::vector& dh_p, const std::vector& inertias, const VecX& q, const VecX& q_dot, const VecX& tau, const Vec3& gravity) { - size_t n = dh_p.size(); - VecX rhs = VecX::Zero(n); - - // Placeholder implementation: When actually used I will define the would involve complex calculations based on the robot's kinematics and dynamics. - // Not fully sure how to do that yet. - // For demonstration, this just computes a simple relation - - for (size_t i = 0; i < n; ++i) { - rhs(i) = tau(i) - 0.1 * q_dot(i) - inertias[i].mass * gravity(2) * 0.5; // Simplified dynamics - } - return rhs; // Return the computed dynamic right-hand side - } -} \ No newline at end of file diff --git a/PxMLib/MathLib/src/integrators/Integration.cpp b/PxMLib/MathLib/src/integrators/Integration.cpp deleted file mode 100644 index 35390497..00000000 --- a/PxMLib/MathLib/src/integrators/Integration.cpp +++ /dev/null @@ -1,54 +0,0 @@ -#include "pch.h" -#include "integrators/Integration.h" - - -namespace integration { - // General ODE integrator function - VecX integrator::integrate_ODE( - const std::function& f, - double y0, double t0, double t_final, double dt, - const std::function&, double, const double&, double)>& step_function - ) { - VecX trajectory; - double y = y0; - double t = t0; - append(trajectory, y); - - while (t < t_final) { - y = step_function(f, t, y, dt); - t += dt; - append(trajectory, y); - } - - return trajectory; - } - - // General PDE integrator function - VecX integrator::integrate_PDE( - const std::function& f, - const VecX& u0, double t0, double t_final, double dt, - const std::function&, double, const VecX&, double)>& step_function - ) { - std::vector trajectory; - VecX u = u0; - double t = t0; - trajectory.push_back(u); - while (t < t_final) { - u = step_function(f, t, u, dt); - t += dt; - trajectory.push_back(u); - } - // Convert std::vector to Eigen::VecX (flattened) - int total_size = trajectory.size() * u0.size(); - VecX result(total_size); - for (size_t i = 0; i < trajectory.size(); ++i) { - result.segment(i * u0.size(), u0.size()) = trajectory[i]; - } - return result; - } - - void integrator::append(VecX& v, double value) { - v.conservativeResize(v.size() + 1); - v(v.size() - 1) = value; - } -} \ No newline at end of file diff --git a/PxMLib/MathLib/src/integrators/IntegrationAnalysis.cpp b/PxMLib/MathLib/src/integrators/IntegrationAnalysis.cpp deleted file mode 100644 index 0e7c0646..00000000 --- a/PxMLib/MathLib/src/integrators/IntegrationAnalysis.cpp +++ /dev/null @@ -1,85 +0,0 @@ -#include "pch.h" -#include "integrators/IntegrationAnalysis.h" -#include -#include -#include -#include - -// Integration analysis methods -namespace integration { - // Compute error statistics from a collection of error values - ErrorStats analysis::computeErrorStats(const std::vector& errors) { - ErrorStats stats; - if (errors.empty()) { - stats.maxError = 0.0; - stats.minError = 0.0; - stats.meanError = 0.0; - stats.rmsError = 0.0; - return stats; - } - - stats.maxError = -std::numeric_limits::infinity(); - stats.minError = std::numeric_limits::infinity(); - - double sumError = 0.0; - double sumSquaredError = 0.0; - - for (double error : errors) { - if (error > stats.maxError) stats.maxError = error; - if (error < stats.minError) stats.minError = error; - sumError += error; - sumSquaredError += error * error; - } - - stats.meanError = sumError / errors.size(); - stats.rmsError = std::sqrt(sumSquaredError / errors.size()); - - return stats; - } - - // Compute error statistics from a collection of error samples - void analysis::computeErrorStatsfromSamples(const std::vector& samples, ErrorStats& eulerStats, ErrorStats& midpointStats, ErrorStats& heunStats, ErrorStats& ralstonStats, ErrorStats& rk4Stats) { - std::vector eulerErrors; - std::vector midpointErrors; - std::vector heunErrors; - std::vector ralstonErrors; - std::vector rk4Errors; - - for (const auto& sample : samples) { - eulerErrors.push_back(sample.errorEuler); - midpointErrors.push_back(sample.errorMidpoint); - heunErrors.push_back(sample.errorHeun); - ralstonErrors.push_back(sample.errorRalston); - rk4Errors.push_back(sample.errorRK4); - } - - eulerStats = computeErrorStats(eulerErrors); - midpointStats = computeErrorStats(midpointErrors); - heunStats = computeErrorStats(heunErrors); - ralstonStats = computeErrorStats(ralstonErrors); - rk4Stats = computeErrorStats(rk4Errors); - } - - // CSV export of error samples to file - void analysis::exportErrorSamplesToCSV(const std::vector& samples, const std::string& filename) { - std::ofstream file(filename); - if (!file.is_open()) { - throw std::runtime_error("Failed to open file for writing: " + filename); - } - - // Write CSV header - file << "Time,Error_Euler,Error_Midpoint,Error_Heun,Error_Ralston,Error_RK4\n"; - file << std::fixed << std::setprecision(10); - - // Write error samples - for (const auto& sample : samples) { - file << sample.time << "," - << sample.errorEuler << "," - << sample.errorMidpoint << "," - << sample.errorHeun << "," - << sample.errorRalston << "," - << sample.errorRK4 << "\n"; - } - file.close(); - } -} \ No newline at end of file diff --git a/PxMLib/MathLib/src/kinematics/DH_Params.cpp b/PxMLib/MathLib/src/kinematics/DH_Params.cpp deleted file mode 100644 index 444ee6e7..00000000 --- a/PxMLib/MathLib/src/kinematics/DH_Params.cpp +++ /dev/null @@ -1,27 +0,0 @@ -#include "pch.h" -#include "kinematics/DH_Params.h" - -namespace kinematics { - /// - Mat4 dhTransform(const DH_Params& p, double joint_val) { - double theta = p.theta + joint_val; // Update joint angle with provided joint value - Mat4 transform = Mat4::Identity(); - transform(0, 0) = cos(theta); - transform(0, 1) = -sin(theta) * cos(p.alpha); - transform(0, 2) = sin(theta) * sin(p.alpha); - transform(0, 3) = p.a * cos(theta); - transform(1, 0) = sin(theta); - transform(1, 1) = cos(theta) * cos(p.alpha); - transform(1, 2) = -cos(theta) * sin(p.alpha); - transform(1, 3) = p.a * sin(theta); - transform(2, 0) = 0.0; - transform(2, 1) = sin(p.alpha); - transform(2, 2) = cos(p.alpha); - transform(2, 3) = p.d; - transform(3, 0) = 0.0; - transform(3, 1) = 0.0; - transform(3, 2) = 0.0; - transform(3, 3) = 1.0; - return transform; - } -} \ No newline at end of file diff --git a/PxMLib/MathLib/src/kinematics/Jacobian.cpp b/PxMLib/MathLib/src/kinematics/Jacobian.cpp deleted file mode 100644 index a11bdb13..00000000 --- a/PxMLib/MathLib/src/kinematics/Jacobian.cpp +++ /dev/null @@ -1,67 +0,0 @@ -#include "pch.h" -#include "kinematics/Jacobian.h" -#include "kinematics/Forward_Kinematics.h" - -namespace kinematics { - /// - MatX Jacobian::jacobianGeometric(const std::vector& dh_p, const VecX& q) { - size_t n = dh_p.size(); // Number of joints - MatX J = MatX::Zero(6, n); // Initialize Jacobian matrix - - auto T_list = Forward_Kinematics().linkTransforms_DH(dh_p, q); // Get link transformations - - Pose T_end = T_list.back(); // End-effector transformation - Vec3 p_end = T_end.block<3, 1>(0, 3); // End-effector position - - Vec3 z_base = Vec3::UnitZ(); // Initial z-axis - Vec3 p_base = Vec3::Zero(); // Initial position - - for (size_t i = 0; i < n; ++i) { - Vec3 z_i, p_i; - - if (i == 0) { - z_i = z_base; - p_i = p_base; - } - else { - z_i = T_list[i - 1].block<3, 1>(0, 2); // z_{i-1} - p_i = T_list[i - 1].block<3, 1>(0, 3); // p_{i-1} - } - if (dh_p[i].type == JointType_DH::Revolute) { - J.block<3, 1>(0, i) = z_i.cross(p_end - p_i); // Linear velocity part - J.block<3, 1>(3, i) = z_i; // Angular velocity part - } - else if (dh_p[i].type == JointType_DH::Prismatic) { - J.block<3, 1>(0, i) = z_i; // Linear velocity part - J.block<3, 1>(3, i) = Vec3::Zero(); // Angular velocity part - } - } - return J; // Return Geometric Jacobian matrix - } - - /// - MatX Jacobian::numericalJacobian(const std::function& f, const VecX& q, double h) { - size_t n = q.size(); - MatX J = MatX::Zero(6, n); - - Pose T0 = f(q); // Compute pose at original joint variables - Vec3 p0 = T0.block<3, 1>(0, 3); // Position part - Eigen::Matrix3d R0 = T0.block<3, 3>(0, 0); // Rotation part - - for (size_t i = 0; i < n; ++i) { - VecX q_perturbed = q; - - q_perturbed(i) += h; // Perturb joint variable - Pose T_perturbed = f(q_perturbed); // Compute perturbed pose - Vec3 p_perturbed = T_perturbed.block<3, 1>(0, 3); // Perturbed position - Eigen::Matrix3d R_perturbed = T_perturbed.block<3, 3>(0, 0); // Perturbed rotation - - J.block<3, 1>(0, i) = (p_perturbed - p0) / h; // Numerical derivative for position - Eigen::Matrix3d R_diff = R0.transpose() * R_perturbed; // Rotation difference - Eigen::AngleAxisd angle_axis(R_diff); // Convert to angle-axis representation - - J.block<3, 1>(3, i) = (angle_axis.axis() * angle_axis.angle()) / h; // Numerical derivative for orientation - } - return J; // Return numerical Jacobian matrix - } -} \ No newline at end of file diff --git a/PxMLib/MathLib/src/kinematics/Transform_Utils.cpp b/PxMLib/MathLib/src/kinematics/Transform_Utils.cpp deleted file mode 100644 index e67c3166..00000000 --- a/PxMLib/MathLib/src/kinematics/Transform_Utils.cpp +++ /dev/null @@ -1,42 +0,0 @@ - -#include "pch.h" -#include "kinematics/Transform_Utils.h" - -namespace kinematics { - /// - Pose Transform_Utils::makeTransform(const Vec3& translation, const Quat& rotation) { - Pose transform = Pose::Identity(); - transform.block<3, 1>(0, 3) = translation; // Set translation vector - transform.block<3, 3>(0, 0) = rotation.toRotationMatrix(); // Set rotation matrix - return transform; - } - - /// - Vec3 Transform_Utils::getTranslation(const Pose& transform) { - return transform.block<3, 1>(0, 3); // Extract translation vector - } - - /// - Quat Transform_Utils::getRotation(const Pose& transform) { - return (Quat)transform.block<3, 3>(0, 0); // Convert rotation matrix to quaternion - } - - /// - Pose Transform_Utils::fromTranslationRotation(const Vec3& translation, const Quat& rotation) { - Pose transform = Pose::Identity(); - transform.block<3, 1>(0, 3) = translation; - transform.block<3, 3>(0, 0) = rotation.toRotationMatrix(); - return transform; - } - - /// - Pose Transform_Utils::interpolateTransforms(const Pose& t1, const Pose& t2, double t) { - // Interpolate translation - return t1 * (1.0 - t) + t2 * t; // Simple linear interpolation for demonstration, can be improved!!! - } - - /// - Pose Transform_Utils::invertTransform(const Pose& transform) { - return transform.inverse(); - } -} \ No newline at end of file diff --git a/PxMLib/MathLib_Unit/AutomaticDifferentiationTests/ADExplicitIntegratorTests.cpp b/PxMLib/MathLib_Unit/AutomaticDifferentiationTests/ADExplicitIntegratorTests.cpp index 31582469..1c94286e 100644 --- a/PxMLib/MathLib_Unit/AutomaticDifferentiationTests/ADExplicitIntegratorTests.cpp +++ b/PxMLib/MathLib_Unit/AutomaticDifferentiationTests/ADExplicitIntegratorTests.cpp @@ -1,7 +1,7 @@ // MathLib_UnitTests ADExplicitIntegratorTests.cpp #include "TestHarness.h" -#include +#include #include #include #include diff --git a/PxMLib/MathLib_Unit/AutomaticDifferentiationTests/ADImplicitIntegratorTests.cpp b/PxMLib/MathLib_Unit/AutomaticDifferentiationTests/ADImplicitIntegratorTests.cpp index f228b053..7d0031cd 100644 --- a/PxMLib/MathLib_Unit/AutomaticDifferentiationTests/ADImplicitIntegratorTests.cpp +++ b/PxMLib/MathLib_Unit/AutomaticDifferentiationTests/ADImplicitIntegratorTests.cpp @@ -1,7 +1,7 @@ // MathLib_UnitTests ADImplicitIntegratorTests.cpp #include "TestHarness.h" -#include +#include #include #include #include diff --git a/PxMLib/MathLib_Unit/AutomaticDifferentiationTests/ADJacobianTests.cpp b/PxMLib/MathLib_Unit/AutomaticDifferentiationTests/ADJacobianTests.cpp index fe61d703..f1d6282e 100644 --- a/PxMLib/MathLib_Unit/AutomaticDifferentiationTests/ADJacobianTests.cpp +++ b/PxMLib/MathLib_Unit/AutomaticDifferentiationTests/ADJacobianTests.cpp @@ -1,7 +1,7 @@ // MathLib_UnitTests ADJacobianTests.cpp #include "TestHarness.h" -#include +#include #include #include diff --git a/PxMLib/MathLib_Unit/AutomaticDifferentiationTests/ADNewtonTests.cpp b/PxMLib/MathLib_Unit/AutomaticDifferentiationTests/ADNewtonTests.cpp index 2679d0d8..d95ae2db 100644 --- a/PxMLib/MathLib_Unit/AutomaticDifferentiationTests/ADNewtonTests.cpp +++ b/PxMLib/MathLib_Unit/AutomaticDifferentiationTests/ADNewtonTests.cpp @@ -1,7 +1,7 @@ // MathLib_UnitTests ADNewtonTests.cpp #include "TestHarness.h" -#include +#include #include #include @@ -38,7 +38,7 @@ TEST("AD Newton-Raphson", ADNewton_RootFinding) { x = newtonRaphsonStep(x); } double root = x.real; - ASSERT_TRUE(std::abs(root - std::sqrt(2.0)) < 1e-6, "Root finding error too large"); + ASSERT_TRUE(abs(root - sqrt(2.0)) < 1e-6, "Root finding error too large"); } // Test that the Newton-Raphson step using dual numbers converges to the root of the nonlinear function from different initial guesses. TEST("AD Newton-Raphson", ADNewton_Convergence) { @@ -49,10 +49,10 @@ TEST("AD Newton-Raphson", ADNewton_Convergence) { x = newtonRaphsonStep(x); } double root = x.real; - double residual = std::abs(nonlinearFunc(x).real); + double residual = abs(nonlinearFunc(x).real); std::string errorMsg = "Root finding error too large for initial guess " + std::to_string(x_guess); - ASSERT_TRUE(std::abs(root - std::sqrt(2.0)) < 1e-6, errorMsg.c_str()); + ASSERT_TRUE(abs(root - sqrt(2.0)) < 1e-6, errorMsg.c_str()); ASSERT_TRUE(residual < 1e-10, "Residual is too large after Newton solve"); } } @@ -62,5 +62,5 @@ TEST("AD Newton-Raphson", ADNewton_JacobianAccuracy) { DualNumber_T g = nonlinearFunc(x); double computedJacobian = g.dual[0]; double expectedJacobian = nonlinearFuncDerivative(x).real; - ASSERT_TRUE(std::abs(computedJacobian - expectedJacobian) < 1e-6, "Jacobian accuracy error too large"); + ASSERT_TRUE(abs(computedJacobian - expectedJacobian) < 1e-6, "Jacobian accuracy error too large"); } \ No newline at end of file diff --git a/PxMLib/MathLib_Unit/DualNumberTests/ArithmeticTests.cpp b/PxMLib/MathLib_Unit/DualNumberTests/ArithmeticTests.cpp index 7d8d687d..1d47d712 100644 --- a/PxMLib/MathLib_Unit/DualNumberTests/ArithmeticTests.cpp +++ b/PxMLib/MathLib_Unit/DualNumberTests/ArithmeticTests.cpp @@ -1,7 +1,7 @@ // MathLib_UnitTests ArithmeticTests.cpp #include "TestHarness.h" -#include +#include #include #include @@ -13,9 +13,9 @@ namespace { DualNumber_T a(1.0, { 0.5, 0.25 }); DualNumber_T b(2.0, { 0.1, 0.05 }); DualNumber_T c = a + b; - ASSERT_TRUE(std::abs(c.real - 3.0) < 1e-12, "Addition real part incorrect"); - ASSERT_TRUE(std::abs(c.dual[0] - 0.6) < 1e-12, "Addition dual[0] part incorrect"); - ASSERT_TRUE(std::abs(c.dual[1] - 0.3) < 1e-12, "Addition dual[1] part incorrect"); + ASSERT_TRUE(abs(c.real - 3.0) < 1e-12, "Addition real part incorrect"); + ASSERT_TRUE(abs(c.dual[0] - 0.6) < 1e-12, "Addition dual[0] part incorrect"); + ASSERT_TRUE(abs(c.dual[1] - 0.3) < 1e-12, "Addition dual[1] part incorrect"); } // Test subtraction of dual numbers works correctly @@ -23,18 +23,18 @@ namespace { DualNumber_T a(1.0, { 0.5, 0.25 }); DualNumber_T b(2.0, { 0.1, 0.05 }); DualNumber_T c = a - b; - ASSERT_TRUE(std::abs(c.real + 1.0) < 1e-12, "Subtraction real part incorrect"); - ASSERT_TRUE(std::abs(c.dual[0] - 0.4) < 1e-12, "Subtraction dual[0] part incorrect"); - ASSERT_TRUE(std::abs(c.dual[1] - 0.2) < 1e-12, "Subtraction dual[1] part incorrect"); + ASSERT_TRUE(abs(c.real + 1.0) < 1e-12, "Subtraction real part incorrect"); + ASSERT_TRUE(abs(c.dual[0] - 0.4) < 1e-12, "Subtraction dual[0] part incorrect"); + ASSERT_TRUE(abs(c.dual[1] - 0.2) < 1e-12, "Subtraction dual[1] part incorrect"); } // Test that addition and subtraction are inverses for dual numbers TEST("Basic Arithmetic", DualNumber_AddSubInverse) { DualNumber_T a(1.0, { 0.5, 0.25 }); DualNumber_T b(2.0, { 0.1, 0.05 }); DualNumber_T c = (a + b) - b; - ASSERT_TRUE(std::abs(c.real - a.real) < 1e-12, "Add/Sub inverse real part incorrect"); - ASSERT_TRUE(std::abs(c.dual[0] - a.dual[0]) < 1e-12, "Add/Sub inverse dual[0] part incorrect"); - ASSERT_TRUE(std::abs(c.dual[1] - a.dual[1]) < 1e-12, "Add/Sub inverse dual[1] part incorrect"); + ASSERT_TRUE(abs(c.real - a.real) < 1e-12, "Add/Sub inverse real part incorrect"); + ASSERT_TRUE(abs(c.dual[0] - a.dual[0]) < 1e-12, "Add/Sub inverse dual[0] part incorrect"); + ASSERT_TRUE(abs(c.dual[1] - a.dual[1]) < 1e-12, "Add/Sub inverse dual[1] part incorrect"); } // Test that multiplication of dual numbers works correctly @@ -42,25 +42,25 @@ namespace { DualNumber_T a(1.0, { 0.5 }); DualNumber_T b(2.0, { 0.1 }); DualNumber_T c = a * b; - ASSERT_TRUE(std::abs(c.real - 2.0) < 1e-12, "Multiplication real part incorrect"); - ASSERT_TRUE(std::abs(c.dual[0] - (1.0 * 0.1 + 2.0 * 0.5)) < 1e-12, "Multiplication dual part incorrect"); + ASSERT_TRUE(abs(c.real - 2.0) < 1e-12, "Multiplication real part incorrect"); + ASSERT_TRUE(abs(c.dual[0] - (1.0 * 0.1 + 2.0 * 0.5)) < 1e-12, "Multiplication dual part incorrect"); } // Test that multiplication of dual numbers with variable value of two works correctly TEST("Multiplication", DualNumber_Multiplication_2Var) { DualNumber_T a(1.0, { 0.5, 0.25 }); DualNumber_T b(2.0, { 0.1, 0.05 }); DualNumber_T c = a * b; - ASSERT_TRUE(std::abs(c.real - 2.0) < 1e-12, "Multiplication real part incorrect"); - ASSERT_TRUE(std::abs(c.dual[0] - (1.0 * 0.1 + 2.0 * 0.5)) < 1e-12, "Multiplication dual[0] part incorrect"); - ASSERT_TRUE(std::abs(c.dual[1] - (1.0 * 0.05 + 2.0 * 0.25)) < 1e-12, "Multiplication dual[1] part incorrect"); + ASSERT_TRUE(abs(c.real - 2.0) < 1e-12, "Multiplication real part incorrect"); + ASSERT_TRUE(abs(c.dual[0] - (1.0 * 0.1 + 2.0 * 0.5)) < 1e-12, "Multiplication dual[0] part incorrect"); + ASSERT_TRUE(abs(c.dual[1] - (1.0 * 0.05 + 2.0 * 0.25)) < 1e-12, "Multiplication dual[1] part incorrect"); } // Test that multiplication of dual numbers works correctly with float type TEST("Multiplication", DualNumber_Multiplication_f) { DualNumber_T a(1.0f, { 0.5f }); DualNumber_T b(2.0f, { 0.1f }); DualNumber_T c = a * b; - ASSERT_TRUE(std::abs(c.real - 2.0f) < 1e-5f, "Multiplication real part incorrect"); - ASSERT_TRUE(std::abs(c.dual[0] - (1.0f * 0.1f + 2.0f * 0.5f)) < 1e-5f, "Multiplication dual part incorrect"); + ASSERT_TRUE(abs(c.real - 2.0f) < 1e-5f, "Multiplication real part incorrect"); + ASSERT_TRUE(abs(c.dual[0] - (1.0f * 0.1f + 2.0f * 0.5f)) < 1e-5f, "Multiplication dual part incorrect"); } // Test that multiplication distributes over addition for dual numbers TEST("Multiplication", DualNumber_Distributivity) { @@ -69,9 +69,9 @@ namespace { DualNumber_T c(3.0, { 0.2, 0.1 }); DualNumber_T left = a * (b + c); DualNumber_T right = a * b + a * c; - ASSERT_TRUE(std::abs(left.real - right.real) < 1e-12, "Distributivity real part incorrect"); - ASSERT_TRUE(std::abs(left.dual[0] - right.dual[0]) < 1e-12, "Distributivity dual[0] part incorrect"); - ASSERT_TRUE(std::abs(left.dual[1] - right.dual[1]) < 1e-12, "Distributivity dual[1] part incorrect"); + ASSERT_TRUE(abs(left.real - right.real) < 1e-12, "Distributivity real part incorrect"); + ASSERT_TRUE(abs(left.dual[0] - right.dual[0]) < 1e-12, "Distributivity dual[0] part incorrect"); + ASSERT_TRUE(abs(left.dual[1] - right.dual[1]) < 1e-12, "Distributivity dual[1] part incorrect"); } // Test that multiplication is commutative for dual numbers TEST("Multiplication", DualNumber_Commutativity) { @@ -79,9 +79,9 @@ namespace { DualNumber_T b(2.0, { 0.1, 0.05 }); DualNumber_T ab = a * b; DualNumber_T ba = b * a; - ASSERT_TRUE(std::abs(ab.real - ba.real) < 1e-12, "Commutativity real part incorrect"); - ASSERT_TRUE(std::abs(ab.dual[0] - ba.dual[0]) < 1e-12, "Commutativity dual[0] part incorrect"); - ASSERT_TRUE(std::abs(ab.dual[1] - ba.dual[1]) < 1e-12, "Commutativity dual[1] part incorrect"); + ASSERT_TRUE(abs(ab.real - ba.real) < 1e-12, "Commutativity real part incorrect"); + ASSERT_TRUE(abs(ab.dual[0] - ba.dual[0]) < 1e-12, "Commutativity dual[0] part incorrect"); + ASSERT_TRUE(abs(ab.dual[1] - ba.dual[1]) < 1e-12, "Commutativity dual[1] part incorrect"); } // Test that multiplication is associative for dual numbers TEST("Multiplication", DualNumber_Associativity) { @@ -90,9 +90,9 @@ namespace { DualNumber_T c(3.0, { 0.2, 0.1 }); DualNumber_T ab_c = (a * b) * c; DualNumber_T a_bc = a * (b * c); - ASSERT_TRUE(std::abs(ab_c.real - a_bc.real) < 1e-12, "Associativity real part incorrect"); - ASSERT_TRUE(std::abs(ab_c.dual[0] - a_bc.dual[0]) < 1e-12, "Associativity dual[0] part incorrect"); - ASSERT_TRUE(std::abs(ab_c.dual[1] - a_bc.dual[1]) < 1e-12, "Associativity dual[1] part incorrect"); + ASSERT_TRUE(abs(ab_c.real - a_bc.real) < 1e-12, "Associativity real part incorrect"); + ASSERT_TRUE(abs(ab_c.dual[0] - a_bc.dual[0]) < 1e-12, "Associativity dual[0] part incorrect"); + ASSERT_TRUE(abs(ab_c.dual[1] - a_bc.dual[1]) < 1e-12, "Associativity dual[1] part incorrect"); } // Test that multiplication by the identity element works correctly for dual numbers TEST("Multiplication", DualNumber_Identity) { @@ -100,12 +100,12 @@ namespace { DualNumber_T identity(1.0, { 0.0, 0.0 }); DualNumber_T left = a * identity; DualNumber_T right = identity * a; - ASSERT_TRUE(std::abs(left.real - a.real) < 1e-12, "Identity left real part incorrect"); - ASSERT_TRUE(std::abs(left.dual[0] - a.dual[0]) < 1e-12, "Identity left dual[0] part incorrect"); - ASSERT_TRUE(std::abs(left.dual[1] - a.dual[1]) < 1e-12, "Identity left dual[1] part incorrect"); - ASSERT_TRUE(std::abs(right.real - a.real) < 1e-12, "Identity right real part incorrect"); - ASSERT_TRUE(std::abs(right.dual[0] - a.dual[0]) < 1e-12, "Identity right dual[0] part incorrect"); - ASSERT_TRUE(std::abs(right.dual[1] - a.dual[1]) < 1e-12, "Identity right dual[1] part incorrect"); + ASSERT_TRUE(abs(left.real - a.real) < 1e-12, "Identity left real part incorrect"); + ASSERT_TRUE(abs(left.dual[0] - a.dual[0]) < 1e-12, "Identity left dual[0] part incorrect"); + ASSERT_TRUE(abs(left.dual[1] - a.dual[1]) < 1e-12, "Identity left dual[1] part incorrect"); + ASSERT_TRUE(abs(right.real - a.real) < 1e-12, "Identity right real part incorrect"); + ASSERT_TRUE(abs(right.dual[0] - a.dual[0]) < 1e-12, "Identity right dual[0] part incorrect"); + ASSERT_TRUE(abs(right.dual[1] - a.dual[1]) < 1e-12, "Identity right dual[1] part incorrect"); } // Test that multiplication by zero works correctly for dual numbers TEST("Multiplication", DualNumber_ZeroMultiplication) { @@ -113,12 +113,12 @@ namespace { DualNumber_T zero(0.0, { 0.0, 0.0 }); DualNumber_T left = a * zero; DualNumber_T right = zero * a; - ASSERT_TRUE(std::abs(left.real) < 1e-12, "Zero multiplication left real part incorrect"); - ASSERT_TRUE(std::abs(left.dual[0]) < 1e-12, "Zero multiplication left dual[0] part incorrect"); - ASSERT_TRUE(std::abs(left.dual[1]) < 1e-12, "Zero multiplication left dual[1] part incorrect"); - ASSERT_TRUE(std::abs(right.real) < 1e-12, "Zero multiplication right real part incorrect"); - ASSERT_TRUE(std::abs(right.dual[0]) < 1e-12, "Zero multiplication right dual[0] part incorrect"); - ASSERT_TRUE(std::abs(right.dual[1]) < 1e-12, "Zero multiplication right dual[1] part incorrect"); + ASSERT_TRUE(abs(left.real) < 1e-12, "Zero multiplication left real part incorrect"); + ASSERT_TRUE(abs(left.dual[0]) < 1e-12, "Zero multiplication left dual[0] part incorrect"); + ASSERT_TRUE(abs(left.dual[1]) < 1e-12, "Zero multiplication left dual[1] part incorrect"); + ASSERT_TRUE(abs(right.real) < 1e-12, "Zero multiplication right real part incorrect"); + ASSERT_TRUE(abs(right.dual[0]) < 1e-12, "Zero multiplication right dual[0] part incorrect"); + ASSERT_TRUE(abs(right.dual[1]) < 1e-12, "Zero multiplication right dual[1] part incorrect"); } // Test that multiplication by a scalar works correctly for dual numbers TEST("Multiplication", DualNumber_ScalarMultiplication) { @@ -126,12 +126,12 @@ namespace { double scalar = 2.0; DualNumber_T left = a * scalar; DualNumber_T right = scalar * a; - ASSERT_TRUE(std::abs(left.real - 2.0) < 1e-12, "Scalar multiplication left real part incorrect"); - ASSERT_TRUE(std::abs(left.dual[0] - 1.0) < 1e-12, "Scalar multiplication left dual[0] part incorrect"); - ASSERT_TRUE(std::abs(left.dual[1] - 0.5) < 1e-12, "Scalar multiplication left dual[1] part incorrect"); - ASSERT_TRUE(std::abs(right.real - 2.0) < 1e-12, "Scalar multiplication right real part incorrect"); - ASSERT_TRUE(std::abs(right.dual[0] - 1.0) < 1e-12, "Scalar multiplication right dual[0] part incorrect"); - ASSERT_TRUE(std::abs(right.dual[1] - 0.5) < 1e-12, "Scalar multiplication right dual[1] part incorrect"); + ASSERT_TRUE(abs(left.real - 2.0) < 1e-12, "Scalar multiplication left real part incorrect"); + ASSERT_TRUE(abs(left.dual[0] - 1.0) < 1e-12, "Scalar multiplication left dual[0] part incorrect"); + ASSERT_TRUE(abs(left.dual[1] - 0.5) < 1e-12, "Scalar multiplication left dual[1] part incorrect"); + ASSERT_TRUE(abs(right.real - 2.0) < 1e-12, "Scalar multiplication right real part incorrect"); + ASSERT_TRUE(abs(right.dual[0] - 1.0) < 1e-12, "Scalar multiplication right dual[0] part incorrect"); + ASSERT_TRUE(abs(right.dual[1] - 0.5) < 1e-12, "Scalar multiplication right dual[1] part incorrect"); } // Test that multiplication by a negative scalar works correctly for dual numbers TEST("Multiplication", DualNumber_NegativeScalarMultiplication) { @@ -139,11 +139,11 @@ namespace { double scalar = -2.0; DualNumber_T left = a * scalar; DualNumber_T right = scalar * a; - ASSERT_TRUE(std::abs(left.real + 2.0) < 1e-12, "Negative scalar multiplication left real part incorrect"); - ASSERT_TRUE(std::abs(left.dual[0] + 1.0) < 1e-12, "Negative scalar multiplication left dual[0] part incorrect"); - ASSERT_TRUE(std::abs(left.dual[1] + 0.5) < 1e-12, "Negative scalar multiplication left dual[1] part incorrect"); - ASSERT_TRUE(std::abs(right.real + 2.0) < 1e-12, "Negative scalar multiplication right real part incorrect"); - ASSERT_TRUE(std::abs(right.dual[0] + 1.0) < 1e-12, "Negative scalar multiplication right dual[0] part incorrect"); - ASSERT_TRUE(std::abs(right.dual[1] + 0.5) < 1e-12, "Negative scalar multiplication right dual[1] part incorrect"); + ASSERT_TRUE(abs(left.real + 2.0) < 1e-12, "Negative scalar multiplication left real part incorrect"); + ASSERT_TRUE(abs(left.dual[0] + 1.0) < 1e-12, "Negative scalar multiplication left dual[0] part incorrect"); + ASSERT_TRUE(abs(left.dual[1] + 0.5) < 1e-12, "Negative scalar multiplication left dual[1] part incorrect"); + ASSERT_TRUE(abs(right.real + 2.0) < 1e-12, "Negative scalar multiplication right real part incorrect"); + ASSERT_TRUE(abs(right.dual[0] + 1.0) < 1e-12, "Negative scalar multiplication right dual[0] part incorrect"); + ASSERT_TRUE(abs(right.dual[1] + 0.5) < 1e-12, "Negative scalar multiplication right dual[1] part incorrect"); } } \ No newline at end of file diff --git a/PxMLib/MathLib_Unit/DualNumberTests/AssignmentOperatorTests.cpp b/PxMLib/MathLib_Unit/DualNumberTests/AssignmentOperatorTests.cpp index 660010ca..50993ee0 100644 --- a/PxMLib/MathLib_Unit/DualNumberTests/AssignmentOperatorTests.cpp +++ b/PxMLib/MathLib_Unit/DualNumberTests/AssignmentOperatorTests.cpp @@ -1,7 +1,7 @@ // MathLib_UnitTests AssignmentOperatorTests.cpp #include "TestHarness.h" -#include +#include #include #include @@ -13,36 +13,36 @@ namespace { DualNumber_T a(1.0, { 0.5, 0.25 }); DualNumber_T b(2.0, { 0.1, 0.05 }); a += b; - ASSERT_TRUE(std::abs(a.real - 3.0) < 1e-12, "Addition assignment real part incorrect"); - ASSERT_TRUE(std::abs(a.dual[0] - 0.6) < 1e-12, "Addition assignment dual[0] part incorrect"); - ASSERT_TRUE(std::abs(a.dual[1] - 0.3) < 1e-12, "Addition assignment dual[1] part incorrect"); + ASSERT_TRUE(abs(a.real - 3.0) < 1e-12, "Addition assignment real part incorrect"); + ASSERT_TRUE(abs(a.dual[0] - 0.6) < 1e-12, "Addition assignment dual[0] part incorrect"); + ASSERT_TRUE(abs(a.dual[1] - 0.3) < 1e-12, "Addition assignment dual[1] part incorrect"); } // Test that addition assignment works with dual numbers and scalars TEST("Assignment Operators", DualNumber_AdditionAssignmentScalar) { DualNumber_T a(1.0, { 0.5, 0.25 }); double scalar = 2.0; a += scalar; - ASSERT_TRUE(std::abs(a.real - 3.0) < 1e-12, "Addition assignment with scalar real part incorrect"); - ASSERT_TRUE(std::abs(a.dual[0] - 0.5) < 1e-12, "Addition assignment with scalar dual[0] part incorrect"); - ASSERT_TRUE(std::abs(a.dual[1] - 0.25) < 1e-12, "Addition assignment with scalar dual[1] part incorrect"); + ASSERT_TRUE(abs(a.real - 3.0) < 1e-12, "Addition assignment with scalar real part incorrect"); + ASSERT_TRUE(abs(a.dual[0] - 0.5) < 1e-12, "Addition assignment with scalar dual[0] part incorrect"); + ASSERT_TRUE(abs(a.dual[1] - 0.25) < 1e-12, "Addition assignment with scalar dual[1] part incorrect"); } // Test that subtraction assignment works correctly for dual numbers TEST("Assignment Operators", DualNumber_SubtractionAssignment) { DualNumber_T a(1.0, { 0.5, 0.25 }); DualNumber_T b(2.0, { 0.1, 0.05 }); a -= b; - ASSERT_TRUE(std::abs(a.real + 1.0) < 1e-12, "Subtraction assignment real part incorrect"); - ASSERT_TRUE(std::abs(a.dual[0] - 0.4) < 1e-12, "Subtraction assignment dual[0] part incorrect"); - ASSERT_TRUE(std::abs(a.dual[1] - 0.2) < 1e-12, "Subtraction assignment dual[1] part incorrect"); + ASSERT_TRUE(abs(a.real + 1.0) < 1e-12, "Subtraction assignment real part incorrect"); + ASSERT_TRUE(abs(a.dual[0] - 0.4) < 1e-12, "Subtraction assignment dual[0] part incorrect"); + ASSERT_TRUE(abs(a.dual[1] - 0.2) < 1e-12, "Subtraction assignment dual[1] part incorrect"); } // Test that subtraction assignment works with dual numbers and scalars TEST("Assignment Operators", DualNumber_SubtractionAssignmentScalar) { DualNumber_T a(1.0, { 0.5, 0.25 }); double scalar = 2.0; a -= scalar; - ASSERT_TRUE(std::abs(a.real + 1.0) < 1e-12, "Subtraction assignment with scalar real part incorrect"); - ASSERT_TRUE(std::abs(a.dual[0] - 0.5) < 1e-12, "Subtraction assignment with scalar dual[0] part incorrect"); - ASSERT_TRUE(std::abs(a.dual[1] - 0.25) < 1e-12, "Subtraction assignment with scalar dual[1] part incorrect"); + ASSERT_TRUE(abs(a.real + 1.0) < 1e-12, "Subtraction assignment with scalar real part incorrect"); + ASSERT_TRUE(abs(a.dual[0] - 0.5) < 1e-12, "Subtraction assignment with scalar dual[0] part incorrect"); + ASSERT_TRUE(abs(a.dual[1] - 0.25) < 1e-12, "Subtraction assignment with scalar dual[1] part incorrect"); } // Test that addition assignment and subtraction assignment are inverses for dual numbers TEST("Assignment Operators", DualNumber_AddSubAssignmentInverse) { @@ -50,18 +50,18 @@ namespace { DualNumber_T b(2.0, { 0.1, 0.05 }); a += b; a -= b; - ASSERT_TRUE(std::abs(a.real - 1.0) < 1e-12, "Add/Sub assignment inverse real part incorrect"); - ASSERT_TRUE(std::abs(a.dual[0] - 0.5) < 1e-12, "Add/Sub assignment inverse dual[0] part incorrect"); - ASSERT_TRUE(std::abs(a.dual[1] - 0.25) < 1e-12, "Add/Sub assignment inverse dual[1] part incorrect"); + ASSERT_TRUE(abs(a.real - 1.0) < 1e-12, "Add/Sub assignment inverse real part incorrect"); + ASSERT_TRUE(abs(a.dual[0] - 0.5) < 1e-12, "Add/Sub assignment inverse dual[0] part incorrect"); + ASSERT_TRUE(abs(a.dual[1] - 0.25) < 1e-12, "Add/Sub assignment inverse dual[1] part incorrect"); } // Test that multiplication assignment works correctly for dual numbers TEST("Assignment Operators", DualNumber_MultiplicationAssignment) { DualNumber_T a(1.0, { 0.5, 0.25 }); DualNumber_T b(2.0, { 0.1, 0.05 }); a *= b; - ASSERT_TRUE(std::abs(a.real - 2.0) < 1e-12, "Multiplication assignment real part incorrect"); - ASSERT_TRUE(std::abs(a.dual[0] - (1.0 * 0.1 + 2.0 * 0.5)) < 1e-12, "Multiplication assignment dual[0] part incorrect"); - ASSERT_TRUE(std::abs(a.dual[1] - (1.0 * 0.05 + 2.0 * 0.25)) < 1e-12, "Multiplication assignment dual[1] part incorrect"); + ASSERT_TRUE(abs(a.real - 2.0) < 1e-12, "Multiplication assignment real part incorrect"); + ASSERT_TRUE(abs(a.dual[0] - (1.0 * 0.1 + 2.0 * 0.5)) < 1e-12, "Multiplication assignment dual[0] part incorrect"); + ASSERT_TRUE(abs(a.dual[1] - (1.0 * 0.05 + 2.0 * 0.25)) < 1e-12, "Multiplication assignment dual[1] part incorrect"); } // Test that division assignment works correctly for dual numbers TEST("Assignment Operators", DualNumber_DivisionAssignment) { @@ -70,9 +70,9 @@ namespace { a /= b; double expected0 = (0.5 * 2.0 - 1.0 * 0.1) / (2.0 * 2.0); double expected1 = (0.25 * 2.0 - 1.0 * 0.05) / (2.0 * 2.0); - ASSERT_TRUE(std::abs(a.real - 0.5) < 1e-12, "Division real part incorrect"); - ASSERT_TRUE(std::abs(a.dual[0] - expected0) < 1e-12, "Division dual[0] part incorrect"); - ASSERT_TRUE(std::abs(a.dual[1] - expected1) < 1e-12, "Division dual[1] part incorrect"); + ASSERT_TRUE(abs(a.real - 0.5) < 1e-12, "Division real part incorrect"); + ASSERT_TRUE(abs(a.dual[0] - expected0) < 1e-12, "Division dual[0] part incorrect"); + ASSERT_TRUE(abs(a.dual[1] - expected1) < 1e-12, "Division dual[1] part incorrect"); } // Test that division assignment by zero throws an exception for dual numbers TEST("Assignment Operators", DualNumber_DivisionAssignmentByZero) { diff --git a/PxMLib/MathLib_Unit/DualNumberTests/DifferentiationTests.cpp b/PxMLib/MathLib_Unit/DualNumberTests/DifferentiationTests.cpp index a24b4477..4a2d2406 100644 --- a/PxMLib/MathLib_Unit/DualNumberTests/DifferentiationTests.cpp +++ b/PxMLib/MathLib_Unit/DualNumberTests/DifferentiationTests.cpp @@ -1,7 +1,7 @@ // MathLib_UnitTests DifferentiationTests.cpp #include "TestHarness.h" -#include +#include #include #include @@ -15,8 +15,8 @@ namespace { }; DualNumber_T x(1.0, { 1.0 }); // Set dual part to 1 to compute derivative DualNumber_T y = f(x); - ASSERT_TRUE(std::abs(y.real - 6.0) < 1e-12, "Polynomial function value incorrect"); - ASSERT_TRUE(std::abs(y.dual[0] - 8.0) < 1e-12, "Polynomial derivative incorrect"); + ASSERT_TRUE(abs(y.real - 6.0) < 1e-12, "Polynomial function value incorrect"); + ASSERT_TRUE(abs(y.dual[0] - 8.0) < 1e-12, "Polynomial derivative incorrect"); } // Test that the derivative of x^2 is computed correctly by the product rule using dual numbers TEST("Differentiation", DualNumber_ProductRule_xSquared) { @@ -25,8 +25,8 @@ namespace { }; DualNumber_T x(2.0, { 1.0 }); // Set dual part to 1 to compute derivative DualNumber_T y = f(x); - ASSERT_TRUE(std::abs(y.real - 4.0) < 1e-12, "Product rule function value incorrect"); - ASSERT_TRUE(std::abs(y.dual[0] - 4.0) < 1e-12, "Product rule derivative incorrect"); + ASSERT_TRUE(abs(y.real - 4.0) < 1e-12, "Product rule function value incorrect"); + ASSERT_TRUE(abs(y.dual[0] - 4.0) < 1e-12, "Product rule derivative incorrect"); } // Test that the sine function's derivative is computed correctly using dual numbers TEST("Differentiation", DualNumber_SineDerivative) { @@ -35,8 +35,8 @@ namespace { }; DualNumber_T x(0.5, { 1.0 }); // Set dual part to 1 to compute derivative DualNumber_T y = f(x); - ASSERT_TRUE(std::abs(y.real - std::sin(0.5)) < 1e-12, "Sine function value incorrect"); - ASSERT_TRUE(std::abs(y.dual[0] - std::cos(0.5)) < 1e-12, "Sine derivative incorrect"); + ASSERT_TRUE(abs(y.real - std::sin(0.5)) < 1e-12, "Sine function value incorrect"); + ASSERT_TRUE(abs(y.dual[0] - std::cos(0.5)) < 1e-12, "Sine derivative incorrect"); } // Test that the cosine function's derivative is computed correctly using dual numbers TEST("Differentiation", DualNumber_CosineDerivative) { @@ -45,8 +45,8 @@ namespace { }; DualNumber_T x(0.5, { 1.0 }); // Set dual part to 1 to compute derivative DualNumber_T y = f(x); - ASSERT_TRUE(std::abs(y.real - std::cos(0.5)) < 1e-12, "Cosine function value incorrect"); - ASSERT_TRUE(std::abs(y.dual[0] + std::sin(0.5)) < 1e-12, "Cosine derivative incorrect"); + ASSERT_TRUE(abs(y.real - std::cos(0.5)) < 1e-12, "Cosine function value incorrect"); + ASSERT_TRUE(abs(y.dual[0] + std::sin(0.5)) < 1e-12, "Cosine derivative incorrect"); } // Test that the tangent function's derivative is computed correctly using dual numbers TEST("Differentiation", DualNumber_TangentDerivative) { @@ -55,9 +55,9 @@ namespace { }; DualNumber_T x(0.5, { 1.0 }); // Set dual part to 1 to compute derivative DualNumber_T y = f(x); - ASSERT_TRUE(std::abs(y.real - std::tan(0.5)) < 1e-12, "Tangent function value incorrect"); + ASSERT_TRUE(abs(y.real - std::tan(0.5)) < 1e-12, "Tangent function value incorrect"); double sec2 = 1.0 / (std::cos(0.5) * std::cos(0.5)); - ASSERT_TRUE(std::abs(y.dual[0] - sec2) < 1e-12, "Tangent derivative incorrect"); + ASSERT_TRUE(abs(y.dual[0] - sec2) < 1e-12, "Tangent derivative incorrect"); } // Test that the derivative of the hyperbolic tangent function is computed correctly using dual numbers TEST("Differentiation", DualNumber_HyperbolicTangentDerivative) { @@ -66,9 +66,9 @@ namespace { }; DualNumber_T x(0.5, { 1.0 }); // Set dual part to 1 to compute derivative DualNumber_T y = f(x); - ASSERT_TRUE(std::abs(y.real - std::tanh(0.5)) < 1e-12, "Hyperbolic tangent function value incorrect"); + ASSERT_TRUE(abs(y.real - std::tanh(0.5)) < 1e-12, "Hyperbolic tangent function value incorrect"); double sech2 = 1.0 / (std::cosh(0.5) * std::cosh(0.5)); - ASSERT_TRUE(std::abs(y.dual[0] - sech2) < 1e-12, "Hyperbolic tangent derivative incorrect"); + ASSERT_TRUE(abs(y.dual[0] - sech2) < 1e-12, "Hyperbolic tangent derivative incorrect"); } // Test that the partial derivatives of a multivariable function are computed correctly using dual numbers TEST("Differentiation", DualNumber_MultivariableFunction) { @@ -82,9 +82,9 @@ namespace { double dfdx = 6.0 * x.real + std::cos(x.real); // df/dx = 6x + cos(x) double dfdy = 2.0; // df/dy = 2 - ASSERT_TRUE(std::abs(f(x, y).real - (3.0 * 4.0 + 2.0 * 3.0 + std::sin(2.0))) < 1e-12, "Multivariable function value incorrect"); - ASSERT_TRUE(std::abs(f(x, y).dual[0] - dfdx) < 1e-12, "Partial derivative with respect to x incorrect"); - ASSERT_TRUE(std::abs(f(x, y).dual[1] - dfdy) < 1e-12, "Partial derivative with respect to y incorrect"); + ASSERT_TRUE(abs(f(x, y).real - (3.0 * 4.0 + 2.0 * 3.0 + std::sin(2.0))) < 1e-12, "Multivariable function value incorrect"); + ASSERT_TRUE(abs(f(x, y).dual[0] - dfdx) < 1e-12, "Partial derivative with respect to x incorrect"); + ASSERT_TRUE(abs(f(x, y).dual[1] - dfdy) < 1e-12, "Partial derivative with respect to y incorrect"); } // Test that the exponential function's derivative is computed correctly using dual numbers TEST("Differentiation", DualNumber_ExponentialDerivative) { @@ -94,7 +94,7 @@ namespace { DualNumber_T x(0.5, { 1.0 }); // Set dual part to 1 to compute derivative DualNumber_T y = f(x); double expected0 = std::exp(0.5); // f(x) = exp(x) -> f'(x) = exp(x) - ASSERT_TRUE(std::abs(y.real - expected0) < 1e-12, "Exponential function value incorrect"); - ASSERT_TRUE(std::abs(y.dual[0] - expected0) < 1e-12, "Exponential derivative incorrect"); + ASSERT_TRUE(abs(y.real - expected0) < 1e-12, "Exponential function value incorrect"); + ASSERT_TRUE(abs(y.dual[0] - expected0) < 1e-12, "Exponential derivative incorrect"); } } \ No newline at end of file diff --git a/PxMLib/MathLib_Unit/DualNumberTests/DivisionTests.cpp b/PxMLib/MathLib_Unit/DualNumberTests/DivisionTests.cpp index 26224ac0..43af8320 100644 --- a/PxMLib/MathLib_Unit/DualNumberTests/DivisionTests.cpp +++ b/PxMLib/MathLib_Unit/DualNumberTests/DivisionTests.cpp @@ -1,21 +1,23 @@ // MathLib_UnitTests DivisionTests.cpp #include "TestHarness.h" -#include +#include #include #include using namespace mathlib; namespace { + using mathlib::abs; + // Test that division of dual numbers works with variable value of two TEST("Division", DualNumber_Division_1Var) { DualNumber_T a(1.0, { 0.5 }); DualNumber_T b(2.0, { 0.1 }); DualNumber_T c = a / b; double expected0 = (0.5 * 2.0 - 1.0 * 0.1) / (2.0 * 2.0); - ASSERT_TRUE(std::abs(c.real - 0.5) < 1e-12, "Division real part incorrect"); - ASSERT_TRUE(std::abs(c.dual[0] - expected0) < 1e-12, "Division dual[0] part incorrect"); + ASSERT_TRUE(abs(c.real - 0.5) < 1e-12, "Division real part incorrect"); + ASSERT_TRUE(abs(c.dual[0] - expected0) < 1e-12, "Division dual[0] part incorrect"); } // Test that division of dual numbers works with variable value of two TEST("Division", DualNumber_Division_2Var) { @@ -24,45 +26,45 @@ namespace { DualNumber_T c = a / b; double expected0 = (0.5 * 2.0 - 1.0 * 0.1) / (2.0 * 2.0); double expected1 = (0.25 * 2.0 - 1.0f * 0.05) / (2.0 * 2.0); - ASSERT_TRUE(std::abs(c.real - 0.5) < 1e-12, "Division real part incorrect"); - ASSERT_TRUE(std::abs(c.dual[0] - expected0) < 1e-12, "Division dual[0] part incorrect"); - ASSERT_TRUE(std::abs(c.dual[1] - expected1) < 1e-12, "Division dual[1] part incorrect"); + ASSERT_TRUE(abs(c.real - 0.5) < 1e-12, "Division real part incorrect"); + ASSERT_TRUE(abs(c.dual[0] - expected0) < 1e-12, "Division dual[0] part incorrect"); + ASSERT_TRUE(abs(c.dual[1] - expected1) < 1e-12, "Division dual[1] part incorrect"); } // Test that division of a dual number defined with a float and double type works correctly for dual numbers TEST("Division", DualNumber_Division_f) { DualNumber_T a(1.0f, { 0.5f }); DualNumber_T b(2.0f, { 0.1f }); DualNumber_T c = a / b; - double expected0 = (0.5f * 2.0f - 1.0f * 0.1f) / (2.0f * 2.0f); - ASSERT_TRUE(std::abs(c.real - 0.5f) < 1e-5f, "Division real part incorrect"); - ASSERT_TRUE(std::abs(c.dual[0] - expected0) < 1e-5f, "Division dual[0] part incorrect"); + float expected0 = (0.5f * 2.0f - 1.0f * 0.1f) / (2.0f * 2.0f); + ASSERT_TRUE(abs(c.real - 0.5f) < 1e-5f, "Division real part incorrect"); + ASSERT_TRUE(abs(c.dual[0] - expected0) < 1e-5f, "Division dual[0] part incorrect"); } // Test that division by the identity element works correctly for dual numbers TEST("Division", DualNumber_DivisionIdentity) { DualNumber_T a(1.0, { 0.5, 0.25 }); DualNumber_T identity(1.0, { 0.0, 0.0 }); DualNumber_T left = a / identity; - ASSERT_TRUE(std::abs(left.real - a.real) < 1e-12, "Division by identity real part incorrect"); - ASSERT_TRUE(std::abs(left.dual[0] - a.dual[0]) < 1e-12, "Division by identity dual[0] part incorrect"); - ASSERT_TRUE(std::abs(left.dual[1] - a.dual[1]) < 1e-12, "Division by identity dual[1] part incorrect"); + ASSERT_TRUE(abs(left.real - a.real) < 1e-12, "Division by identity real part incorrect"); + ASSERT_TRUE(abs(left.dual[0] - a.dual[0]) < 1e-12, "Division by identity dual[0] part incorrect"); + ASSERT_TRUE(abs(left.dual[1] - a.dual[1]) < 1e-12, "Division by identity dual[1] part incorrect"); } // Test that division by zero throws an exception for dual numbers TEST("Division", DualNumber_DivisionByScalar) { DualNumber_T a(1.0, { 0.5, 0.25 }); double scalar = 2.0; DualNumber_T left = a / scalar; - ASSERT_TRUE(std::abs(left.real - 0.5) < 1e-12, "Division by scalar real part incorrect"); - ASSERT_TRUE(std::abs(left.dual[0] - 0.25) < 1e-12, "Division by scalar dual[0] part incorrect"); - ASSERT_TRUE(std::abs(left.dual[1] - 0.125) < 1e-12, "Division by scalar dual[1] part incorrect"); + ASSERT_TRUE(abs(left.real - 0.5) < 1e-12, "Division by scalar real part incorrect"); + ASSERT_TRUE(abs(left.dual[0] - 0.25) < 1e-12, "Division by scalar dual[0] part incorrect"); + ASSERT_TRUE(abs(left.dual[1] - 0.125) < 1e-12, "Division by scalar dual[1] part incorrect"); } // Test that division by a negative scalar works correctly for dual numbers TEST("Division", DualNumber_DivisionByNegativeScalar) { DualNumber_T a(1.0, { 0.5, 0.25 }); double scalar = -2.0; DualNumber_T left = a / scalar; - ASSERT_TRUE(std::abs(left.real + 0.5) < 1e-12, "Division by negative scalar real part incorrect"); - ASSERT_TRUE(std::abs(left.dual[0] + 0.25) < 1e-12, "Division by negative scalar dual[0] part incorrect"); - ASSERT_TRUE(std::abs(left.dual[1] + 0.125) < 1e-12, "Division by negative scalar dual[1] part incorrect"); + ASSERT_TRUE(abs(left.real + 0.5) < 1e-12, "Division by negative scalar real part incorrect"); + ASSERT_TRUE(abs(left.dual[0] + 0.25) < 1e-12, "Division by negative scalar dual[0] part incorrect"); + ASSERT_TRUE(abs(left.dual[1] + 0.125) < 1e-12, "Division by negative scalar dual[1] part incorrect"); } // Test that division by zero throws an exception for dual numbers TEST("Division", DualNumber_DivisionByZero) { diff --git a/PxMLib/MathLib_Unit/DualNumberTests/DualNumber_Eigen_ADTests.cpp b/PxMLib/MathLib_Unit/DualNumberTests/DualNumber_Eigen_ADTests.cpp index 0c1e71de..eef0ad09 100644 --- a/PxMLib/MathLib_Unit/DualNumberTests/DualNumber_Eigen_ADTests.cpp +++ b/PxMLib/MathLib_Unit/DualNumberTests/DualNumber_Eigen_ADTests.cpp @@ -1,5 +1,5 @@ #include "TestHarness.h" -#include +#include #include #include @@ -24,63 +24,36 @@ TEST("DualNumber_T Eigen AD", SinDerivative) { Dual x(PI_d / 4); // π/4 x.dual[0] = 1.0; Dual y = sin(x); - ASSERT_TRUE(std::abs(y.real - std::sin(PI_d / 4)) < EPS, "Sine evaluation real part incorrect"); - ASSERT_TRUE(std::abs(y.dual[0] - std::cos(PI_d / 4)) < EPS, "Sine derivative dual part incorrect"); + ASSERT_TRUE(abs(y.real - sin(PI_d / 4.0)) < EPS, "Sine evaluation real part incorrect"); + ASSERT_TRUE(abs(y.dual[0] - cos(PI_d / 4.0)) < EPS, "Sine derivative dual part incorrect"); } TEST("DualNumber_T Eigen AD", CosDerivative) { Dual x(PI_d / 4); // π/4 x.dual[0] = 1.0; Dual y = cos(x); - ASSERT_TRUE(std::abs(y.real - std::cos(PI_d / 4)) < EPS, "Cosine evaluation real part incorrect"); - ASSERT_TRUE(std::abs(y.dual[0] + std::sin(PI_d / 4)) < EPS, "Cosine derivative dual part incorrect"); + ASSERT_TRUE(abs(y.real - cos(PI_d / 4)) < EPS, "Cosine evaluation real part incorrect"); + ASSERT_TRUE(abs(y.dual[0] + sin(PI_d / 4)) < EPS, "Cosine derivative dual part incorrect"); } TEST("DualNumber_T Eigen AD", TanDerivative) { - Dual x(PI_d / 4); // π/4 + Dual x(PI_d / 4.0); x.dual[0] = 1.0; Dual y = tan(x); - double expectedReal = std::tan(PI_d / 4); - double expectedDual = 1.0 / (std::cos(PI_d / 4) * std::cos(PI_d / 4)); // sec^2(x) - ASSERT_TRUE(std::abs(y.real - expectedReal) < EPS, "Tangent evaluation real part incorrect"); - ASSERT_TRUE(std::abs(y.dual[0] - expectedDual) < EPS, "Tangent derivative dual part incorrect"); + double expectedReal = tan(PI_d / 4.0); + double expectedDual = 1.0 / (cos(PI_d / 4.0) * cos(PI_d / 4.0)); // sec^2(x) + ASSERT_TRUE(abs(y.real - expectedReal) < EPS, "Tangent evaluation real part incorrect"); + ASSERT_TRUE(abs(y.dual[0] - expectedDual) < EPS, "Tangent derivative dual part incorrect"); } TEST("DualNumber_T Eigen AD", ExpDerivative) { Dual x(1.0); x.dual[0] = 1.0; Dual y = exp(x); - ASSERT_TRUE(std::abs(y.real - std::exp(1.0)) < EPS, "Exponential evaluation real part incorrect"); - ASSERT_TRUE(std::abs(y.dual[0] - std::exp(1.0)) < EPS, "Exponential derivative dual part incorrect"); + ASSERT_TRUE(abs(y.real - exp(1.0)) < EPS, "Exponential evaluation real part incorrect"); + ASSERT_TRUE(abs(y.dual[0] - exp(1.0)) < EPS, "Exponential derivative dual part incorrect"); } TEST("DualNumber_T Eigen AD", LogDerivative) { Dual x(2.0); x.dual[0] = 1.0; Dual y = log(x); - ASSERT_TRUE(std::abs(y.real - std::log(2.0)) < EPS, "Logarithm evaluation real part incorrect"); - ASSERT_TRUE(std::abs(y.dual[0] - 0.5) < EPS, "Logarithm derivative dual part incorrect"); -} - -//TEST("DualNumber_T Eigen AD", AutomaticDifferenceJacobian) { -// // Define a simple function f: R^2 -> R^2 for testing -// auto f = [](Dual t, const VecX_T& x) -> VecX_T { -// VecX_T y(2); -// y(0) = x(0) * x(0) + t; // f1(x, t) = x1^2 + t -// y(1) = x(0) * x(1); // f2(x, t) = x1 * x2 -// return y; -// }; -// Dual t = Dual(1.0, { 0.5 }); // Perturbation in time -// VecX_T x(2); -// x << Dual(2.0, { 0.5 }), Dual(3.0, { 0.5 }); // Perturbations in state -// // Compute the Jacobian using automatic differentiation -// mathlib::MatX_T J_ad = integration::NumericalIntegrator::automaticDifferenceJacobian(f, t, x); -// // Compute the expected Jacobian manually -// mathlib::MatX_T J_expected(2, 2); -// J_expected << 4.0, 0.0, -// 3.0, 2.0; -// // Check that the computed Jacobian matches the expected Jacobian within a tolerance -// for (int i = 0; i < J_ad.rows(); ++i) { -// for (int j = 0; j < J_ad.cols(); ++j) { -// -// ASSERT_TRUE(std::abs(J_ad(i, j) - J_expected(i, j)) < EPS, -// "AD Jacobian entry (" + std::to_string(i) + ", " + std::to_string(j) + ") is incorrect"); -// } -// } -//} \ No newline at end of file + ASSERT_TRUE(abs(y.real - log(2.0)) < EPS, "Logarithm evaluation real part incorrect"); + ASSERT_TRUE(abs(y.dual[0] - 0.5) < EPS, "Logarithm derivative dual part incorrect"); +} \ No newline at end of file diff --git a/PxMLib/MathLib_Unit/DualNumberTests/DualNumber_Eigen_ArithmeticTests.cpp b/PxMLib/MathLib_Unit/DualNumberTests/DualNumber_Eigen_ArithmeticTests.cpp index bb4ea197..3aeb048e 100644 --- a/PxMLib/MathLib_Unit/DualNumberTests/DualNumber_Eigen_ArithmeticTests.cpp +++ b/PxMLib/MathLib_Unit/DualNumberTests/DualNumber_Eigen_ArithmeticTests.cpp @@ -1,5 +1,6 @@ +// MathLib_UnitTests DualNumber_Eigen_ArithmeticTests.cpp #include "TestHarness.h" -#include +#include using namespace mathlib; diff --git a/PxMLib/MathLib_Unit/DualNumberTests/DualNumber_Eigen_BasicTests.cpp b/PxMLib/MathLib_Unit/DualNumberTests/DualNumber_Eigen_BasicTests.cpp index e97d9f70..c01304af 100644 --- a/PxMLib/MathLib_Unit/DualNumberTests/DualNumber_Eigen_BasicTests.cpp +++ b/PxMLib/MathLib_Unit/DualNumberTests/DualNumber_Eigen_BasicTests.cpp @@ -1,5 +1,5 @@ #include "TestHarness.h" -#include +#include using namespace mathlib; diff --git a/PxMLib/MathLib_Unit/DualNumberTests/DualNumber_Eigen_SolverTests.cpp b/PxMLib/MathLib_Unit/DualNumberTests/DualNumber_Eigen_SolverTests.cpp index 0076132f..1b8433b6 100644 --- a/PxMLib/MathLib_Unit/DualNumberTests/DualNumber_Eigen_SolverTests.cpp +++ b/PxMLib/MathLib_Unit/DualNumberTests/DualNumber_Eigen_SolverTests.cpp @@ -1,5 +1,5 @@ #include "TestHarness.h" -#include +#include using namespace mathlib; @@ -18,7 +18,7 @@ TEST("DualNumber_T Eigen Solvers", MaxCoeff) { auto m = A.maxCoeff(); - ASSERT_TRUE(std::isfinite(m.real), "Max coefficient real part should be finite"); + ASSERT_TRUE(isfinite(m.real), "Max coefficient real part should be finite"); } TEST("DualNumber_T Eigen Solvers", CwiseAbsMaxCoeff) { Eigen::Matrix A; @@ -30,7 +30,7 @@ TEST("DualNumber_T Eigen Solvers", CwiseAbsMaxCoeff) { auto m = A.cwiseAbs().maxCoeff(); - ASSERT_TRUE(std::isfinite(m), "CwiseAbsMaxCoeff real part should be finite"); + ASSERT_TRUE(isfinite(m), "CwiseAbsMaxCoeff real part should be finite"); } TEST("DualNumber_T Eigen Solvers", PartialPivLU) { Eigen::Matrix A; @@ -42,8 +42,8 @@ TEST("DualNumber_T Eigen Solvers", PartialPivLU) { b << Dual(10.0, { 1.0 }), Dual(11.0, { 1.0 }); auto solver = A.partialPivLu(); auto x = solver.solve(b); - ASSERT_TRUE(std::isfinite(x(0).real), "Real part of solution should be finite"); - ASSERT_TRUE(std::isfinite(x(0).dual[0]), "Dual part of solution should be finite"); + ASSERT_TRUE(isfinite(x(0).real), "Real part of solution should be finite"); + ASSERT_TRUE(isfinite(x(0).dual[0]), "Dual part of solution should be finite"); } TEST("DualNumber_T Eigen Solvers", LDLTComputeOnly) { @@ -67,8 +67,8 @@ TEST("DualNumber_T Eigen Solvers", LDLT) { b << Dual(10.0, { 1.0 }), Dual(11.0, { 1.0 }); auto solver = A.ldlt(); auto x = solver.solve(b); - ASSERT_TRUE(std::isfinite(x(0).real), "Real part of solution should be finite"); - ASSERT_TRUE(std::isfinite(x(0).dual[0]), "Dual part of solution should be finite"); + ASSERT_TRUE(isfinite(x(0).real), "Real part of solution should be finite"); + ASSERT_TRUE(isfinite(x(0).dual[0]), "Dual part of solution should be finite"); } TEST("DualNumber_T Eigen Solvers", FullPivLUComputeAndSolve_DecompWithDualCasts) { diff --git a/PxMLib/MathLib_Unit/DualNumberTests/TranscendentalTests.cpp b/PxMLib/MathLib_Unit/DualNumberTests/TranscendentalTests.cpp index 4c6f4b22..493a4f78 100644 --- a/PxMLib/MathLib_Unit/DualNumberTests/TranscendentalTests.cpp +++ b/PxMLib/MathLib_Unit/DualNumberTests/TranscendentalTests.cpp @@ -1,7 +1,7 @@ // MathLib_UnitTests TranscendentalTests.cpp #include "TestHarness.h" -#include +#include #include #include From 4fd5fb71b5b1e2ba0c9aba51f19127de6138eeb2 Mon Sep 17 00:00:00 2001 From: saltyjoss Date: Mon, 25 May 2026 08:35:29 +0100 Subject: [PATCH 072/210] feat: Added new `IntegratorState` file into the `IntegrationService` and `Differentialntegrator` step logic for a given integrator choice * This will be shared with a shared_ptr --- .../include/Numerics/AutoDiffStep.inl | 32 ++++++++-- .../include/Numerics/IntegrationService.h | 7 ++ .../include/Numerics/IntegrationStep.inl | 64 +++++++++++++++---- .../include/Numerics/IntegratorState.h | 23 +++++++ .../include/Platform/ISimulationCore.h | 2 - .../include/Platform/SimulationState.h | 6 -- .../DSFE_Core/include/Scene/SimulationCore.h | 3 - .../DSFE_Core/src/Scene/SimulationCore.cpp | 3 - 8 files changed, 108 insertions(+), 32 deletions(-) create mode 100644 DSFE_App/DSFE_Core/include/Numerics/IntegratorState.h diff --git a/DSFE_App/DSFE_Core/include/Numerics/AutoDiffStep.inl b/DSFE_App/DSFE_Core/include/Numerics/AutoDiffStep.inl index a76e49cb..0d0d515a 100644 --- a/DSFE_App/DSFE_Core/include/Numerics/AutoDiffStep.inl +++ b/DSFE_App/DSFE_Core/include/Numerics/AutoDiffStep.inl @@ -14,14 +14,34 @@ namespace integration { Real dt_r = mathlib::real(dt); + _state->backend = eIntegrationBackend::AutoDiff; + _state->autoDiff = true; + _state->last_dt_taken = (_state->last_dt_taken != dt && !_state->adaptive) ? dt_r : _state->last_dt_taken; // Unless RK45-or other adaptive methods-it is static. + _state->last_dt_sug = (_state->last_dt_sug != dt && !_state->adaptive) ? dt_r : _state->last_dt_sug; // Unless RK45-or other adaptive methods-it is static. + + // Though right now all methods hold the same fundamental state in AD, I am employing the same structure for consistency switch (m) { - case eAutoDiffIntegrationMethod::AD_ImplicitEuler: return { _integrator->implicitEuler_AD(x, t, dt, std::forward(f), 8, 1e-6), dt_r, dt_r }; - case eAutoDiffIntegrationMethod::AD_ImplicitMidpoint: return { _integrator->implicitMidpoint_AD(x, t, dt, std::forward(f), 10, 1e-7), dt_r, dt_r }; - case eAutoDiffIntegrationMethod::AD_GLRK2: return { _integrator->GLRK2_AD(x, t, dt, std::forward(f), 80, 1e-10), dt_r, dt_r }; - case eAutoDiffIntegrationMethod::AD_GLRK3: return { _integrator->GLRK3_AD(x, t, dt, std::forward(f), 150, 1e-14), dt_r, dt_r }; + case eAutoDiffIntegrationMethod::AD_ImplicitEuler: + _state->name = IntegratorName(m); + _state->adaptive = false; _state->implicit = true; + return { _integrator->implicitEuler_AD(x, t, dt, std::forward(f), 8, 1e-6), dt_r, dt_r }; + case eAutoDiffIntegrationMethod::AD_ImplicitMidpoint: + _state->name = IntegratorName(m); + _state->adaptive = false; _state->implicit = true; + return { _integrator->implicitMidpoint_AD(x, t, dt, std::forward(f), 10, 1e-7), dt_r, dt_r }; + case eAutoDiffIntegrationMethod::AD_GLRK2: + _state->name = IntegratorName(m); + _state->adaptive = false; _state->implicit = true; + return { _integrator->GLRK2_AD(x, t, dt, std::forward(f), 80, 1e-10), dt_r, dt_r }; + case eAutoDiffIntegrationMethod::AD_GLRK3: + _state->name = IntegratorName(m); + _state->adaptive = false; _state->implicit = true; + return { _integrator->GLRK3_AD(x, t, dt, std::forward(f), 150, 1e-14), dt_r, dt_r }; default: - LOG_WARN("Unknown Integrator, defaulting to ImplicitEuler (AutoDiff)."); - return { _integrator->implicitEuler_AD(x, t, dt, std::forward(f), 8, 1e-6), dt_r, dt_r }; + LOG_WARN("Unknown Integrator, defaulting to ImplicitEuler (AutoDiff)."); + _state->name = IntegratorName(eAutoDiffIntegrationMethod::AD_ImplicitEuler); + _state->adaptive = false; _state->implicit = true; + return { _integrator->implicitEuler_AD(x, t, dt, std::forward(f), 8, 1e-6), dt_r, dt_r }; } } } // namespace integration \ No newline at end of file diff --git a/DSFE_App/DSFE_Core/include/Numerics/IntegrationService.h b/DSFE_App/DSFE_Core/include/Numerics/IntegrationService.h index 9a1f7dd3..34b02642 100644 --- a/DSFE_App/DSFE_Core/include/Numerics/IntegrationService.h +++ b/DSFE_App/DSFE_Core/include/Numerics/IntegrationService.h @@ -7,6 +7,7 @@ #include #include #include "Numerics/IntegrationMethods.h" +#include "Numerics/IntegratorState.h" #include "Platform/Logger.h" #include "EngineLib/LogMacros.h" @@ -37,6 +38,7 @@ namespace integration { void setAdaptiveTolerances(double rtol, double atol) { _rtol = rtol; _atol = atol; } void setMaxStep(double max_dt) { _dt_max = max_dt; } void resetAdaptiveState() { _dt_last = 0.0; } + std::shared_ptr runtimeState() const { return _state; } private: const char* toString(eIntegrationMethod m); @@ -45,6 +47,8 @@ namespace integration { std::unique_ptr _integrator; std::string _methodStr = "RK4"; + std::shared_ptr _state; + double _rtol; double _atol; double _dt_last = 0.0; // last successful step @@ -60,6 +64,7 @@ namespace integration { const std::string IntegratorName(eAutoDiffIntegrationMethod m); void setIntegrationMethod(eAutoDiffIntegrationMethod m) { _m = m; } eAutoDiffIntegrationMethod integrationMethod() const { return _m; } + std::shared_ptr runtimeState() const { return _state; } private: const char* toString(eAutoDiffIntegrationMethod m); @@ -67,6 +72,8 @@ namespace integration { integration::eAutoDiffIntegrationMethod _m; std::unique_ptr _integrator; std::string _mStr = "AD_ImplicitEuler"; + + std::shared_ptr _state; }; } // namespace integration diff --git a/DSFE_App/DSFE_Core/include/Numerics/IntegrationStep.inl b/DSFE_App/DSFE_Core/include/Numerics/IntegrationStep.inl index 16dfcb3d..6e768b3b 100644 --- a/DSFE_App/DSFE_Core/include/Numerics/IntegrationStep.inl +++ b/DSFE_App/DSFE_Core/include/Numerics/IntegrationStep.inl @@ -11,24 +11,61 @@ namespace integration { } } + _state->backend = eIntegrationBackend::Standard; + _state->last_dt_taken = (_state->last_dt_taken != dt && !_state->adaptive) ? dt : _state->last_dt_taken; + _state->last_dt_sug = (_state->last_dt_sug != dt && !_state->adaptive) ? dt : _state->last_dt_sug; + _state->autoDiff = false; + switch (m) { // Explicit methods // * currently all explicit methods use fixed step size, apart from RK45 as it is an adaptive method - case eIntegrationMethod::Euler: return { _integrator->eulerStep(x, t, dt, std::forward(f)), dt, dt }; - case eIntegrationMethod::Midpoint: return { _integrator->midpointStep(x, t, dt, std::forward(f)), dt, dt }; - case eIntegrationMethod::Heun: return { _integrator->heunStep(x, t, dt, std::forward(f)), dt, dt }; - case eIntegrationMethod::Ralston: return { _integrator->ralstonStep(x, t, dt, std::forward(f)), dt, dt }; - case eIntegrationMethod::RK4: return { _integrator->rk4Step(x, t, dt, std::forward(f)), dt, dt }; - case eIntegrationMethod::RK45: return step_adaptive(eIntegrationMethod::RK45, x, t, dt, std::forward(f), _rtol, _atol); + case eIntegrationMethod::Euler: + _state->name = IntegratorName(m); // all of these might add more time complexity than what is needed. + _state->adaptive = false; _state->implicit = false; + return { _integrator->eulerStep(x, t, dt, std::forward(f)), dt, dt }; + case eIntegrationMethod::Midpoint: + _state->name = IntegratorName(m); + _state->adaptive = false; _state->implicit = false; + return { _integrator->midpointStep(x, t, dt, std::forward(f)), dt, dt }; + case eIntegrationMethod::Heun: + _state->name = IntegratorName(m); + _state->adaptive = false; _state->implicit = false; + return { _integrator->heunStep(x, t, dt, std::forward(f)), dt, dt }; + case eIntegrationMethod::Ralston: + _state->name = IntegratorName(m); + _state->adaptive = false; _state->implicit = false; + return { _integrator->ralstonStep(x, t, dt, std::forward(f)), dt, dt }; + case eIntegrationMethod::RK4: + _state->name = IntegratorName(m); + _state->adaptive = false; _state->implicit = false; + return { _integrator->rk4Step(x, t, dt, std::forward(f)), dt, dt }; + case eIntegrationMethod::RK45: + _state->name = IntegratorName(m); + _state->adaptive = true; _state->implicit = false; + return step_adaptive(eIntegrationMethod::RK45, x, t, dt, std::forward(f), _rtol, _atol); // Implicit methods // * currently use fixed step size (no error estimation), but are likely to support adaptive stepping in the future - case eIntegrationMethod::ImplicitEuler: return { _integrator->implicitEuler(x, t, dt, std::forward(f), std::forward(jac)), dt, dt }; - case eIntegrationMethod::ImplicitMidpoint: return { _integrator->implicitMidpoint(x, t, dt, std::forward(f), std::forward(jac)), dt, dt }; - case eIntegrationMethod::GLRK2: return { _integrator->GLRK2(x, t, dt, std::forward(f), std::forward(jac), 80, 1e-10), dt, dt }; - case eIntegrationMethod::GLRK3: return { _integrator->GLRK3(x, t, dt, std::forward(f), std::forward(jac), 150, 1e-14), dt, dt }; + case eIntegrationMethod::ImplicitEuler: + _state->name = IntegratorName(m); + _state->adaptive = false; _state->implicit = true; + return { _integrator->implicitEuler(x, t, dt, std::forward(f), std::forward(jac)), dt, dt }; + case eIntegrationMethod::ImplicitMidpoint: + _state->name = IntegratorName(m); + _state->adaptive = false; _state->implicit = true; + return { _integrator->implicitMidpoint(x, t, dt, std::forward(f), std::forward(jac)), dt, dt }; + case eIntegrationMethod::GLRK2: + _state->name = IntegratorName(m); + _state->adaptive = false; _state->implicit = true; + return { _integrator->GLRK2(x, t, dt, std::forward(f), std::forward(jac), 80, 1e-10), dt, dt }; + case eIntegrationMethod::GLRK3: + _state->name = IntegratorName(m); + _state->adaptive = false; _state->implicit = true; + return { _integrator->GLRK3(x, t, dt, std::forward(f), std::forward(jac), 150, 1e-14), dt, dt }; default: - LOG_WARN("Unknown integration method: %s. Defaulting to RK4.", toString(m)); - return { _integrator->rk4Step(x, t, dt, std::forward(f)), dt, dt }; + LOG_WARN("Unknown integration method: %s. Defaulting to RK4.", toString(m)); + _state->name = IntegratorName(eIntegrationMethod::RK4); + _state->adaptive = false; _state->implicit = false; + return { _integrator->rk4Step(x, t, dt, std::forward(f)), dt, dt }; } } @@ -87,6 +124,9 @@ namespace integration { LOG_WARN("Adaptive integration exceeded maximum substeps (%d) at time %f. Returning last computed state.", max_substeps, t_curr); } + _state->last_dt_taken = t_total; + _state->last_dt_sug = h; + // Persist the last good step size for next frame _dt_last = h; return { x_curr, t_total, h }; diff --git a/DSFE_App/DSFE_Core/include/Numerics/IntegratorState.h b/DSFE_App/DSFE_Core/include/Numerics/IntegratorState.h new file mode 100644 index 00000000..f314a738 --- /dev/null +++ b/DSFE_App/DSFE_Core/include/Numerics/IntegratorState.h @@ -0,0 +1,23 @@ +// DSFE_Core IntegratorState.h +#pragma once + +#include + +namespace integration { + // Current simulation backend integration method (e.g., standard numerical integration[explicit, implicit] vs. auto-differentiation for gradients) + enum class eIntegrationBackend { + Standard, + AutoDiff + }; + + struct IntegratorState { + std::string name; + eIntegrationBackend backend = eIntegrationBackend::Standard; + bool adaptive = false; + bool implicit = false; // false means explicit, TODO adapt this correctly for semi- once integrated + bool autoDiff = false; + + double last_dt_taken = 0.0; + double last_dt_sug = 0.0; + }; +} \ No newline at end of file diff --git a/DSFE_App/DSFE_Core/include/Platform/ISimulationCore.h b/DSFE_App/DSFE_Core/include/Platform/ISimulationCore.h index 16b75fed..7135fc68 100644 --- a/DSFE_App/DSFE_Core/include/Platform/ISimulationCore.h +++ b/DSFE_App/DSFE_Core/include/Platform/ISimulationCore.h @@ -31,8 +31,6 @@ namespace core { virtual void startSimulation() = 0; virtual void stopSimulation() = 0; virtual bool isSimRunning() const = 0; - virtual void setSimulationBackend(eSimulationBackend backend) = 0; - virtual eSimulationBackend simulationBackend() const = 0; // Time stepping virtual void setFixedDt(double dt) = 0; virtual double fixedDt() const = 0; diff --git a/DSFE_App/DSFE_Core/include/Platform/SimulationState.h b/DSFE_App/DSFE_Core/include/Platform/SimulationState.h index 77bdd2ec..50173444 100644 --- a/DSFE_App/DSFE_Core/include/Platform/SimulationState.h +++ b/DSFE_App/DSFE_Core/include/Platform/SimulationState.h @@ -32,16 +32,10 @@ enum class eRunMode { Interactive, Synchronous }; -// Current simulation backend integration method (e.g., standard numerical integration[explicit, implicit] vs. auto-differentiation for gradients) -enum class eSimulationBackend { - Standard, - AutoDiff -}; // Struct to hold the current simulation mode and related settings struct DSFE_API modes { eRunMode _runMode = eRunMode::Interactive; - eSimulationBackend _simBackend = eSimulationBackend::Standard; }; diff --git a/DSFE_App/DSFE_Core/include/Scene/SimulationCore.h b/DSFE_App/DSFE_Core/include/Scene/SimulationCore.h index 46bff490..fd3ae603 100644 --- a/DSFE_App/DSFE_Core/include/Scene/SimulationCore.h +++ b/DSFE_App/DSFE_Core/include/Scene/SimulationCore.h @@ -47,8 +47,6 @@ namespace core { void startSimulation() override; void stopSimulation() override; bool isSimRunning() const override { return _simRunning.load(); } - void setSimulationBackend(eSimulationBackend backend) override; - eSimulationBackend simulationBackend() const override; // Time stepping void setFixedDt(double dt) override; @@ -166,7 +164,6 @@ namespace core { // Run mode eRunMode _runMode = eRunMode::Interactive; - eSimulationBackend _simBackend = eSimulationBackend::Standard; // Last script text for comparison re-use std::string _lastScriptText; diff --git a/DSFE_App/DSFE_Core/src/Scene/SimulationCore.cpp b/DSFE_App/DSFE_Core/src/Scene/SimulationCore.cpp index 9c01e59a..676df6e1 100644 --- a/DSFE_App/DSFE_Core/src/Scene/SimulationCore.cpp +++ b/DSFE_App/DSFE_Core/src/Scene/SimulationCore.cpp @@ -68,9 +68,6 @@ namespace core { return _robot->getADIntegrator()->integrationMethod(); } - void SimulationCore::setSimulationBackend(eSimulationBackend backend) { _simBackend = backend; } - eSimulationBackend SimulationCore::simulationBackend() const { return _simBackend; } - // Fixed timestep loop for physics and robot updates, called from the main render loop with the frame delta time void SimulationCore::stepFixed(double frame_dt) { double simTime = _simTime.load(); From 965c732777c2bc5be53937d9491c847913bc3ad5 Mon Sep 17 00:00:00 2001 From: saltyjoss Date: Mon, 25 May 2026 09:12:39 +0100 Subject: [PATCH 073/210] feat: Added basic shared ptr accessor via RobotSystem's `runtimeIntegrationState` --- DSFE_App/DSFE_Core/include/Robots/RobotSystem.h | 6 ++++-- DSFE_App/DSFE_Core/include/Scene/SimulationCore.h | 2 ++ DSFE_App/DSFE_Core/src/Numerics/IntegrationService.cpp | 6 ++++-- DSFE_App/DSFE_Core/src/Robots/RobotSystem.cpp | 4 ++++ 4 files changed, 14 insertions(+), 4 deletions(-) diff --git a/DSFE_App/DSFE_Core/include/Robots/RobotSystem.h b/DSFE_App/DSFE_Core/include/Robots/RobotSystem.h index 86372932..1f3935ed 100644 --- a/DSFE_App/DSFE_Core/include/Robots/RobotSystem.h +++ b/DSFE_App/DSFE_Core/include/Robots/RobotSystem.h @@ -151,12 +151,12 @@ namespace robots { // --- GET AND SET INTEGRATION METHOD --- integration::eIntegrationMethod getIntegrationMethod() const { return _curIntMethod; } - void setIntegrationMethod(integration::eIntegrationMethod method) { _curIntMethod = method; } std::string getIntegratorName() const { return _integrator->IntegratorName(_curIntMethod); } + void setIntegrationMethod(integration::eIntegrationMethod method) { _curIntMethod = method; } integration::eAutoDiffIntegrationMethod AD_IntegrationMethod() const { return _curIntMethod_AD; } - void setIntegrationMethod(integration::eAutoDiffIntegrationMethod method) { _curIntMethod_AD = method; } std::string AD_integratorName() const { return _AD_integrator->IntegratorName(_curIntMethod_AD); } + void setIntegrationMethod(integration::eAutoDiffIntegrationMethod method) { _curIntMethod_AD = method; } integration::IntegrationService* getIntegrator(); const integration::IntegrationService* getIntegrator() const; @@ -164,6 +164,8 @@ namespace robots { integration::DifferentiableIntegrator* getADIntegrator(); const integration::DifferentiableIntegrator* getADIntegrator() const; + std::shared_ptr runtimeIntegratorState() const; + void setRefBuffer(robots::TrajRefBuffer* buf) { _refBuffer = buf; } void setLogBuffer(robots::JointLogBuffer* buf) { _logBuffer = buf; } void setRole(eRole role) { _role = role; } diff --git a/DSFE_App/DSFE_Core/include/Scene/SimulationCore.h b/DSFE_App/DSFE_Core/include/Scene/SimulationCore.h index fd3ae603..feea020e 100644 --- a/DSFE_App/DSFE_Core/include/Scene/SimulationCore.h +++ b/DSFE_App/DSFE_Core/include/Scene/SimulationCore.h @@ -11,6 +11,7 @@ #include "Platform/ISimulationCore.h" #include "Platform/SimulationState.h" +#include "Numerics/IntegratorState.h" #include "Analysis/Telemetry.h" #include "Platform/DataManager.h" @@ -70,6 +71,7 @@ namespace core { std::string integrationMethodName() const override; integration::eIntegrationMethod integrationMethod() const override; integration::eAutoDiffIntegrationMethod autoDiffIntegrationMethod() const override; + void setRunTag(const std::string& tag) override { _runTag = tag; } // Subsystems access diff --git a/DSFE_App/DSFE_Core/src/Numerics/IntegrationService.cpp b/DSFE_App/DSFE_Core/src/Numerics/IntegrationService.cpp index 7e2e240e..a0273000 100644 --- a/DSFE_App/DSFE_Core/src/Numerics/IntegrationService.cpp +++ b/DSFE_App/DSFE_Core/src/Numerics/IntegrationService.cpp @@ -61,11 +61,13 @@ namespace integration { // Constructor IntegrationService::IntegrationService() - : _integrator(std::make_unique()), method(eIntegrationMethod::RK4), _rtol(1e-3), _atol(1e-6), _dt_last(), _dt_max() { + : _integrator(std::make_unique()), method(eIntegrationMethod::RK4), + _state(std::make_shared()), _rtol(1e-3), _atol(1e-6), _dt_last(), _dt_max() { } // Constructor for autodiff DifferentiableIntegrator::DifferentiableIntegrator() - : _integrator(std::make_unique()), _m(eAutoDiffIntegrationMethod::AD_ImplicitEuler) { + : _integrator(std::make_unique()), _m(eAutoDiffIntegrationMethod::AD_ImplicitEuler), + _state(std::make_shared()) { } } // namespace integration \ No newline at end of file diff --git a/DSFE_App/DSFE_Core/src/Robots/RobotSystem.cpp b/DSFE_App/DSFE_Core/src/Robots/RobotSystem.cpp index b3e7a0cc..1e28a0d9 100644 --- a/DSFE_App/DSFE_Core/src/Robots/RobotSystem.cpp +++ b/DSFE_App/DSFE_Core/src/Robots/RobotSystem.cpp @@ -431,6 +431,10 @@ namespace robots { integration::DifferentiableIntegrator* RobotSystem::getADIntegrator() { return _AD_integrator.get(); } const integration::DifferentiableIntegrator* RobotSystem::getADIntegrator() const { return _AD_integrator.get(); } + std::shared_ptr RobotSystem::runtimeIntegratorState() const { + return (_integrator->getIntegrationMethod() == _curIntMethod) ? _integrator->runtimeState() : _AD_integrator->runtimeState(); + } + // --- ROBOT KINEMATICS AND JOINT STATE METHODS --- std::string RobotSystem::findRootLink() const { From 9b7b4f8c9cf249f0492dbb83dd9f21e9cbc9b445 Mon Sep 17 00:00:00 2001 From: saltyjoss Date: Mon, 25 May 2026 09:22:59 +0100 Subject: [PATCH 074/210] refactor: Updated `stepFixed` and `runScriptToCompletion` to reflect step logic for AD and non relative to state. --- DSFE_App/DSFE_Core/src/Scene/SimulationCore.cpp | 11 ++++++++--- 1 file changed, 8 insertions(+), 3 deletions(-) diff --git a/DSFE_App/DSFE_Core/src/Scene/SimulationCore.cpp b/DSFE_App/DSFE_Core/src/Scene/SimulationCore.cpp index 676df6e1..f01d8087 100644 --- a/DSFE_App/DSFE_Core/src/Scene/SimulationCore.cpp +++ b/DSFE_App/DSFE_Core/src/Scene/SimulationCore.cpp @@ -101,6 +101,8 @@ namespace core { _scriptRunning.store(false); } + const auto state = _robot->runtimeIntegratorState(); + // Update physics and robot system if sim is running if (_simRunning.load()) { simTime += _dt; @@ -108,7 +110,8 @@ namespace core { // Update robot trajectory inputs and step the robot forward in time if (hasRobot()) { _robot->updateTrajectoryInputs(*_traj, simTime); - _robot->step_AD(_dt, simTime); + if (state && state->autoDiff) { _robot->step_AD(_dt, simTime); } + else { _robot->step(_dt, simTime); } // Telemetry update if (!_telemetryBegun) { _telemetry.beginRun(simTime, _telHz, 300.0); @@ -314,6 +317,8 @@ namespace core { // Step the program (DSL command execution) program->step(dt); + const auto state = _robot->runtimeIntegratorState(); + // Step physics and robot if sim is running if (_simRunning.load()) { simTime += dt; @@ -322,8 +327,8 @@ namespace core { // Update Trajectory Inputs _robot->updateTrajectoryInputs(*_traj, simTime); - // Step robot system - _robot->step(dt, simTime); + if (state && state->autoDiff) { _robot->step_AD(dt, simTime); } + else { _robot->step(dt, simTime); } // Telemetry beginRun if (!_telemetryBegun) { From b4e5a37cb54364267fb407e72058b1c7a1811e62 Mon Sep 17 00:00:00 2001 From: saltyjoss Date: Mon, 25 May 2026 09:49:17 +0100 Subject: [PATCH 075/210] refactor: Updated `simulationProperties` to support AutoDiff integrators * Still crashing on run, but after robot is loaded, need to maybe add direct assignment in the respective setIntegrator --- DSFE_App/DSFE_GUI/include/ui/ControlPanel.h | 1 + DSFE_App/DSFE_GUI/src/ui/ControlPanel.cpp | 125 +++++++++++++------- 2 files changed, 83 insertions(+), 43 deletions(-) diff --git a/DSFE_App/DSFE_GUI/include/ui/ControlPanel.h b/DSFE_App/DSFE_GUI/include/ui/ControlPanel.h index 7a55f7c2..867f3df9 100644 --- a/DSFE_App/DSFE_GUI/include/ui/ControlPanel.h +++ b/DSFE_App/DSFE_GUI/include/ui/ControlPanel.h @@ -160,6 +160,7 @@ namespace gui { bool _showDisplaySettings = false; bool autoScroll = true; bool scrollToBottom = false; + bool _useAutoDiff = false; // Render Presets render::ResolutionPreset r; diff --git a/DSFE_App/DSFE_GUI/src/ui/ControlPanel.cpp b/DSFE_App/DSFE_GUI/src/ui/ControlPanel.cpp index 7209d603..d4f78287 100644 --- a/DSFE_App/DSFE_GUI/src/ui/ControlPanel.cpp +++ b/DSFE_App/DSFE_GUI/src/ui/ControlPanel.cpp @@ -203,11 +203,12 @@ namespace gui { // --- ControlPanel Implementation --- - ControlPanel::ControlPanel(SimManager* sim) : + ControlPanel::ControlPanel(SimManager* sim) : _sim(sim), _controlMode(&sim->ctrlMode), _obj(nullptr), _light(nullptr), r(render::ResolutionPreset::R_1080p), q(render::QualityPreset::Medium), - _meshLoad(ImGuiFileBrowserFlags_CloseOnEsc | ImGuiFileBrowserFlags_NoModal), - _hdrLoad(ImGuiFileBrowserFlags_CloseOnEsc | ImGuiFileBrowserFlags_NoModal) + _meshLoad(ImGuiFileBrowserFlags_CloseOnEsc | ImGuiFileBrowserFlags_NoModal), + _hdrLoad(ImGuiFileBrowserFlags_CloseOnEsc | ImGuiFileBrowserFlags_NoModal), + _useAutoDiff(false) { diagTime = 0.0f; // initialize diagnostic time simTime = 0.0f; // initialize simulation time @@ -420,59 +421,97 @@ namespace gui { robots::RobotSystem* robot = _sim->robotSystem(); auto currentIntEnum = robot->getIntegrationMethod(); + auto currentIntEnum_AD = robot->AD_IntegrationMethod(); auto currentTauEnum = robot->getTorqueMode(); static const char* intMethodNames[] = { "Euler", "Midpoint", "Heun", "Ralston", "RK4", "RK45", "Implicit Euler", "Implicit Midpoint", "GLRK2", "GLRK3" }; const char* currentIntMethod = intMethodNames[static_cast(currentIntEnum)]; + static const char* intMethodNames_AD[] = { "Implicit Euler (AutoDiff)", "Implicit Midpoint (AutoDiff)", "GLRK2 (AutoDiff)", "GLRK3 (AutoDiff)" }; + const char* currentIntMethod_AD = intMethodNames_AD[static_cast(currentIntEnum_AD)]; + static const char* torqueModeNames[] = { "None", "Passive", "Controlled" }; const char* currentTorqueMode = torqueModeNames[static_cast(currentTauEnum)]; // Disable controls while sim is running to prevent conflicts and ensure stability of the simulations ImGui::BeginDisabled(_sim->isSimRunning()); - + // Integration method combo box - ImGui::SectionHeader("Integration Method"); + ImGui::SectionHeader("Integration Methods"); + ImGui::SetNextItemWidth(150.0f); - if (ImGui::BeginCombo("##", currentIntMethod)) { - for (int n = 0; n < IM_ARRAYSIZE(intMethodNames); ++n) { - bool isSelected = (n == static_cast(currentIntEnum)); - - // When a new method is selected, update the robot's integration method and log the change - if (ImGui::Selectable(intMethodNames[n], isSelected)) { - auto updatedMethod = static_cast(n); - robot->setIntegrationMethod(updatedMethod); - - switch (updatedMethod) { - case integration::eIntegrationMethod::Euler: - D_INFO("Integrator set to Euler"); break; - case integration::eIntegrationMethod::Midpoint: - D_INFO("Integrator set to RK2 (Midpoint)"); break; - case integration::eIntegrationMethod::Heun: - D_INFO("Integrator set to RK2 (Heun)"); break; - case integration::eIntegrationMethod::Ralston: - D_INFO("Integrator set to RK2 (Ralston)"); break; - case integration::eIntegrationMethod::RK4: - D_INFO("Integrator set to RK4"); break; - case integration::eIntegrationMethod::RK45: - D_INFO("Integrator set to RK45 (Dormand-Prince)"); break; - case integration::eIntegrationMethod::ImplicitEuler: - D_INFO("Integrator set to Implicit Euler"); break; - case integration::eIntegrationMethod::ImplicitMidpoint: - D_INFO("Integrator set to Implicit Midpoint"); break; - case integration::eIntegrationMethod::GLRK2: - D_INFO("Integrator set to GLRK2 (Gauss-Legendre Runge-Kutta 2-stage)"); break; - case integration::eIntegrationMethod::GLRK3: - D_INFO("Integrator set to GLRK3 (Gauss-Legendre Runge-Kutta 3-stage)"); break; - default: - break; - } - } - if (isSelected) { ImGui::SetItemDefaultFocus(); } - } - ImGui::EndCombo(); - } + ImGui::Checkbox("Enable Automatic Differentiable Integrators", &_useAutoDiff); + + ImGui::SetNextItemWidth(150.0f); + if (_useAutoDiff) { + if (ImGui::BeginCombo("##", currentIntMethod_AD)) { + for (int n = 0; n < IM_ARRAYSIZE(intMethodNames_AD); ++n) { + bool isSelected = (n == static_cast(currentIntEnum_AD)); + + // When a new method is selected, update the robot's integration method and log the change + if (ImGui::Selectable(intMethodNames_AD[n], isSelected)) { + auto updatedMethod = static_cast(n); + robot->setIntegrationMethod(updatedMethod); + + switch (updatedMethod) { + case integration::eAutoDiffIntegrationMethod::AD_ImplicitEuler: + D_INFO("Integrator set to Implicit Euler (AutoDiff)"); break; + case integration::eAutoDiffIntegrationMethod::AD_ImplicitMidpoint: + D_INFO("Integrator set to Implicit Midpoint (AutoDiff)"); break; + case integration::eAutoDiffIntegrationMethod::AD_GLRK2: + D_INFO("Integrator set to GLRK2 (AutoDiff Gauss-Legendre Runge-Kutta 2-stage)"); break; + case integration::eAutoDiffIntegrationMethod::AD_GLRK3: + D_INFO("Integrator set to GLRK3 (AutoDiff Gauss-Legendre Runge-Kutta 3-stage)"); break; + default: + break; + } + } + if (isSelected) { ImGui::SetItemDefaultFocus(); } + } + ImGui::EndCombo(); + } + } + else { + if (ImGui::BeginCombo("##", currentIntMethod)) { + for (int n = 0; n < IM_ARRAYSIZE(intMethodNames); ++n) { + bool isSelected = (n == static_cast(currentIntEnum)); + + // When a new method is selected, update the robot's integration method and log the change + if (ImGui::Selectable(intMethodNames[n], isSelected)) { + auto updatedMethod = static_cast(n); + robot->setIntegrationMethod(updatedMethod); + + switch (updatedMethod) { + case integration::eIntegrationMethod::Euler: + D_INFO("Integrator set to Euler"); break; + case integration::eIntegrationMethod::Midpoint: + D_INFO("Integrator set to RK2 (Midpoint)"); break; + case integration::eIntegrationMethod::Heun: + D_INFO("Integrator set to RK2 (Heun)"); break; + case integration::eIntegrationMethod::Ralston: + D_INFO("Integrator set to RK2 (Ralston)"); break; + case integration::eIntegrationMethod::RK4: + D_INFO("Integrator set to RK4"); break; + case integration::eIntegrationMethod::RK45: + D_INFO("Integrator set to RK45 (Dormand-Prince)"); break; + case integration::eIntegrationMethod::ImplicitEuler: + D_INFO("Integrator set to Implicit Euler"); break; + case integration::eIntegrationMethod::ImplicitMidpoint: + D_INFO("Integrator set to Implicit Midpoint"); break; + case integration::eIntegrationMethod::GLRK2: + D_INFO("Integrator set to GLRK2 (Gauss-Legendre Runge-Kutta 2-stage)"); break; + case integration::eIntegrationMethod::GLRK3: + D_INFO("Integrator set to GLRK3 (Gauss-Legendre Runge-Kutta 3-stage)"); break; + default: + break; + } + } + if (isSelected) { ImGui::SetItemDefaultFocus(); } + } + ImGui::EndCombo(); + } + } // Torque mode combo box ImGui::SectionHeader("Torque Mode"); From 077e3d6b5eaa26578d6f3e1a9d32cb98127eb3f7 Mon Sep 17 00:00:00 2001 From: saltyjoss Date: Mon, 25 May 2026 10:05:10 +0100 Subject: [PATCH 076/210] feat: Added general state backend and autodiff conditional setters in `setIntegrator()` methods * I actually think the problem is in `SimManager`, debugging that now, but due to forgetting to commit previous changes yesterday I am commit more frequently. --- .../include/Numerics/IntegrationService.h | 2 ++ DSFE_App/DSFE_Core/include/Robots/RobotSystem.h | 14 ++++++++++++-- 2 files changed, 14 insertions(+), 2 deletions(-) diff --git a/DSFE_App/DSFE_Core/include/Numerics/IntegrationService.h b/DSFE_App/DSFE_Core/include/Numerics/IntegrationService.h index 34b02642..1c895c31 100644 --- a/DSFE_App/DSFE_Core/include/Numerics/IntegrationService.h +++ b/DSFE_App/DSFE_Core/include/Numerics/IntegrationService.h @@ -38,6 +38,7 @@ namespace integration { void setAdaptiveTolerances(double rtol, double atol) { _rtol = rtol; _atol = atol; } void setMaxStep(double max_dt) { _dt_max = max_dt; } void resetAdaptiveState() { _dt_last = 0.0; } + std::shared_ptr runtimeState() { return _state; } std::shared_ptr runtimeState() const { return _state; } private: @@ -64,6 +65,7 @@ namespace integration { const std::string IntegratorName(eAutoDiffIntegrationMethod m); void setIntegrationMethod(eAutoDiffIntegrationMethod m) { _m = m; } eAutoDiffIntegrationMethod integrationMethod() const { return _m; } + std::shared_ptr runtimeState() { return _state; } std::shared_ptr runtimeState() const { return _state; } private: diff --git a/DSFE_App/DSFE_Core/include/Robots/RobotSystem.h b/DSFE_App/DSFE_Core/include/Robots/RobotSystem.h index 1f3935ed..692bcf4f 100644 --- a/DSFE_App/DSFE_Core/include/Robots/RobotSystem.h +++ b/DSFE_App/DSFE_Core/include/Robots/RobotSystem.h @@ -152,11 +152,21 @@ namespace robots { integration::eIntegrationMethod getIntegrationMethod() const { return _curIntMethod; } std::string getIntegratorName() const { return _integrator->IntegratorName(_curIntMethod); } - void setIntegrationMethod(integration::eIntegrationMethod method) { _curIntMethod = method; } + void setIntegrationMethod(integration::eIntegrationMethod method) { + _curIntMethod = method; + const auto state = _integrator->runtimeState(); + state->backend = integration::eIntegrationBackend::Standard; + state->autoDiff = false; + } integration::eAutoDiffIntegrationMethod AD_IntegrationMethod() const { return _curIntMethod_AD; } std::string AD_integratorName() const { return _AD_integrator->IntegratorName(_curIntMethod_AD); } - void setIntegrationMethod(integration::eAutoDiffIntegrationMethod method) { _curIntMethod_AD = method; } + void setIntegrationMethod(integration::eAutoDiffIntegrationMethod method) { + _curIntMethod_AD = method; + const auto state = _AD_integrator->runtimeState(); + state->backend = integration::eIntegrationBackend::AutoDiff; + state->autoDiff = true; + } integration::IntegrationService* getIntegrator(); const integration::IntegrationService* getIntegrator() const; From f9cb88ecd2490a7c5b93fd0d71859723d4ae7a6e Mon Sep 17 00:00:00 2001 From: saltyjoss Date: Mon, 25 May 2026 11:13:22 +0100 Subject: [PATCH 077/210] fixes(WIP): Added temporary fix for previously not resized scratch and result definitions in `step_impl`, need to furhter explore BETTER methods * This is why we have a logger... looks cool and helps generally but is a godsend when debugging! --- DSFE_App/DSFE_Core/include/Robots/RobotSystemStep.inl | 9 +++++++++ DSFE_App/DSFE_Core/src/Robots/RobotSystem.cpp | 7 ++++++- DSFE_App/DSFE_Core/src/Scene/SimulationCore.cpp | 3 ++- 3 files changed, 17 insertions(+), 2 deletions(-) diff --git a/DSFE_App/DSFE_Core/include/Robots/RobotSystemStep.inl b/DSFE_App/DSFE_Core/include/Robots/RobotSystemStep.inl index 0d149206..ea11e0e5 100644 --- a/DSFE_App/DSFE_Core/include/Robots/RobotSystemStep.inl +++ b/DSFE_App/DSFE_Core/include/Robots/RobotSystemStep.inl @@ -60,6 +60,10 @@ namespace robots { SpatialModel spatialModel = _spatialModel.template cast(); DynamicsScratch dynScratch; DynamicsResult dynResult; + dynScratch.resize(n, snap.model->links.size()); + LOG_INFO("dynScratch resized"); + dynResult.resize(n); + LOG_INFO("dynResult resized"); SpatialDynamics::computeSpatialKinematicsAndBias( spatialModel, @@ -69,18 +73,23 @@ namespace robots { ); // CRBA only for controller inertia scaling + LOG_INFO("Computing M_start via CRBA"); mathlib::MatX_T M_start = SpatialDynamics::CRBA( spatialModel, dynScratch.spatial.Xup, dynScratch ); + LOG_INFO("M_start dims = %d x %d", (int)M_start.rows(), (int)M_start.cols()); // Cache frozen joint gains for this step mathlib::VecX_T kp_frozen(n), kd_frozen(n); for (size_t i = 0; i < n; ++i) { const auto& joint = snap.model->joints[i]; + LOG_INFO("Snap model joint name = %s", joint.name.c_str()); if (joint.type == eJointType::FIXED) { continue; } + LOG_INFO("Max of M_start and 1e-6: %.3f", mathlib::max(M_start(i, i), Scalar(1e-6))); + LOG_INFO("I_eff_controller size = %d", (int)dynScratch.dense.I_eff_controller.size()); dynScratch.dense.I_eff_controller[i] = mathlib::max(M_start(i, i), Scalar(1e-6)); const Scalar I_eff = dynScratch.dense.I_eff_controller[i]; diff --git a/DSFE_App/DSFE_Core/src/Robots/RobotSystem.cpp b/DSFE_App/DSFE_Core/src/Robots/RobotSystem.cpp index 1e28a0d9..4c6424c7 100644 --- a/DSFE_App/DSFE_Core/src/Robots/RobotSystem.cpp +++ b/DSFE_App/DSFE_Core/src/Robots/RobotSystem.cpp @@ -249,14 +249,19 @@ namespace robots { // Method to advance the robot state by dt using the selected integrator void RobotSystem::step(double dt, double simTime) { if (!_hasRobot) { return; } + LOG_INFO("Entered step()"); + _simTime = simTime; const size_t n = _robot.joints.size(); + LOG_INFO("Packing state"); mathlib::VecX x = packState(); auto result = step_impl(x, dt, simTime, *_integrator); + LOG_INFO("Unpacking state"); unpackState(result.stepOut.x_next); _dynamics->setDt(result.stepOut.dt_taken); + LOG_INFO("PostStepUpadates state"); postStepUpdate(result, result.stepOut.x_next); // Update base pose if free-floating @@ -385,7 +390,7 @@ namespace robots { // Method to reset the robot to its home position void RobotSystem::resetRobot() { - if (!_hasRobot || !_robotHomeValid) { return; } + if (!_hasRobot || !_robotHomeValid) { LOG_ERROR("Reset aborterd."); return; } _robotRootPose = _robotRootHome; _robot.setJointVector(_robotQHome); diff --git a/DSFE_App/DSFE_Core/src/Scene/SimulationCore.cpp b/DSFE_App/DSFE_Core/src/Scene/SimulationCore.cpp index f01d8087..6eb8d773 100644 --- a/DSFE_App/DSFE_Core/src/Scene/SimulationCore.cpp +++ b/DSFE_App/DSFE_Core/src/Scene/SimulationCore.cpp @@ -36,7 +36,7 @@ namespace core { // Simulation System void SimulationCore::setupSimulationIntegrator() { - if (!_robot) return; + if (!_robot) { return; } auto* intgr = _robot->getIntegrator(); intgr->resetAdaptiveState(); intgr->setAdaptiveTolerances(1e-3, 1e-6); @@ -137,6 +137,7 @@ namespace core { if (_simRunning.load()) { return; } telemetry().clear(); D_RUNTIME("starting simulation"); + LOG_INFO("Starting Simulation -> Debug Log"); _simTime.store(0.0, std::memory_order_relaxed); _accum = 0.0; From b8219de67ab9562069fdf221b2c6e1ebbb8620b9 Mon Sep 17 00:00:00 2001 From: saltyjoss Date: Mon, 25 May 2026 11:15:49 +0100 Subject: [PATCH 078/210] fixes: Removal of internal debugging logs. TODO * Once confirmed this is fully working I am going to change the logic to have a template scratch cache owned by trhe executation context --- .../DSFE_Core/include/Robots/RobotSystemStep.inl | 14 ++++++-------- DSFE_App/DSFE_Core/src/Robots/RobotSystem.cpp | 4 ---- 2 files changed, 6 insertions(+), 12 deletions(-) diff --git a/DSFE_App/DSFE_Core/include/Robots/RobotSystemStep.inl b/DSFE_App/DSFE_Core/include/Robots/RobotSystemStep.inl index ea11e0e5..140444c8 100644 --- a/DSFE_App/DSFE_Core/include/Robots/RobotSystemStep.inl +++ b/DSFE_App/DSFE_Core/include/Robots/RobotSystemStep.inl @@ -61,9 +61,7 @@ namespace robots { DynamicsScratch dynScratch; DynamicsResult dynResult; dynScratch.resize(n, snap.model->links.size()); - LOG_INFO("dynScratch resized"); dynResult.resize(n); - LOG_INFO("dynResult resized"); SpatialDynamics::computeSpatialKinematicsAndBias( spatialModel, @@ -73,23 +71,23 @@ namespace robots { ); // CRBA only for controller inertia scaling - LOG_INFO("Computing M_start via CRBA"); + //LOG_INFO("Computing M_start via CRBA"); mathlib::MatX_T M_start = SpatialDynamics::CRBA( spatialModel, dynScratch.spatial.Xup, dynScratch ); - LOG_INFO("M_start dims = %d x %d", (int)M_start.rows(), (int)M_start.cols()); + //LOG_INFO("M_start dims = %d x %d", (int)M_start.rows(), (int)M_start.cols()); // Cache frozen joint gains for this step mathlib::VecX_T kp_frozen(n), kd_frozen(n); for (size_t i = 0; i < n; ++i) { const auto& joint = snap.model->joints[i]; - LOG_INFO("Snap model joint name = %s", joint.name.c_str()); + //LOG_INFO("Snap model joint name = %s", joint.name.c_str()); if (joint.type == eJointType::FIXED) { continue; } - LOG_INFO("Max of M_start and 1e-6: %.3f", mathlib::max(M_start(i, i), Scalar(1e-6))); - LOG_INFO("I_eff_controller size = %d", (int)dynScratch.dense.I_eff_controller.size()); + //LOG_INFO("Max of M_start and 1e-6: %.3f", mathlib::max(M_start(i, i), Scalar(1e-6))); + //LOG_INFO("I_eff_controller size = %d", (int)dynScratch.dense.I_eff_controller.size()); dynScratch.dense.I_eff_controller[i] = mathlib::max(M_start(i, i), Scalar(1e-6)); const Scalar I_eff = dynScratch.dense.I_eff_controller[i]; @@ -104,7 +102,7 @@ namespace robots { dynScratch ); // [Nm] result.tau_rnea = tau_rnea; - LOG_INFO_ONCE("tau_rnea size = %d", (double)tau_rnea.size()); + //LOG_INFO_ONCE("tau_rnea size = %d", (double)tau_rnea.size()); // Define the derivative function for integration, capturing necessary variables by reference auto f_deriv = [&, kp_frozen, kd_frozen](auto t, const auto& xIn) { diff --git a/DSFE_App/DSFE_Core/src/Robots/RobotSystem.cpp b/DSFE_App/DSFE_Core/src/Robots/RobotSystem.cpp index 4c6424c7..576ba867 100644 --- a/DSFE_App/DSFE_Core/src/Robots/RobotSystem.cpp +++ b/DSFE_App/DSFE_Core/src/Robots/RobotSystem.cpp @@ -249,19 +249,15 @@ namespace robots { // Method to advance the robot state by dt using the selected integrator void RobotSystem::step(double dt, double simTime) { if (!_hasRobot) { return; } - LOG_INFO("Entered step()"); _simTime = simTime; const size_t n = _robot.joints.size(); - LOG_INFO("Packing state"); mathlib::VecX x = packState(); auto result = step_impl(x, dt, simTime, *_integrator); - LOG_INFO("Unpacking state"); unpackState(result.stepOut.x_next); _dynamics->setDt(result.stepOut.dt_taken); - LOG_INFO("PostStepUpadates state"); postStepUpdate(result, result.stepOut.x_next); // Update base pose if free-floating From 3d24d636cdde967f36f8fd738650a0a12bf07dff Mon Sep 17 00:00:00 2001 From: saltyjoss Date: Mon, 25 May 2026 11:47:19 +0100 Subject: [PATCH 079/210] feat: Successful integration with AD integration methosd * Leaving this here because I am going bowling TODO * Reimplement friction model * Improve friction model using LPV?? --- DSFE_App/DSFE_Core/include/Robots/RobotSystem.h | 9 ++++++++- .../DSFE_Core/include/Robots/RobotSystemStep.inl | 14 ++++++++------ DSFE_App/DSFE_Core/src/Robots/RobotSystem.cpp | 6 +++++- 3 files changed, 21 insertions(+), 8 deletions(-) diff --git a/DSFE_App/DSFE_Core/include/Robots/RobotSystem.h b/DSFE_App/DSFE_Core/include/Robots/RobotSystem.h index 692bcf4f..0b6ff29e 100644 --- a/DSFE_App/DSFE_Core/include/Robots/RobotSystem.h +++ b/DSFE_App/DSFE_Core/include/Robots/RobotSystem.h @@ -198,7 +198,11 @@ namespace robots { void buildSpatialModel(); template - RobotStepResult_T step_impl(const mathlib::VecX_T& x, Scalar dt, Scalar t, IntegratorT& integrator); + RobotStepResult_T step_impl( + const mathlib::VecX_T& x, + Scalar dt, Scalar t, IntegratorT& integrator, + DynamicsScratch& dynamicScratch, DynamicsResult& dynamicResult + ); template void postStepUpdate(const RobotStepResult_T& result, const mathlib::VecX& x); @@ -248,6 +252,9 @@ namespace robots { DynamicsScratch _dynScratch; DynamicsResult _dynResult; + DynamicsScratch> _dynScratch_AD; + DynamicsResult> _dynResult_AD; + // World to robot base transform (meters) std::vector _worldTransforms; diff --git a/DSFE_App/DSFE_Core/include/Robots/RobotSystemStep.inl b/DSFE_App/DSFE_Core/include/Robots/RobotSystemStep.inl index 140444c8..c761ffcb 100644 --- a/DSFE_App/DSFE_Core/include/Robots/RobotSystemStep.inl +++ b/DSFE_App/DSFE_Core/include/Robots/RobotSystemStep.inl @@ -41,7 +41,11 @@ namespace robots { } template - RobotStepResult_T RobotSystem::step_impl(const mathlib::VecX_T& x, Scalar dt, Scalar t, IntegratorT& integrator) { + RobotStepResult_T RobotSystem::step_impl( + const mathlib::VecX_T& x, + Scalar dt, Scalar t, IntegratorT& integrator, + DynamicsScratch& dynamicScratch, DynamicsResult& dynamicResult + ) { RobotStepResult_T result; result.snap = takeSnapshot(t); auto& snap = result.snap; @@ -58,10 +62,8 @@ namespace robots { std::vector> jointWorldPoses_start = _kinematics->calcJointWorldPoses(T_start, *snap.model); SpatialModel spatialModel = _spatialModel.template cast(); - DynamicsScratch dynScratch; - DynamicsResult dynResult; - dynScratch.resize(n, snap.model->links.size()); - dynResult.resize(n); + DynamicsScratch dynScratch = dynamicScratch; + DynamicsResult dynResult = dynamicResult; SpatialDynamics::computeSpatialKinematicsAndBias( spatialModel, @@ -237,7 +239,7 @@ namespace robots { assert((size_t)x.size() <= NVar && "State size exceeds the number of dual variables."); // Checks state vector size is within the dual variable limit for (size_t i = 0; i < (size_t)x.size(); ++i) { x[i].dual[i] = 1.0; } - auto result = step_impl(x, Dual(dt), Dual(simTime), *_AD_integrator); + auto result = step_impl(x, Dual(dt), Dual(simTime), *_AD_integrator, _dynScratch_AD, _dynResult_AD); mathlib::VecX x_real = result.stepOut.x_next.unaryExpr([](const auto& v) { return mathlib::real(v); }); unpackState(x_real); _dynamics->setDt(result.stepOut.dt_taken); diff --git a/DSFE_App/DSFE_Core/src/Robots/RobotSystem.cpp b/DSFE_App/DSFE_Core/src/Robots/RobotSystem.cpp index 576ba867..2fcd29a3 100644 --- a/DSFE_App/DSFE_Core/src/Robots/RobotSystem.cpp +++ b/DSFE_App/DSFE_Core/src/Robots/RobotSystem.cpp @@ -253,7 +253,7 @@ namespace robots { _simTime = simTime; const size_t n = _robot.joints.size(); mathlib::VecX x = packState(); - auto result = step_impl(x, dt, simTime, *_integrator); + auto result = step_impl(x, dt, simTime, *_integrator, _dynScratch, _dynResult); unpackState(result.stepOut.x_next); _dynamics->setDt(result.stepOut.dt_taken); @@ -376,7 +376,9 @@ namespace robots { _hasRobot = true; _dynScratch.resize(n, m); + _dynScratch_AD.resize(n, m); _dynResult.resize(n); + _dynResult_AD.resize(n); resetRobot(); @@ -408,7 +410,9 @@ namespace robots { _baseYawAcc = 0.0; _dynScratch.resize(_robot.joints.size(), _robot.links.size()); + _dynScratch_AD.resize(_robot.joints.size(), _robot.links.size()); _dynResult.resize(_robot.joints.size()); + _dynResult_AD.resize(_robot.joints.size()); // Reset adaptive integrator so it doesn't carry a stale step size _integrator->resetAdaptiveState(); From b39a376f0c0a7515fbfc1f681d1a7a66eda538d7 Mon Sep 17 00:00:00 2001 From: saltyjoss Date: Mon, 25 May 2026 20:10:06 +0100 Subject: [PATCH 080/210] feat: Added some interpretation of Karnopp Friction model. * Will actual refine later down the line, do not really understand some of the better models, so I need to research and make sure my understanding matches what I am implementing! * Also I am soon going to be working on getting BASIC particle dynamics (iteractions implemented) --> This is a big goal --- DSFE_App/DSFE_Core/include/Robots/RobotDynamics.h | 1 + .../DSFE_Core/include/Robots/RobotDynamics.inl | 8 ++++---- DSFE_App/DSFE_Core/src/Scene/SimulationCore.cpp | 8 +++++++- PxMLib/MathLib/include/dynamics/FrictionModels.h | 15 +++++++++++++++ 4 files changed, 27 insertions(+), 5 deletions(-) create mode 100644 PxMLib/MathLib/include/dynamics/FrictionModels.h diff --git a/DSFE_App/DSFE_Core/include/Robots/RobotDynamics.h b/DSFE_App/DSFE_Core/include/Robots/RobotDynamics.h index c9a850b3..ca7b0e92 100644 --- a/DSFE_App/DSFE_Core/include/Robots/RobotDynamics.h +++ b/DSFE_App/DSFE_Core/include/Robots/RobotDynamics.h @@ -3,6 +3,7 @@ #include "EngineCore.h" #include +#include #include "Robots/DynamicsTypes.h" #include "Robots/RobotMetrics.h" diff --git a/DSFE_App/DSFE_Core/include/Robots/RobotDynamics.inl b/DSFE_App/DSFE_Core/include/Robots/RobotDynamics.inl index 8e971286..0662b430 100644 --- a/DSFE_App/DSFE_Core/include/Robots/RobotDynamics.inl +++ b/DSFE_App/DSFE_Core/include/Robots/RobotDynamics.inl @@ -342,8 +342,8 @@ namespace robots { Scalar tau_i = k_p * err + k_d * err_d + I_eff * snap.qdd_ref[i]; // [Nm], control torque for joint i tau_i += scratch.g[i]; // Gravity compensation tau_i += scratch.dense.h[i]; // add Coriolis and centrifugal bias - tau_i -= b * qd[i]; // subtract viscous damping - tau_i -= c * mathlib::tanh(qd[i] / eps_f); // subtract Coulomb friction + Scalar tau_f = dynamics::computeKarnoppFriction(qd[i], tau_i, b, c); // add friction compensation + tau_i += tau_f; scratch.dense.tau[i] = tau_i; @@ -428,8 +428,8 @@ namespace robots { const Scalar eps_f = static_cast(1e-2); Scalar tau_i = k_p * err + k_d * err_d + I_eff * snap.qdd_ref[i]; - tau_i -= b * qd[i]; - //tau_i -= b * mathlib::tanh(qd[i] / eps_f); + tau_i -= b * qd[i]; // viscous damping + tau_i -= c * mathlib::tanh(qd[i] / eps_f); // friction scratch.dense.tau[i] = tau_i; diff --git a/DSFE_App/DSFE_Core/src/Scene/SimulationCore.cpp b/DSFE_App/DSFE_Core/src/Scene/SimulationCore.cpp index 6eb8d773..beb089b4 100644 --- a/DSFE_App/DSFE_Core/src/Scene/SimulationCore.cpp +++ b/DSFE_App/DSFE_Core/src/Scene/SimulationCore.cpp @@ -55,6 +55,9 @@ namespace core { // Get the name of the current integration method (returns "no_robot" if no robot is loaded) std::string SimulationCore::integrationMethodName() const { if (!_robot) { return "no_robot"; } + + + return _robot->getIntegratorName(); } // Get the current integration method @@ -169,11 +172,14 @@ namespace core { _robot->setRefBuffer(&_trajRefBuffer); } + const auto state = _robot->runtimeIntegratorState(); + std::string intName = (state && state->autoDiff) ? _robot->AD_integratorName() : _robot->getIntegratorName(); + _data.setParentFolder(paths::runs().string()); // Ensure reference sim system have their integrators configured for the new run setupSimulationIntegrator(); - _data.setIntegratorName(integrationMethodName()); + _data.setIntegratorName(intName); _data.setRunTag(_runTag); _simRunning.store(true); diff --git a/PxMLib/MathLib/include/dynamics/FrictionModels.h b/PxMLib/MathLib/include/dynamics/FrictionModels.h new file mode 100644 index 00000000..969ec102 --- /dev/null +++ b/PxMLib/MathLib/include/dynamics/FrictionModels.h @@ -0,0 +1,15 @@ +// PxM/MathLib FrictionModels.h + +#include + +namespace dynamics { + template + inline Scalar computeKarnoppFriction(const Scalar qd, const Scalar tau_active, Scalar c, Scalar b) { + Scalar Dv = Scalar(1e-4); // Small vel deadband + if (mathlib::abs(qd) > Dv) { return c * mathlib::sgn(qd) + b * qd; } // Viscous friction + else { + Scalar max_static_friction = c * Scalar(1.2); // Max static friction (20 percent higher than dynamic friction) + return mathlib::clamp(tau_active, -max_static_friction, max_static_friction); // Static friction with saturation + } + } +} // namespace dynamics \ No newline at end of file From 19caef1092c9d4245788ae84975824a33847de5b Mon Sep 17 00:00:00 2001 From: saltyjoss Date: Thu, 28 May 2026 18:23:00 +0100 Subject: [PATCH 081/210] fixes(wip): Decided to the developement on AutoDiff on the back burner until I have developed my understanding further, this is in addition to sorting some other aspects of my core implementation out. * I am so fed up of the current debugging (that has lasted for a week) and since this is a personal project I can sort of just let it be for now. * I am leaving the current code in, just removing any enablers (i.e. switches) to turn it to AD... I am going to correct and fix this once I am confident in my underwstanding. --- .../include/Interpreter/ProgramData.h | 6 +++- .../include/Numerics/AutoDiffStep.inl | 2 ++ .../include/Numerics/IntegrationMethods.h | 5 ++++ .../include/Numerics/IntegrationStep.inl | 2 ++ .../include/Platform/ISimulationCore.h | 1 + .../include/Robots/RobotDynamics.inl | 26 ++++++++++++++++ .../DSFE_Core/include/Robots/RobotSystem.h | 10 ++++++- .../include/Robots/RobotSystemStep.inl | 28 ++++++++--------- .../DSFE_Core/include/Scene/SimulationCore.h | 1 + .../src/Interpreter/StoredProgram.cpp | 7 ++++- DSFE_App/DSFE_Core/src/Robots/RobotSystem.cpp | 30 ++++++++++++------- .../DSFE_Core/src/Scene/SimulationCore.cpp | 27 +++++++++++++---- .../include/Scene/SimulationManager.h | 6 ++++ .../DSFE_GUI/src/Scene/SimulationManager.cpp | 14 +++++++++ DSFE_App/DSFE_GUI/src/ui/ControlPanel.cpp | 23 ++++++++------ 15 files changed, 146 insertions(+), 42 deletions(-) diff --git a/DSFE_App/DSFE_Core/include/Interpreter/ProgramData.h b/DSFE_App/DSFE_Core/include/Interpreter/ProgramData.h index 0740468b..ea77cfec 100644 --- a/DSFE_App/DSFE_Core/include/Interpreter/ProgramData.h +++ b/DSFE_App/DSFE_Core/include/Interpreter/ProgramData.h @@ -49,7 +49,11 @@ namespace program_data { ImplicitEuler, ImplicitMidpoint, GLRK2, - GLRK3 + GLRK3, + AD_ImplicitEuler, + AD_ImplicitMidpoint, + AD_GLRK2, + AD_GLRK3 }; // Enum for preset colours diff --git a/DSFE_App/DSFE_Core/include/Numerics/AutoDiffStep.inl b/DSFE_App/DSFE_Core/include/Numerics/AutoDiffStep.inl index 0d0d515a..5a8eeecb 100644 --- a/DSFE_App/DSFE_Core/include/Numerics/AutoDiffStep.inl +++ b/DSFE_App/DSFE_Core/include/Numerics/AutoDiffStep.inl @@ -12,6 +12,8 @@ namespace integration { } } + LOG_INFO_ONCE("Using AutoDiff integration method: %s", IntegratorName(m).c_str()); + Real dt_r = mathlib::real(dt); _state->backend = eIntegrationBackend::AutoDiff; diff --git a/DSFE_App/DSFE_Core/include/Numerics/IntegrationMethods.h b/DSFE_App/DSFE_Core/include/Numerics/IntegrationMethods.h index 90914fc5..40322408 100644 --- a/DSFE_App/DSFE_Core/include/Numerics/IntegrationMethods.h +++ b/DSFE_App/DSFE_Core/include/Numerics/IntegrationMethods.h @@ -20,4 +20,9 @@ namespace integration { AD_GLRK2 = 2, AD_GLRK3 = 3 }; + + inline bool isStandardMethod(eIntegrationMethod method) { + auto val = static_cast(method); + return val >= static_cast(eIntegrationMethod::Euler) && val <= static_cast(eIntegrationMethod::GLRK3); + } } \ No newline at end of file diff --git a/DSFE_App/DSFE_Core/include/Numerics/IntegrationStep.inl b/DSFE_App/DSFE_Core/include/Numerics/IntegrationStep.inl index 6e768b3b..7813691e 100644 --- a/DSFE_App/DSFE_Core/include/Numerics/IntegrationStep.inl +++ b/DSFE_App/DSFE_Core/include/Numerics/IntegrationStep.inl @@ -11,6 +11,8 @@ namespace integration { } } + LOG_INFO_ONCE("Using integration method: %s", IntegratorName(m).c_str()); + _state->backend = eIntegrationBackend::Standard; _state->last_dt_taken = (_state->last_dt_taken != dt && !_state->adaptive) ? dt : _state->last_dt_taken; _state->last_dt_sug = (_state->last_dt_sug != dt && !_state->adaptive) ? dt : _state->last_dt_sug; diff --git a/DSFE_App/DSFE_Core/include/Platform/ISimulationCore.h b/DSFE_App/DSFE_Core/include/Platform/ISimulationCore.h index 7135fc68..c400d821 100644 --- a/DSFE_App/DSFE_Core/include/Platform/ISimulationCore.h +++ b/DSFE_App/DSFE_Core/include/Platform/ISimulationCore.h @@ -42,6 +42,7 @@ namespace core { virtual std::string integrationMethodName() const = 0; virtual integration::eIntegrationMethod integrationMethod() const = 0; virtual integration::eAutoDiffIntegrationMethod autoDiffIntegrationMethod() const = 0; + virtual void enableAutoDiff(bool enable) = 0; // Setter for run tag name of current script virtual void setRunTag(const std::string& tag) = 0; // Subsystems diff --git a/DSFE_App/DSFE_Core/include/Robots/RobotDynamics.inl b/DSFE_App/DSFE_Core/include/Robots/RobotDynamics.inl index 0662b430..d2c759d9 100644 --- a/DSFE_App/DSFE_Core/include/Robots/RobotDynamics.inl +++ b/DSFE_App/DSFE_Core/include/Robots/RobotDynamics.inl @@ -390,6 +390,19 @@ namespace robots { Eigen::Map> q(x.data(), n); Eigen::Map> qd(x.data() + n, n); + // Quick guard: check for non-finite states and bail with zero derivative + for (size_t i = 0; i < n; ++i) { + double q_r = mathlib::real(q[i]); + double qd_r = mathlib::real(qd[i]); + if (!std::isfinite(q_r) || !std::isfinite(qd_r)) { + LOG_ERROR("Non-finite state detected in derivative_spatial: q[%zu]=%g qd[%zu]=%g", i, q_r, i, qd_r); + // Return zero derivative to avoid propagating NaNs + dx.setZero(); + out.qdd.setZero(); + return dx; + } + } + SpatialDynamics::computeSpatialKinematicsAndBias( model, q, qd, scratch.spatial.Xup, @@ -398,6 +411,19 @@ namespace robots { ); mathlib::MatX_T M = SpatialDynamics::CRBA(model, scratch.spatial.Xup, scratch); + + // Validate mass matrix diagonal entries to avoid singular/NaN matrices. + // Regularize any non-finite or tiny diagonal entries to stabilize the LDLT solve. + { + const double eps_reg = 1e-8; + for (size_t i = 0; i < n; ++i) { + double mii = mathlib::real(M(i, i)); + if (!std::isfinite(mii) || mii < eps_reg) { + LOG_WARN("Regularizing mass matrix diagonal M(%zu,%zu) = %g", i, i, mii); + M(i, i) = M(i, i) + static_cast(eps_reg); + } + } + } // RNEA to compute gravity compensation (q, 0, 0) for gravity, (q, qd, 0) for Coriolis mathlib::VecX_T qd_zero = mathlib::VecX_T::Zero(n); mathlib::VecX_T qdd_zero = mathlib::VecX_T::Zero(n); diff --git a/DSFE_App/DSFE_Core/include/Robots/RobotSystem.h b/DSFE_App/DSFE_Core/include/Robots/RobotSystem.h index 0b6ff29e..45ad8c8f 100644 --- a/DSFE_App/DSFE_Core/include/Robots/RobotSystem.h +++ b/DSFE_App/DSFE_Core/include/Robots/RobotSystem.h @@ -157,6 +157,7 @@ namespace robots { const auto state = _integrator->runtimeState(); state->backend = integration::eIntegrationBackend::Standard; state->autoDiff = false; + _useAutoDiff = false; } integration::eAutoDiffIntegrationMethod AD_IntegrationMethod() const { return _curIntMethod_AD; } @@ -166,6 +167,7 @@ namespace robots { const auto state = _AD_integrator->runtimeState(); state->backend = integration::eIntegrationBackend::AutoDiff; state->autoDiff = true; + _useAutoDiff = true; } integration::IntegrationService* getIntegrator(); @@ -174,6 +176,10 @@ namespace robots { integration::DifferentiableIntegrator* getADIntegrator(); const integration::DifferentiableIntegrator* getADIntegrator() const; + bool autoDiffEnabled() const { return _useAutoDiff; } + void enableAutoDiff(bool enable) { _useAutoDiff = enable; } + + std::shared_ptr runtimeIntegratorState(); std::shared_ptr runtimeIntegratorState() const; void setRefBuffer(robots::TrajRefBuffer* buf) { _refBuffer = buf; } @@ -205,7 +211,7 @@ namespace robots { ); template - void postStepUpdate(const RobotStepResult_T& result, const mathlib::VecX& x); + void postStepUpdate(const mathlib::VecX& x, const DynamicsScratch& scratch, const RobotStepResult_T& result); std::unique_ptr _kinematics; std::unique_ptr _dynamics; @@ -221,6 +227,8 @@ namespace robots { double _wn = 0.0; // configurable natural frequency for PD control (rad/s) double _zeta = 0.0; // configurable damping ratio for PD control (unitless) + bool _useAutoDiff = false; + // Compute the forward drive (velocity) of the robot's root link based on the current state and robot configuration double computeForwardDrive() const; // Integrate the floating base translation based on the current state and robot configuration diff --git a/DSFE_App/DSFE_Core/include/Robots/RobotSystemStep.inl b/DSFE_App/DSFE_Core/include/Robots/RobotSystemStep.inl index c761ffcb..81470800 100644 --- a/DSFE_App/DSFE_Core/include/Robots/RobotSystemStep.inl +++ b/DSFE_App/DSFE_Core/include/Robots/RobotSystemStep.inl @@ -62,8 +62,8 @@ namespace robots { std::vector> jointWorldPoses_start = _kinematics->calcJointWorldPoses(T_start, *snap.model); SpatialModel spatialModel = _spatialModel.template cast(); - DynamicsScratch dynScratch = dynamicScratch; - DynamicsResult dynResult = dynamicResult; + auto& dynScratch = dynamicScratch; + auto& dynResult = dynamicResult; SpatialDynamics::computeSpatialKinematicsAndBias( spatialModel, @@ -95,6 +95,10 @@ namespace robots { kp_frozen[i] = I_eff * joint.wn_target * joint.wn_target; kd_frozen[i] = Scalar(2) * joint.zeta_target * I_eff * joint.wn_target; + + //LOG_INFO("I_eff[%d] = %.12f", (int)i, (double)mathlib::real(I_eff)); + //LOG_INFO("kp[%d] = %.12f", (int)i, (double)mathlib::real(kp_frozen[i])); + //LOG_INFO("kd[%d] = %.12f", (int)i, (double)mathlib::real(kd_frozen[i])); } // Compute RNEA torques for feedforward control @@ -145,7 +149,7 @@ namespace robots { } template - void RobotSystem::postStepUpdate(const RobotStepResult_T& result, const mathlib::VecX& x) { + void RobotSystem::postStepUpdate(const mathlib::VecX& x, const DynamicsScratch& dynScratch, const RobotStepResult_T& result) { const size_t n = result.snap.model->joints.size(); Eigen::Map q_next(x.data(), n); @@ -159,17 +163,8 @@ namespace robots { _kinematics->computeForwardKinematics_fromState(*result.snap.model, x, T_world); std::vector jointWorldPoses = _kinematics->calcJointWorldPoses(T_world, *result.snap.model); - DynamicsScratch dynScratch; - - // Compute spatial kinematics and bias terms for the new state - SpatialDynamics::computeSpatialKinematicsAndBias( - _spatialModel, - q_next, qd_next, - dynScratch.spatial.Xup, - dynScratch.spatial.v, dynScratch.spatial.c - ); // Compute mass matrix at the new state - mathlib::MatX M = SpatialDynamics::CRBA(_spatialModel, dynScratch.spatial.Xup, dynScratch); + mathlib::MatX M = dynScratch.dense.M.unaryExpr([](const auto& v) { return mathlib::real(v); }); mathlib::VecX tau_g = _dynamics->computeGravityTorque(*result.snap.model, T_world, jointWorldPoses); // Extract real parts of relevant variables for logging and control @@ -210,6 +205,11 @@ namespace robots { const double err = q_ref_real[i] - q_real[i]; const double err_d = qd_ref_real[i] - qd_real[i]; + //LOG_INFO("q norm: %.12f", q_real.norm()); + //LOG_INFO("qd norm: %.12f", qd_real.norm()); + //LOG_INFO("qdd norm: %.12f", dynResult.metrics.qdd.norm()); + //LOG_INFO("tau_rnea norm : % .12f", tau_rnea_real.norm()); + JointLogBuffer::JointLogEntry e{}; e.sim_time = _simTime; @@ -244,7 +244,7 @@ namespace robots { unpackState(x_real); _dynamics->setDt(result.stepOut.dt_taken); - postStepUpdate(result, x_real); + // postStepUpdate(x_real, _dynScratch_AD, result); // Update base pose if free-floating if (_baseIsFree) { diff --git a/DSFE_App/DSFE_Core/include/Scene/SimulationCore.h b/DSFE_App/DSFE_Core/include/Scene/SimulationCore.h index feea020e..d8c781e2 100644 --- a/DSFE_App/DSFE_Core/include/Scene/SimulationCore.h +++ b/DSFE_App/DSFE_Core/include/Scene/SimulationCore.h @@ -71,6 +71,7 @@ namespace core { std::string integrationMethodName() const override; integration::eIntegrationMethod integrationMethod() const override; integration::eAutoDiffIntegrationMethod autoDiffIntegrationMethod() const override; + void enableAutoDiff(bool enable) override; void setRunTag(const std::string& tag) override { _runTag = tag; } diff --git a/DSFE_App/DSFE_Core/src/Interpreter/StoredProgram.cpp b/DSFE_App/DSFE_Core/src/Interpreter/StoredProgram.cpp index 6e7c9ef8..5c3737b6 100644 --- a/DSFE_App/DSFE_Core/src/Interpreter/StoredProgram.cpp +++ b/DSFE_App/DSFE_Core/src/Interpreter/StoredProgram.cpp @@ -151,7 +151,12 @@ namespace interpreter { if (_core->hasRobot()) { robots::RobotSystem* robot = _core->robotSystem(); if (!robot) { LOG_ERROR("No robot system found in simulation manager."); return; } - robot->setIntegrationMethod(static_cast(method)); + if (method == IntegratorMethod::AD_ImplicitEuler || method == IntegratorMethod::AD_ImplicitMidpoint || method == IntegratorMethod::AD_GLRK2 || method == IntegratorMethod::AD_GLRK3) { + _core->setIntegrationMethod(static_cast(method)); + } + else { + _core->setIntegrationMethod(static_cast(method)); + } } } } diff --git a/DSFE_App/DSFE_Core/src/Robots/RobotSystem.cpp b/DSFE_App/DSFE_Core/src/Robots/RobotSystem.cpp index 2fcd29a3..f5f648ec 100644 --- a/DSFE_App/DSFE_Core/src/Robots/RobotSystem.cpp +++ b/DSFE_App/DSFE_Core/src/Robots/RobotSystem.cpp @@ -250,6 +250,16 @@ namespace robots { void RobotSystem::step(double dt, double simTime) { if (!_hasRobot) { return; } + // If AutoDiff is enabled, route the step through the AD path. This + // prevents the non-AD integrator (e.g., RK4) from being used when the + // user expects AutoDiff-enabled integration. The AD path uses a + // compile-time dual-width; match the width used for the AD scratch + // buffers (64) so the DynamicsScratch/_dynResult_AD types align. + if (_useAutoDiff) { + step_AD<64>(dt, simTime); + return; + } + _simTime = simTime; const size_t n = _robot.joints.size(); mathlib::VecX x = packState(); @@ -258,7 +268,7 @@ namespace robots { unpackState(result.stepOut.x_next); _dynamics->setDt(result.stepOut.dt_taken); - postStepUpdate(result, result.stepOut.x_next); + //postStepUpdate(result.stepOut.x_next, _dynScratch, result); // Update base pose if free-floating if (_baseIsFree) { @@ -436,8 +446,12 @@ namespace robots { integration::DifferentiableIntegrator* RobotSystem::getADIntegrator() { return _AD_integrator.get(); } const integration::DifferentiableIntegrator* RobotSystem::getADIntegrator() const { return _AD_integrator.get(); } + std::shared_ptr RobotSystem::runtimeIntegratorState() { + return _useAutoDiff ? _AD_integrator->runtimeState() : _integrator->runtimeState(); + } + std::shared_ptr RobotSystem::runtimeIntegratorState() const { - return (_integrator->getIntegrationMethod() == _curIntMethod) ? _integrator->runtimeState() : _AD_integrator->runtimeState(); + return _useAutoDiff ? _AD_integrator->runtimeState() : _integrator->runtimeState(); } // --- ROBOT KINEMATICS AND JOINT STATE METHODS --- @@ -810,9 +824,7 @@ namespace robots { double RobotSystem::computeForwardDrive() const { double drive = 0.0; for (const auto& j : _robot.joints) { - if (j.name.find("hip_pitch") != std::string::npos) { - drive += -j.qd; - } + if (j.name.find("hip_pitch") != std::string::npos) { drive += -j.qd; } } return drive; } @@ -841,12 +853,8 @@ namespace robots { _baseVel += _baseAcc * dt; _basePos += _baseVel * dt; - LOG_INFO_ONCE("hipL=%.3f hipR=%.3f gaitPhase=%.3f", - hipL, hipR, hipR - hipL - ); - LOG_INFO_ONCE("baseVel = (%.3f, %.3f, %.3f)", - _baseVel.x(), _baseVel.y(), _baseVel.z() - ); + LOG_INFO_ONCE("hipL=%.3f hipR=%.3f gaitPhase=%.3f", hipL, hipR, hipR - hipL); + LOG_INFO_ONCE("baseVel = (%.3f, %.3f, %.3f)", _baseVel.x(), _baseVel.y(), _baseVel.z()); } // Method to update the robot root pose based on the integrated base translation (for legged robots) diff --git a/DSFE_App/DSFE_Core/src/Scene/SimulationCore.cpp b/DSFE_App/DSFE_Core/src/Scene/SimulationCore.cpp index beb089b4..4e3fbb49 100644 --- a/DSFE_App/DSFE_Core/src/Scene/SimulationCore.cpp +++ b/DSFE_App/DSFE_Core/src/Scene/SimulationCore.cpp @@ -36,11 +36,14 @@ namespace core { // Simulation System void SimulationCore::setupSimulationIntegrator() { - if (!_robot) { return; } + if (!_robot) { return; } auto* intgr = _robot->getIntegrator(); + auto* adIntgr = _robot->getADIntegrator(); intgr->resetAdaptiveState(); intgr->setAdaptiveTolerances(1e-3, 1e-6); intgr->setMaxStep(_dt); + adIntgr->runtimeState()->last_dt_taken = _dt; + adIntgr->runtimeState()->last_dt_sug = _dt; } // Set the integration method for the simulation (also updates the robot's integrator if it exists) void SimulationCore::setIntegrationMethod(integration::eIntegrationMethod method) { @@ -55,10 +58,16 @@ namespace core { // Get the name of the current integration method (returns "no_robot" if no robot is loaded) std::string SimulationCore::integrationMethodName() const { if (!_robot) { return "no_robot"; } + std::string intName; + if (_robot->runtimeIntegratorState() && _robot->runtimeIntegratorState()->autoDiff) { + intName = _robot->getADIntegrator()->IntegratorName(_robot->getADIntegrator()->integrationMethod()); + } + else { + intName = _robot->getIntegrator()->IntegratorName(_robot->getIntegrator()->getIntegrationMethod()); + } + LOG_INFO("Integration Method: %s", intName.c_str()); - - - return _robot->getIntegratorName(); + return intName; } // Get the current integration method integration::eIntegrationMethod SimulationCore::integrationMethod() const { @@ -70,6 +79,10 @@ namespace core { if (!_robot) { return integration::eAutoDiffIntegrationMethod::AD_ImplicitEuler; } return _robot->getADIntegrator()->integrationMethod(); } + void SimulationCore::enableAutoDiff(bool enable) { + if (!_robot) { return; } + _robot->enableAutoDiff(enable); + } // Fixed timestep loop for physics and robot updates, called from the main render loop with the frame delta time void SimulationCore::stepFixed(double frame_dt) { @@ -174,6 +187,7 @@ namespace core { const auto state = _robot->runtimeIntegratorState(); std::string intName = (state && state->autoDiff) ? _robot->AD_integratorName() : _robot->getIntegratorName(); + LOG_INFO("Integrator for this run: %s", intName.c_str()); _data.setParentFolder(paths::runs().string()); @@ -209,7 +223,8 @@ namespace core { void SimulationCore::exportLogsToHDF5(const robots::JointLogBuffer& exportBuf) { auto t0 = std::chrono::steady_clock::now(); - const std::string intName = _robot->getIntegratorName(); + const auto state = _robot->runtimeIntegratorState(); + const std::string intName = (state && state->autoDiff) ? _robot->AD_integratorName() : _robot->getIntegratorName(); const std::string robotName = _robot->hasRobot() ? _robot->robotName() : "no_robot"; const std::string header = robotName + "_sim_" + intName; @@ -430,6 +445,8 @@ namespace core { void SimulationCore::setActiveProgram(interpreter::IStoredProgram* p) { _activeProgram = p; } interpreter::IStoredProgram* SimulationCore::activeProgram() const { return _activeProgram; } + // Setter for the + // Thread-based methods // Starts the export thread if it's not already running diff --git a/DSFE_App/DSFE_GUI/include/Scene/SimulationManager.h b/DSFE_App/DSFE_GUI/include/Scene/SimulationManager.h index 95e8e5c8..4318d1f4 100644 --- a/DSFE_App/DSFE_GUI/include/Scene/SimulationManager.h +++ b/DSFE_App/DSFE_GUI/include/Scene/SimulationManager.h @@ -243,6 +243,12 @@ namespace gui { core::SimulationCore* simCore(); const core::SimulationCore* simCore() const; + // Setters for Integration state / method + void setIntegrationMethod(integration::eIntegrationMethod method); + const integration::eIntegrationMethod integrationMethod() const; + void setIntegrationMethod(integration::eAutoDiffIntegrationMethod method); + const integration::eAutoDiffIntegrationMethod autoDiffIntegrationMethod() const; + // Access to the underlying StudyRunner for running batch studies from the GUI StudyRunner* studyRunner() { return _studyRunner.get(); } diff --git a/DSFE_App/DSFE_GUI/src/Scene/SimulationManager.cpp b/DSFE_App/DSFE_GUI/src/Scene/SimulationManager.cpp index e5e117af..14b96d15 100644 --- a/DSFE_App/DSFE_GUI/src/Scene/SimulationManager.cpp +++ b/DSFE_App/DSFE_GUI/src/Scene/SimulationManager.cpp @@ -1392,6 +1392,20 @@ namespace gui { core::SimulationCore* SimManager::simCore() { return _core.get(); } const core::SimulationCore* SimManager::simCore() const { return _core.get(); } + // Set the integrator method for the current simulation run + void SimManager::setIntegrationMethod(integration::eIntegrationMethod method) { + _core->setIntegrationMethod(method); + } + const integration::eIntegrationMethod SimManager::integrationMethod() const { + return _core->integrationMethod(); + } + void SimManager::setIntegrationMethod(integration::eAutoDiffIntegrationMethod method) { + _core->setIntegrationMethod(method); + } + const integration::eAutoDiffIntegrationMethod SimManager::autoDiffIntegrationMethod() const { + return _core->autoDiffIntegrationMethod(); + } + // Helper to replace the integrator method in the script text // A hacky approach my idea, but it works for me and honestly im starting to write up the dissertation so IT WILL DO :) // PS:If anyone has any better solution msg me diff --git a/DSFE_App/DSFE_GUI/src/ui/ControlPanel.cpp b/DSFE_App/DSFE_GUI/src/ui/ControlPanel.cpp index d4f78287..6af6f44f 100644 --- a/DSFE_App/DSFE_GUI/src/ui/ControlPanel.cpp +++ b/DSFE_App/DSFE_GUI/src/ui/ControlPanel.cpp @@ -210,8 +210,8 @@ namespace gui { _hdrLoad(ImGuiFileBrowserFlags_CloseOnEsc | ImGuiFileBrowserFlags_NoModal), _useAutoDiff(false) { - diagTime = 0.0f; // initialize diagnostic time - simTime = 0.0f; // initialize simulation time + diagTime = 0.0f; // initialise diagnostic time + simTime = 0.0f; // initialise simulation time diagRunning = false; simulationRunning = false; @@ -420,8 +420,8 @@ namespace gui { ImGui::SectionDivider(); robots::RobotSystem* robot = _sim->robotSystem(); - auto currentIntEnum = robot->getIntegrationMethod(); - auto currentIntEnum_AD = robot->AD_IntegrationMethod(); + auto currentIntEnum = _sim->integrationMethod(); + auto currentIntEnum_AD = _sim->autoDiffIntegrationMethod(); auto currentTauEnum = robot->getTorqueMode(); static const char* intMethodNames[] = { "Euler", "Midpoint", "Heun", "Ralston", "RK4", "RK45", "Implicit Euler", "Implicit Midpoint", "GLRK2", "GLRK3" }; @@ -440,8 +440,9 @@ namespace gui { // Integration method combo box ImGui::SectionHeader("Integration Methods"); - ImGui::SetNextItemWidth(150.0f); + /*ImGui::SetNextItemWidth(150.0f); ImGui::Checkbox("Enable Automatic Differentiable Integrators", &_useAutoDiff); + robot->enableAutoDiff(_useAutoDiff);*/ ImGui::SetNextItemWidth(150.0f); if (_useAutoDiff) { @@ -451,10 +452,12 @@ namespace gui { // When a new method is selected, update the robot's integration method and log the change if (ImGui::Selectable(intMethodNames_AD[n], isSelected)) { - auto updatedMethod = static_cast(n); - robot->setIntegrationMethod(updatedMethod); + auto updatedMethod_AD = static_cast(n); + auto state = robot->runtimeIntegratorState(); + state->autoDiff = true; // ensure autodiff flag is set in state + robot->getADIntegrator()->setIntegrationMethod(updatedMethod_AD); - switch (updatedMethod) { + switch (updatedMethod_AD) { case integration::eAutoDiffIntegrationMethod::AD_ImplicitEuler: D_INFO("Integrator set to Implicit Euler (AutoDiff)"); break; case integration::eAutoDiffIntegrationMethod::AD_ImplicitMidpoint: @@ -480,7 +483,9 @@ namespace gui { // When a new method is selected, update the robot's integration method and log the change if (ImGui::Selectable(intMethodNames[n], isSelected)) { auto updatedMethod = static_cast(n); - robot->setIntegrationMethod(updatedMethod); + const auto state = robot->runtimeIntegratorState(); + state->autoDiff = false; // ensure autodiff flag is set in state + _sim->setIntegrationMethod(updatedMethod); switch (updatedMethod) { case integration::eIntegrationMethod::Euler: From a30e27d72087f724753c684eaf22a96356cc7b77 Mon Sep 17 00:00:00 2001 From: saltyjoss Date: Sat, 30 May 2026 13:47:22 +0100 Subject: [PATCH 082/210] fixes(WIP): Improved and correct AutoDiff implicit integration stability implementation and execution * There are way too many changes to log (bad VC I know) * I decided to go abck through and see if any of the problems were missing params, or bad logic rather than purely mathematical * I am no renabling commented out coriolis and friction in dynamics --- .../include/Numerics/AutoDiffStep.inl | 8 +- .../include/Numerics/IntegrationService.h | 2 +- .../include/Robots/RobotDynamics.inl | 72 ++++++++++------- .../include/Robots/RobotKinematics.inl | 6 +- .../DSFE_Core/include/Robots/RobotSystem.h | 8 +- .../include/Robots/RobotSystemStep.inl | 11 +-- .../DSFE_Core/include/Scene/SimulationCore.h | 1 - .../src/Numerics/IntegrationService.cpp | 1 + DSFE_App/DSFE_Core/src/Robots/RobotSystem.cpp | 31 ++++--- .../DSFE_Core/src/Scene/SimulationCore.cpp | 11 +-- DSFE_App/DSFE_GUI/src/ui/ControlPanel.cpp | 4 +- .../integrators/auto_diff_integrators.inl | 81 +++++++++++++++++-- .../integrators/numerical_integrators.h | 5 ++ 13 files changed, 165 insertions(+), 76 deletions(-) diff --git a/DSFE_App/DSFE_Core/include/Numerics/AutoDiffStep.inl b/DSFE_App/DSFE_Core/include/Numerics/AutoDiffStep.inl index 5a8eeecb..63a722e4 100644 --- a/DSFE_App/DSFE_Core/include/Numerics/AutoDiffStep.inl +++ b/DSFE_App/DSFE_Core/include/Numerics/AutoDiffStep.inl @@ -8,7 +8,7 @@ namespace integration { if constexpr (std::is_pointer_v> || requires { f == nullptr; }) { if (f == nullptr) { D_WARN_ONCE("No derivative function provided for integration - Assuming constant derivative."); - return { _integrator->implicitEuler_AD(x, t, dt, std::forward(f), 8, 1e-6), dt, dt }; + return { _integrator->implicitEuler_AD(x, t, dt, std::forward(f), 30, 1e-6), dt, dt }; } } @@ -21,12 +21,14 @@ namespace integration { _state->last_dt_taken = (_state->last_dt_taken != dt && !_state->adaptive) ? dt_r : _state->last_dt_taken; // Unless RK45-or other adaptive methods-it is static. _state->last_dt_sug = (_state->last_dt_sug != dt && !_state->adaptive) ? dt_r : _state->last_dt_sug; // Unless RK45-or other adaptive methods-it is static. + auto* state = runtimeState().get(); + // Though right now all methods hold the same fundamental state in AD, I am employing the same structure for consistency switch (m) { case eAutoDiffIntegrationMethod::AD_ImplicitEuler: _state->name = IntegratorName(m); _state->adaptive = false; _state->implicit = true; - return { _integrator->implicitEuler_AD(x, t, dt, std::forward(f), 8, 1e-6), dt_r, dt_r }; + return { _integrator->implicitEuler_AD(x, t, dt, std::forward(f), 30, 1e-6), dt_r, dt_r }; case eAutoDiffIntegrationMethod::AD_ImplicitMidpoint: _state->name = IntegratorName(m); _state->adaptive = false; _state->implicit = true; @@ -43,7 +45,7 @@ namespace integration { LOG_WARN("Unknown Integrator, defaulting to ImplicitEuler (AutoDiff)."); _state->name = IntegratorName(eAutoDiffIntegrationMethod::AD_ImplicitEuler); _state->adaptive = false; _state->implicit = true; - return { _integrator->implicitEuler_AD(x, t, dt, std::forward(f), 8, 1e-6), dt_r, dt_r }; + return { _integrator->implicitEuler_AD(x, t, dt, std::forward(f), 30, 1e-6), dt_r, dt_r }; } } } // namespace integration \ No newline at end of file diff --git a/DSFE_App/DSFE_Core/include/Numerics/IntegrationService.h b/DSFE_App/DSFE_Core/include/Numerics/IntegrationService.h index 1c895c31..3fb8051a 100644 --- a/DSFE_App/DSFE_Core/include/Numerics/IntegrationService.h +++ b/DSFE_App/DSFE_Core/include/Numerics/IntegrationService.h @@ -63,7 +63,7 @@ namespace integration { template StepOut_T step(eAutoDiffIntegrationMethod m, const mathlib::VecX_T& x, Scalar t, Scalar dt, Func&& f); const std::string IntegratorName(eAutoDiffIntegrationMethod m); - void setIntegrationMethod(eAutoDiffIntegrationMethod m) { _m = m; } + void setIntegrationMethod(eAutoDiffIntegrationMethod m) { LOG_INFO("AD integrator changed to enum %d", (int)_m); _m = m; } eAutoDiffIntegrationMethod integrationMethod() const { return _m; } std::shared_ptr runtimeState() { return _state; } std::shared_ptr runtimeState() const { return _state; } diff --git a/DSFE_App/DSFE_Core/include/Robots/RobotDynamics.inl b/DSFE_App/DSFE_Core/include/Robots/RobotDynamics.inl index d2c759d9..557b1769 100644 --- a/DSFE_App/DSFE_Core/include/Robots/RobotDynamics.inl +++ b/DSFE_App/DSFE_Core/include/Robots/RobotDynamics.inl @@ -30,7 +30,7 @@ namespace robots { // Rotation from link frame to world frame mathlib::Mat3_T R_joint = jointWorldPose.template block<3, 3>(0, 0); // Joint axis in world frame - mathlib::Vec3_T axis_world = (R_joint * joint.axis).normalized(); + mathlib::Vec3_T axis_world = mathlib::safeNormalised(R_joint * joint.axis); // Position of joint in world frame mathlib::Vec3_T joint_pos_world = jointWorldPose.template block<3, 1>(0, 3); @@ -94,7 +94,7 @@ namespace robots { // Rotation from joint i frame to world frame const mathlib::Mat3_T R_i = T_joint_i.template block<3, 3>(0, 0); // rotation from joint i frame to world frame const mathlib::Vec3_T p_i = T_joint_i.template block<3, 1>(0, 3); // joint position in world frame - const mathlib::Vec3_T z_i = (R_i * j_i.axis).normalized(); // joint axis in world frame + const mathlib::Vec3_T z_i = mathlib::safeNormalised(R_i * j_i.axis); // joint axis in world frame mathlib::Vec3_T J_vi = z_i.cross(com - p_i); // linear velocity Jacobian column for joint i mathlib::Vec3_T J_wi = z_i; // angular velocity Jacobian column for joint i @@ -161,6 +161,9 @@ namespace robots { jointWorldPoses_eps = _kinematics->calcJointWorldPoses(T_world_eps, robot); computeMassMatrix(robot, T_world_eps, jointWorldPoses_eps, M_plus); // mass matrix for the perturbed configuration + if (!M_plus.allFinite()) { throw std::runtime_error("Mass matrix contains non-finite values"); } + if (!M_plus.isApprox(M_plus, Scalar(1e-8))) { throw std::runtime_error("Mass matrix lost symmetry"); } + dM_dq[k] = (M_plus - M) / eps; // [kg*m^2/rad], partial derivative of mass matrix with } @@ -199,7 +202,7 @@ namespace robots { const mathlib::Mat3_T R_i = T_joint.block<3, 3>(0, 0); const mathlib::Vec3_T p_i = T_joint.block<3, 1>(0, 3); - const mathlib::Vec3_T axis_world = (R_i * robot.joints[i].axis).normalized(); + const mathlib::Vec3_T axis_world = mathlib::safeNormalised(R_i * robot.joints[i].axis); // For each link, compute the gravitational force and its torque contribution about joint i for (size_t k = 0; k < robot.links.size(); ++k) { @@ -304,12 +307,13 @@ namespace robots { scratch.dense.jointWorldPoses = _kinematics->calcJointWorldPoses(scratch.dense.T_world, *snap.model); computeMassMatrix(*snap.model, scratch.dense.T_world, scratch.dense.jointWorldPoses, scratch.dense.M); // [kg*m^2], full mass matrix for the robot at configuration q - scratch.dense.h = computeCoriolisVector(*snap.model, q, qd, scratch.dense.T_world, scratch.dense.M); // [Nm], full Coriolis and centrifugal torque vector + scratch.dense.h = mathlib::VecX_T::Zero(n); // Temp test to isolate potential issues + //scratch.dense.h = computeCoriolisVector(*snap.model, q, qd, scratch.dense.T_world, scratch.dense.M); // [Nm], full Coriolis and centrifugal torque vector + + if (!scratch.dense.M.allFinite()) { throw std::runtime_error("Mass matrix contains non-finite values"); } scratch.g.setZero(); - if (snap.torqueMode != eTorqueMode::NONE) { - scratch.g = computeGravityTorque(*snap.model, scratch.dense.T_world, scratch.dense.jointWorldPoses); - } + if (snap.torqueMode != eTorqueMode::NONE) { scratch.g = computeGravityTorque(*snap.model, scratch.dense.T_world, scratch.dense.jointWorldPoses); } scratch.dense.tau.setZero(); for (size_t i = 0; i < n; ++i) { @@ -342,8 +346,8 @@ namespace robots { Scalar tau_i = k_p * err + k_d * err_d + I_eff * snap.qdd_ref[i]; // [Nm], control torque for joint i tau_i += scratch.g[i]; // Gravity compensation tau_i += scratch.dense.h[i]; // add Coriolis and centrifugal bias - Scalar tau_f = dynamics::computeKarnoppFriction(qd[i], tau_i, b, c); // add friction compensation - tau_i += tau_f; + // Scalar tau_f = dynamics::computeKarnoppFriction(qd[i], tau_i, b, c); // add friction compensation + // tau_i += tau_f; scratch.dense.tau[i] = tau_i; @@ -356,16 +360,26 @@ namespace robots { out.metrics.I_eff[i] = mathlib::real(I_eff); out.metrics.tau[i] = mathlib::real(tau_i); - - //_metrics.tau_sat[i] = tau_sat; - //_metrics.sat_flag[i] = saturated; } // Solve Forward Dynamics: M(q) qdd = tau - h(q, qd) - g(q) scratch.dense.rhs.noalias() = scratch.dense.tau - scratch.dense.h - scratch.g; // [Nm], right-hand side of the dynamics equation M*qdd = tau - h - g // Solve for Accelerations - out.qdd = scratch.dense.M.ldlt().solve(scratch.dense.rhs); // [rad/s^2], joint accelerations computed from dynamics + Eigen::LDLT> solver(scratch.dense.M); + + if (solver.info() != Eigen::Success) { + LOG_ERROR("LDLT decomposition failed for mass matrix M. Matrix may be singular or ill-conditioned."); + throw std::runtime_error("LDLT decomposition failed for mass matrix M"); + } + + out.qdd = solver.solve(scratch.dense.rhs); // [rad/s^2], joint accelerations computed from dynamics + + if (!out.qdd.allFinite()) { + LOG_ERROR("Non-finite joint accelerations computed. Check for singularities or numerical issues in the mass matrix."); + throw std::runtime_error("Non-finite joint accelerations computed from dynamics"); + } + out.metrics.qdd = out.qdd; // Fill derivatives @@ -411,20 +425,6 @@ namespace robots { ); mathlib::MatX_T M = SpatialDynamics::CRBA(model, scratch.spatial.Xup, scratch); - - // Validate mass matrix diagonal entries to avoid singular/NaN matrices. - // Regularize any non-finite or tiny diagonal entries to stabilize the LDLT solve. - { - const double eps_reg = 1e-8; - for (size_t i = 0; i < n; ++i) { - double mii = mathlib::real(M(i, i)); - if (!std::isfinite(mii) || mii < eps_reg) { - LOG_WARN("Regularizing mass matrix diagonal M(%zu,%zu) = %g", i, i, mii); - M(i, i) = M(i, i) + static_cast(eps_reg); - } - } - } - // RNEA to compute gravity compensation (q, 0, 0) for gravity, (q, qd, 0) for Coriolis mathlib::VecX_T qd_zero = mathlib::VecX_T::Zero(n); mathlib::VecX_T qdd_zero = mathlib::VecX_T::Zero(n); mathlib::VecX_T tau_g = SpatialDynamics::RNEA(model, q, qd_zero, qdd_zero, scratch); @@ -454,8 +454,10 @@ namespace robots { const Scalar eps_f = static_cast(1e-2); Scalar tau_i = k_p * err + k_d * err_d + I_eff * snap.qdd_ref[i]; - tau_i -= b * qd[i]; // viscous damping - tau_i -= c * mathlib::tanh(qd[i] / eps_f); // friction + tau_i += scratch.g[i]; // Gravity compensation + tau_i += scratch.dense.h[i]; // add Coriolis and centrifugal bias + // Scalar tau_f = dynamics::computeKarnoppFriction(qd[i], tau_i, b, c); // add friction compensation + // tau_i += tau_f; scratch.dense.tau[i] = tau_i; @@ -520,9 +522,21 @@ namespace robots { } Eigen::LDLT> solver(scratch.dense.M); // compute the Cholesky decomposition of the mass matrix for efficient solving + mathlib::MatX_T dqdd_dtau_q = solver.solve(dTau_dq); // compute the partial derivative of qdd with respect to q + + if (solver.info() != Eigen::Success) { + LOG_ERROR("LDLT decomposition failed for mass matrix M in jacobian_spatial. Matrix may be singular or ill-conditioned."); + throw std::runtime_error("LDLT decomposition failed for mass matrix M in jacobian_spatial"); + } + mathlib::MatX_T dqdd_dtau_v = solver.solve(dTau_dv); // compute the partial derivative of qdd with respect to qd + if (solver.info() != Eigen::Success) { + LOG_ERROR("LDLT decomposition failed for mass matrix M in jacobian_spatial. Matrix may be singular or ill-conditioned."); + throw std::runtime_error("LDLT decomposition failed for mass matrix M in jacobian_spatial"); + } + F_out.block(n, 0, n, n) = dqdd_dtau_q; // fill the Jacobian block for qdd with respect to q F_out.block(n, n, n, n) = dqdd_dtau_v; // fill the Jacobian block for qdd with respect to qd) } diff --git a/DSFE_App/DSFE_Core/include/Robots/RobotKinematics.inl b/DSFE_App/DSFE_Core/include/Robots/RobotKinematics.inl index e44c60b8..84ed0d79 100644 --- a/DSFE_App/DSFE_Core/include/Robots/RobotKinematics.inl +++ b/DSFE_App/DSFE_Core/include/Robots/RobotKinematics.inl @@ -15,14 +15,14 @@ namespace robots { T_world_out.resize(robot.links.size()); - mathlib::Pose_T T = mathlib::Pose_T::Identity(); // world -> base - T_world_out[0] = T; // base link - if (T_world_out.empty()) { LOG_ERROR("T_world_out is empty"); return; } + mathlib::Pose_T T = mathlib::Pose_T::Identity(); // world -> base + T_world_out[0] = T; // base link + // Compute the transform to the next link using each joint for (size_t i = 0; i < n; ++i) { const auto& joint = joints[i]; diff --git a/DSFE_App/DSFE_Core/include/Robots/RobotSystem.h b/DSFE_App/DSFE_Core/include/Robots/RobotSystem.h index 45ad8c8f..9670c894 100644 --- a/DSFE_App/DSFE_Core/include/Robots/RobotSystem.h +++ b/DSFE_App/DSFE_Core/include/Robots/RobotSystem.h @@ -44,6 +44,8 @@ namespace robots { mathlib::VecX_T tau_rnea; }; + inline constexpr size_t AD_VARS = 14; // number of independent variables for autodiff (used for pre-allocating AD integrator buffers) + class DSFE_API RobotSystem { public: RobotSystem(); @@ -153,6 +155,7 @@ namespace robots { integration::eIntegrationMethod getIntegrationMethod() const { return _curIntMethod; } std::string getIntegratorName() const { return _integrator->IntegratorName(_curIntMethod); } void setIntegrationMethod(integration::eIntegrationMethod method) { + LOG_INFO("Integrator changed to enum %d", (int)method); _curIntMethod = method; const auto state = _integrator->runtimeState(); state->backend = integration::eIntegrationBackend::Standard; @@ -163,6 +166,7 @@ namespace robots { integration::eAutoDiffIntegrationMethod AD_IntegrationMethod() const { return _curIntMethod_AD; } std::string AD_integratorName() const { return _AD_integrator->IntegratorName(_curIntMethod_AD); } void setIntegrationMethod(integration::eAutoDiffIntegrationMethod method) { + LOG_INFO("AD integrator changed to enum %d", (int)method); _curIntMethod_AD = method; const auto state = _AD_integrator->runtimeState(); state->backend = integration::eIntegrationBackend::AutoDiff; @@ -260,8 +264,8 @@ namespace robots { DynamicsScratch _dynScratch; DynamicsResult _dynResult; - DynamicsScratch> _dynScratch_AD; - DynamicsResult> _dynResult_AD; + DynamicsScratch> _dynScratch_AD; + DynamicsResult> _dynResult_AD; // World to robot base transform (meters) std::vector _worldTransforms; diff --git a/DSFE_App/DSFE_Core/include/Robots/RobotSystemStep.inl b/DSFE_App/DSFE_Core/include/Robots/RobotSystemStep.inl index 81470800..2e6b4a98 100644 --- a/DSFE_App/DSFE_Core/include/Robots/RobotSystemStep.inl +++ b/DSFE_App/DSFE_Core/include/Robots/RobotSystemStep.inl @@ -73,13 +73,11 @@ namespace robots { ); // CRBA only for controller inertia scaling - //LOG_INFO("Computing M_start via CRBA"); mathlib::MatX_T M_start = SpatialDynamics::CRBA( spatialModel, dynScratch.spatial.Xup, dynScratch ); - //LOG_INFO("M_start dims = %d x %d", (int)M_start.rows(), (int)M_start.cols()); // Cache frozen joint gains for this step mathlib::VecX_T kp_frozen(n), kd_frozen(n); @@ -88,17 +86,11 @@ namespace robots { //LOG_INFO("Snap model joint name = %s", joint.name.c_str()); if (joint.type == eJointType::FIXED) { continue; } - //LOG_INFO("Max of M_start and 1e-6: %.3f", mathlib::max(M_start(i, i), Scalar(1e-6))); - //LOG_INFO("I_eff_controller size = %d", (int)dynScratch.dense.I_eff_controller.size()); dynScratch.dense.I_eff_controller[i] = mathlib::max(M_start(i, i), Scalar(1e-6)); const Scalar I_eff = dynScratch.dense.I_eff_controller[i]; kp_frozen[i] = I_eff * joint.wn_target * joint.wn_target; kd_frozen[i] = Scalar(2) * joint.zeta_target * I_eff * joint.wn_target; - - //LOG_INFO("I_eff[%d] = %.12f", (int)i, (double)mathlib::real(I_eff)); - //LOG_INFO("kp[%d] = %.12f", (int)i, (double)mathlib::real(kp_frozen[i])); - //LOG_INFO("kd[%d] = %.12f", (int)i, (double)mathlib::real(kd_frozen[i])); } // Compute RNEA torques for feedforward control @@ -144,6 +136,7 @@ namespace robots { const auto v = mathlib::real(result.stepOut.x_next[i]); if (std::isnan(v) || std::isinf(v)) { LOG_ERROR("Non-finite x_next[%d] = %f", i, (double)v); } // TODO add Scalar isnan and isinf checks to mathlib and use those instead (need to handle both float and double cases) } + result.dynamics = dynResult; return result; } @@ -238,7 +231,7 @@ namespace robots { assert((size_t)x.size() <= NVar && "State size exceeds the number of dual variables."); // Checks state vector size is within the dual variable limit for (size_t i = 0; i < (size_t)x.size(); ++i) { x[i].dual[i] = 1.0; } - + auto result = step_impl(x, Dual(dt), Dual(simTime), *_AD_integrator, _dynScratch_AD, _dynResult_AD); mathlib::VecX x_real = result.stepOut.x_next.unaryExpr([](const auto& v) { return mathlib::real(v); }); unpackState(x_real); diff --git a/DSFE_App/DSFE_Core/include/Scene/SimulationCore.h b/DSFE_App/DSFE_Core/include/Scene/SimulationCore.h index d8c781e2..e48fc5bd 100644 --- a/DSFE_App/DSFE_Core/include/Scene/SimulationCore.h +++ b/DSFE_App/DSFE_Core/include/Scene/SimulationCore.h @@ -29,7 +29,6 @@ namespace core { inline constexpr double DEFAULT_INTERACTIVE_MINUTES = 60.0; // long runs for interactive mode inline constexpr double DEFAULT_SYNC_MINUTES = 10.0; // short runs for synchronous mode inline constexpr size_t MAX_LOG_ENTRIES = 50'000'000; // hard cap to avoid OutOfMemory crashes - inline constexpr size_t DSFE_AD_VARS = 64; // number of independent variables for autodiff (used for pre-allocating AD integrator buffers) class DSFE_API SimulationCore : public ISimulationCore { public: diff --git a/DSFE_App/DSFE_Core/src/Numerics/IntegrationService.cpp b/DSFE_App/DSFE_Core/src/Numerics/IntegrationService.cpp index a0273000..b03433a8 100644 --- a/DSFE_App/DSFE_Core/src/Numerics/IntegrationService.cpp +++ b/DSFE_App/DSFE_Core/src/Numerics/IntegrationService.cpp @@ -69,5 +69,6 @@ namespace integration { DifferentiableIntegrator::DifferentiableIntegrator() : _integrator(std::make_unique()), _m(eAutoDiffIntegrationMethod::AD_ImplicitEuler), _state(std::make_shared()) { + LOG_INFO("DifferentiableIntegrator constructed. Default=%d", (int)_m); } } // namespace integration \ No newline at end of file diff --git a/DSFE_App/DSFE_Core/src/Robots/RobotSystem.cpp b/DSFE_App/DSFE_Core/src/Robots/RobotSystem.cpp index f5f648ec..31b912ef 100644 --- a/DSFE_App/DSFE_Core/src/Robots/RobotSystem.cpp +++ b/DSFE_App/DSFE_Core/src/Robots/RobotSystem.cpp @@ -250,21 +250,24 @@ namespace robots { void RobotSystem::step(double dt, double simTime) { if (!_hasRobot) { return; } - // If AutoDiff is enabled, route the step through the AD path. This - // prevents the non-AD integrator (e.g., RK4) from being used when the - // user expects AutoDiff-enabled integration. The AD path uses a - // compile-time dual-width; match the width used for the AD scratch - // buffers (64) so the DynamicsScratch/_dynResult_AD types align. if (_useAutoDiff) { - step_AD<64>(dt, simTime); + step_AD(dt, simTime); return; } _simTime = simTime; const size_t n = _robot.joints.size(); mathlib::VecX x = packState(); + auto result = step_impl(x, dt, simTime, *_integrator, _dynScratch, _dynResult); + Eigen::Index dof = x.size() / 2; + size_t links = _robot.links.size(); + + assert(_dynScratch.dense.M.rows() == dof); + assert(_dynScratch.dense.M.cols() == dof); + assert(_dynScratch.spatial.Xup.size() == links); + unpackState(result.stepOut.x_next); _dynamics->setDt(result.stepOut.dt_taken); @@ -385,11 +388,6 @@ namespace robots { buildSpatialModel(); _hasRobot = true; - _dynScratch.resize(n, m); - _dynScratch_AD.resize(n, m); - _dynResult.resize(n); - _dynResult_AD.resize(n); - resetRobot(); LOG_INFO("Loaded robot model -> %s", name.c_str()); @@ -419,11 +417,22 @@ namespace robots { _baseYawRate = 0.0; _baseYawAcc = 0.0; + _dynScratch.clear(); + _dynScratch_AD.clear(); + + _dynResult.resize(0); + _dynResult_AD.resize(0); + _dynScratch.resize(_robot.joints.size(), _robot.links.size()); _dynScratch_AD.resize(_robot.joints.size(), _robot.links.size()); _dynResult.resize(_robot.joints.size()); _dynResult_AD.resize(_robot.joints.size()); + LOG_INFO("dynScratch=%p", &_dynScratch); + LOG_INFO("dynScratchAD=%p", &_dynScratch_AD); + LOG_INFO("M rows=%d cols=%d", (int)_dynScratch.dense.M.rows(), (int)_dynScratch.dense.M.cols()); + LOG_INFO("Xup size=%d", (int)_dynScratch.spatial.Xup.size()); + // Reset adaptive integrator so it doesn't carry a stale step size _integrator->resetAdaptiveState(); diff --git a/DSFE_App/DSFE_Core/src/Scene/SimulationCore.cpp b/DSFE_App/DSFE_Core/src/Scene/SimulationCore.cpp index 4e3fbb49..226af5ed 100644 --- a/DSFE_App/DSFE_Core/src/Scene/SimulationCore.cpp +++ b/DSFE_App/DSFE_Core/src/Scene/SimulationCore.cpp @@ -59,7 +59,7 @@ namespace core { std::string SimulationCore::integrationMethodName() const { if (!_robot) { return "no_robot"; } std::string intName; - if (_robot->runtimeIntegratorState() && _robot->runtimeIntegratorState()->autoDiff) { + if (_robot->autoDiffEnabled()) { intName = _robot->getADIntegrator()->IntegratorName(_robot->getADIntegrator()->integrationMethod()); } else { @@ -126,8 +126,7 @@ namespace core { // Update robot trajectory inputs and step the robot forward in time if (hasRobot()) { _robot->updateTrajectoryInputs(*_traj, simTime); - if (state && state->autoDiff) { _robot->step_AD(_dt, simTime); } - else { _robot->step(_dt, simTime); } + _robot->step(_dt, simTime); // Telemetry update if (!_telemetryBegun) { _telemetry.beginRun(simTime, _telHz, 300.0); @@ -186,8 +185,7 @@ namespace core { } const auto state = _robot->runtimeIntegratorState(); - std::string intName = (state && state->autoDiff) ? _robot->AD_integratorName() : _robot->getIntegratorName(); - LOG_INFO("Integrator for this run: %s", intName.c_str()); + std::string intName = (_robot->autoDiffEnabled()) ? _robot->AD_integratorName() : _robot->getIntegratorName(); _data.setParentFolder(paths::runs().string()); @@ -349,8 +347,7 @@ namespace core { // Update Trajectory Inputs _robot->updateTrajectoryInputs(*_traj, simTime); - if (state && state->autoDiff) { _robot->step_AD(dt, simTime); } - else { _robot->step(dt, simTime); } + _robot->step(dt, simTime); // Telemetry beginRun if (!_telemetryBegun) { diff --git a/DSFE_App/DSFE_GUI/src/ui/ControlPanel.cpp b/DSFE_App/DSFE_GUI/src/ui/ControlPanel.cpp index 6af6f44f..3682ee82 100644 --- a/DSFE_App/DSFE_GUI/src/ui/ControlPanel.cpp +++ b/DSFE_App/DSFE_GUI/src/ui/ControlPanel.cpp @@ -440,9 +440,9 @@ namespace gui { // Integration method combo box ImGui::SectionHeader("Integration Methods"); - /*ImGui::SetNextItemWidth(150.0f); + ImGui::SetNextItemWidth(150.0f); ImGui::Checkbox("Enable Automatic Differentiable Integrators", &_useAutoDiff); - robot->enableAutoDiff(_useAutoDiff);*/ + robot->enableAutoDiff(_useAutoDiff); ImGui::SetNextItemWidth(150.0f); if (_useAutoDiff) { diff --git a/PxMLib/MathLib/include/integrators/auto_diff_integrators.inl b/PxMLib/MathLib/include/integrators/auto_diff_integrators.inl index 58cc3e88..d5de1fe7 100644 --- a/PxMLib/MathLib/include/integrators/auto_diff_integrators.inl +++ b/PxMLib/MathLib/include/integrators/auto_diff_integrators.inl @@ -333,22 +333,87 @@ namespace integration { bool converged = false; for (int iter = 0; iter < maxIter; ++iter) { + if (iter > 20) { + std::ostringstream oss; + oss << "iter=" << iter << '\n' + << "residual=" << g_real.norm() << '\n' + << "delta=" << delta.norm() << '\n'; + std::cerr << oss.str(); + OutputDebugStringA(oss.str().c_str()); + } + + mathlib::VecX_T g_dual; eval_g(x_real.template cast(), g_dual); g_real = g_dual.template cast(); - if (!g_real.allFinite()) { throw std::runtime_error("Newton received non-finite residual at iter = " + std::to_string(iter)); } - if (g_real.norm() < tol) { converged = true; break; } + if (!g_real.allFinite()) { + std::ostringstream oss; + oss << "Newton received non-finite residual at iter = " << iter << '\n'; + std::cerr << oss.str(); + OutputDebugStringA(oss.str().c_str()); + throw std::runtime_error("Newton received non-finite residual at iter = " + std::to_string(iter)); + } + if (g_real.norm() < tol) { + std::ostringstream oss; + oss << "'g_real.norm()' Converged at iter=" << iter << '\n' + << "residual=" << g_real.norm() << '\n'; + std::cerr << oss.str(); + OutputDebugStringA(oss.str().c_str()); + converged = true; + break; + } eval_j(x_real.template cast(), J); - if (!J.allFinite()) { throw std::runtime_error("Newton received non-finite Jacobian at iter = " + std::to_string(iter)); } + if (!J.allFinite()) { + std::ostringstream oss; + oss << "Newton received non-finite Jacobian at iter = " << iter << '\n'; + std::cerr << oss.str(); + OutputDebugStringA(oss.str().c_str()); + throw std::runtime_error("Newton received non-finite Jacobian at iter = " + std::to_string(iter)); + } + + std::cout << "J rows=" << J.rows() << " cols=" << J.cols() << '\n'; + double cond_est = J.fullPivLu().rcond(); + std::cout + << "iter=" << iter + << " residual=" << g_real.norm() + << " rcond=" << cond_est + << std::endl; + solver.compute(J); delta = solver.solve(-g_real); - if (!delta.allFinite()) { throw std::runtime_error("Newton produced non-finite step at iter = " + std::to_string(iter)); } + if (!delta.allFinite()) { + std::ostringstream oss; + oss << "Newton produced non-finite step at iter = " << iter << '\n'; + std::cerr << oss.str(); + OutputDebugStringA(oss.str().c_str()); + throw std::runtime_error("Newton produced non-finite step at iter = " + std::to_string(iter)); + } + x_real += delta; Real x_norm = x_real.norm(); - if (delta.norm() < tol * (Real(1) + x_norm)) { converged = true; break; } + if (delta.norm() < tol * (Real(1) + x_norm)) { + std::ostringstream oss; + oss << "'delta.norm()' Converged at iter=" << iter << '\n' + << "residual=" << g_real.norm() << '\n' + << "delta=" << delta.norm() << '\n' + << "xnorm=" << x_norm << '\n'; + std::cerr << oss.str(); + OutputDebugStringA(oss.str().c_str()); + converged = true; + break; + } } - if (!converged) { throw std::runtime_error("Newton-Raphson failed to converge after " + std::to_string(maxIter) + " iterations."); } + if (!converged) { + std::ostringstream oss; + oss << "Newton failed to converge." << '\n' + << "iter=" << maxIter << '\n' + << "residual=" << g_real.norm() << '\n' + << "tol=" << tol << '\n'; + std::cerr << oss.str(); + OutputDebugStringA(oss.str().c_str()); + throw std::runtime_error("Newton-Raphson failed to converge after " + std::to_string(maxIter) + " iterations."); + } mathlib::VecX_T x_final = x_real.template cast(); eval_j(x_final, J); @@ -386,14 +451,14 @@ namespace integration { mathlib::VecX_T x_dual(n); for (int k = 0; k < n; ++k) { std::array seed_array{}; - if (k == i) { seed_array.fill(RealScalar(0)); seed_array[0] = RealScalar(1); } + if (k == i) { seed_array.fill(RealScalar(0)); seed_array[i] = RealScalar(1); } // Set the seed for the i-th variable to 1, others to 0 x_dual(k) = Dual_T(mathlib::real(x(k)), seed_array); } Dual_T t_dual(mathlib::real(t)); auto f_dual = f(t_dual, x_dual); for (int j = 0; j < m; ++j) { - J(j, i) = mathlib::dualPart(f_dual(j)); + J(j, i) = f_dual(j).dual[i]; // The Jacobian entry J(j, i) is the dual part of f_dual(j) corresponding to the seed for x(i) } } return J; diff --git a/PxMLib/MathLib/include/integrators/numerical_integrators.h b/PxMLib/MathLib/include/integrators/numerical_integrators.h index 0b7037fb..f5501b95 100644 --- a/PxMLib/MathLib/include/integrators/numerical_integrators.h +++ b/PxMLib/MathLib/include/integrators/numerical_integrators.h @@ -6,6 +6,11 @@ #include #include #include +// TEMP +#define NOMINMAX +#define WIN32_LEAN_AND_MEAN +#include +#include // Numerical integration methods namespace integration { From 6ad62a94d1c63fb3a671597bb1bdbbecf5cc78bb Mon Sep 17 00:00:00 2001 From: saltyjoss Date: Sat, 30 May 2026 15:09:31 +0100 Subject: [PATCH 083/210] fixes(WIP): removed debugging outputs and corrected use of friction in `spatial_jacobian` * I am almost certain remaining errros reside in my clearning, resizing, and calling of `DynamicsScratch` and `DynamicsResults` --- .../include/Robots/RobotDynamics.inl | 10 ++--- .../integrators/auto_diff_integrators.inl | 41 ++----------------- 2 files changed, 7 insertions(+), 44 deletions(-) diff --git a/DSFE_App/DSFE_Core/include/Robots/RobotDynamics.inl b/DSFE_App/DSFE_Core/include/Robots/RobotDynamics.inl index 557b1769..68a7a4b1 100644 --- a/DSFE_App/DSFE_Core/include/Robots/RobotDynamics.inl +++ b/DSFE_App/DSFE_Core/include/Robots/RobotDynamics.inl @@ -456,8 +456,8 @@ namespace robots { Scalar tau_i = k_p * err + k_d * err_d + I_eff * snap.qdd_ref[i]; tau_i += scratch.g[i]; // Gravity compensation tau_i += scratch.dense.h[i]; // add Coriolis and centrifugal bias - // Scalar tau_f = dynamics::computeKarnoppFriction(qd[i], tau_i, b, c); // add friction compensation - // tau_i += tau_f; + Scalar tau_f = dynamics::computeKarnoppFriction(qd[i], tau_i, b, c); // add friction compensation + tau_i -= tau_f; scratch.dense.tau[i] = tau_i; @@ -512,13 +512,11 @@ namespace robots { const SpatialJoint& joint = model.joints[i]; if (!isControlledJoint(joint.type)) { continue; } dTau_dq(i, i) = -kp[i]; - const Scalar b = static_cast(0.2); // viscous damping coefficient + const Scalar b = -static_cast(0.2); // viscous damping coefficient const Scalar c = static_cast(0.05); // Coulomb friction coefficient const Scalar eps_f = static_cast(1e-2); - Scalar tanh_term = mathlib::tanh(qd[i] / eps_f); - Scalar stiff_friction_slope = -c * (Scalar(1.0) - tanh_term * tanh_term) / eps_f; - dTau_dv(i, i) = -kd[i] - b; + dTau_dv(i, i) = -kd[i] + b; } Eigen::LDLT> solver(scratch.dense.M); // compute the Cholesky decomposition of the mass matrix for efficient solving diff --git a/PxMLib/MathLib/include/integrators/auto_diff_integrators.inl b/PxMLib/MathLib/include/integrators/auto_diff_integrators.inl index d5de1fe7..8853d4b9 100644 --- a/PxMLib/MathLib/include/integrators/auto_diff_integrators.inl +++ b/PxMLib/MathLib/include/integrators/auto_diff_integrators.inl @@ -333,16 +333,6 @@ namespace integration { bool converged = false; for (int iter = 0; iter < maxIter; ++iter) { - if (iter > 20) { - std::ostringstream oss; - oss << "iter=" << iter << '\n' - << "residual=" << g_real.norm() << '\n' - << "delta=" << delta.norm() << '\n'; - std::cerr << oss.str(); - OutputDebugStringA(oss.str().c_str()); - } - - mathlib::VecX_T g_dual; eval_g(x_real.template cast(), g_dual); g_real = g_dual.template cast(); @@ -353,15 +343,8 @@ namespace integration { OutputDebugStringA(oss.str().c_str()); throw std::runtime_error("Newton received non-finite residual at iter = " + std::to_string(iter)); } - if (g_real.norm() < tol) { - std::ostringstream oss; - oss << "'g_real.norm()' Converged at iter=" << iter << '\n' - << "residual=" << g_real.norm() << '\n'; - std::cerr << oss.str(); - OutputDebugStringA(oss.str().c_str()); - converged = true; - break; - } + if (g_real.norm() < tol) { converged = true; break; } + eval_j(x_real.template cast(), J); if (!J.allFinite()) { std::ostringstream oss; @@ -371,14 +354,6 @@ namespace integration { throw std::runtime_error("Newton received non-finite Jacobian at iter = " + std::to_string(iter)); } - std::cout << "J rows=" << J.rows() << " cols=" << J.cols() << '\n'; - double cond_est = J.fullPivLu().rcond(); - std::cout - << "iter=" << iter - << " residual=" << g_real.norm() - << " rcond=" << cond_est - << std::endl; - solver.compute(J); delta = solver.solve(-g_real); if (!delta.allFinite()) { @@ -391,17 +366,7 @@ namespace integration { x_real += delta; Real x_norm = x_real.norm(); - if (delta.norm() < tol * (Real(1) + x_norm)) { - std::ostringstream oss; - oss << "'delta.norm()' Converged at iter=" << iter << '\n' - << "residual=" << g_real.norm() << '\n' - << "delta=" << delta.norm() << '\n' - << "xnorm=" << x_norm << '\n'; - std::cerr << oss.str(); - OutputDebugStringA(oss.str().c_str()); - converged = true; - break; - } + if (delta.norm() < tol * (Real(1) + x_norm)) { converged = true; break; } } if (!converged) { From f70feaf9c4f6bf0e788d48221c49671bfeeacee7 Mon Sep 17 00:00:00 2001 From: saltyjoss Date: Sat, 30 May 2026 16:58:57 +0100 Subject: [PATCH 084/210] fixes: Removed incorrect asserts * I am now going to merge this with ANOTHER branch (it will be named `Dev/template_refactor_code_cleanup`, in order to focus on correcting template syntax and general code pipeline, I have found that I am struggling to mentally visualise some parts of my code, and I think this is down to rapid development. The resolution I have is to spend some time basically house cleaning (which for me means correcting and cleaning code), aswell as starting the process of prepping DSFE for not just ROBOT SYSTEMs, this may mean a bigger refactor of how I step in time, with a more general method (maybe, or that I actually just have many steppers for each sub project, robotsystems being one, particledynamics being another, stellardynamics being another) --- DSFE_App/DSFE_Core/CMakeLists.txt | 3 +++ DSFE_App/DSFE_Core/include/Robots/DynamicsTypes.h | 13 +++++++++++++ DSFE_App/DSFE_Core/include/Robots/RobotDynamics.inl | 8 ++++---- .../DSFE_Core/include/Robots/SpatialDynamics.inl | 5 +---- DSFE_App/DSFE_Core/src/Robots/RobotSystem.cpp | 7 ------- 5 files changed, 21 insertions(+), 15 deletions(-) diff --git a/DSFE_App/DSFE_Core/CMakeLists.txt b/DSFE_App/DSFE_Core/CMakeLists.txt index aae5a99b..a12a6709 100644 --- a/DSFE_App/DSFE_Core/CMakeLists.txt +++ b/DSFE_App/DSFE_Core/CMakeLists.txt @@ -8,6 +8,9 @@ find_package(Threads REQUIRED) find_package(HDF5 CONFIG REQUIRED) target_compile_definitions(DSFE_Core PRIVATE DSFE_CORE_EXPORTS) +if (MSVC) +target_compile_options(DSFE_Core PRIVATE /bigobj) +endif() set(CORE_SRC src/EngineCore.cpp diff --git a/DSFE_App/DSFE_Core/include/Robots/DynamicsTypes.h b/DSFE_App/DSFE_Core/include/Robots/DynamicsTypes.h index 197d600f..5baa5b61 100644 --- a/DSFE_App/DSFE_Core/include/Robots/DynamicsTypes.h +++ b/DSFE_App/DSFE_Core/include/Robots/DynamicsTypes.h @@ -114,6 +114,12 @@ namespace robots { u.resize(nJoints); d.resize(nJoints); + dXup_dq.resize(nJoints); + dv_dq.resize(nJoints); + dv_dqd.resize(nJoints); + dc_dq.resize(nJoints); + dc_dqd.resize(nJoints); + jointCap = nJoints; } @@ -128,6 +134,13 @@ namespace robots { U.clear(); u.resize(0); d.resize(0); + + dXup_dq.clear(); + dv_dq.clear(); + dv_dqd.clear(); + dc_dq.clear(); + dc_dqd.clear(); + jointCap = 0; } }; diff --git a/DSFE_App/DSFE_Core/include/Robots/RobotDynamics.inl b/DSFE_App/DSFE_Core/include/Robots/RobotDynamics.inl index 68a7a4b1..7685ff21 100644 --- a/DSFE_App/DSFE_Core/include/Robots/RobotDynamics.inl +++ b/DSFE_App/DSFE_Core/include/Robots/RobotDynamics.inl @@ -456,8 +456,8 @@ namespace robots { Scalar tau_i = k_p * err + k_d * err_d + I_eff * snap.qdd_ref[i]; tau_i += scratch.g[i]; // Gravity compensation tau_i += scratch.dense.h[i]; // add Coriolis and centrifugal bias - Scalar tau_f = dynamics::computeKarnoppFriction(qd[i], tau_i, b, c); // add friction compensation - tau_i -= tau_f; + Scalar tau_f = dynamics::computeKarnoppFriction(qd[i], tau_i, c, b); // add friction compensation + tau_i += tau_f; scratch.dense.tau[i] = tau_i; @@ -512,11 +512,11 @@ namespace robots { const SpatialJoint& joint = model.joints[i]; if (!isControlledJoint(joint.type)) { continue; } dTau_dq(i, i) = -kp[i]; - const Scalar b = -static_cast(0.2); // viscous damping coefficient + const Scalar b = static_cast(0.2); // viscous damping coefficient const Scalar c = static_cast(0.05); // Coulomb friction coefficient const Scalar eps_f = static_cast(1e-2); - dTau_dv(i, i) = -kd[i] + b; + dTau_dv(i, i) = -kd[i] - b; } Eigen::LDLT> solver(scratch.dense.M); // compute the Cholesky decomposition of the mass matrix for efficient solving diff --git a/DSFE_App/DSFE_Core/include/Robots/SpatialDynamics.inl b/DSFE_App/DSFE_Core/include/Robots/SpatialDynamics.inl index 01fe14e2..5382736c 100644 --- a/DSFE_App/DSFE_Core/include/Robots/SpatialDynamics.inl +++ b/DSFE_App/DSFE_Core/include/Robots/SpatialDynamics.inl @@ -45,10 +45,7 @@ namespace robots { c_out[i] = crossMotion(v_out[i], vJ); // Coriolis Term using corScalar = typename std::decay_t; - static_assert( - std::is_same_v, - "c_out scalar type does not match model scalar type" - ); + static_assert(std::is_same_v, "c_out scalar type does not match model scalar type"); } } diff --git a/DSFE_App/DSFE_Core/src/Robots/RobotSystem.cpp b/DSFE_App/DSFE_Core/src/Robots/RobotSystem.cpp index 31b912ef..1f4fb65f 100644 --- a/DSFE_App/DSFE_Core/src/Robots/RobotSystem.cpp +++ b/DSFE_App/DSFE_Core/src/Robots/RobotSystem.cpp @@ -261,13 +261,6 @@ namespace robots { auto result = step_impl(x, dt, simTime, *_integrator, _dynScratch, _dynResult); - Eigen::Index dof = x.size() / 2; - size_t links = _robot.links.size(); - - assert(_dynScratch.dense.M.rows() == dof); - assert(_dynScratch.dense.M.cols() == dof); - assert(_dynScratch.spatial.Xup.size() == links); - unpackState(result.stepOut.x_next); _dynamics->setDt(result.stepOut.dt_taken); From c99dc91151e9f4dc15eafbba830e070dd886e5c9 Mon Sep 17 00:00:00 2001 From: saltyjoss Date: Sat, 30 May 2026 19:16:52 +0100 Subject: [PATCH 085/210] fixes: Correct misuses of gravity compinent of scratch, and initialised in reset of robot (temp for now) --- .../DSFE_Core/include/Numerics/IntegrationService.h | 2 +- DSFE_App/DSFE_Core/include/Robots/RobotDynamics.inl | 2 -- .../DSFE_Core/include/Robots/RobotSystemStep.inl | 12 +++++------- DSFE_App/DSFE_Core/src/Robots/RobotSystem.cpp | 7 ++++++- 4 files changed, 12 insertions(+), 11 deletions(-) diff --git a/DSFE_App/DSFE_Core/include/Numerics/IntegrationService.h b/DSFE_App/DSFE_Core/include/Numerics/IntegrationService.h index 3fb8051a..5854876e 100644 --- a/DSFE_App/DSFE_Core/include/Numerics/IntegrationService.h +++ b/DSFE_App/DSFE_Core/include/Numerics/IntegrationService.h @@ -63,7 +63,7 @@ namespace integration { template StepOut_T step(eAutoDiffIntegrationMethod m, const mathlib::VecX_T& x, Scalar t, Scalar dt, Func&& f); const std::string IntegratorName(eAutoDiffIntegrationMethod m); - void setIntegrationMethod(eAutoDiffIntegrationMethod m) { LOG_INFO("AD integrator changed to enum %d", (int)_m); _m = m; } + void setIntegrationMethod(eAutoDiffIntegrationMethod m) { _m = m; LOG_INFO("AD integrator changed to enum %d", (int)_m); } eAutoDiffIntegrationMethod integrationMethod() const { return _m; } std::shared_ptr runtimeState() { return _state; } std::shared_ptr runtimeState() const { return _state; } diff --git a/DSFE_App/DSFE_Core/include/Robots/RobotDynamics.inl b/DSFE_App/DSFE_Core/include/Robots/RobotDynamics.inl index 7685ff21..01b4801c 100644 --- a/DSFE_App/DSFE_Core/include/Robots/RobotDynamics.inl +++ b/DSFE_App/DSFE_Core/include/Robots/RobotDynamics.inl @@ -454,8 +454,6 @@ namespace robots { const Scalar eps_f = static_cast(1e-2); Scalar tau_i = k_p * err + k_d * err_d + I_eff * snap.qdd_ref[i]; - tau_i += scratch.g[i]; // Gravity compensation - tau_i += scratch.dense.h[i]; // add Coriolis and centrifugal bias Scalar tau_f = dynamics::computeKarnoppFriction(qd[i], tau_i, c, b); // add friction compensation tau_i += tau_f; diff --git a/DSFE_App/DSFE_Core/include/Robots/RobotSystemStep.inl b/DSFE_App/DSFE_Core/include/Robots/RobotSystemStep.inl index 2e6b4a98..96d8c63d 100644 --- a/DSFE_App/DSFE_Core/include/Robots/RobotSystemStep.inl +++ b/DSFE_App/DSFE_Core/include/Robots/RobotSystemStep.inl @@ -121,6 +121,9 @@ namespace robots { ); }; + if (!x.allFinite()) { LOG_ERROR("[step_impl] input state already non-finite"); } + LOG_ERROR("[step_impl] input qd = %.17g %.17g %.17g %.17g %.17g %.17g %.17g", x[7], x[8], x[9], x[10], x[11], x[12], x[13]); + if constexpr (std::is_same_v, integration::IntegrationService>) { mathlib::VecX x_real = x.template cast(); auto step = integrator.step(_curIntMethod, x_real, static_cast(t), static_cast(dt), f_deriv, f_J); @@ -129,6 +132,7 @@ namespace robots { result.stepOut.dt_sug = step.dt_sug; } else if constexpr (std::is_same_v, integration::DifferentiableIntegrator>) { + _curIntMethod_AD = integrator.integrationMethod(); result.stepOut = integrator.step(_curIntMethod_AD, x, t, dt, f_deriv); } @@ -149,7 +153,7 @@ namespace robots { Eigen::Map qd_next(x.data() + n, n); // Enforce joint limits - for (auto& j : _robot.joints) { enforceJointLimits(j); } + /*for (auto& j : _robot.joints) { enforceJointLimits(j); }*/ // Recompute kinematics and dynamics at the new state for logging and control purposes std::vector T_world(result.snap.model->links.size()); @@ -197,12 +201,6 @@ namespace robots { const double I_eff = (j.type == eJointType::FIXED) ? 1.0 : mathlib::real(dynResult.metrics.I_eff[i]); const double err = q_ref_real[i] - q_real[i]; const double err_d = qd_ref_real[i] - qd_real[i]; - - //LOG_INFO("q norm: %.12f", q_real.norm()); - //LOG_INFO("qd norm: %.12f", qd_real.norm()); - //LOG_INFO("qdd norm: %.12f", dynResult.metrics.qdd.norm()); - //LOG_INFO("tau_rnea norm : % .12f", tau_rnea_real.norm()); - JointLogBuffer::JointLogEntry e{}; e.sim_time = _simTime; diff --git a/DSFE_App/DSFE_Core/src/Robots/RobotSystem.cpp b/DSFE_App/DSFE_Core/src/Robots/RobotSystem.cpp index 1f4fb65f..5048b180 100644 --- a/DSFE_App/DSFE_Core/src/Robots/RobotSystem.cpp +++ b/DSFE_App/DSFE_Core/src/Robots/RobotSystem.cpp @@ -264,7 +264,10 @@ namespace robots { unpackState(result.stepOut.x_next); _dynamics->setDt(result.stepOut.dt_taken); - //postStepUpdate(result.stepOut.x_next, _dynScratch, result); + const auto scratchCopy = _dynScratch; + const auto resultCopy = result; + + //postStepUpdate(resultCopy.stepOut.x_next, scratchCopy, resultCopy); // Update base pose if free-floating if (_baseIsFree) { @@ -421,6 +424,8 @@ namespace robots { _dynResult.resize(_robot.joints.size()); _dynResult_AD.resize(_robot.joints.size()); + _dynScratch.g.setConstant(_gravity); + LOG_INFO("dynScratch=%p", &_dynScratch); LOG_INFO("dynScratchAD=%p", &_dynScratch_AD); LOG_INFO("M rows=%d cols=%d", (int)_dynScratch.dense.M.rows(), (int)_dynScratch.dense.M.cols()); From 2080c6dd9fee96101aea46cd7b678d0d52cfff9a Mon Sep 17 00:00:00 2001 From: saltyjoss Date: Sun, 31 May 2026 12:37:00 +0100 Subject: [PATCH 086/210] refactor: Updated `RobotDynamics` derivative and jacobian methods to no longer hardcode joint dynamic values. --- .../include/Robots/RobotDynamics.inl | 36 ++++++++++--------- 1 file changed, 19 insertions(+), 17 deletions(-) diff --git a/DSFE_App/DSFE_Core/include/Robots/RobotDynamics.inl b/DSFE_App/DSFE_Core/include/Robots/RobotDynamics.inl index 01b4801c..424f289b 100644 --- a/DSFE_App/DSFE_Core/include/Robots/RobotDynamics.inl +++ b/DSFE_App/DSFE_Core/include/Robots/RobotDynamics.inl @@ -264,10 +264,10 @@ namespace robots { const Scalar I_eff = mathlib::LSE_smoothMax(scratch.M(i, i), eps); const Scalar k_p = I_eff * wn * wn; - const Scalar k_d = Scalar(2.0) * z * I_eff * wn; + const Scalar k_d = Scalar(2) * z * I_eff * wn; - const Scalar b = static_cast(0.2); // viscous damping coefficient - const Scalar c = static_cast(0.05); // Coulomb friction coefficient + const Scalar b = static_cast(joint.dynamics.damping); // viscous damping coefficient + const Scalar c = static_cast(joint.dynamics.friction); // Coulomb friction coefficient const Scalar eps_f = static_cast(1e-2); dTau_dq(i, i) = -k_p; @@ -339,8 +339,8 @@ namespace robots { const Scalar k_p = I_eff * wn * wn; // [Nm/rad], proportional gain const Scalar k_d = Scalar(2.0) * z * I_eff * wn; // [Nm/(rad/s)], derivative gain - const Scalar b = static_cast(0.2); // viscous damping coefficient - const Scalar c = static_cast(0.05); // Coulomb friction coefficient + const Scalar b = static_cast(joint.dynamics.damping); // viscous damping coefficient + const Scalar c = static_cast(joint.dynamics.friction); // Coulomb friction coefficient const Scalar eps_f = static_cast(1e-2); Scalar tau_i = k_p * err + k_d * err_d + I_eff * snap.qdd_ref[i]; // [Nm], control torque for joint i @@ -437,8 +437,8 @@ namespace robots { continue; } - const Scalar wn = static_cast(5); - const Scalar z = static_cast(0.7); + const Scalar wn = static_cast(snap.model->joints[i].wn_target); + const Scalar z = static_cast(snap.model->joints[i].zeta_target); const Scalar err = snap.q_ref[i] - q[i]; const Scalar err_d = snap.qd_ref[i] - qd[i]; @@ -449,8 +449,8 @@ namespace robots { const Scalar k_p = I_eff * wn * wn; const Scalar k_d = Scalar(2) * z * I_eff * wn; - const Scalar b = static_cast(0.2); // viscous damping coefficient - const Scalar c = static_cast(0.05); // Coulomb friction coefficient + const Scalar b = static_cast(snap.model->joints[i].dynamics.damping); // viscous damping coefficient + const Scalar c = static_cast(snap.model->joints[i].dynamics.friction); // Coulomb friction coefficient const Scalar eps_f = static_cast(1e-2); Scalar tau_i = k_p * err + k_d * err_d + I_eff * snap.qdd_ref[i]; @@ -510,8 +510,8 @@ namespace robots { const SpatialJoint& joint = model.joints[i]; if (!isControlledJoint(joint.type)) { continue; } dTau_dq(i, i) = -kp[i]; - const Scalar b = static_cast(0.2); // viscous damping coefficient - const Scalar c = static_cast(0.05); // Coulomb friction coefficient + const Scalar b = static_cast(snap.model->joints[i].dynamics.damping); // viscous damping coefficient + const Scalar c = static_cast(snap.model->joints[i].dynamics.friction); // Coulomb friction coefficient const Scalar eps_f = static_cast(1e-2); dTau_dv(i, i) = -kd[i] - b; @@ -567,11 +567,12 @@ namespace robots { scratch.dense.tau.setZero(); for (size_t i = 0; i < n; ++i) { - if (snap.model->joints[i].type == eJointType::FIXED) continue; + const RobotJoint& joint = snap.model->joints[i]; + if (joint.type == eJointType::FIXED) continue; const Scalar eps = static_cast < Scalar>(1e-6); - const Scalar b = static_cast(0.2); // viscous damping coefficient - const Scalar c = static_cast(0.05); // Coulomb friction coefficient + const Scalar b = static_cast(joint.dynamics.damping); // viscous damping coefficient + const Scalar c = static_cast(joint.dynamics.friction); // Coulomb friction coefficient const Scalar eps_f = static_cast(1e-2); Scalar tau_i = kp[i] * (snap.q_ref[i] - q[i]) + kd[i] * (snap.qd_ref[i] - qd[i]) + mathlib::LSE_smoothMax(scratch.dense.M(i, i), eps) * snap.qdd_ref[i]; @@ -618,12 +619,13 @@ namespace robots { mathlib::MatX_T dTau_dv = mathlib::MatX_T::Zero(n, n); for (size_t i = 0; i < n; ++i) { - if (snap.model->joints[i].type == eJointType::FIXED) continue; + const RobotJoint& joint = snap.model->joints[i]; + if (joint.type == eJointType::FIXED) continue; dTau_dq(i, i) = -kp[i]; - const Scalar b = static_cast(0.2); // viscous damping coefficient - const Scalar c = static_cast(0.05); // Coulomb friction coefficient + const Scalar b = static_cast(joint.dynamics.damping); // viscous damping coefficient + const Scalar c = static_cast(joint.dynamics.friction); // Coulomb friction coefficient const Scalar eps_f = static_cast(1e-2); Scalar tanh_term = mathlib::tanh(qd[i] / eps_f); From dfdbbd9a1aded08713f20d2efe833d80d2b52973 Mon Sep 17 00:00:00 2001 From: saltyjoss Date: Sun, 31 May 2026 12:37:53 +0100 Subject: [PATCH 087/210] feat: Added `packState_AD` and `unpackState_AD` for AutoDiff stepping --- .../DSFE_Core/include/Robots/RobotSystem.h | 37 +++++----- .../include/Robots/RobotSystemStep.inl | 45 ++++++------ DSFE_App/DSFE_Core/src/Robots/RobotSystem.cpp | 68 ++++++++++++++++++- 3 files changed, 108 insertions(+), 42 deletions(-) diff --git a/DSFE_App/DSFE_Core/include/Robots/RobotSystem.h b/DSFE_App/DSFE_Core/include/Robots/RobotSystem.h index 9670c894..9ee3c15f 100644 --- a/DSFE_App/DSFE_Core/include/Robots/RobotSystem.h +++ b/DSFE_App/DSFE_Core/include/Robots/RobotSystem.h @@ -54,6 +54,8 @@ namespace robots { // --- Utility Methods --- static double clampJointAngle(const RobotJoint& joint, double angleRad); + template + static T clampJointAngle_T(const RobotJoint& joint, T angleRad); // ---- Accessors --- @@ -126,8 +128,8 @@ namespace robots { // --- SIMULATION STEP METHOD --- - template - RobotSimSnapshot_T takeSnapshot(Scalar simTime) const; + template + RobotSimSnapshot_T takeSnapshot(T simTime) const; template void step_AD(double dt, double simTime); @@ -154,32 +156,18 @@ namespace robots { integration::eIntegrationMethod getIntegrationMethod() const { return _curIntMethod; } std::string getIntegratorName() const { return _integrator->IntegratorName(_curIntMethod); } - void setIntegrationMethod(integration::eIntegrationMethod method) { - LOG_INFO("Integrator changed to enum %d", (int)method); - _curIntMethod = method; - const auto state = _integrator->runtimeState(); - state->backend = integration::eIntegrationBackend::Standard; - state->autoDiff = false; - _useAutoDiff = false; - } + void setStandardIntegrator(integration::eIntegrationMethod m) { _curIntMethod = m; } integration::eAutoDiffIntegrationMethod AD_IntegrationMethod() const { return _curIntMethod_AD; } std::string AD_integratorName() const { return _AD_integrator->IntegratorName(_curIntMethod_AD); } - void setIntegrationMethod(integration::eAutoDiffIntegrationMethod method) { - LOG_INFO("AD integrator changed to enum %d", (int)method); - _curIntMethod_AD = method; - const auto state = _AD_integrator->runtimeState(); - state->backend = integration::eIntegrationBackend::AutoDiff; - state->autoDiff = true; - _useAutoDiff = true; - } + void setADIntegrator(integration::eAutoDiffIntegrationMethod m) { _curIntMethod_AD = m; } integration::IntegrationService* getIntegrator(); const integration::IntegrationService* getIntegrator() const; integration::DifferentiableIntegrator* getADIntegrator(); const integration::DifferentiableIntegrator* getADIntegrator() const; - + bool autoDiffEnabled() const { return _useAutoDiff; } void enableAutoDiff(bool enable) { _useAutoDiff = enable; } @@ -214,8 +202,8 @@ namespace robots { DynamicsScratch& dynamicScratch, DynamicsResult& dynamicResult ); - template - void postStepUpdate(const mathlib::VecX& x, const DynamicsScratch& scratch, const RobotStepResult_T& result); + template + void postStepUpdate(const mathlib::VecX_T& x, const DynamicsScratch& scratch, const RobotStepResult_T& result); std::unique_ptr _kinematics; std::unique_ptr _dynamics; @@ -244,6 +232,13 @@ namespace robots { mathlib::VecX packState() const; void unpackState(const mathlib::VecX& x); + template + void unpackState(const mathlib::VecX_T& x); + + // State packing and unpacking using a DualNumber vector. + mathlib::VecX_T> packState_AD() const; + void unpackState_AD(const mathlib::VecX_T>& x); + // Reference state packing and unpacking mathlib::VecX packRefState() const; void unpackRefState(const mathlib::VecX& xr); diff --git a/DSFE_App/DSFE_Core/include/Robots/RobotSystemStep.inl b/DSFE_App/DSFE_Core/include/Robots/RobotSystemStep.inl index 96d8c63d..38dd2c44 100644 --- a/DSFE_App/DSFE_Core/include/Robots/RobotSystemStep.inl +++ b/DSFE_App/DSFE_Core/include/Robots/RobotSystemStep.inl @@ -2,10 +2,16 @@ #pragma once namespace robots { + template + T RobotSystem::clampJointAngle_T(const RobotJoint& joint, T angleRad) { + if (joint.limits.continuous) { return mathlib::wrapRad(angleRad); } + else { return std::clamp(angleRad, T(joint.limits.minAngle), T(joint.limits.maxAngle)); } + } + // Method to take a snapshot of the current robot state - template - RobotSimSnapshot_T RobotSystem::takeSnapshot(Scalar simTime) const { - RobotSimSnapshot_T snap; + template + RobotSimSnapshot_T RobotSystem::takeSnapshot(T simTime) const { + RobotSimSnapshot_T snap; snap.model = &_constModel; const size_t n = (size_t)_robot.joints.size(); @@ -27,14 +33,14 @@ namespace robots { snap.qdd_ref[i] = j.qdd_ref; } - snap.robotRootPose = _robotRootPose.template cast(); + snap.robotRootPose = _robotRootPose.template cast(); snap.baseIsFree = _baseIsFree; - snap.lastBaseForwardForce = Scalar(_lastBaseForwardForce); - snap.gravity = Scalar(_gravity); + snap.lastBaseForwardForce = T(_lastBaseForwardForce); + snap.gravity = T(_gravity); snap.torqueMode = _robot.torqueMode; - snap.dt = Scalar(_dynamics->dt()); + snap.dt = T(_dynamics->dt()); snap.simTime = simTime; return snap; @@ -122,7 +128,6 @@ namespace robots { }; if (!x.allFinite()) { LOG_ERROR("[step_impl] input state already non-finite"); } - LOG_ERROR("[step_impl] input qd = %.17g %.17g %.17g %.17g %.17g %.17g %.17g", x[7], x[8], x[9], x[10], x[11], x[12], x[13]); if constexpr (std::is_same_v, integration::IntegrationService>) { mathlib::VecX x_real = x.template cast(); @@ -132,7 +137,6 @@ namespace robots { result.stepOut.dt_sug = step.dt_sug; } else if constexpr (std::is_same_v, integration::DifferentiableIntegrator>) { - _curIntMethod_AD = integrator.integrationMethod(); result.stepOut = integrator.step(_curIntMethod_AD, x, t, dt, f_deriv); } @@ -145,12 +149,12 @@ namespace robots { return result; } - template - void RobotSystem::postStepUpdate(const mathlib::VecX& x, const DynamicsScratch& dynScratch, const RobotStepResult_T& result) { + template + void RobotSystem::postStepUpdate(const mathlib::VecX_T& x, const DynamicsScratch& dynScratch, const RobotStepResult_T& result) { const size_t n = result.snap.model->joints.size(); - Eigen::Map q_next(x.data(), n); - Eigen::Map qd_next(x.data() + n, n); + Eigen::Map> q_next(x.data(), n); + Eigen::Map> qd_next(x.data() + n, n); // Enforce joint limits /*for (auto& j : _robot.joints) { enforceJointLimits(j); }*/ @@ -158,11 +162,11 @@ namespace robots { // Recompute kinematics and dynamics at the new state for logging and control purposes std::vector T_world(result.snap.model->links.size()); _kinematics->computeForwardKinematics_fromState(*result.snap.model, x, T_world); - std::vector jointWorldPoses = _kinematics->calcJointWorldPoses(T_world, *result.snap.model); + + // Alternative would be just // Compute mass matrix at the new state mathlib::MatX M = dynScratch.dense.M.unaryExpr([](const auto& v) { return mathlib::real(v); }); - mathlib::VecX tau_g = _dynamics->computeGravityTorque(*result.snap.model, T_world, jointWorldPoses); // Extract real parts of relevant variables for logging and control mathlib::VecX q_real = result.snap.q.unaryExpr([](const auto& v) { return mathlib::real(v); }); @@ -209,7 +213,7 @@ namespace robots { e.theta = q_real[i]; e.omega = qd_real[i]; e.alpha = mathlib::real(dynResult.metrics.qdd[i]); e.err = err; e.err_d = err_d; e.I_eff = I_eff; - e.tau = mathlib::real(dynResult.metrics.tau[i]); e.tau_ff = tau_rnea_real[i]; e.tau_gravity = tau_g[i]; + e.tau = mathlib::real(dynResult.metrics.tau[i]); e.tau_ff = tau_rnea_real[i]; e.tau_gravity = 0.0; e.tau_sat = mathlib::real(dynResult.metrics.tau_sat[i]); e.KE = sys_KE; e.PE = sys_PE; e.E_total = sys_E; e.clamp_theta = mathlib::real(_clampTheta[i]); e.clamp_omega = mathlib::real(_clampOmega[i]); @@ -225,17 +229,18 @@ namespace robots { using Dual = mathlib::DualNumber_T; _simTime = simTime; const size_t n = _robot.joints.size(); - mathlib::VecX_T x = packState().template cast(); + mathlib::VecX_T x = packState_AD(); assert((size_t)x.size() <= NVar && "State size exceeds the number of dual variables."); // Checks state vector size is within the dual variable limit for (size_t i = 0; i < (size_t)x.size(); ++i) { x[i].dual[i] = 1.0; } auto result = step_impl(x, Dual(dt), Dual(simTime), *_AD_integrator, _dynScratch_AD, _dynResult_AD); - mathlib::VecX x_real = result.stepOut.x_next.unaryExpr([](const auto& v) { return mathlib::real(v); }); - unpackState(x_real); + unpackState_AD(result.stepOut.x_next); _dynamics->setDt(result.stepOut.dt_taken); - // postStepUpdate(x_real, _dynScratch_AD, result); + auto x_real = result.stepOut.x_next.unaryExpr([](const auto& v) { return mathlib::real(v); }); + + //postStepUpdate(x_real, _dynScratch_AD, result); // Update base pose if free-floating if (_baseIsFree) { diff --git a/DSFE_App/DSFE_Core/src/Robots/RobotSystem.cpp b/DSFE_App/DSFE_Core/src/Robots/RobotSystem.cpp index 5048b180..5e04f22e 100644 --- a/DSFE_App/DSFE_Core/src/Robots/RobotSystem.cpp +++ b/DSFE_App/DSFE_Core/src/Robots/RobotSystem.cpp @@ -210,6 +210,72 @@ namespace robots { } } + // Method to pack robot joint states into a state vector + mathlib::VecX_T> RobotSystem::packState_AD() const { + using Dual = DualNumber_T; // only hardcoded since I am testing the same arm, TODO provide a better final way to derive the NVar val. + const size_t n = static_cast(_robot.joints.size()); + mathlib::VecX_T x(2 * n); + + // Pack angles and velocities + for (size_t i = 0; i < n; ++i) { + auto& j = _robot.joints[i]; + + // Current states + x[i] = Dual(j.q, { 0.0 }); + x[i + n] = Dual(j.qd, { 0.0 }); + } + return x; // state vector + } + + // Method to unpack state vector into robot joints + void RobotSystem::unpackState_AD(const mathlib::VecX_T>& x) { + using Dual = DualNumber_T; + const size_t n = static_cast(_robot.joints.size()); + + // Resize clamping vectors if necessary + if (_clampTheta.size() != n) { _clampTheta.assign(n, 0); } + if (_clampOmega.size() != n) { _clampOmega.assign(n, 0); } + + // For each joint + for (size_t i = 0; i < n; ++i) { + auto& j = _robot.joints[i]; + + // Current states + Dual theta_in = x[i]; // [rad] + Dual omega_in = x[i + n]; // [rad/s] + + // Clamp joint angle + Dual theta_out = clampJointAngle_T(j, theta_in); + + // max |omega| + Dual wMax_hw = mathlib::abs(j.limits.maxqd); + Dual omega_out = omega_in; + + // Velocity limit clamping + if (wMax_hw > Dual(0)) { + const Dual eps = Dual(5e-2); + if (mathlib::abs(omega_in) > (Dual(1) + eps) * wMax_hw) { + omega_out = std::clamp(omega_in, -wMax_hw, wMax_hw); + } + } + + // Velocity limit enforcement + if (theta_out != theta_in) { + const double upperLimit = j.limits.maxAngle; + const double lowerLimit = j.limits.minAngle; + if (theta_out >= upperLimit && omega_in > Dual(0)) { omega_out = Dual(0); } + if (theta_out <= lowerLimit && omega_in < Dual(0)) { omega_out = Dual(0); } + } + + // Record clamping + _clampTheta[i] = (theta_in != theta_out) ? 1 : 0; + _clampOmega[i] = (omega_in != omega_out) ? 1 : 0; + // Update joint states + j.q = mathlib::real(theta_out); + j.qd = mathlib::real(omega_out); + } + } + // Method to pack reference state vector (target angles and velocities) for control mathlib::VecX RobotSystem::packRefState() const { const size_t n = (int)_robot.joints.size(); @@ -267,7 +333,7 @@ namespace robots { const auto scratchCopy = _dynScratch; const auto resultCopy = result; - //postStepUpdate(resultCopy.stepOut.x_next, scratchCopy, resultCopy); + postStepUpdate(resultCopy.stepOut.x_next, scratchCopy, resultCopy); // Update base pose if free-floating if (_baseIsFree) { From cfbaab4c00c36ee2931bcdb6761a6485ac2ba4d9 Mon Sep 17 00:00:00 2001 From: saltyjoss Date: Sun, 31 May 2026 12:38:32 +0100 Subject: [PATCH 088/210] fixes: Updated usage of set and get both standard and autodiff integration methods in core and GUI callbacks. --- .../include/Platform/ISimulationCore.h | 2 +- .../DSFE_Core/include/Scene/SimulationCore.h | 3 +- .../src/Interpreter/StoredProgram.cpp | 2 +- .../DSFE_Core/src/Scene/SimulationCore.cpp | 48 +++++++++---------- .../include/Scene/SimulationManager.h | 2 +- .../DSFE_GUI/src/Scene/SimulationManager.cpp | 4 +- DSFE_App/DSFE_GUI/src/ui/ControlPanel.cpp | 2 +- 7 files changed, 31 insertions(+), 32 deletions(-) diff --git a/DSFE_App/DSFE_Core/include/Platform/ISimulationCore.h b/DSFE_App/DSFE_Core/include/Platform/ISimulationCore.h index c400d821..27b7b6da 100644 --- a/DSFE_App/DSFE_Core/include/Platform/ISimulationCore.h +++ b/DSFE_App/DSFE_Core/include/Platform/ISimulationCore.h @@ -38,7 +38,7 @@ namespace core { virtual SimulationSnapshot snapshot() const = 0; // Integrator virtual void setIntegrationMethod(integration::eIntegrationMethod method) = 0; - virtual void setIntegrationMethod(integration::eAutoDiffIntegrationMethod method) = 0; + virtual void setADIntegrationMethod(integration::eAutoDiffIntegrationMethod method) = 0; virtual std::string integrationMethodName() const = 0; virtual integration::eIntegrationMethod integrationMethod() const = 0; virtual integration::eAutoDiffIntegrationMethod autoDiffIntegrationMethod() const = 0; diff --git a/DSFE_App/DSFE_Core/include/Scene/SimulationCore.h b/DSFE_App/DSFE_Core/include/Scene/SimulationCore.h index e48fc5bd..22d6bc17 100644 --- a/DSFE_App/DSFE_Core/include/Scene/SimulationCore.h +++ b/DSFE_App/DSFE_Core/include/Scene/SimulationCore.h @@ -66,7 +66,7 @@ namespace core { // Integrator void setupSimulationIntegrator(); void setIntegrationMethod(integration::eIntegrationMethod method) override; - void setIntegrationMethod(integration::eAutoDiffIntegrationMethod method) override; + void setADIntegrationMethod(integration::eAutoDiffIntegrationMethod method) override; std::string integrationMethodName() const override; integration::eIntegrationMethod integrationMethod() const override; integration::eAutoDiffIntegrationMethod autoDiffIntegrationMethod() const override; @@ -135,6 +135,7 @@ namespace core { private: // Export thread management void exportThreadMain(); + void scriptParallelisation(interpreter::IStoredProgram* program); std::thread _expThread; std::mutex _expMutex; diff --git a/DSFE_App/DSFE_Core/src/Interpreter/StoredProgram.cpp b/DSFE_App/DSFE_Core/src/Interpreter/StoredProgram.cpp index 5c3737b6..682e6942 100644 --- a/DSFE_App/DSFE_Core/src/Interpreter/StoredProgram.cpp +++ b/DSFE_App/DSFE_Core/src/Interpreter/StoredProgram.cpp @@ -152,7 +152,7 @@ namespace interpreter { robots::RobotSystem* robot = _core->robotSystem(); if (!robot) { LOG_ERROR("No robot system found in simulation manager."); return; } if (method == IntegratorMethod::AD_ImplicitEuler || method == IntegratorMethod::AD_ImplicitMidpoint || method == IntegratorMethod::AD_GLRK2 || method == IntegratorMethod::AD_GLRK3) { - _core->setIntegrationMethod(static_cast(method)); + _core->setADIntegrationMethod(static_cast(method)); } else { _core->setIntegrationMethod(static_cast(method)); diff --git a/DSFE_App/DSFE_Core/src/Scene/SimulationCore.cpp b/DSFE_App/DSFE_Core/src/Scene/SimulationCore.cpp index 226af5ed..f0d18b8f 100644 --- a/DSFE_App/DSFE_Core/src/Scene/SimulationCore.cpp +++ b/DSFE_App/DSFE_Core/src/Scene/SimulationCore.cpp @@ -48,22 +48,22 @@ namespace core { // Set the integration method for the simulation (also updates the robot's integrator if it exists) void SimulationCore::setIntegrationMethod(integration::eIntegrationMethod method) { if (!_robot) { return; } - _robot->getIntegrator()->setIntegrationMethod(method); + _robot->setStandardIntegrator(method); } // Set the auto-diff integration method for the simulation (also updates the robot's AD integrator if it exists) - void SimulationCore::setIntegrationMethod(integration::eAutoDiffIntegrationMethod method) { + void SimulationCore::setADIntegrationMethod(integration::eAutoDiffIntegrationMethod method) { if (!_robot) { return; } - _robot->getADIntegrator()->setIntegrationMethod(method); + _robot->setADIntegrator(method); } // Get the name of the current integration method (returns "no_robot" if no robot is loaded) std::string SimulationCore::integrationMethodName() const { if (!_robot) { return "no_robot"; } std::string intName; if (_robot->autoDiffEnabled()) { - intName = _robot->getADIntegrator()->IntegratorName(_robot->getADIntegrator()->integrationMethod()); + intName = _robot->getIntegratorName(); } else { - intName = _robot->getIntegrator()->IntegratorName(_robot->getIntegrator()->getIntegrationMethod()); + intName = _robot->AD_integratorName(); } LOG_INFO("Integration Method: %s", intName.c_str()); @@ -72,12 +72,12 @@ namespace core { // Get the current integration method integration::eIntegrationMethod SimulationCore::integrationMethod() const { if (!_robot) { return integration::eIntegrationMethod::RK4; } - return _robot->getIntegrator()->getIntegrationMethod(); + return _robot->getIntegrationMethod(); } // Get the current auto-diff integration method integration::eAutoDiffIntegrationMethod SimulationCore::autoDiffIntegrationMethod() const { if (!_robot) { return integration::eAutoDiffIntegrationMethod::AD_ImplicitEuler; } - return _robot->getADIntegrator()->integrationMethod(); + return _robot->AD_IntegrationMethod(); } void SimulationCore::enableAutoDiff(bool enable) { if (!_robot) { return; } @@ -300,62 +300,64 @@ namespace core { _trajRefBuffer.clear(); // Inject reference buffer only _robot->setRefBuffer(&_trajRefBuffer); + // Set integrator on both physics and robot systems + _robot->setStandardIntegrator(method); + + scriptParallelisation(program); + + D_SUCCESS("Synchronous run completed: %s (%.1fs, %zu samples)", methodName.c_str(), _simTime.load(), _telemetry.ring.size()); + LOG_INFO("SimulationCore::runScriptToCompletion -> END method=%s result=%d simTime=%.6f samples=%zu", methodName.c_str(), (int)(_telemetry.ring.size() >= 2), _simTime.load(), _telemetry.ring.size()); + return (_telemetry.ring.size() >= 2); + } + void SimulationCore::scriptParallelisation(interpreter::IStoredProgram* program) { // Reset simulation state _simTime.store(0.0, std::memory_order_relaxed); _simRunning.store(false); _telemetryBegun = false; _accum = 0.0; - // Set integrator on both physics and robot systems - _robot->setIntegrationMethod(method); - _activeProgram = program; _scriptRunning.store(true); // Set run mode to synchronous for the duration of this run _runMode = eRunMode::Synchronous; + const double dt = _dt; + const int maxSteps = static_cast((24.0 * 3600.0) / dt); // safety to prevent infinite loops in faulty scripts (max 24 hours of sim time) + // enable sim stepping and telemetry for synchronous run startSimulation(); LOG_INFO("SimulationCore::runScriptToCompletion -> startSimulation called; simRunning=%d simTime=%.6f", (int)_simRunning, _simTime.load()); // Run tight simulation loop until program completes - const double dt = _dt; double simTime = _simTime.load(); - const int maxSteps = static_cast((24.0 * 3600.0) / dt); // safety to prevent infinite loops in faulty scripts (max 24 hours of sim time) // Main loop: step the program and simulation until completion for (int step = 0; step < maxSteps; ++step) { // Check program completion if (program->isCompleted() || program->isFaulted() || program->isStopped()) { - LOG_INFO("SimulationCore::runScriptToCompletion -> program end detected at step=%d completed=%d faulted=%d stopped=%d", step, (int)program->isCompleted(), (int)program->isFaulted(), (int)program->isStopped()); - break; + LOG_INFO("SimulationCore::runScriptToCompletion -> program end detected at step=%d completed=%d faulted=%d stopped=%d", step, (int)program->isCompleted(), (int)program->isFaulted(), (int)program->isStopped()); + break; } // Step the program (DSL command execution) program->step(dt); - const auto state = _robot->runtimeIntegratorState(); - // Step physics and robot if sim is running if (_simRunning.load()) { simTime += dt; - if (hasRobot()) { // Update Trajectory Inputs _robot->updateTrajectoryInputs(*_traj, simTime); - _robot->step(dt, simTime); - // Telemetry beginRun if (!_telemetryBegun) { _telemetry.beginRun(simTime, _telHz, 300.0); _telemetryBegun = true; D_INFO_ONCE("Telemtry Capture Started (dt=%.6f s, simTime=%.3f s)", (1 / _telHz), simTime); } - // Telemetry update _telemetry.update(simTime, *_robot, _traj, diagnostics::eTelemetryLevel::FULL); } @@ -368,7 +370,7 @@ namespace core { else { _simTime.store(0.0); } - + // Clean up stopSimulation(); @@ -380,10 +382,6 @@ namespace core { _scriptRunning.store(false); _simRunning.store(false); _telemetryBegun = false; - - D_SUCCESS("Synchronous run completed: %s (%.1fs, %zu samples)", methodName.c_str(), _simTime.load(), _telemetry.ring.size()); - LOG_INFO("SimulationCore::runScriptToCompletion -> END method=%s result=%d simTime=%.6f samples=%zu", methodName.c_str(), (int)(_telemetry.ring.size() >= 2), _simTime.load(), _telemetry.ring.size()); - return (_telemetry.ring.size() >= 2); } // Setter for fixed timestep duration diff --git a/DSFE_App/DSFE_GUI/include/Scene/SimulationManager.h b/DSFE_App/DSFE_GUI/include/Scene/SimulationManager.h index 4318d1f4..923b47db 100644 --- a/DSFE_App/DSFE_GUI/include/Scene/SimulationManager.h +++ b/DSFE_App/DSFE_GUI/include/Scene/SimulationManager.h @@ -246,7 +246,7 @@ namespace gui { // Setters for Integration state / method void setIntegrationMethod(integration::eIntegrationMethod method); const integration::eIntegrationMethod integrationMethod() const; - void setIntegrationMethod(integration::eAutoDiffIntegrationMethod method); + void setADIntegrationMethod(integration::eAutoDiffIntegrationMethod method); const integration::eAutoDiffIntegrationMethod autoDiffIntegrationMethod() const; // Access to the underlying StudyRunner for running batch studies from the GUI diff --git a/DSFE_App/DSFE_GUI/src/Scene/SimulationManager.cpp b/DSFE_App/DSFE_GUI/src/Scene/SimulationManager.cpp index 14b96d15..b696bf40 100644 --- a/DSFE_App/DSFE_GUI/src/Scene/SimulationManager.cpp +++ b/DSFE_App/DSFE_GUI/src/Scene/SimulationManager.cpp @@ -1399,8 +1399,8 @@ namespace gui { const integration::eIntegrationMethod SimManager::integrationMethod() const { return _core->integrationMethod(); } - void SimManager::setIntegrationMethod(integration::eAutoDiffIntegrationMethod method) { - _core->setIntegrationMethod(method); + void SimManager::setADIntegrationMethod(integration::eAutoDiffIntegrationMethod method) { + _core->setADIntegrationMethod(method); } const integration::eAutoDiffIntegrationMethod SimManager::autoDiffIntegrationMethod() const { return _core->autoDiffIntegrationMethod(); diff --git a/DSFE_App/DSFE_GUI/src/ui/ControlPanel.cpp b/DSFE_App/DSFE_GUI/src/ui/ControlPanel.cpp index 3682ee82..328949ef 100644 --- a/DSFE_App/DSFE_GUI/src/ui/ControlPanel.cpp +++ b/DSFE_App/DSFE_GUI/src/ui/ControlPanel.cpp @@ -455,7 +455,7 @@ namespace gui { auto updatedMethod_AD = static_cast(n); auto state = robot->runtimeIntegratorState(); state->autoDiff = true; // ensure autodiff flag is set in state - robot->getADIntegrator()->setIntegrationMethod(updatedMethod_AD); + _sim->setADIntegrationMethod(updatedMethod_AD); switch (updatedMethod_AD) { case integration::eAutoDiffIntegrationMethod::AD_ImplicitEuler: From 08816b5f44a3c350924092250472df42afe2e989 Mon Sep 17 00:00:00 2001 From: saltyjoss Date: Sun, 31 May 2026 12:39:21 +0100 Subject: [PATCH 089/210] feat: Added `fmod` operation into the standard mathematical functions header and into the DualNumber header, updating relevant methods with new fmod --- PxMLib/MathLib/include/core/DualNumbers.h | 9 +++ PxMLib/MathLib/include/core/ScalarStdFunc.h | 2 + PxMLib/MathLib/include/core/Utils.h | 65 ++++++++++----------- 3 files changed, 42 insertions(+), 34 deletions(-) diff --git a/PxMLib/MathLib/include/core/DualNumbers.h b/PxMLib/MathLib/include/core/DualNumbers.h index 0422ed4a..c0f968ea 100644 --- a/PxMLib/MathLib/include/core/DualNumbers.h +++ b/PxMLib/MathLib/include/core/DualNumbers.h @@ -491,6 +491,15 @@ namespace mathlib { } return out; } + // Modulo function for DualNumber_T (returns the modulo of the real parts, dual parts are set to zero since the derivative of fmod is not well-defined at integer boundaries) + template + inline mathlib::DualNumber_T fmod(const mathlib::DualNumber_T& a, const mathlib::DualNumber_T& b) { + mathlib::DualNumber_T out; + out.real = std::fmod(a.real, b.real); + // The dual part of fmod is more complex and depends on the specific implementation of fmod. For simplicity, we can set it to zero or compute it based on the derivatives if needed. + for (size_t i = 0; i < NVar; ++i) { out.dual[i] = Scalar(0); } + return out; + } // Since dual numbers are not complex, the real part is just the real part of the dual number (for non-dual types, this is just the value itself) template inline T real(const T& x) { return x; } diff --git a/PxMLib/MathLib/include/core/ScalarStdFunc.h b/PxMLib/MathLib/include/core/ScalarStdFunc.h index cb035222..ca2d2d47 100644 --- a/PxMLib/MathLib/include/core/ScalarStdFunc.h +++ b/PxMLib/MathLib/include/core/ScalarStdFunc.h @@ -62,4 +62,6 @@ namespace mathlib { // Squared normalisation function (Scalar) template, int> = 0> inline Scalar norm2(const Scalar& a) { return (a == Scalar(0)) ? Scalar(0) : Scalar(1); } + template, int> = 0> + inline Scalar fmod(const Scalar& a, const Scalar& b) { return std::fmod(a, b); } } // namespace mathlib \ No newline at end of file diff --git a/PxMLib/MathLib/include/core/Utils.h b/PxMLib/MathLib/include/core/Utils.h index 78b95564..b491397a 100644 --- a/PxMLib/MathLib/include/core/Utils.h +++ b/PxMLib/MathLib/include/core/Utils.h @@ -10,21 +10,21 @@ using namespace mathlib; using namespace constants; namespace mathlib { - // Convert degrees to radians (Scalar) - template - inline Scalar radians(Scalar degrees) { return degrees * (Scalar(PI) / Scalar(180)); } + // Convert degrees to radians (T) + template + inline T radians(T degrees) { return degrees * (T(PI) / T(180)); } // Convert degrees to radians (double overload) inline double radians(double degrees) { return radians(degrees); } // Convert radians to degrees - template - inline Scalar degrees(Scalar radians) { return radians * (Scalar(180) / Scalar(PI)); } + template + inline T degrees(T radians) { return radians * (T(180) / T(PI)); } // Convert radians to degrees (double overload) inline double degrees(double radians) { return degrees(radians); } // Clamp a value between min and max - template - inline Scalar clamp(Scalar value, Scalar minVal, Scalar maxVal) { + template + inline T clamp(T value, T minVal, T maxVal) { if (value < minVal) { return minVal; } if (value > maxVal) { return maxVal; } return value; @@ -33,46 +33,43 @@ namespace mathlib { inline double clamp(double value, double minVal, double maxVal) { return clamp(value, minVal, maxVal); } // Method to wrap an angle in radians to the range [-pi, pi] - template - inline Scalar wrapToPi(Scalar angleRad) { - angleRad = std::fmod(angleRad + Scalar(PI), Scalar(TWO_PI)); - if (angleRad < Scalar(0)) { angleRad += Scalar(TWO_PI); } - return angleRad - Scalar(PI); // [rad] + template + inline T wrapToPi(T angleRad) { + angleRad = mathlib::fmod(angleRad + T(PI), T(TWO_PI)); + if (angleRad < T(0)) { angleRad += T(TWO_PI); } + return angleRad - T(PI); // [rad] } // Method to wrap an angle in radians to the range [-pi, pi] (double overload) inline double wrapToPi(double angleRad) { return wrapToPi(angleRad); } // Method to wrap an angle in radians to the range [0, 2pi] - template - inline Scalar wrapRad(Scalar angleRad) { - angleRad = fmod(angleRad, Scalar(TWO_PI)); - if (angleRad < Scalar(0)) { angleRad += Scalar(TWO_PI); } + template + inline T wrapRad(T angleRad) { + angleRad = mathlib::fmod(angleRad, T(TWO_PI)); + if (angleRad < T(0)) { angleRad += T(TWO_PI); } return angleRad; // [rad] } // Method to wrap an angle in radians to the range [0, 2pi] (double overload) inline double wrapRad(double angleRad) { return wrapRad(angleRad); } // Linear interpolation between a and b by factor t (0 <= t <= 1) - template inline double lerp(double a, double b, double t) { return a + t * (b - a); } // Check if two doubles are approximately equal within a tolerance - template inline bool approximatelyEqual(double a, double b, double tolerance = 1e-9) { return std::fabs(a - b) <= tolerance; } // Check if a value is within a specified range [minVal, maxVal] - template inline bool isInRange(double value, double minVal, double maxVal) { return (value >= minVal) && (value <= maxVal); } // Dot product of two 3D vectors - template - inline Scalar dot(const Vec3_T& v1, const Vec3_T& v2) { return v1.x() * v2.x() + v1.y() * v2.y() + v1.z() * v2.z(); } + template + inline T dot(const Vec3_T& v1, const Vec3_T& v2) { return v1.x() * v2.x() + v1.y() * v2.y() + v1.z() * v2.z(); } // Dot product of two 3D vectors (double overload) inline double dot(const Vec3& v1, const Vec3& v2) { return dot(v1, v2); } // Cross product of two 3D vectors - template - inline Vec3_T cross(const Vec3_T& v1, const Vec3_T& v2) { - return Vec3_T( + template + inline Vec3_T cross(const Vec3_T& v1, const Vec3_T& v2) { + return Vec3_T( v1.y() * v2.z() - v1.z() * v2.y(), v1.z() * v2.x() - v1.x() * v2.z(), v1.x() * v2.y() - v1.y() * v2.x() @@ -82,22 +79,22 @@ namespace mathlib { inline Vec3 cross(const Vec3& v1, const Vec3& v2) { return cross(v1, v2); } // Natural frequency of a mass-spring system - template - inline Scalar natural_freq(Scalar k_p, Scalar I) { - if (!mathlib::isfinite(k_p) || !mathlib::isfinite(I)) { return std::numeric_limits::quiet_NaN(); } - if (k_p < Scalar(0) || I <= Scalar(0)) { return std::numeric_limits::quiet_NaN(); } + template + inline T natural_freq(T k_p, T I) { + if (!mathlib::isfinite(k_p) || !mathlib::isfinite(I)) { return std::numeric_limits::quiet_NaN(); } + if (k_p < T(0) || I <= T(0)) { return std::numeric_limits::quiet_NaN(); } return mathlib::sqrt(k_p / I); } // Natural frequency of a mass-spring system (double overload) inline double natural_freq(double k_p, double I) { return natural_freq(k_p, I); } // Damping ratio of a mass-spring-damper system - template - inline Scalar damping_ratio(Scalar k_d, Scalar I, Scalar k_p) { - if (!mathlib::isfinite(k_p) || !mathlib::isfinite(k_d) || !mathlib::isfinite(I)) { return std::numeric_limits::quiet_NaN(); } - if (k_p < Scalar(0) || I <= Scalar(0)) { return std::numeric_limits::quiet_NaN(); } - Scalar w_n = natural_freq(k_p, I); - return k_d / (Scalar(2) * I * w_n); // damping ratio - zeta + template + inline T damping_ratio(T k_d, T I, T k_p) { + if (!mathlib::isfinite(k_p) || !mathlib::isfinite(k_d) || !mathlib::isfinite(I)) { return std::numeric_limits::quiet_NaN(); } + if (k_p < T(0) || I <= T(0)) { return std::numeric_limits::quiet_NaN(); } + T w_n = natural_freq(k_p, I); + return k_d / (T(2) * I * w_n); // damping ratio - zeta } // Damping ratio of a mass-spring-damper system (double overload) inline double damping_ratio(double k_d, double I, double k_p) { return damping_ratio(k_d, I, k_p); } From 113631baaf952c698945183297ea1b50b4bab518 Mon Sep 17 00:00:00 2001 From: saltyjoss Date: Sun, 31 May 2026 14:03:00 +0100 Subject: [PATCH 090/210] fixes: Corrected seed array identity misconfig and allocation hammer (leading to O(N^2)) in `automaticDifferentitationJacobian` --- .../integrators/auto_diff_integrators.inl | 31 +++++++++---------- 1 file changed, 14 insertions(+), 17 deletions(-) diff --git a/PxMLib/MathLib/include/integrators/auto_diff_integrators.inl b/PxMLib/MathLib/include/integrators/auto_diff_integrators.inl index 8853d4b9..a07e57db 100644 --- a/PxMLib/MathLib/include/integrators/auto_diff_integrators.inl +++ b/PxMLib/MathLib/include/integrators/auto_diff_integrators.inl @@ -404,26 +404,23 @@ namespace integration { const mathlib::VecX_T>& x ) { const int n = static_cast(x.size()); - - using SysScalar = mathlib::DualNumber_T; - auto f0 = f(t, x); - const int m = static_cast(f0.size()); - + using Dual_T = mathlib::DualNumber_T; + // Create dual numbers for each input variable in x + mathlib::VecX_T x_dual(n); + for (int k = 0; k < n; ++k) { + std::array seed_array{}; + if (k < static_cast(NVar)) { seed_array[k] = RealScalar(1); } // Set the k-th variable's seed to 1 for forward mode AD, and the rest to 0 + x_dual(k) = Dual_T(mathlib::real(x(k)), seed_array); // Initialise the dual number for x(k) with the real part from x(k) and the appropriate seed for the dual part + } + Dual_T t_dual(mathlib::real(t)); + auto f_dual = f(t_dual, x_dual); + const int m = static_cast(f_dual.size()); + // Construct the Jacobian matrix J where J(j, i) = df_j/dx_i is the dual part of the j-th output corresponding to the seed for the i-th input variable mathlib::MatX_T J(m, n); for (int i = 0; i < n; ++i) { - using Dual_T = mathlib::DualNumber_T; - - mathlib::VecX_T x_dual(n); - for (int k = 0; k < n; ++k) { - std::array seed_array{}; - if (k == i) { seed_array.fill(RealScalar(0)); seed_array[i] = RealScalar(1); } // Set the seed for the i-th variable to 1, others to 0 - x_dual(k) = Dual_T(mathlib::real(x(k)), seed_array); - } - - Dual_T t_dual(mathlib::real(t)); - auto f_dual = f(t_dual, x_dual); for (int j = 0; j < m; ++j) { - J(j, i) = f_dual(j).dual[i]; // The Jacobian entry J(j, i) is the dual part of f_dual(j) corresponding to the seed for x(i) + if (i < static_cast(NVar)) { J(j, i) = f_dual(j).dual[i]; } // The Jacobian entry J(j, i) is the dual part of the j-th output corresponding to the seed for the i-th input variable + else { J(j, i) = RealScalar(0); } // If i >= NVar, then the seed for that variable is zero, so the Jacobian entry is zero } } return J; From 026a590c0a35144337aa144431f8e83260782116 Mon Sep 17 00:00:00 2001 From: saltyjoss Date: Sun, 31 May 2026 14:54:47 +0100 Subject: [PATCH 091/210] feat: Updated VISPA json, and added new script. --- .../vispa_target_sat_intercept_servicing.dsl | 112 ++++++++++++++++++ .../Robotic_Arm_Models/VISPA/VISPA.json | 24 ++-- 2 files changed, 124 insertions(+), 12 deletions(-) create mode 100644 DSFE_App/DSFE_Engine/assets/DSLScripts/Test_Suite/vispa_target_sat_intercept_servicing.dsl diff --git a/DSFE_App/DSFE_Engine/assets/DSLScripts/Test_Suite/vispa_target_sat_intercept_servicing.dsl b/DSFE_App/DSFE_Engine/assets/DSLScripts/Test_Suite/vispa_target_sat_intercept_servicing.dsl new file mode 100644 index 00000000..1bf27b07 --- /dev/null +++ b/DSFE_App/DSFE_Engine/assets/DSLScripts/Test_Suite/vispa_target_sat_intercept_servicing.dsl @@ -0,0 +1,112 @@ +# -------------------------------------------- +# MISSION: Airbus VISPA => Satellite Interception & Abort (Aggressive reversals) +# -------------------------------------------- +# +# MISSION PROFILE +# --------------- +# 1. System Initialization & Calibration Check (0.0s - 5.0s) +# 2. Nominal Deployment to Ready-State Observer Position (5.0s - 35.0s) +# 3. Proximity Close-Approach Synchronisation (35.0s - 95.0s) +# 4. Multi-Point Surface Inspection (95.0s - 215.0s) +# 5. Synchronised Contact & Stabilisation Hold (215.0s - 275.0s) +# 6. Secure Safe-State Stow & Mission Completion (275.0s - 335.0s) +# +# JOINT LIMITS +# ------------ +# j1-j6: +/- ~180 deg (+/-3.14149 rad) +# v_max: 5.38 deg/s (0.0940 rad/s) hardware limit +# v_operating: < 1.5 deg/s +# Q_max: 50 Nm per joint +# Damping / Friction: 0.2 / 0.05 (all joints) +# +# LINK MASSES (kg) +# ---------------- +# link00 0.627 base adapter (gold coloured) +# link01 2.328 shoulder yaw +# link02 3.995 upper arm (0.8m, heaviest link) +# link03 2.328 elbow +# link04 3.157 forearm (0.65m) +# link05 2.695 wrist roll +# link06 0.924 end-effector flange +# +# Create By: Joss Salton +# GitHub: SaltyJoss +# +# -------------------------------------------- + +load(robot, VISPA) + +wait(4.0) +trajClear() +wait(1.0) + +# Begins sim run and logging. +start() + +# Nominal Deployment +parallel(30.0) { + trajSet(link01, TRAP, -30.0, 1.5, 5.0) + trajSet(link02, TRAP, 45.0, 2.0, 5.0) + trajSet(link03, TRAP, -90.0, 2.0, 5.0) + trajSet(link04, TRAP, 0.0, 1.0, 3.0) + trajSet(link05, TRAP, 45.0, 1.5, 4.0) + trajSet(link06, TRAP, 0.0, 1.0, 3.0) +} + +wait(30.0) + +# Close-approach synchronisation +parallel(60.0) { + trajSet(link01, TRAP, -45.0, 0.5, 10.0) + trajSet(link02, TRAP, 55.0, 0.4, 10.0) + trajSet(link03, TRAP, -80.0, 0.4, 10.0) + trajSet(link04, TRAP, 10.0, 0.3, 8.0) + trajSet(link05, TRAP, 30.0, 0.5, 10.0) + trajSet(link06, TRAP, 15.0, 0.5, 8.0) +} + +wait(60.0) + +# Multipoint surface inspection +parallel(120.0) { + trajSet(link01, MSINE, 120.0, -45.0, 4.0, 0.002, 0.0, 2.0, 0.004, 90.0) + trajSet(link02, MSINE, 120.0, 55.0, 3.0, 0.001, 0.0, 1.5, 0.003, 45.0) + trajSet(link03, MSINE, 120.0, -80.0, 3.0, 0.002, 0.0, 1.0, 0.004, 120.0) + trajSet(link04, MSINE, 120.0, 10.0, 2.5, 0.003, 0.0, 1.0, 0.005, 60.0) + trajSet(link05, MSINE, 120.0, 30.0, 2.0, 0.004, 0.0, 1.0, 0.006, 90.0) + trajSet(link06, MSINE, 120.0, 15.0, 2.0, 0.005, 0.0, 1.0, 0.007, 45.0) +} + +wait(120.0) + +# Contact, docking, and servicing lock +parallel(60.0) { + trajSet(link01, TRAP, -20.0, 0.8, 8.0) + trajSet(link02, TRAP, 30.0, 0.8, 8.0) + trajSet(link03, TRAP, -100.0, 0.8, 8.0) + trajSet(link04, TRAP, 0.0, 0.5, 8.0) + trajSet(link05, TRAP, 60.0, 0.8, 8.0) + trajSet(link06, TRAP, 0.0, 0.5, 8.0) +} + +wait(60.0) + +# Safe-state return +parallel(60.0) { + trajSet(link01, TRAP, 0.0, 1.0, 10.0) + trajSet(link02, TRAP, 0.0, 1.0, 10.0) + trajSet(link03, TRAP, 0.0, 1.0, 10.0) + trajSet(link04, TRAP, 0.0, 1.0, 10.0) + trajSet(link05, TRAP, 0.0, 1.0, 10.0) + trajSet(link06, TRAP, 0.0, 1.0, 10.0) +} + +wait(60.0) + +trajClear() +wait(1.0) + +stop() +# -------------------------------------------- +# END -> ~60 seconds +# -------------------------------------------- \ No newline at end of file diff --git a/DSFE_App/DSFE_Engine/assets/objects/Robotic_Arm_Models/VISPA/VISPA.json b/DSFE_App/DSFE_Engine/assets/objects/Robotic_Arm_Models/VISPA/VISPA.json index f0d18fea..41c4b28a 100644 --- a/DSFE_App/DSFE_Engine/assets/objects/Robotic_Arm_Models/VISPA/VISPA.json +++ b/DSFE_App/DSFE_Engine/assets/objects/Robotic_Arm_Models/VISPA/VISPA.json @@ -265,8 +265,8 @@ "effort": 50 }, "dynamics": { - "damping": 0.0, - "friction": 0.0 + "damping": 0.2, + "friction": 0.05 } }, @@ -287,8 +287,8 @@ "effort": 50 }, "dynamics": { - "damping": 0.0, - "friction": 0.0 + "damping": 0.2, + "friction": 0.05 } }, @@ -309,8 +309,8 @@ "effort": 50 }, "dynamics": { - "damping": 0.0, - "friction": 0.0 + "damping": 0.2, + "friction": 0.05 } }, @@ -331,8 +331,8 @@ "effort": 50 }, "dynamics": { - "damping": 0.0, - "friction": 0.0 + "damping": 0.2, + "friction": 0.05 } }, @@ -353,8 +353,8 @@ "effort": 50 }, "dynamics": { - "damping": 0.0, - "friction": 0.0 + "damping": 0.2, + "friction": 0.05 } }, @@ -375,8 +375,8 @@ "effort": 50 }, "dynamics": { - "damping": 0.0, - "friction": 0.0 + "damping": 0.2, + "friction": 0.05 } } ] From 042bc7230461e2d65e89593b7ce84fe133d88f54 Mon Sep 17 00:00:00 2001 From: saltyjoss Date: Sun, 31 May 2026 14:55:19 +0100 Subject: [PATCH 092/210] feat: Updated dynamics friction model back to simple viscous damping with coulomb friction --- DSFE_App/DSFE_Core/include/Robots/RobotDynamics.inl | 9 +++++++-- 1 file changed, 7 insertions(+), 2 deletions(-) diff --git a/DSFE_App/DSFE_Core/include/Robots/RobotDynamics.inl b/DSFE_App/DSFE_Core/include/Robots/RobotDynamics.inl index 424f289b..66836a71 100644 --- a/DSFE_App/DSFE_Core/include/Robots/RobotDynamics.inl +++ b/DSFE_App/DSFE_Core/include/Robots/RobotDynamics.inl @@ -451,12 +451,17 @@ namespace robots { const Scalar b = static_cast(snap.model->joints[i].dynamics.damping); // viscous damping coefficient const Scalar c = static_cast(snap.model->joints[i].dynamics.friction); // Coulomb friction coefficient - const Scalar eps_f = static_cast(1e-2); + const Scalar eps_f = static_cast(1e-3); Scalar tau_i = k_p * err + k_d * err_d + I_eff * snap.qdd_ref[i]; - Scalar tau_f = dynamics::computeKarnoppFriction(qd[i], tau_i, c, b); // add friction compensation + Scalar tau_f = c * mathlib::tanh(qd[i] / Scalar(0.1)) + b * qd[i]; // simple friction model with viscous and Coulomb friction tau_i += tau_f; + const Scalar Q_max = static_cast(snap.model->joints[i].limits.maxEffort); + LOG_INFO_ONCE("Max effort for joint %zu: %g Nm", i, mathlib::real(Q_max)); + + tau_i = Q_max * mathlib::tanh(tau_i / Q_max); // saturate control torque to max effort using smooth tanh saturation + scratch.dense.tau[i] = tau_i; out.metrics.q[i] = mathlib::real(q[i]); From 0d4954f5dfc61b2f6a88adba50f7bff5238cf9c3 Mon Sep 17 00:00:00 2001 From: saltyjoss Date: Sun, 31 May 2026 14:56:22 +0100 Subject: [PATCH 093/210] chore: Small tweaks for QoL --- DSFE_App/DSFE_Core/src/Robots/RobotLoader.cpp | 22 +++---- DSFE_App/DSFE_Core/src/Robots/RobotSystem.cpp | 5 -- DSFE_App/DSFE_GUI/src/ui/ControlPanel.cpp | 66 +++++++++---------- 3 files changed, 42 insertions(+), 51 deletions(-) diff --git a/DSFE_App/DSFE_Core/src/Robots/RobotLoader.cpp b/DSFE_App/DSFE_Core/src/Robots/RobotLoader.cpp index 975182c4..412b6afe 100644 --- a/DSFE_App/DSFE_Core/src/Robots/RobotLoader.cpp +++ b/DSFE_App/DSFE_Core/src/Robots/RobotLoader.cpp @@ -315,7 +315,7 @@ namespace robots { joint.dynamics.damping = 0.0; joint.dynamics.friction = 0.0; - if (!jointData.contains("dynamics") || !jointData["dynamics"].is_object()) { return; } + if (!jointData.contains("dynamics") || !jointData["dynamics"].is_object()) { LOG_ERROR("Could not find joint dynamic data"); return; } const auto& D = jointData["dynamics"]; joint.dynamics.damping = D.value("damping", joint.dynamics.damping); @@ -450,8 +450,8 @@ namespace robots { joint.type = eJointType::FIXED; joint.axis = Vec3::Zero(); joint.limits.continuous = false; - joint.limits.minAngle = 0.0f; - joint.limits.maxAngle = 0.0f; + joint.limits.minAngle = 0.0; + joint.limits.maxAngle = 0.0; } else { parseJointAxis(jointData, joint); @@ -475,16 +475,16 @@ namespace robots { } if (abs(joint.limits.minAngle) == abs(joint.limits.maxAngle) && !joint.limits.continuous) { - LOG_INFO("Joint: %s | Parent: %s, | Child: %s, | Max Speed: %.2f, | Angle Limit: +-%.2f", - joint.name.c_str(), joint.parent.c_str(), joint.child.c_str(), joint.limits.maxqd, joint.limits.maxAngle); - D_INFO("Joint: %s | Parent: %s, | Child: %s, | Max Speed: %.2f, | Angle Limit: +-%.2f", - joint.name.c_str(), joint.parent.c_str(), joint.child.c_str(), joint.limits.maxqd, joint.limits.maxAngle); + LOG_INFO("Joint: %s | Parent: %s, | Child: %s, | Max Speed: %.2f, | Angle Limit: +-%.2f | Dampling: %.2f, | Friction: %.2f", + joint.name.c_str(), joint.parent.c_str(), joint.child.c_str(), joint.limits.maxqd, joint.limits.maxAngle, joint.dynamics.damping, joint.dynamics.friction); + D_INFO("Joint: %s | Parent: %s, | Child: %s, | Max Speed: %.2f, | Angle Limit: +-%.2f | Dampling: %.2f, | Friction: %.2f", + joint.name.c_str(), joint.parent.c_str(), joint.child.c_str(), joint.limits.maxqd, joint.limits.maxAngle, joint.dynamics.damping, joint.dynamics.friction); } else { - LOG_INFO("Joint: %s | Parent: %s, | Child: %s, | Continuous: %s, | Max Speed: %.2f, | Min Angle: %.2f, | Max Angle: %.2f", - joint.name.c_str(), joint.parent.c_str(), joint.child.c_str(), joint.limits.continuous ? "True" : "False", joint.limits.maxqd, joint.limits.minAngle, joint.limits.maxAngle); - D_INFO("Joint: %s | Parent: %s, | Child: %s, | Continuous: %s, | Max Speed: %.2f, | Min Angle: %.2f, | Max Angle: %.2f", - joint.name.c_str(), joint.parent.c_str(), joint.child.c_str(), joint.limits.continuous ? "True" : "False", joint.limits.maxqd, joint.limits.minAngle, joint.limits.maxAngle); + LOG_INFO("Joint: %s | Parent: %s, | Child: %s, | Continuous: %s, | Max Speed: %.2f, | Min Angle: %.2f, | Max Angle: %.2f | Dampling: %.2f, | Friction: %.2f", + joint.name.c_str(), joint.parent.c_str(), joint.child.c_str(), joint.limits.continuous ? "True" : "False", joint.limits.maxqd, joint.limits.minAngle, joint.limits.maxAngle, joint.dynamics.damping, joint.dynamics.friction); + D_INFO("Joint: %s | Parent: %s, | Child: %s, | Continuous: %s, | Max Speed: %.2f, | Min Angle: %.2f, | Max Angle: %.2f | Dampling: % .2f, | Friction : % .2f", + joint.name.c_str(), joint.parent.c_str(), joint.child.c_str(), joint.limits.continuous ? "True" : "False", joint.limits.maxqd, joint.limits.minAngle, joint.limits.maxAngle, joint.dynamics.damping, joint.dynamics.friction);; } } diff --git a/DSFE_App/DSFE_Core/src/Robots/RobotSystem.cpp b/DSFE_App/DSFE_Core/src/Robots/RobotSystem.cpp index 5e04f22e..56aff41d 100644 --- a/DSFE_App/DSFE_Core/src/Robots/RobotSystem.cpp +++ b/DSFE_App/DSFE_Core/src/Robots/RobotSystem.cpp @@ -492,11 +492,6 @@ namespace robots { _dynScratch.g.setConstant(_gravity); - LOG_INFO("dynScratch=%p", &_dynScratch); - LOG_INFO("dynScratchAD=%p", &_dynScratch_AD); - LOG_INFO("M rows=%d cols=%d", (int)_dynScratch.dense.M.rows(), (int)_dynScratch.dense.M.cols()); - LOG_INFO("Xup size=%d", (int)_dynScratch.spatial.Xup.size()); - // Reset adaptive integrator so it doesn't carry a stale step size _integrator->resetAdaptiveState(); diff --git a/DSFE_App/DSFE_GUI/src/ui/ControlPanel.cpp b/DSFE_App/DSFE_GUI/src/ui/ControlPanel.cpp index 328949ef..1c9b6e25 100644 --- a/DSFE_App/DSFE_GUI/src/ui/ControlPanel.cpp +++ b/DSFE_App/DSFE_GUI/src/ui/ControlPanel.cpp @@ -518,33 +518,33 @@ namespace gui { } } - // Torque mode combo box - ImGui::SectionHeader("Torque Mode"); - ImGui::SetNextItemWidth(150.0f); - if (ImGui::BeginCombo("##tau_mode", currentTorqueMode)) { - for (int n = 0; n < IM_ARRAYSIZE(torqueModeNames); ++n) { - bool isSelected = (n == static_cast(currentTauEnum)); - - // When a new mode is selected, update the robot's torque mode and log the change - if (ImGui::Selectable(torqueModeNames[n], isSelected)) { - auto updatedMode = static_cast(n); - robot->setTorqueMode(updatedMode); - - switch (updatedMode) { - case robots::eTorqueMode::NONE: - D_INFO("Torque mode set to None"); break; - case robots::eTorqueMode::PASSIVE: - D_INFO("Torque mode set to Passive"); break; - case robots::eTorqueMode::CONTROLLED: - D_INFO("Torque mode set to Controlled"); break; - default: - break; - } - } - if (isSelected) { ImGui::SetItemDefaultFocus(); } - } - ImGui::EndCombo(); - } + //// Torque mode combo box + //ImGui::SectionHeader("Torque Mode"); + //ImGui::SetNextItemWidth(150.0f); + //if (ImGui::BeginCombo("##tau_mode", currentTorqueMode)) { + // for (int n = 0; n < IM_ARRAYSIZE(torqueModeNames); ++n) { + // bool isSelected = (n == static_cast(currentTauEnum)); + + // // When a new mode is selected, update the robot's torque mode and log the change + // if (ImGui::Selectable(torqueModeNames[n], isSelected)) { + // auto updatedMode = static_cast(n); + // robot->setTorqueMode(updatedMode); + + // switch (updatedMode) { + // case robots::eTorqueMode::NONE: + // D_INFO("Torque mode set to None"); break; + // case robots::eTorqueMode::PASSIVE: + // D_INFO("Torque mode set to Passive"); break; + // case robots::eTorqueMode::CONTROLLED: + // D_INFO("Torque mode set to Controlled"); break; + // default: + // break; + // } + // } + // if (isSelected) { ImGui::SetItemDefaultFocus(); } + // } + // ImGui::EndCombo(); + //} ImGui::EndDisabled(); // Delta time controls @@ -718,15 +718,11 @@ namespace gui { ImGui::BeginDisabled(_sim->isSimRunning()); ImGui::Text("Joint Dynamics:"); - // Damping controls - ImGui::Text("Damping:"); - ImGui::SetNextItemWidth(150.0f); - if (ImGui::DragFloat("kg/s##damp", &c, 0.001f, minDamping, maxDamping)) { j.dynamics.damping = c; } - ImGui::Spacing(); - // Friction controls - ImGui::Text("Friction:"); + // Damping display (just displays current val from model) ImGui::SetNextItemWidth(150.0f); - if (ImGui::DragFloat("##fric", &f, 0.001f, minFriction, maxFriction)) { j.dynamics.friction = f; } + ImGui::TextDisabled("Damping: %.3f", c); + ImGui::SetNextItemWidth(150.0f); + ImGui::TextDisabled("Friction: %.3f", f); ImGui::Spacing(); // Gravity controls with preset menu From 806af4114cb3c96f7592f4352d61599a3e7d64f2 Mon Sep 17 00:00:00 2001 From: saltyjoss Date: Sun, 31 May 2026 14:57:19 +0100 Subject: [PATCH 094/210] fixes(temp): Added a relaxation variable (Real type) for relaxing the x_real var in `newton_raphson` after ten iterations --- PxMLib/MathLib/include/integrators/auto_diff_integrators.inl | 4 +++- 1 file changed, 3 insertions(+), 1 deletion(-) diff --git a/PxMLib/MathLib/include/integrators/auto_diff_integrators.inl b/PxMLib/MathLib/include/integrators/auto_diff_integrators.inl index a07e57db..eec56c11 100644 --- a/PxMLib/MathLib/include/integrators/auto_diff_integrators.inl +++ b/PxMLib/MathLib/include/integrators/auto_diff_integrators.inl @@ -364,7 +364,9 @@ namespace integration { throw std::runtime_error("Newton produced non-finite step at iter = " + std::to_string(iter)); } - x_real += delta; + Real relax = (iter > 10) ? Real(0.5) : Real(1.0); // Simple relaxation strategy after 10 iterations + + x_real += relax * delta; Real x_norm = x_real.norm(); if (delta.norm() < tol * (Real(1) + x_norm)) { converged = true; break; } } From 1f694c6f24e431c9c269f824b710ff419a0141a8 Mon Sep 17 00:00:00 2001 From: saltyjoss Date: Sun, 31 May 2026 15:19:13 +0100 Subject: [PATCH 095/210] feat: Added `wn_target` and `zeta_target` targets relative to each joint rather than constant across all joints. --- .../DSFE_Core/include/Robots/RobotModel.h | 4 ++-- DSFE_App/DSFE_Core/src/Robots/RobotLoader.cpp | 4 +++- .../Robotic_Arm_Models/VISPA/VISPA.json | 24 ++++++++++++++----- 3 files changed, 23 insertions(+), 9 deletions(-) diff --git a/DSFE_App/DSFE_Core/include/Robots/RobotModel.h b/DSFE_App/DSFE_Core/include/Robots/RobotModel.h index 0d56061e..a3b6c4b7 100644 --- a/DSFE_App/DSFE_Core/include/Robots/RobotModel.h +++ b/DSFE_App/DSFE_Core/include/Robots/RobotModel.h @@ -154,8 +154,8 @@ namespace robots { double qdd_ref = 0.0; // rad/s^2 // --- Control Parameters --- - double wn_target = 5.0; // rad/s - double zeta_target = 0.7; // damping ratio + double wn_target = 0.0; // rad/s + double zeta_target = 0.0; // damping ratio // --- Precomputed transforms --- mathlib::Mat4 jointToChildRest = mathlib::Mat4::Identity(); diff --git a/DSFE_App/DSFE_Core/src/Robots/RobotLoader.cpp b/DSFE_App/DSFE_Core/src/Robots/RobotLoader.cpp index 412b6afe..b0b0b171 100644 --- a/DSFE_App/DSFE_Core/src/Robots/RobotLoader.cpp +++ b/DSFE_App/DSFE_Core/src/Robots/RobotLoader.cpp @@ -320,6 +320,8 @@ namespace robots { const auto& D = jointData["dynamics"]; joint.dynamics.damping = D.value("damping", joint.dynamics.damping); joint.dynamics.friction = D.value("friction", joint.dynamics.friction); + joint.wn_target = D.value("wn_target", joint.wn_target); + joint.zeta_target = D.value("zeta_target", joint.zeta_target); if (joint.dynamics.damping < 0.0) { joint.dynamics.damping = 0.0; } if (joint.dynamics.friction < 0.0) { joint.dynamics.friction = 0.0; } @@ -341,7 +343,7 @@ namespace robots { out.a = dh.value("a", 0.0); out.alpha = dh.value("alpha", 0.0); out.d = dh.value("d", 0.0); - out.theta = dh.value("theta0", 0.0); // your key is theta0 + out.theta = dh.value("theta0", 0.0); out.type = parseDHType(dh.value("type", "revolute")); return true; } diff --git a/DSFE_App/DSFE_Engine/assets/objects/Robotic_Arm_Models/VISPA/VISPA.json b/DSFE_App/DSFE_Engine/assets/objects/Robotic_Arm_Models/VISPA/VISPA.json index 41c4b28a..2c337eb5 100644 --- a/DSFE_App/DSFE_Engine/assets/objects/Robotic_Arm_Models/VISPA/VISPA.json +++ b/DSFE_App/DSFE_Engine/assets/objects/Robotic_Arm_Models/VISPA/VISPA.json @@ -266,7 +266,9 @@ }, "dynamics": { "damping": 0.2, - "friction": 0.05 + "friction": 0.05, + "wn_target": 4.0, + "zeta_target": 0.8 } }, @@ -288,7 +290,9 @@ }, "dynamics": { "damping": 0.2, - "friction": 0.05 + "friction": 0.05, + "wn_target": 4.0, + "zeta_target": 0.8 } }, @@ -310,7 +314,9 @@ }, "dynamics": { "damping": 0.2, - "friction": 0.05 + "friction": 0.05, + "wn_target": 2.5, + "zeta_target": 1.0 } }, @@ -332,7 +338,9 @@ }, "dynamics": { "damping": 0.2, - "friction": 0.05 + "friction": 0.05, + "wn_target": 2.5, + "zeta_target": 1.0 } }, @@ -354,7 +362,9 @@ }, "dynamics": { "damping": 0.2, - "friction": 0.05 + "friction": 0.05, + "wn_target": 1.5, + "zeta_target": 1.3 } }, @@ -376,7 +386,9 @@ }, "dynamics": { "damping": 0.2, - "friction": 0.05 + "friction": 0.05, + "wn_target": 1.5, + "zeta_target": 1.3 } } ] From 29e07db0c8dfef7fa42b770ddd6f68f48fa8a706 Mon Sep 17 00:00:00 2001 From: saltyjoss Date: Sun, 31 May 2026 15:25:08 +0100 Subject: [PATCH 096/210] refactor: Updated `unpack_AD` to use a smooth velocity saturation method rather than if-else clamp --- DSFE_App/DSFE_Core/src/Robots/RobotSystem.cpp | 9 +++------ 1 file changed, 3 insertions(+), 6 deletions(-) diff --git a/DSFE_App/DSFE_Core/src/Robots/RobotSystem.cpp b/DSFE_App/DSFE_Core/src/Robots/RobotSystem.cpp index 56aff41d..8dbed328 100644 --- a/DSFE_App/DSFE_Core/src/Robots/RobotSystem.cpp +++ b/DSFE_App/DSFE_Core/src/Robots/RobotSystem.cpp @@ -248,15 +248,12 @@ namespace robots { Dual theta_out = clampJointAngle_T(j, theta_in); // max |omega| - Dual wMax_hw = mathlib::abs(j.limits.maxqd); + const double wMax_hw = mathlib::abs(j.limits.maxqd); Dual omega_out = omega_in; // Velocity limit clamping - if (wMax_hw > Dual(0)) { - const Dual eps = Dual(5e-2); - if (mathlib::abs(omega_in) > (Dual(1) + eps) * wMax_hw) { - omega_out = std::clamp(omega_in, -wMax_hw, wMax_hw); - } + if (wMax_hw > 0.0) { + omega_out = wMax_hw * mathlib::tanh(omega_in / wMax_hw); // smoothly clamp omega to wMax_hw using a tanh function } // Velocity limit enforcement From a8aaf8ec7172e90f35c0aa78d0eaec94d7479404 Mon Sep 17 00:00:00 2001 From: saltyjoss Date: Sun, 31 May 2026 15:44:45 +0100 Subject: [PATCH 097/210] feat: Added joint error real-time mapping in gui (starting plan to remove results window, just have it inhouse again, but safely). --- DSFE_App/DSFE_GUI/include/ui/ControlPanel.h | 1 + DSFE_App/DSFE_GUI/src/ui/ControlPanel.cpp | 52 ++++++++++++++++++++- 2 files changed, 52 insertions(+), 1 deletion(-) diff --git a/DSFE_App/DSFE_GUI/include/ui/ControlPanel.h b/DSFE_App/DSFE_GUI/include/ui/ControlPanel.h index 867f3df9..eec5ed36 100644 --- a/DSFE_App/DSFE_GUI/include/ui/ControlPanel.h +++ b/DSFE_App/DSFE_GUI/include/ui/ControlPanel.h @@ -141,6 +141,7 @@ namespace gui { void selectJointAndFollow(int jointIndex); //void drawTelemetryPlots(const diagnostics::TelemetryRecorder& rec); void drawTrajectoryInspector(const diagnostics::TelemetryRecorder& rec, int jointCount, int& selectedJoint); + void drawJointErrorPlot(); // Results Methods void drawResultsWindow(); diff --git a/DSFE_App/DSFE_GUI/src/ui/ControlPanel.cpp b/DSFE_App/DSFE_GUI/src/ui/ControlPanel.cpp index 1c9b6e25..c3122a33 100644 --- a/DSFE_App/DSFE_GUI/src/ui/ControlPanel.cpp +++ b/DSFE_App/DSFE_GUI/src/ui/ControlPanel.cpp @@ -751,6 +751,9 @@ namespace gui { drawTrajectoryInspector(rec, (int)robot->joints().size(), _selection.index); ImGui::TextDisabled("Selected Joint: %s - Child Link: %s", j.name.c_str(), L.name.c_str()); ImGui::Spacing(); + ImGui::Text("Joint (1-%d) Errors:", (int)robot->joints().size()); + drawJointErrorPlot(); + ImGui::Spacing(); // Reset joint properties to initial conditions ImGui::Spacing(); ImGui::Text("Reset Robot:"); @@ -1276,6 +1279,53 @@ namespace gui { } } + void ControlPanel::drawJointErrorPlot() { + // Get the latest telemetry record + const auto& rec = _sim->telemetry(); + const auto& ring = rec.ring; + + if (ring.size() < 2) { ImGui::TextUnformatted("No telemetry data yet."); return; } + + const auto& last = ring.at(ring.size() - 1); + + static std::vector rX; + static std::vector> rY; + + const size_t sampleCount = ring.size(); + const size_t jointCount = last.j.size(); + + rX.resize(sampleCount); + + if (rY.size() != jointCount) { rY.resize(jointCount); } + for (size_t j = 0; j < jointCount; ++j) { rY[j].resize(sampleCount); } + + for (int k = 0; k < sampleCount; ++k) { + const auto& s = ring.at(k); + rX[k] = (float)s.timeSec; + + const size_t m = std::min(jointCount, s.j.size()); + for (size_t j = 0; j < m; ++j) { + rY[j][k] = (float)(s.j[j].q_ref - s.j[j].q); + } + for (size_t j = m; j < jointCount; ++j) { + rY[j][k] = 0.0f; + } + } + + // --- Joint Error Overlay --- + const ImVec2 plotSz(ImGui::GetContentRegionAvail().x, 250.0f); + if (ImPlot::BeginPlot("Joint Error Overlay##results", plotSz)) { + ImPlot::SetupAxes("t (s)", "e (rad)", ImPlotAxisFlags_AutoFit, ImPlotAxisFlags_AutoFit); + ImPlot::SetupLegend(ImPlotLocation_NorthEast); + for (size_t j = 0; j < jointCount; ++j) { + char label[16]; + snprintf(label, sizeof(label), "J%02d", (int)j); + ImPlot::PlotLine(label, rX.data(), rY[j].data(), (int)sampleCount); + } + ImPlot::EndPlot(); + } + } + // --- CSV Export --- // Export telemetry data to CSV for external analysis (e.g. Python, Excel) @@ -1682,7 +1732,7 @@ namespace gui { ImGui::TextDisabled(" Robot: %s", _requestedRobot.c_str()); } - const auto& last = ring.at(ring.size() - 1); + const auto& last = ring.at(ring.size() - 1); // get the latest sample for summary info ImGui::TextDisabled("Total Time: %.3f s | Samples: %d", last.timeSec, (int)ring.size()); ImGui::Separator(); From 8e35b0d71e11f1c93eda885a59b6a37ff62c5994 Mon Sep 17 00:00:00 2001 From: saltyjoss Date: Sun, 31 May 2026 16:45:50 +0100 Subject: [PATCH 098/210] feat: Added local optimisations to implicit and AD_implicit `newton_raphson` methods --- .../include/Robots/RobotDynamics.inl | 1 - .../DSFE_Core/include/Robots/RobotSystem.h | 2 +- .../integrators/auto_diff_integrators.inl | 36 +++++++++----- .../integrators/implicit_integrators.inl | 48 +++++++++++-------- 4 files changed, 55 insertions(+), 32 deletions(-) diff --git a/DSFE_App/DSFE_Core/include/Robots/RobotDynamics.inl b/DSFE_App/DSFE_Core/include/Robots/RobotDynamics.inl index 66836a71..25b8383f 100644 --- a/DSFE_App/DSFE_Core/include/Robots/RobotDynamics.inl +++ b/DSFE_App/DSFE_Core/include/Robots/RobotDynamics.inl @@ -349,7 +349,6 @@ namespace robots { // Scalar tau_f = dynamics::computeKarnoppFriction(qd[i], tau_i, b, c); // add friction compensation // tau_i += tau_f; - scratch.dense.tau[i] = tau_i; out.metrics.q[i] = mathlib::real(q[i]); diff --git a/DSFE_App/DSFE_Core/include/Robots/RobotSystem.h b/DSFE_App/DSFE_Core/include/Robots/RobotSystem.h index 9ee3c15f..91512d4e 100644 --- a/DSFE_App/DSFE_Core/include/Robots/RobotSystem.h +++ b/DSFE_App/DSFE_Core/include/Robots/RobotSystem.h @@ -44,7 +44,7 @@ namespace robots { mathlib::VecX_T tau_rnea; }; - inline constexpr size_t AD_VARS = 14; // number of independent variables for autodiff (used for pre-allocating AD integrator buffers) + inline constexpr size_t AD_VARS = 14; // number of independent variables for autodiff (used for pre-allocating AD integrator buffers) class DSFE_API RobotSystem { public: diff --git a/PxMLib/MathLib/include/integrators/auto_diff_integrators.inl b/PxMLib/MathLib/include/integrators/auto_diff_integrators.inl index eec56c11..d4c4c4e9 100644 --- a/PxMLib/MathLib/include/integrators/auto_diff_integrators.inl +++ b/PxMLib/MathLib/include/integrators/auto_diff_integrators.inl @@ -324,16 +324,30 @@ namespace integration { ) { using Real = typename mathlib::DualTraits::BaseScalar; const Eigen::Index n = x0.size(); + constexpr size_t NVar = mathlib::DualTraits::Dimension; + + static thread_local mathlib::VecX_T g_real; + static thread_local mathlib::VecX_T delta; + static thread_local mathlib::MatX_T J; + static thread_local mathlib::MatX_T RHS_seed; + static thread_local mathlib::MatX_T corrected_sensitivies; + + if (g_real.size() != n) { + g_real.resize(n); + delta.resize(n); + J.resize(n, n); + RHS_seed.resize(n, NVar); + corrected_sensitivies.resize(n, NVar); + } mathlib::VecX_T x_real = x0.template cast(); - mathlib::VecX_T g_real(n); - mathlib::VecX_T delta(n); - mathlib::MatX_T J(n, n); - Eigen::FullPivLU> solver; + Eigen::PartialPivLU> solver; bool converged = false; + mathlib::VecX_T g_dual; + g_dual.resize(n); + for (int iter = 0; iter < maxIter; ++iter) { - mathlib::VecX_T g_dual; eval_g(x_real.template cast(), g_dual); g_real = g_dual.template cast(); if (!g_real.allFinite()) { @@ -388,12 +402,12 @@ namespace integration { mathlib::VecX_T g_final; eval_g(x_final, g_final); - constexpr size_t NVar = mathlib::DualTraits::Dimension; - for (size_t d = 0; d < NVar; ++d) { - mathlib::VecX_T rhs_seed(n); - for (Eigen::Index j = 0; j < n; ++j) { rhs_seed(j) = g_final(j).dual[d]; } - mathlib::VecX_T corrected_sensitivies = solver.solve(-rhs_seed); - for (Eigen::Index i = 0; i < n; ++i) { x_final(i).dual[d] = corrected_sensitivies(i); } + for (Eigen::Index j = 0; j < n; ++j) { + for (size_t d = 0; d < NVar; ++d) { RHS_seed(j, d) = g_final(j).dual[d]; } + } + corrected_sensitivies = solver.solve(-RHS_seed); + for (Eigen::Index i = 0; i < n; ++i) { + for (size_t d = 0; d < NVar; ++d) { x_final(i).dual[d] = corrected_sensitivies(i, d); } } return x_final; } diff --git a/PxMLib/MathLib/include/integrators/implicit_integrators.inl b/PxMLib/MathLib/include/integrators/implicit_integrators.inl index 160e4a5e..931bf866 100644 --- a/PxMLib/MathLib/include/integrators/implicit_integrators.inl +++ b/PxMLib/MathLib/include/integrators/implicit_integrators.inl @@ -377,24 +377,32 @@ namespace integration { int maxIter, Scalar tol ) { - mathlib::VecX_T x = x0, g, x_trial; - mathlib::VecX_T delta = mathlib::VecX_T::Constant(x0.size(), std::numeric_limits::infinity()); - mathlib::MatX_T J; - Eigen::FullPivLU> solver; + const Eigen::Index n = x0.size(); + static thread_local mathlib::VecX_T x; + static thread_local mathlib::VecX_T g; + static thread_local mathlib::VecX_T delta; + static thread_local mathlib::MatX_T J; + if (x.size() != n) { + x.resize(n); + g.resize(n); + delta.resize(n); + J.resize(n, n); + } + x = x0; + Eigen::PartialPivLU> solver; for (int iter = 0; iter < maxIter; ++iter) { eval_g(x, g); - if (!g.allFinite()) { throw std::runtime_error("Newton received non-finite residual at iter = " + std::to_string(iter) + ", residual norm = " + std::to_string(g.norm())); } + if (!g.allFinite()) { throw std::runtime_error("Newton received non-finite residual"); } if (g.norm() < tol) { return x; } eval_j(x, J); - if (!J.allFinite()) { throw std::runtime_error("Newton received non-finite Jacobian at iter = " + std::to_string(iter)); } + if (!J.allFinite()) { throw std::runtime_error("Newton received non-finite Jacobian"); } solver.compute(J); delta = solver.solve(-g); - if (!delta.allFinite()) { throw std::runtime_error("Newton produced non-finite step at iter = " + std::to_string(iter)); } + if (!delta.allFinite()) { throw std::runtime_error("Newton produced non-finite step"); } x += delta; if (delta.norm() < tol * (Scalar(1) + x.norm())) { return x; } - if (!x.allFinite()) { throw std::runtime_error("Newton state became non-finite at iter = " + std::to_string(iter)); } } - throw std::runtime_error("Newton-Raphson failed to converge after " + std::to_string(maxIter) + " iterations. Final residual norm = " + std::to_string(g.norm()) + ", final step norm = " + std::to_string(delta.norm())); + return x; // Return the last iterate even if we didn't converge, as it may still be a useful approximation } // Finite difference approximation of the Jacobian matrix df/dx for a vector-valued function f: R^n -> R^m at a point x @@ -407,17 +415,19 @@ namespace integration { const Scalar eps_rel = Scalar(1e-8); const int n = static_cast(x.size()); mathlib::VecX_T f_0 = f(t, x); - mathlib::MatX_T J(f_0.size(), n); - // Compute the Jacobian column by column using central differences + + static thread_local mathlib::MatX_T J; + if (J.rows() != f_0.size() || J.cols() != n) { J.resize(f_0.size(), n); } + + mathlib::VecX_T x_perturbed = x; + + // Compute the Jacobian column by column using forward difference for (int i = 0; i < n; ++i) { - mathlib::VecX_T x_fwd = x; - mathlib::VecX_T x_bwd = x; - Scalar h = eps_rel * std::max(Scalar(1), Scalar(std::abs(mathlib::real(x(i))))); - x_fwd(i) += h; - x_bwd(i) -= h; - mathlib::VecX_T f_fwd = f(t, x_fwd); - mathlib::VecX_T f_bwd = f(t, x_bwd); - J.col(i) = (f_fwd - f_bwd) / (Scalar(2) * h); + Scalar h = eps_rel * std::max(Scalar(1), Scalar(std::abs(x(i)))); + x_perturbed(i) += h; + mathlib::VecX_T f_fwd = f(t, x_perturbed); + x_perturbed(i) = x(i); + J.col(i) = (f_fwd - f_0) / h; } return J; } From 0a696e8d1e1a01ef826e05bdd1dd9ceed1bcfa4a Mon Sep 17 00:00:00 2001 From: saltyjoss Date: Sun, 31 May 2026 17:21:55 +0100 Subject: [PATCH 099/210] fixes: Renabled `postStepUpdate` in `step_AD`, meaning a move back to the non-templated VecX (VecX_T def) rather than ambiguous templating of vector type * And just like that it works, there are many smaller issues I have noted to fix, but again I am really on the edge of my understanding here. * Next up for a change of pace is going to be the particle dynamics, but first that means making a functional state-driven workspace system in the GUI -> I am planning on really orientating this towards a visual proof, then going back and rebuilding the CLI batch to match newer logic -> Since I am no longer being graded on this (for now) it allows me to enjoy the process in my own way! :) --- DSFE_App/DSFE_Core/include/Robots/RobotSystem.h | 2 +- DSFE_App/DSFE_Core/include/Robots/RobotSystemStep.inl | 9 ++++----- 2 files changed, 5 insertions(+), 6 deletions(-) diff --git a/DSFE_App/DSFE_Core/include/Robots/RobotSystem.h b/DSFE_App/DSFE_Core/include/Robots/RobotSystem.h index 91512d4e..2a0149f6 100644 --- a/DSFE_App/DSFE_Core/include/Robots/RobotSystem.h +++ b/DSFE_App/DSFE_Core/include/Robots/RobotSystem.h @@ -203,7 +203,7 @@ namespace robots { ); template - void postStepUpdate(const mathlib::VecX_T& x, const DynamicsScratch& scratch, const RobotStepResult_T& result); + void postStepUpdate(const mathlib::VecX& x, const DynamicsScratch& scratch, const RobotStepResult_T& result); std::unique_ptr _kinematics; std::unique_ptr _dynamics; diff --git a/DSFE_App/DSFE_Core/include/Robots/RobotSystemStep.inl b/DSFE_App/DSFE_Core/include/Robots/RobotSystemStep.inl index 38dd2c44..55f85b73 100644 --- a/DSFE_App/DSFE_Core/include/Robots/RobotSystemStep.inl +++ b/DSFE_App/DSFE_Core/include/Robots/RobotSystemStep.inl @@ -150,11 +150,11 @@ namespace robots { } template - void RobotSystem::postStepUpdate(const mathlib::VecX_T& x, const DynamicsScratch& dynScratch, const RobotStepResult_T& result) { + void RobotSystem::postStepUpdate(const mathlib::VecX& x, const DynamicsScratch& dynScratch, const RobotStepResult_T& result) { const size_t n = result.snap.model->joints.size(); - Eigen::Map> q_next(x.data(), n); - Eigen::Map> qd_next(x.data() + n, n); + Eigen::Map q_next(x.data(), n); + Eigen::Map qd_next(x.data() + n, n); // Enforce joint limits /*for (auto& j : _robot.joints) { enforceJointLimits(j); }*/ @@ -239,8 +239,7 @@ namespace robots { _dynamics->setDt(result.stepOut.dt_taken); auto x_real = result.stepOut.x_next.unaryExpr([](const auto& v) { return mathlib::real(v); }); - - //postStepUpdate(x_real, _dynScratch_AD, result); + postStepUpdate(x_real, _dynScratch_AD, result); // Update base pose if free-floating if (_baseIsFree) { From 98bc4c29c921ca300f91200fab61129e3d400e5e Mon Sep 17 00:00:00 2001 From: saltyjoss Date: Tue, 2 Jun 2026 15:37:50 +0100 Subject: [PATCH 100/210] feat: Added Qt6 find_package to GUI cmake * This will be hardcoded UNTIL I have it working as expected, then I will convert it to a cmake preset file! --- DSFE_App/DSFE_GUI/CMakeLists.txt | 15 +++++++++++++++ 1 file changed, 15 insertions(+) diff --git a/DSFE_App/DSFE_GUI/CMakeLists.txt b/DSFE_App/DSFE_GUI/CMakeLists.txt index 72f4af9a..b2e6300e 100644 --- a/DSFE_App/DSFE_GUI/CMakeLists.txt +++ b/DSFE_App/DSFE_GUI/CMakeLists.txt @@ -4,10 +4,21 @@ set(CMAKE_POSITION_INDEPENDENT_CODE ON) cmake_minimum_required(VERSION 3.10) project(DSFE_GUI LANGUAGES C CXX) +set(CMAKE_AUTOMOC ON) +set(CMAKE_AUTOUIC ON) +set(CMAKE_AUTORCC ON) +set(CMAKE_PREFIX_PATH "C:/Qt/6.11.1/msvc2022_64") + add_library(DSFE_GUI SHARED) find_package(Threads REQUIRED) find_package(OpenGL REQUIRED) +find_package(Qt6 REQUIRED COMPONENTS + Core + Gui + Widgets + OpenGLWidgets +) target_compile_definitions(DSFE_GUI PRIVATE DSFE_GUI_EXPORTS @@ -133,4 +144,8 @@ target_link_libraries(DSFE_GUI glm::glm imgui OpenGL::GL + Qt6::Core + Qt6::Gui + Qt6::Widgets + Qt6::OpenGLWidgets ) \ No newline at end of file From 45120c39249f67098e56f989f4bce81ee95b4ae7 Mon Sep 17 00:00:00 2001 From: saltyjoss Date: Tue, 2 Jun 2026 15:38:09 +0100 Subject: [PATCH 101/210] feat: Added basic debug test initialisation of qt6 in `Application` --- DSFE_App/DSFE_GUI/src/Application.cpp | 22 ++++++++++++++++++++++ 1 file changed, 22 insertions(+) diff --git a/DSFE_App/DSFE_GUI/src/Application.cpp b/DSFE_App/DSFE_GUI/src/Application.cpp index 4e56e905..81fee1ca 100644 --- a/DSFE_App/DSFE_GUI/src/Application.cpp +++ b/DSFE_App/DSFE_GUI/src/Application.cpp @@ -13,10 +13,17 @@ #include "Platform/Paths.h" #include "EngineLib/LogMacros.h" +#include +#include +#include + namespace fs = std::filesystem; // Initialise the static instance pointer to nullptr Application* Application::sInstance = nullptr; +static QApplication* gQtApp = nullptr; +static QMainWindow* gQtWindow = nullptr; + // Constructor: Initialises paths, sets up data manager, and creates the main application window Application::Application(const std::string& appName) { paths::init(); @@ -28,6 +35,7 @@ Application::Application(const std::string& appName) { LOG_INFO("Runs path: %s", paths::runs().string().c_str()); int winW = 1920, winH = 1080; + if (glfwInit()) { const GLFWvidmode* mode = glfwGetVideoMode(glfwGetPrimaryMonitor()); if (mode) { @@ -43,6 +51,18 @@ Application::Application(const std::string& appName) { glfwTerminate(); // OpenGLContext::init will call glfwInit again } + if (!gQtApp) { + QCoreApplication::addLibraryPath("C:/Qt/6.11.1/msvc2022_64/plugins"); + qputenv("QT_DEBUG_PLUGINS", "1"); + int argc = 0; + gQtApp = new QApplication(argc, nullptr); + gQtWindow = new QMainWindow(); + gQtWindow->setWindowTitle(QString::fromStdString(appName)); + gQtWindow->resize(800, 600); + gQtWindow->show(); + LOG_INFO("Qt application and main window created with title '%s'", appName.c_str()); + } + _window = std::make_unique(); _window->init(winW, winH, appName); } @@ -52,6 +72,8 @@ Application::~Application() = default; // Main application loop: Continues running until the window signals to close, updating and rendering each frame void Application::run() { while (_window->isRunning() && !_window->shouldClose()) { + QCoreApplication::processEvents(); + _window->update(); _window->render(); } From 22e3a9728daefe63647112184b9f8ec43350eab4 Mon Sep 17 00:00:00 2001 From: saltyjoss Date: Tue, 2 Jun 2026 15:55:35 +0100 Subject: [PATCH 102/210] feat: Added `DSFE_MainWindow` header and compiler files, with defined constructor. --- DSFE_App/DSFE_GUI/CMakeLists.txt | 5 +++++ .../DSFE_GUI/include/MainWindow/DSFE_MainWindow.h | 11 +++++++++++ DSFE_App/DSFE_GUI/src/Application.cpp | 1 - DSFE_App/DSFE_GUI/src/MainWindow/DSFE_MainWindow.cpp | 9 +++++++++ 4 files changed, 25 insertions(+), 1 deletion(-) create mode 100644 DSFE_App/DSFE_GUI/include/MainWindow/DSFE_MainWindow.h create mode 100644 DSFE_App/DSFE_GUI/src/MainWindow/DSFE_MainWindow.cpp diff --git a/DSFE_App/DSFE_GUI/CMakeLists.txt b/DSFE_App/DSFE_GUI/CMakeLists.txt index b2e6300e..321e911d 100644 --- a/DSFE_App/DSFE_GUI/CMakeLists.txt +++ b/DSFE_App/DSFE_GUI/CMakeLists.txt @@ -37,6 +37,10 @@ target_include_directories(stb INTERFACE ${CMAKE_CURRENT_SOURCE_DIR}/include ) +set(MAIN_WINDOW_SRC + src/MainWindow/DSFE_MainWindow.cpp +) + set(RENDER_SRC src/Rendering/CubeVertices.cpp src/Rendering/GUIContext.cpp @@ -82,6 +86,7 @@ set(ASSETS_SRC ) target_sources(DSFE_GUI PRIVATE + ${MAIN_WINDOW_SRC} ${RENDER_SRC} ${SCENE_SRC} ${UI_SRC} diff --git a/DSFE_App/DSFE_GUI/include/MainWindow/DSFE_MainWindow.h b/DSFE_App/DSFE_GUI/include/MainWindow/DSFE_MainWindow.h new file mode 100644 index 00000000..5ed46a4c --- /dev/null +++ b/DSFE_App/DSFE_GUI/include/MainWindow/DSFE_MainWindow.h @@ -0,0 +1,11 @@ +// DSFE_GUI DSFE_MainWindow.h +#pragma once + +#include + +namespace window { + class DSFE_MainWindow : public QMainWindow { + public: + explicit DSFE_MainWindow(QWidget* parent = nullptr); + }; +} diff --git a/DSFE_App/DSFE_GUI/src/Application.cpp b/DSFE_App/DSFE_GUI/src/Application.cpp index 81fee1ca..d7ce485f 100644 --- a/DSFE_App/DSFE_GUI/src/Application.cpp +++ b/DSFE_App/DSFE_GUI/src/Application.cpp @@ -53,7 +53,6 @@ Application::Application(const std::string& appName) { if (!gQtApp) { QCoreApplication::addLibraryPath("C:/Qt/6.11.1/msvc2022_64/plugins"); - qputenv("QT_DEBUG_PLUGINS", "1"); int argc = 0; gQtApp = new QApplication(argc, nullptr); gQtWindow = new QMainWindow(); diff --git a/DSFE_App/DSFE_GUI/src/MainWindow/DSFE_MainWindow.cpp b/DSFE_App/DSFE_GUI/src/MainWindow/DSFE_MainWindow.cpp new file mode 100644 index 00000000..b6eb8a5c --- /dev/null +++ b/DSFE_App/DSFE_GUI/src/MainWindow/DSFE_MainWindow.cpp @@ -0,0 +1,9 @@ +//DSFE_GUI DSFE_MainWindow.cpp +#include "MainWindow/DSFE_MainWindow.h" + +namespace window { + DSFE_MainWindow::DSFE_MainWindow(QWidget* parent) : QMainWindow(parent) { + setWindowTitle("DSFE Simulator"); + resize(800, 600); + } +} \ No newline at end of file From 62a964e4150fd71fc7b2d98cb91df3c50ec2d1ec Mon Sep 17 00:00:00 2001 From: saltyjoss Date: Tue, 2 Jun 2026 15:59:45 +0100 Subject: [PATCH 103/210] feat: Added basic call from `Application` --- DSFE_App/DSFE_GUI/src/Application.cpp | 10 ++++------ DSFE_App/DSFE_GUI/src/MainWindow/DSFE_MainWindow.cpp | 2 +- 2 files changed, 5 insertions(+), 7 deletions(-) diff --git a/DSFE_App/DSFE_GUI/src/Application.cpp b/DSFE_App/DSFE_GUI/src/Application.cpp index d7ce485f..5e8313da 100644 --- a/DSFE_App/DSFE_GUI/src/Application.cpp +++ b/DSFE_App/DSFE_GUI/src/Application.cpp @@ -2,6 +2,7 @@ #include "Application.h" #include "Platform/WindowManager.h" +#include "MainWindow/DSFE_MainWindow.h" #include "Scene/Camera.h" #ifdef __gl_h_ @@ -22,7 +23,7 @@ namespace fs = std::filesystem; Application* Application::sInstance = nullptr; static QApplication* gQtApp = nullptr; -static QMainWindow* gQtWindow = nullptr; +static window::DSFE_MainWindow* gQtWindow = nullptr; // Constructor: Initialises paths, sets up data manager, and creates the main application window Application::Application(const std::string& appName) { @@ -55,11 +56,8 @@ Application::Application(const std::string& appName) { QCoreApplication::addLibraryPath("C:/Qt/6.11.1/msvc2022_64/plugins"); int argc = 0; gQtApp = new QApplication(argc, nullptr); - gQtWindow = new QMainWindow(); - gQtWindow->setWindowTitle(QString::fromStdString(appName)); - gQtWindow->resize(800, 600); - gQtWindow->show(); - LOG_INFO("Qt application and main window created with title '%s'", appName.c_str()); + gQtWindow = new window::DSFE_MainWindow(); + gQtWindow->show();` } _window = std::make_unique(); diff --git a/DSFE_App/DSFE_GUI/src/MainWindow/DSFE_MainWindow.cpp b/DSFE_App/DSFE_GUI/src/MainWindow/DSFE_MainWindow.cpp index b6eb8a5c..e260eb21 100644 --- a/DSFE_App/DSFE_GUI/src/MainWindow/DSFE_MainWindow.cpp +++ b/DSFE_App/DSFE_GUI/src/MainWindow/DSFE_MainWindow.cpp @@ -3,7 +3,7 @@ namespace window { DSFE_MainWindow::DSFE_MainWindow(QWidget* parent) : QMainWindow(parent) { - setWindowTitle("DSFE Simulator"); + setWindowTitle("DSFE"); resize(800, 600); } } \ No newline at end of file From c75ae3a77eb00ce2506bb921e45a796735a75da1 Mon Sep 17 00:00:00 2001 From: saltyjoss Date: Tue, 2 Jun 2026 16:17:02 +0100 Subject: [PATCH 104/210] feat: Added shell of `ProjectPage` class * This eventually will be the workspaces internal default for a "new" project, however for now its going to be used to ensure the project can effectively replicate the imgui version --- DSFE_App/DSFE_GUI/CMakeLists.txt | 2 ++ .../include/MainWindow/Workspace/ProjectPage.h | 11 +++++++++++ DSFE_App/DSFE_GUI/src/MainWindow/DSFE_MainWindow.cpp | 5 ++++- .../DSFE_GUI/src/MainWindow/Workspace/ProjectPage.cpp | 8 ++++++++ 4 files changed, 25 insertions(+), 1 deletion(-) create mode 100644 DSFE_App/DSFE_GUI/include/MainWindow/Workspace/ProjectPage.h create mode 100644 DSFE_App/DSFE_GUI/src/MainWindow/Workspace/ProjectPage.cpp diff --git a/DSFE_App/DSFE_GUI/CMakeLists.txt b/DSFE_App/DSFE_GUI/CMakeLists.txt index 321e911d..484cf0cc 100644 --- a/DSFE_App/DSFE_GUI/CMakeLists.txt +++ b/DSFE_App/DSFE_GUI/CMakeLists.txt @@ -39,6 +39,7 @@ target_include_directories(stb INTERFACE set(MAIN_WINDOW_SRC src/MainWindow/DSFE_MainWindow.cpp + src/MainWindow/Workspace/ProjectPage.cpp ) set(RENDER_SRC @@ -128,6 +129,7 @@ target_include_directories(DSFE_GUI PUBLIC ${CMAKE_CURRENT_SOURCE_DIR} ${CMAKE_CURRENT_SOURCE_DIR}/include + ${CMAKE_CURRENT_SOURCE_DIR}/include/MainWindow PRIVATE ${imfilebrowser_ROOT} diff --git a/DSFE_App/DSFE_GUI/include/MainWindow/Workspace/ProjectPage.h b/DSFE_App/DSFE_GUI/include/MainWindow/Workspace/ProjectPage.h new file mode 100644 index 00000000..bed7285a --- /dev/null +++ b/DSFE_App/DSFE_GUI/include/MainWindow/Workspace/ProjectPage.h @@ -0,0 +1,11 @@ +// DSFE_GUI ProjectPage.h +#pragma once + +#include + +namespace Workspace { + class ProjectPage : public QWidget { + public: + explicit ProjectPage(QWidget* parent = nullptr); + }; +} // namespace Workspace \ No newline at end of file diff --git a/DSFE_App/DSFE_GUI/src/MainWindow/DSFE_MainWindow.cpp b/DSFE_App/DSFE_GUI/src/MainWindow/DSFE_MainWindow.cpp index e260eb21..c801b805 100644 --- a/DSFE_App/DSFE_GUI/src/MainWindow/DSFE_MainWindow.cpp +++ b/DSFE_App/DSFE_GUI/src/MainWindow/DSFE_MainWindow.cpp @@ -1,9 +1,12 @@ //DSFE_GUI DSFE_MainWindow.cpp #include "MainWindow/DSFE_MainWindow.h" +#include "Workspace/ProjectPage.h" namespace window { DSFE_MainWindow::DSFE_MainWindow(QWidget* parent) : QMainWindow(parent) { setWindowTitle("DSFE"); resize(800, 600); + + setCentralWidget(new Workspace::ProjectPage(this)); } -} \ No newline at end of file +} // namespace window \ No newline at end of file diff --git a/DSFE_App/DSFE_GUI/src/MainWindow/Workspace/ProjectPage.cpp b/DSFE_App/DSFE_GUI/src/MainWindow/Workspace/ProjectPage.cpp new file mode 100644 index 00000000..db2eac64 --- /dev/null +++ b/DSFE_App/DSFE_GUI/src/MainWindow/Workspace/ProjectPage.cpp @@ -0,0 +1,8 @@ +// DSFE_GUI ProjectPage.cpp +#include "Workspace/ProjectPage.h" + +namespace Workspace { + ProjectPage::ProjectPage(QWidget* parent) : QWidget(parent) { + // Set up the UI for the project page here (e.g., using QVBoxLayout, adding widgets, etc.) + } +} // namespace Workspace \ No newline at end of file From 7953ebeb489bcb84d4edf2241ee76c7b252b94e6 Mon Sep 17 00:00:00 2001 From: saltyjoss Date: Tue, 2 Jun 2026 17:34:49 +0100 Subject: [PATCH 105/210] feat: Addded viewport widget logic and docking variant logic (though unused due to differng logic I am still exploring) --- DSFE_App/DSFE_GUI/CMakeLists.txt | 2 ++ .../include/MainWindow/DockWidgets/ViewportDock.h | 13 +++++++++++++ .../include/MainWindow/Widgets/ViewportWidget.h | 11 +++++++++++ DSFE_App/DSFE_GUI/src/Application.cpp | 2 +- .../DSFE_GUI/src/MainWindow/DSFE_MainWindow.cpp | 4 +++- .../src/MainWindow/DockWidgets/ViewportDock.cpp | 11 +++++++++++ .../src/MainWindow/Widgets/ViewportWidget.cpp | 10 ++++++++++ .../src/MainWindow/Workspace/ProjectPage.cpp | 13 ++++++++++++- 8 files changed, 63 insertions(+), 3 deletions(-) create mode 100644 DSFE_App/DSFE_GUI/include/MainWindow/DockWidgets/ViewportDock.h create mode 100644 DSFE_App/DSFE_GUI/include/MainWindow/Widgets/ViewportWidget.h create mode 100644 DSFE_App/DSFE_GUI/src/MainWindow/DockWidgets/ViewportDock.cpp create mode 100644 DSFE_App/DSFE_GUI/src/MainWindow/Widgets/ViewportWidget.cpp diff --git a/DSFE_App/DSFE_GUI/CMakeLists.txt b/DSFE_App/DSFE_GUI/CMakeLists.txt index 484cf0cc..8e04f4ce 100644 --- a/DSFE_App/DSFE_GUI/CMakeLists.txt +++ b/DSFE_App/DSFE_GUI/CMakeLists.txt @@ -40,6 +40,8 @@ target_include_directories(stb INTERFACE set(MAIN_WINDOW_SRC src/MainWindow/DSFE_MainWindow.cpp src/MainWindow/Workspace/ProjectPage.cpp + src/MainWindow/DockWidgets/ViewportDock.cpp + src/MainWindow/Widgets/ViewportWidget.cpp ) set(RENDER_SRC diff --git a/DSFE_App/DSFE_GUI/include/MainWindow/DockWidgets/ViewportDock.h b/DSFE_App/DSFE_GUI/include/MainWindow/DockWidgets/ViewportDock.h new file mode 100644 index 00000000..7ea44d8a --- /dev/null +++ b/DSFE_App/DSFE_GUI/include/MainWindow/DockWidgets/ViewportDock.h @@ -0,0 +1,13 @@ +// DSFE_GUI ViewportDock.h +#pragma once + +#include + +class ViewportWidget; + +namespace dockwidgets { + class ViewportDock : public QDockWidget { + public: + explicit ViewportDock(QWidget* parent = nullptr); + }; +} // namespace dockwidgets \ No newline at end of file diff --git a/DSFE_App/DSFE_GUI/include/MainWindow/Widgets/ViewportWidget.h b/DSFE_App/DSFE_GUI/include/MainWindow/Widgets/ViewportWidget.h new file mode 100644 index 00000000..51868f9e --- /dev/null +++ b/DSFE_App/DSFE_GUI/include/MainWindow/Widgets/ViewportWidget.h @@ -0,0 +1,11 @@ +// DSFE_GUI ViewportWidget.h +#pragma once + +#include + +namespace widgets { + class ViewportWidget : public QOpenGLWidget { + public: + explicit ViewportWidget(QWidget* parent = nullptr); + }; +} \ No newline at end of file diff --git a/DSFE_App/DSFE_GUI/src/Application.cpp b/DSFE_App/DSFE_GUI/src/Application.cpp index 5e8313da..5e95f7e1 100644 --- a/DSFE_App/DSFE_GUI/src/Application.cpp +++ b/DSFE_App/DSFE_GUI/src/Application.cpp @@ -57,7 +57,7 @@ Application::Application(const std::string& appName) { int argc = 0; gQtApp = new QApplication(argc, nullptr); gQtWindow = new window::DSFE_MainWindow(); - gQtWindow->show();` + gQtWindow->show(); } _window = std::make_unique(); diff --git a/DSFE_App/DSFE_GUI/src/MainWindow/DSFE_MainWindow.cpp b/DSFE_App/DSFE_GUI/src/MainWindow/DSFE_MainWindow.cpp index c801b805..8c2ea55a 100644 --- a/DSFE_App/DSFE_GUI/src/MainWindow/DSFE_MainWindow.cpp +++ b/DSFE_App/DSFE_GUI/src/MainWindow/DSFE_MainWindow.cpp @@ -7,6 +7,8 @@ namespace window { setWindowTitle("DSFE"); resize(800, 600); - setCentralWidget(new Workspace::ProjectPage(this)); + auto* page = new Workspace::ProjectPage(this); + setCentralWidget(page); + page->show(); } } // namespace window \ No newline at end of file diff --git a/DSFE_App/DSFE_GUI/src/MainWindow/DockWidgets/ViewportDock.cpp b/DSFE_App/DSFE_GUI/src/MainWindow/DockWidgets/ViewportDock.cpp new file mode 100644 index 00000000..732f92c1 --- /dev/null +++ b/DSFE_App/DSFE_GUI/src/MainWindow/DockWidgets/ViewportDock.cpp @@ -0,0 +1,11 @@ +// DSFE_GUI ViewportDock.cpp +#include "DockWidgets/ViewportDock.h" +#include "Widgets/ViewportWidget.h" + +namespace dockwidgets { + ViewportDock::ViewportDock(QWidget* parent) : QDockWidget("Viewport", parent) { + setAllowedAreas(Qt::AllDockWidgetAreas); + setFeatures(QDockWidget::DockWidgetMovable | QDockWidget::DockWidgetFloatable); + setWidget(new widgets::ViewportWidget(this)); + } +} // namespace dockwidgets \ No newline at end of file diff --git a/DSFE_App/DSFE_GUI/src/MainWindow/Widgets/ViewportWidget.cpp b/DSFE_App/DSFE_GUI/src/MainWindow/Widgets/ViewportWidget.cpp new file mode 100644 index 00000000..5cae3be8 --- /dev/null +++ b/DSFE_App/DSFE_GUI/src/MainWindow/Widgets/ViewportWidget.cpp @@ -0,0 +1,10 @@ +// DSFE_GUI ViewportWidget.cpp +#include "Widgets/ViewportWidget.h" + +#include +#include + +namespace widgets { + ViewportWidget::ViewportWidget(QWidget* parent) : QOpenGLWidget(parent) { + } +} // namespace widgets \ No newline at end of file diff --git a/DSFE_App/DSFE_GUI/src/MainWindow/Workspace/ProjectPage.cpp b/DSFE_App/DSFE_GUI/src/MainWindow/Workspace/ProjectPage.cpp index db2eac64..dc4feb7e 100644 --- a/DSFE_App/DSFE_GUI/src/MainWindow/Workspace/ProjectPage.cpp +++ b/DSFE_App/DSFE_GUI/src/MainWindow/Workspace/ProjectPage.cpp @@ -1,8 +1,19 @@ // DSFE_GUI ProjectPage.cpp #include "Workspace/ProjectPage.h" +#include "Widgets/ViewportWidget.h" + +#include +#include +#include namespace Workspace { ProjectPage::ProjectPage(QWidget* parent) : QWidget(parent) { - // Set up the UI for the project page here (e.g., using QVBoxLayout, adding widgets, etc.) + auto* layout = new QVBoxLayout(this); + auto* splitter = new QSplitter(Qt::Horizontal, this); + splitter->addWidget(new widgets::ViewportWidget(splitter)); + splitter->addWidget(new QLabel("Properties(PlaceHolder)", splitter)); + splitter->setStretchFactor(0, 4); // Viewport takes 4/5 of space + splitter->setStretchFactor(1, 1); // Properties takes 1/5 of space + layout->addWidget(splitter); } } // namespace Workspace \ No newline at end of file From b3ca889dfb558c49431fa46e72d167dac746f45c Mon Sep 17 00:00:00 2001 From: saltyjoss Date: Thu, 4 Jun 2026 09:03:12 +0100 Subject: [PATCH 106/210] feat: Separated `SimManager` compiler file into domain-specific subfiles to reduce file size and code stink --- .../include/Scene/Manager/SimImplementation.h | 611 ++++++ .../include/Scene/SimulationManager.h | 1 + .../DSFE_GUI/src/Scene/Manager/SimBackend.cpp | 112 ++ .../DSFE_GUI/src/Scene/Manager/SimCamera.cpp | 143 ++ .../DSFE_GUI/src/Scene/Manager/SimObjects.cpp | 110 ++ .../DSFE_GUI/src/Scene/Manager/SimPostFX.cpp | 10 + .../src/Scene/Manager/SimRendering.cpp | 433 +++++ .../DSFE_GUI/src/Scene/Manager/SimRobots.cpp | 77 + DSFE_App/DSFE_GUI/src/Scene/Manager/SimUI.cpp | 180 ++ .../src/Scene/Manager/SimViewport.cpp | 10 + .../DSFE_GUI/src/Scene/SimulationManager.cpp | 1634 +---------------- 11 files changed, 1691 insertions(+), 1630 deletions(-) create mode 100644 DSFE_App/DSFE_GUI/include/Scene/Manager/SimImplementation.h create mode 100644 DSFE_App/DSFE_GUI/src/Scene/Manager/SimBackend.cpp create mode 100644 DSFE_App/DSFE_GUI/src/Scene/Manager/SimCamera.cpp create mode 100644 DSFE_App/DSFE_GUI/src/Scene/Manager/SimObjects.cpp create mode 100644 DSFE_App/DSFE_GUI/src/Scene/Manager/SimPostFX.cpp create mode 100644 DSFE_App/DSFE_GUI/src/Scene/Manager/SimRendering.cpp create mode 100644 DSFE_App/DSFE_GUI/src/Scene/Manager/SimRobots.cpp create mode 100644 DSFE_App/DSFE_GUI/src/Scene/Manager/SimUI.cpp create mode 100644 DSFE_App/DSFE_GUI/src/Scene/Manager/SimViewport.cpp diff --git a/DSFE_App/DSFE_GUI/include/Scene/Manager/SimImplementation.h b/DSFE_App/DSFE_GUI/include/Scene/Manager/SimImplementation.h new file mode 100644 index 00000000..6b4303d3 --- /dev/null +++ b/DSFE_App/DSFE_GUI/include/Scene/Manager/SimImplementation.h @@ -0,0 +1,611 @@ +// DSFE_GUI SimImplementation.h +#include "Scene/SimulationManager.h" +#include "Scene/Object.h" + +#include +#include +#include +#include +#include +#include +#include + +#include "Scene/Input.h" +#include "Scene/Camera.h" +#include "Scene/Mesh.h" +#include "Assets/MeshLoader.h" +#include "Scene/Light.h" +#include "Scene/AxisOrientator.h" + +#include "Robots/RobotSystem.h" +#include "Robots/RobotModel.h" +#include "Robots/TrajectoryManager.h" + +#include "Rendering/SkyboxRenderer.h" +#include "Rendering/ShaderUtil.h" +#include "Rendering/OpenGLBufferManager.h" +#include "Rendering/IBL.h" +#include "Rendering/Texture.h" + +#include "Robots/RobotPresentationBuilder.h" +#include "Robots/RobotRenderer.h" + +#include +#include "Platform/Paths.h" +#include "EngineLib/LogMacros.h" +#include "Platform/DataManager.h" + +namespace gui { + // --- PIMPL Implementation --- + struct SimManager::Impl { + // Completed Simulation Runs (thread-safe) + std::mutex _completedRunsMutex; + std::vector _completedRuns; + + // View ID Alias + using VID = gui::ViewID; + + // View Modes + enum class ViewMode { Single, Quad }; + + // Current View Mode + ViewMode viewMode = ViewMode::Single; + + // Viewport Structure + struct Viewport { + std::unique_ptr cam; + std::unique_ptr fb; + std::unique_ptr post; + + // Cache size + int w = 1, h = 1; + int displayW = 1, displayH = 1; + + // Follow Target + scene::Object* followTarget = nullptr; + glm::vec3 followOffset = glm::vec3(0.0f, 0.25f, 1.0f); // tweak + bool followEnabled = false; + }; + + // Viewports + std::array _views; + VID activeView = VID::Manual; + + // Skybox & IBL + std::unique_ptr _ibl; + std::unique_ptr _skybox; + + // Post-Processing Shader + std::unique_ptr _postShader; + std::shared_ptr _shaderBasic; + std::shared_ptr _shaderLit; + std::shared_ptr _shaderPBR; + + // World Grid & Shadow Shaders + std::unique_ptr _worldGridShader; + std::unique_ptr _shadowShader; + shaders::Shader* currentShader; + + // Fullscreen Quad VAO + GLuint _fullscreenVAO = 0; + GLuint _worldGridVAO = 0; + + // Shadow Mapping (Cascaded) + GLuint _cascadeFBO[SimManager::NUM_CASCADES]{}; + GLuint _cascadeDepth[SimManager::NUM_CASCADES]{}; + glm::mat4 _lightSpaceMatrixCascade[SimManager::NUM_CASCADES] = {}; + + // Scene Objects + std::unique_ptr _light; + std::unique_ptr _axisOrientator; + + scene::Object* _selectedObject = nullptr; + scene::Object* _cameraFollowTarget = nullptr; + + std::shared_ptr _mesh; + std::vector> _objects; + std::unordered_map> _linkToObjects; + std::unordered_map _primaryLinkObject; + + // Robot System + std::unique_ptr _robotSystem; // simulation + + // Robot Rendering + std::unique_ptr _robotRenderer; // rendering + RobotRenderBinding _currentBinding; // current render binding + + // Robot Follow Target + bool eeFollowBound = false; + scene::Object* eeObject = nullptr; + + // Trajectory Manager + control::TrajectoryManager _traj; + + // SSAO Resources + std::unique_ptr _ssaoShader; + std::unique_ptr _ssaoBlurShader; + GLuint _ssaoFBO = 0, _ssaoTex = 0; + GLuint _ssaoBlurFBO = 0, _ssaoBlurTex = 0; + GLuint _ssaoNoiseTex = 0; + int _ssaoW = 0, _ssaoH = 0; + std::vector _ssaoKernel; + + Impl(SimManager& owner) { + _postShader = std::make_unique(); + _postShader->load((paths::assets() / "shaders" / "post.vert.glsl").string(), (paths::assets() / "shaders" / "post.frag.glsl").string()); + + glGenVertexArrays(1, &_fullscreenVAO); + + // Lambda to create views + auto makeView = [&](ViewID id, glm::vec3 pos, float fovDeg, glm::vec3 target, glm::vec3 /*upHint*/) { + auto& v = _views[(size_t)id]; + + // INTERNAL render target size (scene render) + const int rtW = std::max(1, (int)owner._internalSize.x); + const int rtH = std::max(1, (int)owner._internalSize.y); + + // DISPLAY size (final post-process target) + const int dispW = std::max(1, (int)owner._displaySize.x); + const int dispH = std::max(1, (int)owner._displaySize.y); + const int postW = (dispW > 1 && dispH > 1) ? dispW : rtW; + const int postH = (dispW > 1 && dispH > 1) ? dispH : rtH; + + // Cache sizes + v.w = rtW; + v.h = rtH; + v.displayW = postW; + v.displayH = postH; + + // Framebuffers + v.fb = std::make_unique(); + v.fb->createBuffers(rtW, rtH, std::max(1, owner._settingsCurrent.msaaSamples)); + + v.post = std::make_unique(); + v.post->createBuffers(postW, postH, 1); + + // Camera aspect should match DISPLAY (what you're presenting in ImGui) + v.cam = std::make_unique(pos, fovDeg, (float)postW / (float)postH, 0.1f, 5000.0f); + v.cam->setFocus(target); + v.cam->updateViewMatrix(); + }; + + glm::vec3 target(0.0f); + + // Perspective + makeView(ViewID::Manual, { 0.0f, 0.5f, 1.0f }, 60.0f, target, { 0.0f, 1.0f, 0.0f }); // Default + makeView(ViewID::Follow, { 3.0f, 0.25f, 0.0f }, 20.0f, target, { 0.0f, 1.0f, 0.0f }); // Follow + // Ortho-ish + makeView(ViewID::Top, { 0.0f, 2.0f, 0.0f }, 20.0f, target, { 0.0f, 0.0f, -1.0f }); // Top + makeView(ViewID::Right, { 3.0f, 0.25f, 0.0f }, 20.0f, target, { 0.0f, 1.0f, 0.0f }); // Right + makeView(ViewID::Front, { 0.0f, 0.25f, 3.0f }, 20.0f, target, { 0.0f, -1.0f, 0.0f }); // Front + + { + // Top + auto* camTop = _views[(size_t)ViewID::Top].cam.get(); + camTop->setFocus(target); + camTop->setYaw(-glm::half_pi()); + camTop->setPitch(-glm::half_pi() + 0.001f); + camTop->setOrbitDistance(2.0f); + camTop->setMinDistance(0.5f); + camTop->updateViewMatrix(); + + // Right + auto* camRight = _views[(size_t)ViewID::Right].cam.get(); + camRight->setFocus(target); + camRight->setYaw(glm::pi()); + camRight->setPitch(0.0f); + camRight->setOrbitDistance(3.0f); + camRight->setMinDistance(0.5f); + camRight->updateViewMatrix(); + + // Front + auto* camFront = _views[(size_t)ViewID::Front].cam.get(); + camFront->setFocus(target); + camFront->setYaw(-glm::half_pi()); + camFront->setPitch(0.0f); + camFront->setOrbitDistance(3.0f); + camFront->setMinDistance(0.5f); + camFront->updateViewMatrix(); + + // Follow + auto* camFollow = _views[(size_t)ViewID::Follow].cam.get(); + camFollow->setFocus(target); + camFollow->setYaw(glm::pi()); + camFollow->setPitch(0.0f); + camFollow->setOrbitDistance(3.0f); + camFollow->setMinDistance(0.5f); + camFollow->updateViewMatrix(); + } + + // Shader Types A + // Basic shader with no lighting + _shaderBasic = std::make_shared(); + _shaderBasic->load((paths::assets() / "shaders" / "vs_pbr.vert.glsl").string(), (paths::assets() / "shaders" / "mesh_basic.frag.glsl").string()); + // Lit shader with simple Blinn-Phong lighting + _shaderLit = std::make_shared(); + _shaderLit->load((paths::assets() / "shaders" / "vs_pbr.vert.glsl").string(), (paths::assets() / "shaders" / "mesh_lit.frag.glsl").string()); + // PBR shader with full Physically Based Rendering (for release visuals) + _shaderPBR = std::make_shared(); + _shaderPBR->load((paths::assets() / "shaders" / "vs_pbr.vert.glsl").string(), (paths::assets() / "shaders" / "mesh_pbr.frag.glsl").string()); + + currentShader = _shaderPBR.get(); + _skybox = std::make_unique(); + + // Shader Types B + _worldGridShader = std::make_unique(); + _worldGridShader->load((paths::assets() / "shaders" / "world_grid.vert.glsl").string(), (paths::assets() / "shaders" / "world_grid.frag.glsl").string()); + + _shadowShader = std::make_unique(); + _shadowShader->load((paths::assets() / "shaders" / "shadow_depth.vert.glsl").string(), (paths::assets() / "shaders" / "shadow_depth.frag.glsl").string()); + + // Light + _light = std::make_unique(); + _light->_isDirectional = true; + + // Axis Orientator + _axisOrientator = std::make_unique(); + + // World Grid VAO + glGenVertexArrays(1, &_worldGridVAO); + + // Test Mesh + _mesh = std::make_shared(); + _mesh->init(); + + // Robot system with mesh loading (for normal simulation) + _robotSystem = std::make_unique(); + + _robotRenderer = std::make_unique(); + + // SSAO shaders + _ssaoShader = std::make_unique(); + _ssaoShader->load((paths::assets() / "shaders" / "post.vert.glsl").string(), (paths::assets() / "shaders" / "ssao.frag.glsl").string()); + + _ssaoBlurShader = std::make_unique(); + _ssaoBlurShader->load((paths::assets() / "shaders" / "post.vert.glsl").string(), (paths::assets() / "shaders" / "ssao_blur.frag.glsl").string()); + + // Generate hemisphere kernel + initSSAOKernel(64); + + // Generate noise texture (4x4 random rotation vectors) + initSSAONoise(); + } + + void clearRobotPresentation() { + _primaryLinkObject.clear(); + _linkToObjects.clear(); + _objects.clear(); + } + + scene::Object* primaryObjectForLink(const std::string& linkName) { + auto it = _primaryLinkObject.find(linkName); + if (it == _primaryLinkObject.end()) { return nullptr; } + return it->second; + } + + void buildRobotPresentationFromModel(const robots::RobotModel& model, SimManager& owner) { + clearRobotPresentation(); + + RobotPresentationBuilder builder; + RobotRenderBinding binding = builder.build(model); + + for (auto& owned : binding.ownedObjects) { + _objects.push_back(std::move(owned)); + } + + _robotRenderer->bind(binding); + + for (const auto& [linkName, visuals] : binding.linkVisuals) { + auto& target = _linkToObjects[linkName]; + + for (auto* obj : visuals) { + if (!obj) { continue; } + + target.push_back(obj); + + if (!_primaryLinkObject.contains(linkName)) { + _primaryLinkObject[linkName] = obj; + } + } + } + } + + // Random number generation for SSAO kernel and noise + std::mt19937 _rng{ std::random_device{}() }; + std::uniform_real_distribution _uni{ -1.0f, 1.0f }; + + // Helper to get random float in [a,b] + inline float rngFloat(float a, float b) { + std::uniform_real_distribution d(a, b); + return d(_rng); + } + + // Initialises the SSAO kernel with random samples in a hemisphere oriented along the positive Z axis, scaled to favor samples closer to the origin + void initSSAOKernel(int size) { + _ssaoKernel.clear(); + _ssaoKernel.reserve(size); + for (int i = 0; i < size; ++i) { + glm::vec3 sample( + rngFloat(-1.0f, 1.0f), + rngFloat(-1.0f, 1.0f), + rngFloat(0.0f, 1.0f) // hemisphere: z in [0,1] + ); + sample = glm::normalize(sample); + sample *= rngFloat(0.0f, 1.0f); + float scale = (float)i / (float)size; + scale = 0.1f + scale * scale * 0.9f; + sample *= scale; + _ssaoKernel.push_back(sample); + } + } + + // Initializes the SSAO noise texture with random rotation vectors in the XY plane + void initSSAONoise() { + std::vector noise(16); + // Generate 16 random rotation vectors in the XY plane (Z=0) + for (int i = 0; i < 16; ++i) { + noise[i] = glm::vec3( + rngFloat(-1.0f, 1.0f), + rngFloat(-1.0f, 1.0f), + 0.0f + ); + } + // Create OpenGL texture + glGenTextures(1, &_ssaoNoiseTex); + glBindTexture(GL_TEXTURE_2D, _ssaoNoiseTex); + glTexImage2D(GL_TEXTURE_2D, 0, GL_RGBA16F, 4, 4, 0, GL_RGB, GL_FLOAT, noise.data()); + glTexParameteri(GL_TEXTURE_2D, GL_TEXTURE_MIN_FILTER, GL_NEAREST); + glTexParameteri(GL_TEXTURE_2D, GL_TEXTURE_MAG_FILTER, GL_NEAREST); + glTexParameteri(GL_TEXTURE_2D, GL_TEXTURE_WRAP_S, GL_REPEAT); + glTexParameteri(GL_TEXTURE_2D, GL_TEXTURE_WRAP_T, GL_REPEAT); + } + + // Ensures SSAO FBOs and textures are created and match the given size, recreating them if necessary + void ensureSSAOBuffers(int w, int h) { + if (_ssaoW == w && _ssaoH == h) return; + _ssaoW = w; _ssaoH = h; + // Delete old buffers if they exist + if (_ssaoFBO) { glDeleteFramebuffers(1, &_ssaoFBO); glDeleteTextures(1, &_ssaoTex); } + if (_ssaoBlurFBO) { glDeleteFramebuffers(1, &_ssaoBlurFBO); glDeleteTextures(1, &_ssaoBlurTex); } + // Lambda to create a single-channel floating point FBO and texture + auto makeSingleChannelFBO = [](GLuint& fbo, GLuint& tex, int w, int h) { + glGenFramebuffers(1, &fbo); + glGenTextures(1, &tex); + glBindFramebuffer(GL_FRAMEBUFFER, fbo); + glBindTexture(GL_TEXTURE_2D, tex); + glTexImage2D(GL_TEXTURE_2D, 0, GL_R32F, w, h, 0, GL_RED, GL_FLOAT, nullptr); + glTexParameteri(GL_TEXTURE_2D, GL_TEXTURE_MIN_FILTER, GL_NEAREST); + glTexParameteri(GL_TEXTURE_2D, GL_TEXTURE_MAG_FILTER, GL_NEAREST); + glTexParameteri(GL_TEXTURE_2D, GL_TEXTURE_WRAP_S, GL_CLAMP_TO_EDGE); + glTexParameteri(GL_TEXTURE_2D, GL_TEXTURE_WRAP_T, GL_CLAMP_TO_EDGE); + glFramebufferTexture2D(GL_FRAMEBUFFER, GL_COLOR_ATTACHMENT0, GL_TEXTURE_2D, tex, 0); + glBindFramebuffer(GL_FRAMEBUFFER, 0); + }; + // Create SSAO and blur FBOs/textures + makeSingleChannelFBO(_ssaoFBO, _ssaoTex, w, h); + makeSingleChannelFBO(_ssaoBlurFBO, _ssaoBlurTex, w, h); + } + + // Renders the SSAO pass and subsequent blur pass, writing results to SSAO FBOs. Should be called after rendering the scene to the view's framebuffer (to provide depth texture input). + void renderSSAO(SimManager& owner, Viewport& v) { + if (!owner._settingsCurrent.ssao) return; + // Determine SSAO buffer size based on division factor + int ssaoDiv = std::max(1, owner._settingsCurrent.ssaoResDiv); + int ssaoW = std::max(1, v.w / ssaoDiv); + int ssaoH = std::max(1, v.h / ssaoDiv); + ensureSSAOBuffers(ssaoW, ssaoH); + // Clamp kernel size to available samples + int kernelSize = std::min((int)_ssaoKernel.size(), owner._settingsCurrent.ssaoSamples); + // SSAO Pass + glBindFramebuffer(GL_FRAMEBUFFER, _ssaoFBO); + glViewport(0, 0, ssaoW, ssaoH); + glClear(GL_COLOR_BUFFER_BIT); + glDisable(GL_DEPTH_TEST); + glDisable(GL_BLEND); + + _ssaoShader->use(); + + // Depth texture from the resolved scene FBO + glActiveTexture(GL_TEXTURE0); + glBindTexture(GL_TEXTURE_2D, v.fb->getDepthTexture()); + _ssaoShader->setInt1(0, "gDepth"); + // Noise texture + glActiveTexture(GL_TEXTURE1); + glBindTexture(GL_TEXTURE_2D, _ssaoNoiseTex); + _ssaoShader->setInt1(1, "gNoise"); + + // Camera matrices and parameters + _ssaoShader->setMat4(v.cam->getProjection(), "projection"); + _ssaoShader->setMat4(glm::inverse(v.cam->getProjection()), "invProjection"); + _ssaoShader->setVec2(glm::vec2((float)ssaoW / 4.0f, (float)ssaoH / 4.0f), "noiseScale"); + _ssaoShader->setInt1(kernelSize, "kernelSize"); + _ssaoShader->setFlt1(0.25f, "radius"); + _ssaoShader->setFlt1(0.035f, "bias"); + _ssaoShader->setFlt1(owner._settingsCurrent.ssaoStrength, "strength"); + + // SSAO kernel samples + for (int i = 0; i < kernelSize; ++i) { _ssaoShader->setVec3(_ssaoKernel[i], "samples[" + std::to_string(i) + "]"); } + + glBindVertexArray(_fullscreenVAO); + glDrawArrays(GL_TRIANGLES, 0, 3); + + // Blur pass + glBindFramebuffer(GL_FRAMEBUFFER, _ssaoBlurFBO); + glViewport(0, 0, ssaoW, ssaoH); + glClear(GL_COLOR_BUFFER_BIT); + + // No need for depth test or blending for a simple fullscreen blur + _ssaoBlurShader->use(); + glActiveTexture(GL_TEXTURE0); + glBindTexture(GL_TEXTURE_2D, _ssaoTex); + _ssaoBlurShader->setInt1(0, "ssaoInput"); + glDrawArrays(GL_TRIANGLES, 0, 3); + glBindVertexArray(0); + glBindFramebuffer(GL_FRAMEBUFFER, 0); + } + + // Renders the given viewport to its framebuffer, handling dynamic resizing of the framebuffer and camera aspect ratio based on the provided display size + void renderView(SimManager& owner, Viewport& v, int displayW, int displayH) { + displayW = std::max(1, displayW); + displayH = std::max(1, displayH); + + // Internal render target size + glm::ivec2 rt(std::max(1, (int)owner._internalSize.x), std::max(1, (int)owner._internalSize.y)); + + // In quad mode, render at the cell's display resolution to avoid scaling artifacts + if (owner._impl->viewMode == Impl::ViewMode::Quad) { + rt.x = std::max(1, displayW); + rt.y = std::max(1, displayH); + } + const int rtW = std::max(1, (int)rt.x); + const int rtH = std::max(1, (int)rt.y); + + LOG_INFO_ONCE("Rendering Viewport: RT Size = %dx%d, Display Size = %dx%d", rtW, rtH, displayW, displayH); + + // Resize only when internal RT changes OR display changes (post buffer) + const bool rtChanged = (v.w != rtW) || (v.h != rtH); + const bool displayChanged = (v.displayW != displayW) || (v.displayH != displayH); + const bool msaaChanged = false; + + if (rtChanged || displayChanged || msaaChanged) { + // Store internal RT size + v.w = rtW; + v.h = rtH; + + // Store display size + v.displayW = displayW; + v.displayH = displayH; + + // Adjust MSAA based on internal RT size + int msaa = std::max(1, owner._settingsCurrent.msaaSamples); + + // Scene framebuffer: INTERNAL resolution (rtW x rtH) + v.fb->deleteBuffers(); + v.fb->createBuffers(rtW, rtH, msaa); + + // Post-processing framebuffer: DISPLAY resolution (displayW x displayH) + v.post->deleteBuffers(); + v.post->createBuffers(displayW, displayH, 1); + + // Camera aspect must match DISPLAY aspect + v.cam->setAspect((float)displayW / (float)displayH); + } + + v.fb->bind(); + glViewport(0, 0, v.w, v.h); + glEnable(GL_DEPTH_TEST); + glDepthMask(GL_TRUE); + glDepthFunc(GL_LESS); + + glClearColor(owner._backgroundColour.r, owner._backgroundColour.g, owner._backgroundColour.b, owner._backgroundAlpha); + glClear(GL_COLOR_BUFFER_BIT | GL_DEPTH_BUFFER_BIT); + + if (owner._settingsCurrent.msaaSamples > 1) { + glEnable(GL_MULTISAMPLE); + } + else { + glDisable(GL_MULTISAMPLE); + } + + // Update Follow Target: use the explicitly bound target (e.g. end-effector from loadRobot), + // only fall back to _selectedObject if no explicit target was set via setViewFollowTarget + if (&v == &_views[(size_t)gui::ViewID::Follow]) { + scene::Object* target = v.followEnabled ? v.followTarget : _selectedObject; + + if (target && target->getMesh()) { + glm::mat4 M = target->transform.toMatrix() * target->getMesh()->localTransform; + glm::vec3 worldPos = glm::vec3(M[3]); + v.cam->setFollowTarget(worldPos, target->transform.rotQ); + } + } + + // Skybox (renders before all opaque geometry, doesn't write depth) + if (owner.skyboxEnabled) { + glDepthMask(GL_FALSE); + glDepthFunc(GL_LEQUAL); + owner.SkyboxRender(v.cam.get()); + glDepthMask(GL_TRUE); + glDepthFunc(GL_LESS); + } + + // Meshes & World Grid + GLint sampleBuffers = 0, samples = 0; + glGetIntegerv(GL_SAMPLE_BUFFERS, &sampleBuffers); + glGetIntegerv(GL_SAMPLES, &samples); + LOG_INFO_ONCE("FB MSAA state: GL_SAMPLE_BUFFERS=%d GL_SAMPLES=%d", sampleBuffers, samples); + + owner.MeshRender(v.cam.get()); + if (owner._settingsCurrent.grid) { owner.WorldGridRender(v.cam.get(), v.w); } + + v.fb->unbind(); + + // SSAO pass (reads resolved depth, writes to _ssaoBlurTex) + renderSSAO(owner, v); + + // Only valid if you allocated mip levels for _texID (via glTexStorage2D) + glBindTexture(GL_TEXTURE_2D, v.fb->getTexture()); + glGenerateMipmap(GL_TEXTURE_2D); + glBindTexture(GL_TEXTURE_2D, 0); + + // Post-Processing + v.post->bind(); + glDisable(GL_MULTISAMPLE); + glViewport(0, 0, v.displayW, v.displayH); + + glDisable(GL_DEPTH_TEST); + glDisable(GL_BLEND); + glClear(GL_COLOR_BUFFER_BIT); + + _postShader->use(); + _postShader->setInt1(0, "hdrScene"); + _postShader->setInt1(1, "ssaoTex"); + _postShader->setBool(owner._settingsCurrent.ssao, "ssaoEnabled"); + _postShader->setFlt1(owner._settingsCurrent.exposure, "exposure"); + _postShader->setFlt1(owner._settingsCurrent.whitePoint, "whitePoint"); + _postShader->setVec2(glm::vec2(v.w, v.h), "uRes"); + + glActiveTexture(GL_TEXTURE0); + glBindTexture(GL_TEXTURE_2D, v.fb->getTexture()); + + glActiveTexture(GL_TEXTURE1); + glBindTexture(GL_TEXTURE_2D, owner._settingsCurrent.ssao ? _ssaoBlurTex : 0); + + glBindVertexArray(_fullscreenVAO); + glDrawArrays(GL_TRIANGLES, 0, 3); + if (owner._settingsCurrent.axisOrientator) { _axisOrientator->render(v.cam->getViewMatrix()); } + glBindVertexArray(0); + + v.post->unbind(); + } + + static bool icontains(const std::string& s, const char* sub) { + if (sub == nullptr || *sub == '\0') { return false; } + + // Case-insensitive search using std::search with a custom comparator + auto it = std::search( + s.begin(), s.end(), + sub, sub + std::strlen(sub), + [](char a, char b) { + return std::tolower((unsigned char)a) == std::tolower((unsigned char)b); + } + ); + return it != s.end(); + } + + scene::Object* findEndEffectorFromRange(size_t startIdx) { + for (size_t i = startIdx; i < _objects.size(); ++i) { + scene::Object* o = _objects[i].get(); + if (!o) continue; + + const std::string& n = o->name; + if (icontains(n, "end") || icontains(n, "eff") || icontains(n, "ee") || icontains(n, "tool") || icontains(n, "tcp") || icontains(n, "gripper")) { + return o; + } + } + + // Fallback: last object added (usually the last link) + if (_objects.size() > startIdx) return _objects.back().get(); + return nullptr; + } + }; +} \ No newline at end of file diff --git a/DSFE_App/DSFE_GUI/include/Scene/SimulationManager.h b/DSFE_App/DSFE_GUI/include/Scene/SimulationManager.h index 923b47db..b0dda2f2 100644 --- a/DSFE_App/DSFE_GUI/include/Scene/SimulationManager.h +++ b/DSFE_App/DSFE_GUI/include/Scene/SimulationManager.h @@ -161,6 +161,7 @@ namespace gui { // Rendering Entry Points void render(); + void tick(double dt); void resize(int32_t width, int32_t height); void syncRobotToScene(); diff --git a/DSFE_App/DSFE_GUI/src/Scene/Manager/SimBackend.cpp b/DSFE_App/DSFE_GUI/src/Scene/Manager/SimBackend.cpp new file mode 100644 index 00000000..c746a575 --- /dev/null +++ b/DSFE_App/DSFE_GUI/src/Scene/Manager/SimBackend.cpp @@ -0,0 +1,112 @@ +// DSFE_GUI SimBackend.cpp +#include "Scene/SimulationManager.h" +#include "Scene/SimulationCore.h" +#ifdef __gl_h_ +#undef __gl_h_ +#endif +#include "Manager/SimImplementation.h" + +#include "Interpreter/IStoredProgram.h" +#include "Interpreter/StoredProgram.h" +#include "Interpreter/Parser.h" + +namespace gui { + // Start the simulation + void SimManager::startSimulation() { + if (!hasRobot()) { + LOG_WARN("Cannot start simulation: no robot loaded"); + return; + } + if (hasRobot() && !_bodyLoaded) { + loadRobot(_impl->_robotSystem->robotName()); + return; + } + _core->startSimulation(); + } + + // Stop the simulation + void SimManager::stopSimulation() { _core->stopSimulation(); } + + // Check if the simulation is currently running + const bool SimManager::isSimRunning() const { return _core->isSimRunning(); } + + // Setter for current simulation time (in seconds) + void SimManager::setSimTime(double time) { _core->setSimTime(time); } + const double SimManager::simTime() const { return _core->simTime(); } + + // Setter and gettter for fixed timstep (in seconds) + void SimManager::setFixedDt(double dt) { _core->setFixedDt(dt); } + const double SimManager::fixedDt() const { return _core->fixedDt(); } + + // Setter and getter for telemetry frequency (in Hz) + void SimManager::setTelemetryHz(double hz) { _core->setTelemetryHz(hz); } + const double SimManager::telemetryHz() const { return _core->telemetryHz(); } + + // Set whether a script is currently running (used to disable UI elements, etc.) + void SimManager::setScriptRunning(bool running) { _core->setScriptRunning(running); } + const bool SimManager::isScriptRunning() const { return _core->isScriptRunning(); } + + // Setters and getters for last script text + void SimManager::setLastScriptText(const std::string& text) { _core->setLastScriptText(text); } + const std::string& SimManager::lastScriptText() const { return _core->lastScriptText(); } + + // Accessors for the Simulation Core's telemetry data + diagnostics::TelemetryRecorder& SimManager::telemetry() { return _core->telemetry(); } + const diagnostics::TelemetryRecorder& SimManager::telemetry() const { return _core->telemetry(); } + + // Accesors for the active program (if any) + void SimManager::setActiveProgram(interpreter::IStoredProgram* program) { _core->setActiveProgram(program); } + interpreter::IStoredProgram* SimManager::activeProgram() { return _core->activeProgram(); } + const interpreter::IStoredProgram* SimManager::activeProgram() const { return _core->activeProgram(); } + + // Access the simulation core interface (non-const and const versions) + core::ISimulationCore* SimManager::simCoreInterface() { return _core.get(); } + const core::ISimulationCore* SimManager::simCoreInterface() const { return _core.get(); } + + // Access the concrete simulation core (non-const and const versions) + core::SimulationCore* SimManager::simCore() { return _core.get(); } + const core::SimulationCore* SimManager::simCore() const { return _core.get(); } + + // Set the integrator method for the current simulation run + void SimManager::setIntegrationMethod(integration::eIntegrationMethod method) { + _core->setIntegrationMethod(method); + } + const integration::eIntegrationMethod SimManager::integrationMethod() const { + return _core->integrationMethod(); + } + void SimManager::setADIntegrationMethod(integration::eAutoDiffIntegrationMethod method) { + _core->setADIntegrationMethod(method); + } + const integration::eAutoDiffIntegrationMethod SimManager::autoDiffIntegrationMethod() const { + return _core->autoDiffIntegrationMethod(); + } + + // This seems to be the better solution? + static std::string replaceIntegratorInScript(const std::string& script, const std::string& methodName) { + std::regex re(R"((?i)set\s*\(\s*integrator\s*,\s*([a-z0-9_]+)\s*\))"); // case-insensitive regex to match my DSL command -> set(integrator, method) + std::string replacement = "set(integrator, " + methodName + ")"; + return std::regex_replace(script, re, replacement); + } + + // Run a script to completion synchronously with a specific integrator + bool SimManager::runScriptToCompletion(const std::string& scriptText, integration::eIntegrationMethod method) { + if (!hasRobot()) { return false; } + + // Map method enum to string name + static const char* names[] = { "euler", "midpoint", "heun", "ralston", "rk4", "rk45", "implicit_euler", "implicit_midpoint", "glrk2", "glrk3" }; + const std::string methodName = names[static_cast(method)]; + + // Replace the integrator method in the script text + std::string modifiedScript = replaceIntegratorInScript(scriptText, methodName); + + // Create program and parser (bound to headless core) + auto program = std::make_unique(_core.get()); + //if (scene::Object* o = getObject()) program->setDefaultObject(o); + auto parser = std::make_unique(program.get()); + + // Parse the modified script and start the program + parser->parse(modifiedScript); + program->start(); + return _core->runScriptToCompletion(program.get(), method); // this will block until the script finishes + } +} \ No newline at end of file diff --git a/DSFE_App/DSFE_GUI/src/Scene/Manager/SimCamera.cpp b/DSFE_App/DSFE_GUI/src/Scene/Manager/SimCamera.cpp new file mode 100644 index 00000000..c61cd927 --- /dev/null +++ b/DSFE_App/DSFE_GUI/src/Scene/Manager/SimCamera.cpp @@ -0,0 +1,143 @@ +// DSFE_GUI SimCamera.cpp +#include "Scene/SimulationManager.h" +#ifdef __gl_h_ +#undef __gl_h_ +#endif +#include "Manager/SimImplementation.h" + +namespace gui { + // Get the active view camera + scene::Camera* SimManager::getCamera() { return _impl->_views[static_cast(_impl->activeView)].cam.get(); } + // Reset the active view camera to default position + void SimManager::resetView() { + auto& v = _impl->_views[static_cast(_impl->activeView)]; + + glm::vec3 pos = { 0.0f, 0.25f, 1.0f }; + float fov = 60.0f; + + switch (_impl->activeView) { + case gui::ViewID::Top: pos = { 0, 5, 0 }; fov = 20.0f; break; + case gui::ViewID::Right: pos = { 5, 0, 0 }; fov = 20.0f; break; + case gui::ViewID::Front: pos = { 0, 0, 5 }; fov = 20.0f; break; + case gui::ViewID::Follow: fov = 20.0f; break; + case gui::ViewID::Manual: fov = 20.0f; break; + default: break; + } + + float aspect = (float)std::max(1, v.w) / (float)std::max(1, v.h); + v.cam = std::make_unique(pos, fov, aspect, 0.1f, 5000.0f); + + v.cam->setFocus(glm::vec3(0.0f)); + + switch (_impl->activeView) { + case gui::ViewID::Top: + v.cam->setYaw(-glm::half_pi()); + v.cam->setPitch(-glm::half_pi() + 0.001f); + break; + case gui::ViewID::Right: + v.cam->setYaw(glm::pi()); + v.cam->setPitch(0.0f); + break; + case gui::ViewID::Front: + v.cam->setYaw(-glm::half_pi()); + v.cam->setPitch(0.0f); + break; + default: + break; + } + + v.cam->updateViewMatrix(); + } + + // Attach camera to an object and start following it. The camera will maintain a fixed offset from the object's position and orientation. + void SimManager::attachCameraToObject(scene::Object* obj) { + if (!obj) return; + scene::Camera* cam = _impl->_views[static_cast(_impl->activeView)].cam.get(); + + _impl->_cameraFollowTarget = obj; + + glm::vec3 pos = obj->transform.position; + glm::quat rot = obj->transform.rotQ; + + cam->startFollow(pos, rot, glm::vec3(0, 2, 5)); + } + + // Detach camera from any object and stop following + void SimManager::detachCameraFromObject() { + scene::Camera* cam = _impl->_views[static_cast(_impl->activeView)].cam.get(); + _impl->_cameraFollowTarget = nullptr; + cam->clearFollow(); + } + + // Set the follow target for a specific view + void SimManager::setViewFollowTarget(ViewID view, scene::Object* obj, const glm::vec3& offset) { + if (view < ViewID::Manual || view >= ViewID::COUNT) { return; } + auto& v = _impl->_views[static_cast(view)]; + v.followTarget = obj; + v.followOffset = offset; + v.followEnabled = (obj != nullptr); + + if (v.followEnabled && v.cam && obj) { + v.cam->startFollow(obj->transform.position, obj->transform.rotQ, offset); + } + } + + // Clear the follow target for a specific view + void SimManager::clearViewFollowTarget(ViewID view) { + if (view < ViewID::Manual || view >= ViewID::COUNT) { return; } + auto& v = _impl->_views[static_cast(view)]; + v.followTarget = nullptr; + v.followEnabled = false; + if (v.cam) { v.cam->clearFollow(); } + } + + // Convenience for Follow view: set the follow target to the object attached to a robot joint (e.g. end-effector) + bool SimManager::setViewFollowRobotJoint(ViewID view, const std::string& jointName, const glm::vec3& offset) { + if (!hasRobot()) { + LOG_WARN("setViewFollowRobotJoint: no robot loaded"); + return false; + } + + robots::RobotSystem* rs = robotSystem(); + if (!rs) return false; + + auto& joints = rs->joints(); + auto& links = rs->links(); + + // 1) Find joint by name + const robots::RobotJoint* jPtr = nullptr; + for (auto& j : joints) { + if (j.name == jointName) { jPtr = &j; break; } + } + if (!jPtr) { + LOG_WARN("setViewFollowRobotJoint: joint not found: %s", jointName.c_str()); + return false; + } + + // 2) Find child link -> attached object + scene::Object* targetObj = nullptr; + if (auto it = _impl->_primaryLinkObject.find(jPtr->child); + it != _impl->_primaryLinkObject.end()) { + targetObj = it->second; + } + + if (!targetObj) { + LOG_WARN("setViewFollowRobotJoint: no attached object for joint=%s child=%s", + jointName.c_str(), jPtr->child.c_str()); + return false; + } + + // 3) Bind the view follow target + setViewFollowTarget(view, targetObj, offset); + + /*LOG_INFO("Follow view=%d bound to joint='%s' -> child='%s' -> obj='%s'", + (int)view, jointName.c_str(), jPtr->child.c_str(), targetObj->name.c_str());*/ + + return true; + } + + // Convenience for Follow view + bool SimManager::followRobotJoint(const std::string& jointName, const glm::vec3& offset) { + return setViewFollowRobotJoint(gui::ViewID::Follow, jointName, offset); + } +} \ No newline at end of file diff --git a/DSFE_App/DSFE_GUI/src/Scene/Manager/SimObjects.cpp b/DSFE_App/DSFE_GUI/src/Scene/Manager/SimObjects.cpp new file mode 100644 index 00000000..6335f9c2 --- /dev/null +++ b/DSFE_App/DSFE_GUI/src/Scene/Manager/SimObjects.cpp @@ -0,0 +1,110 @@ +// DSFE_GUI SimObjects.cpp +#include "Scene/SimulationManager.h" +#ifdef __gl_h_ +#undef __gl_h_ +#endif +#include "Manager/SimImplementation.h" + +namespace gui { + // Helper to get the next ObjectID + static inline scene::ObjectID next(scene::ObjectID id) { + return static_cast(static_cast(id) + 1); + } + + // Load a mesh from file and create one Object per submesh. The last loaded mesh becomes the active selection. + void SimManager::loadMesh(const std::string& filepath) { + assets::MeshLoader loader; + auto meshes = loader.load(filepath); + + if (meshes.empty()) { + LOG_WARN("No meshes imported from %s", filepath.c_str()); + D_WARN("No meshes imported from %s", filepath.c_str()); + return; + } + + // For now: spawn one Object per submesh + for (auto& m : meshes) { + auto obj = std::make_unique(m); + obj->id = next(_nextObjectID); + obj->source.filename = filepath; + obj->name = m->getName().empty() ? "Object_" + std::to_string(scene::toUInt32(obj->id)) : m->getName(); + + // initialise physics state + obj->state.q = Quat(1.0, 0.0, 0.0, 0.0); + obj->state.angularVelocity = Vec3::Zero(); + obj->state.linearVelocity = Vec3::Zero(); + obj->state.mass = 1.0; + obj->state.damping = 0.0; + obj->state.inertia = Mat3::Identity(); + obj->state.forces = Vec3::Zero(); + obj->state.torques = Vec3::Zero(); + + _impl->_selectedObject = obj.get(); + _impl->_objects.push_back(std::move(obj)); + } + + LOG_INFO("Loaded %zu submeshes from %s", meshes.size(), filepath.c_str()); + D_INFO("Loaded %zu submeshes from %s", meshes.size(), filepath.c_str()); + } + + // Returns the loaded objects so they can be used as targets for robot joints in the same frame (e.g. end-effector) + std::vector SimManager::loadMeshReturn(const std::string& filepath) { + assets::MeshLoader loader; + auto meshes = loader.load(filepath); + std::vector result; + for (auto& m : meshes) { + auto obj = std::make_unique(m); + auto raw = obj.get(); + raw->internal = true; + _impl->_objects.push_back(std::move(obj)); + result.push_back(raw); + } + return result; + } + + // Setter and Getter for the active mesh + void SimManager::setMesh(std::shared_ptr mesh) { _impl->_mesh = mesh; } + std::shared_ptr SimManager::getMesh() { return _impl->_mesh; } + + // Set the currently selected object (can be nullptr to deselect) + void SimManager::setSelectedObject(scene::Object* obj) { _impl->_selectedObject = obj; } + // Add a new object to the scene and select it + void SimManager::addObject(std::unique_ptr obj) { _impl->_objects.push_back(std::move(obj)); } // Cache the unique_ptr + + // Remove an object by index + void SimManager::deleteObject(int index) { + if (index < 0 || index >= _impl->_objects.size()) { return; } + if (_impl->_selectedObject == _impl->_objects[index].get()) { _impl->_selectedObject = nullptr; } + _impl->_objects.erase(_impl->_objects.begin() + index); + } + + // Remove an object by pointer + void SimManager::removeObject(scene::Object* obj) { + if (!obj) return; + + auto it = std::remove_if( + _impl->_objects.begin(), + _impl->_objects.end(), + [obj](const std::unique_ptr& o) { + return o.get() == obj; + } + ); + + _impl->_objects.erase(it, _impl->_objects.end()); + } + + // Access the objects as raw pointers for use in the rest of the codebase, while maintaining ownership in SimManager + std::vector>& SimManager::getObjects() { return _impl->_objects; } + // Get the currently selected object (can be nullptr) + scene::Object* SimManager::getObject() { return _impl->_selectedObject; } + + // Helper to find an object by its ID (returns nullptr if not found) + scene::Object* SimManager::getObjectByID(scene::ObjectID id) { + for (auto& obj : _impl->_objects) { + if (obj && obj->id == id) { + return obj.get(); + } + } + return nullptr; + } +} \ No newline at end of file diff --git a/DSFE_App/DSFE_GUI/src/Scene/Manager/SimPostFX.cpp b/DSFE_App/DSFE_GUI/src/Scene/Manager/SimPostFX.cpp new file mode 100644 index 00000000..94109812 --- /dev/null +++ b/DSFE_App/DSFE_GUI/src/Scene/Manager/SimPostFX.cpp @@ -0,0 +1,10 @@ +// DSFE_GUI SimPostFX.cpp +#include "Scene/SimulationManager.h" +#ifdef __gl_h_ +#undef __gl_h_ +#endif +#include "Manager/SimImplementation.h" + +namespace gui { + +} \ No newline at end of file diff --git a/DSFE_App/DSFE_GUI/src/Scene/Manager/SimRendering.cpp b/DSFE_App/DSFE_GUI/src/Scene/Manager/SimRendering.cpp new file mode 100644 index 00000000..2e3dda89 --- /dev/null +++ b/DSFE_App/DSFE_GUI/src/Scene/Manager/SimRendering.cpp @@ -0,0 +1,433 @@ +// DSFE_GUI SimRendering.cpp +#include "Scene/Object.h" +#include "Scene/SimulationManager.h" +#ifdef __gl_h_ +#undef __gl_h_ +#endif +#include "Manager/SimImplementation.h" + +namespace gui { + scene::Light* SimManager::getLight() { return _impl->_light.get(); } + + void SimManager::loadNewHDR(const std::string& path) { + D_INFO("Loading new HDR: %s", path.c_str()); + + // Make sure IBL system exists + if (!_impl->_ibl) { + LOG_ERROR("Cannot load HDR because IBL system is not initialised."); + D_FAIL("Cannot load HDR because IBL system is not initialised."); + return; + } + + _impl->_ibl->init(path); // rebuild envCubemap, irradiance, prefilter, brdfLUT + D_SUCCESS("IBL rebuilt successfully."); + + // Update skybox + _impl->_skybox->setEnvironmentTexture(_impl->_ibl->getEnvCubemap()); + + _activeHDRPath = path; + + D_SUCCESS("Loaded HDR successfully."); + } + + void SimManager::loadNewHDR_UI(const std::string& path) { + loadNewHDR(path); + _hdrUserOverride = true; + } + + void SimManager::loadNewHDR_Preset(const std::string& path) { + loadNewHDR(path); + _hdrUserOverride = false; + } + + // Initialize shadow map resources for cascaded shadow mapping + void SimManager::InitShadowResource(int baseRes) { + if (_shadowsInit) { + glDeleteFramebuffers(SimManager::NUM_CASCADES, _impl->_cascadeFBO); + glDeleteTextures(SimManager::NUM_CASCADES, _impl->_cascadeDepth); + } + + glGenFramebuffers(SimManager::NUM_CASCADES, _impl->_cascadeFBO); + glGenTextures(SimManager::NUM_CASCADES, _impl->_cascadeDepth); + + for (int i = 0; i < SimManager::NUM_CASCADES; i++) { + const int res = (i == 0) ? baseRes : (baseRes / 2); // 8192, 4096, 2048, 1024, 512, 256, 128 + + glBindTexture(GL_TEXTURE_2D, _impl->_cascadeDepth[i]); + glTexImage2D(GL_TEXTURE_2D, 0, GL_DEPTH_COMPONENT32F, res, res, 0, GL_DEPTH_COMPONENT, GL_FLOAT, nullptr); + + glTexParameteri(GL_TEXTURE_2D, GL_TEXTURE_MIN_FILTER, GL_NEAREST); + glTexParameteri(GL_TEXTURE_2D, GL_TEXTURE_MAG_FILTER, GL_NEAREST); + glTexParameteri(GL_TEXTURE_2D, GL_TEXTURE_WRAP_S, GL_CLAMP_TO_BORDER); + glTexParameteri(GL_TEXTURE_2D, GL_TEXTURE_WRAP_T, GL_CLAMP_TO_BORDER); + const float border[] = { 1.0f, 1.0f, 1.0f, 1.0f }; + glTexParameterfv(GL_TEXTURE_2D, GL_TEXTURE_BORDER_COLOR, border); + + glTexParameteri(GL_TEXTURE_2D, GL_TEXTURE_COMPARE_MODE, GL_COMPARE_REF_TO_TEXTURE); + glTexParameteri(GL_TEXTURE_2D, GL_TEXTURE_COMPARE_FUNC, GL_LEQUAL); + + glBindFramebuffer(GL_FRAMEBUFFER, _impl->_cascadeFBO[i]); + glFramebufferTexture2D(GL_FRAMEBUFFER, GL_DEPTH_ATTACHMENT, GL_TEXTURE_2D, _impl->_cascadeDepth[i], 0); + + glDrawBuffer(GL_NONE); + glReadBuffer(GL_NONE); + } + glBindFramebuffer(GL_FRAMEBUFFER, 0); + + _shadowsInit = true; + } + + // Initialise the IBL system with a default HDR environment map + void SimManager::InitIBL() { + _impl->_ibl = std::make_unique(); + _impl->_ibl->init((paths::assets() / "hdr" / "default_white.hdr").string()); + } + + // Render the world grid overlay in the viewport + void SimManager::WorldGridRender(scene::Camera* cam, int rtW) { + glEnable(GL_DEPTH_TEST); + glDepthFunc(GL_LEQUAL); + glDepthMask(GL_FALSE); + + const int msaa = std::max(1, _settingsCurrent.msaaSamples); + + if (msaa > 1) { + glDisable(GL_BLEND); + glEnable(GL_SAMPLE_ALPHA_TO_COVERAGE); + glEnable(GL_MULTISAMPLE); + + glEnable(GL_POLYGON_OFFSET_FILL); + glPolygonOffset(-0.2f, -0.2f); + } + else { + glDisable(GL_SAMPLE_ALPHA_TO_COVERAGE); + glDisable(GL_MULTISAMPLE); + glEnable(GL_BLEND); + glBlendFunc(GL_ONE, GL_ONE_MINUS_SRC_ALPHA); + } + + // Scale grid line thickness so smaller RTs (quad mode) match single view appearance + const float fullW = std::max(1.0f, _internalSize.x); + const float internalScale = (float)std::max(1, rtW) / fullW; + + _impl->_worldGridShader->use(); + _impl->_worldGridShader->setMat4(cam->getViewProjection(), "gVP"); + _impl->_worldGridShader->setMat4(cam->getViewMatrix(), "gView"); + _impl->_worldGridShader->setVec3(cam->getPosition(), "gCameraWorldPos"); + _impl->_worldGridShader->setFlt1(_settingsCurrent.renderScale, "gRenderScale"); + _impl->_worldGridShader->setFlt1(internalScale, "gInternalScale"); + _impl->_worldGridShader->setFlt1(2500.0f, "gGridSize"); + + glBindVertexArray(_impl->_worldGridVAO); + glDrawArrays(GL_TRIANGLES, 0, 6); + glBindVertexArray(0); + + glDisable(GL_POLYGON_OFFSET_FILL); + glDisable(GL_SAMPLE_ALPHA_TO_COVERAGE); + + glDepthMask(GL_TRUE); + glDisable(GL_BLEND); + glDepthFunc(GL_LESS); + } + + // Render the meshes in the scene using the currently selected shader mode, setting appropriate uniforms for each mode + void SimManager::MeshRender(scene::Camera* cam) { + glEnable(GL_DEPTH_TEST); + glDepthFunc(GL_LESS); + glDepthMask(GL_TRUE); + glDisable(GL_BLEND); + + shaders::Shader* shader = nullptr; + + switch (currentShaderMode) { + case ShaderMode::Basic: + shader = _impl->_shaderBasic.get(); + break; + case ShaderMode::Lit: + shader = _impl->_shaderLit.get(); + break; + case ShaderMode::PBR: + shader = _impl->_shaderPBR.get(); + break; + } + + if (!shader) { + LOG_ERROR("Shader is NULL after switch!"); + return; + } + + shader->use(); + shader->setBool(false, "isFloor"); + + // Only PBR know about cascades & those uniforms + if (currentShaderMode == ShaderMode::PBR) { + for (int i = 0; i < NUM_CASCADES; i++) { + glActiveTexture(GL_TEXTURE5 + i); + glBindTexture(GL_TEXTURE_2D, _impl->_cascadeDepth[i]); + shader->setInt1(5 + i, "cascadeShadowMap[" + std::to_string(i) + "]"); + shader->setMat4(_impl->_lightSpaceMatrixCascade[i], "lightSpaceMatrix[" + std::to_string(i) + "]"); + } + + // Convert fractional splits (0..1 of far) into world-space distances + const float nearPlane = cam->getNear(); + const float farPlane = cam->getFar(); + + const float splitFrac0 = _cascadeSplits[0]; + const float splitFrac1 = _cascadeSplits[1]; + + const float splitDist0 = nearPlane + splitFrac0 * farPlane; + const float splitDist1 = nearPlane + splitFrac1 * farPlane; + + shader->setFlt2(splitDist0, splitDist1, "cascadeSplits"); + } + + // Camera / SunLight / light common to all mesh shaders + cam->update(shader); + _impl->_light->update(shader); + + // Main mesh rendering loop + for (auto& obj : _impl->_objects) { + if (!obj || !obj->getMesh()) continue; + + if (_impl->_cameraFollowTarget == obj.get()) { cam->setFollowTarget(obj->transform.position, obj->transform.rotQ); } + + glm::mat4 model = obj->transform.toMatrix() * obj->getMesh()->localTransform; + shader->setMat4(model, "model"); + + // Per-mode material uniforms + switch (currentShaderMode) { + case ShaderMode::Basic: + // (IMPORTANT) mesh_basic.frag needs: uniform vec3 color; + shader->setVec3(obj->material.albedo, "albedo"); + break; + + case ShaderMode::Lit: + // (IMPORTANT) mesh_lit.frag needs: albedo, lightPosition, lightColour, lightIntensity, camPos + shader->setVec3(obj->material.albedo, "albedo"); + shader->setVec3(_impl->_light->getPosition(), "lightPosition"); + shader->setFlt1(_impl->_light->getIntensity(), "lightIntensity"); + shader->setVec3(_impl->_light->getColour(), "lightColour"); + shader->setVec3(cam->getPosition(), "camPos"); + break; + + case ShaderMode::PBR: + // Per-mesh PBR material properties + shader->setVec3(obj->material.albedo, "albedo"); + shader->setFlt1(obj->material.metallic, "metallic"); + shader->setFlt1(obj->material.roughness, "roughness"); + shader->setFlt1(1.0f, "ao"); + shader->setFlt1(_settingsCurrent.ambientStrength, "ambientStrength"); + + shader->setVec3(glm::normalize(_impl->_light->getDirection()), "lightDirection"); + shader->setFlt1(_impl->_light->getIntensity(), "lightIntensity"); + shader->setVec3(_impl->_light->getColour(), "lightColour"); + shader->setVec3(cam->getPosition(), "camPos"); + + shader->setInt1(0, "irradianceMap"); + shader->setInt1(1, "prefilterMap"); + shader->setInt1(2, "brdfLUT"); + + glActiveTexture(GL_TEXTURE0); + glBindTexture(GL_TEXTURE_CUBE_MAP, _impl->_ibl->getIrradianceMap()); + + glActiveTexture(GL_TEXTURE1); + glBindTexture(GL_TEXTURE_CUBE_MAP, _impl->_ibl->getPrefilterMap()); + + glActiveTexture(GL_TEXTURE2); + glBindTexture(GL_TEXTURE_2D, _impl->_ibl->getBRDFLUT()); + + break; + } + + _impl->currentShader = shader; // for external access + obj->getMesh()->render(); + } + } + + // Render the shadow maps for each cascade by rendering the scene from the light's perspective into the depth textures + void SimManager::ShadowPass(scene::Camera* cam) { + float nearPlane = cam->getNear(); + float farPlane = cam->getFar(); + + float cascadeNear[NUM_CASCADES]{}; + float cascadeFar[NUM_CASCADES]{}; + + cascadeNear[0] = nearPlane; + cascadeFar[0] = nearPlane + _cascadeSplits[0] * (farPlane); + + cascadeNear[1] = cascadeFar[0]; + cascadeFar[1] = nearPlane + _cascadeSplits[1] * (farPlane); + + glEnable(GL_POLYGON_OFFSET_FILL); + glPolygonOffset(2.0f, 4.0f); + + for (int i = 0; i < NUM_CASCADES; i++) { + _impl->_lightSpaceMatrixCascade[i] = LightSpaceMatrix(cam, cascadeNear[i], cascadeFar[i]); + + // set viewport to shadow map size + int baseRes = _settingsCurrent.shadowMapRes; + int res = (i == 0) ? baseRes : (baseRes / 2); // 4096, 2048, 1024, 512, 256, 128 + glViewport(0, 0, res, res); + + // render to cascade FBO + glBindFramebuffer(GL_FRAMEBUFFER, _impl->_cascadeFBO[i]); + glClear(GL_DEPTH_BUFFER_BIT); + + // render scene from light's point of view + _impl->_shadowShader->use(); + _impl->_shadowShader->setMat4(_impl->_lightSpaceMatrixCascade[i], "lightSpaceMatrix"); + + // main mesh + for (auto& obj : _impl->_objects) { + if (!obj || !obj->getMesh()) continue; + + glm::mat4 model = obj->transform.toMatrix() * obj->getMesh()->localTransform; + + _impl->_shadowShader->setMat4(model, "model"); + obj->getMesh()->render(); + } + } + glBindFramebuffer(GL_FRAMEBUFFER, 0); + glViewport(0, 0, (int)_internalSize.x, (int)_internalSize.y); + + glDisable(GL_POLYGON_OFFSET_FILL); + } + + // Compute the light-space matrix for a given camera frustum slice (cascade) + glm::mat4 SimManager::LightSpaceMatrix(scene::Camera* cam, float nearPlane, float farPlane) { + std::array corners = cam->getFrustumCornersWorldSpace(nearPlane, farPlane); + + glm::vec3 lightDir = glm::normalize(_impl->_light->getDirection()); + + // Fake camera position far along direction + glm::vec3 lightPos = -lightDir * 50.0f; + + glm::mat4 lightView = glm::lookAt( + lightPos, + glm::vec3(0.0f), + glm::vec3(0, 1, 0) + ); + + float minX = FLT_MAX, maxX = -FLT_MAX; + float minY = FLT_MAX, maxY = -FLT_MAX; + float minZ = FLT_MAX, maxZ = -FLT_MAX; + + for (auto& corner : corners) { + glm::vec4 trf = lightView * glm::vec4(corner); + minX = std::min(minX, trf.x); + maxX = std::max(maxX, trf.x); + minY = std::min(minY, trf.y); + maxY = std::max(maxY, trf.y); + minZ = std::min(minZ, trf.z); + maxZ = std::max(maxZ, trf.z); + } + + // Compute cascade center in light space + glm::vec3 center = { + 0.5f * (minX + maxX), + 0.5f * (minY + maxY), + 0.5f * (minZ + maxZ) + }; + + // Cascade radius (half-size of the bounding sphere) + float radius = glm::length(glm::vec3(maxX - minX, maxY - minY, 0.0f)) * 0.5f; + + int shadowMapResolution = _settingsCurrent.shadowMapRes; + + // The size of one texel in world-space + float worldUnitsPerTexel = (radius * 2.0f) / shadowMapResolution; + + // Snap X and Y (Z never snapped) + center.x = std::floor(center.x / worldUnitsPerTexel) * worldUnitsPerTexel; + center.y = std::floor(center.y / worldUnitsPerTexel) * worldUnitsPerTexel; + + // Recompute min/max using snapped centre + minX = center.x - radius; + maxX = center.x + radius; + minY = center.y - radius; + maxY = center.y + radius; + + glm::mat4 lightProj = glm::ortho(minX, maxX, minY, maxY, minZ - 20.0f, maxZ + 20.0f); + + return lightProj * lightView; + } + + // Render the skybox using the IBL environment cubemap + void SimManager::SkyboxRender(scene::Camera* cam) { + glm::mat4 view = cam->getViewMatrix(); + glm::mat4 projection = cam->getProjection(); + + _impl->_skybox->setEnvironmentTexture(_impl->_ibl->getEnvCubemap()); + _impl->_skybox->render(projection, view); + } + + // Load a new HDR environment map for IBL + std::string SimManager::getDefaultHDR() const { return (paths::assets() / "hdr" / "default_white.hdr").string(); } + + // Load a new HDR environment map for IBL + const shaders::Shader* SimManager::getCurrentShader() const { return _impl->currentShader; } + void SimManager::applyRenderSettings(const render::RenderSettings& s, render::ResolutionPreset r) { applyRenderProfile(s, r); } + + void SimManager::applyRenderProfile(const render::RenderSettings& s, render::ResolutionPreset r) { + const bool first = !_settingsValid; + + const bool shadowResChanged = first || (s.shadowMapRes != _settingsCurrent.shadowMapRes); + const bool msaaChanged = first || (s.msaaSamples != _settingsCurrent.msaaSamples); + const bool renderScaleChanged = first || (s.renderScale != _settingsCurrent.renderScale); + const bool presetChanged = first || (r != _resCurrent); + + if (shadowResChanged) { + InitShadowResource(s.shadowMapRes); + } + + _settingsCurrent = s; + _resCurrent = r; + + if (msaaChanged || renderScaleChanged || presetChanged) { + for (auto& v : _impl->_views) { v.w = v.h = 0; v.displayW = v.displayH = 0; } + } + + glm::vec2 px = getPresetResolutionPx(); + _internalSize = px; + for (auto& v : _impl->_views) { v.w = v.h = 0; } // internal invalidation + + //LOG_INFO("Render settings applied: resPreset=%d shadowRes=%d msaa=%d renderScale=%.2f", (int)r, _settingsCurrent.shadowMapRes, _settingsCurrent.msaaSamples, _settingsCurrent.renderScale); + D_RUNTIME("Render settings applied: resPreset=%d shadowRes=%d msaa=%d renderScale=%.2f", (int)r, _settingsCurrent.shadowMapRes, _settingsCurrent.msaaSamples, _settingsCurrent.renderScale); + + _settingsValid = true; + } + + // Get the pixel dimensions for the current resolution preset + glm::vec2 SimManager::getPresetResolutionPx() const { + switch (_resCurrent) { + case render::ResolutionPreset::R_720p: return { 1280, 720 }; + case render::ResolutionPreset::R_1080p: return { 1920, 1080 }; + case render::ResolutionPreset::R_1440p: return { 2560, 1440 }; + case render::ResolutionPreset::R_4K: return { 3840, 2160 }; + default: return { 1920, 1080 }; + } + } + + glm::vec2 SimManager::getInternalResolutionSizePx() const { + return getPresetResolutionPx(); // no scale + } + + void SimManager::resetHDRToPreset() { + _hdrUserOverride = false; + const std::string hdr = getDefaultHDR(); + if (hdr != _activeHDRPath) { loadNewHDR(hdr); } + } + + void SimManager::reloadAllShaders() { + _impl->_shaderBasic->load((paths::assets() / "shaders" / "vs_pbr.vert.glsl").string(), (paths::assets() / "shaders" / "mesh_basic.frag.glsl").string()); + _impl->_shaderLit->load((paths::assets() / "shaders" / "vs_pbr.vert.glsl").string(), (paths::assets() / "shaders" / "mesh_lit.frag.glsl").string()); + _impl->_shaderPBR->load((paths::assets() / "shaders" / "vs_pbr.vert.glsl").string(), (paths::assets() / "shaders" / "mesh_pbr.frag.glsl").string()); + _impl->_ssaoShader->load((paths::assets() / "shaders" / "post.vert.glsl").string(), (paths::assets() / "shaders" / "ssao.frag.glsl").string()); + _impl->_ssaoBlurShader->load((paths::assets() / "shaders" / "post.vert.glsl").string(), (paths::assets() / "shaders" / "ssao_blur.frag.glsl").string()); + + //LOG_INFO("All shaders reloaded from disk."); + D_INFO_ONCE("All shaders reloaded from disk."); + } + + void SimManager::setLightColour(const glm::vec3& colour) { _impl->_light->_colour = colour; } +} \ No newline at end of file diff --git a/DSFE_App/DSFE_GUI/src/Scene/Manager/SimRobots.cpp b/DSFE_App/DSFE_GUI/src/Scene/Manager/SimRobots.cpp new file mode 100644 index 00000000..ff40df7d --- /dev/null +++ b/DSFE_App/DSFE_GUI/src/Scene/Manager/SimRobots.cpp @@ -0,0 +1,77 @@ +// DSFE_GUI SimRobots.cpp +#include "Scene/SimulationManager.h" +#include "Scene/SimulationCore.h" +#ifdef __gl_h_ +#undef __gl_h_ +#endif +#include "Manager/SimImplementation.h" + +namespace gui { + // Load a robot by name from the robot system + void SimManager::loadRobot(const std::string& name) { + // Clear any existing robot first + if (hasRobot()) { clearRobot(); } + _bodyLoaded = true; + + setSelectedObject(nullptr); + detachCameraFromObject(); + clearViewFollowTarget(gui::ViewID::Follow); + _impl->eeFollowBound = false; + _impl->eeObject = nullptr; + + if (!_impl->_robotSystem) { return; } + _core->loadRobotInternal(name); + + size_t startIdx = _impl->_objects.size(); + + _impl->buildRobotPresentationFromModel(_impl->_robotSystem->model(), *this); + + _impl->_robotRenderer->applyTransforms( + _impl->_robotSystem->model(), + _impl->_robotSystem->worldTransforms() + ); + + if (auto* simInteg = _impl->_robotSystem->getIntegrator()) { + simInteg->resetAdaptiveState(); + } + + // Attempt to find an end-effector candidate among the newly added objects and bind the Follow view to it + scene::Object* ee = _impl->findEndEffectorFromRange(startIdx); + if (ee) { + _impl->eeObject = ee; + setViewFollowTarget(gui::ViewID::Follow, ee, glm::vec3(0.0f, 0.2f, 0.6f)); + _impl->eeFollowBound = true; + LOG_INFO("Follow view bound to end-effector candidate: %s", ee->name.c_str()); + } + } + void SimManager::setRobotLinkRotation(const std::string& linkName, double angle) { + if (_impl->_robotSystem) { _impl->_robotSystem->setRobotLinkRotation(linkName, angle); } + } + void SimManager::setRobotRootPose(const Vec3& pos, Quat& rot) { + if (_impl->_robotSystem) { _impl->_robotSystem->setRobotRootPose(pos, rot); } + } + void SimManager::setRobotRootHome(const Vec3& pos, Quat& rot) { + if (_impl->_robotSystem) { _impl->_robotSystem->setRobotRootHome(pos, rot); } + } + void SimManager::resetRobot() { + if (_impl->_robotSystem) { _impl->_robotSystem->resetRobot(); } + } + void SimManager::clearRobot() { + setSelectedObject(nullptr); // deselect any selected object + _impl->clearRobotPresentation(); + _bodyLoaded = false; + + clearViewFollowTarget(gui::ViewID::Follow); + _impl->eeFollowBound = false; + _impl->eeObject = nullptr; + } + const bool SimManager::hasRobot() const { return _impl->_robotSystem && _impl->_robotSystem->hasRobot(); } + + // Access the robot system (non-const and const versions) + robots::RobotSystem* SimManager::robotSystem() { return _core->robotSystem(); } + const robots::RobotSystem* SimManager::robotSystem() const { return _core->robotSystem(); } + + // Access the trajectory manager (non-const and const versions) + control::TrajectoryManager* SimManager::traj() { return _core->trajectoryManager(); } + const control::TrajectoryManager* SimManager::traj() const { return _core->trajectoryManager(); } +} \ No newline at end of file diff --git a/DSFE_App/DSFE_GUI/src/Scene/Manager/SimUI.cpp b/DSFE_App/DSFE_GUI/src/Scene/Manager/SimUI.cpp new file mode 100644 index 00000000..addd2a6b --- /dev/null +++ b/DSFE_App/DSFE_GUI/src/Scene/Manager/SimUI.cpp @@ -0,0 +1,180 @@ +// DSFE_GUI SimUI.cpp +#include "Scene/SimulationManager.h" +#ifdef __gl_h_ +#undef __gl_h_ +#endif +#include "Manager/SimImplementation.h" + +#include + +namespace gui { + // Main Dockspace with Menu Bar + void SimManager::drawMainDockspace() { + ImGuiWindowFlags flags = + ImGuiWindowFlags_NoDocking | + ImGuiWindowFlags_NoTitleBar | + ImGuiWindowFlags_NoCollapse | + ImGuiWindowFlags_NoResize | + ImGuiWindowFlags_NoMove | + ImGuiWindowFlags_NoBringToFrontOnFocus | + ImGuiWindowFlags_NoNavFocus; + + const ImGuiViewport* vp = ImGui::GetMainViewport(); + ImGui::SetNextWindowPos(vp->Pos); + ImGui::SetNextWindowSize(vp->Size); + ImGui::SetNextWindowViewport(vp->ID); + + ImGui::PushStyleVar(ImGuiStyleVar_WindowRounding, 0.0f); + ImGui::PushStyleVar(ImGuiStyleVar_WindowBorderSize, 0.0f); + + // Must be a window so DockSpace has somewhere to live + ImGui::Begin("##MainDockspace", nullptr, flags); + + ImGui::PopStyleVar(2); + + beginSimManager("##MainDockspaceChild"); + + ImGuiID dock_id = ImGui::GetID("MainDockspaceID"); + ImGui::DockSpace(dock_id, ImVec2(0, 0), ImGuiDockNodeFlags_PassthruCentralNode); + + endSimManager(); + + ImGui::End(); + } + + // Viewport Window + void SimManager::drawViewportWindow() { + ImGui::Begin("Viewport", nullptr, + ImGuiWindowFlags_NoScrollbar | + ImGuiWindowFlags_NoScrollWithMouse); + + beginSimManager("##ViewportBody"); + + // --- Tabs: Single / Quad --- + if (ImGui::BeginTabBar("ViewportTabs", ImGuiTabBarFlags_None)) { + const bool singleSelected = ImGui::BeginTabItem("Single"); + if (singleSelected) { + // Restore to Manual view when switching back from Quad + if (_impl->viewMode == Impl::ViewMode::Quad) { + _impl->activeView = gui::ViewID::Manual; + } + _impl->viewMode = Impl::ViewMode::Single; + ImGui::EndTabItem(); + } + + const bool quadSelected = ImGui::BeginTabItem("Quad"); + if (quadSelected) { + _impl->viewMode = Impl::ViewMode::Quad; + ImGui::EndTabItem(); + } + + ImGui::EndTabBar(); + } + + // Everything below tabs is render output + _isHovered = ImGui::IsWindowHovered(ImGuiHoveredFlags_RootAndChildWindows); + + ImVec2 panel = ImGui::GetContentRegionAvail(); + + // Pixel size (framebuffer coords) + int vpW = (int)(panel.x); + int vpH = (int)(panel.y); + vpW = std::max(1, vpW); + vpH = std::max(1, vpH); + + // Update DISPLAY size only + if ((int)_displaySize.x != vpW || (int)_displaySize.y != vpH) { + _displaySize = { (float)vpW, (float)vpH }; + + // invalidate only display cached sizes so post buffers resize + for (auto& v : _impl->_views) { + v.displayW = 0; + v.displayH = 0; + } + //LOG_INFO("Viewport display size updated to %dx%d", vpW, vpH); + } + + // --- Render + Present --- + if (_impl->viewMode == Impl::ViewMode::Quad) { + int halfW = std::max(1, vpW / 2); + int halfH = std::max(1, vpH / 2); + + _impl->renderView(*this, _impl->_views[(size_t)gui::ViewID::Top], halfW, halfH); + _impl->renderView(*this, _impl->_views[(size_t)gui::ViewID::Front], halfW, halfH); + _impl->renderView(*this, _impl->_views[(size_t)gui::ViewID::Right], halfW, halfH); + _impl->renderView(*this, _impl->_views[(size_t)gui::ViewID::Follow], halfW, halfH); + + // Stable 2x2 layout + ImVec2 avail = ImGui::GetContentRegionAvail(); + ImVec2 cell = ImVec2(avail.x * 0.5f, avail.y * 0.5f); + + // Helper to draw each cell with the same pattern + auto drawCell = [&](const char* childId, gui::ViewID id, bool sameLine) { + if (sameLine) ImGui::SameLine(); + ImGui::BeginChild(childId, cell, false, ImGuiWindowFlags_NoScrollbar | ImGuiWindowFlags_NoScrollWithMouse); + + auto& v = _impl->_views[(size_t)id]; + ImVec2 inner = ImGui::GetContentRegionAvail(); + + ImGui::Image((ImTextureID)(intptr_t)v.post->getTexture(), inner, ImVec2(0, 1), ImVec2(1, 0)); + + // Set active view when clicking inside the quad cell + if (ImGui::IsWindowHovered() && ImGui::IsMouseClicked(ImGuiMouseButton_Left)) { + _impl->activeView = id; + } + + // Handle scroll zoom on hovered quad cell + if (ImGui::IsWindowHovered()) { + float scrollY = ImGui::GetIO().MouseWheel; + if (scrollY != 0.0f) { + v.cam->onMouseWheel((double)scrollY); + } + } + + // Visual indication: draw a border around the active cell + if (_impl->activeView == id) { + ImDrawList* dl = ImGui::GetWindowDrawList(); + ImVec2 p0 = ImGui::GetWindowPos(); + ImVec2 p1 = ImVec2(p0.x + ImGui::GetWindowSize().x, p0.y + ImGui::GetWindowSize().y); + dl->AddRect(p0, p1, IM_COL32(255, 200, 0, 180), 4.0f, 0, 2.0f); + } + + ImGui::EndChild(); + }; + + drawCell("##Top", gui::ViewID::Top, false); + drawCell("##Front", gui::ViewID::Front, true); + drawCell("##Right", gui::ViewID::Right, false); + drawCell("##Follow", gui::ViewID::Follow, true); + } + else { + auto& v = _impl->_views[static_cast(_impl->activeView)]; + _impl->renderView(*this, v, vpW, vpH); + + ImGui::Image((ImTextureID)(intptr_t)v.post->getTexture(), panel, ImVec2(0, 1), ImVec2(1, 0)); + } + + endSimManager(); + + ImGui::End(); + } + + // Begin Control Panel Helper + void SimManager::beginSimManager(const char* id) { + ImGui::PushStyleVar(ImGuiStyleVar_WindowPadding, ImVec2(12.0f, 10.0f)); + ImGui::PushStyleVar(ImGuiStyleVar_ChildRounding, 6.0f); + ImGui::PushStyleVar(ImGuiStyleVar_FramePadding, ImVec2(8.0f, 5.0f)); + ImGui::PushStyleVar(ImGuiStyleVar_ItemSpacing, ImVec2(10.0f, 8.0f)); + + ImGui::BeginChild(id, ImVec2(0, 0), true, + ImGuiChildFlags_AlwaysUseWindowPadding | + ImGuiWindowFlags_NoScrollbar | + ImGuiWindowFlags_NoScrollWithMouse); + } + + // End Control Panel Helper + void SimManager::endSimManager() { + ImGui::EndChild(); + ImGui::PopStyleVar(4); + } +} \ No newline at end of file diff --git a/DSFE_App/DSFE_GUI/src/Scene/Manager/SimViewport.cpp b/DSFE_App/DSFE_GUI/src/Scene/Manager/SimViewport.cpp new file mode 100644 index 00000000..71c13e70 --- /dev/null +++ b/DSFE_App/DSFE_GUI/src/Scene/Manager/SimViewport.cpp @@ -0,0 +1,10 @@ +// DSFE_GUI SimViewport.cpp +#include "Scene/SimulationManager.h" +#ifdef __gl_h_ +#undef __gl_h_ +#endif +#include "Manager/SimImplementation.h" + +namespace gui { + +} \ No newline at end of file diff --git a/DSFE_App/DSFE_GUI/src/Scene/SimulationManager.cpp b/DSFE_App/DSFE_GUI/src/Scene/SimulationManager.cpp index b696bf40..6d0279c7 100644 --- a/DSFE_App/DSFE_GUI/src/Scene/SimulationManager.cpp +++ b/DSFE_App/DSFE_GUI/src/Scene/SimulationManager.cpp @@ -14,40 +14,8 @@ extern "C" void DestroySimulationCore(core::ISimulationCore*); #endif #include #include -#include -#include - -#include -#include -#include -#include -#include -#include - -#include "Scene/Input.h" -#include "Scene/Camera.h" -#include "Scene/Mesh.h" -#include "Assets/MeshLoader.h" -#include "Scene/Light.h" -#include "Scene/AxisOrientator.h" - -#include "Robots/RobotSystem.h" -#include "Robots/RobotModel.h" -#include "Robots/TrajectoryManager.h" - -#include "Interpreter/IStoredProgram.h" -#include "Interpreter/StoredProgram.h" -#include "Interpreter/Parser.h" - -#include "Rendering/SkyboxRenderer.h" -#include "Rendering/ShaderUtil.h" -#include "Rendering/OpenGLBufferManager.h" -#include "Rendering/IBL.h" -#include "Rendering/Texture.h" - -#include "Robots/RobotPresentationBuilder.h" -#include "Robots/RobotRenderer.h" +#include "Manager/SimImplementation.h" #include @@ -96,579 +64,6 @@ namespace gui { return g; // (4x4) } - // --- PIMPL Implementation --- - struct SimManager::Impl { - // Completed Simulation Runs (thread-safe) - std::mutex _completedRunsMutex; - std::vector _completedRuns; - - // View ID Alias - using VID = gui::ViewID; - - // View Modes - enum class ViewMode { Single, Quad }; - - // Current View Mode - ViewMode viewMode = ViewMode::Single; - - // Viewport Structure - struct Viewport { - std::unique_ptr cam; - std::unique_ptr fb; - std::unique_ptr post; - - // Cache size - int w = 1, h = 1; - int displayW = 1, displayH = 1; - - // Follow Target - scene::Object* followTarget = nullptr; - glm::vec3 followOffset = glm::vec3(0.0f, 0.25f, 1.0f); // tweak - bool followEnabled = false; - }; - - // Viewports - std::array _views; - VID activeView = VID::Manual; - - // Skybox & IBL - std::unique_ptr _ibl; - std::unique_ptr _skybox; - - // Post-Processing Shader - std::unique_ptr _postShader; - std::shared_ptr _shaderBasic; - std::shared_ptr _shaderLit; - std::shared_ptr _shaderPBR; - - // World Grid & Shadow Shaders - std::unique_ptr _worldGridShader; - std::unique_ptr _shadowShader; - shaders::Shader* currentShader; - - // Fullscreen Quad VAO - GLuint _fullscreenVAO = 0; - GLuint _worldGridVAO = 0; - - // Shadow Mapping (Cascaded) - GLuint _cascadeFBO[SimManager::NUM_CASCADES]{}; - GLuint _cascadeDepth[SimManager::NUM_CASCADES]{}; - glm::mat4 _lightSpaceMatrixCascade[SimManager::NUM_CASCADES] = {}; - - // Scene Objects - std::unique_ptr _light; - std::unique_ptr _axisOrientator; - - scene::Object* _selectedObject = nullptr; - scene::Object* _cameraFollowTarget = nullptr; - - std::shared_ptr _mesh; - std::vector> _objects; - std::unordered_map> _linkToObjects; - std::unordered_map _primaryLinkObject; - - // Robot System - std::unique_ptr _robotSystem; // simulation - - // Robot Rendering - std::unique_ptr _robotRenderer; // rendering - RobotRenderBinding _currentBinding; // current render binding - - // Robot Follow Target - bool eeFollowBound = false; - scene::Object* eeObject = nullptr; - - // Trajectory Manager - control::TrajectoryManager _traj; - - // SSAO Resources - std::unique_ptr _ssaoShader; - std::unique_ptr _ssaoBlurShader; - GLuint _ssaoFBO = 0, _ssaoTex = 0; - GLuint _ssaoBlurFBO = 0, _ssaoBlurTex = 0; - GLuint _ssaoNoiseTex = 0; - int _ssaoW = 0, _ssaoH = 0; - std::vector _ssaoKernel; - - Impl(SimManager& owner) { - _postShader = std::make_unique(); - _postShader->load((paths::assets() / "shaders" / "post.vert.glsl").string(), (paths::assets() / "shaders" / "post.frag.glsl").string()); - - glGenVertexArrays(1, &_fullscreenVAO); - - // Lambda to create views - auto makeView = [&](ViewID id, glm::vec3 pos, float fovDeg, glm::vec3 target, glm::vec3 /*upHint*/) { - auto& v = _views[(size_t)id]; - - // INTERNAL render target size (scene render) - const int rtW = std::max(1, (int)owner._internalSize.x); - const int rtH = std::max(1, (int)owner._internalSize.y); - - // DISPLAY size (final post-process target) - const int dispW = std::max(1, (int)owner._displaySize.x); - const int dispH = std::max(1, (int)owner._displaySize.y); - const int postW = (dispW > 1 && dispH > 1) ? dispW : rtW; - const int postH = (dispW > 1 && dispH > 1) ? dispH : rtH; - - // Cache sizes - v.w = rtW; - v.h = rtH; - v.displayW = postW; - v.displayH = postH; - - // Framebuffers - v.fb = std::make_unique(); - v.fb->createBuffers(rtW, rtH, std::max(1, owner._settingsCurrent.msaaSamples)); - - v.post = std::make_unique(); - v.post->createBuffers(postW, postH, 1); - - // Camera aspect should match DISPLAY (what you're presenting in ImGui) - v.cam = std::make_unique(pos, fovDeg, (float)postW / (float)postH, 0.1f, 5000.0f); - v.cam->setFocus(target); - v.cam->updateViewMatrix(); - }; - - glm::vec3 target(0.0f); - - // Perspective - makeView(ViewID::Manual, { 0.0f, 0.5f, 1.0f }, 60.0f, target, { 0.0f, 1.0f, 0.0f }); // Default - makeView(ViewID::Follow, { 3.0f, 0.25f, 0.0f }, 20.0f, target, { 0.0f, 1.0f, 0.0f }); // Follow - // Ortho-ish - makeView(ViewID::Top, { 0.0f, 2.0f, 0.0f }, 20.0f, target, { 0.0f, 0.0f, -1.0f }); // Top - makeView(ViewID::Right, { 3.0f, 0.25f, 0.0f }, 20.0f, target, { 0.0f, 1.0f, 0.0f }); // Right - makeView(ViewID::Front, { 0.0f, 0.25f, 3.0f }, 20.0f, target, { 0.0f, -1.0f, 0.0f }); // Front - - { - // Top - auto* camTop = _views[(size_t)ViewID::Top].cam.get(); - camTop->setFocus(target); - camTop->setYaw(-glm::half_pi()); - camTop->setPitch(-glm::half_pi() + 0.001f); - camTop->setOrbitDistance(2.0f); - camTop->setMinDistance(0.5f); - camTop->updateViewMatrix(); - - // Right - auto* camRight = _views[(size_t)ViewID::Right].cam.get(); - camRight->setFocus(target); - camRight->setYaw(glm::pi()); - camRight->setPitch(0.0f); - camRight->setOrbitDistance(3.0f); - camRight->setMinDistance(0.5f); - camRight->updateViewMatrix(); - - // Front - auto* camFront = _views[(size_t)ViewID::Front].cam.get(); - camFront->setFocus(target); - camFront->setYaw(-glm::half_pi()); - camFront->setPitch(0.0f); - camFront->setOrbitDistance(3.0f); - camFront->setMinDistance(0.5f); - camFront->updateViewMatrix(); - - // Follow - auto* camFollow = _views[(size_t)ViewID::Follow].cam.get(); - camFollow->setFocus(target); - camFollow->setYaw(glm::pi()); - camFollow->setPitch(0.0f); - camFollow->setOrbitDistance(3.0f); - camFollow->setMinDistance(0.5f); - camFollow->updateViewMatrix(); - } - - // Shader Types A - // Basic shader with no lighting - _shaderBasic = std::make_shared(); - _shaderBasic->load((paths::assets() / "shaders" / "vs_pbr.vert.glsl").string(), (paths::assets() / "shaders" / "mesh_basic.frag.glsl").string()); - // Lit shader with simple Blinn-Phong lighting - _shaderLit = std::make_shared(); - _shaderLit->load((paths::assets() / "shaders" / "vs_pbr.vert.glsl").string(), (paths::assets() / "shaders" / "mesh_lit.frag.glsl").string()); - // PBR shader with full Physically Based Rendering (for release visuals) - _shaderPBR = std::make_shared(); - _shaderPBR->load((paths::assets() / "shaders" / "vs_pbr.vert.glsl").string(), (paths::assets() / "shaders" / "mesh_pbr.frag.glsl").string()); - - currentShader = _shaderPBR.get(); - _skybox = std::make_unique(); - - // Shader Types B - _worldGridShader = std::make_unique(); - _worldGridShader->load((paths::assets() / "shaders" / "world_grid.vert.glsl").string(), (paths::assets() / "shaders" / "world_grid.frag.glsl").string()); - - _shadowShader = std::make_unique(); - _shadowShader->load((paths::assets() / "shaders" / "shadow_depth.vert.glsl").string(), (paths::assets() / "shaders" / "shadow_depth.frag.glsl").string()); - - // Light - _light = std::make_unique(); - _light->_isDirectional = true; - - // Axis Orientator - _axisOrientator = std::make_unique(); - - // World Grid VAO - glGenVertexArrays(1, &_worldGridVAO); - - // Test Mesh - _mesh = std::make_shared(); - _mesh->init(); - - // Robot system with mesh loading (for normal simulation) - _robotSystem = std::make_unique(); - - _robotRenderer = std::make_unique(); - - // SSAO shaders - _ssaoShader = std::make_unique(); - _ssaoShader->load((paths::assets() / "shaders" / "post.vert.glsl").string(), (paths::assets() / "shaders" / "ssao.frag.glsl").string()); - - _ssaoBlurShader = std::make_unique(); - _ssaoBlurShader->load((paths::assets() / "shaders" / "post.vert.glsl").string(), (paths::assets() / "shaders" / "ssao_blur.frag.glsl").string()); - - // Generate hemisphere kernel - initSSAOKernel(64); - - // Generate noise texture (4x4 random rotation vectors) - initSSAONoise(); - } - - void clearRobotPresentation() { - _primaryLinkObject.clear(); - _linkToObjects.clear(); - _objects.clear(); - } - - scene::Object* primaryObjectForLink(const std::string& linkName) { - auto it = _primaryLinkObject.find(linkName); - if (it == _primaryLinkObject.end()) { return nullptr; } - return it->second; - } - - void buildRobotPresentationFromModel(const robots::RobotModel& model, SimManager& owner) { - clearRobotPresentation(); - - RobotPresentationBuilder builder; - RobotRenderBinding binding = builder.build(model); - - for (auto& owned : binding.ownedObjects) { - _objects.push_back(std::move(owned)); - } - - _robotRenderer->bind(binding); - - for (const auto& [linkName, visuals] : binding.linkVisuals) { - auto& target = _linkToObjects[linkName]; - - for (auto* obj : visuals) { - if (!obj) { continue; } - - target.push_back(obj); - - if (!_primaryLinkObject.contains(linkName)) { - _primaryLinkObject[linkName] = obj; - } - } - } - } - - // Random number generation for SSAO kernel and noise - std::mt19937 _rng{ std::random_device{}() }; - std::uniform_real_distribution _uni{ -1.0f, 1.0f }; - - // Helper to get random float in [a,b] - inline float rngFloat(float a, float b) { - std::uniform_real_distribution d(a, b); - return d(_rng); - } - - // Initialises the SSAO kernel with random samples in a hemisphere oriented along the positive Z axis, scaled to favor samples closer to the origin - void initSSAOKernel(int size) { - _ssaoKernel.clear(); - _ssaoKernel.reserve(size); - for (int i = 0; i < size; ++i) { - glm::vec3 sample( - rngFloat(-1.0f, 1.0f), - rngFloat(-1.0f, 1.0f), - rngFloat(0.0f, 1.0f) // hemisphere: z in [0,1] - ); - sample = glm::normalize(sample); - sample *= rngFloat(0.0f, 1.0f); - float scale = (float)i / (float)size; - scale = 0.1f + scale * scale * 0.9f; - sample *= scale; - _ssaoKernel.push_back(sample); - } - } - - // Initializes the SSAO noise texture with random rotation vectors in the XY plane - void initSSAONoise() { - std::vector noise(16); - // Generate 16 random rotation vectors in the XY plane (Z=0) - for (int i = 0; i < 16; ++i) { - noise[i] = glm::vec3( - rngFloat(-1.0f, 1.0f), - rngFloat(-1.0f, 1.0f), - 0.0f - ); - } - // Create OpenGL texture - glGenTextures(1, &_ssaoNoiseTex); - glBindTexture(GL_TEXTURE_2D, _ssaoNoiseTex); - glTexImage2D(GL_TEXTURE_2D, 0, GL_RGBA16F, 4, 4, 0, GL_RGB, GL_FLOAT, noise.data()); - glTexParameteri(GL_TEXTURE_2D, GL_TEXTURE_MIN_FILTER, GL_NEAREST); - glTexParameteri(GL_TEXTURE_2D, GL_TEXTURE_MAG_FILTER, GL_NEAREST); - glTexParameteri(GL_TEXTURE_2D, GL_TEXTURE_WRAP_S, GL_REPEAT); - glTexParameteri(GL_TEXTURE_2D, GL_TEXTURE_WRAP_T, GL_REPEAT); - } - - // Ensures SSAO FBOs and textures are created and match the given size, recreating them if necessary - void ensureSSAOBuffers(int w, int h) { - if (_ssaoW == w && _ssaoH == h) return; - _ssaoW = w; _ssaoH = h; - // Delete old buffers if they exist - if (_ssaoFBO) { glDeleteFramebuffers(1, &_ssaoFBO); glDeleteTextures(1, &_ssaoTex); } - if (_ssaoBlurFBO) { glDeleteFramebuffers(1, &_ssaoBlurFBO); glDeleteTextures(1, &_ssaoBlurTex); } - // Lambda to create a single-channel floating point FBO and texture - auto makeSingleChannelFBO = [](GLuint& fbo, GLuint& tex, int w, int h) { - glGenFramebuffers(1, &fbo); - glGenTextures(1, &tex); - glBindFramebuffer(GL_FRAMEBUFFER, fbo); - glBindTexture(GL_TEXTURE_2D, tex); - glTexImage2D(GL_TEXTURE_2D, 0, GL_R32F, w, h, 0, GL_RED, GL_FLOAT, nullptr); - glTexParameteri(GL_TEXTURE_2D, GL_TEXTURE_MIN_FILTER, GL_NEAREST); - glTexParameteri(GL_TEXTURE_2D, GL_TEXTURE_MAG_FILTER, GL_NEAREST); - glTexParameteri(GL_TEXTURE_2D, GL_TEXTURE_WRAP_S, GL_CLAMP_TO_EDGE); - glTexParameteri(GL_TEXTURE_2D, GL_TEXTURE_WRAP_T, GL_CLAMP_TO_EDGE); - glFramebufferTexture2D(GL_FRAMEBUFFER, GL_COLOR_ATTACHMENT0, GL_TEXTURE_2D, tex, 0); - glBindFramebuffer(GL_FRAMEBUFFER, 0); - }; - // Create SSAO and blur FBOs/textures - makeSingleChannelFBO(_ssaoFBO, _ssaoTex, w, h); - makeSingleChannelFBO(_ssaoBlurFBO, _ssaoBlurTex, w, h); - } - - // Renders the SSAO pass and subsequent blur pass, writing results to SSAO FBOs. Should be called after rendering the scene to the view's framebuffer (to provide depth texture input). - void renderSSAO(SimManager& owner, Viewport& v) { - if (!owner._settingsCurrent.ssao) return; - // Determine SSAO buffer size based on division factor - int ssaoDiv = std::max(1, owner._settingsCurrent.ssaoResDiv); - int ssaoW = std::max(1, v.w / ssaoDiv); - int ssaoH = std::max(1, v.h / ssaoDiv); - ensureSSAOBuffers(ssaoW, ssaoH); - // Clamp kernel size to available samples - int kernelSize = std::min((int)_ssaoKernel.size(), owner._settingsCurrent.ssaoSamples); - // SSAO Pass - glBindFramebuffer(GL_FRAMEBUFFER, _ssaoFBO); - glViewport(0, 0, ssaoW, ssaoH); - glClear(GL_COLOR_BUFFER_BIT); - glDisable(GL_DEPTH_TEST); - glDisable(GL_BLEND); - - _ssaoShader->use(); - - // Depth texture from the resolved scene FBO - glActiveTexture(GL_TEXTURE0); - glBindTexture(GL_TEXTURE_2D, v.fb->getDepthTexture()); - _ssaoShader->setInt1(0, "gDepth"); - // Noise texture - glActiveTexture(GL_TEXTURE1); - glBindTexture(GL_TEXTURE_2D, _ssaoNoiseTex); - _ssaoShader->setInt1(1, "gNoise"); - - // Camera matrices and parameters - _ssaoShader->setMat4(v.cam->getProjection(), "projection"); - _ssaoShader->setMat4(glm::inverse(v.cam->getProjection()), "invProjection"); - _ssaoShader->setVec2(glm::vec2((float)ssaoW / 4.0f, (float)ssaoH / 4.0f), "noiseScale"); - _ssaoShader->setInt1(kernelSize, "kernelSize"); - _ssaoShader->setFlt1(0.25f, "radius"); - _ssaoShader->setFlt1(0.035f, "bias"); - _ssaoShader->setFlt1(owner._settingsCurrent.ssaoStrength, "strength"); - - // SSAO kernel samples - for (int i = 0; i < kernelSize; ++i) { _ssaoShader->setVec3(_ssaoKernel[i], "samples[" + std::to_string(i) + "]"); } - - glBindVertexArray(_fullscreenVAO); - glDrawArrays(GL_TRIANGLES, 0, 3); - - // Blur pass - glBindFramebuffer(GL_FRAMEBUFFER, _ssaoBlurFBO); - glViewport(0, 0, ssaoW, ssaoH); - glClear(GL_COLOR_BUFFER_BIT); - - // No need for depth test or blending for a simple fullscreen blur - _ssaoBlurShader->use(); - glActiveTexture(GL_TEXTURE0); - glBindTexture(GL_TEXTURE_2D, _ssaoTex); - _ssaoBlurShader->setInt1(0, "ssaoInput"); - glDrawArrays(GL_TRIANGLES, 0, 3); - glBindVertexArray(0); - glBindFramebuffer(GL_FRAMEBUFFER, 0); - } - - // Renders the given viewport to its framebuffer, handling dynamic resizing of the framebuffer and camera aspect ratio based on the provided display size - void renderView(SimManager& owner, Viewport& v, int displayW, int displayH) { - displayW = std::max(1, displayW); - displayH = std::max(1, displayH); - - // Internal render target size - glm::ivec2 rt(std::max(1, (int)owner._internalSize.x), std::max(1, (int)owner._internalSize.y)); - - // In quad mode, render at the cell's display resolution to avoid scaling artifacts - if (owner._impl->viewMode == Impl::ViewMode::Quad) { - rt.x = std::max(1, displayW); - rt.y = std::max(1, displayH); - } - const int rtW = std::max(1, (int)rt.x); - const int rtH = std::max(1, (int)rt.y); - - LOG_INFO_ONCE("Rendering Viewport: RT Size = %dx%d, Display Size = %dx%d", rtW, rtH, displayW, displayH); - - // Resize only when internal RT changes OR display changes (post buffer) - const bool rtChanged = (v.w != rtW) || (v.h != rtH); - const bool displayChanged = (v.displayW != displayW) || (v.displayH != displayH); - const bool msaaChanged = false; - - if (rtChanged || displayChanged || msaaChanged) { - // Store internal RT size - v.w = rtW; - v.h = rtH; - - // Store display size - v.displayW = displayW; - v.displayH = displayH; - - // Adjust MSAA based on internal RT size - int msaa = std::max(1, owner._settingsCurrent.msaaSamples); - - // Scene framebuffer: INTERNAL resolution (rtW x rtH) - v.fb->deleteBuffers(); - v.fb->createBuffers(rtW, rtH, msaa); - - // Post-processing framebuffer: DISPLAY resolution (displayW x displayH) - v.post->deleteBuffers(); - v.post->createBuffers(displayW, displayH, 1); - - // Camera aspect must match DISPLAY aspect - v.cam->setAspect((float)displayW / (float)displayH); - } - - v.fb->bind(); - glViewport(0, 0, v.w, v.h); - glEnable(GL_DEPTH_TEST); - glDepthMask(GL_TRUE); - glDepthFunc(GL_LESS); - - glClearColor(owner._backgroundColour.r, owner._backgroundColour.g, owner._backgroundColour.b, owner._backgroundAlpha); - glClear(GL_COLOR_BUFFER_BIT | GL_DEPTH_BUFFER_BIT); - - if (owner._settingsCurrent.msaaSamples > 1) { - glEnable(GL_MULTISAMPLE); - } - else { - glDisable(GL_MULTISAMPLE); - } - - // Update Follow Target: use the explicitly bound target (e.g. end-effector from loadRobot), - // only fall back to _selectedObject if no explicit target was set via setViewFollowTarget - if (&v == &_views[(size_t)gui::ViewID::Follow]) { - scene::Object* target = v.followEnabled ? v.followTarget : _selectedObject; - - if (target && target->getMesh()) { - glm::mat4 M = target->transform.toMatrix() * target->getMesh()->localTransform; - glm::vec3 worldPos = glm::vec3(M[3]); - v.cam->setFollowTarget(worldPos, target->transform.rotQ); - } - } - - // Skybox (renders before all opaque geometry, doesn't write depth) - if (owner.skyboxEnabled) { - glDepthMask(GL_FALSE); - glDepthFunc(GL_LEQUAL); - owner.SkyboxRender(v.cam.get()); - glDepthMask(GL_TRUE); - glDepthFunc(GL_LESS); - } - - // Meshes & World Grid - GLint sampleBuffers = 0, samples = 0; - glGetIntegerv(GL_SAMPLE_BUFFERS, &sampleBuffers); - glGetIntegerv(GL_SAMPLES, &samples); - LOG_INFO_ONCE("FB MSAA state: GL_SAMPLE_BUFFERS=%d GL_SAMPLES=%d", sampleBuffers, samples); - - owner.MeshRender(v.cam.get()); - if (owner._settingsCurrent.grid) { owner.WorldGridRender(v.cam.get(), v.w); } - - v.fb->unbind(); - - // SSAO pass (reads resolved depth, writes to _ssaoBlurTex) - renderSSAO(owner, v); - - // Only valid if you allocated mip levels for _texID (via glTexStorage2D) - glBindTexture(GL_TEXTURE_2D, v.fb->getTexture()); - glGenerateMipmap(GL_TEXTURE_2D); - glBindTexture(GL_TEXTURE_2D, 0); - - // Post-Processing - v.post->bind(); - glDisable(GL_MULTISAMPLE); - glViewport(0, 0, v.displayW, v.displayH); - - glDisable(GL_DEPTH_TEST); - glDisable(GL_BLEND); - glClear(GL_COLOR_BUFFER_BIT); - - _postShader->use(); - _postShader->setInt1(0, "hdrScene"); - _postShader->setInt1(1, "ssaoTex"); - _postShader->setBool(owner._settingsCurrent.ssao, "ssaoEnabled"); - _postShader->setFlt1(owner._settingsCurrent.exposure, "exposure"); - _postShader->setFlt1(owner._settingsCurrent.whitePoint, "whitePoint"); - _postShader->setVec2(glm::vec2(v.w, v.h), "uRes"); - - glActiveTexture(GL_TEXTURE0); - glBindTexture(GL_TEXTURE_2D, v.fb->getTexture()); - - glActiveTexture(GL_TEXTURE1); - glBindTexture(GL_TEXTURE_2D, owner._settingsCurrent.ssao ? _ssaoBlurTex : 0); - - glBindVertexArray(_fullscreenVAO); - glDrawArrays(GL_TRIANGLES, 0, 3); - if (owner._settingsCurrent.axisOrientator) { _axisOrientator->render(v.cam->getViewMatrix()); } - glBindVertexArray(0); - - v.post->unbind(); - } - - static bool icontains(const std::string& s, const char* sub) { - if (sub == nullptr || *sub == '\0') { return false; } - - // Case-insensitive search using std::search with a custom comparator - auto it = std::search( - s.begin(), s.end(), - sub, sub + std::strlen(sub), - [](char a, char b) { - return std::tolower((unsigned char)a) == std::tolower((unsigned char)b); - } - ); - return it != s.end(); - } - - scene::Object* findEndEffectorFromRange(size_t startIdx) { - for (size_t i = startIdx; i < _objects.size(); ++i) { - scene::Object* o = _objects[i].get(); - if (!o) continue; - - const std::string& n = o->name; - if (icontains(n, "end") || icontains(n, "eff") || icontains(n, "ee") || icontains(n, "tool") || icontains(n, "tcp") || icontains(n, "gripper")) { - return o; - } - } - - // Fallback: last object added (usually the last link) - if (_objects.size() > startIdx) return _objects.back().get(); - return nullptr; - } - }; - // Helper: create CorePtr (unique_ptr with std::function deleter) static CorePtr makeCoreFactory() { core::ISimulationCore* raw = CreateSimulationCore_v1(); @@ -726,11 +121,6 @@ namespace gui { if (_impl && _impl->_mesh) { _impl->_mesh->clean(); } } - // Helper to get the next ObjectID - static inline scene::ObjectID next(scene::ObjectID id) { - return static_cast(static_cast(id) + 1); - } - // -------------------------------------------------- // THREAD-SAFE SIMULATION RESULTS // -------------------------------------------------- @@ -763,287 +153,6 @@ namespace gui { return copy; } - - // -------------------------------------------------- - // LIGHT & SKYBOX - // -------------------------------------------------- - scene::Light* SimManager::getLight() { return _impl->_light.get(); } - - void SimManager::loadNewHDR(const std::string& path) { - D_INFO("Loading new HDR: %s", path.c_str()); - - // Make sure IBL system exists - if (!_impl->_ibl) { - LOG_ERROR("Cannot load HDR because IBL system is not initialised."); - D_FAIL("Cannot load HDR because IBL system is not initialised."); - return; - } - - _impl->_ibl->init(path); // rebuild envCubemap, irradiance, prefilter, brdfLUT - D_SUCCESS("IBL rebuilt successfully."); - - // Update skybox - _impl->_skybox->setEnvironmentTexture(_impl->_ibl->getEnvCubemap()); - - _activeHDRPath = path; - - D_SUCCESS("Loaded HDR successfully."); - } - - void SimManager::loadNewHDR_UI(const std::string& path) { - loadNewHDR(path); - _hdrUserOverride = true; - } - - void SimManager::loadNewHDR_Preset(const std::string& path) { - loadNewHDR(path); - _hdrUserOverride = false; - } - - // -------------------------------------------------- - // CONTROL MODES & CAMERA - // -------------------------------------------------- - - // Get the active view camera - scene::Camera* SimManager::getCamera() { return _impl->_views[static_cast(_impl->activeView)].cam.get(); } - // Reset the active view camera to default position - void SimManager::resetView() { - auto& v = _impl->_views[static_cast(_impl->activeView)]; - - glm::vec3 pos = { 0.0f, 0.25f, 1.0f }; - float fov = 60.0f; - - switch (_impl->activeView) { - case gui::ViewID::Top: pos = { 0, 5, 0 }; fov = 20.0f; break; - case gui::ViewID::Right: pos = { 5, 0, 0 }; fov = 20.0f; break; - case gui::ViewID::Front: pos = { 0, 0, 5 }; fov = 20.0f; break; - case gui::ViewID::Follow: fov = 20.0f; break; - case gui::ViewID::Manual: fov = 20.0f; break; - default: break; - } - - float aspect = (float)std::max(1, v.w) / (float)std::max(1, v.h); - v.cam = std::make_unique(pos, fov, aspect, 0.1f, 5000.0f); - - v.cam->setFocus(glm::vec3(0.0f)); - - switch (_impl->activeView) { - case gui::ViewID::Top: - v.cam->setYaw(-glm::half_pi()); - v.cam->setPitch(-glm::half_pi() + 0.001f); - break; - case gui::ViewID::Right: - v.cam->setYaw(glm::pi()); - v.cam->setPitch(0.0f); - break; - case gui::ViewID::Front: - v.cam->setYaw(-glm::half_pi()); - v.cam->setPitch(0.0f); - break; - default: - break; - } - - v.cam->updateViewMatrix(); - } - - // Attach camera to an object and start following it. The camera will maintain a fixed offset from the object's position and orientation. - void SimManager::attachCameraToObject(scene::Object* obj) { - if (!obj) return; - scene::Camera* cam = _impl->_views[static_cast(_impl->activeView)].cam.get(); - - _impl->_cameraFollowTarget = obj; - - glm::vec3 pos = obj->transform.position; - glm::quat rot = obj->transform.rotQ; - - cam->startFollow(pos, rot, glm::vec3(0, 2, 5)); - } - - // Detach camera from any object and stop following - void SimManager::detachCameraFromObject() { - scene::Camera* cam = _impl->_views[static_cast(_impl->activeView)].cam.get(); - _impl->_cameraFollowTarget = nullptr; - cam->clearFollow(); - } - - // Set the follow target for a specific view - void SimManager::setViewFollowTarget(ViewID view, scene::Object* obj, const glm::vec3& offset) { - if (view < ViewID::Manual || view >= ViewID::COUNT) { return; } - auto& v = _impl->_views[static_cast(view)]; - v.followTarget = obj; - v.followOffset = offset; - v.followEnabled = (obj != nullptr); - - if (v.followEnabled && v.cam && obj) { - v.cam->startFollow(obj->transform.position, obj->transform.rotQ, offset); - } - } - - // Clear the follow target for a specific view - void SimManager::clearViewFollowTarget(ViewID view) { - if (view < ViewID::Manual || view >= ViewID::COUNT) { return; } - auto& v = _impl->_views[static_cast(view)]; - v.followTarget = nullptr; - v.followEnabled = false; - if (v.cam) { v.cam->clearFollow(); } - } - - // Convenience for Follow view: set the follow target to the object attached to a robot joint (e.g. end-effector) - bool SimManager::setViewFollowRobotJoint(ViewID view, const std::string& jointName, const glm::vec3& offset) { - if (!hasRobot()) { - LOG_WARN("setViewFollowRobotJoint: no robot loaded"); - return false; - } - - robots::RobotSystem* rs = robotSystem(); - if (!rs) return false; - - auto& joints = rs->joints(); - auto& links = rs->links(); - - // 1) Find joint by name - const robots::RobotJoint* jPtr = nullptr; - for (auto& j : joints) { - if (j.name == jointName) { jPtr = &j; break; } - } - if (!jPtr) { - LOG_WARN("setViewFollowRobotJoint: joint not found: %s", jointName.c_str()); - return false; - } - - // 2) Find child link -> attached object - scene::Object* targetObj = nullptr; - if (auto it = _impl->_primaryLinkObject.find(jPtr->child); - it != _impl->_primaryLinkObject.end()) { - targetObj = it->second; - } - - if (!targetObj) { - LOG_WARN("setViewFollowRobotJoint: no attached object for joint=%s child=%s", - jointName.c_str(), jPtr->child.c_str()); - return false; - } - - // 3) Bind the view follow target - setViewFollowTarget(view, targetObj, offset); - - /*LOG_INFO("Follow view=%d bound to joint='%s' -> child='%s' -> obj='%s'", - (int)view, jointName.c_str(), jPtr->child.c_str(), targetObj->name.c_str());*/ - - return true; - } - - // Convenience for Follow view - bool SimManager::followRobotJoint(const std::string& jointName, const glm::vec3& offset) { - return setViewFollowRobotJoint(gui::ViewID::Follow, jointName, offset); - } - - // -------------------------------------------------- - // MESH LOADING & GEOMETRY - // -------------------------------------------------- - - // Load a mesh from file and create one Object per submesh. The last loaded mesh becomes the active selection. - void SimManager::loadMesh(const std::string& filepath) { - assets::MeshLoader loader; - auto meshes = loader.load(filepath); - - if (meshes.empty()) { - LOG_WARN("No meshes imported from %s", filepath.c_str()); - D_WARN("No meshes imported from %s", filepath.c_str()); - return; - } - - // For now: spawn one Object per submesh - for (auto& m : meshes) { - auto obj = std::make_unique(m); - obj->id = next(_nextObjectID); - obj->source.filename = filepath; - obj->name = m->getName().empty() ? "Object_" + std::to_string(scene::toUInt32(obj->id)) : m->getName(); - - // initialise physics state - obj->state.q = Quat(1.0, 0.0, 0.0, 0.0); - obj->state.angularVelocity = Vec3::Zero(); - obj->state.linearVelocity = Vec3::Zero(); - obj->state.mass = 1.0; - obj->state.damping = 0.0; - obj->state.inertia = Mat3::Identity(); - obj->state.forces = Vec3::Zero(); - obj->state.torques = Vec3::Zero(); - - _impl->_selectedObject = obj.get(); - _impl->_objects.push_back(std::move(obj)); - } - - LOG_INFO("Loaded %zu submeshes from %s", meshes.size(), filepath.c_str()); - D_INFO("Loaded %zu submeshes from %s", meshes.size(), filepath.c_str()); - } - - // Returns the loaded objects so they can be used as targets for robot joints in the same frame (e.g. end-effector) - std::vector SimManager::loadMeshReturn(const std::string& filepath) { - assets::MeshLoader loader; - auto meshes = loader.load(filepath); - std::vector result; - for (auto& m : meshes) { - auto obj = std::make_unique(m); - auto raw = obj.get(); - raw->internal = true; - _impl->_objects.push_back(std::move(obj)); - result.push_back(raw); - } - return result; - } - - // Setter and Getter for the active mesh - void SimManager::setMesh(std::shared_ptr mesh) { _impl->_mesh = mesh; } - std::shared_ptr SimManager::getMesh() { return _impl->_mesh; } - - // Set the currently selected object (can be nullptr to deselect) - void SimManager::setSelectedObject(scene::Object* obj) { _impl->_selectedObject = obj; } - // Add a new object to the scene and select it - void SimManager::addObject(std::unique_ptr obj) { _impl->_objects.push_back(std::move(obj)); } // Cache the unique_ptr - - // Remove an object by index - void SimManager::deleteObject(int index) { - if (index < 0 || index >= _impl->_objects.size()) { return; } - if (_impl->_selectedObject == _impl->_objects[index].get()) { _impl->_selectedObject = nullptr; } - _impl->_objects.erase(_impl->_objects.begin() + index); - } - - // Remove an object by pointer - void SimManager::removeObject(scene::Object* obj) { - if (!obj) return; - - auto it = std::remove_if( - _impl->_objects.begin(), - _impl->_objects.end(), - [obj](const std::unique_ptr& o) { - return o.get() == obj; - } - ); - - _impl->_objects.erase(it, _impl->_objects.end()); - } - - // Access the objects as raw pointers for use in the rest of the codebase, while maintaining ownership in SimManager - std::vector>& SimManager::getObjects() { return _impl->_objects; } - // Get the currently selected object (can be nullptr) - scene::Object* SimManager::getObject() { return _impl->_selectedObject; } - - // Helper to find an object by its ID (returns nullptr if not found) - scene::Object* SimManager::getObjectByID(scene::ObjectID id) { - for (auto& obj : _impl->_objects) { - if (obj && obj->id == id) { - return obj.get(); - } - } - return nullptr; - } - - // -------------------------------------------------- - // RENDERING ENTRY POINTS - // -------------------------------------------------- - // Main render function called by the application void SimManager::render() { if (hasCompletedStudy()) { @@ -1057,7 +166,7 @@ namespace gui { ImGuiIO& io = ImGui::GetIO(); - _core->tick(io.DeltaTime); + tick(io.DeltaTime); if (_impl->_robotSystem && hasRobot()) { _impl->_robotRenderer->applyTransforms( @@ -1077,6 +186,8 @@ namespace gui { drawViewportWindow(); } + void SimManager::tick(double dt) { _core->tick(dt); } + void SimManager::syncRobotToScene() { if (!hasRobot()) return; auto* rs = robotSystem(); @@ -1103,159 +214,6 @@ namespace gui { } } - // --- UI Elements --- - - // Main Dockspace with Menu Bar - void SimManager::drawMainDockspace() { - ImGuiWindowFlags flags = - ImGuiWindowFlags_NoDocking | - ImGuiWindowFlags_NoTitleBar | - ImGuiWindowFlags_NoCollapse | - ImGuiWindowFlags_NoResize | - ImGuiWindowFlags_NoMove | - ImGuiWindowFlags_NoBringToFrontOnFocus | - ImGuiWindowFlags_NoNavFocus; - - const ImGuiViewport* vp = ImGui::GetMainViewport(); - ImGui::SetNextWindowPos(vp->Pos); - ImGui::SetNextWindowSize(vp->Size); - ImGui::SetNextWindowViewport(vp->ID); - - ImGui::PushStyleVar(ImGuiStyleVar_WindowRounding, 0.0f); - ImGui::PushStyleVar(ImGuiStyleVar_WindowBorderSize, 0.0f); - - // Must be a window so DockSpace has somewhere to live - ImGui::Begin("##MainDockspace", nullptr, flags); - - ImGui::PopStyleVar(2); - - beginSimManager("##MainDockspaceChild"); - - ImGuiID dock_id = ImGui::GetID("MainDockspaceID"); - ImGui::DockSpace(dock_id, ImVec2(0, 0), ImGuiDockNodeFlags_PassthruCentralNode); - - endSimManager(); - - ImGui::End(); - } - - // Viewport Window - void SimManager::drawViewportWindow() { - ImGui::Begin("Viewport", nullptr, - ImGuiWindowFlags_NoScrollbar | - ImGuiWindowFlags_NoScrollWithMouse); - - beginSimManager("##ViewportBody"); - - // --- Tabs: Single / Quad --- - if (ImGui::BeginTabBar("ViewportTabs", ImGuiTabBarFlags_None)) { - const bool singleSelected = ImGui::BeginTabItem("Single"); - if (singleSelected) { - // Restore to Manual view when switching back from Quad - if (_impl->viewMode == Impl::ViewMode::Quad) { - _impl->activeView = gui::ViewID::Manual; - } - _impl->viewMode = Impl::ViewMode::Single; - ImGui::EndTabItem(); - } - - const bool quadSelected = ImGui::BeginTabItem("Quad"); - if (quadSelected) { - _impl->viewMode = Impl::ViewMode::Quad; - ImGui::EndTabItem(); - } - - ImGui::EndTabBar(); - } - - // Everything below tabs is render output - _isHovered = ImGui::IsWindowHovered(ImGuiHoveredFlags_RootAndChildWindows); - - ImVec2 panel = ImGui::GetContentRegionAvail(); - - // Pixel size (framebuffer coords) - int vpW = (int)(panel.x); - int vpH = (int)(panel.y); - vpW = std::max(1, vpW); - vpH = std::max(1, vpH); - - // Update DISPLAY size only - if ((int)_displaySize.x != vpW || (int)_displaySize.y != vpH) { - _displaySize = { (float)vpW, (float)vpH }; - - // invalidate only display cached sizes so post buffers resize - for (auto& v : _impl->_views) { - v.displayW = 0; - v.displayH = 0; - } - //LOG_INFO("Viewport display size updated to %dx%d", vpW, vpH); - } - - // --- Render + Present --- - if (_impl->viewMode == Impl::ViewMode::Quad) { - int halfW = std::max(1, vpW / 2); - int halfH = std::max(1, vpH / 2); - - _impl->renderView(*this, _impl->_views[(size_t)gui::ViewID::Top], halfW, halfH); - _impl->renderView(*this, _impl->_views[(size_t)gui::ViewID::Front], halfW, halfH); - _impl->renderView(*this, _impl->_views[(size_t)gui::ViewID::Right], halfW, halfH); - _impl->renderView(*this, _impl->_views[(size_t)gui::ViewID::Follow], halfW, halfH); - - // Stable 2x2 layout - ImVec2 avail = ImGui::GetContentRegionAvail(); - ImVec2 cell = ImVec2(avail.x * 0.5f, avail.y * 0.5f); - - // Helper to draw each cell with the same pattern - auto drawCell = [&](const char* childId, gui::ViewID id, bool sameLine) { - if (sameLine) ImGui::SameLine(); - ImGui::BeginChild(childId, cell, false, ImGuiWindowFlags_NoScrollbar | ImGuiWindowFlags_NoScrollWithMouse); - - auto& v = _impl->_views[(size_t)id]; - ImVec2 inner = ImGui::GetContentRegionAvail(); - - ImGui::Image((ImTextureID)(intptr_t)v.post->getTexture(), inner, ImVec2(0, 1), ImVec2(1, 0)); - - // Set active view when clicking inside the quad cell - if (ImGui::IsWindowHovered() && ImGui::IsMouseClicked(ImGuiMouseButton_Left)) { - _impl->activeView = id; - } - - // Handle scroll zoom on hovered quad cell - if (ImGui::IsWindowHovered()) { - float scrollY = ImGui::GetIO().MouseWheel; - if (scrollY != 0.0f) { - v.cam->onMouseWheel((double)scrollY); - } - } - - // Visual indication: draw a border around the active cell - if (_impl->activeView == id) { - ImDrawList* dl = ImGui::GetWindowDrawList(); - ImVec2 p0 = ImGui::GetWindowPos(); - ImVec2 p1 = ImVec2(p0.x + ImGui::GetWindowSize().x, p0.y + ImGui::GetWindowSize().y); - dl->AddRect(p0, p1, IM_COL32(255, 200, 0, 180), 4.0f, 0, 2.0f); - } - - ImGui::EndChild(); - }; - - drawCell("##Top", gui::ViewID::Top, false); - drawCell("##Front", gui::ViewID::Front, true); - drawCell("##Right", gui::ViewID::Right, false); - drawCell("##Follow", gui::ViewID::Follow, true); - } - else { - auto& v = _impl->_views[static_cast(_impl->activeView)]; - _impl->renderView(*this, v, vpW, vpH); - - ImGui::Image((ImTextureID)(intptr_t)v.post->getTexture(), panel, ImVec2(0, 1), ImVec2(1, 0)); - } - - endSimManager(); - - ImGui::End(); - } - void SimManager::resize(int32_t width, int32_t height) { if (width <= 0 || height <= 0) return; _internalSize = { (float)width, (float)height }; @@ -1268,571 +226,6 @@ namespace gui { //LOG_INFO("Resized SimManager INTERNAL RT to %dx%d", width, height); } - - // Load a robot by name from the robot system - void SimManager::loadRobot(const std::string& name) { - // Clear any existing robot first - if (hasRobot()) { clearRobot(); } - _bodyLoaded = true; - - setSelectedObject(nullptr); - detachCameraFromObject(); - clearViewFollowTarget(gui::ViewID::Follow); - _impl->eeFollowBound = false; - _impl->eeObject = nullptr; - - if (!_impl->_robotSystem) { return; } - _core->loadRobotInternal(name); - - size_t startIdx = _impl->_objects.size(); - - _impl->buildRobotPresentationFromModel(_impl->_robotSystem->model(), *this); - - _impl->_robotRenderer->applyTransforms( - _impl->_robotSystem->model(), - _impl->_robotSystem->worldTransforms() - ); - - if (auto* simInteg = _impl->_robotSystem->getIntegrator()) { - simInteg->resetAdaptiveState(); - } - - // Attempt to find an end-effector candidate among the newly added objects and bind the Follow view to it - scene::Object* ee = _impl->findEndEffectorFromRange(startIdx); - if (ee) { - _impl->eeObject = ee; - setViewFollowTarget(gui::ViewID::Follow, ee, glm::vec3(0.0f, 0.2f, 0.6f)); - _impl->eeFollowBound = true; - LOG_INFO("Follow view bound to end-effector candidate: %s", ee->name.c_str()); - } - } - void SimManager::setRobotLinkRotation(const std::string& linkName, double angle) { - if (_impl->_robotSystem) { _impl->_robotSystem->setRobotLinkRotation(linkName, angle); } - } - void SimManager::setRobotRootPose(const Vec3& pos, Quat& rot) { - if (_impl->_robotSystem) { _impl->_robotSystem->setRobotRootPose(pos, rot); } - } - void SimManager::setRobotRootHome(const Vec3& pos, Quat& rot) { - if (_impl->_robotSystem) { _impl->_robotSystem->setRobotRootHome(pos, rot); } - } - void SimManager::resetRobot() { - if (_impl->_robotSystem) { _impl->_robotSystem->resetRobot(); } - } - void SimManager::clearRobot() { - setSelectedObject(nullptr); // deselect any selected object - _impl->clearRobotPresentation(); - _bodyLoaded = false; - - clearViewFollowTarget(gui::ViewID::Follow); - _impl->eeFollowBound = false; - _impl->eeObject = nullptr; - } - const bool SimManager::hasRobot() const { return _impl->_robotSystem && _impl->_robotSystem->hasRobot(); } - - // Access the robot system (non-const and const versions) - robots::RobotSystem* SimManager::robotSystem() { return _core->robotSystem(); } - const robots::RobotSystem* SimManager::robotSystem() const { return _core->robotSystem(); } - - // Access the trajectory manager (non-const and const versions) - control::TrajectoryManager* SimManager::traj() { return _core->trajectoryManager(); } - const control::TrajectoryManager* SimManager::traj() const { return _core->trajectoryManager(); } - - // Start the simulation - void SimManager::startSimulation() { - if (!hasRobot()) { - LOG_WARN("Cannot start simulation: no robot loaded"); - return; - } - if (hasRobot() && !_bodyLoaded) { - loadRobot(_impl->_robotSystem->robotName()); - return; - } - _core->startSimulation(); - } - // Stop the simulation - void SimManager::stopSimulation() { _core->stopSimulation(); } - - // Check if the simulation is currently running - const bool SimManager::isSimRunning() const { return _core->isSimRunning(); } - - // Setter for current simulation time (in seconds) - void SimManager::setSimTime(double time) { _core->setSimTime(time); } - const double SimManager::simTime() const { return _core->simTime(); } - - // Setter and gettter for fixed timstep (in seconds) - void SimManager::setFixedDt(double dt) { _core->setFixedDt(dt); } - const double SimManager::fixedDt() const { return _core->fixedDt(); } - - // Setter and getter for telemetry frequency (in Hz) - void SimManager::setTelemetryHz(double hz) { _core->setTelemetryHz(hz); } - const double SimManager::telemetryHz() const { return _core->telemetryHz(); } - - // Set whether a script is currently running (used to disable UI elements, etc.) - void SimManager::setScriptRunning(bool running) { _core->setScriptRunning(running); } - const bool SimManager::isScriptRunning() const { return _core->isScriptRunning(); } - - // Setters and getters for last script text - void SimManager::setLastScriptText(const std::string& text) { _core->setLastScriptText(text); } - const std::string& SimManager::lastScriptText() const { return _core->lastScriptText(); } - - // Accessors for the Simulation Core's telemetry data - diagnostics::TelemetryRecorder& SimManager::telemetry() { return _core->telemetry(); } - const diagnostics::TelemetryRecorder& SimManager::telemetry() const { return _core->telemetry(); } - - // Accesors for the active program (if any) - void SimManager::setActiveProgram(interpreter::IStoredProgram* program) { _core->setActiveProgram(program); } - interpreter::IStoredProgram* SimManager::activeProgram() { return _core->activeProgram(); } - const interpreter::IStoredProgram* SimManager::activeProgram() const { return _core->activeProgram(); } - - // Access the simulation core interface (non-const and const versions) - core::ISimulationCore* SimManager::simCoreInterface() { return _core.get(); } - const core::ISimulationCore* SimManager::simCoreInterface() const { return _core.get(); } - - // Access the concrete simulation core (non-const and const versions) - core::SimulationCore* SimManager::simCore() { return _core.get(); } - const core::SimulationCore* SimManager::simCore() const { return _core.get(); } - - // Set the integrator method for the current simulation run - void SimManager::setIntegrationMethod(integration::eIntegrationMethod method) { - _core->setIntegrationMethod(method); - } - const integration::eIntegrationMethod SimManager::integrationMethod() const { - return _core->integrationMethod(); - } - void SimManager::setADIntegrationMethod(integration::eAutoDiffIntegrationMethod method) { - _core->setADIntegrationMethod(method); - } - const integration::eAutoDiffIntegrationMethod SimManager::autoDiffIntegrationMethod() const { - return _core->autoDiffIntegrationMethod(); - } - - // Helper to replace the integrator method in the script text - // A hacky approach my idea, but it works for me and honestly im starting to write up the dissertation so IT WILL DO :) - // PS:If anyone has any better solution msg me - - // This seems to be the better solution? - static std::string replaceIntegratorInScript(const std::string& script, const std::string& methodName) { - std::regex re(R"((?i)set\s*\(\s*integrator\s*,\s*([a-z0-9_]+)\s*\))"); // case-insensitive regex to match my DSL command -> set(integrator, method) - std::string replacement = "set(integrator, " + methodName + ")"; - return std::regex_replace(script, re, replacement); - } - - // Run a script to completion synchronously with a specific integrator - bool SimManager::runScriptToCompletion(const std::string& scriptText, integration::eIntegrationMethod method) { - if (!hasRobot()) { return false; } - - // Map method enum to string name - static const char* names[] = { "euler", "midpoint", "heun", "ralston", "rk4", "rk45", "implicit_euler", "implicit_midpoint", "glrk2", "glrk3" }; - const std::string methodName = names[static_cast(method)]; - - // Replace the integrator method in the script text - std::string modifiedScript = replaceIntegratorInScript(scriptText, methodName); - - // Create program and parser (bound to headless core) - auto program = std::make_unique(_core.get()); - //if (scene::Object* o = getObject()) program->setDefaultObject(o); - auto parser = std::make_unique(program.get()); - - // Parse the modified script and start the program - parser->parse(modifiedScript); - program->start(); - return _core->runScriptToCompletion(program.get(), method); // this will block until the script finishes - } - - // -------------------------------------------------- - // INTERNAL REDNDERING PIPELINE - // -------------------------------------------------- - - // Initialize shadow map resources for cascaded shadow mapping - void SimManager::InitShadowResource(int baseRes) { - if (_shadowsInit) { - glDeleteFramebuffers(SimManager::NUM_CASCADES, _impl->_cascadeFBO); - glDeleteTextures(SimManager::NUM_CASCADES, _impl->_cascadeDepth); - } - - glGenFramebuffers(SimManager::NUM_CASCADES, _impl->_cascadeFBO); - glGenTextures(SimManager::NUM_CASCADES, _impl->_cascadeDepth); - - for (int i = 0; i < SimManager::NUM_CASCADES; i++) { - const int res = (i == 0) ? baseRes : (baseRes / 2); // 8192, 4096, 2048, 1024, 512, 256, 128 - - glBindTexture(GL_TEXTURE_2D, _impl->_cascadeDepth[i]); - glTexImage2D(GL_TEXTURE_2D, 0, GL_DEPTH_COMPONENT32F, res, res, 0, GL_DEPTH_COMPONENT, GL_FLOAT, nullptr); - - glTexParameteri(GL_TEXTURE_2D, GL_TEXTURE_MIN_FILTER, GL_NEAREST); - glTexParameteri(GL_TEXTURE_2D, GL_TEXTURE_MAG_FILTER, GL_NEAREST); - glTexParameteri(GL_TEXTURE_2D, GL_TEXTURE_WRAP_S, GL_CLAMP_TO_BORDER); - glTexParameteri(GL_TEXTURE_2D, GL_TEXTURE_WRAP_T, GL_CLAMP_TO_BORDER); - const float border[] = { 1.0f, 1.0f, 1.0f, 1.0f }; - glTexParameterfv(GL_TEXTURE_2D, GL_TEXTURE_BORDER_COLOR, border); - - glTexParameteri(GL_TEXTURE_2D, GL_TEXTURE_COMPARE_MODE, GL_COMPARE_REF_TO_TEXTURE); - glTexParameteri(GL_TEXTURE_2D, GL_TEXTURE_COMPARE_FUNC, GL_LEQUAL); - - glBindFramebuffer(GL_FRAMEBUFFER, _impl->_cascadeFBO[i]); - glFramebufferTexture2D(GL_FRAMEBUFFER, GL_DEPTH_ATTACHMENT, GL_TEXTURE_2D, _impl->_cascadeDepth[i], 0); - - glDrawBuffer(GL_NONE); - glReadBuffer(GL_NONE); - } - glBindFramebuffer(GL_FRAMEBUFFER, 0); - - _shadowsInit = true; - } - - // Initialize the IBL system with a default HDR environment map - void SimManager::InitIBL() { - _impl->_ibl = std::make_unique(); - _impl->_ibl->init((paths::assets() / "hdr" / "default_white.hdr").string()); - } - - // Render the world grid overlay in the viewport - void SimManager::WorldGridRender(scene::Camera* cam, int rtW) { - glEnable(GL_DEPTH_TEST); - glDepthFunc(GL_LEQUAL); - glDepthMask(GL_FALSE); - - const int msaa = std::max(1, _settingsCurrent.msaaSamples); - - if (msaa > 1) { - glDisable(GL_BLEND); - glEnable(GL_SAMPLE_ALPHA_TO_COVERAGE); - glEnable(GL_MULTISAMPLE); - - glEnable(GL_POLYGON_OFFSET_FILL); - glPolygonOffset(-0.2f, -0.2f); - } - else { - glDisable(GL_SAMPLE_ALPHA_TO_COVERAGE); - glDisable(GL_MULTISAMPLE); - glEnable(GL_BLEND); - glBlendFunc(GL_ONE, GL_ONE_MINUS_SRC_ALPHA); - } - - // Scale grid line thickness so smaller RTs (quad mode) match single view appearance - const float fullW = std::max(1.0f, _internalSize.x); - const float internalScale = (float)std::max(1, rtW) / fullW; - - _impl->_worldGridShader->use(); - _impl->_worldGridShader->setMat4(cam->getViewProjection(), "gVP"); - _impl->_worldGridShader->setMat4(cam->getViewMatrix(), "gView"); - _impl->_worldGridShader->setVec3(cam->getPosition(), "gCameraWorldPos"); - _impl->_worldGridShader->setFlt1(_settingsCurrent.renderScale, "gRenderScale"); - _impl->_worldGridShader->setFlt1(internalScale, "gInternalScale"); - _impl->_worldGridShader->setFlt1(2500.0f, "gGridSize"); - - glBindVertexArray(_impl->_worldGridVAO); - glDrawArrays(GL_TRIANGLES, 0, 6); - glBindVertexArray(0); - - glDisable(GL_POLYGON_OFFSET_FILL); - glDisable(GL_SAMPLE_ALPHA_TO_COVERAGE); - - glDepthMask(GL_TRUE); - glDisable(GL_BLEND); - glDepthFunc(GL_LESS); - } - - // Render the meshes in the scene using the currently selected shader mode, setting appropriate uniforms for each mode - void SimManager::MeshRender(scene::Camera* cam) { - glEnable(GL_DEPTH_TEST); - glDepthFunc(GL_LESS); - glDepthMask(GL_TRUE); - glDisable(GL_BLEND); - - shaders::Shader* shader = nullptr; - - switch (currentShaderMode) { - case ShaderMode::Basic: - shader = _impl->_shaderBasic.get(); - break; - case ShaderMode::Lit: - shader = _impl->_shaderLit.get(); - break; - case ShaderMode::PBR: - shader = _impl->_shaderPBR.get(); - break; - } - - if (!shader) { - LOG_ERROR("Shader is NULL after switch!"); - return; - } - - shader->use(); - shader->setBool(false, "isFloor"); - - // Only PBR know about cascades & those uniforms - if (currentShaderMode == ShaderMode::PBR) { - for (int i = 0; i < NUM_CASCADES; i++) { - glActiveTexture(GL_TEXTURE5 + i); - glBindTexture(GL_TEXTURE_2D, _impl->_cascadeDepth[i]); - shader->setInt1(5 + i, "cascadeShadowMap[" + std::to_string(i) + "]"); - shader->setMat4(_impl->_lightSpaceMatrixCascade[i], "lightSpaceMatrix[" + std::to_string(i) + "]"); - } - - // Convert fractional splits (0..1 of far) into world-space distances - const float nearPlane = cam->getNear(); - const float farPlane = cam->getFar(); - - const float splitFrac0 = _cascadeSplits[0]; - const float splitFrac1 = _cascadeSplits[1]; - - const float splitDist0 = nearPlane + splitFrac0 * farPlane; - const float splitDist1 = nearPlane + splitFrac1 * farPlane; - - shader->setFlt2(splitDist0, splitDist1, "cascadeSplits"); - } - - // Camera / SunLight / light common to all mesh shaders - cam->update(shader); - _impl->_light->update(shader); - - // Main mesh rendering loop - for (auto& obj : _impl->_objects) { - if (!obj || !obj->getMesh()) continue; - - if (_impl->_cameraFollowTarget == obj.get()) { cam->setFollowTarget(obj->transform.position, obj->transform.rotQ); } - - glm::mat4 model = obj->transform.toMatrix() * obj->getMesh()->localTransform; - shader->setMat4(model, "model"); - - // Per-mode material uniforms - switch (currentShaderMode) { - case ShaderMode::Basic: - // (IMPORTANT) mesh_basic.frag needs: uniform vec3 color; - shader->setVec3(obj->material.albedo, "albedo"); - break; - - case ShaderMode::Lit: - // (IMPORTANT) mesh_lit.frag needs: albedo, lightPosition, lightColour, lightIntensity, camPos - shader->setVec3(obj->material.albedo, "albedo"); - shader->setVec3(_impl->_light->getPosition(), "lightPosition"); - shader->setFlt1(_impl->_light->getIntensity(), "lightIntensity"); - shader->setVec3(_impl->_light->getColour(), "lightColour"); - shader->setVec3(cam->getPosition(), "camPos"); - break; - - case ShaderMode::PBR: - // Per-mesh PBR material properties - shader->setVec3(obj->material.albedo, "albedo"); - shader->setFlt1(obj->material.metallic, "metallic"); - shader->setFlt1(obj->material.roughness, "roughness"); - shader->setFlt1(1.0f, "ao"); - shader->setFlt1(_settingsCurrent.ambientStrength, "ambientStrength"); - - shader->setVec3(glm::normalize(_impl->_light->getDirection()), "lightDirection"); - shader->setFlt1(_impl->_light->getIntensity(), "lightIntensity"); - shader->setVec3(_impl->_light->getColour(), "lightColour"); - shader->setVec3(cam->getPosition(), "camPos"); - - shader->setInt1(0, "irradianceMap"); - shader->setInt1(1, "prefilterMap"); - shader->setInt1(2, "brdfLUT"); - - glActiveTexture(GL_TEXTURE0); - glBindTexture(GL_TEXTURE_CUBE_MAP, _impl->_ibl->getIrradianceMap()); - - glActiveTexture(GL_TEXTURE1); - glBindTexture(GL_TEXTURE_CUBE_MAP, _impl->_ibl->getPrefilterMap()); - - glActiveTexture(GL_TEXTURE2); - glBindTexture(GL_TEXTURE_2D, _impl->_ibl->getBRDFLUT()); - - break; - } - - _impl->currentShader = shader; // for external access - obj->getMesh()->render(); - } - } - - // Render the shadow maps for each cascade by rendering the scene from the light's perspective into the depth textures - void SimManager::ShadowPass(scene::Camera* cam) { - float nearPlane = cam->getNear(); - float farPlane = cam->getFar(); - - float cascadeNear[NUM_CASCADES]{}; - float cascadeFar[NUM_CASCADES]{}; - - cascadeNear[0] = nearPlane; - cascadeFar[0] = nearPlane + _cascadeSplits[0] * (farPlane); - - cascadeNear[1] = cascadeFar[0]; - cascadeFar[1] = nearPlane + _cascadeSplits[1] * (farPlane); - - glEnable(GL_POLYGON_OFFSET_FILL); - glPolygonOffset(2.0f, 4.0f); - - for (int i = 0; i < NUM_CASCADES; i++) { - _impl->_lightSpaceMatrixCascade[i] = LightSpaceMatrix(cam, cascadeNear[i], cascadeFar[i]); - - // set viewport to shadow map size - int baseRes = _settingsCurrent.shadowMapRes; - int res = (i == 0) ? baseRes : (baseRes / 2); // 4096, 2048, 1024, 512, 256, 128 - glViewport(0, 0, res, res); - - // render to cascade FBO - glBindFramebuffer(GL_FRAMEBUFFER, _impl->_cascadeFBO[i]); - glClear(GL_DEPTH_BUFFER_BIT); - - // render scene from light's point of view - _impl->_shadowShader->use(); - _impl->_shadowShader->setMat4(_impl->_lightSpaceMatrixCascade[i], "lightSpaceMatrix"); - - // main mesh - for (auto& obj : _impl->_objects) { - if (!obj || !obj->getMesh()) continue; - - glm::mat4 model = obj->transform.toMatrix() * obj->getMesh()->localTransform; - - _impl->_shadowShader->setMat4(model, "model"); - obj->getMesh()->render(); - } - } - glBindFramebuffer(GL_FRAMEBUFFER, 0); - glViewport(0, 0, (int)_internalSize.x, (int)_internalSize.y); - - glDisable(GL_POLYGON_OFFSET_FILL); - } - - // Compute the light-space matrix for a given camera frustum slice (cascade) - glm::mat4 SimManager::LightSpaceMatrix(scene::Camera* cam, float nearPlane, float farPlane) { - std::array corners = cam->getFrustumCornersWorldSpace(nearPlane, farPlane); - - glm::vec3 lightDir = glm::normalize(_impl->_light->getDirection()); - - // Fake camera position far along direction - glm::vec3 lightPos = -lightDir * 50.0f; - - glm::mat4 lightView = glm::lookAt( - lightPos, - glm::vec3(0.0f), - glm::vec3(0, 1, 0) - ); - - float minX = FLT_MAX, maxX = -FLT_MAX; - float minY = FLT_MAX, maxY = -FLT_MAX; - float minZ = FLT_MAX, maxZ = -FLT_MAX; - - for (auto& corner : corners) { - glm::vec4 trf = lightView * glm::vec4(corner); - minX = std::min(minX, trf.x); - maxX = std::max(maxX, trf.x); - minY = std::min(minY, trf.y); - maxY = std::max(maxY, trf.y); - minZ = std::min(minZ, trf.z); - maxZ = std::max(maxZ, trf.z); - } - - // Compute cascade center in light space - glm::vec3 center = { - 0.5f * (minX + maxX), - 0.5f * (minY + maxY), - 0.5f * (minZ + maxZ) - }; - - // Cascade radius (half-size of the bounding sphere) - float radius = glm::length(glm::vec3(maxX - minX, maxY - minY, 0.0f)) * 0.5f; - - int shadowMapResolution = _settingsCurrent.shadowMapRes; - - // The size of one texel in world-space - float worldUnitsPerTexel = (radius * 2.0f) / shadowMapResolution; - - // Snap X and Y (Z never snapped) - center.x = std::floor(center.x / worldUnitsPerTexel) * worldUnitsPerTexel; - center.y = std::floor(center.y / worldUnitsPerTexel) * worldUnitsPerTexel; - - // Recompute min/max using snapped centre - minX = center.x - radius; - maxX = center.x + radius; - minY = center.y - radius; - maxY = center.y + radius; - - glm::mat4 lightProj = glm::ortho(minX, maxX, minY, maxY, minZ - 20.0f, maxZ + 20.0f); - - return lightProj * lightView; - } - - // Render the skybox using the IBL environment cubemap - void SimManager::SkyboxRender(scene::Camera* cam) { - glm::mat4 view = cam->getViewMatrix(); - glm::mat4 projection = cam->getProjection(); - - _impl->_skybox->setEnvironmentTexture(_impl->_ibl->getEnvCubemap()); - _impl->_skybox->render(projection, view); - } - - // Load a new HDR environment map for IBL - std::string SimManager::getDefaultHDR() const { return (paths::assets() / "hdr"/ "default_white.hdr").string(); } - - // Load a new HDR environment map for IBL - const shaders::Shader* SimManager::getCurrentShader() const { return _impl->currentShader; } - void SimManager::applyRenderSettings(const render::RenderSettings& s, render::ResolutionPreset r) { applyRenderProfile(s, r); } - - void SimManager::applyRenderProfile(const render::RenderSettings& s, render::ResolutionPreset r) { - const bool first = !_settingsValid; - - const bool shadowResChanged = first || (s.shadowMapRes != _settingsCurrent.shadowMapRes); - const bool msaaChanged = first || (s.msaaSamples != _settingsCurrent.msaaSamples); - const bool renderScaleChanged = first || (s.renderScale != _settingsCurrent.renderScale); - const bool presetChanged = first || (r != _resCurrent); - - if (shadowResChanged) { - InitShadowResource(s.shadowMapRes); - } - - _settingsCurrent = s; - _resCurrent = r; - - if (msaaChanged || renderScaleChanged || presetChanged) { - for (auto& v : _impl->_views) { v.w = v.h = 0; v.displayW = v.displayH = 0; } - } - - glm::vec2 px = getPresetResolutionPx(); - _internalSize = px; - for (auto& v : _impl->_views) { v.w = v.h = 0; } // internal invalidation - - //LOG_INFO("Render settings applied: resPreset=%d shadowRes=%d msaa=%d renderScale=%.2f", (int)r, _settingsCurrent.shadowMapRes, _settingsCurrent.msaaSamples, _settingsCurrent.renderScale); - D_RUNTIME("Render settings applied: resPreset=%d shadowRes=%d msaa=%d renderScale=%.2f", (int)r, _settingsCurrent.shadowMapRes, _settingsCurrent.msaaSamples, _settingsCurrent.renderScale); - - _settingsValid = true; - } - - // Get the pixel dimensions for the current resolution preset - glm::vec2 SimManager::getPresetResolutionPx() const { - switch (_resCurrent) { - case render::ResolutionPreset::R_720p: return { 1280, 720 }; - case render::ResolutionPreset::R_1080p: return { 1920, 1080 }; - case render::ResolutionPreset::R_1440p: return { 2560, 1440 }; - case render::ResolutionPreset::R_4K: return { 3840, 2160 }; - default: return { 1920, 1080 }; - } - } - - glm::vec2 SimManager::getInternalResolutionSizePx() const { - return getPresetResolutionPx(); // no scale - } - - void SimManager::resetHDRToPreset() { - _hdrUserOverride = false; - const std::string hdr = getDefaultHDR(); - if (hdr != _activeHDRPath) { loadNewHDR(hdr); } - } - - void SimManager::reloadAllShaders() { - _impl->_shaderBasic->load((paths::assets() / "shaders" / "vs_pbr.vert.glsl").string(), (paths::assets() / "shaders" / "mesh_basic.frag.glsl").string()); - _impl->_shaderLit->load((paths::assets() / "shaders" / "vs_pbr.vert.glsl").string(), (paths::assets() / "shaders" / "mesh_lit.frag.glsl").string()); - _impl->_shaderPBR->load((paths::assets() / "shaders" / "vs_pbr.vert.glsl").string(), (paths::assets() / "shaders" / "mesh_pbr.frag.glsl").string()); - _impl->_ssaoShader->load((paths::assets() / "shaders" / "post.vert.glsl").string(), (paths::assets() / "shaders" / "ssao.frag.glsl").string()); - _impl->_ssaoBlurShader->load((paths::assets() / "shaders" / "post.vert.glsl").string(), (paths::assets() / "shaders" / "ssao_blur.frag.glsl").string()); - - //LOG_INFO("All shaders reloaded from disk."); - D_INFO_ONCE("All shaders reloaded from disk."); - } - - void SimManager::setLightColour(const glm::vec3& colour) { _impl->_light->_colour = colour; } // -------------------------------------------------- // INPUT HANDLING @@ -1921,23 +314,4 @@ namespace gui { void gui::SimManager::resetMouseDelta() { _firstMouse = true; } // --- Helpers --- - - // Begin Control Panel Helper - void SimManager::beginSimManager(const char* id) { - ImGui::PushStyleVar(ImGuiStyleVar_WindowPadding, ImVec2(12.0f, 10.0f)); - ImGui::PushStyleVar(ImGuiStyleVar_ChildRounding, 6.0f); - ImGui::PushStyleVar(ImGuiStyleVar_FramePadding, ImVec2(8.0f, 5.0f)); - ImGui::PushStyleVar(ImGuiStyleVar_ItemSpacing, ImVec2(10.0f, 8.0f)); - - ImGui::BeginChild(id, ImVec2(0, 0), true, - ImGuiChildFlags_AlwaysUseWindowPadding | - ImGuiWindowFlags_NoScrollbar | - ImGuiWindowFlags_NoScrollWithMouse); - } - - // End Control Panel Helper - void SimManager::endSimManager() { - ImGui::EndChild(); - ImGui::PopStyleVar(4); - } } \ No newline at end of file From c3f782c961f29d83ffe3730cd699d7bf180d2ff2 Mon Sep 17 00:00:00 2001 From: saltyjoss Date: Thu, 4 Jun 2026 19:36:09 +0100 Subject: [PATCH 107/210] refactor: Port application from GLFW to Qt (QOpenGLWidget) NOTES (partially generated): * Refactored main window and rendering to use Qt (QApplication, QMainWindow, QOpenGLWidget) instead of GLFW. * Updated `Application`, `DSFE_MainWindow`, `ProjectPage`, and `ViewportWidget` to support Qt-based management and input. * Introduced eKeyCode for unified key handling. * Adapted `SimulationManager` and rendering pipeline for Qt, including new `present.frag.glsl` shader. * Removed GLFW-specific code and update CMakeLists.txt accordingly. * Added KeyCode.h for cross-platform input. TODO: * Test loading of dynamic systems * Start to really remove GLFW from code * Cleanup existing code * Replicate process for ControlPanel, DSL editor, and DebugPanel(CLI) * Remove hardcoded plugin path and cmake path, replace with permanent solution --- .../assets/shaders/present.frag.glsl | 11 ++ DSFE_App/DSFE_Engine/src/Main.cpp | 3 +- DSFE_App/DSFE_GUI/CMakeLists.txt | 14 +- DSFE_App/DSFE_GUI/include/Application.h | 38 +++-- .../include/MainWindow/DSFE_MainWindow.h | 4 +- .../MainWindow/DockWidgets/ViewportDock.h | 13 -- .../MainWindow/Widgets/ViewportWidget.h | 38 ++++- .../MainWindow/Workspace/ProjectPage.h | 4 +- DSFE_App/DSFE_GUI/include/Platform/KeyCode.h | 13 ++ DSFE_App/DSFE_GUI/include/Scene/Camera.h | 8 + .../include/Scene/Manager/SimImplementation.h | 15 +- .../include/Scene/SimulationManager.h | 16 +- DSFE_App/DSFE_GUI/src/Application.cpp | 85 +++++------ .../src/MainWindow/DSFE_MainWindow.cpp | 9 +- .../MainWindow/DockWidgets/ViewportDock.cpp | 11 -- .../src/MainWindow/Widgets/ViewportWidget.cpp | 140 +++++++++++++++++- .../src/MainWindow/Workspace/ProjectPage.cpp | 8 +- .../DSFE_GUI/src/Platform/WindowManager.cpp | 3 +- .../src/Rendering/OpenGLBufferManager.cpp | 11 +- DSFE_App/DSFE_GUI/src/Scene/Camera.cpp | 14 +- .../DSFE_GUI/src/Scene/Manager/SimBackend.cpp | 2 +- .../DSFE_GUI/src/Scene/SimulationManager.cpp | 90 ++++++++--- 22 files changed, 408 insertions(+), 142 deletions(-) create mode 100644 DSFE_App/DSFE_Engine/assets/shaders/present.frag.glsl delete mode 100644 DSFE_App/DSFE_GUI/include/MainWindow/DockWidgets/ViewportDock.h create mode 100644 DSFE_App/DSFE_GUI/include/Platform/KeyCode.h delete mode 100644 DSFE_App/DSFE_GUI/src/MainWindow/DockWidgets/ViewportDock.cpp diff --git a/DSFE_App/DSFE_Engine/assets/shaders/present.frag.glsl b/DSFE_App/DSFE_Engine/assets/shaders/present.frag.glsl new file mode 100644 index 00000000..054f2500 --- /dev/null +++ b/DSFE_App/DSFE_Engine/assets/shaders/present.frag.glsl @@ -0,0 +1,11 @@ +#version 460 core + +in vec2 uv; + +out vec4 FragColour; + +uniform sampler2D screenTexture; + +void main() { + FragColour = vec4(1,0,0,1); +} \ No newline at end of file diff --git a/DSFE_App/DSFE_Engine/src/Main.cpp b/DSFE_App/DSFE_Engine/src/Main.cpp index 39994ac8..571390d0 100644 --- a/DSFE_App/DSFE_Engine/src/Main.cpp +++ b/DSFE_App/DSFE_Engine/src/Main.cpp @@ -79,8 +79,7 @@ int main(int argc, char** argv) { } Application app("DSFE"); - app.run(); - return 0; + return app.run(); } // Batch mode: parse batch-specific arguments diff --git a/DSFE_App/DSFE_GUI/CMakeLists.txt b/DSFE_App/DSFE_GUI/CMakeLists.txt index 8e04f4ce..2fbad3fc 100644 --- a/DSFE_App/DSFE_GUI/CMakeLists.txt +++ b/DSFE_App/DSFE_GUI/CMakeLists.txt @@ -40,7 +40,6 @@ target_include_directories(stb INTERFACE set(MAIN_WINDOW_SRC src/MainWindow/DSFE_MainWindow.cpp src/MainWindow/Workspace/ProjectPage.cpp - src/MainWindow/DockWidgets/ViewportDock.cpp src/MainWindow/Widgets/ViewportWidget.cpp ) @@ -64,6 +63,17 @@ set(SCENE_SRC src/Scene/SimulationManager.cpp ) +set(MANAGER_SRC + src/Scene/Manager/SimBackend.cpp + src/Scene/Manager/SimCamera.cpp + src/Scene/Manager/SimObjects.cpp + src/Scene/Manager/SimPostFX.cpp + src/Scene/Manager/SimRendering.cpp + src/Scene/Manager/SimRobots.cpp + src/Scene/Manager/SimUI.cpp + src/Scene/Manager/SimViewport.cpp +) + set(UI_SRC src/ui/CommandScriptEditor.cpp src/ui/ControlPanel.cpp @@ -92,6 +102,7 @@ target_sources(DSFE_GUI PRIVATE ${MAIN_WINDOW_SRC} ${RENDER_SRC} ${SCENE_SRC} + ${MANAGER_SRC} ${UI_SRC} ${ROBOT_SRC} ${PLATFORM_SRC} @@ -132,6 +143,7 @@ target_include_directories(DSFE_GUI ${CMAKE_CURRENT_SOURCE_DIR} ${CMAKE_CURRENT_SOURCE_DIR}/include ${CMAKE_CURRENT_SOURCE_DIR}/include/MainWindow + ${CMAKE_CURRENT_SOURCE_DIR}/include/Scene PRIVATE ${imfilebrowser_ROOT} diff --git a/DSFE_App/DSFE_GUI/include/Application.h b/DSFE_App/DSFE_GUI/include/Application.h index 994129f2..f2ddd436 100644 --- a/DSFE_App/DSFE_GUI/include/Application.h +++ b/DSFE_App/DSFE_GUI/include/Application.h @@ -5,10 +5,14 @@ #include #include +#include +#include "Platform/Logger.h" // forward declarations -namespace window { class GLWindow; } -namespace scene { class Camera; } +//namespace window { class GLWindow; } +class QApplication; +namespace gui { class SimManager; } +namespace window { class DSFE_MainWindow; } class DSFE_GUI_API Application { public: @@ -16,18 +20,28 @@ class DSFE_GUI_API Application { ~Application(); static Application& Instance() { return *sInstance; } - void run(); + int run(); private: static Application* sInstance; -#ifdef _MSC_VER -#pragma warning(push) -#pragma warning(disable: 4251) // Suppress C4251 for private members -#endif - std::unique_ptr _window; - std::unique_ptr _camera; -#ifdef _MSC_VER -#pragma warning(pop) -#endif + std::string _name; + + int _qtArgc = 0; + std::vector _qtArgStorage; + std::vector _qtArgv; + + std::unique_ptr _qtApp; + std::unique_ptr _sim; + std::unique_ptr _mainW; + +//#ifdef _MSC_VER +//#pragma warning(push) +//#pragma warning(disable: 4251) // Suppress C4251 for private members +//#endif +// std::unique_ptr _window; +// std::unique_ptr _camera; +//#ifdef _MSC_VER +//#pragma warning(pop) +//#endif }; \ No newline at end of file diff --git a/DSFE_App/DSFE_GUI/include/MainWindow/DSFE_MainWindow.h b/DSFE_App/DSFE_GUI/include/MainWindow/DSFE_MainWindow.h index 5ed46a4c..94264db7 100644 --- a/DSFE_App/DSFE_GUI/include/MainWindow/DSFE_MainWindow.h +++ b/DSFE_App/DSFE_GUI/include/MainWindow/DSFE_MainWindow.h @@ -3,9 +3,11 @@ #include +namespace gui { class SimManager; } + namespace window { class DSFE_MainWindow : public QMainWindow { public: - explicit DSFE_MainWindow(QWidget* parent = nullptr); + explicit DSFE_MainWindow(gui::SimManager* sim, QWidget* parent = nullptr); }; } diff --git a/DSFE_App/DSFE_GUI/include/MainWindow/DockWidgets/ViewportDock.h b/DSFE_App/DSFE_GUI/include/MainWindow/DockWidgets/ViewportDock.h deleted file mode 100644 index 7ea44d8a..00000000 --- a/DSFE_App/DSFE_GUI/include/MainWindow/DockWidgets/ViewportDock.h +++ /dev/null @@ -1,13 +0,0 @@ -// DSFE_GUI ViewportDock.h -#pragma once - -#include - -class ViewportWidget; - -namespace dockwidgets { - class ViewportDock : public QDockWidget { - public: - explicit ViewportDock(QWidget* parent = nullptr); - }; -} // namespace dockwidgets \ No newline at end of file diff --git a/DSFE_App/DSFE_GUI/include/MainWindow/Widgets/ViewportWidget.h b/DSFE_App/DSFE_GUI/include/MainWindow/Widgets/ViewportWidget.h index 51868f9e..006de699 100644 --- a/DSFE_App/DSFE_GUI/include/MainWindow/Widgets/ViewportWidget.h +++ b/DSFE_App/DSFE_GUI/include/MainWindow/Widgets/ViewportWidget.h @@ -2,10 +2,44 @@ #pragma once #include +#include +#include +#include +#include +#include +#include + +#include + +namespace gui { class SimManager; enum class eKeyCode; } namespace widgets { - class ViewportWidget : public QOpenGLWidget { + class ViewportWidget : public QOpenGLWidget, protected QOpenGLFunctions_4_5_Core { public: - explicit ViewportWidget(QWidget* parent = nullptr); + explicit ViewportWidget(gui::SimManager* sim, QWidget* parent = nullptr); + + protected: + void initializeGL() override; + void resizeGL(int w, int h) override; + void paintGL() override; + + void keyPressEvent(QKeyEvent* event) override; + void keyReleaseEvent(QKeyEvent* event) override; + + void mousePressEvent(QMouseEvent* event) override; + void mouseReleaseEvent(QMouseEvent* event) override; + void mouseMoveEvent(QMouseEvent* event) override; + + void wheelEvent(QWheelEvent* event) override; + + private: + gui::SimManager* _sim = nullptr; + QElapsedTimer _frameTimer; + qint64 _lastNs = 0; + QTimer _updateTimer; + QPoint _screenCenter; + + bool _mouseCaptured = false; + std::unordered_set _pressedKeys; }; } \ No newline at end of file diff --git a/DSFE_App/DSFE_GUI/include/MainWindow/Workspace/ProjectPage.h b/DSFE_App/DSFE_GUI/include/MainWindow/Workspace/ProjectPage.h index bed7285a..eac14829 100644 --- a/DSFE_App/DSFE_GUI/include/MainWindow/Workspace/ProjectPage.h +++ b/DSFE_App/DSFE_GUI/include/MainWindow/Workspace/ProjectPage.h @@ -3,9 +3,11 @@ #include +namespace gui { class SimManager; } + namespace Workspace { class ProjectPage : public QWidget { public: - explicit ProjectPage(QWidget* parent = nullptr); + explicit ProjectPage(gui::SimManager* sim, QWidget* parent = nullptr); }; } // namespace Workspace \ No newline at end of file diff --git a/DSFE_App/DSFE_GUI/include/Platform/KeyCode.h b/DSFE_App/DSFE_GUI/include/Platform/KeyCode.h new file mode 100644 index 00000000..fcd1356d --- /dev/null +++ b/DSFE_App/DSFE_GUI/include/Platform/KeyCode.h @@ -0,0 +1,13 @@ +// DSFE_GUI KeyCode.h +#pragma once + +namespace gui { + enum class eKeyCode { + W = 0, A = 1, S = 2, D = 3, + LShift = 4, Ctrl = 5, + Space = 6, + Tab = 7, + Esc = 8, + Unknown = 9 + }; +} \ No newline at end of file diff --git a/DSFE_App/DSFE_GUI/include/Scene/Camera.h b/DSFE_App/DSFE_GUI/include/Scene/Camera.h index d738b281..bc36f9ed 100644 --- a/DSFE_App/DSFE_GUI/include/Scene/Camera.h +++ b/DSFE_App/DSFE_GUI/include/Scene/Camera.h @@ -138,6 +138,14 @@ namespace scene { void setMinDistance(float d) { _minDistance = glm::max(0.05f, d); } private: + enum class eKeyCode { + W, A, S, D, + LShift, Ctrl, + Space, + Tab, + Unknown + }; + void rebuildAxesFromFrontUp_(const glm::vec3& front, const glm::vec3& upHint); void updateProjectionMatrix() { if (!std::isfinite(_FOV) || _FOV <= 0.001f) { _FOV = glm::radians(70.0f); } diff --git a/DSFE_App/DSFE_GUI/include/Scene/Manager/SimImplementation.h b/DSFE_App/DSFE_GUI/include/Scene/Manager/SimImplementation.h index 6b4303d3..12ef54e7 100644 --- a/DSFE_App/DSFE_GUI/include/Scene/Manager/SimImplementation.h +++ b/DSFE_App/DSFE_GUI/include/Scene/Manager/SimImplementation.h @@ -49,7 +49,7 @@ namespace gui { enum class ViewMode { Single, Quad }; // Current View Mode - ViewMode viewMode = ViewMode::Single; + ViewMode viewMode; // Viewport Structure struct Viewport { @@ -69,7 +69,7 @@ namespace gui { // Viewports std::array _views; - VID activeView = VID::Manual; + VID activeView; // Skybox & IBL std::unique_ptr _ibl; @@ -77,6 +77,7 @@ namespace gui { // Post-Processing Shader std::unique_ptr _postShader; + std::unique_ptr _presentShader; std::shared_ptr _shaderBasic; std::shared_ptr _shaderLit; std::shared_ptr _shaderPBR; @@ -131,8 +132,15 @@ namespace gui { std::vector _ssaoKernel; Impl(SimManager& owner) { + activeView = VID::Manual; + viewMode = ViewMode::Single; + } + + void initGLResources(SimManager& owner) { _postShader = std::make_unique(); _postShader->load((paths::assets() / "shaders" / "post.vert.glsl").string(), (paths::assets() / "shaders" / "post.frag.glsl").string()); + _presentShader = std::make_unique(); + _presentShader->load((paths::assets() / "shaders" / "post.vert.glsl").string(), (paths::assets() / "shaders" / "present.frag.glsl").string()); glGenVertexArrays(1, &_fullscreenVAO); @@ -254,7 +262,6 @@ namespace gui { // Robot system with mesh loading (for normal simulation) _robotSystem = std::make_unique(); - _robotRenderer = std::make_unique(); // SSAO shaders @@ -497,6 +504,8 @@ namespace gui { glDepthMask(GL_TRUE); glDepthFunc(GL_LESS); + LOG_INFO_ONCE("Background = %.3f %.3f %.3f", owner._backgroundColour.r, owner._backgroundColour.g, owner._backgroundColour.b); + glClearColor(owner._backgroundColour.r, owner._backgroundColour.g, owner._backgroundColour.b, owner._backgroundAlpha); glClear(GL_COLOR_BUFFER_BIT | GL_DEPTH_BUFFER_BIT); diff --git a/DSFE_App/DSFE_GUI/include/Scene/SimulationManager.h b/DSFE_App/DSFE_GUI/include/Scene/SimulationManager.h index b0dda2f2..a86bc4d5 100644 --- a/DSFE_App/DSFE_GUI/include/Scene/SimulationManager.h +++ b/DSFE_App/DSFE_GUI/include/Scene/SimulationManager.h @@ -6,6 +6,7 @@ #include #include #include +#include #include "Platform/StudyRunner.h" #include "Rendering/ModelGroup.h" @@ -13,6 +14,8 @@ #include "ui/RenderPreset.h" #include "FpsCounter.h" +#include "Platform/KeyCode.h" + #include "Analysis/Telemetry.h" #include "Analysis/MetricLogger.h" @@ -52,6 +55,8 @@ namespace gui { // Forward Declarations for Axis Orientator class AxisOrientator; + // Forward Declarations for eKeyCode + enum class eKeyCode; // Control Modes & Camera enum class ControlMode { @@ -69,6 +74,9 @@ namespace gui { // OpenGL Initialisation void initGL(); + void renderViewport(int w, int h); + void setDisplaySize(int w, int h); + // Light scene::Light* getLight(); void setLightColour(const glm::vec3& c); @@ -164,6 +172,8 @@ namespace gui { void tick(double dt); void resize(int32_t width, int32_t height); + void setPresentationFBO(GLuint fbo) { _presentationFBO = fbo; } + void syncRobotToScene(); // Scene Objects Management @@ -261,8 +271,8 @@ namespace gui { // Input Handling void processMovementKey(int key, float delta); - void handleContinuousMovement(GLFWwindow* window, float dt); - void handleMouseLook(GLFWwindow* window, double xpos, double ypos); + void handleContinuousMovement(const std::unordered_set& pressedKeys, float dt); + void handleMouseLook(double xpos, double ypos, bool mouseCaptured); void onMouseMove(double x, double y, scene::eInputButton button); void onMouseWheel(double delta); void resetMouseDelta(); @@ -330,6 +340,8 @@ namespace gui { robots::TrajRefBuffer _trajRefBuffer; // Buffer for logging trajectory reference data each step bool _telemetryBegun = false; + GLuint _presentationFBO = 0; // FBO for final post-processed output to the screen + // Environment & Lighting render::RenderSettings _settingsCurrent{}; render::ResolutionPreset _resCurrent = render::ResolutionPreset::R_1080p; diff --git a/DSFE_App/DSFE_GUI/src/Application.cpp b/DSFE_App/DSFE_GUI/src/Application.cpp index 5e95f7e1..5df7a13a 100644 --- a/DSFE_App/DSFE_GUI/src/Application.cpp +++ b/DSFE_App/DSFE_GUI/src/Application.cpp @@ -1,77 +1,68 @@ // DSFE_GUI Application.cpp #include "Application.h" -#include "Platform/WindowManager.h" #include "MainWindow/DSFE_MainWindow.h" -#include "Scene/Camera.h" - -#ifdef __gl_h_ -#undef __gl_h_ -#endif -#include - -#include +#include "Scene/SimulationManager.h" #include "Platform/Paths.h" #include "EngineLib/LogMacros.h" +#include #include -#include -#include +#include +#include +#include namespace fs = std::filesystem; // Initialise the static instance pointer to nullptr Application* Application::sInstance = nullptr; -static QApplication* gQtApp = nullptr; -static window::DSFE_MainWindow* gQtWindow = nullptr; - // Constructor: Initialises paths, sets up data manager, and creates the main application window -Application::Application(const std::string& appName) { +Application::Application(const std::string& appName) : _name(appName) { paths::init(); - LOG_INFO("Root path: %s", paths::root().string().c_str()); LOG_INFO("Assets path: %s", paths::assets().string().c_str()); LOG_INFO("Configs path: %s", paths::configs().string().c_str()); LOG_INFO("Logs path: %s", paths::logs().string().c_str()); LOG_INFO("Runs path: %s", paths::runs().string().c_str()); - int winW = 1920, winH = 1080; + qputenv("QSG_RHI_BACKEND", "opengl"); +#if defined(Q_OS_WIN) + QCoreApplication::setAttribute(Qt::AA_UseDesktopOpenGL); +#endif + QCoreApplication::addLibraryPath("C:/Qt/6.11.1/msvc2022_64/plugins"); - if (glfwInit()) { - const GLFWvidmode* mode = glfwGetVideoMode(glfwGetPrimaryMonitor()); - if (mode) { - winH = (int)(mode->height * 0.8f); - winW = (winH * 16) / 9; - // Clamp width to 90% of monitor width in case ultra-wide - if (winW > (int)(mode->width * 0.9f)) { - winW = (int)(mode->width * 0.9f); - winH = (winW * 9) / 16; - } - LOG_INFO("Monitor: %dx%d -> Window: %dx%d (16:9)", mode->width, mode->height, winW, winH); - } - glfwTerminate(); // OpenGLContext::init will call glfwInit again - } + _qtArgStorage.clear(); + _qtArgStorage.emplace_back(_name.empty() ? "DSFE" : _name); + _qtArgc = static_cast(_qtArgStorage.size()); + _qtArgv.clear(); + _qtArgv.reserve(_qtArgStorage.size()); + for (std::string& arg : _qtArgStorage) { _qtArgv.push_back(arg.data()); } + + _qtApp = std::make_unique(_qtArgc, _qtArgv.data()); + + QSurfaceFormat format; + format.setProfile(QSurfaceFormat::CoreProfile); + format.setVersion(4, 5); + format.setDepthBufferSize(24); + format.setStencilBufferSize(8); + QSurfaceFormat::setDefaultFormat(format); - if (!gQtApp) { - QCoreApplication::addLibraryPath("C:/Qt/6.11.1/msvc2022_64/plugins"); - int argc = 0; - gQtApp = new QApplication(argc, nullptr); - gQtWindow = new window::DSFE_MainWindow(); - gQtWindow->show(); + _sim = std::make_unique(); + + int winW = 1920, winH = 1080; + if (QScreen* screen = QGuiApplication::primaryScreen()) { + const QRect g = screen->geometry(); + winH = int(g.height() * 0.8f); // maintain 16:9 aspect ratio and fit within 90% of screen height + winW = (winH * 16) / 9; // maintain 16:9 aspect ratio } - _window = std::make_unique(); - _window->init(winW, winH, appName); + _mainW = std::make_unique(_sim.get()); + _mainW->resize(winW, winH); + _mainW->show(); } Application::~Application() = default; -// Main application loop: Continues running until the window signals to close, updating and rendering each frame -void Application::run() { - while (_window->isRunning() && !_window->shouldClose()) { - QCoreApplication::processEvents(); - - _window->update(); - _window->render(); - } +int Application::run() { + return _qtApp ? _qtApp->exec() : 0; } \ No newline at end of file diff --git a/DSFE_App/DSFE_GUI/src/MainWindow/DSFE_MainWindow.cpp b/DSFE_App/DSFE_GUI/src/MainWindow/DSFE_MainWindow.cpp index 8c2ea55a..3a06c138 100644 --- a/DSFE_App/DSFE_GUI/src/MainWindow/DSFE_MainWindow.cpp +++ b/DSFE_App/DSFE_GUI/src/MainWindow/DSFE_MainWindow.cpp @@ -1,14 +1,15 @@ //DSFE_GUI DSFE_MainWindow.cpp #include "MainWindow/DSFE_MainWindow.h" +#include "Scene/SimulationManager.h" #include "Workspace/ProjectPage.h" namespace window { - DSFE_MainWindow::DSFE_MainWindow(QWidget* parent) : QMainWindow(parent) { + DSFE_MainWindow::DSFE_MainWindow(gui::SimManager* sim, QWidget* parent) : QMainWindow(parent) { setWindowTitle("DSFE"); - resize(800, 600); + resize(1280, 720); - auto* page = new Workspace::ProjectPage(this); + auto* page = new Workspace::ProjectPage(sim, this); setCentralWidget(page); - page->show(); } + } // namespace window \ No newline at end of file diff --git a/DSFE_App/DSFE_GUI/src/MainWindow/DockWidgets/ViewportDock.cpp b/DSFE_App/DSFE_GUI/src/MainWindow/DockWidgets/ViewportDock.cpp deleted file mode 100644 index 732f92c1..00000000 --- a/DSFE_App/DSFE_GUI/src/MainWindow/DockWidgets/ViewportDock.cpp +++ /dev/null @@ -1,11 +0,0 @@ -// DSFE_GUI ViewportDock.cpp -#include "DockWidgets/ViewportDock.h" -#include "Widgets/ViewportWidget.h" - -namespace dockwidgets { - ViewportDock::ViewportDock(QWidget* parent) : QDockWidget("Viewport", parent) { - setAllowedAreas(Qt::AllDockWidgetAreas); - setFeatures(QDockWidget::DockWidgetMovable | QDockWidget::DockWidgetFloatable); - setWidget(new widgets::ViewportWidget(this)); - } -} // namespace dockwidgets \ No newline at end of file diff --git a/DSFE_App/DSFE_GUI/src/MainWindow/Widgets/ViewportWidget.cpp b/DSFE_App/DSFE_GUI/src/MainWindow/Widgets/ViewportWidget.cpp index 5cae3be8..cb0c3110 100644 --- a/DSFE_App/DSFE_GUI/src/MainWindow/Widgets/ViewportWidget.cpp +++ b/DSFE_App/DSFE_GUI/src/MainWindow/Widgets/ViewportWidget.cpp @@ -1,10 +1,148 @@ // DSFE_GUI ViewportWidget.cpp #include "Widgets/ViewportWidget.h" +#include "Scene/SimulationManager.h" + +#ifdef __gl_h_ +#undef __gl_h_ +#endif +#include +#include #include #include +#include "Platform/KeyCode.h" + namespace widgets { - ViewportWidget::ViewportWidget(QWidget* parent) : QOpenGLWidget(parent) { + ViewportWidget::ViewportWidget(gui::SimManager* sim, QWidget* parent) : QOpenGLWidget(parent), _sim(sim) { + setFocusPolicy(Qt::StrongFocus); + setMouseTracking(true); + + connect(&_updateTimer, &QTimer::timeout, this, [this]() {update(); }); + _updateTimer.start(7); // ~144 FPS + } + + void ViewportWidget::initializeGL() { + initializeOpenGLFunctions(); + + auto* ctx = QOpenGLContext::currentContext(); + + if (!ctx) { + LOG_ERROR("No current OpenGL context"); + return; + } + + const int gladResult = gladLoadGLLoader([](const char* name) -> void* { + auto* ctx = QOpenGLContext::currentContext(); + if (!ctx) { return nullptr; } + return reinterpret_cast( ctx->getProcAddress(name)); + }); + + if (!gladResult) { + LOG_ERROR("Failed to initialise GLAD"); + return; + } + + _frameTimer.start(); + if (_sim) { _sim->initGL(); } + } + void ViewportWidget::resizeGL(int w, int h) { + if (_sim) { _sim->setDisplaySize(w, h); } + } + void ViewportWidget::paintGL() { + LOG_INFO_ONCE("Qt default FBO = %u", defaultFramebufferObject()); + GLint qtFBO = defaultFramebufferObject(); + + const qint64 now = _frameTimer.nsecsElapsed(); + const float dt = (_lastNs == 0) ? (1.0f / 144.0f) : static_cast(now - _lastNs) * 1e-9f; + _lastNs = now; + if (!_sim) { + glClearColor(0.1f, 0.1f, 0.1f, 1.0f); + glClear(GL_COLOR_BUFFER_BIT | GL_DEPTH_BUFFER_BIT); + return; + } + + _sim->setPresentationFBO(static_cast(qtFBO)); + _sim->tick(dt); + _sim->renderViewport(width(), height()); + + static float smoothedDt = (1.0f / 144.0f); + smoothedDt = glm::mix(smoothedDt, dt, 0.5f); + if (_mouseCaptured) { _sim->handleContinuousMovement(_pressedKeys, smoothedDt); } + } + + void ViewportWidget::keyPressEvent(QKeyEvent* event) { + switch (event->key()) { + case Qt::Key_W: _pressedKeys.insert(gui::eKeyCode::W); break; + case Qt::Key_A: _pressedKeys.insert(gui::eKeyCode::A); break; + case Qt::Key_S: _pressedKeys.insert(gui::eKeyCode::S); break; + case Qt::Key_D: _pressedKeys.insert(gui::eKeyCode::D); break; + case Qt::Key_Space: _pressedKeys.insert(gui::eKeyCode::Space); break; + case Qt::Key_Control: _pressedKeys.insert(gui::eKeyCode::Ctrl); break; + case Qt::Key_Shift: _pressedKeys.insert(gui::eKeyCode::LShift); break; + case Qt::Key_Escape: + _mouseCaptured = !_mouseCaptured; + if (_mouseCaptured) { + setFocus(); + setCursor(Qt::BlankCursor); + _screenCenter = mapToGlobal(rect().center()); + QCursor::setPos(_screenCenter); + grabMouse(); + if (_sim) { _sim->resetMouseDelta(); } + } + else { + releaseMouse(); + unsetCursor(); + if (_sim) { _sim->resetMouseDelta(); } + } + break; + } + QOpenGLWidget::keyPressEvent(event); + } + + void ViewportWidget::keyReleaseEvent(QKeyEvent* event) { + switch (event->key()) { + case Qt::Key_W: _pressedKeys.erase(gui::eKeyCode::W); break; + case Qt::Key_A: _pressedKeys.erase(gui::eKeyCode::A); break; + case Qt::Key_S: _pressedKeys.erase(gui::eKeyCode::S); break; + case Qt::Key_D: _pressedKeys.erase(gui::eKeyCode::D); break; + case Qt::Key_Space: _pressedKeys.erase(gui::eKeyCode::Space); break; + case Qt::Key_Control: _pressedKeys.erase(gui::eKeyCode::Ctrl); break; + case Qt::Key_Shift: _pressedKeys.erase(gui::eKeyCode::LShift); break; + } + QOpenGLWidget::keyReleaseEvent(event); + } + + void ViewportWidget::mousePressEvent(QMouseEvent* event) { + if (event->button() == Qt::RightButton) { + _mouseCaptured = true; + setFocus(); + setCursor(Qt::BlankCursor); + QCursor::setPos(mapToGlobal(rect().center())); + grabMouse(); + if (_sim) { _sim->resetMouseDelta(); } + } } + + void ViewportWidget::mouseReleaseEvent(QMouseEvent* event) { + if (event->button() == Qt::RightButton) { + _mouseCaptured = false; + releaseMouse(); + unsetCursor(); + } + } + + void ViewportWidget::mouseMoveEvent(QMouseEvent* event) { + if (!_sim || !_mouseCaptured) { return; } + QPoint current = QCursor::pos(); + QPoint delta = current - _screenCenter; + _sim->handleMouseLook(delta.x(), -delta.y(), true); + QCursor::setPos(_screenCenter); + } + + void ViewportWidget::wheelEvent(QWheelEvent* event) { + if (!_sim) { return; } + _sim->onMouseWheel(event->angleDelta().y() / 120.0); + } + } // namespace widgets \ No newline at end of file diff --git a/DSFE_App/DSFE_GUI/src/MainWindow/Workspace/ProjectPage.cpp b/DSFE_App/DSFE_GUI/src/MainWindow/Workspace/ProjectPage.cpp index dc4feb7e..25efa15d 100644 --- a/DSFE_App/DSFE_GUI/src/MainWindow/Workspace/ProjectPage.cpp +++ b/DSFE_App/DSFE_GUI/src/MainWindow/Workspace/ProjectPage.cpp @@ -1,5 +1,6 @@ // DSFE_GUI ProjectPage.cpp #include "Workspace/ProjectPage.h" +#include "Scene/SimulationManager.h" #include "Widgets/ViewportWidget.h" #include @@ -7,13 +8,14 @@ #include namespace Workspace { - ProjectPage::ProjectPage(QWidget* parent) : QWidget(parent) { + ProjectPage::ProjectPage(gui::SimManager* sim, QWidget* parent) : QWidget(parent) { auto* layout = new QVBoxLayout(this); + layout->setContentsMargins(0, 0, 0, 0); auto* splitter = new QSplitter(Qt::Horizontal, this); - splitter->addWidget(new widgets::ViewportWidget(splitter)); + splitter->addWidget(new widgets::ViewportWidget(sim, splitter)); splitter->addWidget(new QLabel("Properties(PlaceHolder)", splitter)); + layout->addWidget(splitter); splitter->setStretchFactor(0, 4); // Viewport takes 4/5 of space splitter->setStretchFactor(1, 1); // Properties takes 1/5 of space - layout->addWidget(splitter); } } // namespace Workspace \ No newline at end of file diff --git a/DSFE_App/DSFE_GUI/src/Platform/WindowManager.cpp b/DSFE_App/DSFE_GUI/src/Platform/WindowManager.cpp index 967a6c07..ca5ecebe 100644 --- a/DSFE_App/DSFE_GUI/src/Platform/WindowManager.cpp +++ b/DSFE_App/DSFE_GUI/src/Platform/WindowManager.cpp @@ -142,7 +142,7 @@ namespace window { smoothedDt = glm::mix(smoothedDt, dt, 0.2f); if (_sim) { - _sim->handleContinuousMovement(_window, smoothedDt); + //_sim->handleContinuousMovement(_window, smoothedDt); //_sim->getCamera()->applyGravity(smoothedDt, _sim->getPlaneHeight()); } } @@ -192,7 +192,6 @@ namespace window { // Forward window resize events to the SimManager to adjust the internal rendering resolution and aspect ratio void window::GLWindow::onCursorPos(double xpos, double ypos) { // LOG_INFO("Mouse moved to: X=%.2f, Y=%.2f", xpos, ypos); - if (_sim) { _sim->handleMouseLook(_window, xpos, ypos); } } // Window states diff --git a/DSFE_App/DSFE_GUI/src/Rendering/OpenGLBufferManager.cpp b/DSFE_App/DSFE_GUI/src/Rendering/OpenGLBufferManager.cpp index d7cb7032..02d46bc6 100644 --- a/DSFE_App/DSFE_GUI/src/Rendering/OpenGLBufferManager.cpp +++ b/DSFE_App/DSFE_GUI/src/Rendering/OpenGLBufferManager.cpp @@ -242,11 +242,10 @@ namespace render { // CRITICAL: reset ALL framebuffer targets, not just GL_FRAMEBUFFER glBindFramebuffer(GL_READ_FRAMEBUFFER, 0); glBindFramebuffer(GL_DRAW_FRAMEBUFFER, 0); - glBindFramebuffer(GL_FRAMEBUFFER, 0); - // Restore default backbuffer state - glDrawBuffer(GL_BACK); - glReadBuffer(GL_BACK); + //// Restore default backbuffer state + //glDrawBuffer(GL_BACK); + //glReadBuffer(GL_BACK); glColorMask(GL_TRUE, GL_TRUE, GL_TRUE, GL_TRUE); glDisable(GL_SCISSOR_TEST); @@ -256,8 +255,8 @@ namespace render { void OpenGLFrameBuffer::endSetup() { glBindFramebuffer(GL_FRAMEBUFFER, 0); - glDrawBuffer(GL_BACK); - glReadBuffer(GL_BACK); + //glDrawBuffer(GL_BACK); + //glReadBuffer(GL_BACK); glColorMask(GL_TRUE, GL_TRUE, GL_TRUE, GL_TRUE); glDisable(GL_SCISSOR_TEST); } diff --git a/DSFE_App/DSFE_GUI/src/Scene/Camera.cpp b/DSFE_App/DSFE_GUI/src/Scene/Camera.cpp index 93134865..bfa327ab 100644 --- a/DSFE_App/DSFE_GUI/src/Scene/Camera.cpp +++ b/DSFE_App/DSFE_GUI/src/Scene/Camera.cpp @@ -8,6 +8,8 @@ #include #include "Scene/Camera.h" +#include "Platform/KeyCode.h" + #include "EngineLib/LogMacros.h" namespace scene { @@ -26,12 +28,12 @@ namespace scene { float velocity = _currentSpeed * dt; switch (key) { - case GLFW_KEY_W: moveForward(velocity); break; - case GLFW_KEY_S: moveBackward(velocity); break; - case GLFW_KEY_A: moveLeft(velocity); break; - case GLFW_KEY_D: moveRight(velocity); break; - case GLFW_KEY_SPACE: moveUp(velocity); break; - case GLFW_KEY_LEFT_SHIFT: moveDown(velocity); break; + case static_cast(gui::eKeyCode::W): moveForward(velocity); break; + case static_cast(gui::eKeyCode::A): moveLeft(velocity); break; + case static_cast(gui::eKeyCode::S): moveBackward(velocity); break; + case static_cast(gui::eKeyCode::D): moveRight(velocity); break; + case static_cast(gui::eKeyCode::Space): moveUp(velocity); break; + case static_cast(gui::eKeyCode::LShift): moveDown(velocity); break; } updateViewMatrix(); diff --git a/DSFE_App/DSFE_GUI/src/Scene/Manager/SimBackend.cpp b/DSFE_App/DSFE_GUI/src/Scene/Manager/SimBackend.cpp index c746a575..ab59ff42 100644 --- a/DSFE_App/DSFE_GUI/src/Scene/Manager/SimBackend.cpp +++ b/DSFE_App/DSFE_GUI/src/Scene/Manager/SimBackend.cpp @@ -1,6 +1,6 @@ // DSFE_GUI SimBackend.cpp -#include "Scene/SimulationManager.h" #include "Scene/SimulationCore.h" +#include "Scene/SimulationManager.h" #ifdef __gl_h_ #undef __gl_h_ #endif diff --git a/DSFE_App/DSFE_GUI/src/Scene/SimulationManager.cpp b/DSFE_App/DSFE_GUI/src/Scene/SimulationManager.cpp index 6d0279c7..6b2612a0 100644 --- a/DSFE_App/DSFE_GUI/src/Scene/SimulationManager.cpp +++ b/DSFE_App/DSFE_GUI/src/Scene/SimulationManager.cpp @@ -19,6 +19,8 @@ extern "C" void DestroySimulationCore(core::ISimulationCore*); #include +#include "Platform/KeyCode.h" + #include #include "Platform/Paths.h" @@ -90,6 +92,8 @@ namespace gui { if (_glReady) return; _glReady = true; + _impl->initGLResources(*this); + InitShadowResource(_settingsCurrent.shadowMapRes); InitIBL(); @@ -186,7 +190,42 @@ namespace gui { drawViewportWindow(); } - void SimManager::tick(double dt) { _core->tick(dt); } + void SimManager::tick(double dt) { + if (!hasRobot()) { return; } + _core->tick(dt); + } + + void SimManager::renderViewport(int w, int h) { + LOG_INFO_ONCE("renderViewport entered"); + if (!_glReady || !_impl) { return; } + if (w <= 0 || h <= 0) { return; } + if (_impl->_robotSystem && hasRobot()) { + _impl->_robotRenderer->applyTransforms( + _impl->_robotSystem->model(), + _impl->_robotSystem->worldTransforms() + ); + } + if (_core->robotPresentationDirty()) { + loadRobot(_core->robotSystem()->robotName()); + _core->clearRobotPresentationDirty(); + } + _fpsCounter.update(); + auto& view = _impl->_views[static_cast(_impl->activeView)]; + _impl->renderView(*this, view, w, h); + + glBindFramebuffer(GL_FRAMEBUFFER, _presentationFBO); + glViewport(0, 0, w, h); + glDisable(GL_DEPTH_TEST); + glDisable(GL_BLEND); + _impl->_presentShader->use(); + _impl->_presentShader->setInt1(0, "screenTexture"); + glActiveTexture(GL_TEXTURE0); + glBindTexture(GL_TEXTURE_2D, view.post->getTexture()); + glBindVertexArray(_impl->_fullscreenVAO); + glDrawArrays(GL_TRIANGLES, 0, 3); + glBindVertexArray(0); + glBindTexture(GL_TEXTURE_2D, 0); + } void SimManager::syncRobotToScene() { if (!hasRobot()) return; @@ -227,6 +266,14 @@ namespace gui { //LOG_INFO("Resized SimManager INTERNAL RT to %dx%d", width, height); } + void SimManager::setDisplaySize(int w, int h) { + if (w <= 0.0f || h <= 0.0f) return; + _displaySize = { w, h }; + for (auto& v : _impl->_views) { + v.displayW = 0; v.displayH = 0; // Force per-view reallocation next frame + } + } + // -------------------------------------------------- // INPUT HANDLING // -------------------------------------------------- @@ -237,45 +284,40 @@ namespace gui { else if (ctrlMode == ControlMode::Object && _impl->_mesh) { /*idea is to add multiple angles to switch between!*/ } } - void gui::SimManager::handleContinuousMovement(GLFWwindow* window, float dt) { - auto* win = static_cast(glfwGetWindowUserPointer(window)); - if (!win || !win->isMouseCaptured()) return; + void gui::SimManager::handleContinuousMovement(const std::unordered_set& pressedKeys, float dt) { + if (_impl->viewMode == Impl::ViewMode::Quad) { return; } // No keyboard movement in quad view float kspd = 0.2f * dt; // base speed m/s - if (scene::Input::IsKeyPressed(window, GLFW_KEY_W)) { processMovementKey(GLFW_KEY_W, kspd); } - if (scene::Input::IsKeyPressed(window, GLFW_KEY_S)) { processMovementKey(GLFW_KEY_S, kspd); } - if (scene::Input::IsKeyPressed(window, GLFW_KEY_A)) { processMovementKey(GLFW_KEY_A, kspd); } - if (scene::Input::IsKeyPressed(window, GLFW_KEY_D)) { processMovementKey(GLFW_KEY_D, kspd); } - if (scene::Input::IsKeyPressed(window, GLFW_KEY_SPACE)) { processMovementKey(GLFW_KEY_SPACE, kspd); } - if (scene::Input::IsKeyPressed(window, GLFW_KEY_LEFT_SHIFT)) { processMovementKey(GLFW_KEY_LEFT_SHIFT, kspd); } + LOG_INFO("dt = %f", dt); + + if (pressedKeys.contains(eKeyCode::W)) { processMovementKey((int)eKeyCode::W, kspd); } + if (pressedKeys.contains(eKeyCode::A)) { processMovementKey((int)eKeyCode::A, kspd); } + if (pressedKeys.contains(eKeyCode::S)) { processMovementKey((int)eKeyCode::S, kspd); } + if (pressedKeys.contains(eKeyCode::D)) { processMovementKey((int)eKeyCode::D, kspd); } + if (pressedKeys.contains(eKeyCode::Space)) { processMovementKey((int)eKeyCode::Space, kspd); } + if (pressedKeys.contains(eKeyCode::LShift)) { processMovementKey((int)eKeyCode::LShift, kspd); } } - void gui::SimManager::handleMouseLook(GLFWwindow* window, double xpos, double ypos) { + void gui::SimManager::handleMouseLook(double xpos, double ypos, bool mouseCaptured) { if (_impl->viewMode == Impl::ViewMode::Quad) { return; } // No mouse look in quad view scene::Camera* cam = _impl->_views[static_cast(_impl->activeView)].cam.get(); - auto* win = static_cast(glfwGetWindowUserPointer(window)); - if (!win || !win->isMouseCaptured()) { return; } - - bool captured = true; - if (win == static_cast(glfwGetWindowUserPointer(window))) { captured = win->isMouseCaptured(); } - - if (!captured && !_isHovered) { - _lastMousePos = { (float)xpos, (float)ypos }; + if (!mouseCaptured) { + _lastMousePos = { static_cast(xpos), static_cast(ypos) }; _firstMouse = true; return; } if (_firstMouse) { - _lastMousePos = { (float)xpos, (float)ypos }; + _lastMousePos = { static_cast(xpos), static_cast(ypos) }; _firstMouse = false; } - double xoffset = xpos - _lastMousePos.x; - double yoffset = _lastMousePos.y - ypos; - _lastMousePos = { (float)xpos, (float)ypos }; + double xoffset = xpos; + double yoffset = ypos; + _lastMousePos = { static_cast(xpos), static_cast(ypos) }; - if (ctrlMode == ControlMode::Camera) { cam->processMouseMovement((float)xoffset, (float)yoffset); } + if (ctrlMode == ControlMode::Camera) { cam->processMouseMovement(static_cast(xoffset), static_cast(yoffset)); } else if (ctrlMode == ControlMode::Object && _impl->_selectedObject) { _impl->_selectedObject->onMouseMove(xpos, ypos, scene::eInputButton::Right); } } From c20a3d7a4ac8e03c45f0dcac6fb1b3851452c685 Mon Sep 17 00:00:00 2001 From: saltyjoss Date: Fri, 5 Jun 2026 16:24:16 +0100 Subject: [PATCH 108/210] feat: Added control panel and robot selector UI integration --- DSFE_App/DSFE_GUI/CMakeLists.txt | 2 + .../MainWindow/Widgets/ControlPanelWidget.h | 15 +++++++ .../MainWindow/Widgets/RobotSelectorWidget.h | 16 +++++++ .../MainWindow/Widgets/ControlPanelWidget.cpp | 23 ++++++++++ .../Widgets/RobotSelectorWidget.cpp | 43 +++++++++++++++++++ 5 files changed, 99 insertions(+) create mode 100644 DSFE_App/DSFE_GUI/include/MainWindow/Widgets/ControlPanelWidget.h create mode 100644 DSFE_App/DSFE_GUI/include/MainWindow/Widgets/RobotSelectorWidget.h create mode 100644 DSFE_App/DSFE_GUI/src/MainWindow/Widgets/ControlPanelWidget.cpp create mode 100644 DSFE_App/DSFE_GUI/src/MainWindow/Widgets/RobotSelectorWidget.cpp diff --git a/DSFE_App/DSFE_GUI/CMakeLists.txt b/DSFE_App/DSFE_GUI/CMakeLists.txt index 2fbad3fc..80286bb5 100644 --- a/DSFE_App/DSFE_GUI/CMakeLists.txt +++ b/DSFE_App/DSFE_GUI/CMakeLists.txt @@ -41,6 +41,8 @@ set(MAIN_WINDOW_SRC src/MainWindow/DSFE_MainWindow.cpp src/MainWindow/Workspace/ProjectPage.cpp src/MainWindow/Widgets/ViewportWidget.cpp + src/MainWindow/Widgets/RobotSelectorWidget.cpp + src/MainWindow/Widgets/ControlPanelWidget.cpp ) set(RENDER_SRC diff --git a/DSFE_App/DSFE_GUI/include/MainWindow/Widgets/ControlPanelWidget.h b/DSFE_App/DSFE_GUI/include/MainWindow/Widgets/ControlPanelWidget.h new file mode 100644 index 00000000..57cb7192 --- /dev/null +++ b/DSFE_App/DSFE_GUI/include/MainWindow/Widgets/ControlPanelWidget.h @@ -0,0 +1,15 @@ +// DSFE_GUI ControlPanelWidget.h +#pragma once + +#include + +namespace gui { class SimManager; } + +namespace widgets { + class ControlPanelWidget : public QWidget { + public: + explicit ControlPanelWidget(gui::SimManager* sim, QWidget* parent = nullptr); + private: + gui::SimManager* _sim = nullptr; + }; +} // namespace widgets \ No newline at end of file diff --git a/DSFE_App/DSFE_GUI/include/MainWindow/Widgets/RobotSelectorWidget.h b/DSFE_App/DSFE_GUI/include/MainWindow/Widgets/RobotSelectorWidget.h new file mode 100644 index 00000000..8631f022 --- /dev/null +++ b/DSFE_App/DSFE_GUI/include/MainWindow/Widgets/RobotSelectorWidget.h @@ -0,0 +1,16 @@ +// DSFE_GUI RobotSelectorWidget.h +#pragma once + +#include + +namespace gui {class SimManager; } +class QPushButton; +namespace widgets { + class RobotSelectorWidget : public QWidget { + public: + explicit RobotSelectorWidget(gui::SimManager* sim, QWidget* parent = nullptr); + private: + gui::SimManager* _sim; + void addRobotButton(const QString& robotName, QString company); + }; +} // namespace widgets \ No newline at end of file diff --git a/DSFE_App/DSFE_GUI/src/MainWindow/Widgets/ControlPanelWidget.cpp b/DSFE_App/DSFE_GUI/src/MainWindow/Widgets/ControlPanelWidget.cpp new file mode 100644 index 00000000..64e7d7e2 --- /dev/null +++ b/DSFE_App/DSFE_GUI/src/MainWindow/Widgets/ControlPanelWidget.cpp @@ -0,0 +1,23 @@ +// DSFE_GUI ControlPanelWidget.cpp +#include "Widgets/ControlPanelWidget.h" +#include "Scene/SimulationManager.h" +#include "Widgets/RobotSelectorWidget.h" + +#include +#include + +namespace widgets { + ControlPanelWidget::ControlPanelWidget(gui::SimManager* sim, QWidget* parent) : QWidget(parent), _sim(sim) { + auto* rootLayout = new QVBoxLayout(this); + rootLayout->setContentsMargins(4, 4, 4, 4); + auto* scrollArea = new QScrollArea(this); + scrollArea->setWidgetResizable(true); + auto* content = new QWidget(scrollArea); + auto* contentLayout = new QVBoxLayout(content); + contentLayout->addWidget(new RobotSelectorWidget(sim, content)); + contentLayout->addStretch(); // push widgets to top + content->setLayout(contentLayout); + scrollArea->setWidget(content); + rootLayout->addWidget(scrollArea); + } +} // namespace widgets} \ No newline at end of file diff --git a/DSFE_App/DSFE_GUI/src/MainWindow/Widgets/RobotSelectorWidget.cpp b/DSFE_App/DSFE_GUI/src/MainWindow/Widgets/RobotSelectorWidget.cpp new file mode 100644 index 00000000..683a5861 --- /dev/null +++ b/DSFE_App/DSFE_GUI/src/MainWindow/Widgets/RobotSelectorWidget.cpp @@ -0,0 +1,43 @@ +// DSFE_GUI RobotSelectorWidget.cpp +#include "Widgets/RobotSelectorWidget.h" +#include "Scene/SimulationManager.h" + +#include +#include "Platform/SystemMap.h" + +#include +#include +#include + +namespace widgets { + RobotSelectorWidget::RobotSelectorWidget(gui::SimManager* sim, QWidget* parent) : QWidget(parent), _sim(sim) { + setWindowTitle("Choose Robotic Arm"); + setMinimumWidth(300); + auto* layout = new QVBoxLayout(this); + if (!_sim) { + layout->addWidget(new QLabel("No simulation manager available", this)); + return; + } + + const std::unordered_map& robotMap = platform::getRoboticArmMap(); + + if (robotMap.empty()) { + layout->addWidget(new QLabel("No robotic arms available", this)); + return; + } + + for (const auto& [arm, family] : robotMap) { + const QString robotName = QString::fromStdString(platform::RoboticArms().toString(arm)); + const QString familyName = QString::fromStdString(platform::RoboticArms().toString(family)); + addRobotButton(robotName, familyName); + } + } + void RobotSelectorWidget::addRobotButton(const QString& robotName, QString company) { + auto* button = new QPushButton(robotName + "\n" + company); + layout()->addWidget(button); + connect(button, &QPushButton::clicked, this, [this, robotName]() { + LOG_INFO("Selected robot: %s", robotName.toStdString().c_str()); + if (_sim) { _sim->loadRobot(robotName.toStdString()); } + }); + } +} // namespace widgets \ No newline at end of file From 69eb91f1baf353797974b9a63aae8b1918d212c6 Mon Sep 17 00:00:00 2001 From: saltyjoss Date: Fri, 5 Jun 2026 16:24:46 +0100 Subject: [PATCH 109/210] feat: Added SystemMap.h for robotic arm enums and mappings --- .../DSFE_GUI/include/Platform/SystemMap.h | 60 +++++++++++++++++++ 1 file changed, 60 insertions(+) create mode 100644 DSFE_App/DSFE_GUI/include/Platform/SystemMap.h diff --git a/DSFE_App/DSFE_GUI/include/Platform/SystemMap.h b/DSFE_App/DSFE_GUI/include/Platform/SystemMap.h new file mode 100644 index 00000000..ff88305b --- /dev/null +++ b/DSFE_App/DSFE_GUI/include/Platform/SystemMap.h @@ -0,0 +1,60 @@ +// DSFE_GUI SystemMap.h +#pragma once + +#include +#include + +namespace platform { + enum class eRoboticArms { + Z1 = 0, + UR5e = 1, + Panda = 2, + iiwa14 = 3, + VISPA = 4, + H1 = 5 + }; + + enum class eRoboticArmFamilies { + Unitree = 0, + Universal = 1, + Franka = 2, + KUKA = 3, + Airbus = 4 + }; + + struct RoboticArms { + std::unordered_map armMap = { + { eRoboticArms::Z1, eRoboticArmFamilies::Unitree }, + { eRoboticArms::UR5e, eRoboticArmFamilies::Universal }, + { eRoboticArms::Panda, eRoboticArmFamilies::Franka }, + { eRoboticArms::iiwa14, eRoboticArmFamilies::KUKA }, + { eRoboticArms::VISPA, eRoboticArmFamilies::Airbus }, + { eRoboticArms::H1, eRoboticArmFamilies::Unitree } + }; + + inline std::string toString(eRoboticArms arm) { + switch (arm) { + case eRoboticArms::Z1: return "Z1"; + case eRoboticArms::UR5e: return "UR5e"; + case eRoboticArms::Panda: return "Panda"; + case eRoboticArms::iiwa14: return "iiwa14"; + case eRoboticArms::VISPA: return "VISPA"; + case eRoboticArms::H1: return "H1"; + default: return "Unknown"; + } + } + + inline std::string toString(eRoboticArmFamilies family) { + switch (family) { + case eRoboticArmFamilies::Unitree: return "Unitree Robotics"; + case eRoboticArmFamilies::Universal: return "Universal Robots"; + case eRoboticArmFamilies::Franka: return "Franka Robotics"; + case eRoboticArmFamilies::KUKA: return "KUKA"; + case eRoboticArmFamilies::Airbus: return "Airbus"; + default: return "Unknown"; + } + } + }; + + std::unordered_map getRoboticArmMap() { return RoboticArms().armMap; } +} \ No newline at end of file From 00f78c03dada3064157edf65b503d8149edff810 Mon Sep 17 00:00:00 2001 From: saltyjoss Date: Fri, 5 Jun 2026 16:28:33 +0100 Subject: [PATCH 110/210] fixes: Removed QOpenGLFunctions from ViewportWidget, add ControlPanel --- DSFE_App/DSFE_GUI/include/MainWindow/Widgets/ViewportWidget.h | 3 +-- DSFE_App/DSFE_GUI/src/MainWindow/Workspace/ProjectPage.cpp | 3 ++- 2 files changed, 3 insertions(+), 3 deletions(-) diff --git a/DSFE_App/DSFE_GUI/include/MainWindow/Widgets/ViewportWidget.h b/DSFE_App/DSFE_GUI/include/MainWindow/Widgets/ViewportWidget.h index 006de699..a9e39bcb 100644 --- a/DSFE_App/DSFE_GUI/include/MainWindow/Widgets/ViewportWidget.h +++ b/DSFE_App/DSFE_GUI/include/MainWindow/Widgets/ViewportWidget.h @@ -2,7 +2,6 @@ #pragma once #include -#include #include #include #include @@ -14,7 +13,7 @@ namespace gui { class SimManager; enum class eKeyCode; } namespace widgets { - class ViewportWidget : public QOpenGLWidget, protected QOpenGLFunctions_4_5_Core { + class ViewportWidget : public QOpenGLWidget { public: explicit ViewportWidget(gui::SimManager* sim, QWidget* parent = nullptr); diff --git a/DSFE_App/DSFE_GUI/src/MainWindow/Workspace/ProjectPage.cpp b/DSFE_App/DSFE_GUI/src/MainWindow/Workspace/ProjectPage.cpp index 25efa15d..4790322d 100644 --- a/DSFE_App/DSFE_GUI/src/MainWindow/Workspace/ProjectPage.cpp +++ b/DSFE_App/DSFE_GUI/src/MainWindow/Workspace/ProjectPage.cpp @@ -2,6 +2,7 @@ #include "Workspace/ProjectPage.h" #include "Scene/SimulationManager.h" #include "Widgets/ViewportWidget.h" +#include "Widgets/ControlPanelWidget.h" #include #include @@ -13,7 +14,7 @@ namespace Workspace { layout->setContentsMargins(0, 0, 0, 0); auto* splitter = new QSplitter(Qt::Horizontal, this); splitter->addWidget(new widgets::ViewportWidget(sim, splitter)); - splitter->addWidget(new QLabel("Properties(PlaceHolder)", splitter)); + splitter->addWidget(new widgets::ControlPanelWidget(sim, splitter)); layout->addWidget(splitter); splitter->setStretchFactor(0, 4); // Viewport takes 4/5 of space splitter->setStretchFactor(1, 1); // Properties takes 1/5 of space From 421db1018d3ef3a5b098938319d80c9711488c99 Mon Sep 17 00:00:00 2001 From: saltyjoss Date: Fri, 5 Jun 2026 16:30:15 +0100 Subject: [PATCH 111/210] fixes: Added setFltArray2, cleanup includes, and FBO update * Introduced setFltArray2 for float array uniforms in shaders and update usage for cascade splits. * Removed unused GLFW includes and redundant headers. * Adjusted QApplication initialization order. * Changed shadow pass FBO binding to _presentationFBO. --- DSFE_App/DSFE_GUI/include/Rendering/ShaderUtil.h | 1 + DSFE_App/DSFE_GUI/src/Application.cpp | 3 +-- .../DSFE_GUI/src/Rendering/OpenGLBufferManager.cpp | 3 --- DSFE_App/DSFE_GUI/src/Rendering/ShaderUtil.cpp | 5 +++++ DSFE_App/DSFE_GUI/src/Scene/Camera.cpp | 1 - DSFE_App/DSFE_GUI/src/Scene/Manager/SimRendering.cpp | 10 ++++------ DSFE_App/DSFE_GUI/src/Scene/Mesh.cpp | 4 ++-- DSFE_App/DSFE_GUI/src/Scene/SimulationManager.cpp | 4 ---- 8 files changed, 13 insertions(+), 18 deletions(-) diff --git a/DSFE_App/DSFE_GUI/include/Rendering/ShaderUtil.h b/DSFE_App/DSFE_GUI/include/Rendering/ShaderUtil.h index 4daf22ec..3fae8d15 100644 --- a/DSFE_App/DSFE_GUI/include/Rendering/ShaderUtil.h +++ b/DSFE_App/DSFE_GUI/include/Rendering/ShaderUtil.h @@ -26,6 +26,7 @@ namespace shaders { void setFlt1(float a, const std::string& name); void setFlt2(float a, float b, const std::string& name); void setFlt3(float a, float b, float c, const std::string& name); + void setFltArray2(float a, float b, const std::string& name); void setVec2(const glm::vec2& vec2, const std::string& name); void setVec3(const glm::vec3& vec3, const std::string& name); void setVec4(const glm::vec4& vec4, const std::string& name); diff --git a/DSFE_App/DSFE_GUI/src/Application.cpp b/DSFE_App/DSFE_GUI/src/Application.cpp index 5df7a13a..566b5e38 100644 --- a/DSFE_App/DSFE_GUI/src/Application.cpp +++ b/DSFE_App/DSFE_GUI/src/Application.cpp @@ -38,8 +38,6 @@ Application::Application(const std::string& appName) : _name(appName) { _qtArgv.reserve(_qtArgStorage.size()); for (std::string& arg : _qtArgStorage) { _qtArgv.push_back(arg.data()); } - _qtApp = std::make_unique(_qtArgc, _qtArgv.data()); - QSurfaceFormat format; format.setProfile(QSurfaceFormat::CoreProfile); format.setVersion(4, 5); @@ -47,6 +45,7 @@ Application::Application(const std::string& appName) : _name(appName) { format.setStencilBufferSize(8); QSurfaceFormat::setDefaultFormat(format); + _qtApp = std::make_unique(_qtArgc, _qtArgv.data()); _sim = std::make_unique(); int winW = 1920, winH = 1080; diff --git a/DSFE_App/DSFE_GUI/src/Rendering/OpenGLBufferManager.cpp b/DSFE_App/DSFE_GUI/src/Rendering/OpenGLBufferManager.cpp index 02d46bc6..cd60aa44 100644 --- a/DSFE_App/DSFE_GUI/src/Rendering/OpenGLBufferManager.cpp +++ b/DSFE_App/DSFE_GUI/src/Rendering/OpenGLBufferManager.cpp @@ -4,7 +4,6 @@ #undef __gl_h_ #endif #include -#include #include "Rendering/OpenGLBufferManager.h" #include "EngineLib/LogMacros.h" @@ -38,8 +37,6 @@ namespace render { glVertexAttribPointer(2, 2, GL_FLOAT, GL_FALSE, sizeof(assets::VertexHolder), (void*)offsetof(assets::VertexHolder, _texCoord)); glBindVertexArray(0); - - //LOG_INFO("OpenGLVertexIndexBuffer buffers created successfully"); } // Deletes the VAO, VBO, and EBO associated with this buffer diff --git a/DSFE_App/DSFE_GUI/src/Rendering/ShaderUtil.cpp b/DSFE_App/DSFE_GUI/src/Rendering/ShaderUtil.cpp index 7c44d4b3..c15ce99d 100644 --- a/DSFE_App/DSFE_GUI/src/Rendering/ShaderUtil.cpp +++ b/DSFE_App/DSFE_GUI/src/Rendering/ShaderUtil.cpp @@ -127,6 +127,11 @@ namespace shaders { GLint matLoc = glGetUniformLocation(getProgramID(), name.c_str()); glUniform3f(matLoc, a, b, c); } + void Shader::setFltArray2(float a, float b, const std::string& name) { + GLint loc = glGetUniformLocation(getProgramID(), name.c_str()); + float vals[2] = { a, b }; + glUniform1fv(loc, 2, vals); + } // Set a vec4 uniform in the shader program void Shader::setVec2(const glm::vec2& vec2, const std::string& name) { GLint matLoc = glGetUniformLocation(getProgramID(), name.c_str()); diff --git a/DSFE_App/DSFE_GUI/src/Scene/Camera.cpp b/DSFE_App/DSFE_GUI/src/Scene/Camera.cpp index bfa327ab..5ec66667 100644 --- a/DSFE_App/DSFE_GUI/src/Scene/Camera.cpp +++ b/DSFE_App/DSFE_GUI/src/Scene/Camera.cpp @@ -5,7 +5,6 @@ #undef __gl_h_ #endif #include -#include #include "Scene/Camera.h" #include "Platform/KeyCode.h" diff --git a/DSFE_App/DSFE_GUI/src/Scene/Manager/SimRendering.cpp b/DSFE_App/DSFE_GUI/src/Scene/Manager/SimRendering.cpp index 2e3dda89..210f7c98 100644 --- a/DSFE_App/DSFE_GUI/src/Scene/Manager/SimRendering.cpp +++ b/DSFE_App/DSFE_GUI/src/Scene/Manager/SimRendering.cpp @@ -178,19 +178,17 @@ namespace gui { const float splitDist0 = nearPlane + splitFrac0 * farPlane; const float splitDist1 = nearPlane + splitFrac1 * farPlane; - shader->setFlt2(splitDist0, splitDist1, "cascadeSplits"); + shader->setFltArray2(splitDist0, splitDist1, "cascadeSplits"); } - // Camera / SunLight / light common to all mesh shaders + // Update camera and light uniforms (only those relevant to the current shader mode) cam->update(shader); _impl->_light->update(shader); // Main mesh rendering loop for (auto& obj : _impl->_objects) { - if (!obj || !obj->getMesh()) continue; - + if (!obj || !obj->getMesh()) { continue; } if (_impl->_cameraFollowTarget == obj.get()) { cam->setFollowTarget(obj->transform.position, obj->transform.rotQ); } - glm::mat4 model = obj->transform.toMatrix() * obj->getMesh()->localTransform; shader->setMat4(model, "model"); @@ -287,7 +285,7 @@ namespace gui { obj->getMesh()->render(); } } - glBindFramebuffer(GL_FRAMEBUFFER, 0); + glBindFramebuffer(GL_FRAMEBUFFER, _presentationFBO); glViewport(0, 0, (int)_internalSize.x, (int)_internalSize.y); glDisable(GL_POLYGON_OFFSET_FILL); diff --git a/DSFE_App/DSFE_GUI/src/Scene/Mesh.cpp b/DSFE_App/DSFE_GUI/src/Scene/Mesh.cpp index fc98ea39..7610fc20 100644 --- a/DSFE_App/DSFE_GUI/src/Scene/Mesh.cpp +++ b/DSFE_App/DSFE_GUI/src/Scene/Mesh.cpp @@ -11,7 +11,7 @@ #include "EngineLib/LogMacros.h" namespace scene { - // Mesh Initialization + // Mesh Initialisation void Mesh::init() { _rndrBffrMngr = std::make_unique(); createBuffers(); @@ -39,7 +39,7 @@ namespace scene { void Mesh::unbind() { _rndrBffrMngr->unbind(); } // Render the mesh using the current GPU buffers - void Mesh::render() { _rndrBffrMngr->draw((int) _indices.size()); } + void Mesh::render() { _rndrBffrMngr->draw((int)_indices.size()); } // Clean up CPU and GPU buffers void Mesh::clean() { diff --git a/DSFE_App/DSFE_GUI/src/Scene/SimulationManager.cpp b/DSFE_App/DSFE_GUI/src/Scene/SimulationManager.cpp index 6b2612a0..42b97400 100644 --- a/DSFE_App/DSFE_GUI/src/Scene/SimulationManager.cpp +++ b/DSFE_App/DSFE_GUI/src/Scene/SimulationManager.cpp @@ -13,12 +13,8 @@ extern "C" void DestroySimulationCore(core::ISimulationCore*); #undef __gl_h_ #endif #include -#include #include "Manager/SimImplementation.h" - -#include - #include "Platform/KeyCode.h" #include From ac242cac5b42faf31ac20ef63e70e42216bfa218 Mon Sep 17 00:00:00 2001 From: saltyjoss Date: Fri, 5 Jun 2026 16:43:01 +0100 Subject: [PATCH 112/210] feat: Added RAII OpenGL context management and context hooks * Introduced ScopeGLContext for RAII-based OpenGL context management using user-provided hooks (this was like 6 hours of debugging btw, knowing it was context but not knowing how to fix it) * Added context hook support to SimManager and set hooks from ViewportWidget. * Added ScopeGLContext for current use in robot loading to ensure correct context. * Added GL_CHECKPOINT macro for error checking and perform minor code clean-ups when debugging (REMOVE FOR RELEASE). --- .../DSFE_GUI/include/Platform/ScopeGLContext.h | 14 ++++++++++++++ .../include/Scene/Manager/SimImplementation.h | 13 +++++-------- .../DSFE_GUI/include/Scene/SimulationManager.h | 17 ++++++++++++++++- .../src/MainWindow/Widgets/ViewportWidget.cpp | 5 ++--- .../DSFE_GUI/src/Scene/Manager/SimRobots.cpp | 2 ++ 5 files changed, 39 insertions(+), 12 deletions(-) create mode 100644 DSFE_App/DSFE_GUI/include/Platform/ScopeGLContext.h diff --git a/DSFE_App/DSFE_GUI/include/Platform/ScopeGLContext.h b/DSFE_App/DSFE_GUI/include/Platform/ScopeGLContext.h new file mode 100644 index 00000000..756c45d4 --- /dev/null +++ b/DSFE_App/DSFE_GUI/include/Platform/ScopeGLContext.h @@ -0,0 +1,14 @@ +// DSFE_GUI ScopeGLContext.h +#pragma once + +#include + +namespace platform { + class ScopeGLContext { + public: + ScopeGLContext(std::function make, std::function done) : _done(std::move(done)) { if (make) { make(); } } + ~ScopeGLContext() { if (_done) { _done(); } } + private: + std::function _done; + }; +} \ No newline at end of file diff --git a/DSFE_App/DSFE_GUI/include/Scene/Manager/SimImplementation.h b/DSFE_App/DSFE_GUI/include/Scene/Manager/SimImplementation.h index 12ef54e7..b7d7b8b4 100644 --- a/DSFE_App/DSFE_GUI/include/Scene/Manager/SimImplementation.h +++ b/DSFE_App/DSFE_GUI/include/Scene/Manager/SimImplementation.h @@ -134,6 +134,9 @@ namespace gui { Impl(SimManager& owner) { activeView = VID::Manual; viewMode = ViewMode::Single; + + // Robot system with mesh loading (for normal simulation) + _robotSystem = std::make_unique(); } void initGLResources(SimManager& owner) { @@ -260,8 +263,6 @@ namespace gui { _mesh = std::make_shared(); _mesh->init(); - // Robot system with mesh loading (for normal simulation) - _robotSystem = std::make_unique(); _robotRenderer = std::make_unique(); // SSAO shaders @@ -509,12 +510,8 @@ namespace gui { glClearColor(owner._backgroundColour.r, owner._backgroundColour.g, owner._backgroundColour.b, owner._backgroundAlpha); glClear(GL_COLOR_BUFFER_BIT | GL_DEPTH_BUFFER_BIT); - if (owner._settingsCurrent.msaaSamples > 1) { - glEnable(GL_MULTISAMPLE); - } - else { - glDisable(GL_MULTISAMPLE); - } + if (owner._settingsCurrent.msaaSamples > 1) { glEnable(GL_MULTISAMPLE); } + else { glDisable(GL_MULTISAMPLE); } // Update Follow Target: use the explicitly bound target (e.g. end-effector from loadRobot), // only fall back to _selectedObject if no explicit target was set via setViewFollowTarget diff --git a/DSFE_App/DSFE_GUI/include/Scene/SimulationManager.h b/DSFE_App/DSFE_GUI/include/Scene/SimulationManager.h index a86bc4d5..eae0291d 100644 --- a/DSFE_App/DSFE_GUI/include/Scene/SimulationManager.h +++ b/DSFE_App/DSFE_GUI/include/Scene/SimulationManager.h @@ -74,6 +74,11 @@ namespace gui { // OpenGL Initialisation void initGL(); + void setContextHooks(std::function makeCurrentHook, std::function doneCurrentHook) { + _makeCurrentHook = makeCurrentHook; + _doneCurrentHook = doneCurrentHook; + } + void renderViewport(int w, int h); void setDisplaySize(int w, int h); @@ -278,6 +283,9 @@ namespace gui { void resetMouseDelta(); private: + std::function _makeCurrentHook; + std::function _doneCurrentHook; + std::unique_ptr _core = nullptr; std::unique_ptr _studyRunner = nullptr; // Background worker for running batch studies bool _hasCompletedStudy = false; @@ -370,4 +378,11 @@ namespace gui { // Camera & Mouse glm::vec2 _lastMousePos{ 0.f, 0.f }; }; -} // namespace gui \ No newline at end of file +} // namespace gui + +#define GL_CHECKPOINT(name) \ +do { \ + GLenum err = glGetError(); \ + if (err != GL_NO_ERROR) \ + LOG_ERROR("%s -> 0x%X", name, err); \ +} while (0) \ No newline at end of file diff --git a/DSFE_App/DSFE_GUI/src/MainWindow/Widgets/ViewportWidget.cpp b/DSFE_App/DSFE_GUI/src/MainWindow/Widgets/ViewportWidget.cpp index cb0c3110..2b89a681 100644 --- a/DSFE_App/DSFE_GUI/src/MainWindow/Widgets/ViewportWidget.cpp +++ b/DSFE_App/DSFE_GUI/src/MainWindow/Widgets/ViewportWidget.cpp @@ -11,6 +11,7 @@ #include #include +#include #include "Platform/KeyCode.h" namespace widgets { @@ -23,10 +24,8 @@ namespace widgets { } void ViewportWidget::initializeGL() { - initializeOpenGLFunctions(); - auto* ctx = QOpenGLContext::currentContext(); - + if (_sim) { _sim->setContextHooks([this]() { this->makeCurrent(); }, [this]() { this->doneCurrent(); }); } if (!ctx) { LOG_ERROR("No current OpenGL context"); return; diff --git a/DSFE_App/DSFE_GUI/src/Scene/Manager/SimRobots.cpp b/DSFE_App/DSFE_GUI/src/Scene/Manager/SimRobots.cpp index ff40df7d..24013153 100644 --- a/DSFE_App/DSFE_GUI/src/Scene/Manager/SimRobots.cpp +++ b/DSFE_App/DSFE_GUI/src/Scene/Manager/SimRobots.cpp @@ -5,6 +5,7 @@ #undef __gl_h_ #endif #include "Manager/SimImplementation.h" +#include "Platform/ScopeGLContext.h" namespace gui { // Load a robot by name from the robot system @@ -24,6 +25,7 @@ namespace gui { size_t startIdx = _impl->_objects.size(); + platform::ScopeGLContext guard(_makeCurrentHook, _doneCurrentHook); _impl->buildRobotPresentationFromModel(_impl->_robotSystem->model(), *this); _impl->_robotRenderer->applyTransforms( From 37ba554337d7cda190ad71db21f4efe87db07a9f Mon Sep 17 00:00:00 2001 From: saltyjoss Date: Sat, 6 Jun 2026 08:05:46 +0100 Subject: [PATCH 113/210] fixes: Corrected stale FragColour assignment in `present.frag.glsl` --- DSFE_App/DSFE_Engine/assets/shaders/present.frag.glsl | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/DSFE_App/DSFE_Engine/assets/shaders/present.frag.glsl b/DSFE_App/DSFE_Engine/assets/shaders/present.frag.glsl index 054f2500..51d0d135 100644 --- a/DSFE_App/DSFE_Engine/assets/shaders/present.frag.glsl +++ b/DSFE_App/DSFE_Engine/assets/shaders/present.frag.glsl @@ -7,5 +7,5 @@ out vec4 FragColour; uniform sampler2D screenTexture; void main() { - FragColour = vec4(1,0,0,1); + FragColour = texture(screenTexture, uv); } \ No newline at end of file From 2ba6b4bb3bd82176d8a44faae0327fe80a6f9af7 Mon Sep 17 00:00:00 2001 From: saltyjoss Date: Sat, 6 Jun 2026 08:06:03 +0100 Subject: [PATCH 114/210] feat: Added visual outline for top task bar. --- .../include/MainWindow/DSFE_MainWindow.h | 4 + .../src/MainWindow/DSFE_MainWindow.cpp | 102 +++++++++++++++++- .../MainWindow/Widgets/ControlPanelWidget.cpp | 2 +- 3 files changed, 106 insertions(+), 2 deletions(-) diff --git a/DSFE_App/DSFE_GUI/include/MainWindow/DSFE_MainWindow.h b/DSFE_App/DSFE_GUI/include/MainWindow/DSFE_MainWindow.h index 94264db7..15c9d823 100644 --- a/DSFE_App/DSFE_GUI/include/MainWindow/DSFE_MainWindow.h +++ b/DSFE_App/DSFE_GUI/include/MainWindow/DSFE_MainWindow.h @@ -9,5 +9,9 @@ namespace window { class DSFE_MainWindow : public QMainWindow { public: explicit DSFE_MainWindow(gui::SimManager* sim, QWidget* parent = nullptr); + + private: + void buildMenuBar(); + gui::SimManager* _sim; }; } diff --git a/DSFE_App/DSFE_GUI/src/MainWindow/DSFE_MainWindow.cpp b/DSFE_App/DSFE_GUI/src/MainWindow/DSFE_MainWindow.cpp index 3a06c138..b4bb949e 100644 --- a/DSFE_App/DSFE_GUI/src/MainWindow/DSFE_MainWindow.cpp +++ b/DSFE_App/DSFE_GUI/src/MainWindow/DSFE_MainWindow.cpp @@ -3,13 +3,113 @@ #include "Scene/SimulationManager.h" #include "Workspace/ProjectPage.h" +#include +#include +#include +#include + namespace window { - DSFE_MainWindow::DSFE_MainWindow(gui::SimManager* sim, QWidget* parent) : QMainWindow(parent) { + DSFE_MainWindow::DSFE_MainWindow(gui::SimManager* sim, QWidget* parent) : QMainWindow(parent), _sim(sim) { setWindowTitle("DSFE"); resize(1280, 720); + buildMenuBar(); + auto* page = new Workspace::ProjectPage(sim, this); setCentralWidget(page); } + void DSFE_MainWindow::buildMenuBar() { + auto* fileMenu = menuBar()->addMenu("&File"); + auto* editMenu = menuBar()->addMenu("&Project"); + auto* viewMenu = menuBar()->addMenu("&View"); + auto* ToolsMenu = menuBar()->addMenu("&Tools"); + auto* helpMenu = menuBar()->addMenu("&Help"); + + // File menu + { + auto* openAction = new QAction("Open", this); + connect(openAction, &QAction::triggered, this, []() { + LOG_INFO("Menu clicked: File -> Open"); + }); + auto* saveAction = new QAction("Save", this); + connect(saveAction, &QAction::triggered, this, []() { + LOG_INFO("Menu clicked: File -> Save"); + }); + fileMenu->addSeparator(); + auto* exitAction = new QAction("Exit", this); + connect(exitAction, &QAction::triggered, this, []() { + LOG_INFO("Menu clicked: File -> Exit"); + QApplication::quit(); + }); + } + // Project menu + { + auto* newProjectAction = new QAction("New Project", this); + connect(newProjectAction, &QAction::triggered, this, []() { + LOG_INFO("Menu clicked: Project -> New Project"); + }); + auto* loadProjectAction = new QAction("Load Project", this); + connect(loadProjectAction, &QAction::triggered, this, []() { + LOG_INFO("Menu clicked: Project -> Load Project"); + }); + auto* saveProjectAction = new QAction("Save Project", this); + connect(saveProjectAction, &QAction::triggered, this, []() { + LOG_INFO("Menu clicked: Project -> Save Project"); + }); + fileMenu->addSeparator(); + auto* loadRobotAction = new QAction("Load Robot", this); + connect(loadRobotAction, &QAction::triggered, this, [this]() { + LOG_INFO("Menu clicked: Project -> Load Robot"); + _sim->loadRobot("panda"); + }); + auto* loadMeshAction = new QAction("Load Mesh", this); + connect(loadMeshAction, &QAction::triggered, this, []() { + LOG_INFO("Menu clicked: Project -> Load Mesh"); + }); + auto* loadHDRAction = new QAction("Load HDRI", this); + connect(loadHDRAction, &QAction::triggered, this, []() { + LOG_INFO("Menu clicked: Project -> Load HDRI"); + }); + } + // View menu + { + auto* resetCameraAction = new QAction("Reset Camera", this); + connect(resetCameraAction, &QAction::triggered, this, []() { + LOG_INFO("Menu clicked: View -> Reset Camera"); + }); + auto* toggleGridAction = new QAction("Toggle Grid", this); + toggleGridAction->setCheckable(true); + toggleGridAction->setChecked(true); + connect(toggleGridAction, &QAction::toggled, this, [](bool checked) { + LOG_INFO("Menu toggled: View -> Toggle Grid -> %s", checked ? "On" : "Off"); + }); + } + // Tools menu + { + auto* physicsDebugAction = new QAction("Toggle Physics Debug", this); + physicsDebugAction->setCheckable(true); + physicsDebugAction->setChecked(false); + connect(physicsDebugAction, &QAction::toggled, this, [](bool checked) { + LOG_INFO("Menu toggled: Tools -> Toggle Physics Debug -> %s", checked ? "On" : "Off"); + }); + auto* reloadShadersAction = new QAction("Reload Shaders", this); + connect(reloadShadersAction, &QAction::triggered, this, []() { + LOG_INFO("Menu clicked: Tools -> Reload Shaders"); + }); + auto* diagnosticsAction = new QAction("Run Diagnostics", this); + } + // Help menu + { + auto* aboutAction = new QAction("About", this); + connect(aboutAction, &QAction::triggered, this, []() { + LOG_INFO("Menu clicked: Help -> About"); + }); + auto* docsAction = new QAction("Documentation", this); + connect(docsAction, &QAction::triggered, this, []() { + LOG_INFO("Menu clicked: Help -> Documentation"); + }); + } + } + } // namespace window \ No newline at end of file diff --git a/DSFE_App/DSFE_GUI/src/MainWindow/Widgets/ControlPanelWidget.cpp b/DSFE_App/DSFE_GUI/src/MainWindow/Widgets/ControlPanelWidget.cpp index 64e7d7e2..8dad56ff 100644 --- a/DSFE_App/DSFE_GUI/src/MainWindow/Widgets/ControlPanelWidget.cpp +++ b/DSFE_App/DSFE_GUI/src/MainWindow/Widgets/ControlPanelWidget.cpp @@ -20,4 +20,4 @@ namespace widgets { scrollArea->setWidget(content); rootLayout->addWidget(scrollArea); } -} // namespace widgets} \ No newline at end of file +} // namespace widgets \ No newline at end of file From e14bfe32e8892e3ab82b4b40147fa91d634c303f Mon Sep 17 00:00:00 2001 From: saltyjoss Date: Sat, 6 Jun 2026 08:41:17 +0100 Subject: [PATCH 115/210] feat: Reintroduced old layout style from imgui dimesions --- .../src/MainWindow/Workspace/ProjectPage.cpp | 21 +++++++++++++------ 1 file changed, 15 insertions(+), 6 deletions(-) diff --git a/DSFE_App/DSFE_GUI/src/MainWindow/Workspace/ProjectPage.cpp b/DSFE_App/DSFE_GUI/src/MainWindow/Workspace/ProjectPage.cpp index 4790322d..c6d6bc5e 100644 --- a/DSFE_App/DSFE_GUI/src/MainWindow/Workspace/ProjectPage.cpp +++ b/DSFE_App/DSFE_GUI/src/MainWindow/Workspace/ProjectPage.cpp @@ -12,11 +12,20 @@ namespace Workspace { ProjectPage::ProjectPage(gui::SimManager* sim, QWidget* parent) : QWidget(parent) { auto* layout = new QVBoxLayout(this); layout->setContentsMargins(0, 0, 0, 0); - auto* splitter = new QSplitter(Qt::Horizontal, this); - splitter->addWidget(new widgets::ViewportWidget(sim, splitter)); - splitter->addWidget(new widgets::ControlPanelWidget(sim, splitter)); - layout->addWidget(splitter); - splitter->setStretchFactor(0, 4); // Viewport takes 4/5 of space - splitter->setStretchFactor(1, 1); // Properties takes 1/5 of space + auto* rootSplitter = new QSplitter(Qt::Horizontal, this); + auto* centreSplitter = new QSplitter(Qt::Vertical); + auto* rightSplitter = new QSplitter(Qt::Vertical); + rootSplitter->addWidget(new QLabel("Script Editor (TODO)", this)); + centreSplitter->addWidget(new widgets::ViewportWidget(sim, this)); + centreSplitter->addWidget(new QLabel("Console Output (TODO)", this)); + rightSplitter->addWidget(new widgets::ControlPanelWidget(sim, this)); + rightSplitter->addWidget(new QLabel("Scene Object? (TODO)", this)); + rootSplitter->addWidget(centreSplitter); + rootSplitter->addWidget(rightSplitter); + layout->addWidget(rootSplitter); + // Sizing reused from the old imgui layout + rootSplitter->setSizes({ 635, 1016, 393 }); + centreSplitter->setSizes({ 733, 396 }); + rightSplitter->setSizes({ 733, 396 }); } } // namespace Workspace \ No newline at end of file From 5c0acf6c54f19d1e5377ca03fea5b3a6e825b50f Mon Sep 17 00:00:00 2001 From: saltyjoss Date: Sat, 6 Jun 2026 09:16:49 +0100 Subject: [PATCH 116/210] feat: Updated menu items in `buildMenuBar` to have actual actions, including loading of robotic systems --- .../include/MainWindow/DSFE_MainWindow.h | 4 +- .../DSFE_GUI/include/Platform/SystemMap.h | 53 ++++++------ .../src/MainWindow/DSFE_MainWindow.cpp | 81 +++++++++++++------ .../Widgets/RobotSelectorWidget.cpp | 9 +-- .../src/Rendering/OpenGLBufferManager.cpp | 2 +- 5 files changed, 90 insertions(+), 59 deletions(-) diff --git a/DSFE_App/DSFE_GUI/include/MainWindow/DSFE_MainWindow.h b/DSFE_App/DSFE_GUI/include/MainWindow/DSFE_MainWindow.h index 15c9d823..da9520c5 100644 --- a/DSFE_App/DSFE_GUI/include/MainWindow/DSFE_MainWindow.h +++ b/DSFE_App/DSFE_GUI/include/MainWindow/DSFE_MainWindow.h @@ -2,9 +2,9 @@ #pragma once #include +#include namespace gui { class SimManager; } - namespace window { class DSFE_MainWindow : public QMainWindow { public: @@ -13,5 +13,7 @@ namespace window { private: void buildMenuBar(); gui::SimManager* _sim; + + void buildRobotMenu(QMenu* projectMenu); }; } diff --git a/DSFE_App/DSFE_GUI/include/Platform/SystemMap.h b/DSFE_App/DSFE_GUI/include/Platform/SystemMap.h index ff88305b..b21bd609 100644 --- a/DSFE_App/DSFE_GUI/include/Platform/SystemMap.h +++ b/DSFE_App/DSFE_GUI/include/Platform/SystemMap.h @@ -5,7 +5,7 @@ #include namespace platform { - enum class eRoboticArms { + enum class eRoboticSystems { Z1 = 0, UR5e = 1, Panda = 2, @@ -14,47 +14,48 @@ namespace platform { H1 = 5 }; - enum class eRoboticArmFamilies { + enum class eRoboticSystemFamilies { Unitree = 0, Universal = 1, Franka = 2, KUKA = 3, - Airbus = 4 + Airbus = 4, + Othjer = 5 }; - struct RoboticArms { - std::unordered_map armMap = { - { eRoboticArms::Z1, eRoboticArmFamilies::Unitree }, - { eRoboticArms::UR5e, eRoboticArmFamilies::Universal }, - { eRoboticArms::Panda, eRoboticArmFamilies::Franka }, - { eRoboticArms::iiwa14, eRoboticArmFamilies::KUKA }, - { eRoboticArms::VISPA, eRoboticArmFamilies::Airbus }, - { eRoboticArms::H1, eRoboticArmFamilies::Unitree } + struct RoboticSystems { + std::unordered_map robotMap = { + { eRoboticSystems::Z1, eRoboticSystemFamilies::Unitree }, + { eRoboticSystems::UR5e, eRoboticSystemFamilies::Universal }, + { eRoboticSystems::Panda, eRoboticSystemFamilies::Franka }, + { eRoboticSystems::iiwa14, eRoboticSystemFamilies::KUKA }, + { eRoboticSystems::VISPA, eRoboticSystemFamilies::Airbus }, + { eRoboticSystems::H1, eRoboticSystemFamilies::Unitree } }; - inline std::string toString(eRoboticArms arm) { - switch (arm) { - case eRoboticArms::Z1: return "Z1"; - case eRoboticArms::UR5e: return "UR5e"; - case eRoboticArms::Panda: return "Panda"; - case eRoboticArms::iiwa14: return "iiwa14"; - case eRoboticArms::VISPA: return "VISPA"; - case eRoboticArms::H1: return "H1"; + inline std::string toString(eRoboticSystems sys) { + switch (sys) { + case eRoboticSystems::Z1: return "Z1"; + case eRoboticSystems::UR5e: return "UR5e"; + case eRoboticSystems::Panda: return "Panda"; + case eRoboticSystems::iiwa14: return "iiwa14"; + case eRoboticSystems::VISPA: return "VISPA"; + case eRoboticSystems::H1: return "H1"; default: return "Unknown"; } } - inline std::string toString(eRoboticArmFamilies family) { + inline std::string toString(eRoboticSystemFamilies family) { switch (family) { - case eRoboticArmFamilies::Unitree: return "Unitree Robotics"; - case eRoboticArmFamilies::Universal: return "Universal Robots"; - case eRoboticArmFamilies::Franka: return "Franka Robotics"; - case eRoboticArmFamilies::KUKA: return "KUKA"; - case eRoboticArmFamilies::Airbus: return "Airbus"; + case eRoboticSystemFamilies::Unitree: return "Unitree Robotics"; + case eRoboticSystemFamilies::Universal: return "Universal Robots"; + case eRoboticSystemFamilies::Franka: return "Franka Robotics"; + case eRoboticSystemFamilies::KUKA: return "KUKA"; + case eRoboticSystemFamilies::Airbus: return "Airbus"; default: return "Unknown"; } } }; - std::unordered_map getRoboticArmMap() { return RoboticArms().armMap; } + inline std::unordered_map getRobotSystemMap() { return RoboticSystems().robotMap; } } \ No newline at end of file diff --git a/DSFE_App/DSFE_GUI/src/MainWindow/DSFE_MainWindow.cpp b/DSFE_App/DSFE_GUI/src/MainWindow/DSFE_MainWindow.cpp index b4bb949e..ce06bc42 100644 --- a/DSFE_App/DSFE_GUI/src/MainWindow/DSFE_MainWindow.cpp +++ b/DSFE_App/DSFE_GUI/src/MainWindow/DSFE_MainWindow.cpp @@ -3,10 +3,12 @@ #include "Scene/SimulationManager.h" #include "Workspace/ProjectPage.h" +#include "Platform/SystemMap.h" + #include #include -#include #include +#include namespace window { DSFE_MainWindow::DSFE_MainWindow(gui::SimManager* sim, QWidget* parent) : QMainWindow(parent), _sim(sim) { @@ -21,23 +23,23 @@ namespace window { void DSFE_MainWindow::buildMenuBar() { auto* fileMenu = menuBar()->addMenu("&File"); - auto* editMenu = menuBar()->addMenu("&Project"); + auto* projectMenu = menuBar()->addMenu("&Project"); auto* viewMenu = menuBar()->addMenu("&View"); - auto* ToolsMenu = menuBar()->addMenu("&Tools"); + auto* toolsMenu = menuBar()->addMenu("&Tools"); auto* helpMenu = menuBar()->addMenu("&Help"); // File menu { - auto* openAction = new QAction("Open", this); + auto* openAction = fileMenu->addAction("Open"); connect(openAction, &QAction::triggered, this, []() { LOG_INFO("Menu clicked: File -> Open"); }); - auto* saveAction = new QAction("Save", this); + auto* saveAction = fileMenu->addAction("Save"); connect(saveAction, &QAction::triggered, this, []() { LOG_INFO("Menu clicked: File -> Save"); }); fileMenu->addSeparator(); - auto* exitAction = new QAction("Exit", this); + auto* exitAction = fileMenu->addAction("Exit"); connect(exitAction, &QAction::triggered, this, []() { LOG_INFO("Menu clicked: File -> Exit"); QApplication::quit(); @@ -45,40 +47,46 @@ namespace window { } // Project menu { - auto* newProjectAction = new QAction("New Project", this); + auto* newProjectAction = projectMenu->addAction("New Project"); connect(newProjectAction, &QAction::triggered, this, []() { LOG_INFO("Menu clicked: Project -> New Project"); }); - auto* loadProjectAction = new QAction("Load Project", this); + auto* loadProjectAction = projectMenu->addAction("Load Project"); connect(loadProjectAction, &QAction::triggered, this, []() { LOG_INFO("Menu clicked: Project -> Load Project"); }); - auto* saveProjectAction = new QAction("Save Project", this); + auto* saveProjectAction = projectMenu->addAction("Save Project"); connect(saveProjectAction, &QAction::triggered, this, []() { LOG_INFO("Menu clicked: Project -> Save Project"); }); - fileMenu->addSeparator(); - auto* loadRobotAction = new QAction("Load Robot", this); - connect(loadRobotAction, &QAction::triggered, this, [this]() { - LOG_INFO("Menu clicked: Project -> Load Robot"); - _sim->loadRobot("panda"); + projectMenu->addSeparator(); + auto* robotMenu = projectMenu->addMenu("Load Robot"); + connect(robotMenu, &QMenu::aboutToShow, this, [this, robotMenu]() { + robotMenu->clear(); + buildRobotMenu(robotMenu); }); - auto* loadMeshAction = new QAction("Load Mesh", this); - connect(loadMeshAction, &QAction::triggered, this, []() { + auto* loadMeshAction = projectMenu->addAction("Load Mesh"); + connect(loadMeshAction, &QAction::triggered, this, [this]() { LOG_INFO("Menu clicked: Project -> Load Mesh"); + QString path = QFileDialog::getOpenFileName(nullptr, "Select Mesh File", "", "Mesh Files (*.obj *.fbx *.gltf *.dae *.stl"); + if (path.isEmpty()) { return; } + _sim->loadMesh(path.toStdString()); }); - auto* loadHDRAction = new QAction("Load HDRI", this); - connect(loadHDRAction, &QAction::triggered, this, []() { + auto* loadHDRAction = projectMenu->addAction("Load HDRI"); + connect(loadHDRAction, &QAction::triggered, this, [this]() { LOG_INFO("Menu clicked: Project -> Load HDRI"); + QString path = QFileDialog::getOpenFileName(nullptr, "Select HDRI File", "", "HDRI Files (*.hdr *.exr)"); + if (path.isEmpty()) { return; } + _sim->loadNewHDR(path.toStdString()); }); } // View menu { - auto* resetCameraAction = new QAction("Reset Camera", this); + auto* resetCameraAction = viewMenu->addAction("Reset Camera"); connect(resetCameraAction, &QAction::triggered, this, []() { LOG_INFO("Menu clicked: View -> Reset Camera"); }); - auto* toggleGridAction = new QAction("Toggle Grid", this); + auto* toggleGridAction = viewMenu->addAction("Toggle Grid"); toggleGridAction->setCheckable(true); toggleGridAction->setChecked(true); connect(toggleGridAction, &QAction::toggled, this, [](bool checked) { @@ -87,29 +95,50 @@ namespace window { } // Tools menu { - auto* physicsDebugAction = new QAction("Toggle Physics Debug", this); + auto* physicsDebugAction = toolsMenu->addAction("Toggle Physics Debug"); physicsDebugAction->setCheckable(true); physicsDebugAction->setChecked(false); connect(physicsDebugAction, &QAction::toggled, this, [](bool checked) { LOG_INFO("Menu toggled: Tools -> Toggle Physics Debug -> %s", checked ? "On" : "Off"); }); - auto* reloadShadersAction = new QAction("Reload Shaders", this); - connect(reloadShadersAction, &QAction::triggered, this, []() { + auto* reloadShadersAction = toolsMenu->addAction("Reload Shaders"); + connect(reloadShadersAction, &QAction::triggered, this, [this]() { LOG_INFO("Menu clicked: Tools -> Reload Shaders"); + _sim->reloadAllShaders(); + }); + auto* diagnosticsAction = toolsMenu->addAction("Run Diagnostics"); + connect(diagnosticsAction, &QAction::triggered, this, []() { + LOG_INFO("Menu clicked: Tools -> Run Diagnostics"); }); - auto* diagnosticsAction = new QAction("Run Diagnostics", this); } // Help menu { - auto* aboutAction = new QAction("About", this); + auto* aboutAction = helpMenu->addAction("About"); connect(aboutAction, &QAction::triggered, this, []() { LOG_INFO("Menu clicked: Help -> About"); }); - auto* docsAction = new QAction("Documentation", this); + auto* docsAction = helpMenu->addAction("Documentation"); connect(docsAction, &QAction::triggered, this, []() { LOG_INFO("Menu clicked: Help -> Documentation"); }); } } + void DSFE_MainWindow::buildRobotMenu(QMenu* projectMenu) { + const auto& robotMap = platform::getRobotSystemMap(); + std::unordered_map familyMenus; + for (const auto& [sys, family] : robotMap) { + if (!familyMenus.contains(family)) { + QString familyName = QString::fromStdString(platform::RoboticSystems().toString(family)); + familyMenus[family] = projectMenu->addMenu(familyName); + } + QString robotName = QString::fromStdString(platform::RoboticSystems().toString(sys)); + QAction* robotAction = familyMenus[family]->addAction(robotName); + connect(robotAction, &QAction::triggered, this, [this, robotName]() { + LOG_INFO("Menu clicked: Project -> Load Robot -> %s", robotName.toStdString().c_str()); + _sim->loadRobot(robotName.toStdString()); + }); + } + } + } // namespace window \ No newline at end of file diff --git a/DSFE_App/DSFE_GUI/src/MainWindow/Widgets/RobotSelectorWidget.cpp b/DSFE_App/DSFE_GUI/src/MainWindow/Widgets/RobotSelectorWidget.cpp index 683a5861..8129530f 100644 --- a/DSFE_App/DSFE_GUI/src/MainWindow/Widgets/RobotSelectorWidget.cpp +++ b/DSFE_App/DSFE_GUI/src/MainWindow/Widgets/RobotSelectorWidget.cpp @@ -2,7 +2,6 @@ #include "Widgets/RobotSelectorWidget.h" #include "Scene/SimulationManager.h" -#include #include "Platform/SystemMap.h" #include @@ -19,16 +18,16 @@ namespace widgets { return; } - const std::unordered_map& robotMap = platform::getRoboticArmMap(); + const std::unordered_map& robotMap = platform::getRobotSystemMap(); if (robotMap.empty()) { layout->addWidget(new QLabel("No robotic arms available", this)); return; } - for (const auto& [arm, family] : robotMap) { - const QString robotName = QString::fromStdString(platform::RoboticArms().toString(arm)); - const QString familyName = QString::fromStdString(platform::RoboticArms().toString(family)); + for (const auto& [sys, family] : robotMap) { + const QString robotName = QString::fromStdString(platform::RoboticSystems().toString(sys)); + const QString familyName = QString::fromStdString(platform::RoboticSystems().toString(family)); addRobotButton(robotName, familyName); } } diff --git a/DSFE_App/DSFE_GUI/src/Rendering/OpenGLBufferManager.cpp b/DSFE_App/DSFE_GUI/src/Rendering/OpenGLBufferManager.cpp index cd60aa44..b9ed8165 100644 --- a/DSFE_App/DSFE_GUI/src/Rendering/OpenGLBufferManager.cpp +++ b/DSFE_App/DSFE_GUI/src/Rendering/OpenGLBufferManager.cpp @@ -174,7 +174,7 @@ namespace render { // Deletes the framebuffer and its associated attachments void OpenGLFrameBuffer::deleteBuffers() { if (_FBO) { - LOG_INFO("Deleting framebuffer buffers"); + /*LOG_INFO("Deleting framebuffer buffers");*/ if (_msaaFBO) glDeleteFramebuffers(1, &_msaaFBO); if (_msaaColour) glDeleteTextures(1, &_msaaColour); if (_msaaDepthRBO) glDeleteRenderbuffers(1, &_msaaDepthRBO); From 88ab656f2bf5b61de33ed616a8627969cdaf8056 Mon Sep 17 00:00:00 2001 From: saltyjoss Date: Sat, 6 Jun 2026 11:05:30 +0100 Subject: [PATCH 117/210] feat: Implemented Qt6-based version of old imgui integrator selection logic in `simPropertiesPanel` and `buildIntegratorCombos` --- .../MainWindow/Widgets/ControlPanelWidget.h | 112 +++++++++++++++++ .../MainWindow/Widgets/ControlPanelWidget.cpp | 116 ++++++++++++++++-- 2 files changed, 220 insertions(+), 8 deletions(-) diff --git a/DSFE_App/DSFE_GUI/include/MainWindow/Widgets/ControlPanelWidget.h b/DSFE_App/DSFE_GUI/include/MainWindow/Widgets/ControlPanelWidget.h index 57cb7192..3196e462 100644 --- a/DSFE_App/DSFE_GUI/include/MainWindow/Widgets/ControlPanelWidget.h +++ b/DSFE_App/DSFE_GUI/include/MainWindow/Widgets/ControlPanelWidget.h @@ -2,14 +2,126 @@ #pragma once #include +#include +#include +#include +#include +#include +#include + +#include "Platform/SimulationState.h" +#include "Numerics/IntegrationMethods.h" +#include "Platform/Logger.h" + +namespace scene { + class Mesh; + class Object; + class Light; + class Camera; +} +namespace render { + enum class ResolutionPreset; + enum class QualityPreset; +} + +namespace robots { class RobotSystem; } +namespace diagnostics { class TelemetryRecorder; } namespace gui { class SimManager; } +class QVBoxLayout; +class QCheckBox; +class QComboBox; +class QGroupBox; + namespace widgets { class ControlPanelWidget : public QWidget { public: explicit ControlPanelWidget(gui::SimManager* sim, QWidget* parent = nullptr); private: + struct IntegratorEntry { + integration::eIntegrationMethod method; + const char* name; + }; + + struct ADIntegratorEntry { + integration::eAutoDiffIntegrationMethod method; + const char* name; + }; + + void simPropertiesPanel(); + void buildIntegratorCombos(); + + void jointInfoPanel(); + void displayPanel(); + void selectJointAndFollow(int jointIdx); + gui::SimManager* _sim = nullptr; + + QVBoxLayout* _contentLayout = nullptr; + QGroupBox* _simPropertiesGroup = nullptr; + QCheckBox* _useAutoDiffCheck = nullptr; + QComboBox* _integratorCombo = nullptr; + + static constexpr IntegratorEntry integrators[] = { + { integration::eIntegrationMethod::Euler, "Euler" }, + { integration::eIntegrationMethod::Midpoint, "Midpoint" }, + { integration::eIntegrationMethod::Heun, "Heun" }, + { integration::eIntegrationMethod::Ralston, "Ralston" }, + { integration::eIntegrationMethod::RK4, "RK4" }, + { integration::eIntegrationMethod::RK45, "RK45" }, + { integration::eIntegrationMethod::ImplicitEuler, "Implicit Euler" }, + { integration::eIntegrationMethod::ImplicitMidpoint, "Implicit Midpoint" }, + { integration::eIntegrationMethod::GLRK2, "GLRK2" }, + { integration::eIntegrationMethod::GLRK3, "GLRK3" } + }; + + static constexpr ADIntegratorEntry adIntegrators[] = { + { integration::eAutoDiffIntegrationMethod::AD_ImplicitEuler, "Implicit Euler (AutoDiff)" }, + { integration::eAutoDiffIntegrationMethod::AD_ImplicitMidpoint, "Implicit Midpoint (AutoDiff)" }, + { integration::eAutoDiffIntegrationMethod::AD_GLRK2, "GLRK2 (AutoDiff)" }, + { integration::eAutoDiffIntegrationMethod::AD_GLRK3, "GLRK3 (AutoDiff)" } + }; + + // Current selection state + Selection _selection; + + // Current items selected + std::string _requestedRobot; // name of requested robot to load + std::string _currentObjectName; // name of currently selected object + std::string _currentLinkName; // name of currently selected link + std::string _currentJointName; // name of currently selected joint + std::string _lastLinkName; // name of last selected link + + // Storage of joint angles + std::unordered_map _linkAngles; + + // Internal states + bool simulationRunning = false; + bool _jointSelected = false; + bool diagRunning = false; + bool _robotRequested = false; + bool _hasRobot = false; + bool _openStats = true; + bool _useAutoDiff = false; + // Simulation and diagnostics timing + float simLength = 30.0f; // ~30 seconds default + float diagLength = 15.0f; // ~15 seconds default + double deltaTime = 1.0f / 180.0f; // ~180 FPS default + float simTime = 0.0f; // current simulation time + float diagTime = 0.0f; // current diagnostic time + // Camera properties + int povMode = 0; + float fov = 60.0f; + // Internal Physics + float velocity = 0.0f; + float torque = 0.0f; + float linkLength = 1.0f; + float damping = 0.1f; + float position = 0.0f; + + // Time tracking for simulation updates + std::chrono::high_resolution_clock::time_point simLastUpdateTime = std::chrono::high_resolution_clock::now(); + std::chrono::high_resolution_clock::time_point diagLastUpdateTime = std::chrono::high_resolution_clock::now(); }; } // namespace widgets \ No newline at end of file diff --git a/DSFE_App/DSFE_GUI/src/MainWindow/Widgets/ControlPanelWidget.cpp b/DSFE_App/DSFE_GUI/src/MainWindow/Widgets/ControlPanelWidget.cpp index 8dad56ff..8de97d97 100644 --- a/DSFE_App/DSFE_GUI/src/MainWindow/Widgets/ControlPanelWidget.cpp +++ b/DSFE_App/DSFE_GUI/src/MainWindow/Widgets/ControlPanelWidget.cpp @@ -1,23 +1,123 @@ // DSFE_GUI ControlPanelWidget.cpp #include "Widgets/ControlPanelWidget.h" -#include "Scene/SimulationManager.h" -#include "Widgets/RobotSelectorWidget.h" -#include #include +#include +#include +#include +#include + +#include "Scene/Mesh.h" +#include "Scene/Object.h" +#include "Scene/Light.h" +#include "Scene/Camera.h" + +#include "Scene/SimulationManager.h" +#include "Robots/RobotSystem.h" + +#include "Analysis/Telemetry.h" +#include "Platform/Paths.h" +#include "EngineLib/LogMacros.h" namespace widgets { - ControlPanelWidget::ControlPanelWidget(gui::SimManager* sim, QWidget* parent) : QWidget(parent), _sim(sim) { + ControlPanelWidget::ControlPanelWidget(gui::SimManager* sim, QWidget* parent) + : QWidget(parent), _sim(sim) + { auto* rootLayout = new QVBoxLayout(this); rootLayout->setContentsMargins(4, 4, 4, 4); auto* scrollArea = new QScrollArea(this); scrollArea->setWidgetResizable(true); auto* content = new QWidget(scrollArea); - auto* contentLayout = new QVBoxLayout(content); - contentLayout->addWidget(new RobotSelectorWidget(sim, content)); - contentLayout->addStretch(); // push widgets to top - content->setLayout(contentLayout); + _contentLayout = new QVBoxLayout(content); + content->setLayout(_contentLayout); scrollArea->setWidget(content); rootLayout->addWidget(scrollArea); + + simPropertiesPanel(); + + _contentLayout->addStretch(); + } + + // SimSetupPanel for + void ControlPanelWidget::simPropertiesPanel() { + auto* robot = _sim->robotSystem(); + _simPropertiesGroup = new QGroupBox("Simulation Properties"); + auto* layout = new QVBoxLayout(_simPropertiesGroup); + _useAutoDiffCheck = new QCheckBox("Enable Automatic Differentiable Integrators"); + layout->addWidget(_useAutoDiffCheck); + _integratorCombo = new QComboBox(); + layout->addWidget(_integratorCombo); + _contentLayout->addWidget(_simPropertiesGroup); + + buildIntegratorCombos(); + connect(_useAutoDiffCheck, &QCheckBox::toggled, this, [this, robot](bool checked) { + _useAutoDiff = checked; + robot->enableAutoDiff(checked); + buildIntegratorCombos(); + }); + + connect(_integratorCombo, QOverload::of(&QComboBox::currentIndexChanged), this, [this](int) { + if (_useAutoDiff) { + auto selectedMethod = static_cast(_integratorCombo->currentData().toInt()); + _sim->setADIntegrationMethod(selectedMethod); + } + else { + auto selectedMethod = static_cast(_integratorCombo->currentData().toInt()); + _sim->setIntegrationMethod(selectedMethod); + } + }); + } + + void ControlPanelWidget::buildIntegratorCombos() { + QSignalBlocker blocker(_integratorCombo); + _integratorCombo->clear(); + if (_useAutoDiff) { + for (const auto& entry : adIntegrators) { + _integratorCombo->addItem(entry.name, static_cast(entry.method)); + } + auto currentMethod = _sim->autoDiffIntegrationMethod(); + int index = _integratorCombo->findData(static_cast(currentMethod)); + if (index != -1) { + _integratorCombo->setCurrentIndex(index); + } + } + else { + for (const auto& entry : integrators) { + _integratorCombo->addItem(entry.name, static_cast(entry.method)); + } + auto currentMethod = _sim->integrationMethod(); + int index = _integratorCombo->findData(static_cast(currentMethod)); + if (index != -1) { + _integratorCombo->setCurrentIndex(index); + } + } + } + + void ControlPanelWidget::jointInfoPanel() { + } + + void ControlPanelWidget::displayPanel() { + } + + void ControlPanelWidget::selectJointAndFollow(int jointIdx) { + if (!_sim || !_sim->hasRobot()) { return; } + + robots::RobotSystem* robot = _sim->robotSystem(); + if (!robot) { return; } + + auto& joints = robot->joints(); + auto& links = robot->links(); + if (joints.empty()) { return; } + + jointIdx = std::clamp(jointIdx, 0, (int)joints.size() - 1); + + const auto& joint = joints[jointIdx]; + _currentJointName = joint.name; + + _selection.type = SelectionType::JOINT; + _selection.index = jointIdx; + _selection.source = SelectionSource::CONTROL_PANEL; + + _sim->followRobotJoint(_currentJointName, glm::vec3(0.0f, 0.2f, 0.6f)); } } // namespace widgets \ No newline at end of file From b0abf123a76e364baacdc10e0d49cc7bab038a44 Mon Sep 17 00:00:00 2001 From: saltyjoss Date: Sat, 6 Jun 2026 11:05:53 +0100 Subject: [PATCH 118/210] fixes: Small code corrections to compliment new logic --- .../include/Platform/SimulationState.h | 10 ++- .../src/MainWindow/DSFE_MainWindow.cpp | 61 +++++++++++++++++-- 2 files changed, 64 insertions(+), 7 deletions(-) diff --git a/DSFE_App/DSFE_Core/include/Platform/SimulationState.h b/DSFE_App/DSFE_Core/include/Platform/SimulationState.h index 50173444..446bbee0 100644 --- a/DSFE_App/DSFE_Core/include/Platform/SimulationState.h +++ b/DSFE_App/DSFE_Core/include/Platform/SimulationState.h @@ -38,4 +38,12 @@ struct DSFE_API modes { eRunMode _runMode = eRunMode::Interactive; }; - +// Struct to hold comparison results for integrator analysis +struct ComparisonSnapshot { + std::string integratorName; + std::vector time; // time samples + std::vector errRms; // RMS error time series + std::vector errMax; // Max error time series + std::vector> jointErr; // [joint][sample] + int jointCount = 0; +}; \ No newline at end of file diff --git a/DSFE_App/DSFE_GUI/src/MainWindow/DSFE_MainWindow.cpp b/DSFE_App/DSFE_GUI/src/MainWindow/DSFE_MainWindow.cpp index ce06bc42..dc3de915 100644 --- a/DSFE_App/DSFE_GUI/src/MainWindow/DSFE_MainWindow.cpp +++ b/DSFE_App/DSFE_GUI/src/MainWindow/DSFE_MainWindow.cpp @@ -30,13 +30,62 @@ namespace window { // File menu { - auto* openAction = fileMenu->addAction("Open"); - connect(openAction, &QAction::triggered, this, []() { + auto* newMenu = fileMenu->addMenu("New"); + connect(newMenu, &QMenu::aboutToShow, this, [this, newMenu]() { + LOG_INFO("Menu clicked: File -> New"); + auto* newProjectAction = newMenu->addAction("Project"); + connect(newProjectAction, &QAction::triggered, this, []() { + LOG_INFO("Menu clicked: File -> New -> New Project"); + }); + newMenu->addSeparator(); + auto* newScriptAction = newMenu->addAction("Script"); + connect(newScriptAction, &QAction::triggered, this, []() { + LOG_INFO("Menu clicked: File -> New -> New Script"); + }); + }); + auto* openMenu = fileMenu->addMenu("Open"); + connect(openMenu, &QMenu::aboutToShow, this, [this, openMenu]() { LOG_INFO("Menu clicked: File -> Open"); + auto* openProjectAction = openMenu->addAction("Project"); + connect(openProjectAction, &QAction::triggered, this, []() { + LOG_INFO("Menu clicked: File -> Open -> Project"); + }); + openMenu->addSeparator(); + auto* openScriptAction = openMenu->addAction("Script (*.dsl *.txt)"); + connect(openScriptAction, &QAction::triggered, this, []() { + LOG_INFO("Menu clicked: File -> Open -> Script"); + }); }); - auto* saveAction = fileMenu->addAction("Save"); - connect(saveAction, &QAction::triggered, this, []() { + fileMenu->addSeparator(); + auto* saveMenu = fileMenu->addMenu("Save"); + connect(saveMenu, &QMenu::aboutToShow, this, [this, saveMenu]() { LOG_INFO("Menu clicked: File -> Save"); + auto* saveProjectAction = saveMenu->addAction("Project"); + connect(saveProjectAction, &QAction::triggered, this, []() { + LOG_INFO("Menu clicked: File -> Save -> Project"); + }); + auto* saveScriptAction = saveMenu->addAction("Script"); + connect(saveScriptAction, &QAction::triggered, this, []() { + LOG_INFO("Menu clicked: File -> Save -> Script"); + QString path = QFileDialog::getSaveFileName(nullptr, "Save Script", "", "DSL Script Files (*.dsl);;Text Files (*.txt)"); + if (path.isEmpty()) { return; } + LOG_INFO("Selected path: %s", path.toStdString().c_str()); + }); + }); + auto* saveAsMenu = fileMenu->addMenu("Save As"); + connect(saveAsMenu, &QMenu::aboutToShow, this, [this, saveAsMenu]() { + LOG_INFO("Menu clicked: File -> Save As"); + auto* saveProjectAsAction = saveAsMenu->addAction("Project"); + connect(saveProjectAsAction, &QAction::triggered, this, []() { + LOG_INFO("Menu clicked: File -> Save As -> Project"); + }); + auto* saveScriptAsAction = saveAsMenu->addAction("Script"); + connect(saveScriptAsAction, &QAction::triggered, this, []() { + LOG_INFO("Menu clicked: File -> Save As -> Script"); + QString path = QFileDialog::getSaveFileName(nullptr, "Save Script As", "", "DSL Script Files (*.dsl);;Text Files (*.txt)"); + if (path.isEmpty()) { return; } + LOG_INFO("Selected path: %s", path.toStdString().c_str()); + }); }); fileMenu->addSeparator(); auto* exitAction = fileMenu->addAction("Exit"); @@ -70,14 +119,14 @@ namespace window { LOG_INFO("Menu clicked: Project -> Load Mesh"); QString path = QFileDialog::getOpenFileName(nullptr, "Select Mesh File", "", "Mesh Files (*.obj *.fbx *.gltf *.dae *.stl"); if (path.isEmpty()) { return; } - _sim->loadMesh(path.toStdString()); + _sim->loadMesh(path.toStdString()); // Crashes at the moment, TODO fix crash }); auto* loadHDRAction = projectMenu->addAction("Load HDRI"); connect(loadHDRAction, &QAction::triggered, this, [this]() { LOG_INFO("Menu clicked: Project -> Load HDRI"); QString path = QFileDialog::getOpenFileName(nullptr, "Select HDRI File", "", "HDRI Files (*.hdr *.exr)"); if (path.isEmpty()) { return; } - _sim->loadNewHDR(path.toStdString()); + _sim->loadNewHDR_UI(path.toStdString()); }); } // View menu From 171739c2aadd86c4d02fe9c8aaefd335aeb6b653 Mon Sep 17 00:00:00 2001 From: saltyjoss Date: Sat, 6 Jun 2026 12:37:26 +0100 Subject: [PATCH 119/210] feat: Added basic joint telemetry in the new control panel widget via `jointInfoPanel` and `displayJointInfo` * Not really sure why I did this before getting the scripts working. * Going to add the dt fraction next, then focus on just getting script functioning back! --- .../MainWindow/Widgets/ControlPanelWidget.h | 11 ++ .../MainWindow/Widgets/ControlPanelWidget.cpp | 134 ++++++++++++++++++ DSFE_App/DSFE_GUI/src/ui/ControlPanel.cpp | 27 ---- 3 files changed, 145 insertions(+), 27 deletions(-) diff --git a/DSFE_App/DSFE_GUI/include/MainWindow/Widgets/ControlPanelWidget.h b/DSFE_App/DSFE_GUI/include/MainWindow/Widgets/ControlPanelWidget.h index 3196e462..6e9b56e2 100644 --- a/DSFE_App/DSFE_GUI/include/MainWindow/Widgets/ControlPanelWidget.h +++ b/DSFE_App/DSFE_GUI/include/MainWindow/Widgets/ControlPanelWidget.h @@ -33,6 +33,8 @@ class QVBoxLayout; class QCheckBox; class QComboBox; class QGroupBox; +class QLabel; +class QSlider; namespace widgets { class ControlPanelWidget : public QWidget { @@ -51,8 +53,11 @@ namespace widgets { void simPropertiesPanel(); void buildIntegratorCombos(); + void buildDtFractions(); void jointInfoPanel(); + void displayJointInfo(const diagnostics::TelemetryRecorder& rec, int& selectedJoint, QVBoxLayout* layout); + void displayPanel(); void selectJointAndFollow(int jointIdx); @@ -62,6 +67,12 @@ namespace widgets { QGroupBox* _simPropertiesGroup = nullptr; QCheckBox* _useAutoDiffCheck = nullptr; QComboBox* _integratorCombo = nullptr; + QLabel* _currentIntegratorLabel = nullptr; + + QGroupBox* _jointInfoGroup = nullptr; + QSlider* _jointIdxSlider = nullptr; + QLabel* _currentSimTimeJointLabel = nullptr; + static constexpr IntegratorEntry integrators[] = { { integration::eIntegrationMethod::Euler, "Euler" }, diff --git a/DSFE_App/DSFE_GUI/src/MainWindow/Widgets/ControlPanelWidget.cpp b/DSFE_App/DSFE_GUI/src/MainWindow/Widgets/ControlPanelWidget.cpp index 8de97d97..cc36d600 100644 --- a/DSFE_App/DSFE_GUI/src/MainWindow/Widgets/ControlPanelWidget.cpp +++ b/DSFE_App/DSFE_GUI/src/MainWindow/Widgets/ControlPanelWidget.cpp @@ -6,6 +6,10 @@ #include #include #include +#include +#include +#include +#include #include "Scene/Mesh.h" #include "Scene/Object.h" @@ -34,6 +38,7 @@ namespace widgets { rootLayout->addWidget(scrollArea); simPropertiesPanel(); + jointInfoPanel(); _contentLayout->addStretch(); } @@ -48,6 +53,7 @@ namespace widgets { _integratorCombo = new QComboBox(); layout->addWidget(_integratorCombo); _contentLayout->addWidget(_simPropertiesGroup); + _currentIntegratorLabel = new QLabel(); buildIntegratorCombos(); connect(_useAutoDiffCheck, &QCheckBox::toggled, this, [this, robot](bool checked) { @@ -60,12 +66,18 @@ namespace widgets { if (_useAutoDiff) { auto selectedMethod = static_cast(_integratorCombo->currentData().toInt()); _sim->setADIntegrationMethod(selectedMethod); + _currentIntegratorLabel->setText(QString("Current Integrator: ") + _integratorCombo->currentText()); } else { auto selectedMethod = static_cast(_integratorCombo->currentData().toInt()); _sim->setIntegrationMethod(selectedMethod); + _currentIntegratorLabel->setText(QString("Current Integrator: ") + _integratorCombo->currentText()); } }); + _currentIntegratorLabel->setWordWrap(true); + layout->addWidget(_currentIntegratorLabel); + + // TODO reintroduce old dt selection logic here, my aim is to still use the visual fraction selection as it looks way better (and its cool). } void ControlPanelWidget::buildIntegratorCombos() { @@ -94,6 +106,128 @@ namespace widgets { } void ControlPanelWidget::jointInfoPanel() { + _jointInfoGroup = new QGroupBox("Robot Joint Information"); + auto* layout = new QVBoxLayout(_jointInfoGroup); + _contentLayout->addWidget(_jointInfoGroup); + + if (!_sim->hasRobot()) { + layout->addWidget(new QLabel("No Robot Loaded.")); + return; + } + robots::RobotSystem* robot = _sim->robotSystem(); + if (!robot) { + layout->addWidget(new QLabel("Robotic system unavailable")); + return; + } + auto& joints = robot->joints(); + auto& links = robot->links(); + + + _jointIdxSlider = new QSlider(Qt::Orientation::Horizontal); + layout->addWidget(_jointIdxSlider); + + static int currentJointIndex = 0; + currentJointIndex = std::clamp(currentJointIndex, 0, (int)joints.size() - 1); + + auto& j = joints[currentJointIndex]; + int linkIndex = currentJointIndex; + linkIndex = std::clamp(linkIndex, 0, (int)links.size() - 1); + auto& l = links[linkIndex]; + + const auto& rec = _sim->telemetry(); + displayJointInfo(rec, currentJointIndex, layout); + layout->addSpacing(5); + layout->addWidget(new QLabel("Selected Joint: " + QString::fromStdString(j.name) + " - Child Link: " + QString::fromStdString(l.name))); + } + + void ControlPanelWidget::displayJointInfo(const diagnostics::TelemetryRecorder& rec, int& selectedJoint, QVBoxLayout* layout) { + const auto& ring = rec.ring; + if (ring.size() < 1) { return; } + const diagnostics::TelemetrySample& s = ring.at(ring.size() - 1); // Get the most recent sample + if (selectedJoint < 0) { selectedJoint = 0; } + if (selectedJoint >= (int)s.j.size()) { selectedJoint = (int)s.j.size() - 1; } + + _currentSimTimeJointLabel = new QLabel(QString("Current Simulation Time: ") + QString::number(s.timeSec) + " s"); + layout->addWidget(_currentSimTimeJointLabel); + + int currentJ = selectedJoint + 1; + if (!_jointIdxSlider) { + selectedJoint = currentJ - 1; + + _jointIdxSlider->setMinimum(1); + _jointIdxSlider->setMaximum((int)s.j.size()); + _jointIdxSlider->setValue(currentJ); + connect(_jointIdxSlider, &QSlider::valueChanged, this, [this](int value) { + int jointIdx = value - 1; + selectJointAndFollow(jointIdx); + }); + } + else { + selectedJoint = currentJ - 1; + _jointIdxSlider->setMaximum((int)s.j.size()); + _jointIdxSlider->setValue(currentJ); + } + + const diagnostics::JointTelemetry& j = s.j[selectedJoint]; + const float e = static_cast(j.q_ref - j.q); + + layout->addSpacing(10); + + auto headerFont = [](QLabel* label) { + QFont font = label->font(); + font.setBold(true); + font.setPointSize(font.pointSize() + 2); + label->setFont(font); + }; + + + auto* stateLabel = new QLabel(QString("State: ")); + headerFont(stateLabel); + auto* refLabel = new QLabel(QString("Reference: ")); + headerFont(refLabel); + auto* trajLabel = new QLabel(QString("Trajectory: ")); + headerFont(trajLabel); + auto* clampLabel = new QLabel(QString("Clamped: ")); + headerFont(clampLabel); + auto* constLabel = new QLabel(QString("Constants: ")); + headerFont(constLabel); + + // Joint State Telemetry + layout->addWidget(stateLabel); + layout->addWidget(new QLabel(QString("pos: ") + QString::number(j.q) + " rad")); + layout->addWidget(new QLabel(QString("vel: ") + QString::number(j.qd) + " rad/s")); + layout->addWidget(new QLabel(QString("torque: ") + QString::number(j.torqueNm) + " Nm")); + + layout->addSpacing(5); + + // Joint Reference Telemetry + layout->addWidget(refLabel); + layout->addWidget(new QLabel(QString("pos_ref: ") + QString::number(j.q_ref) + " rad")); + layout->addWidget(new QLabel(QString("vel_ref: ") + QString::number(j.qd_ref) + " rad/s")); + layout->addWidget(new QLabel(QString("acc_ref: ") + QString::number(j.qdd_ref) + " rad/s²")); + layout->addWidget(new QLabel(QString("error: ") + QString::number(e) + " rad")); + + layout->addSpacing(5); + + // Joint Trajectory Telemetry + layout->addWidget(trajLabel); + layout->addWidget(new QLabel(QString("pos_traj: ") + QString::number(j.traj_q) + " rad")); + layout->addWidget(new QLabel(QString("vel_traj: ") + QString::number(j.traj_qd) + " rad/s")); + layout->addWidget(new QLabel(QString("acc_traj: ") + QString::number(j.traj_qdd) + " rad/s²")); + + layout->addSpacing(5); + + // Joint Clamping Telemetry + layout->addWidget(clampLabel); + layout->addWidget(new QLabel(QString("pos_clamped: ") + QString(j.clampTheta ? "true" : "false"))); + layout->addWidget(new QLabel(QString("vel_clamped: ") + QString(j.clampOmega ? "true" : "false"))); + + layout->addSpacing(5); + + // Joint Constants Telemetry + layout->addWidget(constLabel); + layout->addWidget(new QLabel(QString("damping: ") + QString::number(j.damping) + " kg·m²/s")); + layout->addWidget(new QLabel(QString("friction: ") + QString::number(j.friction) + " N·m")); } void ControlPanelWidget::displayPanel() { diff --git a/DSFE_App/DSFE_GUI/src/ui/ControlPanel.cpp b/DSFE_App/DSFE_GUI/src/ui/ControlPanel.cpp index c3122a33..50f2b096 100644 --- a/DSFE_App/DSFE_GUI/src/ui/ControlPanel.cpp +++ b/DSFE_App/DSFE_GUI/src/ui/ControlPanel.cpp @@ -518,33 +518,6 @@ namespace gui { } } - //// Torque mode combo box - //ImGui::SectionHeader("Torque Mode"); - //ImGui::SetNextItemWidth(150.0f); - //if (ImGui::BeginCombo("##tau_mode", currentTorqueMode)) { - // for (int n = 0; n < IM_ARRAYSIZE(torqueModeNames); ++n) { - // bool isSelected = (n == static_cast(currentTauEnum)); - - // // When a new mode is selected, update the robot's torque mode and log the change - // if (ImGui::Selectable(torqueModeNames[n], isSelected)) { - // auto updatedMode = static_cast(n); - // robot->setTorqueMode(updatedMode); - - // switch (updatedMode) { - // case robots::eTorqueMode::NONE: - // D_INFO("Torque mode set to None"); break; - // case robots::eTorqueMode::PASSIVE: - // D_INFO("Torque mode set to Passive"); break; - // case robots::eTorqueMode::CONTROLLED: - // D_INFO("Torque mode set to Controlled"); break; - // default: - // break; - // } - // } - // if (isSelected) { ImGui::SetItemDefaultFocus(); } - // } - // ImGui::EndCombo(); - //} ImGui::EndDisabled(); // Delta time controls From 51add8503d0c2aa00f80979dbd0977831f08e8ba Mon Sep 17 00:00:00 2001 From: saltyjoss Date: Sat, 6 Jun 2026 12:55:00 +0100 Subject: [PATCH 120/210] feat: refactor old fraction logic from imgui widgets into Qt6 * With help from AI for this, only for the actual Qt6 syntax (still learning). --- DSFE_App/DSFE_GUI/CMakeLists.txt | 1 + .../Widgets/FractionSelectorWidget.h | 27 +++++++++ .../Widgets/FractionSelectorWidget.cpp | 55 +++++++++++++++++++ 3 files changed, 83 insertions(+) create mode 100644 DSFE_App/DSFE_GUI/include/MainWindow/Widgets/FractionSelectorWidget.h create mode 100644 DSFE_App/DSFE_GUI/src/MainWindow/Widgets/FractionSelectorWidget.cpp diff --git a/DSFE_App/DSFE_GUI/CMakeLists.txt b/DSFE_App/DSFE_GUI/CMakeLists.txt index 80286bb5..8b8572de 100644 --- a/DSFE_App/DSFE_GUI/CMakeLists.txt +++ b/DSFE_App/DSFE_GUI/CMakeLists.txt @@ -43,6 +43,7 @@ set(MAIN_WINDOW_SRC src/MainWindow/Widgets/ViewportWidget.cpp src/MainWindow/Widgets/RobotSelectorWidget.cpp src/MainWindow/Widgets/ControlPanelWidget.cpp + src/MainWindow/Widgets/FractionSelectorWidget.cpp ) set(RENDER_SRC diff --git a/DSFE_App/DSFE_GUI/include/MainWindow/Widgets/FractionSelectorWidget.h b/DSFE_App/DSFE_GUI/include/MainWindow/Widgets/FractionSelectorWidget.h new file mode 100644 index 00000000..03c9b854 --- /dev/null +++ b/DSFE_App/DSFE_GUI/include/MainWindow/Widgets/FractionSelectorWidget.h @@ -0,0 +1,27 @@ +// DSFE_GUI FractionSelectorWidget.h +#pragma once + +#include + +namespace widgets { + class FractionSelectorWidget : public QWidget { + public: + explicit FractionSelectorWidget(bool telemetryMode = false, QWidget* parent = nullptr); + double dt() const; + + signals: + void valueChanged(double denom); + + protected: + void paintEvent(QPaintEvent* event) override; + void mousePressEvent(QMouseEvent* event) override; + void mouseMoveEvent(QMouseEvent* event) override; + void wheelEvent(QWheelEvent* event) override; + + private: + int _k = 6; + int _minK = 1; + int _lastMouseX = 0; + bool _telemetryMode = false; // If true, the widget is being used to select a telemetry recording fraction, so it should display "Telemetry: 1/k" instead of "dt: 1/k" + }; +} \ No newline at end of file diff --git a/DSFE_App/DSFE_GUI/src/MainWindow/Widgets/FractionSelectorWidget.cpp b/DSFE_App/DSFE_GUI/src/MainWindow/Widgets/FractionSelectorWidget.cpp new file mode 100644 index 00000000..72c0f45e --- /dev/null +++ b/DSFE_App/DSFE_GUI/src/MainWindow/Widgets/FractionSelectorWidget.cpp @@ -0,0 +1,55 @@ +// DSFE_GUI FractionSelectorWidget.cpp +#include "Widgets/FractionSelectorWidget.h" + +#include +#include +#include + +namespace widgets { + FractionSelectorWidget::FractionSelectorWidget(bool telemetryMode, QWidget* parent) + : QWidget(parent), _telemetryMode(telemetryMode) + { + setMinimumSize(100, 70); + } + + double FractionSelectorWidget::dt() const { + return 1.0 / static_cast(10 * _k); + } + + void FractionSelectorWidget::paintEvent(QPaintEvent* event) { + QPainter p(this); + QString numer = "1"; + QString denom = QString::number(10 * _k); + QFontMetrics fm(p.font()); + + int centreX = width() / 2; + int numerW = fm.horizontalAdvance(numer); + int denomW = fm.horizontalAdvance(denom); + + p.drawText(centreX - numerW / 2, 20, numer); + p.drawLine(centreX - std::max(numerW, denomW) / 2 - 2, 30, centreX + std::max(numerW, denomW) / 2 + 2, 30); + p.drawText(centreX - denomW / 2, 50, denom); + } + + void FractionSelectorWidget::mousePressEvent(QMouseEvent* event) { _lastMouseX = event->pos().x(); } + + void FractionSelectorWidget::mouseMoveEvent(QMouseEvent* event) { + int dx = event->pos().x() - _lastMouseX; + if (dx != 0) { + _k += dx; + int max = _telemetryMode ? 100 : 5000; + _k = std::clamp(_k, _minK, max); + _lastMouseX = event->pos().x(); + update(); + emit valueChanged(1.0 / static_cast(10 * _k)); + } + } + + void FractionSelectorWidget::wheelEvent(QWheelEvent* event) { + int delta = event->angleDelta().y() > 0 ? 1 : -1; + int max = _telemetryMode ? 100 : 5000; + _k = std::clamp(_k + delta, _minK, max); + update(); + emit valueChanged(1.0 / static_cast(10 * _k)); + } +} \ No newline at end of file From e6b0a923bf96ba1d48c8c6911c678fdcdc53ffaf Mon Sep 17 00:00:00 2001 From: saltyjoss Date: Sat, 6 Jun 2026 13:26:07 +0100 Subject: [PATCH 121/210] feat: Implemented timestepping fractions into `simPropertiesPanel()` --- .../MainWindow/Widgets/ControlPanelWidget.h | 4 +++ .../MainWindow/Widgets/ControlPanelWidget.cpp | 25 ++++++++++++++++--- 2 files changed, 25 insertions(+), 4 deletions(-) diff --git a/DSFE_App/DSFE_GUI/include/MainWindow/Widgets/ControlPanelWidget.h b/DSFE_App/DSFE_GUI/include/MainWindow/Widgets/ControlPanelWidget.h index 6e9b56e2..b0892c54 100644 --- a/DSFE_App/DSFE_GUI/include/MainWindow/Widgets/ControlPanelWidget.h +++ b/DSFE_App/DSFE_GUI/include/MainWindow/Widgets/ControlPanelWidget.h @@ -37,6 +37,8 @@ class QLabel; class QSlider; namespace widgets { + class FractionSelectorWidget; + class ControlPanelWidget : public QWidget { public: explicit ControlPanelWidget(gui::SimManager* sim, QWidget* parent = nullptr); @@ -68,6 +70,8 @@ namespace widgets { QCheckBox* _useAutoDiffCheck = nullptr; QComboBox* _integratorCombo = nullptr; QLabel* _currentIntegratorLabel = nullptr; + FractionSelectorWidget* _simDtSelector = nullptr; + FractionSelectorWidget* _telemetryDtSelector = nullptr; QGroupBox* _jointInfoGroup = nullptr; QSlider* _jointIdxSlider = nullptr; diff --git a/DSFE_App/DSFE_GUI/src/MainWindow/Widgets/ControlPanelWidget.cpp b/DSFE_App/DSFE_GUI/src/MainWindow/Widgets/ControlPanelWidget.cpp index cc36d600..37d9d133 100644 --- a/DSFE_App/DSFE_GUI/src/MainWindow/Widgets/ControlPanelWidget.cpp +++ b/DSFE_App/DSFE_GUI/src/MainWindow/Widgets/ControlPanelWidget.cpp @@ -11,6 +11,8 @@ #include #include +#include "Widgets/FractionSelectorWidget.h" + #include "Scene/Mesh.h" #include "Scene/Object.h" #include "Scene/Light.h" @@ -46,14 +48,28 @@ namespace widgets { // SimSetupPanel for void ControlPanelWidget::simPropertiesPanel() { auto* robot = _sim->robotSystem(); + _simPropertiesGroup = new QGroupBox("Simulation Properties"); auto* layout = new QVBoxLayout(_simPropertiesGroup); + _useAutoDiffCheck = new QCheckBox("Enable Automatic Differentiable Integrators"); layout->addWidget(_useAutoDiffCheck); _integratorCombo = new QComboBox(); layout->addWidget(_integratorCombo); - _contentLayout->addWidget(_simPropertiesGroup); + _currentIntegratorLabel = new QLabel(); + layout->addWidget(_currentIntegratorLabel); + + layout->addSpacing(5); + + _simDtSelector = new FractionSelectorWidget(false); + layout->addWidget(new QLabel("Simulation Time Step (dt):")); + layout->addWidget(_simDtSelector); + _telemetryDtSelector = new FractionSelectorWidget(true); + layout->addWidget(new QLabel("Telemetry Time Step (dt):")); + layout->addWidget(_telemetryDtSelector); + + _contentLayout->addWidget(_simPropertiesGroup); buildIntegratorCombos(); connect(_useAutoDiffCheck, &QCheckBox::toggled, this, [this, robot](bool checked) { @@ -66,18 +82,19 @@ namespace widgets { if (_useAutoDiff) { auto selectedMethod = static_cast(_integratorCombo->currentData().toInt()); _sim->setADIntegrationMethod(selectedMethod); + _currentIntegratorLabel->setWordWrap(true); _currentIntegratorLabel->setText(QString("Current Integrator: ") + _integratorCombo->currentText()); } else { auto selectedMethod = static_cast(_integratorCombo->currentData().toInt()); _sim->setIntegrationMethod(selectedMethod); + _currentIntegratorLabel->setWordWrap(true); _currentIntegratorLabel->setText(QString("Current Integrator: ") + _integratorCombo->currentText()); } }); - _currentIntegratorLabel->setWordWrap(true); - layout->addWidget(_currentIntegratorLabel); - // TODO reintroduce old dt selection logic here, my aim is to still use the visual fraction selection as it looks way better (and its cool). + connect(_simDtSelector, &FractionSelectorWidget::valueChanged, this, [this](double dt) { _sim->setFixedDt(dt); }); + connect(_telemetryDtSelector, &FractionSelectorWidget::valueChanged, this, [this](double dt) { _sim->setTelemetryHz(1.0/dt); }); } void ControlPanelWidget::buildIntegratorCombos() { From c41f5cea57b1fd8fc619746eaac88681e4710780 Mon Sep 17 00:00:00 2001 From: saltyjoss Date: Sat, 6 Jun 2026 13:26:43 +0100 Subject: [PATCH 122/210] fixes: Added declaration of QObject for class def, hardcoded header location in CMAKE for MOC --- DSFE_App/DSFE_GUI/CMakeLists.txt | 1 + .../DSFE_GUI/include/MainWindow/Widgets/FractionSelectorWidget.h | 1 + 2 files changed, 2 insertions(+) diff --git a/DSFE_App/DSFE_GUI/CMakeLists.txt b/DSFE_App/DSFE_GUI/CMakeLists.txt index 8b8572de..9ba5c59f 100644 --- a/DSFE_App/DSFE_GUI/CMakeLists.txt +++ b/DSFE_App/DSFE_GUI/CMakeLists.txt @@ -44,6 +44,7 @@ set(MAIN_WINDOW_SRC src/MainWindow/Widgets/RobotSelectorWidget.cpp src/MainWindow/Widgets/ControlPanelWidget.cpp src/MainWindow/Widgets/FractionSelectorWidget.cpp + include/MainWindow/Widgets/FractionSelectorWidget.h ) set(RENDER_SRC diff --git a/DSFE_App/DSFE_GUI/include/MainWindow/Widgets/FractionSelectorWidget.h b/DSFE_App/DSFE_GUI/include/MainWindow/Widgets/FractionSelectorWidget.h index 03c9b854..5ebc5946 100644 --- a/DSFE_App/DSFE_GUI/include/MainWindow/Widgets/FractionSelectorWidget.h +++ b/DSFE_App/DSFE_GUI/include/MainWindow/Widgets/FractionSelectorWidget.h @@ -5,6 +5,7 @@ namespace widgets { class FractionSelectorWidget : public QWidget { + Q_OBJECT public: explicit FractionSelectorWidget(bool telemetryMode = false, QWidget* parent = nullptr); double dt() const; From a760994f8d71af04a51fd6fc4e68f35a9532dd95 Mon Sep 17 00:00:00 2001 From: saltyjoss Date: Sat, 6 Jun 2026 15:09:08 +0100 Subject: [PATCH 123/210] feat(WIP): Added basic DSL script loading and running (which needs fixing) * The running is malformed somewhere (need to fix now) * I believe Qt6 means I can add syntax highlighting!! * Need to coordinate colours --- DSFE_App/DSFE_GUI/CMakeLists.txt | 2 + .../include/MainWindow/DSFE_MainWindow.h | 3 + .../MainWindow/Widgets/DSLEditorWidget.h | 64 ++++++ .../MainWindow/Workspace/ProjectPage.h | 5 + .../src/MainWindow/DSFE_MainWindow.cpp | 28 ++- .../MainWindow/Widgets/DSLEditorWidget.cpp | 201 ++++++++++++++++++ .../src/MainWindow/Workspace/ProjectPage.cpp | 7 +- 7 files changed, 298 insertions(+), 12 deletions(-) create mode 100644 DSFE_App/DSFE_GUI/include/MainWindow/Widgets/DSLEditorWidget.h create mode 100644 DSFE_App/DSFE_GUI/src/MainWindow/Widgets/DSLEditorWidget.cpp diff --git a/DSFE_App/DSFE_GUI/CMakeLists.txt b/DSFE_App/DSFE_GUI/CMakeLists.txt index 9ba5c59f..3f8d1e6c 100644 --- a/DSFE_App/DSFE_GUI/CMakeLists.txt +++ b/DSFE_App/DSFE_GUI/CMakeLists.txt @@ -43,6 +43,8 @@ set(MAIN_WINDOW_SRC src/MainWindow/Widgets/ViewportWidget.cpp src/MainWindow/Widgets/RobotSelectorWidget.cpp src/MainWindow/Widgets/ControlPanelWidget.cpp + src/MainWindow/Widgets/DSLEditorWidget.cpp + src/MainWindow/Widgets/FractionSelectorWidget.cpp include/MainWindow/Widgets/FractionSelectorWidget.h ) diff --git a/DSFE_App/DSFE_GUI/include/MainWindow/DSFE_MainWindow.h b/DSFE_App/DSFE_GUI/include/MainWindow/DSFE_MainWindow.h index da9520c5..9f381d08 100644 --- a/DSFE_App/DSFE_GUI/include/MainWindow/DSFE_MainWindow.h +++ b/DSFE_App/DSFE_GUI/include/MainWindow/DSFE_MainWindow.h @@ -5,6 +5,8 @@ #include namespace gui { class SimManager; } +namespace widgets { class DSLEditorWidget; } + namespace window { class DSFE_MainWindow : public QMainWindow { public: @@ -13,6 +15,7 @@ namespace window { private: void buildMenuBar(); gui::SimManager* _sim; + widgets::DSLEditorWidget* _dslEditor = nullptr; void buildRobotMenu(QMenu* projectMenu); }; diff --git a/DSFE_App/DSFE_GUI/include/MainWindow/Widgets/DSLEditorWidget.h b/DSFE_App/DSFE_GUI/include/MainWindow/Widgets/DSLEditorWidget.h new file mode 100644 index 00000000..9ae11d6d --- /dev/null +++ b/DSFE_App/DSFE_GUI/include/MainWindow/Widgets/DSLEditorWidget.h @@ -0,0 +1,64 @@ +// DSFE_GUI DSLEditorWidget.h +#pragma once + +#include + +#include +#include +#include +#include + +class QTextEdit; +class QLabel; +class QPushButton; +class QTabWidget; +class QTimer; + +namespace gui { class SimManager; } +namespace interpreter { + class Parser; + class IStoredProgram; + class RunWrapper; +} + +namespace widgets { + class DSLEditorWidget : public QWidget { + public: + explicit DSLEditorWidget(gui::SimManager* sim, QWidget* parent = nullptr); + + bool loadScript(const QString& fileName); + bool saveScript(const QString& fileName); + + private: + void runScript(); + void stopScript(); + + void buildEditorTab(); + void buildHelpTab(); + + void terminateScript(const char* reason, bool fault); + void pollScriptState(); + void updateButtonState(bool running); + void setStatusMessage(const QString& message); + + void setScript(const std::string& s) { _scriptText = s; } + std::string getScript() const { return _scriptText; } + + std::string _scriptText; + + gui::SimManager* _sim = nullptr; + interpreter::Parser* _parser; + interpreter::IStoredProgram* _program; + interpreter::RunWrapper* _wrapper; + + QTabWidget* _tabs = nullptr; + QWidget* _editorTab = nullptr; + QWidget* _helpTab = nullptr; + QTextEdit* _scriptEditor = nullptr; + QTextEdit* _dslHelp = nullptr; + QPushButton* _runStopButton = nullptr; + QLabel* _statusLabel = nullptr; + QString _currentScriptPath; + QTimer* _stateTimer = nullptr; + }; +} // namespace widgets \ No newline at end of file diff --git a/DSFE_App/DSFE_GUI/include/MainWindow/Workspace/ProjectPage.h b/DSFE_App/DSFE_GUI/include/MainWindow/Workspace/ProjectPage.h index eac14829..bc338d3a 100644 --- a/DSFE_App/DSFE_GUI/include/MainWindow/Workspace/ProjectPage.h +++ b/DSFE_App/DSFE_GUI/include/MainWindow/Workspace/ProjectPage.h @@ -4,10 +4,15 @@ #include namespace gui { class SimManager; } +namespace widgets { class DSLEditorWidget; } namespace Workspace { class ProjectPage : public QWidget { public: explicit ProjectPage(gui::SimManager* sim, QWidget* parent = nullptr); + widgets::DSLEditorWidget* editor() const; + + private: + widgets::DSLEditorWidget* _editor = nullptr; }; } // namespace Workspace \ No newline at end of file diff --git a/DSFE_App/DSFE_GUI/src/MainWindow/DSFE_MainWindow.cpp b/DSFE_App/DSFE_GUI/src/MainWindow/DSFE_MainWindow.cpp index dc3de915..7f42a8f6 100644 --- a/DSFE_App/DSFE_GUI/src/MainWindow/DSFE_MainWindow.cpp +++ b/DSFE_App/DSFE_GUI/src/MainWindow/DSFE_MainWindow.cpp @@ -2,6 +2,7 @@ #include "MainWindow/DSFE_MainWindow.h" #include "Scene/SimulationManager.h" #include "Workspace/ProjectPage.h" +#include "Widgets/DSLEditorWidget.h" #include "Platform/SystemMap.h" @@ -11,13 +12,16 @@ #include namespace window { - DSFE_MainWindow::DSFE_MainWindow(gui::SimManager* sim, QWidget* parent) : QMainWindow(parent), _sim(sim) { + DSFE_MainWindow::DSFE_MainWindow(gui::SimManager* sim, QWidget* parent) + : QMainWindow(parent), _sim(sim), _dslEditor(nullptr) + { setWindowTitle("DSFE"); resize(1280, 720); buildMenuBar(); auto* page = new Workspace::ProjectPage(sim, this); + _dslEditor = page->editor(); setCentralWidget(page); } @@ -52,8 +56,10 @@ namespace window { }); openMenu->addSeparator(); auto* openScriptAction = openMenu->addAction("Script (*.dsl *.txt)"); - connect(openScriptAction, &QAction::triggered, this, []() { - LOG_INFO("Menu clicked: File -> Open -> Script"); + connect(openScriptAction, &QAction::triggered, this, [this]() { + QString fileName = QFileDialog::getOpenFileName(nullptr, "Open Script", "", "DSL Script Files (*.dsl);;Text Files (*.txt)"); + if (fileName.isEmpty()) { return; } + _dslEditor->loadScript(fileName); }); }); fileMenu->addSeparator(); @@ -65,11 +71,11 @@ namespace window { LOG_INFO("Menu clicked: File -> Save -> Project"); }); auto* saveScriptAction = saveMenu->addAction("Script"); - connect(saveScriptAction, &QAction::triggered, this, []() { + connect(saveScriptAction, &QAction::triggered, this, [this]() { LOG_INFO("Menu clicked: File -> Save -> Script"); - QString path = QFileDialog::getSaveFileName(nullptr, "Save Script", "", "DSL Script Files (*.dsl);;Text Files (*.txt)"); - if (path.isEmpty()) { return; } - LOG_INFO("Selected path: %s", path.toStdString().c_str()); + QString fileName = QFileDialog::getSaveFileName(nullptr, "Save Script", "", "DSL Script Files (*.dsl);;Text Files (*.txt)"); + if (fileName.isEmpty()) { return; } + _dslEditor->saveScript(fileName); }); }); auto* saveAsMenu = fileMenu->addMenu("Save As"); @@ -80,11 +86,11 @@ namespace window { LOG_INFO("Menu clicked: File -> Save As -> Project"); }); auto* saveScriptAsAction = saveAsMenu->addAction("Script"); - connect(saveScriptAsAction, &QAction::triggered, this, []() { + connect(saveScriptAsAction, &QAction::triggered, this, [this]() { LOG_INFO("Menu clicked: File -> Save As -> Script"); - QString path = QFileDialog::getSaveFileName(nullptr, "Save Script As", "", "DSL Script Files (*.dsl);;Text Files (*.txt)"); - if (path.isEmpty()) { return; } - LOG_INFO("Selected path: %s", path.toStdString().c_str()); + QString fileName = QFileDialog::getSaveFileName(nullptr, "Save Script As", "", "DSL Script Files (*.dsl);;Text Files (*.txt)"); + if (fileName.isEmpty()) { return; } + _dslEditor->saveScript(fileName); }); }); fileMenu->addSeparator(); diff --git a/DSFE_App/DSFE_GUI/src/MainWindow/Widgets/DSLEditorWidget.cpp b/DSFE_App/DSFE_GUI/src/MainWindow/Widgets/DSLEditorWidget.cpp new file mode 100644 index 00000000..f2ed6d59 --- /dev/null +++ b/DSFE_App/DSFE_GUI/src/MainWindow/Widgets/DSLEditorWidget.cpp @@ -0,0 +1,201 @@ +// DSFE_GUI DSLEditorWidget.cpp +#include "Widgets/DSLEditorWidget.h" + +#include +#include +#include +#include +#include +#include +#include +#include + +#include "Platform/StudyRunner.h" +#include "Interpreter/RunWrapper.h" + +#include +#include +#include + +#include "Scene/SimulationManager.h" +#include "Platform/Paths.h" + +#include "Numerics/IntegrationMethods.h" +#include "Interpreter/StoredProgram.h" + +#include "EngineLib/LogMacros.h" + +namespace widgets { + DSLEditorWidget::DSLEditorWidget(gui::SimManager* sim, QWidget* parent) + : QWidget(parent), _sim(sim), _parser(nullptr), _program(nullptr), _wrapper(nullptr) + { + auto* rootLayout = new QVBoxLayout(this); + auto* buttonLayout = new QHBoxLayout(); + + _runStopButton = new QPushButton("Run", this); // May also move to the menu, but I want to test the logic first + + buttonLayout->addStretch(); + buttonLayout->addWidget(_runStopButton); + + rootLayout->addLayout(buttonLayout); + + _tabs = new QTabWidget(this); + rootLayout->addWidget(_tabs); + + buildEditorTab(); + buildHelpTab(); + + _statusLabel = new QLabel("Idle", this); + rootLayout->addWidget(_statusLabel); + + _stateTimer = new QTimer(this); + connect(_stateTimer, &QTimer::timeout, this, [this]() { pollScriptState(); }); + _stateTimer->start(100); // Poll every 100 ms + + connect(_runStopButton, &QPushButton::clicked, this, [this]() { + if (_sim->isScriptRunning()) { stopScript(); } + else { runScript(); } + }); + + pollScriptState(); + } + + bool DSLEditorWidget::loadScript(const QString& fileName) { + QFile file(fileName); + if (!file.open(QIODevice::ReadOnly | QIODevice::Text)) { + LOG_ERROR("Failed to open script file: %s", fileName.toStdString().c_str()); + return false; + } + _scriptEditor->setPlainText(file.readAll()); + file.close(); + _currentScriptPath = fileName; + + LOG_INFO("DSL script loaded from file: %s", fileName.toStdString().c_str()); + D_INFO("DSL script loaded from file: %s", fileName.toStdString().c_str()); + + return true; + } + + bool DSLEditorWidget::saveScript(const QString& fileName) { + QFile file(fileName); + if (!file.open(QIODevice::WriteOnly | QIODevice::Text)) { + LOG_ERROR("Failed to open script file for writing: %s", fileName.toStdString().c_str()); + return false; + } + file.write(_scriptEditor->toPlainText().toUtf8()); + file.close(); + _currentScriptPath = fileName; + + LOG_INFO("DSL script saved to file: %s", fileName.toStdString().c_str()); + D_INFO("DSL script saved to file: %s", fileName.toStdString().c_str()); + + return true; + } + + void DSLEditorWidget::setStatusMessage(const QString& message) { if (_statusLabel) { _statusLabel->setText(message); } } + + void DSLEditorWidget::buildEditorTab() { + _editorTab = new QWidget(this); + auto* layout = new QVBoxLayout(_editorTab); + _scriptEditor = new QTextEdit(_editorTab); + layout->addWidget(_scriptEditor); + _tabs->addTab(_editorTab, "Script Editor"); + } + + void DSLEditorWidget::buildHelpTab() { + _helpTab = new QWidget(this); + auto* layout = new QVBoxLayout(_helpTab); + _dslHelp = new QTextEdit(_helpTab); + _dslHelp->setReadOnly(true); + + + _dslHelp->setPlainText( + "trajSet(...)\n" + "trajClear()\n" + "rotateJointTo(...)\n" + "rotateJointBy(...)\n" + "load(...)\n" + "set(...)\n" + "start()\n" + "stop()\n" + "wait(...)\n" + ); + + + layout->addWidget(_dslHelp); + _tabs->addTab(_helpTab, "DSL Help"); + } + + void DSLEditorWidget::runScript() { + if (_sim->isScriptRunning()) { return; } + LOG_INFO("DSL script started."); D_INFO("DSL script started."); + + _sim->setActiveProgram(nullptr); + delete _wrapper; _wrapper = nullptr; + delete _parser; _parser = nullptr; + delete _program; _program = nullptr; + + _program = new interpreter::StoredProgram(_sim->simCoreInterface()); + _parser = new interpreter::Parser(_program); + _wrapper = new interpreter::RunWrapper(_parser, _program); + + _scriptText = _scriptEditor->toPlainText().toStdString(); + + _sim->setActiveProgram(_program); + _sim->setScriptRunning(true); + _sim->setLastScriptText(_scriptText); + + std::string code = _scriptText; + if (!code.empty() && code.back() == '\0') { code.pop_back(); } + + _wrapper->runProgram(_scriptText); + + updateButtonState(true); + } + + void DSLEditorWidget::stopScript() { + if (!_sim->isScriptRunning()) { return; } + if (_sim->activeProgram()) { + _sim->activeProgram()->stop(); + _sim->activeProgram()->reset(); + } + _sim->setActiveProgram(nullptr); + _sim->stopSimulation(); + _sim->setScriptRunning(false); + LOG_INFO("DSL script stopped."); D_INFO("DSL script stopped."); + + updateButtonState(false); + } + + void DSLEditorWidget::terminateScript(const char* reason, bool fault) { + _sim->setActiveProgram(nullptr); + _sim->stopSimulation(); + _sim->setScriptRunning(false); + + if (fault) { LOG_WARN("%s", reason); D_FAIL("%s", reason); } + else { LOG_INFO("%s", reason); D_SUCCESS("%s", reason); } + + delete _wrapper; _wrapper = nullptr; + delete _parser; _parser = nullptr; + delete _program; _program = nullptr; + updateButtonState(false); + setStatusMessage(reason); + _statusLabel->setStyleSheet(fault ? "color: rgb(180,40,40);" : "color: rgb(40,180,40);"); + } + + void DSLEditorWidget::pollScriptState() { + if (!_sim->isScriptRunning()) { return; } + auto* prog = _sim->activeProgram(); + if (!prog) { terminateScript("No active program found in simulation manager.", true); return; } + if (prog->isEmpty()) { terminateScript("Command script stopped -> program is empty.", true); return; } + if (prog->isFaulted()) { terminateScript("Command script stopped due to fault.", true); return; } + if (prog->isCompleted()) { terminateScript("Command script completed.", false); return; } + if (prog->isStopped() && !prog->isCompleted()) { terminateScript("Command script stopped.", true); return; } + } + + void DSLEditorWidget::updateButtonState(bool running) { + _runStopButton->setText(running ? "Stop" : "Run"); + _runStopButton->setStyleSheet(running ? "background-color: rgb(180,40,40);" : "background-color: rgb(40,180,40);"); + } + +} // namespace widgets \ No newline at end of file diff --git a/DSFE_App/DSFE_GUI/src/MainWindow/Workspace/ProjectPage.cpp b/DSFE_App/DSFE_GUI/src/MainWindow/Workspace/ProjectPage.cpp index c6d6bc5e..70f7d168 100644 --- a/DSFE_App/DSFE_GUI/src/MainWindow/Workspace/ProjectPage.cpp +++ b/DSFE_App/DSFE_GUI/src/MainWindow/Workspace/ProjectPage.cpp @@ -3,6 +3,7 @@ #include "Scene/SimulationManager.h" #include "Widgets/ViewportWidget.h" #include "Widgets/ControlPanelWidget.h" +#include "Widgets/DSLEditorWidget.h" #include #include @@ -15,7 +16,8 @@ namespace Workspace { auto* rootSplitter = new QSplitter(Qt::Horizontal, this); auto* centreSplitter = new QSplitter(Qt::Vertical); auto* rightSplitter = new QSplitter(Qt::Vertical); - rootSplitter->addWidget(new QLabel("Script Editor (TODO)", this)); + _editor = new widgets::DSLEditorWidget(sim, this); + rootSplitter->addWidget(_editor); centreSplitter->addWidget(new widgets::ViewportWidget(sim, this)); centreSplitter->addWidget(new QLabel("Console Output (TODO)", this)); rightSplitter->addWidget(new widgets::ControlPanelWidget(sim, this)); @@ -28,4 +30,7 @@ namespace Workspace { centreSplitter->setSizes({ 733, 396 }); rightSplitter->setSizes({ 733, 396 }); } + + widgets::DSLEditorWidget* ProjectPage::editor() const { return _editor; } + } // namespace Workspace \ No newline at end of file From 40a0c30a5680d8d4a57e02c254f97f709128f44d Mon Sep 17 00:00:00 2001 From: saltyjoss Date: Sat, 6 Jun 2026 15:13:23 +0100 Subject: [PATCH 124/210] fixes --- DSFE_App/DSFE_GUI/src/Scene/SimulationManager.cpp | 2 -- 1 file changed, 2 deletions(-) diff --git a/DSFE_App/DSFE_GUI/src/Scene/SimulationManager.cpp b/DSFE_App/DSFE_GUI/src/Scene/SimulationManager.cpp index 42b97400..5b0df1ad 100644 --- a/DSFE_App/DSFE_GUI/src/Scene/SimulationManager.cpp +++ b/DSFE_App/DSFE_GUI/src/Scene/SimulationManager.cpp @@ -285,8 +285,6 @@ namespace gui { float kspd = 0.2f * dt; // base speed m/s - LOG_INFO("dt = %f", dt); - if (pressedKeys.contains(eKeyCode::W)) { processMovementKey((int)eKeyCode::W, kspd); } if (pressedKeys.contains(eKeyCode::A)) { processMovementKey((int)eKeyCode::A, kspd); } if (pressedKeys.contains(eKeyCode::S)) { processMovementKey((int)eKeyCode::S, kspd); } From 0b79bfeb52f529e9153882cd7130b726a2b76cae Mon Sep 17 00:00:00 2001 From: saltyjoss Date: Sat, 6 Jun 2026 15:33:01 +0100 Subject: [PATCH 125/210] chore: Updated relevant file paths for widget dialogs --- .../MainWindow/Widgets/DSLEditorWidget.h | 2 ++ .../src/MainWindow/DSFE_MainWindow.cpp | 12 +++++---- .../MainWindow/Widgets/DSLEditorWidget.cpp | 26 +++++++++++++------ 3 files changed, 27 insertions(+), 13 deletions(-) diff --git a/DSFE_App/DSFE_GUI/include/MainWindow/Widgets/DSLEditorWidget.h b/DSFE_App/DSFE_GUI/include/MainWindow/Widgets/DSLEditorWidget.h index 9ae11d6d..794fb0b9 100644 --- a/DSFE_App/DSFE_GUI/include/MainWindow/Widgets/DSLEditorWidget.h +++ b/DSFE_App/DSFE_GUI/include/MainWindow/Widgets/DSLEditorWidget.h @@ -51,6 +51,8 @@ namespace widgets { interpreter::IStoredProgram* _program; interpreter::RunWrapper* _wrapper; + std::string _scriptWorkingDir; + QTabWidget* _tabs = nullptr; QWidget* _editorTab = nullptr; QWidget* _helpTab = nullptr; diff --git a/DSFE_App/DSFE_GUI/src/MainWindow/DSFE_MainWindow.cpp b/DSFE_App/DSFE_GUI/src/MainWindow/DSFE_MainWindow.cpp index 7f42a8f6..07ffa16a 100644 --- a/DSFE_App/DSFE_GUI/src/MainWindow/DSFE_MainWindow.cpp +++ b/DSFE_App/DSFE_GUI/src/MainWindow/DSFE_MainWindow.cpp @@ -11,6 +11,8 @@ #include #include +#include "Platform/Paths.h" + namespace window { DSFE_MainWindow::DSFE_MainWindow(gui::SimManager* sim, QWidget* parent) : QMainWindow(parent), _sim(sim), _dslEditor(nullptr) @@ -57,7 +59,7 @@ namespace window { openMenu->addSeparator(); auto* openScriptAction = openMenu->addAction("Script (*.dsl *.txt)"); connect(openScriptAction, &QAction::triggered, this, [this]() { - QString fileName = QFileDialog::getOpenFileName(nullptr, "Open Script", "", "DSL Script Files (*.dsl);;Text Files (*.txt)"); + QString fileName = QFileDialog::getOpenFileName(nullptr, "Open Script", QString::fromStdString((paths::assets() / "DSLScripts").string()), "DSL Script Files (*.dsl);;Text Files (*.txt)"); if (fileName.isEmpty()) { return; } _dslEditor->loadScript(fileName); }); @@ -73,7 +75,7 @@ namespace window { auto* saveScriptAction = saveMenu->addAction("Script"); connect(saveScriptAction, &QAction::triggered, this, [this]() { LOG_INFO("Menu clicked: File -> Save -> Script"); - QString fileName = QFileDialog::getSaveFileName(nullptr, "Save Script", "", "DSL Script Files (*.dsl);;Text Files (*.txt)"); + QString fileName = QFileDialog::getSaveFileName(nullptr, "Save Script", QString::fromStdString((paths::assets() / "DSLScripts").string()), "DSL Script Files (*.dsl);;Text Files (*.txt)"); // ARGS are if (fileName.isEmpty()) { return; } _dslEditor->saveScript(fileName); }); @@ -88,7 +90,7 @@ namespace window { auto* saveScriptAsAction = saveAsMenu->addAction("Script"); connect(saveScriptAsAction, &QAction::triggered, this, [this]() { LOG_INFO("Menu clicked: File -> Save As -> Script"); - QString fileName = QFileDialog::getSaveFileName(nullptr, "Save Script As", "", "DSL Script Files (*.dsl);;Text Files (*.txt)"); + QString fileName = QFileDialog::getSaveFileName(nullptr, "Save Script As", QString::fromStdString((paths::assets() / "DSLScripts").string()), "DSL Script Files (*.dsl);;Text Files (*.txt)"); if (fileName.isEmpty()) { return; } _dslEditor->saveScript(fileName); }); @@ -123,14 +125,14 @@ namespace window { auto* loadMeshAction = projectMenu->addAction("Load Mesh"); connect(loadMeshAction, &QAction::triggered, this, [this]() { LOG_INFO("Menu clicked: Project -> Load Mesh"); - QString path = QFileDialog::getOpenFileName(nullptr, "Select Mesh File", "", "Mesh Files (*.obj *.fbx *.gltf *.dae *.stl"); + QString path = QFileDialog::getOpenFileName(nullptr, "Select Mesh File", QString::fromStdString((paths::assets() / "objects" / "Shapes").string()), "Mesh Files (*.obj *.fbx *.gltf *.dae *.stl"); if (path.isEmpty()) { return; } _sim->loadMesh(path.toStdString()); // Crashes at the moment, TODO fix crash }); auto* loadHDRAction = projectMenu->addAction("Load HDRI"); connect(loadHDRAction, &QAction::triggered, this, [this]() { LOG_INFO("Menu clicked: Project -> Load HDRI"); - QString path = QFileDialog::getOpenFileName(nullptr, "Select HDRI File", "", "HDRI Files (*.hdr *.exr)"); + QString path = QFileDialog::getOpenFileName(nullptr, "Select HDRI File", QString::fromStdString((paths::assets() / "hdr").string()), "HDRI Files (*.hdr *.exr)"); if (path.isEmpty()) { return; } _sim->loadNewHDR_UI(path.toStdString()); }); diff --git a/DSFE_App/DSFE_GUI/src/MainWindow/Widgets/DSLEditorWidget.cpp b/DSFE_App/DSFE_GUI/src/MainWindow/Widgets/DSLEditorWidget.cpp index f2ed6d59..92665434 100644 --- a/DSFE_App/DSFE_GUI/src/MainWindow/Widgets/DSLEditorWidget.cpp +++ b/DSFE_App/DSFE_GUI/src/MainWindow/Widgets/DSLEditorWidget.cpp @@ -27,18 +27,13 @@ namespace widgets { DSLEditorWidget::DSLEditorWidget(gui::SimManager* sim, QWidget* parent) - : QWidget(parent), _sim(sim), _parser(nullptr), _program(nullptr), _wrapper(nullptr) + : QWidget(parent), _sim(sim), _parser(nullptr), _program(nullptr), _wrapper(nullptr), _scriptWorkingDir((paths::assets() / "DSLScripts").string()) { auto* rootLayout = new QVBoxLayout(this); auto* buttonLayout = new QHBoxLayout(); _runStopButton = new QPushButton("Run", this); // May also move to the menu, but I want to test the logic first - buttonLayout->addStretch(); - buttonLayout->addWidget(_runStopButton); - - rootLayout->addLayout(buttonLayout); - _tabs = new QTabWidget(this); rootLayout->addWidget(_tabs); @@ -46,7 +41,11 @@ namespace widgets { buildHelpTab(); _statusLabel = new QLabel("Idle", this); + rootLayout->addWidget(_statusLabel); + buttonLayout->addStretch(); + buttonLayout->addWidget(_runStopButton); + rootLayout->addLayout(buttonLayout); _stateTimer = new QTimer(this); connect(_stateTimer, &QTimer::timeout, this, [this]() { pollScriptState(); }); @@ -128,6 +127,7 @@ namespace widgets { void DSLEditorWidget::runScript() { if (_sim->isScriptRunning()) { return; } + _sim->setScriptRunning(!_sim->isScriptRunning()); LOG_INFO("DSL script started."); D_INFO("DSL script started."); _sim->setActiveProgram(nullptr); @@ -139,15 +139,25 @@ namespace widgets { _parser = new interpreter::Parser(_program); _wrapper = new interpreter::RunWrapper(_parser, _program); + LOG_INFO("ScriptEditor content size = %d", _scriptEditor->toPlainText().size()); _scriptText = _scriptEditor->toPlainText().toStdString(); + LOG_INFO("Script size = %zu", _scriptText.size()); _sim->setActiveProgram(_program); _sim->setScriptRunning(true); + + LOG_INFO("Program = %p", _program); + LOG_INFO("Parser = %p", _parser); + LOG_INFO("Wrapper = %p", _wrapper); + _sim->setLastScriptText(_scriptText); std::string code = _scriptText; + LOG_INFO("Original script size = %zu", code.size()); if (!code.empty() && code.back() == '\0') { code.pop_back(); } + LOG_INFO("Running script:\n%s", code.c_str()); + _wrapper->runProgram(_scriptText); updateButtonState(true); @@ -180,7 +190,7 @@ namespace widgets { delete _program; _program = nullptr; updateButtonState(false); setStatusMessage(reason); - _statusLabel->setStyleSheet(fault ? "color: rgb(180,40,40);" : "color: rgb(40,180,40);"); + _statusLabel->setStyleSheet(fault ? "color: rgb(180,40,40);" : ""); } void DSLEditorWidget::pollScriptState() { @@ -195,7 +205,7 @@ namespace widgets { void DSLEditorWidget::updateButtonState(bool running) { _runStopButton->setText(running ? "Stop" : "Run"); - _runStopButton->setStyleSheet(running ? "background-color: rgb(180,40,40);" : "background-color: rgb(40,180,40);"); + _runStopButton->setStyleSheet(running ? "background-color: rgb(180,40,40);" : ""); } } // namespace widgets \ No newline at end of file From bf6f6aaadc0bdbf39927f36213e5417f3a06ca1e Mon Sep 17 00:00:00 2001 From: saltyjoss Date: Sun, 7 Jun 2026 11:33:28 +0100 Subject: [PATCH 126/210] fixes: Removed unsured file --- DSFE_App/DSFE_GUI/src/Scene/Manager/SimPostFX.cpp | 10 ---------- 1 file changed, 10 deletions(-) delete mode 100644 DSFE_App/DSFE_GUI/src/Scene/Manager/SimPostFX.cpp diff --git a/DSFE_App/DSFE_GUI/src/Scene/Manager/SimPostFX.cpp b/DSFE_App/DSFE_GUI/src/Scene/Manager/SimPostFX.cpp deleted file mode 100644 index 94109812..00000000 --- a/DSFE_App/DSFE_GUI/src/Scene/Manager/SimPostFX.cpp +++ /dev/null @@ -1,10 +0,0 @@ -// DSFE_GUI SimPostFX.cpp -#include "Scene/SimulationManager.h" -#ifdef __gl_h_ -#undef __gl_h_ -#endif -#include "Manager/SimImplementation.h" - -namespace gui { - -} \ No newline at end of file From 3dbe40263d4a0471462360f9481d36e955a39d46 Mon Sep 17 00:00:00 2001 From: saltyjoss Date: Sun, 7 Jun 2026 11:33:55 +0100 Subject: [PATCH 127/210] feat: Added destructor to `ViewportWidget` --- DSFE_App/DSFE_GUI/include/MainWindow/Widgets/ViewportWidget.h | 1 + DSFE_App/DSFE_GUI/src/MainWindow/Widgets/ViewportWidget.cpp | 4 +++- 2 files changed, 4 insertions(+), 1 deletion(-) diff --git a/DSFE_App/DSFE_GUI/include/MainWindow/Widgets/ViewportWidget.h b/DSFE_App/DSFE_GUI/include/MainWindow/Widgets/ViewportWidget.h index a9e39bcb..9f9d8396 100644 --- a/DSFE_App/DSFE_GUI/include/MainWindow/Widgets/ViewportWidget.h +++ b/DSFE_App/DSFE_GUI/include/MainWindow/Widgets/ViewportWidget.h @@ -16,6 +16,7 @@ namespace widgets { class ViewportWidget : public QOpenGLWidget { public: explicit ViewportWidget(gui::SimManager* sim, QWidget* parent = nullptr); + ~ViewportWidget(); protected: void initializeGL() override; diff --git a/DSFE_App/DSFE_GUI/src/MainWindow/Widgets/ViewportWidget.cpp b/DSFE_App/DSFE_GUI/src/MainWindow/Widgets/ViewportWidget.cpp index 2b89a681..e592c69a 100644 --- a/DSFE_App/DSFE_GUI/src/MainWindow/Widgets/ViewportWidget.cpp +++ b/DSFE_App/DSFE_GUI/src/MainWindow/Widgets/ViewportWidget.cpp @@ -10,6 +10,7 @@ #include #include +#include #include #include "Platform/KeyCode.h" @@ -23,9 +24,10 @@ namespace widgets { _updateTimer.start(7); // ~144 FPS } + ViewportWidget::~ViewportWidget() { if (_sim) _sim->setContextHooks({}, {}); } + void ViewportWidget::initializeGL() { auto* ctx = QOpenGLContext::currentContext(); - if (_sim) { _sim->setContextHooks([this]() { this->makeCurrent(); }, [this]() { this->doneCurrent(); }); } if (!ctx) { LOG_ERROR("No current OpenGL context"); return; From 45aca0519199b33f06d9e755cefefabc33f3f3b2 Mon Sep 17 00:00:00 2001 From: saltyjoss Date: Sun, 7 Jun 2026 11:35:09 +0100 Subject: [PATCH 128/210] chore: Added code comments and small corrections --- DSFE_App/DSFE_GUI/include/Platform/ScopeGLContext.h | 11 ++++++++--- .../include/Scene/Manager/SimImplementation.h | 2 +- DSFE_App/DSFE_GUI/src/Robots/RobotRenderer.cpp | 9 ++++----- DSFE_App/DSFE_GUI/src/Scene/Manager/SimRobots.cpp | 2 +- DSFE_App/DSFE_GUI/src/Scene/Manager/SimUI.cpp | 2 ++ 5 files changed, 16 insertions(+), 10 deletions(-) diff --git a/DSFE_App/DSFE_GUI/include/Platform/ScopeGLContext.h b/DSFE_App/DSFE_GUI/include/Platform/ScopeGLContext.h index 756c45d4..78209a5a 100644 --- a/DSFE_App/DSFE_GUI/include/Platform/ScopeGLContext.h +++ b/DSFE_App/DSFE_GUI/include/Platform/ScopeGLContext.h @@ -6,9 +6,14 @@ namespace platform { class ScopeGLContext { public: - ScopeGLContext(std::function make, std::function done) : _done(std::move(done)) { if (make) { make(); } } - ~ScopeGLContext() { if (_done) { _done(); } } + ScopeGLContext(std::function makeHook, std::function doneHook) + : _makeHook(std::move(makeHook)), _doneHook(std::move(doneHook)) + { + if (_makeHook) { _makeHook(); } + } + ~ScopeGLContext() { if (_doneHook) { _doneHook(); } } private: - std::function _done; + std::function _makeHook; + std::function _doneHook; }; } \ No newline at end of file diff --git a/DSFE_App/DSFE_GUI/include/Scene/Manager/SimImplementation.h b/DSFE_App/DSFE_GUI/include/Scene/Manager/SimImplementation.h index b7d7b8b4..e171722c 100644 --- a/DSFE_App/DSFE_GUI/include/Scene/Manager/SimImplementation.h +++ b/DSFE_App/DSFE_GUI/include/Scene/Manager/SimImplementation.h @@ -85,7 +85,7 @@ namespace gui { // World Grid & Shadow Shaders std::unique_ptr _worldGridShader; std::unique_ptr _shadowShader; - shaders::Shader* currentShader; + shaders::Shader* currentShader = nullptr; // Fullscreen Quad VAO GLuint _fullscreenVAO = 0; diff --git a/DSFE_App/DSFE_GUI/src/Robots/RobotRenderer.cpp b/DSFE_App/DSFE_GUI/src/Robots/RobotRenderer.cpp index 3f97d3c2..b2dba692 100644 --- a/DSFE_App/DSFE_GUI/src/Robots/RobotRenderer.cpp +++ b/DSFE_App/DSFE_GUI/src/Robots/RobotRenderer.cpp @@ -87,15 +87,14 @@ void RobotRenderer::bind(const RobotRenderBinding& binding) { void RobotRenderer::applyTransforms(const robots::RobotModel& robot, const std::vector& world) { const bool isAligned = robot.baseFrameIsEngineAligned; const size_t n = robot.links.size(); - - LOG_INFO_ONCE("applyTransforms world size = %zu", world.size()); - + if (world.size() < robot.links.size()) { + LOG_ERROR("Transform mismatch: links=%zu world=%zu", robot.links.size(), world.size()); + return; + } for (size_t i = 0; i < n; ++i) { const auto& link = robot.links[i]; - auto it = linkRenderMap.find(link.name); if (it == linkRenderMap.end()) { continue; } - glm::mat4 T = toGlm(world[i]); glm::vec3 pos = glm::vec3(T[3]); // Extract translation from the 4x4 matrix glm::quat q = glm::quat_cast(T); // Extract rotation as a quaternion diff --git a/DSFE_App/DSFE_GUI/src/Scene/Manager/SimRobots.cpp b/DSFE_App/DSFE_GUI/src/Scene/Manager/SimRobots.cpp index 24013153..4a0555aa 100644 --- a/DSFE_App/DSFE_GUI/src/Scene/Manager/SimRobots.cpp +++ b/DSFE_App/DSFE_GUI/src/Scene/Manager/SimRobots.cpp @@ -25,7 +25,7 @@ namespace gui { size_t startIdx = _impl->_objects.size(); - platform::ScopeGLContext guard(_makeCurrentHook, _doneCurrentHook); + platform::ScopeGLContext guard(_makeCurrentHook, _doneCurrentHook); // Hooks may be empty under Qt: In that case the guard becomes a no-op. _impl->buildRobotPresentationFromModel(_impl->_robotSystem->model(), *this); _impl->_robotRenderer->applyTransforms( diff --git a/DSFE_App/DSFE_GUI/src/Scene/Manager/SimUI.cpp b/DSFE_App/DSFE_GUI/src/Scene/Manager/SimUI.cpp index addd2a6b..36c20079 100644 --- a/DSFE_App/DSFE_GUI/src/Scene/Manager/SimUI.cpp +++ b/DSFE_App/DSFE_GUI/src/Scene/Manager/SimUI.cpp @@ -7,6 +7,8 @@ #include +// TODO: Convert logic to Qt6 and only ever use ImGui for debugging at most (or not at all) + namespace gui { // Main Dockspace with Menu Bar void SimManager::drawMainDockspace() { From 1d3ecd195c44002d4096337ce8aee45d12421082 Mon Sep 17 00:00:00 2001 From: saltyjoss Date: Sun, 7 Jun 2026 11:37:27 +0100 Subject: [PATCH 129/210] refactor: Updated `tick()` to contain the dirty robot presentation checks rather than previous placement in `renderViewport` * Also removed old `render()` method as it is no decrepid --- .../include/Scene/SimulationManager.h | 9 +-- .../DSFE_GUI/src/Scene/SimulationManager.cpp | 60 +++++-------------- 2 files changed, 16 insertions(+), 53 deletions(-) diff --git a/DSFE_App/DSFE_GUI/include/Scene/SimulationManager.h b/DSFE_App/DSFE_GUI/include/Scene/SimulationManager.h index eae0291d..fd1b5c93 100644 --- a/DSFE_App/DSFE_GUI/include/Scene/SimulationManager.h +++ b/DSFE_App/DSFE_GUI/include/Scene/SimulationManager.h @@ -79,9 +79,6 @@ namespace gui { _doneCurrentHook = doneCurrentHook; } - void renderViewport(int w, int h); - void setDisplaySize(int w, int h); - // Light scene::Light* getLight(); void setLightColour(const glm::vec3& c); @@ -166,15 +163,13 @@ namespace gui { // Render Settings & Profiles void applyRenderSettings(const render::RenderSettings& s, render::ResolutionPreset r); void applyRenderProfile(const render::RenderSettings& s, render::ResolutionPreset r); - - // Reset HDR to default void resetHDRToPreset(); - // Reload shaders (e.g. after editing source files) void reloadAllShaders(); // Rendering Entry Points - void render(); void tick(double dt); + void renderViewport(int w, int h); + void setDisplaySize(int w, int h); void resize(int32_t width, int32_t height); void setPresentationFBO(GLuint fbo) { _presentationFBO = fbo; } diff --git a/DSFE_App/DSFE_GUI/src/Scene/SimulationManager.cpp b/DSFE_App/DSFE_GUI/src/Scene/SimulationManager.cpp index 5b0df1ad..d11b1720 100644 --- a/DSFE_App/DSFE_GUI/src/Scene/SimulationManager.cpp +++ b/DSFE_App/DSFE_GUI/src/Scene/SimulationManager.cpp @@ -3,7 +3,6 @@ #include "Scene/SimulationManager.h" #include "Scene/SimulationCore.h" -#include #include extern "C" core::ISimulationCore* CreateSimulationCore_v1(); @@ -76,7 +75,7 @@ namespace gui { // CONSTRUCTOR & DESTRUCTOR // -------------------------------------------------- - SimManager::SimManager() : _internalSize(1920, 1080), _displaySize(1.0f, 1.0f), _backgroundColour(0.18f, 0.18f, 0.20f), + SimManager::SimManager() : _internalSize(1280, 720), _displaySize(1.0f, 1.0f), _backgroundColour(0.18f, 0.18f, 0.20f), _backgroundAlpha(1.0f), _impl(std::make_unique(*this)), _core(std::make_unique()), _studyRunner(std::make_unique(makeCoreFactory, std::thread::hardware_concurrency() > 1 ? std::thread::hardware_concurrency() - 1 : 1)) { _core->setRobotSystem(_impl->_robotSystem.get()); @@ -153,68 +152,37 @@ namespace gui { return copy; } - // Main render function called by the application - void SimManager::render() { - if (hasCompletedStudy()) { - auto results = consumeCompletedStudy(); - - for (const auto& r : results) { - LOG_INFO("Study completed: %s", r.tag.c_str()); - // TODO: update plots, telemetry graphs, UI panels here - } - } - - ImGuiIO& io = ImGui::GetIO(); - - tick(io.DeltaTime); - - if (_impl->_robotSystem && hasRobot()) { - _impl->_robotRenderer->applyTransforms( - _impl->_robotSystem->model(), - _impl->_robotSystem->worldTransforms() - ); - } - + void SimManager::tick(double dt) { + _core->tick(dt); if (_core->robotPresentationDirty()) { - loadRobot(_core->robotSystem()->robotName()); + loadRobot(_impl->_robotSystem->robotName()); _core->clearRobotPresentationDirty(); } - - _fpsCounter.update(); - - drawMainDockspace(); - drawViewportWindow(); - } - - void SimManager::tick(double dt) { - if (!hasRobot()) { return; } - _core->tick(dt); } void SimManager::renderViewport(int w, int h) { - LOG_INFO_ONCE("renderViewport entered"); if (!_glReady || !_impl) { return; } if (w <= 0 || h <= 0) { return; } - if (_impl->_robotSystem && hasRobot()) { - _impl->_robotRenderer->applyTransforms( - _impl->_robotSystem->model(), - _impl->_robotSystem->worldTransforms() - ); + if (hasCompletedStudy()) { + auto results = consumeCompletedStudy(); + for (const auto& r : results) { + LOG_INFO("Study completed: %s", r.tag.c_str()); + // TODO: update plots, telemetry graphs, UI panels here + } } - if (_core->robotPresentationDirty()) { - loadRobot(_core->robotSystem()->robotName()); - _core->clearRobotPresentationDirty(); + if (_impl->_robotSystem && hasRobot()) { + _impl->_robotRenderer->applyTransforms(_impl->_robotSystem->model(), _impl->_robotSystem->worldTransforms()); } _fpsCounter.update(); auto& view = _impl->_views[static_cast(_impl->activeView)]; _impl->renderView(*this, view, w, h); - glBindFramebuffer(GL_FRAMEBUFFER, _presentationFBO); glViewport(0, 0, w, h); glDisable(GL_DEPTH_TEST); glDisable(GL_BLEND); _impl->_presentShader->use(); _impl->_presentShader->setInt1(0, "screenTexture"); + glActiveTexture(GL_TEXTURE0); glBindTexture(GL_TEXTURE_2D, view.post->getTexture()); glBindVertexArray(_impl->_fullscreenVAO); @@ -224,7 +192,7 @@ namespace gui { } void SimManager::syncRobotToScene() { - if (!hasRobot()) return; + if (!hasRobot()) { return; } auto* rs = robotSystem(); const auto& model = rs->model(); const auto& T = rs->worldTransforms(); From 7f5d2858b472ccef7d52280ceb3a57f483d86646 Mon Sep 17 00:00:00 2001 From: saltyjoss Date: Sun, 7 Jun 2026 11:39:06 +0100 Subject: [PATCH 130/210] feat: Added more alignments with previous DSL script editor in `DSLEditorWidget` * Still need to add a few more things in this, but they are purely visual and do not really improve functionality * One of these planned editions is syntax highlighting! * Need to move onto a more refined CLI or debug window and plot logging (not sure the best way to do that right now) --- .../MainWindow/Widgets/DSLEditorWidget.h | 34 ++++++-- .../MainWindow/Widgets/DSLEditorWidget.cpp | 86 ++++++++++++------- 2 files changed, 80 insertions(+), 40 deletions(-) diff --git a/DSFE_App/DSFE_GUI/include/MainWindow/Widgets/DSLEditorWidget.h b/DSFE_App/DSFE_GUI/include/MainWindow/Widgets/DSLEditorWidget.h index 794fb0b9..1ef4fd14 100644 --- a/DSFE_App/DSFE_GUI/include/MainWindow/Widgets/DSLEditorWidget.h +++ b/DSFE_App/DSFE_GUI/include/MainWindow/Widgets/DSLEditorWidget.h @@ -7,6 +7,11 @@ #include #include #include +#include "Platform/StudyRunner.h" +#include +#include +#include +#include "Interpreter/RunWrapper.h" class QTextEdit; class QLabel; @@ -18,7 +23,13 @@ namespace gui { class SimManager; } namespace interpreter { class Parser; class IStoredProgram; - class RunWrapper; +} + +namespace runs { + struct ActiveRun { + std::future fut; + std::string tag; + }; } namespace widgets { @@ -28,8 +39,17 @@ namespace widgets { bool loadScript(const QString& fileName); bool saveScript(const QString& fileName); + void runButtonHandler(); private: + std::mutex _activeRunsMutex; // Mutex for synchronizing access to active runs + std::vector _activeRuns; // Vector to hold active runs and their futures + + gui::SimManager* _sim = nullptr; + interpreter::Parser* _parser = nullptr; + interpreter::IStoredProgram* _program = nullptr; + interpreter::RunWrapper* _wrapper = nullptr; + void runScript(); void stopScript(); @@ -44,14 +64,12 @@ namespace widgets { void setScript(const std::string& s) { _scriptText = s; } std::string getScript() const { return _scriptText; } - std::string _scriptText; - - gui::SimManager* _sim = nullptr; - interpreter::Parser* _parser; - interpreter::IStoredProgram* _program; - interpreter::RunWrapper* _wrapper; + bool _loadedFromFile = false; + std::string _scriptText; std::string _scriptWorkingDir; + std::vector _script; + int lastClickedLine = -1; QTabWidget* _tabs = nullptr; QWidget* _editorTab = nullptr; @@ -60,6 +78,8 @@ namespace widgets { QTextEdit* _dslHelp = nullptr; QPushButton* _runStopButton = nullptr; QLabel* _statusLabel = nullptr; + QLabel* _scriptLinesLabel = nullptr; + QLabel* _scriptCharsLabel = nullptr; QString _currentScriptPath; QTimer* _stateTimer = nullptr; }; diff --git a/DSFE_App/DSFE_GUI/src/MainWindow/Widgets/DSLEditorWidget.cpp b/DSFE_App/DSFE_GUI/src/MainWindow/Widgets/DSLEditorWidget.cpp index 92665434..453a1342 100644 --- a/DSFE_App/DSFE_GUI/src/MainWindow/Widgets/DSLEditorWidget.cpp +++ b/DSFE_App/DSFE_GUI/src/MainWindow/Widgets/DSLEditorWidget.cpp @@ -10,13 +10,6 @@ #include #include -#include "Platform/StudyRunner.h" -#include "Interpreter/RunWrapper.h" - -#include -#include -#include - #include "Scene/SimulationManager.h" #include "Platform/Paths.h" @@ -27,12 +20,10 @@ namespace widgets { DSLEditorWidget::DSLEditorWidget(gui::SimManager* sim, QWidget* parent) - : QWidget(parent), _sim(sim), _parser(nullptr), _program(nullptr), _wrapper(nullptr), _scriptWorkingDir((paths::assets() / "DSLScripts").string()) + : QWidget(parent), _sim(sim), _scriptWorkingDir((paths::assets() / "DSLScripts").string()) { auto* rootLayout = new QVBoxLayout(this); - auto* buttonLayout = new QHBoxLayout(); - - _runStopButton = new QPushButton("Run", this); // May also move to the menu, but I want to test the logic first + auto* scriptStateLayout = new QHBoxLayout(); _tabs = new QTabWidget(this); rootLayout->addWidget(_tabs); @@ -40,58 +31,81 @@ namespace widgets { buildEditorTab(); buildHelpTab(); - _statusLabel = new QLabel("Idle", this); - - rootLayout->addWidget(_statusLabel); - buttonLayout->addStretch(); - buttonLayout->addWidget(_runStopButton); - rootLayout->addLayout(buttonLayout); + _statusLabel = new QLabel("State: Idle", this); + auto* pipe1 = new QLabel("|", this); + auto* pipe2 = new QLabel("|", this); + _scriptLinesLabel = new QLabel("Lines: 0", this); + _scriptCharsLabel = new QLabel("Chars: 0", this); + _runStopButton = new QPushButton("Run", this); + + scriptStateLayout->addWidget(_statusLabel); + scriptStateLayout->addWidget(pipe1); + scriptStateLayout->addWidget(_scriptLinesLabel); + scriptStateLayout->addWidget(pipe2); + scriptStateLayout->addWidget(_scriptCharsLabel); + scriptStateLayout->addStretch(); + scriptStateLayout->addWidget(_runStopButton); + + rootLayout->addLayout(scriptStateLayout); + + scriptStateLayout->setContentsMargins(6, 6, 6, 6); + scriptStateLayout->setSpacing(8); + rootLayout->setContentsMargins(6, 6, 6, 6); + rootLayout->setSpacing(6); _stateTimer = new QTimer(this); connect(_stateTimer, &QTimer::timeout, this, [this]() { pollScriptState(); }); _stateTimer->start(100); // Poll every 100 ms - connect(_runStopButton, &QPushButton::clicked, this, [this]() { - if (_sim->isScriptRunning()) { stopScript(); } - else { runScript(); } - }); + connect(_runStopButton, &QPushButton::clicked, this, [this]() { _sim->isScriptRunning() ? stopScript() : runScript(); }); pollScriptState(); } + static std::string filenameFromPath(const std::string& path) { + size_t lastSlash = path.find_last_of("/\\"); + if (lastSlash == std::string::npos) { return path; } + return path.substr(lastSlash + 1); + } + bool DSLEditorWidget::loadScript(const QString& fileName) { QFile file(fileName); + std::string nameStr = filenameFromPath(fileName.toStdString()).c_str(); if (!file.open(QIODevice::ReadOnly | QIODevice::Text)) { - LOG_ERROR("Failed to open script file: %s", fileName.toStdString().c_str()); + LOG_ERROR("Failed to open script file: %s", nameStr); return false; } _scriptEditor->setPlainText(file.readAll()); file.close(); _currentScriptPath = fileName; - LOG_INFO("DSL script loaded from file: %s", fileName.toStdString().c_str()); - D_INFO("DSL script loaded from file: %s", fileName.toStdString().c_str()); + _scriptLinesLabel->setText(QString("Lines: %1").arg(_scriptEditor->toPlainText().split('\n').size())); + _scriptCharsLabel->setText(QString("Chars: %1").arg(_scriptEditor->toPlainText().size())); + + LOG_INFO("DSL script loaded from file: %s", nameStr); + D_INFO("DSL script loaded from file: %s", nameStr); return true; } bool DSLEditorWidget::saveScript(const QString& fileName) { QFile file(fileName); + std::string nameStr = filenameFromPath(fileName.toStdString()).c_str(); if (!file.open(QIODevice::WriteOnly | QIODevice::Text)) { - LOG_ERROR("Failed to open script file for writing: %s", fileName.toStdString().c_str()); + LOG_ERROR("Failed to open script file for writing: %s", nameStr); return false; } file.write(_scriptEditor->toPlainText().toUtf8()); file.close(); _currentScriptPath = fileName; - LOG_INFO("DSL script saved to file: %s", fileName.toStdString().c_str()); - D_INFO("DSL script saved to file: %s", fileName.toStdString().c_str()); + LOG_INFO("DSL script saved to file: %s", nameStr); + D_INFO("DSL script saved to file: %s", nameStr); return true; } - void DSLEditorWidget::setStatusMessage(const QString& message) { if (_statusLabel) { _statusLabel->setText(message); } } + void DSLEditorWidget::setStatusMessage(const QString& message) { if (_statusLabel) { _statusLabel->setText("State: " + message); } } void DSLEditorWidget::buildEditorTab() { _editorTab = new QWidget(this); @@ -106,8 +120,9 @@ namespace widgets { auto* layout = new QVBoxLayout(_helpTab); _dslHelp = new QTextEdit(_helpTab); _dslHelp->setReadOnly(true); - - + + // TODO: Use a markdown viewer or similar to format and display this help content in an accessible way, with sections, examples, etc. + // * (also means I can make a website with the same content) _dslHelp->setPlainText( "trajSet(...)\n" "trajClear()\n" @@ -120,31 +135,34 @@ namespace widgets { "wait(...)\n" ); - layout->addWidget(_dslHelp); _tabs->addTab(_helpTab, "DSL Help"); } void DSLEditorWidget::runScript() { if (_sim->isScriptRunning()) { return; } - _sim->setScriptRunning(!_sim->isScriptRunning()); - LOG_INFO("DSL script started."); D_INFO("DSL script started."); + if (_scriptEditor->toPlainText().isEmpty()) { setStatusMessage("Empty script"); return; } _sim->setActiveProgram(nullptr); delete _wrapper; _wrapper = nullptr; delete _parser; _parser = nullptr; delete _program; _program = nullptr; + LOG_INFO("Core = %p", _sim->simCoreInterface()); + _program = new interpreter::StoredProgram(_sim->simCoreInterface()); _parser = new interpreter::Parser(_program); _wrapper = new interpreter::RunWrapper(_parser, _program); + LOG_INFO("Program = %p", _program); + LOG_INFO("ScriptEditor content size = %d", _scriptEditor->toPlainText().size()); _scriptText = _scriptEditor->toPlainText().toStdString(); LOG_INFO("Script size = %zu", _scriptText.size()); _sim->setActiveProgram(_program); _sim->setScriptRunning(true); + LOG_INFO("DSL script started."); D_INFO("DSL script started."); LOG_INFO("Program = %p", _program); LOG_INFO("Parser = %p", _parser); @@ -163,6 +181,8 @@ namespace widgets { updateButtonState(true); } + void DSLEditorWidget::runButtonHandler() { _sim->isScriptRunning() ? stopScript() : runScript(); } + void DSLEditorWidget::stopScript() { if (!_sim->isScriptRunning()) { return; } if (_sim->activeProgram()) { From add31d0ddc226c22eb6559d1b214d40d1ad78fa1 Mon Sep 17 00:00:00 2001 From: saltyjoss Date: Sun, 7 Jun 2026 11:39:57 +0100 Subject: [PATCH 131/210] refactor: Updated `buildMenuBar()` to call DSLEditorWidget methods for loading and saving files. --- DSFE_App/DSFE_GUI/CMakeLists.txt | 1 - DSFE_App/DSFE_GUI/src/MainWindow/DSFE_MainWindow.cpp | 7 +++++-- 2 files changed, 5 insertions(+), 3 deletions(-) diff --git a/DSFE_App/DSFE_GUI/CMakeLists.txt b/DSFE_App/DSFE_GUI/CMakeLists.txt index 3f8d1e6c..5eb1cd28 100644 --- a/DSFE_App/DSFE_GUI/CMakeLists.txt +++ b/DSFE_App/DSFE_GUI/CMakeLists.txt @@ -73,7 +73,6 @@ set(MANAGER_SRC src/Scene/Manager/SimBackend.cpp src/Scene/Manager/SimCamera.cpp src/Scene/Manager/SimObjects.cpp - src/Scene/Manager/SimPostFX.cpp src/Scene/Manager/SimRendering.cpp src/Scene/Manager/SimRobots.cpp src/Scene/Manager/SimUI.cpp diff --git a/DSFE_App/DSFE_GUI/src/MainWindow/DSFE_MainWindow.cpp b/DSFE_App/DSFE_GUI/src/MainWindow/DSFE_MainWindow.cpp index 07ffa16a..2c978d9c 100644 --- a/DSFE_App/DSFE_GUI/src/MainWindow/DSFE_MainWindow.cpp +++ b/DSFE_App/DSFE_GUI/src/MainWindow/DSFE_MainWindow.cpp @@ -18,7 +18,7 @@ namespace window { : QMainWindow(parent), _sim(sim), _dslEditor(nullptr) { setWindowTitle("DSFE"); - resize(1280, 720); + resize(1920, 1080); buildMenuBar(); @@ -61,6 +61,7 @@ namespace window { connect(openScriptAction, &QAction::triggered, this, [this]() { QString fileName = QFileDialog::getOpenFileName(nullptr, "Open Script", QString::fromStdString((paths::assets() / "DSLScripts").string()), "DSL Script Files (*.dsl);;Text Files (*.txt)"); if (fileName.isEmpty()) { return; } + if (!_dslEditor) { LOG_ERROR("DSL Editor not found!"); return; } _dslEditor->loadScript(fileName); }); }); @@ -77,6 +78,7 @@ namespace window { LOG_INFO("Menu clicked: File -> Save -> Script"); QString fileName = QFileDialog::getSaveFileName(nullptr, "Save Script", QString::fromStdString((paths::assets() / "DSLScripts").string()), "DSL Script Files (*.dsl);;Text Files (*.txt)"); // ARGS are if (fileName.isEmpty()) { return; } + if (!_dslEditor) { LOG_ERROR("DSL Editor not found!"); return; } _dslEditor->saveScript(fileName); }); }); @@ -92,6 +94,7 @@ namespace window { LOG_INFO("Menu clicked: File -> Save As -> Script"); QString fileName = QFileDialog::getSaveFileName(nullptr, "Save Script As", QString::fromStdString((paths::assets() / "DSLScripts").string()), "DSL Script Files (*.dsl);;Text Files (*.txt)"); if (fileName.isEmpty()) { return; } + if (!_dslEditor) { LOG_ERROR("DSL Editor not found!"); return; } _dslEditor->saveScript(fileName); }); }); @@ -127,7 +130,7 @@ namespace window { LOG_INFO("Menu clicked: Project -> Load Mesh"); QString path = QFileDialog::getOpenFileName(nullptr, "Select Mesh File", QString::fromStdString((paths::assets() / "objects" / "Shapes").string()), "Mesh Files (*.obj *.fbx *.gltf *.dae *.stl"); if (path.isEmpty()) { return; } - _sim->loadMesh(path.toStdString()); // Crashes at the moment, TODO fix crash + _sim->loadMesh(path.toStdString()); }); auto* loadHDRAction = projectMenu->addAction("Load HDRI"); connect(loadHDRAction, &QAction::triggered, this, [this]() { From 7d4429193d760064f82a606048b56ad16b970661 Mon Sep 17 00:00:00 2001 From: saltyjoss Date: Sun, 7 Jun 2026 14:38:50 +0100 Subject: [PATCH 132/210] feat: Added basic joint telemetry in `ControlPanelWidget` --- DSFE_App/DSFE_GUI/CMakeLists.txt | 20 +- .../include/MainWindow/DSFE_MainWindow.h | 2 + .../MainWindow/Widgets/ControlPanelWidget.h | 47 +- .../DSFE_GUI/include/Platform/WindowManager.h | 81 - .../DSFE_GUI/include/Platform/imguiWidgets.h | 298 --- .../DSFE_GUI/include/Rendering/GUIContext.h | 26 - .../include/Rendering/OpenGLContext.h | 21 - .../DSFE_GUI/include/ui/CommandScriptEditor.h | 104 - DSFE_App/DSFE_GUI/include/ui/ControlPanel.h | 232 -- DSFE_App/DSFE_GUI/include/ui/DebugPanel.h | 58 - DSFE_App/DSFE_GUI/include/ui/Styles.h | 15 - .../MainWindow/Widgets/ControlPanelWidget.cpp | 240 ++- .../MainWindow/Widgets/DSLEditorWidget.cpp | 2 +- .../DSFE_GUI/src/Platform/WindowManager.cpp | 200 -- .../DSFE_GUI/src/Rendering/GUIContext.cpp | 114 - .../DSFE_GUI/src/Rendering/OpenGLContext.cpp | 113 - .../DSFE_GUI/src/Scene/SimulationManager.cpp | 9 +- .../DSFE_GUI/src/ui/CommandScriptEditor.cpp | 703 ------- DSFE_App/DSFE_GUI/src/ui/ControlPanel.cpp | 1861 ----------------- DSFE_App/DSFE_GUI/src/ui/DebugPanel.cpp | 317 --- 20 files changed, 213 insertions(+), 4250 deletions(-) delete mode 100644 DSFE_App/DSFE_GUI/include/Platform/WindowManager.h delete mode 100644 DSFE_App/DSFE_GUI/include/Platform/imguiWidgets.h delete mode 100644 DSFE_App/DSFE_GUI/include/Rendering/GUIContext.h delete mode 100644 DSFE_App/DSFE_GUI/include/Rendering/OpenGLContext.h delete mode 100644 DSFE_App/DSFE_GUI/include/ui/CommandScriptEditor.h delete mode 100644 DSFE_App/DSFE_GUI/include/ui/ControlPanel.h delete mode 100644 DSFE_App/DSFE_GUI/include/ui/DebugPanel.h delete mode 100644 DSFE_App/DSFE_GUI/include/ui/Styles.h delete mode 100644 DSFE_App/DSFE_GUI/src/Platform/WindowManager.cpp delete mode 100644 DSFE_App/DSFE_GUI/src/Rendering/GUIContext.cpp delete mode 100644 DSFE_App/DSFE_GUI/src/Rendering/OpenGLContext.cpp delete mode 100644 DSFE_App/DSFE_GUI/src/ui/CommandScriptEditor.cpp delete mode 100644 DSFE_App/DSFE_GUI/src/ui/ControlPanel.cpp delete mode 100644 DSFE_App/DSFE_GUI/src/ui/DebugPanel.cpp diff --git a/DSFE_App/DSFE_GUI/CMakeLists.txt b/DSFE_App/DSFE_GUI/CMakeLists.txt index 5eb1cd28..ba958df6 100644 --- a/DSFE_App/DSFE_GUI/CMakeLists.txt +++ b/DSFE_App/DSFE_GUI/CMakeLists.txt @@ -40,21 +40,12 @@ target_include_directories(stb INTERFACE set(MAIN_WINDOW_SRC src/MainWindow/DSFE_MainWindow.cpp src/MainWindow/Workspace/ProjectPage.cpp - src/MainWindow/Widgets/ViewportWidget.cpp - src/MainWindow/Widgets/RobotSelectorWidget.cpp - src/MainWindow/Widgets/ControlPanelWidget.cpp - src/MainWindow/Widgets/DSLEditorWidget.cpp - - src/MainWindow/Widgets/FractionSelectorWidget.cpp - include/MainWindow/Widgets/FractionSelectorWidget.h ) set(RENDER_SRC src/Rendering/CubeVertices.cpp - src/Rendering/GUIContext.cpp src/Rendering/IBL.cpp src/Rendering/OpenGLBufferManager.cpp - src/Rendering/OpenGLContext.cpp src/Rendering/ShaderUtil.cpp src/Rendering/SkyboxRenderer.cpp src/Rendering/Texture.cpp @@ -80,11 +71,13 @@ set(MANAGER_SRC ) set(UI_SRC - src/ui/CommandScriptEditor.cpp - src/ui/ControlPanel.cpp - src/ui/DebugPanel.cpp src/ui/RenderPreset.cpp - src/ui/Styles.cpp + src/MainWindow/Widgets/ViewportWidget.cpp + src/MainWindow/Widgets/RobotSelectorWidget.cpp + src/MainWindow/Widgets/ControlPanelWidget.cpp + src/MainWindow/Widgets/DSLEditorWidget.cpp + src/MainWindow/Widgets/FractionSelectorWidget.cpp + include/MainWindow/Widgets/FractionSelectorWidget.h ) set(ROBOT_SRC @@ -93,7 +86,6 @@ set(ROBOT_SRC ) set(PLATFORM_SRC - src/Platform/WindowManager.cpp src/Application.cpp ) diff --git a/DSFE_App/DSFE_GUI/include/MainWindow/DSFE_MainWindow.h b/DSFE_App/DSFE_GUI/include/MainWindow/DSFE_MainWindow.h index 9f381d08..771cad2e 100644 --- a/DSFE_App/DSFE_GUI/include/MainWindow/DSFE_MainWindow.h +++ b/DSFE_App/DSFE_GUI/include/MainWindow/DSFE_MainWindow.h @@ -1,6 +1,8 @@ // DSFE_GUI DSFE_MainWindow.h #pragma once +#include "GUIExports.h" + #include #include diff --git a/DSFE_App/DSFE_GUI/include/MainWindow/Widgets/ControlPanelWidget.h b/DSFE_App/DSFE_GUI/include/MainWindow/Widgets/ControlPanelWidget.h index b0892c54..15b71b1c 100644 --- a/DSFE_App/DSFE_GUI/include/MainWindow/Widgets/ControlPanelWidget.h +++ b/DSFE_App/DSFE_GUI/include/MainWindow/Widgets/ControlPanelWidget.h @@ -25,8 +25,7 @@ namespace render { } namespace robots { class RobotSystem; } -namespace diagnostics { class TelemetryRecorder; } - +namespace diagnostics { class TelemetryRecorder; struct JointTelemetry; } namespace gui { class SimManager; } class QVBoxLayout; @@ -53,16 +52,53 @@ namespace widgets { const char* name; }; + struct TelemetryLabels { + // Headers + QLabel* stateHeader = nullptr; + QLabel* referenceHeader = nullptr; + QLabel* trajectoryHeader = nullptr; + QLabel* clampedHeader = nullptr; + QLabel* constantsHeader = nullptr; + + // State + QLabel* q = nullptr; + QLabel* qd = nullptr; + QLabel* tau = nullptr; + + // Reference + QLabel* qRef = nullptr; + QLabel* qdRef = nullptr; + QLabel* qddRef = nullptr; + QLabel* err = nullptr; + + // Trajectory + QLabel* qTraj = nullptr; + QLabel* qdTraj = nullptr; + QLabel* qddTraj = nullptr; + + // Clamping + QLabel* qClamped = nullptr; + QLabel* qdClamped = nullptr; + + // Constants + QLabel* damping = nullptr; + QLabel* friction = nullptr; + }; + void simPropertiesPanel(); void buildIntegratorCombos(); - void buildDtFractions(); void jointInfoPanel(); - void displayJointInfo(const diagnostics::TelemetryRecorder& rec, int& selectedJoint, QVBoxLayout* layout); + void updateTelemetryInfo(const diagnostics::JointTelemetry& j); + void buildTelemetryWidgets(QVBoxLayout* layout); + + void updateTelemetryDisplay(); void displayPanel(); void selectJointAndFollow(int jointIdx); + void updateSimClock(); + gui::SimManager* _sim = nullptr; QVBoxLayout* _contentLayout = nullptr; @@ -70,6 +106,7 @@ namespace widgets { QCheckBox* _useAutoDiffCheck = nullptr; QComboBox* _integratorCombo = nullptr; QLabel* _currentIntegratorLabel = nullptr; + QLabel* _simTimeLabel = nullptr; FractionSelectorWidget* _simDtSelector = nullptr; FractionSelectorWidget* _telemetryDtSelector = nullptr; @@ -98,6 +135,8 @@ namespace widgets { { integration::eAutoDiffIntegrationMethod::AD_GLRK3, "GLRK3 (AutoDiff)" } }; + TelemetryLabels _telemetryLabels; + // Current selection state Selection _selection; diff --git a/DSFE_App/DSFE_GUI/include/Platform/WindowManager.h b/DSFE_App/DSFE_GUI/include/Platform/WindowManager.h deleted file mode 100644 index a5a42f63..00000000 --- a/DSFE_App/DSFE_GUI/include/Platform/WindowManager.h +++ /dev/null @@ -1,81 +0,0 @@ -// DSFE_GUI WindowManager.h -#pragma once - -#include "GUIExports.h" - -#include "Platform/Window.h" -#include "Platform/Logger.h" - -// Forward Declarations -struct GLFWwindow; -namespace render { - class GUIContext; - class OpenGLContext; -} -namespace gui { - class SimManager; - class ControlPanel; - class DebugPanel; - class CommandScriptEditor; -} - -namespace window { - class GLWindow : public IWindow { - public: - GLWindow(); - ~GLWindow(); - - bool init(int width, int height, const std::string& title) override; - - // IWindow interface - bool isRunning() const override; - bool shouldClose() const override; - void pollEvents() override; - void swapBuffers() override; - - void* getNativeWin() override; - void setNativeWin(void* window) override; - - int getWidth() const override; - int getHeight() const override; - const std::string& getHeader() const override; - - // Input handling - void setMouseCaptured(bool captured); - bool isMouseCaptured() const { return _mouseCaptured; } - void onKey(int key, int scancode, int action, int mods) override; - void onScroll(double delta) override; - void onResize(int width, int height) override; - void onCursorPos(double xpos, double ypos) override; - void onClose() override; - - void update(); - void render(); - - private: - // UI State - struct UIState { - bool sceneViewOpen = true; - bool controlPanelOpen = true; - bool debugPanelOpen = true; - }; - - bool _isRunning = true; - GLFWwindow* _window = nullptr; - - std::unique_ptr _GUICntx; - std::unique_ptr _renderCntx; - std::unique_ptr _sim; - std::unique_ptr _controlPanel; - std::unique_ptr _debugPanel; - std::unique_ptr _cmdEditor; - - bool _isHovered = false; - bool _mouseCaptured = true; - - // Window properties - int _width = 0; - int _height = 0; - std::string *_header; - }; -} // namespace window \ No newline at end of file diff --git a/DSFE_App/DSFE_GUI/include/Platform/imguiWidgets.h b/DSFE_App/DSFE_GUI/include/Platform/imguiWidgets.h deleted file mode 100644 index ccac8db6..00000000 --- a/DSFE_App/DSFE_GUI/include/Platform/imguiWidgets.h +++ /dev/null @@ -1,298 +0,0 @@ -// DSFE_GUI imguiWidgets.h -#pragma once - -#include -#include -#include "Platform/Logger.h" - -namespace ImGui { - // Draggable horizontal splitter for resizable panels - inline float hSplitter(const char* id, float* h, float minH = 200.0f, float maxH = 0.0f, float thickness = 2.0f) { - if (maxH <= 0.0f) { maxH = GetContentRegionAvail().y; } - - // Get the current cursor position in screen coordinates - ImVec2 cursor = GetCursorScreenPos(); - float w = GetContentRegionAvail().x; - - // Push a unique ID for this splitter to avoid conflicts with other widgets - PushID(id); - - // Position the invisible button for the splitter - if (w <= 0.0f) { - ImGui::Dummy(ImVec2(0.0f, thickness)); - PopID(); - return *h; - } - - InvisibleButton("##hsplit", ImVec2(w, thickness)); - - // Handle dragging - bool dragging = IsItemActive(); - if (dragging) { - *h += GetIO().MouseDelta.y; - } - - *h = ImClamp(*h, minH, maxH); - - // Draw the draggable splitter - if (IsItemHovered() || dragging) { - SetMouseCursor(ImGuiMouseCursor_ResizeNS); - ImVec2 p = GetItemRectMin(); - ImVec2 q = GetItemRectMax(); - GetWindowDrawList()->AddRectFilled( - ImVec2(p.x, q.y), ImVec2(p.x, p.y + thickness), - dragging ? IM_COL32(100, 150, 255, 200) // brighter color when dragging - : IM_COL32(150, 150, 150, 120) // default color - ); - } - - // Restore cursor position and ID stack - if (w > 0.0f) { ImGui::Dummy(ImVec2(0, thickness)); } - PopID(); - return *h; - } - - // Draggable vertical splitter for resizable panels - inline float vSplitter(const char* id, float* w, float minW = 200.0f, float maxW = 0.0f, float thickness = 2.0f) { - if (maxW <= 0.0f) { maxW = GetContentRegionAvail().x; } - - // Get the current cursor position in screen coordinates - ImVec2 cursor = GetCursorScreenPos(); - float h = GetContentRegionAvail().y; - - // Push a unique ID for this splitter to avoid conflicts with other widgets - PushID(id); - SetCursorScreenPos(ImVec2(cursor.x + *w, cursor.y)); - InvisibleButton("##vsplit", ImVec2(thickness, h)); - - // Handle dragging - bool dragging = IsItemActive(); - if (dragging) { - *w += GetIO().MouseDelta.x; - *w = ImClamp(*w, minW, maxW); - } - - // Draw the draggable splitter - if (IsItemHovered() || dragging) { - SetMouseCursor(ImGuiMouseCursor_ResizeEW); - ImVec2 p = GetItemRectMin(); - ImVec2 q = GetItemRectMax(); - GetWindowDrawList()->AddRectFilled( - ImVec2(p.x, p.y), ImVec2(p.x + thickness, q.y), - dragging ? IM_COL32(100, 150, 255, 200) // brighter color when dragging - : IM_COL32(150, 150, 150, 120) // default color - ); - } - - // Restore cursor position and ID stack - SetCursorScreenPos(cursor); - PopID(); - return *w; - } - - // Draw a section divider with spacing - inline void SectionDivider() { - ImGui::Spacing(); - ImGui::Separator(); - ImGui::Spacing(); - } - - // Segmented Button Row Helper - inline bool SegmentedButtonRow(const char* label, const char* const* items, int itemCount, int& current, float buttonWidth) { - ImGui::TextUnformatted(label); - - // Track if selection changed - bool changed = false; - ImGui::PushID(label); - - // Draw buttons in a row - for (int i = 0; i < itemCount; ++i) { - if (i > 0) ImGui::SameLine(); - - // Highlight the currently selected button - const bool selected = (current == i); - if (selected) { - ImGui::PushStyleColor(ImGuiCol_Button, ImVec4(0.2f, 0.6f, 0.2f, 1.0f)); - ImGui::PushStyleColor(ImGuiCol_ButtonHovered, ImVec4(0.3f, 0.7f, 0.3f, 1.0f)); - ImGui::PushStyleColor(ImGuiCol_ButtonActive, ImVec4(0.1f, 0.5f, 0.1f, 1.0f)); - } - - // Draw the button and check for clicks - if (ImGui::Button(items[i], ImVec2(buttonWidth, 0))) { - if (current != i) { - current = i; - changed = true; - } - } - - if (selected) { ImGui::PopStyleColor(3); } - } - - ImGui::PopID(); - return changed; - } - - // Reusable section header - inline void SectionHeader(const char* text, const ImVec4& color = ImVec4(1.0f, 1.0f, 1.0f, 1.0f)) { - ImGui::Spacing(); - - // Get draw list and position for custom drawing - ImDrawList* dl = ImGui::GetWindowDrawList(); - ImVec2 p = ImGui::GetCursorScreenPos(); - - // Subtle left accent bar - dl->AddRectFilled( - ImVec2(p.x, p.y), - ImVec2(p.x + 3.0f, p.y + ImGui::GetTextLineHeightWithSpacing()), - ImGui::ColorConvertFloat4ToU32(color) - ); - - // Header text with indentation - ImGui::Indent(12.0f); - ImGui::TextColored(color, "%s", text); - ImGui::Unindent(12.0f); - ImGui::Spacing(); - } - - // Draw a simple fraction (numerator over denominator) with a horizontal line in between - inline void DrawFraction(const char* numerator, const char* denom) { - ImDrawList* draw = ImGui::GetWindowDrawList(); - ImVec2 pos = ImGui::GetCursorScreenPos(); - - // Calculate text sizes - ImVec2 numSize = ImGui::CalcTextSize(numerator); - ImVec2 denSize = ImGui::CalcTextSize(denom); - - // Calculate positions - float lineY = pos.y + numSize.y + 2.0f; - float width = std::max(numSize.x, denSize.x) + 10.0f; - - // Center numerator - draw->AddText( - ImVec2(pos.x + (width - numSize.x) * 0.5f, pos.y), - IM_COL32_WHITE, numerator - ); - - // Draw fraction bar - draw->AddLine( - ImVec2(pos.x, lineY), ImVec2(pos.x + width, lineY), - IM_COL32_WHITE, 1.5f - ); - - // Center denominator - draw->AddText( - ImVec2(pos.x + (width - denSize.x) * 0.5f, lineY + 2.0f), - IM_COL32_WHITE, denom - ); - - // Advance cursor so layout continues correctly - ImGui::Dummy(ImVec2(width, numSize.y + denSize.y + 6.0f)); - } - - // Interactive fraction where the denominator is 60 * k, and k can be adjusted by dragging horizontally - inline bool DragDtFraction(const char* id, int& k, bool isTelem = false) { - ImDrawList* draw = ImGui::GetWindowDrawList(); - ImVec2 pos = ImGui::GetCursorScreenPos(); - - // Ensure k is at least 1 to avoid zero or negative denominators - k = std::max(k, 1); - int denom = 30 * k; - - // Limits for k to prevent unreasonable values - const int MIN_K = 1; - const int MAX_K = isTelem ? 20 : 800; - - // Format denominator text - char denomBuf[32]; - snprintf(denomBuf, sizeof(denomBuf), "%d", denom); - - // Calculate text sizes - ImVec2 numSize = ImGui::CalcTextSize("1"); - ImVec2 denSize = ImGui::CalcTextSize(denomBuf); - - // Calculate width and positions - float width = std::max(numSize.x, denSize.x) + 10.0f; - float lineY = pos.y + numSize.y + 2.0f; - float totalHeight = numSize.y + denSize.y + 6.0f; - - // Numerator - draw->AddText( - ImVec2(pos.x + (width - numSize.x) * 0.5f, pos.y), - IM_COL32_WHITE, "1" - ); - - // Line - draw->AddLine( - ImVec2(pos.x, lineY), ImVec2(pos.x + width, lineY), - IM_COL32_WHITE, 1.5f - ); - - // Denominator position - ImVec2 denPos(pos.x + (width - denSize.x) * 0.5f, lineY + 2.0f); - - // Interactive region - ImGui::SetCursorScreenPos(denPos); - ImVec2 hitSize(denSize.x + 10.0f, denSize.y + 6.0f); - ImGui::InvisibleButton(id, hitSize); - - // Determine color based on interaction state - ImU32 colour = ImGui::IsItemActive() - ? IM_COL32(255, 220, 120, 255) // active color - : ImGui::IsItemHovered() - ? IM_COL32(200, 200, 255, 255) // hover color - : IM_COL32_WHITE; - - // Draw denominator with state-aware color - draw->AddText(denPos, colour, denomBuf); - - bool changed = false; - static float dragAccum = 0.0f; - - if (ImGui::IsItemActive()) { - ImGuiIO& io = ImGui::GetIO(); - dragAccum += io.MouseDelta.x; - int step = (int)dragAccum; - - // Update k if drag has accumulated enough to cross a step threshold - if (step != 0) { - dragAccum -= step * 1.0f; - int newK = k + step; - newK = std::clamp(newK, MIN_K, MAX_K); - // Only update if k actually changes - if (newK != k) { - k = newK; - changed = true; - } - } - } - else { dragAccum = 0.0f; } - - // Also allow adjusting k with the mouse wheel when hovering - if (ImGui::IsItemHovered()) { - ImGuiIO& io = ImGui::GetIO(); - - // Mouse wheel typically scrolls vertically, but we can interpret it as horizontal adjustment for this widget - if (io.MouseWheel != 0.0f) { - int newK = k + (int)io.MouseWheel; - newK = std::clamp(newK, MIN_K, MAX_K); - // Only update if k actually changes - if (newK != k) { - k = newK; - changed = true; - } - - // Reset mouse wheel to prevent affecting other widgets - io.MouseWheel = 0.0f; - } - } - - ImGui::NewLine(); - float dt = 1.0f / (float)denom; - ImGui::Text("dt: %.6f s", dt); - - // Advance cursor to account for the space taken by the fraction - ImGui::Dummy(ImVec2(width, totalHeight)); - return changed; - } -} - diff --git a/DSFE_App/DSFE_GUI/include/Rendering/GUIContext.h b/DSFE_App/DSFE_GUI/include/Rendering/GUIContext.h deleted file mode 100644 index 8acd3dd6..00000000 --- a/DSFE_App/DSFE_GUI/include/Rendering/GUIContext.h +++ /dev/null @@ -1,26 +0,0 @@ -// DSFE_Core GUIContext.h -#pragma once - -#include "RenderBase.h" -#include "ui/Styles.h" -#include "Platform/Logger.h" - -#include - -namespace render { - class GUIContext : public RenderContext { - public: - GUIContext() {} - - bool init(window::IWindow* win) override; - void preRender() override; - void postRender() override; - void end() override; - - void setMenuCallback(std::function callback) { _menuCallback = std::move(callback); } - - private: - std::unique_ptr _style; - std::function _menuCallback; - }; -} // namespace render \ No newline at end of file diff --git a/DSFE_App/DSFE_GUI/include/Rendering/OpenGLContext.h b/DSFE_App/DSFE_GUI/include/Rendering/OpenGLContext.h deleted file mode 100644 index 1dc44015..00000000 --- a/DSFE_App/DSFE_GUI/include/Rendering/OpenGLContext.h +++ /dev/null @@ -1,21 +0,0 @@ -// DSFE_GUI OpenGLContext.h -#pragma once - -#include "RenderBase.h" -#include "ui/Styles.h" -#include "Platform/Logger.h" - -namespace render { - class OpenGLContext : public RenderContext { - public: - bool init(window::IWindow* window) override; - void preRender() override; - void postRender() override; - void end() override; - - GLFWwindow* getGLFWWindow() const { return _glfwWindow; } - - private: - GLFWwindow* _glfwWindow = nullptr; - }; -} // namespace render \ No newline at end of file diff --git a/DSFE_App/DSFE_GUI/include/ui/CommandScriptEditor.h b/DSFE_App/DSFE_GUI/include/ui/CommandScriptEditor.h deleted file mode 100644 index 112e9af6..00000000 --- a/DSFE_App/DSFE_GUI/include/ui/CommandScriptEditor.h +++ /dev/null @@ -1,104 +0,0 @@ -// DSFE_GUI CommandScriptEditor.h -#pragma once - -#include -#include -#include -#include "Platform/StudyRunner.h" - -#include -#include -#include -#include "Scene/SimulationManager.h" - -#include "Interpreter/RunWrapper.h" - -#include -#include "Platform/imguiWidgets.h" -#include - -#include "Platform/Logger.h" - -struct StudyResult; // forward declaration to avoid circular dependency - -namespace gui { - struct ActiveRun { - std::future fut; - std::string tag; - }; - - class CommandScriptEditor { - public: - CommandScriptEditor(gui::SimManager* sims); - ~CommandScriptEditor(); - - // Menu Render - void drawMenus(); - // Main render function - void render(); - - private: - // Active runs management - std::mutex _activeRunsMutex; // Mutex for synchronizing access to active runs - std::vector _activeRuns; // Vector to hold active runs and their futures - - // Simulation manager reference - gui::SimManager* _sim; - - // Interpreter components - interpreter::Parser* _parser; - interpreter::IStoredProgram* _program; - interpreter::RunWrapper* _wrapper; - - // File browser for loading/saving scripts - ImGui::FileBrowser _load; - ImGui::FileBrowser _save; - - // Current script file directory path - std::string _currentScriptPath; - // Current script file path - std::string _currentScriptFile; - - std::string _pendingSaveName = "Untitled.scl"; - std::string _pendingSavePath = "Engine/assets/scripts"; - bool _requestSaveAsPopup = false; - - void runButtonHandler(); - - void renderEnvironment(); - void renderCmdInstructions(); - - void terminateScript(const char* reason, bool fault); - - bool tryLoadFromDialog(); - bool trySaveScriptToFile(const std::string& filepath); - - void pollRuns(); - void launchBackgroundRun(const std::string& code, const std::string& tag); - - void renderSaveAsPopup(); - - void beginEditorPanel(const char* id); - void endEditorPanel(); - - void setScript(const std::string& s) { _scriptText = s; } - std::string getScript() const { return _scriptText; } - - // Command script contents as lines - std::vector _script; - // Full script text - std::string _scriptText; - - // For log selection - std::unordered_set selectedLines; - // Last clicked line index - int lastClickedLine = -1; - - // Colours - const ImVec4 CMD_COL = ImVec4(0.95f, 0.85f, 0.25f, 1.0f); // yellow - const ImVec4 VEC_COL = ImVec4(0.40f, 0.65f, 0.95f, 1.0f); // blue - const ImVec4 ARG_COL = ImVec4(0.95f, 0.55f, 0.25f, 1.0f); // orange - const ImVec4 DESC_COL = ImVec4(0.60f, 0.60f, 0.60f, 1.0f); // grey - - }; -} // namespace gui \ No newline at end of file diff --git a/DSFE_App/DSFE_GUI/include/ui/ControlPanel.h b/DSFE_App/DSFE_GUI/include/ui/ControlPanel.h deleted file mode 100644 index eec5ed36..00000000 --- a/DSFE_App/DSFE_GUI/include/ui/ControlPanel.h +++ /dev/null @@ -1,232 +0,0 @@ -// DSFE_GUI ControlPanel.h -#pragma once - -#include -#include -#include -#include -#include - -#include -#include -#include - -#include "Platform/SimulationState.h" -#include "Platform/Logger.h" - -// Forward Declarations -namespace scene { - class Mesh; - class Object; - class Light; - class Camera; -} -namespace render { - enum class ResolutionPreset; - enum class QualityPreset; -} - -namespace robots { class RobotSystem; } -namespace diagnostics { class TelemetryRecorder; } - -namespace gui { - // Forward Declaration for SimManager - class SimManager; - enum class ControlMode; - - // Gravity UI Modes - enum class GravityUIMode { - Preset, - SolarSystem, - Custom - }; - - // Gravity Presets (in m/s^2) - enum GravityPreset { - // Common Presets - PRESET_ZERO_G, - PRESET_MICRO_G, - PRESET_SOLAR_SYSTEM, - PRESET_CUSTOM, - // Solar System Bodies - PRESET_SUN, - PRESET_MERCURY, - PRESET_VENUS, - PRESET_EARTH, - PRESET_MARS, - PRESET_JUPITER, - PRESET_SATURN, - PRESET_URANUS, - PRESET_NEPTUNE, - PRESET_PLUTO, - // Moons - PRESET_MOON, - PRESET_TITAN, - PRESET_ENCELADUS, - PRESET_EUROPA, - PRESET_GANYMEDE, - PRESET_IO - }; - - // UI Navigation Levels for Gravity Presets - enum class GravityLevel { - Root, - SolarSystem, - Planets, - Moons - }; - - inline GravityLevel gravityLevel = GravityLevel::Root; - inline GravityUIMode gravityMode = GravityUIMode::Preset; - - // Struct to hold comparison results for integrator analysis - struct ComparisonSnapshot { - std::string integratorName; - std::vector time; // time samples - std::vector errRms; // RMS error time series - std::vector errMax; // Max error time series - std::vector> jointErr; // [joint][sample] - int jointCount = 0; - }; - - // ControlPanel Class - class ControlPanel { - public: - ControlPanel(SimManager* sim); - - void drawMenus(SimManager* sim); - void render(SimManager* sim); - void setSimulationCallback(const std::function& callback) { simCallback = callback; } - void setMeshLoadCallback(const std::function& callback) { meshLoadCallback = callback; } - - private: - // Comparison results management - std::vector _comparisonResults; - std::vector> _comparisonFutures; - std::atomic _comparisonPending{ 0 }; - std::mutex _comparisonMutex; - bool _comparisonReady = false; - - // Internal Pointers - SimManager* _sim = nullptr; - std::shared_ptr _mesh = nullptr; - scene::Light* _light = nullptr; - scene::Object* _obj = nullptr; - ImGui::FileBrowser _meshLoad; - ImGui::FileBrowser _hdrLoad; - std::string _currentMeshFile; - std::string _currentHDRFile; - - ControlMode* _controlMode; - - std::function meshLoadCallback; - std::function simCallback; - - Selection _selection; - - // Internal Methods - void simulationProperties(); - void objectProperties(); - void jointProperties(); - void displaySettings(); - - void tempLightControls(); - - void roboticArmSelector(); - void roboticCardDisplay(const char* name, const char* company); - - void sceneObjectsTable(); - - // Follow View Methods - void selectJointAndFollow(int jointIndex); - //void drawTelemetryPlots(const diagnostics::TelemetryRecorder& rec); - void drawTrajectoryInspector(const diagnostics::TelemetryRecorder& rec, int jointCount, int& selectedJoint); - void drawJointErrorPlot(); - - // Results Methods - void drawResultsWindow(); - void exportTelemetryCSV(const char* filepath); - void runComparisonAllIntegratorsAsync(); - void pollComparisonFutures(); - void drawComparisonPlots(); - - // Helper Methods - void beginControlPanel(const char* id, ImVec2 size = ImVec2(0, 0)); - void endControlPanel(); - - // Menu Button States - bool _showRobotSelector = false; - bool _showSimulationProperties = false; - bool _showCameraProperties = false; - bool _showDisplaySettings = false; - bool autoScroll = true; - bool scrollToBottom = false; - bool _useAutoDiff = false; - - // Render Presets - render::ResolutionPreset r; - render::QualityPreset q; - - bool _qualityChanged = false; - bool _resChanged = false; - - // Internal states - bool simulationRunning = false; - bool _jointSelected = false; - bool diagRunning = false; - bool _robotRequested = false; - bool _hasRobot = false; - bool _openStats = true; - - // Results tab/window state - bool _showResultsWindow = false; - bool _simWasRunningLastFrame = false; - bool _resultsFocusNeeded = false; - bool _selectResultsTab = false; - - // Currently selected items - std::string _requestedRobot; // name of requested robot to load - std::string _currentObjectName; // name of currently selected object - std::string _currentLinkName; // name of currently selected link - std::string _currentJointName; // name of currently selected joint - std::string _lastLinkName; // name of last selected link - - // Store joint angles for display - std::unordered_map _linkAngles; - - // Simulation and diagnostics timing - float simLength = 30.0f; // ~30 seconds default - float diagLength = 15.0f; // ~15 seconds default - double deltaTime = 1.0f / 180.0f; // ~180 FPS default - float simTime = 0.0f; // current simulation time - float diagTime = 0.0f; // current diagnostic time - - // Plot heights for comparison - float _cPlotH1 = 250.0f; - float _cPlotH2 = 250.0f; - - // Plot heights for results - float _plotH1 = 250.0f; - float _plotH2 = 250.0f; - - // Camera properties - int povMode = 0; - float fov = 60.0f; - - // Internal Physics - float velocity = 0.0f; - float torque = 0.0f; - float linkLength = 1.0f; - float damping = 0.1f; - float position = 0.0f; - - // Time tracking for simulation updates - std::chrono::high_resolution_clock::time_point simLastUpdateTime = std::chrono::high_resolution_clock::now(); - std::chrono::high_resolution_clock::time_point diagLastUpdateTime = std::chrono::high_resolution_clock::now(); - }; -} // namespace gui - -// Helper Macros - -#define INDENT() ImGui::Indent(20.0f) -#define UNINDENT() ImGui::Unindent(20.0f) \ No newline at end of file diff --git a/DSFE_App/DSFE_GUI/include/ui/DebugPanel.h b/DSFE_App/DSFE_GUI/include/ui/DebugPanel.h deleted file mode 100644 index 6af36ec2..00000000 --- a/DSFE_App/DSFE_GUI/include/ui/DebugPanel.h +++ /dev/null @@ -1,58 +0,0 @@ -// DSFE_GUI DebugPanel.h -#pragma once - -#include "imgui.h" -#include "Scene/SimulationManager.h" -#include "Scene/Camera.h" -#include - -#include "Platform/Logger.h" - -namespace gui { - struct debugLogEntry { - std::string level; - std::string desc; - bool isError = false; - }; - - struct TerminalLine { - enum class Level { Info, Warn, Error, Debug }; - Level level = Level::Info; - std::string text; - }; - - class DebugPanel { - public: - void render(); - void clearSimLog() { gLog.Instance().clearSimLog(); simSelectedLines.clear(); } - - private: - bool autoScroll = true; - std::vector _entries; - std::vector _simEnteries; - - void renderLog(); - void renderSimLog(); - void renderErrorTable(); - - void beginDebugPanel(const char* id, ImVec2 size = ImVec2(0, 0)); - void endDebugPanel(); - - std::unordered_set selectedLines; - std::unordered_set simSelectedLines; - int lastClickedLine = -1; - - // Colour Keys (I am colour deficient so GPT has been used to generate the colour values) - const ImVec4 traceCol = { 0.6275f, 0.6275f, 0.6275f, 1.0f }; - const ImVec4 debugCol = { 0.3137f, 0.6667f, 0.9412f, 1.0f }; - const ImVec4 infoCol = { 0.3137f, 0.6275f, 1.0f, 1.0f }; - const ImVec4 warnCol = { 1.0f, 0.7059f, 0.0f, 1.0f }; - const ImVec4 errorCol = { 0.8627f, 0.2353f, 0.2745f, 1.0f }; - const ImVec4 okCol = { 0.0f, 0.7451f, 0.3922f, 1.0f }; - const ImVec4 failCol = { 0.6667f, 0.0f, 0.1961f, 1.0f }; - const ImVec4 runtimeCol = { 0.7451f, 0.4706f, 1.0f, 1.0f }; - const ImVec4 outputCol = { 0.8627f, 0.8627f, 0.8627f, 1.0f }; - const ImVec4 rotateCol = { 0.4666f, 1.0f, 0.0f, 1.0f }; - const ImVec4 translateCol = { 0.2745f, 0.5098f, 0.7059f, 1.0f }; - }; -} // namespace gui \ No newline at end of file diff --git a/DSFE_App/DSFE_GUI/include/ui/Styles.h b/DSFE_App/DSFE_GUI/include/ui/Styles.h deleted file mode 100644 index e3ba75ee..00000000 --- a/DSFE_App/DSFE_GUI/include/ui/Styles.h +++ /dev/null @@ -1,15 +0,0 @@ -#pragma once -// File: Styles.h -// GitHub: SaltyJoss -#include "EngineCore.h" - -namespace gui { - class Styles { - public: - /// DARK MODE STYLE (VS 2022 esc) - void DarkMode(); - - /// LIGHT MODE STYLE (VS 2022 esc) - void LightMode(); - }; -} // namespace gui diff --git a/DSFE_App/DSFE_GUI/src/MainWindow/Widgets/ControlPanelWidget.cpp b/DSFE_App/DSFE_GUI/src/MainWindow/Widgets/ControlPanelWidget.cpp index 37d9d133..36d5b7f8 100644 --- a/DSFE_App/DSFE_GUI/src/MainWindow/Widgets/ControlPanelWidget.cpp +++ b/DSFE_App/DSFE_GUI/src/MainWindow/Widgets/ControlPanelWidget.cpp @@ -10,6 +10,9 @@ #include #include #include +#include + +#include #include "Widgets/FractionSelectorWidget.h" @@ -42,6 +45,14 @@ namespace widgets { simPropertiesPanel(); jointInfoPanel(); + auto* timer = new QTimer(this); + connect(timer, &QTimer::timeout, this, [this]() { + updateSimClock(); + jointInfoPanel(); + updateTelemetryDisplay(); + }); + timer->start(7); + _contentLayout->addStretch(); } @@ -68,6 +79,11 @@ namespace widgets { _telemetryDtSelector = new FractionSelectorWidget(true); layout->addWidget(new QLabel("Telemetry Time Step (dt):")); layout->addWidget(_telemetryDtSelector); + _simTimeLabel = new QLabel(); + _simTimeLabel->setTextFormat(Qt::RichText); + _simTimeLabel->setWordWrap(true); + layout->addWidget(new QLabel("Simulation Time:")); + layout->addWidget(_simTimeLabel); _contentLayout->addWidget(_simPropertiesGroup); @@ -123,73 +139,70 @@ namespace widgets { } void ControlPanelWidget::jointInfoPanel() { - _jointInfoGroup = new QGroupBox("Robot Joint Information"); - auto* layout = new QVBoxLayout(_jointInfoGroup); - _contentLayout->addWidget(_jointInfoGroup); + if (!_jointInfoGroup) { + _jointInfoGroup = new QGroupBox("Robot Joint Information"); + auto* layout = new QVBoxLayout(_jointInfoGroup); + _jointInfoGroup->setLayout(layout); + _currentSimTimeJointLabel = new QLabel(_jointInfoGroup); + layout->addWidget(_currentSimTimeJointLabel); + + _jointIdxSlider = new QSlider(Qt::Horizontal, _jointInfoGroup); + _jointIdxSlider->setMinimum(1); + _jointIdxSlider->setValue(1); + connect(_jointIdxSlider, &QSlider::valueChanged, this, [this](int value) { selectJointAndFollow(value - 1); }); + layout->addWidget(_jointIdxSlider); + + buildTelemetryWidgets(layout); + + _contentLayout->addWidget(_jointInfoGroup); + } - if (!_sim->hasRobot()) { - layout->addWidget(new QLabel("No Robot Loaded.")); + if (!_sim || !_sim->hasRobot()) { + _jointInfoGroup->setVisible(false); return; } robots::RobotSystem* robot = _sim->robotSystem(); if (!robot) { - layout->addWidget(new QLabel("Robotic system unavailable")); + _jointInfoGroup->setVisible(false); return; } auto& joints = robot->joints(); auto& links = robot->links(); - - _jointIdxSlider = new QSlider(Qt::Orientation::Horizontal); - layout->addWidget(_jointIdxSlider); + _jointInfoGroup->setVisible(!joints.empty() && !links.empty()); + if (joints.empty() || links.empty()) { _jointInfoGroup->setVisible(false); return; } + _jointInfoGroup->setVisible(true); + _jointIdxSlider->setMaximum(static_cast(joints.size())); static int currentJointIndex = 0; currentJointIndex = std::clamp(currentJointIndex, 0, (int)joints.size() - 1); - - auto& j = joints[currentJointIndex]; - int linkIndex = currentJointIndex; - linkIndex = std::clamp(linkIndex, 0, (int)links.size() - 1); - auto& l = links[linkIndex]; - - const auto& rec = _sim->telemetry(); - displayJointInfo(rec, currentJointIndex, layout); - layout->addSpacing(5); - layout->addWidget(new QLabel("Selected Joint: " + QString::fromStdString(j.name) + " - Child Link: " + QString::fromStdString(l.name))); } - void ControlPanelWidget::displayJointInfo(const diagnostics::TelemetryRecorder& rec, int& selectedJoint, QVBoxLayout* layout) { - const auto& ring = rec.ring; - if (ring.size() < 1) { return; } - const diagnostics::TelemetrySample& s = ring.at(ring.size() - 1); // Get the most recent sample - if (selectedJoint < 0) { selectedJoint = 0; } - if (selectedJoint >= (int)s.j.size()) { selectedJoint = (int)s.j.size() - 1; } + void ControlPanelWidget::updateTelemetryInfo(const diagnostics::JointTelemetry& j) { + auto& t = _telemetryLabels; + const float e = static_cast(j.q_ref - j.q); - _currentSimTimeJointLabel = new QLabel(QString("Current Simulation Time: ") + QString::number(s.timeSec) + " s"); - layout->addWidget(_currentSimTimeJointLabel); + t.q->setText(QString("pos:\t%1 rad").arg(j.q)); + t.qd->setText(QString("vel:\t%1 rad/s").arg(j.qd)); + t.tau->setText(QString("torque:\t%1 Nm").arg(j.torqueNm)); - int currentJ = selectedJoint + 1; - if (!_jointIdxSlider) { - selectedJoint = currentJ - 1; - - _jointIdxSlider->setMinimum(1); - _jointIdxSlider->setMaximum((int)s.j.size()); - _jointIdxSlider->setValue(currentJ); - connect(_jointIdxSlider, &QSlider::valueChanged, this, [this](int value) { - int jointIdx = value - 1; - selectJointAndFollow(jointIdx); - }); - } - else { - selectedJoint = currentJ - 1; - _jointIdxSlider->setMaximum((int)s.j.size()); - _jointIdxSlider->setValue(currentJ); - } + t.qRef->setText(QString("pos_ref:\t%1 rad").arg(j.q_ref)); + t.qdRef->setText(QString("vel_ref:\t%1 rad/s").arg(j.qd_ref)); + t.qddRef->setText(QString("acc_ref:\t%1 rad/s²").arg(j.qdd_ref)); + t.err->setText(QString("error:\t%1 rad").arg(e)); - const diagnostics::JointTelemetry& j = s.j[selectedJoint]; - const float e = static_cast(j.q_ref - j.q); + t.qTraj->setText(QString("pos_traj:\t%1 rad").arg(j.traj_q)); + t.qdTraj->setText(QString("vel_traj:\t%1 rad/s").arg(j.traj_qd)); + t.qddTraj->setText(QString("acc_traj:\t%1 rad/s²").arg(j.traj_qdd)); + + t.qClamped->setText(QString("pos_clamped:\t%1").arg(j.clampTheta ? "true" : "false")); + t.qdClamped->setText(QString("vel_clamped:\t%1").arg(j.clampOmega ? "true" : "false")); - layout->addSpacing(10); + t.damping->setText(QString("damping:\t%1 kg·m²/s").arg(j.damping)); + t.friction->setText(QString("friction:\t%1 N·m").arg(j.friction)); + } + void ControlPanelWidget::buildTelemetryWidgets(QVBoxLayout* layout) { auto headerFont = [](QLabel* label) { QFont font = label->font(); font.setBold(true); @@ -197,54 +210,90 @@ namespace widgets { label->setFont(font); }; - - auto* stateLabel = new QLabel(QString("State: ")); - headerFont(stateLabel); - auto* refLabel = new QLabel(QString("Reference: ")); - headerFont(refLabel); - auto* trajLabel = new QLabel(QString("Trajectory: ")); - headerFont(trajLabel); - auto* clampLabel = new QLabel(QString("Clamped: ")); - headerFont(clampLabel); - auto* constLabel = new QLabel(QString("Constants: ")); - headerFont(constLabel); - - // Joint State Telemetry - layout->addWidget(stateLabel); - layout->addWidget(new QLabel(QString("pos: ") + QString::number(j.q) + " rad")); - layout->addWidget(new QLabel(QString("vel: ") + QString::number(j.qd) + " rad/s")); - layout->addWidget(new QLabel(QString("torque: ") + QString::number(j.torqueNm) + " Nm")); + auto& t = _telemetryLabels; + + t.stateHeader = new QLabel("State:"); + t.referenceHeader = new QLabel("Reference:"); + t.trajectoryHeader = new QLabel("Trajectory:"); + t.clampedHeader = new QLabel("Clamped:"); + t.constantsHeader = new QLabel("Constants:"); + + headerFont(t.stateHeader); + headerFont(t.referenceHeader); + headerFont(t.trajectoryHeader); + headerFont(t.clampedHeader); + headerFont(t.constantsHeader); + + t.q = new QLabel(); + t.qd= new QLabel(); + t.tau = new QLabel(); + + t.qRef = new QLabel(); + t.qdRef = new QLabel(); + t.qddRef = new QLabel(); + t.err = new QLabel(); + + t.qTraj = new QLabel(); + t.qdTraj = new QLabel(); + t.qddTraj = new QLabel(); + + t.qClamped = new QLabel(); + t.qdClamped = new QLabel(); + + t.damping = new QLabel(); + t.friction = new QLabel(); layout->addSpacing(5); - // Joint Reference Telemetry - layout->addWidget(refLabel); - layout->addWidget(new QLabel(QString("pos_ref: ") + QString::number(j.q_ref) + " rad")); - layout->addWidget(new QLabel(QString("vel_ref: ") + QString::number(j.qd_ref) + " rad/s")); - layout->addWidget(new QLabel(QString("acc_ref: ") + QString::number(j.qdd_ref) + " rad/s²")); - layout->addWidget(new QLabel(QString("error: ") + QString::number(e) + " rad")); + layout->addWidget(t.stateHeader); + layout->addWidget(t.q); + layout->addWidget(t.qd); + layout->addWidget(t.tau); layout->addSpacing(5); - // Joint Trajectory Telemetry - layout->addWidget(trajLabel); - layout->addWidget(new QLabel(QString("pos_traj: ") + QString::number(j.traj_q) + " rad")); - layout->addWidget(new QLabel(QString("vel_traj: ") + QString::number(j.traj_qd) + " rad/s")); - layout->addWidget(new QLabel(QString("acc_traj: ") + QString::number(j.traj_qdd) + " rad/s²")); + layout->addWidget(t.referenceHeader); + layout->addWidget(t.qRef); + layout->addWidget(t.qdRef); + layout->addWidget(t.qddRef); + layout->addWidget(t.err); layout->addSpacing(5); - // Joint Clamping Telemetry - layout->addWidget(clampLabel); - layout->addWidget(new QLabel(QString("pos_clamped: ") + QString(j.clampTheta ? "true" : "false"))); - layout->addWidget(new QLabel(QString("vel_clamped: ") + QString(j.clampOmega ? "true" : "false"))); + layout->addWidget(t.trajectoryHeader); + layout->addWidget(t.qTraj); + layout->addWidget(t.qdTraj); + layout->addWidget(t.qddTraj); layout->addSpacing(5); - // Joint Constants Telemetry - layout->addWidget(constLabel); - layout->addWidget(new QLabel(QString("damping: ") + QString::number(j.damping) + " kg·m²/s")); - layout->addWidget(new QLabel(QString("friction: ") + QString::number(j.friction) + " N·m")); + layout->addWidget(t.clampedHeader); + layout->addWidget(t.qClamped); + layout->addWidget(t.qdClamped); + + layout->addSpacing(5); + + layout->addWidget(t.constantsHeader); + layout->addWidget(t.damping); + layout->addWidget(t.friction); + } + + void ControlPanelWidget::updateTelemetryDisplay() { + if (!_sim) { return; } + const auto& rec = _sim->telemetry(); + const auto& ring = rec.ring; + if (ring.size() < 1) { return; } + const auto& s = ring.at(ring.size() - 1); + if (s.j.empty()) { return; } + int jointIdx = _selection.type == SelectionType::JOINT ? _selection.index : 0; + jointIdx = std::clamp(jointIdx, 0, static_cast(s.j.size()) - 1); + if (_jointIdxSlider) { + QSignalBlocker blocker(_jointIdxSlider); + _jointIdxSlider->setMaximum(static_cast(s.j.size())); + _jointIdxSlider->setValue(jointIdx + 1); + } + updateTelemetryInfo(s.j[jointIdx]); + if (_currentSimTimeJointLabel) { _currentSimTimeJointLabel->setText(QString("Current Simulation Time: %1 s").arg(simTime, 0, 'f', 3)); } } void ControlPanelWidget::displayPanel() { @@ -271,4 +320,29 @@ namespace widgets { _sim->followRobotJoint(_currentJointName, glm::vec3(0.0f, 0.2f, 0.6f)); } + + void ControlPanelWidget::updateSimClock() { + if (!_sim || !_simTimeLabel) { return; } + + const double elapsed = _sim->simTime(); + const bool running = _sim->isSimRunning(); + + if (!running && elapsed <= 0.0) { + _simTimeLabel->clear(); + _simTimeLabel->setVisible(false); + return; + } + + _simTimeLabel->setVisible(true); + _simTimeLabel->setTextFormat(Qt::RichText); + _simTimeLabel->setWordWrap(true); + + if (_sim->isSimRunning()) { + simTime = _sim->simTime(); + _simTimeLabel->setText(QString("Elapsed Time: %1 s").arg(_sim->simTime(), 0, 'f', 3)); + } + else { + _simTimeLabel->setText(QString("Elapsed Time: %1 s").arg(_sim->simTime(), 0, 'f', 3)); + } + } } // namespace widgets \ No newline at end of file diff --git a/DSFE_App/DSFE_GUI/src/MainWindow/Widgets/DSLEditorWidget.cpp b/DSFE_App/DSFE_GUI/src/MainWindow/Widgets/DSLEditorWidget.cpp index 453a1342..77233689 100644 --- a/DSFE_App/DSFE_GUI/src/MainWindow/Widgets/DSLEditorWidget.cpp +++ b/DSFE_App/DSFE_GUI/src/MainWindow/Widgets/DSLEditorWidget.cpp @@ -58,7 +58,6 @@ namespace widgets { _stateTimer->start(100); // Poll every 100 ms connect(_runStopButton, &QPushButton::clicked, this, [this]() { _sim->isScriptRunning() ? stopScript() : runScript(); }); - pollScriptState(); } @@ -214,6 +213,7 @@ namespace widgets { } void DSLEditorWidget::pollScriptState() { + updateButtonState(_sim->isScriptRunning()); if (!_sim->isScriptRunning()) { return; } auto* prog = _sim->activeProgram(); if (!prog) { terminateScript("No active program found in simulation manager.", true); return; } diff --git a/DSFE_App/DSFE_GUI/src/Platform/WindowManager.cpp b/DSFE_App/DSFE_GUI/src/Platform/WindowManager.cpp deleted file mode 100644 index ca5ecebe..00000000 --- a/DSFE_App/DSFE_GUI/src/Platform/WindowManager.cpp +++ /dev/null @@ -1,200 +0,0 @@ -// DSFE_GUI WindowManager.cpp -#include "pch.h" - -#ifdef __gl_h_ -#undef __gl_h_ -#endif -#include -#include - -#include "Platform/Window.h" - -#include "Rendering/GUIContext.h" -#include "Rendering/OpenGLContext.h" -#include "Rendering/OpenGLBufferManager.h" -#include "Rendering/ShaderUtil.h" - -#include "Scene/Camera.h" -#include "Scene/Light.h" -#include "Scene/Input.h" -#include "Scene/Mesh.h" - -#include "Scene/SimulationManager.h" -#include "ui/DebugPanel.h" -#include "ui/ControlPanel.h" -#include "ui/CommandScriptEditor.h" - -#include "Platform/WindowManager.h" - -#include "EngineLib/LogMacros.h" - -bool controlPanelOpen = false; - -namespace window { - // Constructor - GLWindow::GLWindow() { _header = new std::string(); } - // Destructor - GLWindow::~GLWindow() { - _renderCntx->end(); - _GUICntx->end(); - - if (_window) { glfwDestroyWindow(_window); } - - glfwTerminate(); - delete _header; - - LOG_INFO("GLWindow destroyed, rendering and GUI contexts ended"); - } - - void GLWindow::render() { - _renderCntx->preRender(); - _GUICntx->preRender(); - - if (_sim) _sim->render(); - if (_controlPanel) _controlPanel->render(_sim.get()); - if (_debugPanel) _debugPanel->render(); - if (_cmdEditor) _cmdEditor->render(); - - - // Menu Callback - _GUICntx->setMenuCallback([this]() { - - ImGuiStyle& style = ImGui::GetStyle(); - // Vertical spacing between menu items - style.ItemSpacing = ImVec2(10.0f, 6.0f); - // Padding inside menus - style.WindowPadding = ImVec2(12.0f, 12.0f); // (left/right, top/bottom) - // Padding inside each menu item - style.FramePadding = ImVec2(8.0f, 4.0f); - - _controlPanel->drawMenus(_sim.get()); - _cmdEditor->drawMenus(); - }); - - _GUICntx->postRender(); - _renderCntx->postRender(); - } - - // Initialisation: Sets up the OpenGL context, GUI context, and simulation manager - bool GLWindow::init(int width, int height, const std::string& header) { - _width = width; - _height = height; - *_header = header; - - // Context layers - _renderCntx = std::make_unique(); - _renderCntx->init(this); - - _window = _renderCntx->getGLFWWindow(); - - _GUICntx = std::make_unique(); - _GUICntx->init(this); - - // UI + scene - _sim = std::make_unique(); - _sim->initGL(); - _controlPanel = std::make_unique(_sim.get()); - _debugPanel = std::make_unique(); - _cmdEditor = std::make_unique(_sim.get()); - - _controlPanel->setMeshLoadCallback([this](std::string path) { - _sim->loadMesh(path); - LOG_INFO("Mesh loaded: %s", path.c_str()); - } - ); - - _isRunning = true; - return true; - } - - // Window resize callback: Enforces 16:9 aspect ratio and updates the OpenGL viewport - void GLWindow::onResize(int width, int height) { - if (width <= 0 || height <= 0) return; - - _width = width; - _height = height; - - // Update the GL viewport for the default framebuffer - glViewport(0, 0, width, height); - } - - // Miscellaneous - bool GLWindow::shouldClose() const { return glfwWindowShouldClose(_window); } - void GLWindow::pollEvents() { glfwPollEvents(); } - void GLWindow::swapBuffers() { glfwSwapBuffers(_window); } - void* window::GLWindow::getNativeWin() { return static_cast(_window); } - void window::GLWindow::setNativeWin(void* window) { _window = static_cast(window);} - int window::GLWindow::getWidth() const { return _width; } - int window::GLWindow::getHeight() const { return _height; } - const std::string& window::GLWindow::getHeader() const { return *_header; } - - // Update loop: Handles input and updates the simulation state - void GLWindow::update() { - pollEvents(); - - static double lastFrame = glfwGetTime(); - double currentFrame = glfwGetTime(); - float dt = static_cast(currentFrame - lastFrame); - lastFrame = currentFrame; - - // minimal smoothing (optional) - static float smoothedDt = 0.016f; - smoothedDt = glm::mix(smoothedDt, dt, 0.2f); - - if (_sim) { - //_sim->handleContinuousMovement(_window, smoothedDt); - //_sim->getCamera()->applyGravity(smoothedDt, _sim->getPlaneHeight()); - } - } - - // Input handling - void window::GLWindow::setMouseCaptured(bool captured) { - _mouseCaptured = captured; - GLFWwindow* w = _window; - if (!w) { return; } - - // When mouse is captured, disable the cursor and enable raw mouse motion for high-precision input - if (_mouseCaptured) { - // When mouse is captured, disable the cursor and enable raw mouse motion for high-precision input - glfwSetInputMode(w, GLFW_CURSOR, GLFW_CURSOR_DISABLED); - if (glfwRawMouseMotionSupported()) { - glfwSetInputMode(w, GLFW_RAW_MOUSE_MOTION, GLFW_TRUE); - } - ImGui::GetIO().ConfigFlags |= ImGuiConfigFlags_NoMouse; - // tell SimManager to reset its first-mouse state - if (_sim) { _sim->resetMouseDelta(); } - } - else { - // When mouse is released, show the cursor and disable raw mouse motion - glfwSetInputMode(w, GLFW_CURSOR, GLFW_CURSOR_NORMAL); - if (glfwRawMouseMotionSupported()) { - glfwSetInputMode(w, GLFW_RAW_MOUSE_MOTION, GLFW_FALSE); - } - ImGui::GetIO().ConfigFlags &= ~ImGuiConfigFlags_NoMouse; - } - } - - // Toggle mouse capture on Escape key press, and also toggle the control panel visibility - void window::GLWindow::onKey(int key, int /*scancode*/, int action, int /*mods*/) { - if (action == GLFW_PRESS && key == GLFW_KEY_ESCAPE) { - setMouseCaptured(!_mouseCaptured); - controlPanelOpen = !controlPanelOpen; - ImGui::SetWindowFocus(nullptr); - return; - } - } - - // Forward scroll events to the SimManager for zooming or other scroll-based interactions - void window::GLWindow::onScroll(double delta) { - if (_sim) { _sim->onMouseWheel(delta); } - } - - // Forward window resize events to the SimManager to adjust the internal rendering resolution and aspect ratio - void window::GLWindow::onCursorPos(double xpos, double ypos) { - // LOG_INFO("Mouse moved to: X=%.2f, Y=%.2f", xpos, ypos); - } - - // Window states - bool GLWindow::isRunning() const { return _isRunning; } - void GLWindow::onClose() { _isRunning = false; } -} diff --git a/DSFE_App/DSFE_GUI/src/Rendering/GUIContext.cpp b/DSFE_App/DSFE_GUI/src/Rendering/GUIContext.cpp deleted file mode 100644 index 50631e21..00000000 --- a/DSFE_App/DSFE_GUI/src/Rendering/GUIContext.cpp +++ /dev/null @@ -1,114 +0,0 @@ -// DSFE_GUI GUIContext.cpp -#include "Utils.h" - -#ifdef __gl_h_ -#undef __gl_h_ -#endif -#include -#include - -#include "Rendering/GUIContext.h" -#include "EngineLib/LogMacros.h" - -// ImGui -#include -#include -#include - -// ImPlot -#include - -namespace render { - bool render::GUIContext::init(window::IWindow* window) { - __super::init(window); - - const char* glslVersion = "#version 460 core"; - - IMGUI_CHECKVERSION(); - ImGui::CreateContext(); - ImPlot::CreateContext(); - - ImGuiIO& io = ImGui::GetIO(); (void)io; - io.ConfigFlags |= ImGuiConfigFlags_NavEnableKeyboard - | ImGuiConfigFlags_DockingEnable - | ImGuiConfigFlags_ViewportsEnable; - - try { - io.IniFilename = "config/imgui.ini"; - } - catch (const std::exception& e) { - LOG_ERROR("Failed to set ImGui ini file path: %s, loading default", e.what()); - io.IniFilename = nullptr; // fallback to default in-memory ini - } - - _style = std::make_unique(); - _style->DarkMode(); - - ImGui_ImplGlfw_InitForOpenGL((GLFWwindow*)_window->getNativeWin(), true); - ImGui_ImplOpenGL3_Init(glslVersion); - - LOG_INFO("ImGui context initialised (GLSL %s)", glslVersion); - return true; - } - - void render::GUIContext::preRender() { - ImGui_ImplOpenGL3_NewFrame(); - ImGui_ImplGlfw_NewFrame(); - ImGui::NewFrame(); - - ImGuiWindowFlags windowFlags = ImGuiWindowFlags_NoDocking | ImGuiWindowFlags_NoTitleBar - | ImGuiWindowFlags_NoCollapse | ImGuiWindowFlags_NoResize - | ImGuiWindowFlags_NoMove | ImGuiWindowFlags_NoBringToFrontOnFocus - | ImGuiWindowFlags_NoNavFocus | ImGuiWindowFlags_NoBackground; - - ImGuiViewport* viewport = ImGui::GetMainViewport(); - ImGui::SetNextWindowPos(viewport->Pos); - ImGui::SetNextWindowSize(viewport->Size); - ImGui::SetNextWindowViewport(viewport->ID); - - ImGui::PushStyleVar(ImGuiStyleVar_WindowRounding, 0.0f); - ImGui::PushStyleVar(ImGuiStyleVar_WindowBorderSize, 0.0f); - ImGui::PushStyleVar(ImGuiStyleVar_WindowPadding, ImVec2(0.0f, 0.0f)); - ImGui::Begin("Invisible-Window", nullptr, windowFlags | ImGuiWindowFlags_MenuBar); - - // Menu from control panel, moved here -> need to find a way to connect the two! - if (ImGui::BeginMenuBar()) { - if (_menuCallback) { _menuCallback(); } - ImGui::EndMenuBar(); - } - - ImGui::PopStyleVar(3); - - ImGuiID dockingSpaceID = ImGui::GetID("Invisible-Window-Docking-Space"); - - ImGui::DockSpace(dockingSpaceID, ImVec2(0.0f, 0.0f), ImGuiDockNodeFlags_PassthruCentralNode); - ImGui::End(); - - LOG_INFO_ONCE("ImGui preRender frame prepared with docking space ID %u", dockingSpaceID); - } - - void render::GUIContext::postRender() { - ImGui::Render(); - ImGui_ImplOpenGL3_RenderDrawData(ImGui::GetDrawData()); - - ImGuiIO& io = ImGui::GetIO(); - - if (io.ConfigFlags & ImGuiConfigFlags_ViewportsEnable) { - GLFWwindow* backupCurrentContext = glfwGetCurrentContext(); - ImGui::UpdatePlatformWindows(); - ImGui::RenderPlatformWindowsDefault(); - glfwMakeContextCurrent(backupCurrentContext); - - LOG_INFO_ONCE("ImGui platform windows rendered (viewports enabled)"); - } - } - - void render::GUIContext::end() { - ImPlot::DestroyContext(); - ImGui_ImplOpenGL3_Shutdown(); - ImGui_ImplGlfw_Shutdown(); - ImGui::DestroyContext(); - - LOG_INFO("ImGui context shutdown completed"); - } -} \ No newline at end of file diff --git a/DSFE_App/DSFE_GUI/src/Rendering/OpenGLContext.cpp b/DSFE_App/DSFE_GUI/src/Rendering/OpenGLContext.cpp deleted file mode 100644 index 0ab0ccfd..00000000 --- a/DSFE_App/DSFE_GUI/src/Rendering/OpenGLContext.cpp +++ /dev/null @@ -1,113 +0,0 @@ -#include "pch.h" -// File: OpenGLContext.cpp -// GitHub: SaltyJoss -#ifdef __gl_h_ -#undef __gl_h_ -#endif -#include -#include -#include "Rendering/OpenGLContext.h" -#include - -#include "EngineLib/LogMacros.h" - -namespace render { - // GLFW Callback for key events - static void onKey_Callback(GLFWwindow* win, int key, int scancode, int action, int mods) { - auto currentWindow = static_cast(glfwGetWindowUserPointer(win)); - currentWindow->onKey(key, scancode, action, mods); - } - - // GLFW Callback for cursor position events - static void CursorPos_Callback(GLFWwindow* win, double xpos, double ypos) { - auto currentWindow = static_cast(glfwGetWindowUserPointer(win)); - currentWindow->onCursorPos(xpos, ypos); - } - - // GLFW Callback for scroll events - static void onScroll_Callback(GLFWwindow* win, double /*xoffset*/, double yoffset) { - auto currentWindow = static_cast(glfwGetWindowUserPointer(win)); - currentWindow->onScroll(yoffset); - } - - // GLFW Callback for window resize events - static void onResize_Callback(GLFWwindow* win, int width, int height) { - auto currentWindow = static_cast(glfwGetWindowUserPointer(win)); - currentWindow->onResize(width, height); - } - - // GLFW Callback for window close events - static void onClose_Callback(GLFWwindow* win) { - window::IWindow* currentWindow = static_cast(glfwGetWindowUserPointer(win)); - currentWindow->onClose(); - } - - // Initialize OpenGL context and create GLFW window - bool render::OpenGLContext::init(window::IWindow* window) { - __super::init(window); - - if (!window->getWidth() || !window->getHeight()) { - LOG_ERROR("Window dimensions not set!"); - return false; - } - - if (!glfwInit()) { - LOG_ERROR("Failed to initialize GLFW -> ", glfwGetError(NULL)); - return false; - } - - // Set GLFW window hints for OpenGL version and profile - auto glWindow = glfwCreateWindow(window->getWidth(), window->getHeight(), window->getHeader().c_str(), nullptr, nullptr); - glfwSetInputMode(glWindow, GLFW_CURSOR, GLFW_CURSOR_DISABLED); - glfwSetInputMode(glWindow, GLFW_RAW_MOUSE_MOTION, GLFW_TRUE); - - // Check if window creation succeeded - if (!glWindow) { - LOG_ERROR("Failed to create GLFW window -> ", glfwGetError(NULL)); - glfwTerminate(); - return false; - } - - window->setNativeWin(glWindow); - _glfwWindow = glWindow; - - // Set up OpenGL context and callbacks - glfwSwapInterval(1); - glfwSetWindowUserPointer(glWindow, window); // Set user pointer to access window instance in callbacks - glfwSetWindowSizeLimits(glWindow, 800, 450, GLFW_DONT_CARE, GLFW_DONT_CARE); // Minimum 16:9 at 800x450, no maximum - glfwSetWindowAspectRatio(glWindow, 16, 9); // Enforce 16:9 aspect ratio - glfwSetCursorPosCallback(glWindow, CursorPos_Callback); - glfwSetKeyCallback(glWindow, onKey_Callback); - glfwSetScrollCallback(glWindow, onScroll_Callback); - glfwSetWindowSizeCallback(glWindow, onResize_Callback); - glfwSetWindowCloseCallback(glWindow, onClose_Callback); - glfwMakeContextCurrent(glWindow); - - // Load OpenGL function pointers using GLAD - if (!gladLoadGLLoader((GLADloadproc)glfwGetProcAddress)) { - LOG_ERROR("Failed to initialise GLAD"); - return false; - } - - glEnable(GL_DEPTH_TEST); - return true; - } - - // Set viewport and clear buffers before rendering - void render::OpenGLContext::preRender() { - glViewport(0, 0, _window->getWidth(), _window->getHeight()); - glClearColor(0.33f, 0.33f, 0.33f, 1.0f); - glClear(GL_COLOR_BUFFER_BIT | GL_DEPTH_BUFFER_BIT); - } - - // Swap buffers after rendering - void render::OpenGLContext::postRender() { - glfwSwapBuffers((GLFWwindow*)_window->getNativeWin()); - } - - // Clean up GLFW resources and terminate context - void render::OpenGLContext::end() { - glfwDestroyWindow((GLFWwindow*)_window->getNativeWin()); - glfwTerminate(); - } -} diff --git a/DSFE_App/DSFE_GUI/src/Scene/SimulationManager.cpp b/DSFE_App/DSFE_GUI/src/Scene/SimulationManager.cpp index d11b1720..3f2b6845 100644 --- a/DSFE_App/DSFE_GUI/src/Scene/SimulationManager.cpp +++ b/DSFE_App/DSFE_GUI/src/Scene/SimulationManager.cpp @@ -3,7 +3,10 @@ #include "Scene/SimulationManager.h" #include "Scene/SimulationCore.h" +#include +#include #include +#include extern "C" core::ISimulationCore* CreateSimulationCore_v1(); extern "C" void DestroySimulationCore(core::ISimulationCore*); @@ -64,9 +67,7 @@ namespace gui { // Helper: create CorePtr (unique_ptr with std::function deleter) static CorePtr makeCoreFactory() { core::ISimulationCore* raw = CreateSimulationCore_v1(); - if (!raw) { - return CorePtr(nullptr, [](core::ISimulationCore*) {}); - } + if (!raw) { return CorePtr(nullptr, [](core::ISimulationCore*) {}); } // std::function deleter is constructed from the lambda implicitly return CorePtr(raw, [](core::ISimulationCore* p) { DestroySimulationCore(p); }); } @@ -316,6 +317,4 @@ namespace gui { } void gui::SimManager::resetMouseDelta() { _firstMouse = true; } - - // --- Helpers --- } \ No newline at end of file diff --git a/DSFE_App/DSFE_GUI/src/ui/CommandScriptEditor.cpp b/DSFE_App/DSFE_GUI/src/ui/CommandScriptEditor.cpp deleted file mode 100644 index 104a65b0..00000000 --- a/DSFE_App/DSFE_GUI/src/ui/CommandScriptEditor.cpp +++ /dev/null @@ -1,703 +0,0 @@ -// DSFE_GUI CommandScriptEditor.cpp -#include -#include "ui/CommandScriptEditor.h" - -#include "Numerics/IntegrationMethods.h" -#include "Interpreter/StoredProgram.h" - -#include - -#include "Platform/Paths.h" -#include "EngineLib/LogMacros.h" - -namespace gui { - CommandScriptEditor::CommandScriptEditor(gui::SimManager* sim) : _sim(sim), _parser(nullptr), _program(nullptr), _wrapper(nullptr) { - _script = std::vector(); - _sim->setScriptRunning(false); - - // Preallocate script text buffer - _scriptText.reserve(8192); - - _currentScriptPath = (paths::assets() / "DSLScripts").string(); - _currentScriptFile = "<...>"; - - _load.SetTitle("Load Command Script"); - _load.SetDirectory(_currentScriptPath); - _load.SetTypeFilters({ ".dsl", ".txt" }); - _load.SetCurrentTypeFilterIndex(1); // default to .dsl - - _save.SetTitle("Save Command Script"); - _save.SetDirectory(_currentScriptPath); - _save.SetTypeFilters({ ".dsl", ".txt" }); - _save.SetCurrentTypeFilterIndex(1); // default to .dsl - }// .dsl ([Dynamical [S]ystems [L]anguage), a domain-specific language for defining constrained dynamical systems in MGRE - - // Destructor - CommandScriptEditor::~CommandScriptEditor() { - _script.clear(); - _sim->setScriptRunning(false); - } - - static int textResizeCallback(ImGuiInputTextCallbackData* data) { - if (data->EventFlag == ImGuiInputTextFlags_CallbackResize) { - auto* str = static_cast(data->UserData); - // Resize string callback - str->resize(data->BufTextLen + 1); - data->Buf = str->data(); - // Ensure string size matches buffer length - str->resize(data->BufTextLen); - } - return 0; - } - - static std::string filenameOnly(const std::string& path) { - if (path.empty()) return "<...>"; - return std::filesystem::path(path).filename().string(); - } - - // Draw the menu items - void CommandScriptEditor::drawMenus() { - // File menu for loading/saving scripts - if (ImGui::BeginMenu("Script")) { - if (ImGui::MenuItem("Load")) { - _load.Open(); - } - if (ImGui::MenuItem("Save")) { - if (_currentScriptFile == "Engine/assets/scripts") { - ImGui::OpenPopup("Save Command Script"); - } - else { - std::string path = _currentScriptFile; - trySaveScriptToFile(path); - } - } - if (ImGui::MenuItem("Save As")) { - _pendingSavePath = _currentScriptPath; - _pendingSaveName = "unititledScript.dsl"; - _requestSaveAsPopup = true; - } - - ImGui::EndMenu(); - } - - // AFTER menu closes - if (_requestSaveAsPopup) { - ImGui::OpenPopup("Save Command Script"); - _requestSaveAsPopup = false; - } - const bool loadedThisFrame = tryLoadFromDialog(); - renderSaveAsPopup(); - - if (loadedThisFrame) { - _script.clear(); - - std::string tmp; - tmp.reserve(_scriptText.size()); - - for (char c : _scriptText) { - if (c == '\0') break; - if (c == '\n') { - if (!tmp.empty() && tmp.back() == '\r') { tmp.pop_back(); } - _script.push_back(tmp); - tmp.clear(); - } - else { tmp.push_back(c); } - } - if (!tmp.empty()) { _script.push_back(tmp); } - - selectedLines.clear(); - lastClickedLine = -1; - - LOG_INFO("Command script loaded from file: %s", _currentScriptFile.c_str()); - D_INFO("Command script loaded from file: %s", _currentScriptFile.c_str()); - } - - // Run/Stop button for the command script - - const bool wasRunning = _sim->isScriptRunning(); // snapshot - - if (wasRunning) { - ImGui::PushStyleColor(ImGuiCol_Button, ImVec4(0.80f, 0.15f, 0.15f, 1.0f)); - ImGui::PushStyleColor(ImGuiCol_ButtonHovered, ImVec4(0.90f, 0.20f, 0.20f, 1.0f)); - ImGui::PushStyleColor(ImGuiCol_ButtonActive, ImVec4(0.70f, 0.10f, 0.10f, 1.0f)); - } - - if (ImGui::Button(_sim->isScriptRunning() ? "Stop" : "Run")) { - _sim->setScriptRunning(!_sim->isScriptRunning()); - LOG_INFO("Command script %s.", _sim->isScriptRunning() ? "started" : "stopped"); - D_INFO("Command script %s.", _sim->isScriptRunning() ? "started" : "stopped"); - - if (!_sim->isScriptRunning()) { - if (_program) _program->stop(); - _sim->setActiveProgram(nullptr); - _sim->stopSimulation(); - _sim->setScriptRunning(false); - } - else { - // If a script is already running, stop it and clean up before starting a new one - _sim->setActiveProgram(nullptr); - delete _wrapper; _wrapper = nullptr; - delete _parser; _parser = nullptr; - delete _program; _program = nullptr; - - // Create new program, parser, and wrapper instances - _program = new interpreter::StoredProgram(_sim->simCoreInterface()); - //_program->setDefaultObject(_sim->getObject()); - _parser = new interpreter::Parser(_program); - _wrapper = new interpreter::RunWrapper(_parser, _program); - - // Set the active program in the simulation manager before running - _sim->setActiveProgram(_program); - _sim->setScriptRunning(true); - _sim->setLastScriptText(_scriptText); - - // Remove trailing null character if present - std::string code = _scriptText; - if (!code.empty() && code.back() == '\0') code.pop_back(); - - _wrapper->runProgram(_scriptText); - } - } - ImGui::SameLine(); - // Background run button - if (ImGui::Button("Run (background)")) { - std::string code = _scriptText; - if (!code.empty() && code.back() == '\0') { code.pop_back(); } - std::string tag = filenameOnly(_currentScriptFile); - launchBackgroundRun(code, tag); - } - // Pop button style colors if we pushed them - if (wasRunning) { ImGui::PopStyleColor(3); } - // Check script status and handle termination conditions - if (_sim->isScriptRunning()) { - auto* prog = _sim->activeProgram(); - if (!prog) { terminateScript("Command script stopped -> active program is null.", true); return; } - - if (prog->isEmpty()) { terminateScript("Command script stopped -> program is empty.", true); return; } - if (prog->isFaulted()) { terminateScript("Command script stopped due to fault.", true); return; } - if (prog->isCompleted()) { terminateScript("Command script completed.", false); return; } - if (prog->isStopped() && !prog->isCompleted()) { terminateScript("Command script stopped.", true); return; } - } - } - - // Handler for the Run button -> launches the script in a background thread using the StudyRunner - void CommandScriptEditor::runButtonHandler() { - if (!ImGui::Button("Run (background)")) { return; } - - // Snapshot script text - std::string code = _scriptText; - if (!code.empty() && code.back() == '\0') { code.pop_back(); } - std::string tag = filenameOnly(_currentScriptFile); - - // Build single config (or multiple if desired) - std::vector configs; - StudyRunner::config c; - - // Read integration method and dt from the sim core (defaults will be used if not set in the sim core) - c.method = _sim->simCoreInterface()->integrationMethod(); - c.dt = _sim->simCoreInterface()->fixedDt(); - c.len_min = 1.0; - c.tag = tag; - configs.push_back(c); - - // Launch background thread - std::thread([this, configs, code]() { - auto results = _sim->studyRunner()->runStudies(configs, code); - _sim->pushCompletedStudies(std::move(results)); - }).detach(); - } - - void CommandScriptEditor::terminateScript(const char* reason, bool fault) { - _sim->setActiveProgram(nullptr); - _sim->stopSimulation(); - _sim->setScriptRunning(false); - - if (fault) { LOG_WARN("%s", reason); D_FAIL("%s", reason); } - else { LOG_INFO("%s", reason); D_SUCCESS("%s", reason); } - - delete _wrapper; _wrapper = nullptr; - delete _parser; _parser = nullptr; - delete _program; _program = nullptr; - } - - void CommandScriptEditor::render() { - ImGui::SetNextWindowPos(ImVec2(ImGui::GetIO().DisplaySize.x - 310, ImGui::GetIO().DisplaySize.y - 200), ImGuiCond_FirstUseEver); - ImGui::SetNextWindowSize(ImVec2(300, 200), ImGuiCond_FirstUseEver); - - ImGui::PushStyleColor(ImGuiCol_WindowBg, ImVec4(0.129f, 0.129f, 0.129f, 0.8f)); - ImGui::Begin("Command Code Editor", nullptr, ImGuiWindowFlags_NoCollapse | ImGuiWindowFlags_NoScrollbar | ImGuiWindowFlags_NoTitleBar); - - beginEditorPanel("ScriptEditorPanel"); - - if (ImGui::BeginTabBar("Debug Tabs")) { - if (ImGui::BeginTabItem("Code Editor")) { - renderEnvironment(); - ImGui::EndTabItem(); - } - - if (ImGui::BeginTabItem("Syntax List")) { - renderCmdInstructions(); - ImGui::EndTabItem(); - } - ImGui::EndTabBar(); - } - - endEditorPanel(); - - ImGui::End(); - ImGui::PopStyleColor(); - } - - void CommandScriptEditor::renderEnvironment() { - const std::string fileLabel = filenameOnly(_currentScriptFile); - - // Header info - ImGui::AlignTextToFramePadding(); - ImGui::Text("Script:"); - ImGui::SameLine(); - ImGui::TextDisabled("%s", fileLabel.c_str()); - - ImGui::Separator(); - - const float statusH = ImGui::GetFrameHeightWithSpacing() + ImGui::GetStyle().ItemSpacing.y; - ImGui::BeginChild("EditorScroll", ImVec2(0, -statusH), false, - ImGuiWindowFlags_AlwaysHorizontalScrollbar); - - ImGui::PushStyleColor(ImGuiCol_FrameBg, ImVec4(0.1f, 0.1f, 0.1f, 0.925f)); - // Ensure _scriptText has at least 1 char so data() is valid for ImGui - if (_scriptText.empty()) _scriptText.push_back('\0'); - - - ImGuiInputTextFlags flags = - ImGuiInputTextFlags_AllowTabInput | - ImGuiInputTextFlags_CallbackResize; - - if (_scriptText.empty() || _scriptText.back() != '\0') { _scriptText.push_back('\0'); } - - ImGui::InputTextMultiline( - "##editor", - _scriptText.data(), - _scriptText.capacity() + 1, - ImVec2(-FLT_MIN, -FLT_MIN), - flags, - textResizeCallback, - &_scriptText - ); - - ImGui::PopStyleColor(); - ImGui::EndChild(); - - // Footer status - ImGui::Separator(); - - // Count lines - int lineCount = 0; - for (char c : _scriptText) { - if (c == '\0') break; - if (c == '\n') ++lineCount; - } - // If there's any content, lines = newlines + 1 - if (!_scriptText.empty() && _scriptText[0] != '\0') ++lineCount; - - - // Render status info - ImGui::TextDisabled("Lines: %d", lineCount); - ImGui::SameLine(); ImGui::TextDisabled(" | "); - ImGui::SameLine(); ImGui::TextDisabled("Chars: %d", (int)_scriptText.size()); - ImGui::SameLine(); ImGui::TextDisabled(" | "); - ImGui::SameLine(); ImGui::TextDisabled("State: %s", _sim->isScriptRunning() ? "Running" : "Idle"); - } - - bool CommandScriptEditor::tryLoadFromDialog() { - _load.Display(); - if (!_load.HasSelected()) { return false; } - - const std::string filePath = _load.GetSelected().string(); - _load.ClearSelected(); - - FILE* file = nullptr; - if (fopen_s(&file, filePath.c_str(), "r") != 0 || !file) { - LOG_ERROR("Failed to open script file: %s", filePath.c_str()); - return false; - } - - _scriptText.clear(); - - char lineBuf[1024]; - while (fgets(lineBuf, sizeof(lineBuf), file)) { - _scriptText += lineBuf; - } - fclose(file); - - // Ensure null-termination - if (_scriptText.empty() || _scriptText.back() != '\0') { - _scriptText.push_back('\0'); - } - - _currentScriptFile = filePath; - _currentScriptPath = filePath.substr(0, filePath.find_last_of("/\\")); - - _pendingSavePath = _currentScriptPath; - - LOG_INFO("Command script loaded from file: %s", _currentScriptFile.c_str()); - D_SUCCESS("Command script loaded from file: %s", _currentScriptFile.c_str()); - - return true; - } - - bool CommandScriptEditor::trySaveScriptToFile(const std::string& filepath) { - FILE* file = nullptr; - if (fopen_s(&file, filepath.c_str(), "w") != 0 || !file) { - LOG_ERROR("Failed to save to script file for writing: %s", filepath.c_str()); - return false; - } - - size_t n = _scriptText.size(); - if (n > 0 && _scriptText.back() == '\0') n -= 1; - fwrite(_scriptText.data(), 1, n, file); - fclose(file); - - LOG_INFO("Command script saved to file: %s", filepath.c_str()); - D_SUCCESS("Command script saved to file: %s", filepath.c_str()); - - return true; - } - - // Poll active background runs for completion and forward results to SimManager - void CommandScriptEditor::pollRuns() { - std::lock_guard lk(_activeRunsMutex); - // Iterate over active runs and check for completion - for (auto it = _activeRuns.begin(); it != _activeRuns.end(); ) { - auto& ar = *it; - if (ar.fut.valid() && ar.fut.wait_for(std::chrono::milliseconds(0)) == std::future_status::ready) { - StudyResult r; - try { r = ar.fut.get(); } - catch (const std::exception& e) { - // convert exception into a failed StudyResult - r.success = false; - r.tag = ar.tag; - } - // forward to SimManager (thread-safe push) - if (_sim) { _sim->pushCompletedStudy(std::move(r)); } - it = _activeRuns.erase(it); - } - else { - ++it; - } - } - } - - // Launch a background run of the current script (for studies) - captures code and tag by value for thread safety - void CommandScriptEditor::launchBackgroundRun(const std::string& code, const std::string& tag) { - // capture code and tag by value -> worker owns its own SimulationCore and program - std::future f = std::async(std::launch::async, [code, tag]() -> StudyResult { - StudyResult r{}; - r.tag = tag; - - // Create fresh simulation core for worker thread - core::ISimulationCore* raw = CreateSimulationCore_v1(); - if (!raw) { r.success = false; return r; } - CorePtr core(raw, [](core::ISimulationCore* p) { DestroySimulationCore(p); }); - - // bind program+parser to the new core - auto program = std::make_unique(core.get()); - interpreter::Parser parser(program.get()); - parser.parse(code); - program->start(); - - // Choose integration method (could be read from script or set as default) - integration::eIntegrationMethod method = integration::eIntegrationMethod::RK4; // NOTE: we need to infer integrator from script or choose a default (RK4 is default everywhere!) - - // Block inside worker thread until completion - bool ok = core->runScriptToCompletion(program.get(), method); - - // Fill results - r.success = ok; - r.intName = core->integrationMethodName(); - r.simTime = core->simTime(); - try { r.samples = core->telemetrySampleCount(); } - catch (...) { r.samples = 0; } - return r; - }); - - // register active run - { - std::lock_guard lk(_activeRunsMutex); - _activeRuns.push_back(ActiveRun{ std::move(f), tag }); - } - } - - void CommandScriptEditor::renderSaveAsPopup() { - // Window Padding - ImGui::PushStyleVar(ImGuiStyleVar_WindowPadding, ImVec2(16.0f, 12.0f)); - - // Save As popup - if (!ImGui::BeginPopupModal("Save Command Script", nullptr, ImGuiWindowFlags_AlwaysAutoResize)) { ImGui::PopStyleVar(); return; } - - // Display current directory - ImGui::Text("Directory: "); - ImGui::SameLine(); - ImGui::TextUnformatted(_pendingSavePath.c_str()); - - ImGui::Spacing(); - - if (ImGui::Button("Choose Folder...")) { _save.Open(); } - - _save.Display(); - if (_save.HasSelected()) { - _pendingSavePath = _save.GetSelected().string(); - _save.ClearSelected(); - } - - ImGui::Spacing(); - - // Filename input - static char filenameBuf[256] = {}; - if (_pendingSaveName.empty()) { - _pendingSaveName = "script.scl"; - std::snprintf(filenameBuf, sizeof(filenameBuf), "%s", _pendingSaveName.c_str()); - } - - if (ImGui::InputText("Filename", filenameBuf, sizeof(filenameBuf))) { _pendingSaveName = filenameBuf; } - - ImGui::SectionDivider(); - - if (ImGui::Button("Save")) { - std::string fullPath = _pendingSavePath; - - if (!fullPath.empty() && fullPath.back() != '/' && fullPath.back() != '\\') { fullPath += "/"; } - - fullPath += _pendingSaveName; - - if (trySaveScriptToFile(fullPath)) { - _currentScriptFile = fullPath; - _currentScriptPath = _pendingSavePath; - LOG_INFO("Command script saved to file: %s", _currentScriptFile.c_str()); - D_SUCCESS("Command script saved to file: %s", _currentScriptFile.c_str()); - - ImGui::CloseCurrentPopup(); - } - else { LOG_ERROR("Failed to save command script to file: %s", fullPath.c_str()); } - } - - ImGui::SameLine(); - if (ImGui::Button("Cancel")) { ImGui::CloseCurrentPopup(); } - - ImGui::EndPopup(); - ImGui::PopStyleVar(); - - } - - // Helper function to render inline colored text - static void TextInlineColored(const ImVec4& color, const char* text) { - ImGui::SameLine(0.0f, 0.0f); - ImGui::TextColored(color, "%s", text); - } - - // Render the command syntax instructions - void CommandScriptEditor::renderCmdInstructions() { - ImGui::BeginChild("CmdInstructions", ImVec2(0, -30), true, ImGuiWindowFlags_HorizontalScrollbar | ImGuiWindowFlags_AlwaysVerticalScrollbar); - - ImGui::SectionHeader("DSL Command Format:"); - ImGui::Spacing(); - - ImGui::TextColored(CMD_COL, "command"); - TextInlineColored(ARG_COL, "(identifier, arg1, arg2, ...)"); - TextInlineColored(DESC_COL, " \'#\' Denotes a comment"); - - ImGui::Spacing(); - - ImGui::TextDisabled("Notes:"); - ImGui::BulletText("Commands and identifiers are case-insensitive (parser lowercases)."); - ImGui::BulletText("Strings can be quoted: \"...\" to allow spaces in paths."); - ImGui::BulletText("Inline comments use '#': rotate(obj, 30, 0, 90) # quarter turn"); - - ImGui::Spacing(); - ImGui::SectionHeader("DSL Command List:"); - ImGui::Spacing(); - - // --- TRAJSET --- - ImGui::TextColored(CMD_COL, "trajSet"); - TextInlineColored(ARG_COL, "(, , )"); - if (ImGui::IsItemHovered()) { - ImGui::BeginTooltip(); - TextInlineColored(DESC_COL, "# Sets a trajectory for robotic arm joint (J_i) of some type with parameters"); - ImGui::TextDisabled("Types and parameters:"); - ImGui::TextDisabled(" • : TRAP, SINE, MSINE"); - ImGui::TextDisabled(" • TRAP, : q(°), vmax(°/s), amax(°/s²)"); - ImGui::TextDisabled(" • SINE, : duration(s), center(°), amp(°), freq(Hz) [, phase(°)]"); - ImGui::TextDisabled(" • MSINE, : duration(s), center(°) amp1(°), f1(Hz), ph1(°), amp2, f2, ph2, ..."); - ImGui::EndTooltip(); - } - - ImGui::SectionDivider(); - - // --- TRAJCLEAR --- - ImGui::TextColored(CMD_COL, "trajClear"); - if (ImGui::IsItemHovered()) { - ImGui::BeginTooltip(); - ImGui::TextDisabled("No arguments."); - TextInlineColored(DESC_COL, "# Clears any trajectory set for robotic arm joints"); - ImGui::EndTooltip(); - } - TextInlineColored(ARG_COL, "()"); - - // --- ROTATEJOINTTO --- - ImGui::TextColored(CMD_COL, "rotateJointTo"); - TextInlineColored(ARG_COL, "(, <°w^(-1)>, )"); - - if (ImGui::IsItemHovered()) { - ImGui::BeginTooltip(); - TextInlineColored(DESC_COL, "# Rotates robotic arm joint (J) to some angle (°) by some max angular velocity (°w^(-1))"); - ImGui::EndTooltip(); - } - - ImGui::SectionDivider(); - - // --- ROTATEJOINTBY --- - ImGui::TextColored(CMD_COL, "rotateJointBy"); - TextInlineColored(ARG_COL, "(, <°w^(-1)>, )"); - if (ImGui::IsItemHovered()) { - ImGui::BeginTooltip(); - TextInlineColored(DESC_COL, "# Rotates robotic arm joint (J_i) by some delta angle (°) by some angular velocity (°w^(-1))"); - ImGui::EndTooltip(); - } - - ImGui::SectionDivider(); - - // --- ROTATETO --- - ImGui::TextColored(CMD_COL, "rotateTo"); - TextInlineColored(ARG_COL, "(, <{x,y,z}>, <°ω⁻¹>, )"); - if (ImGui::IsItemHovered()) { - ImGui::BeginTooltip(); - TextInlineColored(DESC_COL, "# Rotates a rigid body (B) to some angle (°) by some max angular velocity (°w^(-1)) across some axis ({x,y,z})"); - ImGui::EndTooltip(); - } - - ImGui::SectionDivider(); - - // --- ROTATEBY --- - ImGui::TextColored(CMD_COL, "rotateBy"); - TextInlineColored(ARG_COL, "(, <{x,y,z}>, <°w^(-1)>, )"); - if (ImGui::IsItemHovered()) { - ImGui::BeginTooltip(); - TextInlineColored(DESC_COL, "# Rotates a rigid body (B) by some delta angle (°) at some angular velocity (°w^(-1)) across some axis ({x,y,z})"); - ImGui::EndTooltip(); - } - - ImGui::SectionDivider(); - - // --- LOAD --- - ImGui::TextColored(CMD_COL, "load"); - if (ImGui::IsItemHovered()) { - ImGui::BeginTooltip(); - TextInlineColored(DESC_COL, "# Load some single or chain rigid body model"); - ImGui::EndTooltip(); - } - TextInlineColored(ARG_COL, "(, )"); - if (ImGui::IsItemHovered()) { - ImGui::BeginTooltip(); - ImGui::TextDisabled("Identifiers & Arguments:"); - ImGui::TextDisabled(" • \"Robot\" -> args: \"Z1\", \"UR5e\", \"Panda\", \"iiwa14\", \"VISPA\", \"H1\", \"some\\path\\to\\robot.json\""); - ImGui::TextDisabled(" • \"Obj\" -> args: \"cube\", \"circle\", \"some\\path\\to\\object\\location.fbx\""); - ImGui::EndTooltip(); - } - - ImGui::SectionDivider(); - - // --- SET --- - ImGui::TextColored(CMD_COL, "set"); - if (ImGui::IsItemHovered()) { - ImGui::BeginTooltip(); - TextInlineColored(DESC_COL, "# Set some identifier by a value in its respective format"); - ImGui::EndTooltip(); - } - TextInlineColored(ARG_COL, "(, )"); - if (ImGui::IsItemHovered()) { - ImGui::BeginTooltip(); - ImGui::TextDisabled("Identifiers & Arguments:"); - ImGui::TextDisabled(" • \"integrator\" -> args:\n\t\t\t\t # (Explicit) \"euler\", \"midpoint\", \"heun\", \"ralston\", \"rk4\", \"rk45\"\n\t\t\t\t # (Implicit) \"implicit_euler\", \"implicit_midpoint\", \"glrk2\", \"glrk3\","); - ImGui::TextDisabled(" • \"dt\" -> args: \"numerical val\""); - ImGui::TextDisabled(" • \"omega\" -> args: \"numerical val\""); - ImGui::TextDisabled(" • \"colour\" -> args: \"{0.0-1.0, 0.0-1.0, 0.0-1.0}\", \"#ffffff\", \"red\""); - ImGui::EndTooltip(); - } - - ImGui::SectionDivider(); - - // --- START --- - ImGui::TextColored(CMD_COL, "start"); - if (ImGui::IsItemHovered()) { - ImGui::BeginTooltip(); - TextInlineColored(DESC_COL, "# Start the simulation run"); - ImGui::TextDisabled("No arguments."); - ImGui::TextDisabled("Note: 'start' must be called to begin a simulation run after loading models and setting parameters."); - ImGui::EndTooltip(); - } - TextInlineColored(ARG_COL, "()"); - - ImGui::SectionDivider(); - - // --- STOP --- - ImGui::TextColored(CMD_COL, "stop"); - if (ImGui::IsItemHovered()) { - ImGui::BeginTooltip(); - TextInlineColored(DESC_COL, "# Stops the simulation"); - ImGui::TextDisabled("No arguments."); - ImGui::TextDisabled("Note: \'stop\' halts the simulation run but does not reset loaded models or parameters."); - ImGui::EndTooltip(); - } - TextInlineColored(ARG_COL, "()"); - - ImGui::SectionDivider(); - - // --- WAIT --- - ImGui::TextColored(CMD_COL, "wait"); - if (ImGui::IsItemHovered()) { - ImGui::BeginTooltip(); - TextInlineColored(DESC_COL, "# Pauses script execution for some time (s)"); - ImGui::TextDisabled("Argument: time in seconds (s)."); - ImGui::EndTooltip(); - } - TextInlineColored(ARG_COL, "()"); - - ImGui::SectionDivider(); - - // --- SELECT --- - ImGui::TextColored(CMD_COL, "select"); - if (ImGui::IsItemHovered()) { - ImGui::BeginTooltip(); - TextInlineColored(DESC_COL, "# Selects a rigid body or joint by its identifier"); - ImGui::TextDisabled("Argument: identifier string of the object to select."); - ImGui::EndTooltip(); - } - TextInlineColored(ARG_COL, "()"); - - ImGui::Text("DSL Parallel Execution:"); - - // --- PARALLEL --- - ImGui::TextColored(CMD_COL, "parallel"); - if (ImGui::IsItemHovered()) { - ImGui::BeginTooltip(); - TextInlineColored(DESC_COL, "# Begins a parallel block where commands run concurrently"); - ImGui::TextDisabled("Argument: timeout in seconds (s) for the parallel block to auto-complete."); - ImGui::EndTooltip(); - } - TextInlineColored(ARG_COL, "()"); - - ImGui::EndChild(); - } - - void CommandScriptEditor::beginEditorPanel(const char* id) { - ImGui::PushStyleVar(ImGuiStyleVar_WindowPadding, ImVec2(12.0f, 10.0f)); - ImGui::PushStyleVar(ImGuiStyleVar_ChildRounding, 6.0f); - ImGui::PushStyleVar(ImGuiStyleVar_FramePadding, ImVec2(8.0f, 6.0f)); - ImGui::PushStyleVar(ImGuiStyleVar_ItemSpacing, ImVec2(10.0f, 8.0f)); - ImGui::BeginChild(id, ImVec2(0, 0), true, ImGuiChildFlags_AlwaysUseWindowPadding); - } - - void CommandScriptEditor::endEditorPanel() { - ImGui::EndChild(); - ImGui::PopStyleVar(4); - } -} \ No newline at end of file diff --git a/DSFE_App/DSFE_GUI/src/ui/ControlPanel.cpp b/DSFE_App/DSFE_GUI/src/ui/ControlPanel.cpp deleted file mode 100644 index 50f2b096..00000000 --- a/DSFE_App/DSFE_GUI/src/ui/ControlPanel.cpp +++ /dev/null @@ -1,1861 +0,0 @@ -// DSFE_GUI ControlPanel.cpp -#include "ui/ControlPanel.h" - -#ifdef __gl_h_ -#undef __gl_h_ -#endif -#include -#include - -#include - -#include "Scene/Mesh.h" -#include "Scene/Object.h" -#include "Scene/Light.h" -#include "Scene/Camera.h" - -#include "Scene/SimulationManager.h" -#include "Robots/RobotSystem.h" -#include "Interpreter/Parser.h" - -#include "Platform/imguiWidgets.h" -#include - -#include "Analysis/Telemetry.h" -#include "Platform/Paths.h" -#include "EngineLib/LogMacros.h" - -namespace gui { - // --- Helper Functions --- - - // Get human-readable name for gravity level - static const char* gravityLevelName(GravityLevel level) { - switch (level) { - case GravityLevel::Root: return "Presets"; - case GravityLevel::SolarSystem: return "Solar System"; - case GravityLevel::Planets: return "Planets"; - case GravityLevel::Moons: return "Moons"; - default: return ""; - } - } - - // Gravity value lookup from preset - static double gravityFromPreset(GravityPreset p) { - switch (p) { - case PRESET_ZERO_G: return constants::g_zero; - case PRESET_MICRO_G: return constants::g_micro; - - case PRESET_SUN: return constants::g_Sun; - case PRESET_MERCURY: return constants::g_Mercury; - case PRESET_VENUS: return constants::g_Venus; - case PRESET_EARTH: return constants::g_Earth; - case PRESET_MARS: return constants::g_Mars; - case PRESET_JUPITER: return constants::g_Jupiter; - case PRESET_SATURN: return constants::g_Saturn; - case PRESET_URANUS: return constants::g_Uranus; - case PRESET_NEPTUNE: return constants::g_Neptune; - case PRESET_PLUTO: return constants::g_Pluto; - - case PRESET_MOON: return constants::g_Moon; - case PRESET_TITAN: return constants::g_Titan; - case PRESET_ENCELADUS: return constants::g_Enceladus; - case PRESET_EUROPA: return constants::g_Europa; - case PRESET_GANYMEDE: return constants::g_Ganymede; - case PRESET_IO: return constants::g_Io; - - default: return constants::g_Earth; - } - } - - // Draw the gravity preset combo menu - static void drawGravityChoiceMenu(GravityPreset& preset, double& g) { - // Combo label shows navigation state - char label[64]; - snprintf(label, sizeof(label), "Gravity / %s", gravityLevelName(gravityLevel)); - - ImGui::Text("Gravity Preset"); - ImGui::SetNextItemWidth(175.0f); - - ImGui::PushStyleColor(ImGuiCol_Text, ImVec4(0.7f, 0.8f, 1.0f, 1.0f)); - - if (ImGui::BeginCombo("##GravityCombo", label)) { - - // ---------------- ROOT ---------------- - if (gravityLevel == GravityLevel::Root) { - - if (ImGui::Selectable("Zero-G")) { - preset = PRESET_ZERO_G; - g = gravityFromPreset(preset); - ImGui::CloseCurrentPopup(); - } - - if (ImGui::Selectable("Micro-G")) { - preset = PRESET_MICRO_G; - g = gravityFromPreset(preset); - ImGui::CloseCurrentPopup(); - } - - ImGui::Separator(); - - if (ImGui::Selectable("Solar System >", false, ImGuiSelectableFlags_DontClosePopups)) { - gravityLevel = GravityLevel::SolarSystem; - } - - if (ImGui::Selectable("Custom")) { - preset = PRESET_CUSTOM; - gravityMode = GravityUIMode::Custom; - ImGui::CloseCurrentPopup(); - } - } - - // ---------------- SOLAR SYSTEM ---------------- - else if (gravityLevel == GravityLevel::SolarSystem) { - - if (ImGui::Selectable("< Back", false, ImGuiSelectableFlags_DontClosePopups)) { - gravityLevel = GravityLevel::Root; - } - - if (ImGui::Selectable("Planets >", false, ImGuiSelectableFlags_DontClosePopups)) { - gravityLevel = GravityLevel::Planets; - } - - if (ImGui::Selectable("Moons >", false, ImGuiSelectableFlags_DontClosePopups)) { - gravityLevel = GravityLevel::Moons; - } - } - - // ---------------- PLANETS ---------------- - else if (gravityLevel == GravityLevel::Planets) { - - if (ImGui::Selectable("< Back")) { - gravityLevel = GravityLevel::SolarSystem; - } - - ImGui::Separator(); - - struct { const char* name; GravityPreset p; } planets[] = { - { "Sun", PRESET_SUN }, - { "Mercury", PRESET_MERCURY }, - { "Venus", PRESET_VENUS }, - { "Earth", PRESET_EARTH }, - { "Mars", PRESET_MARS }, - { "Jupiter", PRESET_JUPITER }, - { "Saturn", PRESET_SATURN }, - { "Uranus", PRESET_URANUS }, - { "Neptune", PRESET_NEPTUNE }, - { "Pluto", PRESET_PLUTO } - }; - - // List planets - for (auto& p : planets) { - if (ImGui::Selectable(p.name)) { - preset = p.p; - g = gravityFromPreset(preset); - } - } - } - - // ---------------- MOONS ---------------- - else if (gravityLevel == GravityLevel::Moons) { - if (ImGui::Selectable("< Back")) { - gravityLevel = GravityLevel::SolarSystem; - } - - ImGui::Separator(); - - struct { const char* name; GravityPreset p; } moons[] = { - { "Moon (Earth)", PRESET_MOON }, - { "Titan", PRESET_TITAN }, - { "Enceladus", PRESET_ENCELADUS }, - { "Europa", PRESET_EUROPA }, - { "Ganymede", PRESET_GANYMEDE }, - { "Io", PRESET_IO } - }; - - // List moons - for (auto& m : moons) { - if (ImGui::Selectable(m.name)) { - preset = m.p; - g = gravityFromPreset(preset); - } - } - } - ImGui::EndCombo(); - } - ImGui::PopStyleColor(); - } - - // Begin Control Panel Helper - void ControlPanel::beginControlPanel(const char* id, ImVec2 size) { - ImGui::PushStyleVar(ImGuiStyleVar_WindowPadding, ImVec2(12.0f, 10.0f)); - ImGui::PushStyleVar(ImGuiStyleVar_ChildRounding, 6.0f); - ImGui::PushStyleVar(ImGuiStyleVar_FramePadding, ImVec2(8.0f, 5.0f)); - ImGui::PushStyleVar(ImGuiStyleVar_ItemSpacing, ImVec2(10.0f, 8.0f)); - - ImGui::BeginChild(id, size, true, ImGuiChildFlags_AlwaysUseWindowPadding); - } - - // End Control Panel Helper - void ControlPanel::endControlPanel() { - ImGui::EndChild(); - ImGui::PopStyleVar(4); - } - - // --- ControlPanel Implementation --- - - ControlPanel::ControlPanel(SimManager* sim) : - _sim(sim), _controlMode(&sim->ctrlMode), _obj(nullptr), _light(nullptr), - r(render::ResolutionPreset::R_1080p), q(render::QualityPreset::Medium), - _meshLoad(ImGuiFileBrowserFlags_CloseOnEsc | ImGuiFileBrowserFlags_NoModal), - _hdrLoad(ImGuiFileBrowserFlags_CloseOnEsc | ImGuiFileBrowserFlags_NoModal), - _useAutoDiff(false) - { - diagTime = 0.0f; // initialise diagnostic time - simTime = 0.0f; // initialise simulation time - - diagRunning = false; - simulationRunning = false; - - // File browsers - _currentMeshFile = "<...>"; - _currentHDRFile = "<...>"; - // Mesh loader - _meshLoad.SetTitle("Open Object Model"); - _meshLoad.SetDirectory((paths::assets() / "objects").string()); - _meshLoad.SetTypeFilters({ ".fbx", ".obj", ".dae", ".stl"}); - // HDR loader - _hdrLoad.SetTitle("Load HDR Environment"); - _hdrLoad.SetDirectory((paths::assets() / "hdr").string()); - _hdrLoad.SetTypeFilters({ ".hdr", ".exr" }); - - // One-time ImPlot styling - static bool plotStyled = false; - if (!plotStyled) { - ImPlotStyle& style = ImPlot::GetStyle(); - ImVec4* colors = style.Colors; - - style.LineWeight = 1.1f; - style.PlotPadding = ImVec2(14, 12); - style.LabelPadding = ImVec2(6, 4); - style.LegendPadding = ImVec2(6, 4); - style.FitPadding = ImVec2(0.05f, 0.05f); - - colors[ImPlotCol_PlotBg] = ImVec4(0.129f, 0.129f, 0.129f, 1.0f); - colors[ImPlotCol_PlotBorder] = ImVec4(0.3f, 0.3f, 0.3f, 1.0f); - colors[ImPlotCol_AxisGrid] = ImVec4(0.32f, 0.32f, 0.32f, 0.7f); - colors[ImPlotCol_AxisText] = ImVec4(0.85f, 0.85f, 0.85f, 1.0f); - colors[ImPlotCol_AxisTick] = ImVec4(0.75f, 0.75f, 0.75f, 1.0f); - colors[ImPlotCol_LegendBg] = ImVec4(0.129f, 0.129f, 0.129f, 0.9f); - colors[ImPlotCol_LegendBorder] = ImVec4(0.4f, 0.4f, 0.4f, 0.6f); - - plotStyled = true; - } - } - - void ControlPanel::drawMenus(SimManager* sim) { - _sim = sim; - _obj = _sim->getObject(); - - if (ImGui::BeginMenu("File")) { - if (ImGui::MenuItem("Save Layout")) { - ImGui::SaveIniSettingsToDisk((paths::configs() / "imgui.ini").string().c_str()); - ImGui::SaveIniSettingsToDisk("imgui.ini"); - } - if (ImGui::MenuItem("Load Layout")) { - ImGui::LoadIniSettingsFromDisk((paths::configs() / "imgui.ini").string().c_str()); - } - ImGui::Separator(); - if (ImGui::MenuItem("Load HDR")) { _hdrLoad.Open(); /*LOG_INFO("HDR file dialog opened");*/ } - ImGui::EndMenu(); - } - - if (ImGui::BeginMenu("Edit")) { - if (ImGui::MenuItem("Reset View")) { - _sim->resetView(); - LOG_INFO("Scene view reset to default position and orientation."); - } - if (ImGui::MenuItem("Reset HDR")) { - _sim->resetHDRToPreset(); - } - ImGui::Separator(); - if (ImGui::MenuItem("Properties")) { - // Placeholder for future properties dialog - } - ImGui::EndMenu(); - } - - if (ImGui::BeginMenu("Project")) { - if (ImGui::MenuItem("Load Obj")) { _meshLoad.Open(); /*LOG_INFO("File dialog opened");*/ } - if (ImGui::MenuItem("Load Robotic Arm")) { _showRobotSelector = true; /*LOG_INFO("Robotic Arm Menu Opened");*/ } - - ImGui::Separator(); - const bool canShowResults = (_sim->telemetry().ring.size() >= 2); - if (ImGui::MenuItem("View Results", nullptr, false, canShowResults)) { - _showResultsWindow = true; - _resultsFocusNeeded = true; - } - - ImGui::EndMenu(); - } - } - - void ControlPanel::render(SimManager* sceneView) { - _sim = sceneView; // update internal pointer to current SimManager - pollComparisonFutures(); // poll any pending comparison results and update state accordingly - fov = _sim->getCamera()->getFOVRadians(); - - // Update pointers to current scene data - _mesh = _sim->getMesh(); - _obj = _sim->getObject(); - _light = _sim->getLight(); - _hasRobot = _sim->hasRobot(); - - // Begin Control Panel Window - ImGui::SetNextWindowPos(ImVec2(10, 10), ImGuiCond_FirstUseEver); - ImGui::SetNextWindowSize(ImVec2(300, 400), ImGuiCond_FirstUseEver); - ImGui::PushStyleColor(ImGuiCol_WindowBg, ImVec4(0.129f, 0.129f, 0.129f, 0.8f)); - ImGui::Begin("Control Panel", nullptr, ImGuiWindowFlags_NoCollapse | ImGuiWindowFlags_NoTitleBar); - const bool wasRunning = _sim->isSimRunning(); // snapshot - - // Simulation Start/Stop Button - if (_sim->isSimRunning()) { - if (wasRunning && !_sim->isSimRunning()) { _sim->setSimTime(0.0f); } // reset time if just stopped - LOG_INFO_ONCE("Simulation %s", _sim->isSimRunning() ? "started" : "stopped"); - D_RUNTIME_ONCE("Simulation %s", _sim->isSimRunning() ? "started" : "stopped"); - } - - beginControlPanel("ControlPanel"); // Begin Decorative Child Panel - - // Tab bar for organizing control sections - roboticArmSelector(); // Draw robotic arm selector (if enabled) - if (ImGui::BeginTabBar("ControlPanelTabs")) { - if (ImGui::BeginTabItem("Rigid Body Properties")) { - simulationProperties(); - objectProperties(); - ImGui::EndTabItem(); - } - if (ImGui::BeginTabItem("Multi-Body Properties")) { - simulationProperties(); - jointProperties(); - ImGui::EndTabItem(); - } - if (ImGui::BeginTabItem("Display Settings")) { - displaySettings(); - // Temporary light controls (for testing) - // tempLightControls(); - ImGui::EndTabItem(); - } - ImGui::EndTabBar(); - } - endControlPanel(); // End Decorative Child Panel - - ImGui::End(); - ImGui::PopStyleColor(); - - sceneObjectsTable(); - - // Detect simulation completion: was running last frame, stopped this frame - { - const bool runningNow = _sim->isSimRunning(); - if (_simWasRunningLastFrame && !runningNow) { - LOG_INFO("Simulation stopped detected (edge). Ring size: %zu", _sim->telemetry().ring.size()); - if (_sim->telemetry().ring.size() >= 2) { - _selectResultsTab = true; - _showResultsWindow = true; - _resultsFocusNeeded = true; - LOG_INFO("Auto-opening Results window."); - } - } - _simWasRunningLastFrame = runningNow; - } - drawResultsWindow(); // Draw separate results window (if open) - - // Handle file dialogs for mesh loading - _meshLoad.Display(); - if (_meshLoad.HasSelected()) { - auto file_path = _meshLoad.GetSelected().string(); - _currentMeshFile = file_path.substr(file_path.find_last_of("/\\") + 1); - meshLoadCallback(file_path); - LOG_INFO("Mesh loaded from file: %s", _currentMeshFile.c_str()); - D_SUCCESS("Mesh loaded from file: %s", _currentMeshFile.c_str()); - - _meshLoad.ClearSelected(); - } - // Handle file dialog for HDR loading - _hdrLoad.Display(); - if (_hdrLoad.HasSelected()) { - auto file_path = _hdrLoad.GetSelected().string(); - _currentHDRFile = file_path.substr(file_path.find_last_of("/\\") + 1); - _sim->loadNewHDR_UI(file_path); - LOG_INFO("HDR loaded from file: %s", _currentHDRFile.c_str()); - D_SUCCESS("HDR loaded from file: %s", _currentHDRFile.c_str()); - _hdrLoad.ClearSelected(); - } - } - - // Temporary light controls for testing and debugging - void ControlPanel::tempLightControls() { - if (!_light) return; - ImGui::SeparatorText("Light Settings:"); - ImGui::Text("Intensity"); - ImGui::SetNextItemWidth(150.0f); - ImGui::DragFloat("##intensity", &_light->_intensity, 0.1f, 0.0f, 100.0f, "%.1f"); - ImGui::Text("Color"); - ImGui::SetNextItemWidth(150.0f); - ImGui::ColorEdit3("##Colour", glm::value_ptr(_light->_colour)), ImGui::SameLine(); - ImGui::Separator(); - ImGui::Text("Direction"); - ImGui::SetNextItemWidth(150.0f); - ImGui::DragFloat3("##direction", &_light->_direction.x, 0.1f, -25.0f, 25.0f, "%.2f"); - ImGui::Separator(); - ImGui::Text("Position"); - ImGui::SetNextItemWidth(150.0f); - ImGui::DragFloat3("##position", &_light->_position.x, 0.1f, -100.0f, 100.0f, "%.1f"); - ImGui::Separator(); - } - - // Simulation properties controls, including integration method, torque mode, and delta time settings - void ControlPanel::simulationProperties() { - ImGui::SectionHeader("Simulation Settings"); - ImGui::SectionDivider(); - - robots::RobotSystem* robot = _sim->robotSystem(); - auto currentIntEnum = _sim->integrationMethod(); - auto currentIntEnum_AD = _sim->autoDiffIntegrationMethod(); - auto currentTauEnum = robot->getTorqueMode(); - - static const char* intMethodNames[] = { "Euler", "Midpoint", "Heun", "Ralston", "RK4", "RK45", "Implicit Euler", "Implicit Midpoint", "GLRK2", "GLRK3" }; - const char* currentIntMethod = intMethodNames[static_cast(currentIntEnum)]; - - static const char* intMethodNames_AD[] = { "Implicit Euler (AutoDiff)", "Implicit Midpoint (AutoDiff)", "GLRK2 (AutoDiff)", "GLRK3 (AutoDiff)" }; - const char* currentIntMethod_AD = intMethodNames_AD[static_cast(currentIntEnum_AD)]; - - static const char* torqueModeNames[] = { "None", "Passive", "Controlled" }; - const char* currentTorqueMode = torqueModeNames[static_cast(currentTauEnum)]; - - - // Disable controls while sim is running to prevent conflicts and ensure stability of the simulations - ImGui::BeginDisabled(_sim->isSimRunning()); - - // Integration method combo box - ImGui::SectionHeader("Integration Methods"); - - ImGui::SetNextItemWidth(150.0f); - ImGui::Checkbox("Enable Automatic Differentiable Integrators", &_useAutoDiff); - robot->enableAutoDiff(_useAutoDiff); - - ImGui::SetNextItemWidth(150.0f); - if (_useAutoDiff) { - if (ImGui::BeginCombo("##", currentIntMethod_AD)) { - for (int n = 0; n < IM_ARRAYSIZE(intMethodNames_AD); ++n) { - bool isSelected = (n == static_cast(currentIntEnum_AD)); - - // When a new method is selected, update the robot's integration method and log the change - if (ImGui::Selectable(intMethodNames_AD[n], isSelected)) { - auto updatedMethod_AD = static_cast(n); - auto state = robot->runtimeIntegratorState(); - state->autoDiff = true; // ensure autodiff flag is set in state - _sim->setADIntegrationMethod(updatedMethod_AD); - - switch (updatedMethod_AD) { - case integration::eAutoDiffIntegrationMethod::AD_ImplicitEuler: - D_INFO("Integrator set to Implicit Euler (AutoDiff)"); break; - case integration::eAutoDiffIntegrationMethod::AD_ImplicitMidpoint: - D_INFO("Integrator set to Implicit Midpoint (AutoDiff)"); break; - case integration::eAutoDiffIntegrationMethod::AD_GLRK2: - D_INFO("Integrator set to GLRK2 (AutoDiff Gauss-Legendre Runge-Kutta 2-stage)"); break; - case integration::eAutoDiffIntegrationMethod::AD_GLRK3: - D_INFO("Integrator set to GLRK3 (AutoDiff Gauss-Legendre Runge-Kutta 3-stage)"); break; - default: - break; - } - } - if (isSelected) { ImGui::SetItemDefaultFocus(); } - } - ImGui::EndCombo(); - } - } - else { - if (ImGui::BeginCombo("##", currentIntMethod)) { - for (int n = 0; n < IM_ARRAYSIZE(intMethodNames); ++n) { - bool isSelected = (n == static_cast(currentIntEnum)); - - // When a new method is selected, update the robot's integration method and log the change - if (ImGui::Selectable(intMethodNames[n], isSelected)) { - auto updatedMethod = static_cast(n); - const auto state = robot->runtimeIntegratorState(); - state->autoDiff = false; // ensure autodiff flag is set in state - _sim->setIntegrationMethod(updatedMethod); - - switch (updatedMethod) { - case integration::eIntegrationMethod::Euler: - D_INFO("Integrator set to Euler"); break; - case integration::eIntegrationMethod::Midpoint: - D_INFO("Integrator set to RK2 (Midpoint)"); break; - case integration::eIntegrationMethod::Heun: - D_INFO("Integrator set to RK2 (Heun)"); break; - case integration::eIntegrationMethod::Ralston: - D_INFO("Integrator set to RK2 (Ralston)"); break; - case integration::eIntegrationMethod::RK4: - D_INFO("Integrator set to RK4"); break; - case integration::eIntegrationMethod::RK45: - D_INFO("Integrator set to RK45 (Dormand-Prince)"); break; - case integration::eIntegrationMethod::ImplicitEuler: - D_INFO("Integrator set to Implicit Euler"); break; - case integration::eIntegrationMethod::ImplicitMidpoint: - D_INFO("Integrator set to Implicit Midpoint"); break; - case integration::eIntegrationMethod::GLRK2: - D_INFO("Integrator set to GLRK2 (Gauss-Legendre Runge-Kutta 2-stage)"); break; - case integration::eIntegrationMethod::GLRK3: - D_INFO("Integrator set to GLRK3 (Gauss-Legendre Runge-Kutta 3-stage)"); break; - default: - break; - } - } - if (isSelected) { ImGui::SetItemDefaultFocus(); } - } - ImGui::EndCombo(); - } - } - - ImGui::EndDisabled(); - - // Delta time controls - ImGui::SectionHeader("Delta Time (dt) Settings:"); - ImGui::Spacing(); - - ImGui::BeginDisabled(_sim->isSimRunning()); - // Simulation dt controls - ImGui::BeginGroup(); - ImGui::Text("Simulation dt:"); - - static int k = 6; - if (ImGui::DragDtFraction("##simDtDrag", k, false)) { - int x = 30 * k; - double dt = 1.0 / (double)x; - _sim->setFixedDt(dt); - } - ImGui::EndGroup(); - - // Add spacing between the two groups of controls (40px) - ImGui::SameLine(0.0f, 40.0f); - - // Telemetry dt controls - ImGui::BeginGroup(); - ImGui::Text("Telemetry dt:"); - - static int k_tel = 4; - if (ImGui::DragDtFraction("##telDtDrag", k_tel, true)) { - int x = 30 * k_tel; - _sim->setTelemetryHz(x); - } - ImGui::EndGroup(); - ImGui::EndDisabled(); - - // Deals with simulation time tracking using chrono - if (_sim->isSimRunning()) { - ImGui::TextColored(ImVec4(1, 0.4f, 0.4f, 1), "Simulation Running..."); - ImGui::TextColored(ImVec4(1, 0.4f, 0.4f, 1), "Elapsed Time: %.3f", _sim->simTime()); - - // make sure to stop sim when commands are finished - if (!_sim->isSimRunning()) { - ImGui::Text("Simulation Stopped."); - ImGui::Text("Elapsed Time: %.3f", _sim->simTime()); - } - } - ImGui::Separator(); - } - - // Object properties implementation - void ControlPanel::objectProperties() { - // Prevent editing properties while sim is running - if (_sim->isSimRunning()) { - ImGui::TextColored(ImVec4(1, 0.4f, 0.4f, 1), "Cannot edit object properties while simulation is running."); - return; - } - // Ensure we have an object to edit - if (!_obj) { - ImGui::TextColored(ImVec4(1, 0.4f, 0.4f, 1), "No object selected."); - return; - } - - // Display object name - ImGui::SectionHeader("Physics Settings:"); - ImGui::Separator(); - - // Mass, Damping, Gravity Controls - double minMass = 0.25; double maxMass = 100.0; // mass limits - float minDamping = 0.0; float maxDamping = 1.0; // damping limits - double minGravity = 0.0; double maxGravity = 10.0; // gravity limits - - // Disable controls while sim is running to prevent conflicts - ImGui::BeginDisabled(_sim->isSimRunning()); - - // Mass Control - ImGui::Text("Mass:"); - ImGui::SetNextItemWidth(150.0f); ImGui::DragScalar("kg##mass", ImGuiDataType_Double, &_obj->state.mass, 0.025f, &minMass, &maxMass); - ImGui::Spacing(); - - // Damping Control - ImGui::Text("Damping:"); - ImGui::SetNextItemWidth(150.0f); ImGui::DragScalar("kg/s##damp", ImGuiDataType_Double, &_obj->state.damping, 0.001f, &minDamping, &maxDamping); - ImGui::Spacing(); - - // Gravity Control - ImGui::Text("Gravity:"); - ImGui::SetNextItemWidth(150.0f); ImGui::DragScalar("m/s^2##g", ImGuiDataType_Double, &_obj->state.gravity, 0.00005f, &minGravity, &maxGravity); - ImGui::Spacing(); - - ImGui::Separator(); - - // Scale Controls - ImGui::Text("Scale:"); - float minScale = 0.0001f; float maxScale = 100.0f; - ImGui::SetNextItemWidth(150.0f); ImGui::DragFloat("(x)", &_obj->transform.scale.x, 0.001f, minScale, maxScale); - ImGui::SetNextItemWidth(150.0f); ImGui::DragFloat("(y)", &_obj->transform.scale.y, 0.001f, minScale, maxScale); - ImGui::SetNextItemWidth(150.0f); ImGui::DragFloat("(z)", &_obj->transform.scale.z, 0.001f, minScale, maxScale); - - ImGui::Spacing(); - - // Linear Velocity Controls - ImGui::Text("Linear Velocity:"); - double minVelocity = -100.0; double maxVelocity = 100.0; - ImGui::SetNextItemWidth(150.0f); ImGui::DragScalar("X##linVelX", ImGuiDataType_Double, &_obj->state.linearVelocity.x(), 0.0025f, &minVelocity, &maxVelocity); - ImGui::SetNextItemWidth(150.0f); ImGui::DragScalar("Y##linVelY", ImGuiDataType_Double, &_obj->state.linearVelocity.y(), 0.0025f, &minVelocity, &maxVelocity); - ImGui::SetNextItemWidth(150.0f); ImGui::DragScalar("Z##linVelZ", ImGuiDataType_Double, &_obj->state.linearVelocity.z(), 0.0025f, &minVelocity, &maxVelocity); - - ImGui::Spacing(); - - // Angular Velocity Controls - ImGui::Text("Angular Velocity:"); - double minTorque = -100.0; double maxTorque = 100.0; - ImGui::SetNextItemWidth(150.0f); ImGui::DragScalar("X##angVelX", ImGuiDataType_Double, &_obj->state.angularVelocity.x(), 0.0025f, &minTorque, &maxTorque); - ImGui::SetNextItemWidth(150.0f); ImGui::DragScalar("Y##angVelY", ImGuiDataType_Double, &_obj->state.angularVelocity.y(), 0.0025f, &minTorque, &maxTorque); - ImGui::SetNextItemWidth(150.0f); ImGui::DragScalar("Z##angVelZ", ImGuiDataType_Double, &_obj->state.angularVelocity.z(), 0.0025f, &minTorque, &maxTorque); - - ImGui::Separator(); - ImGui::EndDisabled(); - - // Reset Object Button - ImGui::Text("Reset Object:"); - if (ImGui::Button("Reset")) { - // Should not happen since button is disabled when no object - if (!_obj) { - LOG_WARN("No object selected to reset."); - return; - } - // Reset object state to initial conditions - _obj->reset(); - LOG_INFO("Object reset to initial position and orientation."); - D_INFO("Reset %s", _obj); - } - ImGui::Separator(); - } - - // Joint properties implementation - void ControlPanel::jointProperties() { - // Prevent editing properties while sim is running - if (!_hasRobot) { ImGui::TextColored(ImVec4(1, 0.4f, 0.4f, 1), "No robot model loaded."); return; } - robots::RobotSystem* robot = _sim->robotSystem(); - - // Get references to robot's links and joints for easy access - auto& links = robot->links(); - auto& joints = robot->joints(); - - // Ensure we have joints to edit - if (joints.empty()) { - ImGui::TextDisabled("Robot has no joints."); - return; - } - // Display joint properties header - float minDamping = 0.0; float maxDamping = 1.0; // damping limits - float minFriction = 0.0; float maxFriction = 10.0; // friction limits - double minGravity = 0.0; double maxGravity = 100.0; // gravity limits - - // Clamp current joint index to valid range to prevent out-of-bounds access - static int currentJointIndex = 0; - currentJointIndex = std::clamp(currentJointIndex, 0, (int)joints.size() - 1); - - // Get references to currently selected joint and its corresponding link - auto& j = joints[currentJointIndex]; - int linkIndex = currentJointIndex; - linkIndex = std::clamp(linkIndex, 0, (int)links.size() - 1); - auto& L = links[linkIndex]; - - // Copy joint properties to local variables for editing - float c = (float)j.dynamics.damping; - float f = (float)j.dynamics.friction; - double g = (double)robot->getGravity(); - - const auto& rec = _sim->telemetry(); - ImGui::BeginDisabled(_sim->isSimRunning()); - - ImGui::Text("Joint Dynamics:"); - // Damping display (just displays current val from model) - ImGui::SetNextItemWidth(150.0f); - ImGui::TextDisabled("Damping: %.3f", c); - ImGui::SetNextItemWidth(150.0f); - ImGui::TextDisabled("Friction: %.3f", f); - ImGui::Spacing(); - - // Gravity controls with preset menu - ImGui::Text("Gravity:"); - static GravityPreset gravityPreset = PRESET_MICRO_G; // default preset - - // Draw gravity preset menu and update gravity value based on selection - drawGravityChoiceMenu(gravityPreset, g); - // If in custom gravity mode, allow direct editing of gravity value - if (gravityMode == GravityUIMode::Custom) { - ImGui::SetNextItemWidth(150.0f); - if (ImGui::DragScalar("m/s²##g", ImGuiDataType_Double, &g, 0.005f, &minGravity, &maxGravity)) { - robot->setGravity(g); - } - } - else { robot->setGravity(g); } - ImGui::TextDisabled("Gravity: %.3f", g); - - ImGui::Spacing(); - ImGui::Separator(); - ImGui::EndDisabled(); - - // Telemetry inspector for currently selected joint - ImGui::SectionHeader("Joint Telemetry:"); - // Draw telemetry inspector for the selected joint, showing its state over time - drawTrajectoryInspector(rec, (int)robot->joints().size(), _selection.index); - ImGui::TextDisabled("Selected Joint: %s - Child Link: %s", j.name.c_str(), L.name.c_str()); - ImGui::Spacing(); - ImGui::Text("Joint (1-%d) Errors:", (int)robot->joints().size()); - drawJointErrorPlot(); - ImGui::Spacing(); - // Reset joint properties to initial conditions - ImGui::Spacing(); - ImGui::Text("Reset Robot:"); - ImGui::Spacing(); - - // Reset button to reset the entire robot to its initial state, including all joints and links - ImGui::SetNextItemWidth(150.0f); - if (ImGui::Button("Reset")) { - if (!_hasRobot) { LOG_WARN("No robot selected to reset."); return; } // should not happen - _sim->robotSystem()->resetRobot(); - - // Clear selection - _selection.type = SelectionType::NONE; - _selection.index = -1; - _selection.source = SelectionSource::NONE; - _currentJointName = ""; - _currentLinkName = ""; - - D_INFO("Reset Robot to initial position and orientation."); - return; - } - } - - // Display settings implementation - void ControlPanel::displaySettings() { - ImGui::SectionHeader("Display Settings"); - ImGui::Spacing(); - - static float fovDeg = 70.0f; - ImGui::BeginDisabled(_sim->isSimRunning()); - - ImGui::Spacing(); - ImGui::Text("Camera Field of View (FOV):"); - - ImGui::SetNextItemWidth(150.0f); - bool edited = ImGui::SliderFloat("Field of View", &fovDeg, 25.0f, 125.0f, "%.f"); - bool active = ImGui::IsItemActive(); - - if (!active && !edited) { fovDeg = _sim->getCamera()->getFOVDegrees(); } - if (edited) { _sim->getCamera()->setFOVDegrees(fovDeg); } - ImGui::EndDisabled(); - - ImGui::Spacing(); - - // Graphics Quality Presets - static int graphicsIndx = 1; - const char* qualityOptions[] = { "Low", "Medium", "High", "Ultra" }; - - ImGui::PushStyleVar(ImGuiStyleVar_ItemSpacing, ImVec2(4.0f, ImGui::GetStyle().ItemSpacing.y)); - ImGui::PushStyleVar(ImGuiStyleVar_FrameRounding, 6.0f); - - // Quality Buttons - bool qualityChanged = ImGui::SegmentedButtonRow("Graphics Settings:", qualityOptions, IM_ARRAYSIZE(qualityOptions), graphicsIndx, 70.0f); - - ImGui::PopStyleVar(2); - - // Apply quality changes if needed - if (qualityChanged) { - switch (graphicsIndx) { - case 0: q = render::QualityPreset::Low; break; - case 1: q = render::QualityPreset::Medium; break; - case 2: q = render::QualityPreset::High; break; - case 3: q = render::QualityPreset::Ultra; break; - default: break; - } - - auto s = render::MakeSettings(r, q); - _sim->applyRenderProfile(s, r); - - LOG_INFO("Render quality preset changed to %s", - graphicsIndx == 0 ? "Low" : - graphicsIndx == 1 ? "Medium" : - graphicsIndx == 2 ? "High" : "Ultra"); - D_INFO("Render quality preset changed to %s", - graphicsIndx == 0 ? "Low" : - graphicsIndx == 1 ? "Medium" : - graphicsIndx == 2 ? "High" : "Ultra"); - } - - ImGui::Spacing(); - - // Resolution Presets - static int resIndx = 1; - const char* resOptions[] = { "1280x720", "1920x1080", "2560x1440", "3840x2160" }; - - ImGui::PushStyleVar(ImGuiStyleVar_ItemSpacing, ImVec2(4.0f, ImGui::GetStyle().ItemSpacing.y)); - ImGui::PushStyleVar(ImGuiStyleVar_FrameRounding, 6.0f); - - // Resolution Buttons - bool resChanged = ImGui::SegmentedButtonRow("Resolution Presets:", resOptions, IM_ARRAYSIZE(resOptions), resIndx, 90.0f); - - ImGui::PopStyleVar(2); - - // Apply resolution changes if needed - if (resChanged) { - switch (resIndx) { - case 0: r = render::ResolutionPreset::R_720p; break; - case 1: r = render::ResolutionPreset::R_1080p; break; - case 2: r = render::ResolutionPreset::R_1440p; break; - case 3: r = render::ResolutionPreset::R_4K; break; - default: break; - } - - auto s = render::MakeSettings(r, q); - _sim->applyRenderProfile(s, r); - - LOG_INFO("Render resolution preset changed to %dx%d", - (int)(_sim->size().x * s.renderScale), - (int)(_sim->size().y * s.renderScale)); - - D_INFO("Render resolution preset changed to %dx%d", - (int)(_sim->size().x * s.renderScale), - (int)(_sim->size().y * s.renderScale)); - } - - ImGui::Spacing(); - - // Shader Mode Selector - static int shaderIndx = 2; - const char* shaderOptions[] = { "Basic Shader", "Lit Shader", "PBR Shader" }; - - ImGui::PushStyleVar(ImGuiStyleVar_ItemSpacing, ImVec2(4.0f, ImGui::GetStyle().ItemSpacing.y)); - ImGui::PushStyleVar(ImGuiStyleVar_FrameRounding, 6.0f); - - // Shader Buttons - bool shaderChanged = ImGui::SegmentedButtonRow("Shader Mode:", shaderOptions, IM_ARRAYSIZE(shaderOptions), shaderIndx, 110.0f); - - ImGui::PopStyleVar(2); - - // Apply shader changes if needed - if (shaderChanged) { - switch (shaderIndx) { - case 0: _sim->currentShaderMode = SimManager::ShaderMode::Basic; D_INFO("Shader -> Basic Shader"); break; - case 1: _sim->currentShaderMode = SimManager::ShaderMode::Lit; D_INFO("Shader -> Lit Shader"); break; - case 2: _sim->currentShaderMode = SimManager::ShaderMode::PBR; D_INFO("Shader -> PBR Shader"); break; - default: break; - } - } - - if (ImGui::Button("Reload Shaders")) { - LOG_INFO("Shader reload requested."); - _sim->reloadAllShaders(); - } - } - - // Robotic Arm Selector - void ControlPanel::roboticArmSelector() { - if (!_showRobotSelector) return; - - ImGui::Begin("Choose Robotic Arm", &_showRobotSelector, ImGuiWindowFlags_NoDocking); - - ImGui::SectionHeader("Select a robotic arm model:"); - ImGui::Separator(); - ImGui::Spacing(); - - roboticCardDisplay("Z1", "Unitree Robotics"); - roboticCardDisplay("UR5e", "Universal Robots"); - roboticCardDisplay("Panda", "Franka Robotics"); - roboticCardDisplay("iiwa14", "KUKA"); - roboticCardDisplay("VISPA", "Airbus"); - roboticCardDisplay("H1", "Unitree Robotics"); - - ImGui::End(); - } - - // Robotic Arm Card for Selector (lists robotic arms to choose from) - void ControlPanel::roboticCardDisplay(const char* name, const char* company) { - ImGui::PushID(name); - - ImGui::BeginChild("robot_card", ImVec2(0, 55), true, ImGuiWindowFlags_NoScrollbar); - - // Loads robot - if (ImGui::Selectable(name, false, ImGuiSelectableFlags_AllowDoubleClick)) { - LOG_INFO("Selected robot: %s", name); - _showRobotSelector = false; - - _requestedRobot = name; - _robotRequested = true; - - _sim->loadRobot(_requestedRobot); - _hasRobot = true; - } - - ImGui::TextDisabled("Company: %s", company); - - ImGui::EndChild(); - ImGui::Spacing(); - - ImGui::PopID(); - } - - // Scene Objects List - void ControlPanel::sceneObjectsTable() { - ImGui::SetNextWindowPos(ImVec2(0, 250), ImGuiCond_FirstUseEver); - ImGui::SetNextWindowSize(ImVec2(300, 200), ImGuiCond_FirstUseEver); - - ImGui::PushStyleColor(ImGuiCol_WindowBg, ImVec4(0.129f, 0.129f, 0.129f, 0.8f)); - ImGui::Begin("SceneObjectsTable", nullptr, ImGuiWindowFlags_NoCollapse | ImGuiWindowFlags_NoTitleBar); - - beginControlPanel("Rigid Body Table"); - - auto& objs = _sim->getObjects(); - int indexToDelete = -1; - - ImGui::SectionHeader("Active Rigid-Bodies"); - ImGui::SectionDivider(); - - ImGuiTableFlags tableFlags = - ImGuiTableFlags_BordersV | - ImGuiTableFlags_BordersOuterH | - ImGuiTableFlags_Resizable | - ImGuiTableFlags_RowBg | - ImGuiTableFlags_NoBordersInBody; - - if (ImGui::BeginTable("SceneTable", 3, tableFlags)) { - ImGui::TableSetupColumn("Name", ImGuiTableColumnFlags_NoHide); - ImGui::TableSetupColumn("Type", ImGuiTableColumnFlags_WidthFixed); - ImGui::TableSetupColumn("Child / Action"); - ImGui::TableHeadersRow(); - - float rowH = 20.0f; - - // Robot section - if (_hasRobot) { - robots::RobotSystem* robotSys = _sim->robotSystem(); - if (robotSys && robotSys->hasRobot()) { - const auto& links = robotSys->links(); - const auto& joints = robotSys->joints(); - - // Root label - std::string rootName = robotSys->robotName(); // or robotSys->robotName() - if (rootName.empty()) rootName = "Robot"; - - ImGui::TableNextRow(ImGuiTableRowFlags_None, rowH); - ImGui::TableSetColumnIndex(0); - - ImGuiTreeNodeFlags rootFlags = - ImGuiTreeNodeFlags_SpanAllColumns | - ImGuiTreeNodeFlags_OpenOnArrow; - - bool openRoot = ImGui::TreeNodeEx(rootName.c_str(), rootFlags); - - bool rowHovered = ImGui::IsItemHovered(); - if (rowHovered) { ImGui::TableSetBgColor(ImGuiTableBgTarget_RowBg0, ImGui::GetColorU32(ImGuiCol_HeaderHovered)); } - - // Column 1: centered "ROOT" - ImGui::TableSetColumnIndex(1); - { - const char* txt = "ROOT"; - float columnWidth = ImGui::GetColumnWidth(); - float textWidth = ImGui::CalcTextSize(txt).x; - ImGui::SetCursorPosX(ImGui::GetCursorPosX() + (columnWidth - textWidth) * 0.5f); - ImGui::TextUnformatted(txt); - } - - // Column 2: centered Remove button - ImGui::TableSetColumnIndex(2); - { - float columnWidth = ImGui::GetColumnWidth(); - float buttonWidth = 80.0f; - ImGui::SetCursorPosX(ImGui::GetCursorPosX() + (columnWidth - buttonWidth) * 0.5f); - - if (ImGui::Button(("Remove##robot_" + rootName).c_str(), ImVec2(buttonWidth, 0))) { - _sim->clearRobot(); - _hasRobot = false; - } - } - - if (openRoot) { - for (int i = 0; i < (int)joints.size(); ++i) { - auto& joint = joints[i]; - scene::Object* attachedObj = nullptr; - rowH = 10.0f; - - // Find attached object for this joint's child link - /*for (auto& l : links) { if (l.name == joint.child) { attachedObj = l.attachedObject; break; } }*/ - - bool jointSelected = (_selection.type == SelectionType::JOINT && _selection.index == i); - - ImGui::TableNextRow(ImGuiTableRowFlags_None, rowH); - ImGui::TableSetColumnIndex(0); - - ImGui::PushID(i); - - bool rowClicked = ImGui::Selectable("##joint_row", jointSelected, - ImGuiSelectableFlags_SpanAllColumns, ImVec2(0.0f, rowH) - ); - - rowHovered = ImGui::IsItemHovered(); - if (rowHovered) { ImGui::TableSetBgColor(ImGuiTableBgTarget_RowBg0, ImGui::GetColorU32(ImGuiCol_HeaderHovered)); } - - ImGui::SameLine(0.0f, 6.0f); - - ImGuiTreeNodeFlags leafFlags = ImGuiTreeNodeFlags_Leaf - | ImGuiTreeNodeFlags_NoTreePushOnOpen - | ImGuiTreeNodeFlags_NoAutoOpenOnLog - | ImGuiTreeNodeFlags_SpanAllColumns - | ImGuiTreeNodeFlags_NoTreePushOnOpen; - - ImGui::TreeNodeEx("##leaf", leafFlags); - - // Center joint name *within column 0* - { - float columnWidth = ImGui::GetColumnWidth(); - float textWidth = ImGui::CalcTextSize(joint.name.c_str()).x; - ImGui::SetCursorPosX(ImGui::GetCursorPosX() + (columnWidth - textWidth) * 0.5f); - } - - ImGui::SameLine(); - - if (jointSelected) ImGui::PushStyleColor(ImGuiCol_Text, ImVec4(1.0f, 0.95f, 0.6f, 1.0f)); - ImGui::TextUnformatted(joint.name.c_str()); - if (ImGui::IsItemHovered()) { - ImGui::BeginTooltip(); - ImGui::TextDisabled("Angle(rad): %.2f\nOmega(rad/s): %.2f\nDamping: %.2f\nFriction: %.2f", - joint.q, joint.qd, joint.dynamics.damping, joint.dynamics.friction); - ImGui::EndTooltip(); - } - if (jointSelected) ImGui::PopStyleColor(); - - if (rowClicked) { - _currentJointName = joint.name; - _selection.type = SelectionType::JOINT; - _selection.index = i; - _selection.source = SelectionSource::CONTROL_PANEL; - if (attachedObj) { _sim->setSelectedObject(attachedObj); } - - _sim->followRobotJoint(_currentJointName, glm::vec3(0.0f, 0.2f, 0.6f)); - } - - ImGui::PopID(); - - // Column 1: centered "Joint" - ImGui::TableSetColumnIndex(1); - { - const char* txt = "Joint"; - float columnWidth = ImGui::GetColumnWidth(); - float textWidth = ImGui::CalcTextSize(txt).x; - ImGui::SetCursorPosX(ImGui::GetCursorPosX() + (columnWidth - textWidth) * 0.5f); - ImGui::TextUnformatted(txt); - } - - // Column 2: centered child link name - ImGui::TableSetColumnIndex(2); - { - float columnWidth = ImGui::GetColumnWidth(); - float textWidth = ImGui::CalcTextSize(joint.child.c_str()).x; - ImGui::SetCursorPosX(ImGui::GetCursorPosX() + (columnWidth - textWidth) * 0.5f); - ImGui::TextUnformatted(joint.child.c_str()); - } - } - ImGui::TreePop(); - } - _currentObjectName = "Robot: " + rootName; - } - } - - // General objects - rowH = 20.0f; - ImGui::TableNextRow(ImGuiTableRowFlags_None, rowH); - - // General Object Loop - for (int i = 0; i < objs.size(); i++) { - auto* obj = objs[i].get(); - rowH = 10.0f; - bool isSelected = (_selection.type == SelectionType::BODY && _selection.index == i); - - if (obj->category != scene::ObjectCategory::General) { continue; } // skip non-general objects - - ImGui::TableNextRow(ImGuiTableRowFlags_None, rowH); - ImGui::TableSetColumnIndex(0); - std::string label = "Object " + std::to_string(i); - - if (ImGui::Selectable(label.c_str(), isSelected)) { - _currentObjectName = label; - _selection.type = SelectionType::BODY; - _selection.index = i; - _selection.source = SelectionSource::CONTROL_PANEL; - _sim->setSelectedObject(obj); // fine to keep for inspector - LOG_INFO("Selected Object: %s", label.c_str()); - } - - // Column 1: centered "OBJECT" - ImGui::TableSetColumnIndex(1); - { - const char* txt = "OBJECT"; - float columnWidth = ImGui::GetColumnWidth(); - float textWidth = ImGui::CalcTextSize(txt).x; - ImGui::SetCursorPosX(ImGui::GetCursorPosX() + (columnWidth - textWidth) * 0.5f); - ImGui::TextUnformatted(txt); - } - - // Column 2: centered Delete button - ImGui::TableSetColumnIndex(2); - { - float columnWidth = ImGui::GetColumnWidth(); - float buttonWidth = 80.0f; - ImGui::SetCursorPosX(ImGui::GetCursorPosX() + (columnWidth - buttonWidth) * 0.5f); - if (ImGui::Button(("Delete##" + std::to_string(i)).c_str())) { indexToDelete = i; } - } - } - - ImGui::EndTable(); - } - - if (indexToDelete != -1) { - _sim->deleteObject(indexToDelete); - LOG_INFO("Deleted object at index %d", indexToDelete); - } - - endControlPanel(); - - ImGui::End(); - ImGui::PopStyleColor(); - } - - // Select joint from table and set camera to follow it - void ControlPanel::selectJointAndFollow(int jointIdx) { - if (!_sim || !_sim->hasRobot()) return; - - robots::RobotSystem* robot = _sim->robotSystem(); - if (!robot) return; - - auto& joints = robot->joints(); - auto& links = robot->links(); - if (joints.empty()) return; - - jointIdx = std::clamp(jointIdx, 0, (int)joints.size() - 1); - - const auto& joint = joints[jointIdx]; - _currentJointName = joint.name; - - _selection.type = SelectionType::JOINT; - _selection.index = jointIdx; - _selection.source = SelectionSource::CONTROL_PANEL; - - //// find attached object for child link - //scene::Object* attachedObj = nullptr; - //for (auto& l : links) { - // if (l.name == joint.child) { attachedObj = l.attachedObject; break; } - //} - //if (attachedObj) { - // _sim->setSelectedObject(attachedObj); - //} - - // camera follow - _sim->followRobotJoint(_currentJointName, glm::vec3(0.0f, 0.2f, 0.6f)); - } - // --- Telemetry Plots --- - - // Trajectory Inspector - void ControlPanel::drawTrajectoryInspector(const diagnostics::TelemetryRecorder& rec, int /*jointCount*/, int& selectedJoint) { - const auto& ring = rec.ring; - if (ring.size() < 1) { ImGui::TextUnformatted("No trajectory telemetry yet."); return; } - - const diagnostics::TelemetrySample& s = ring.at(ring.size() - 1); - if (selectedJoint < 0) { selectedJoint = 0; } - if (selectedJoint >= (int)s.j.size()) { selectedJoint = (int)s.j.size() - 1; } - - ImGui::Text("t = %.3f s", s.timeSec); - - int currentJ = selectedJoint + 1; - - if (ImGui::SliderInt("Joint index", ¤tJ, 1, (int)s.j.size())) { - selectedJoint = currentJ - 1; - selectJointAndFollow(selectedJoint); - } - else { - selectedJoint = currentJ - 1; - } - - const diagnostics::JointTelemetry& j = s.j[selectedJoint]; - const float e = (float)(j.q_ref - j.q); - - ImGui::Separator(); - ImGui::TextDisabled("Robot loaded: %s", _requestedRobot.c_str()); - ImGui::TextDisabled("Selected Joint: %s", _currentJointName.c_str()); - - // --------------- Joint Inspector ---------------- - ImGui::Spacing(); - // Joint Info - ImGui::SectionHeader("State:"); - ImGui::Text("theta: %.6f rad", j.q); - ImGui::Text("omega: %.6f rad/s", j.qd); - ImGui::Text("damping: %.6f kg·m^2/s", j.damping); - ImGui::Text("friction: %.6f N·m", j.friction); - ImGui::Text("torque: %.6f N·m", j.torqueNm); - - ImGui::Spacing(); - // Reference Info - ImGui::SectionHeader("Reference:"); - ImGui::Text("theta_ref: %.6f rad", j.q_ref); - ImGui::Text("omega_ref: %.6f rad/s", j.qd_ref); - ImGui::Text("alpha_ref: %.6f rad/s^2", j.qdd_ref); - ImGui::Text("error e: %.6f drad", e); - - ImGui::Spacing(); - // Control Info - ImGui::SectionHeader("Control:"); - ImGui::Text("Active: %s", j.traj_active ? "Yes" : "No"); - if (j.traj_active) { - ImGui::Text("traj q: %.6f", j.traj_q); - ImGui::Text("traj qd: %.6f", j.traj_qd); - ImGui::Text("traj qdd: %.6f", j.traj_qdd); - } - - ImGui::SectionDivider(); - // Limit Info - ImGui::SectionHeader("Limits:"); - ImGui::Text("Clamp_theta: %s", j.clampTheta ? "Yes" : "No"); - ImGui::Text("Clamp_omega: %s", j.clampOmega ? "Yes" : "No"); - - // Find and show worst joint button - if (ImGui::Button("Show Worst Joint")) { - float worstErr = 0.0f; - int worstIdx = 0; - for (int i = 0; i < (int)s.j.size(); ++i) { - float err = (float)std::abs(s.j[i].q_ref - s.j[i].q); - if (err > worstErr) { worstErr = err; worstIdx = i; } - } - selectedJoint = worstIdx; - selectJointAndFollow(selectedJoint); - } - } - - void ControlPanel::drawJointErrorPlot() { - // Get the latest telemetry record - const auto& rec = _sim->telemetry(); - const auto& ring = rec.ring; - - if (ring.size() < 2) { ImGui::TextUnformatted("No telemetry data yet."); return; } - - const auto& last = ring.at(ring.size() - 1); - - static std::vector rX; - static std::vector> rY; - - const size_t sampleCount = ring.size(); - const size_t jointCount = last.j.size(); - - rX.resize(sampleCount); - - if (rY.size() != jointCount) { rY.resize(jointCount); } - for (size_t j = 0; j < jointCount; ++j) { rY[j].resize(sampleCount); } - - for (int k = 0; k < sampleCount; ++k) { - const auto& s = ring.at(k); - rX[k] = (float)s.timeSec; - - const size_t m = std::min(jointCount, s.j.size()); - for (size_t j = 0; j < m; ++j) { - rY[j][k] = (float)(s.j[j].q_ref - s.j[j].q); - } - for (size_t j = m; j < jointCount; ++j) { - rY[j][k] = 0.0f; - } - } - - // --- Joint Error Overlay --- - const ImVec2 plotSz(ImGui::GetContentRegionAvail().x, 250.0f); - if (ImPlot::BeginPlot("Joint Error Overlay##results", plotSz)) { - ImPlot::SetupAxes("t (s)", "e (rad)", ImPlotAxisFlags_AutoFit, ImPlotAxisFlags_AutoFit); - ImPlot::SetupLegend(ImPlotLocation_NorthEast); - for (size_t j = 0; j < jointCount; ++j) { - char label[16]; - snprintf(label, sizeof(label), "J%02d", (int)j); - ImPlot::PlotLine(label, rX.data(), rY[j].data(), (int)sampleCount); - } - ImPlot::EndPlot(); - } - } - - // --- CSV Export --- - - // Export telemetry data to CSV for external analysis (e.g. Python, Excel) - void ControlPanel::exportTelemetryCSV(const char* filepath) { - const auto& rec = _sim->telemetry(); - const auto& ring = rec.ring; - if (ring.size() < 2) return; - - FILE* f = nullptr; - if (fopen_s(&f, filepath, "w") != 0 || !f) { - D_FAIL("Failed to export CSV to: %s", filepath); - LOG_ERROR("Failed to export CSV to: %s", filepath); - return; - } - - const size_t sampleCount = ring.size(); - const size_t jointCount = ring.at(sampleCount - 1).j.size(); - - // Header - fprintf(f, "time_s,err_rms,err_max,clamp_sum"); - for (size_t j = 0; j < jointCount; ++j) { - fprintf(f, ",J%02d_theta,J%02d_omega,J%02d_torque,J%02d_theta_ref,J%02d_err", (int)j+1, (int)j+1, (int)j+1, (int)j+1, (int)j+1); - } - fprintf(f, "\n"); - - // Data rows - for (size_t k = 0; k < sampleCount; ++k) { - const auto& s = ring.at(k); - fprintf(f, "%.6f,%.9f,%.9f,%d", s.timeSec, s.err_rms, s.err_max, s.clamp_sum); - const size_t m = std::min(jointCount, s.j.size()); - for (size_t j = 0; j < m; ++j) { - const auto& jt = s.j[j]; - fprintf(f, ",%.9f,%.9f,%.9f,%.9f,%.9f", - jt.q, jt.qd, jt.torqueNm, jt.q_ref, - jt.q_ref - jt.q); - } - fprintf(f, "\n"); - } - - fclose(f); - D_SUCCESS("Telemetry CSV exported to: %s", filepath); - LOG_INFO("Telemetry CSV exported to: %s", filepath); - } - - // --- Integrator Comparison --- - - // Run the loaded script with all integrators and capture telemetry for comparison plots - void ControlPanel::runComparisonAllIntegratorsAsync() { - const std::string scriptText = _sim->lastScriptText(); - if (scriptText.empty()) { - D_FAIL("No script loaded. Run a script first, then compare."); - return; - } - if (!_sim->hasRobot()) { - D_FAIL("No robot loaded. Cannot run comparison."); - return; - } - - static const integration::eIntegrationMethod methods[] = { - integration::eIntegrationMethod::Euler, - integration::eIntegrationMethod::Midpoint, - integration::eIntegrationMethod::Heun, - integration::eIntegrationMethod::Ralston, - integration::eIntegrationMethod::RK4, - integration::eIntegrationMethod::RK45, - integration::eIntegrationMethod::ImplicitEuler, - integration::eIntegrationMethod::ImplicitMidpoint, - integration::eIntegrationMethod::GLRK2, - integration::eIntegrationMethod::GLRK3 - }; - static const char* names[] = { "Euler", "Midpoint", "Heun", "Ralston", "RK4", "RK45", "ImplicitEuler", "ImplicitMidpoint", "GLRK2", "GLRK3" }; - - // Clear previous state - { - std::lock_guard lk(_comparisonMutex); - _comparisonResults.clear(); - _comparisonReady = false; - _comparisonFutures.clear(); - _comparisonPending = 0; - } - - D_INFO("Starting integrator comparison (parallel, %zu methods)...", (size_t)(sizeof(methods) / sizeof(methods[0]))); - - const size_t methodCount = sizeof(methods) / sizeof(methods[0]); - - // Launch one async worker per integrator - for (size_t i = 0; i < methodCount; ++i) { - const auto method = methods[i]; - const std::string methodName = names[i]; - _comparisonPending.fetch_add(1, std::memory_order_relaxed); - - // Capture by value: scriptText and methodName - _comparisonFutures.emplace_back(std::async(std::launch::async, [scriptText, method, methodName]() -> ComparisonSnapshot { - ComparisonSnapshot snap; - snap.integratorName = methodName; - try { - // Create fresh simulation core local to this worker - core::ISimulationCore* raw = CreateSimulationCore_v1(); - if (!raw) { D_FAIL("Worker: failed to CreateSimulationCore_v1 for %s", methodName.c_str()); return snap; } - CorePtr core(raw, [](core::ISimulationCore* p) { DestroySimulationCore(p); }); - - // Program and parser bound to new core - auto program = std::make_unique(core.get()); - interpreter::Parser parser(program.get()); - parser.parse(scriptText); - program->start(); - - // Run to completion with this integrator - bool ok = core->runScriptToCompletion(program.get(), method); - if (!ok) { - D_WARN("Worker: integrator %s returned failure.", methodName.c_str()); - // still try to gather telemetry if any - } - - // Snapshot telemetry from the worker core - const auto& ring = core->telemetry().ring; - if (ring.size() < 2) { return snap; } - - const size_t sampleCount = ring.size(); - snap.time.assign(sampleCount, 0.0f); - snap.errRms.assign(sampleCount, 0.0f); - snap.errMax.assign(sampleCount, 0.0f); - - // infer joint count from last sample - size_t jointCount = ring.at(ring.size() - 1).j.size(); - snap.jointCount = (int)jointCount; - snap.jointErr.resize(jointCount); - for (size_t j = 0; j < jointCount; ++j) snap.jointErr[j].assign(sampleCount, 0.0f); - - for (size_t k = 0; k < sampleCount; ++k) { - const auto& s = ring.at(k); - snap.time[k] = (float)s.timeSec; - snap.errRms[k] = (float)s.err_rms; - snap.errMax[k] = (float)s.err_max; - const size_t m = std::min(jointCount, s.j.size()); - for (size_t j = 0; j < m; ++j) { - snap.jointErr[j][k] = (float)(s.j[j].q_ref - s.j[j].q); - } - } - - // optional: record integrator name already set - return snap; - } - catch (const std::exception& ex) { - D_FAIL("Worker exception for %s: %s", methodName.c_str(), ex.what()); - return snap; - } - catch (...) { - D_FAIL("Worker unknown exception for %s", methodName.c_str()); - return snap; - } - })); - } - // Results are polled by pollComparisonFutures() - } - - // Poll the async comparison workers for completion, gather results, and mark ready when all done - void ControlPanel::pollComparisonFutures() { - if (_comparisonFutures.empty()) return; - // Check all futures for completion, gather results, and remove completed ones from the list - for (auto it = _comparisonFutures.begin(); it != _comparisonFutures.end();) { - auto& fut = *it; - if (fut.valid() && fut.wait_for(std::chrono::milliseconds(0)) == std::future_status::ready) { - try { - auto snap = fut.get(); - // If snapshot has data, add to results for plotting - { - std::lock_guard lk(_comparisonMutex); - _comparisonResults.emplace_back(std::move(snap)); - } - } - catch (const std::exception& e) { D_FAIL("pollComparisonFutures: future.get() threw: %s", e.what()); } - catch (...) { D_FAIL("pollComparisonFutures: unknown exception from future.get()"); } - // Remove this future from the list - it = _comparisonFutures.erase(it); - _comparisonPending.fetch_sub(1, std::memory_order_relaxed); - } - else ++it; - } - - // When all workers finished, mark ready so UI plotting code can run - if (_comparisonFutures.empty()) { - // sort results into canonical integrator order so UI & CSV are deterministic - if (!_comparisonResults.empty()) { - // canonical order for display - static const std::vector canonical = { "Euler", "Midpoint", "Heun", "Ralston", "RK4", "RK45", "ImplicitEuler", "ImplicitMidpoint", "GLRK2", "GLRK3"}; - std::unordered_map order; - for (int i = 0; i < (int)canonical.size(); ++i) order[canonical[i]] = i; - // sort by canonical order if both integrators are known, otherwise keep order of completion - std::stable_sort(_comparisonResults.begin(), _comparisonResults.end(), - [&order](const ComparisonSnapshot& a, const ComparisonSnapshot& b) { - auto ia = order.find(a.integratorName); - auto ib = order.find(b.integratorName); - if (ia == order.end() && ib == order.end()) return false; - if (ia == order.end()) return false; - if (ib == order.end()) return true; - return ia->second < ib->second; - }); - // mark ready for plotting - _comparisonReady = true; - D_INFO("Integrator comparison complete: %d methods captured.", (int)_comparisonResults.size()); - } - // if no results, still mark ready to avoid indefinite "loading" state in UI - else { - _comparisonReady = true; - D_WARN("Integrator comparison complete: no usable results."); - } - } - } - - // Draw comparison plots (overlays of all integrators) - void ControlPanel::drawComparisonPlots() { - if (!_comparisonReady || _comparisonResults.empty()) { - ImGui::TextDisabled("No comparison data. Click 'Compare All Integrators' first."); - return; - } - - ImGui::TextColored(ImVec4(0.4f, 0.8f, 1.0f, 1.0f), "Integrator Comparison (%d methods)", (int)_comparisonResults.size()); - ImGui::Separator(); - - float totalAvail = ImGui::GetWindowHeight() - 200.0f; - float splitterThickness = 4.0f; - - float reserved = 2.0f * splitterThickness; - float usable = totalAvail - reserved; - float minH = 150.0f; - - _cPlotH1 = ImClamp(_cPlotH1, minH, usable - minH); - _cPlotH2 = ImClamp(_cPlotH2, minH, usable - _cPlotH1 - minH); - - float _cPlotH3 = usable - _cPlotH1 - _cPlotH2; - - float _plotWidth = -1; - - // --- RMS Error Overlay --- - const ImVec2 cPlotSz1(_plotWidth, _cPlotH1); - if (ImPlot::BeginPlot("RMS Error - All Integrators##cmp", cPlotSz1)) { - ImPlot::SetupAxes("t (s)", "RMS error (rad)", ImPlotAxisFlags_AutoFit, ImPlotAxisFlags_AutoFit); - ImPlot::SetupLegend(ImPlotLocation_NorthEast); - for (const auto& res : _comparisonResults) { - ImPlot::PlotLine(res.integratorName.c_str(), res.time.data(), res.errRms.data(), (int)res.time.size()); - } - ImPlot::EndPlot(); - } - - // --- Splitter A --- - ImGui::hSplitter("##cSplit1", &_plotH2, minH, usable - _cPlotH2 - minH, splitterThickness); - - // --- Max Error Overlay --- - const ImVec2 cPlotSz2(_plotWidth, _cPlotH2); - if (ImPlot::BeginPlot("Max Error - All Integrators##cmp", cPlotSz2)) { - ImPlot::SetupAxes("t (s)", "Max error (rad)", ImPlotAxisFlags_AutoFit, ImPlotAxisFlags_AutoFit); - ImPlot::SetupLegend(ImPlotLocation_NorthEast); - for (const auto& res : _comparisonResults) { - ImPlot::PlotLine(res.integratorName.c_str(), res.time.data(), res.errMax.data(), (int)res.time.size()); - } - ImPlot::EndPlot(); - } - - // --- Splitter B --- - ImGui::hSplitter("##cSplit2", &_cPlotH2, minH, usable - _cPlotH1 - minH, splitterThickness); - - // --- Per-joint error for worst joint (joint with max final error) --- - const ImVec2 cPlotSz3(_plotWidth, _cPlotH3); - if (_comparisonResults.front().jointCount > 0) { - // Find which joint has the most variation across integrators - static int selectedCmpJoint = 0; - ImGui::SetNextItemWidth(150.0f); - ImGui::SliderInt("Compare Joint##cmp", &selectedCmpJoint, 0, (int)_comparisonResults.front().jointCount - 1, "J%02d"); - - char plotLabel[64]; - snprintf(plotLabel, sizeof(plotLabel), "J%02d Error - All Integrators##cmpjoint", selectedCmpJoint + 1); - if (ImPlot::BeginPlot(plotLabel, cPlotSz3)) { - ImPlot::SetupAxes("t (s)", "error (rad)", ImPlotAxisFlags_AutoFit, ImPlotAxisFlags_AutoFit); - ImPlot::SetupLegend(ImPlotLocation_NorthEast); - for (const auto& res : _comparisonResults) { - if (selectedCmpJoint < (int)res.jointErr.size()) { - ImPlot::PlotLine(res.integratorName.c_str(), res.time.data(), res.jointErr[selectedCmpJoint].data(), (int)res.time.size()); - } - } - ImPlot::EndPlot(); - } - } - - // --- Export comparison CSV --- - if (ImGui::Button("Export Comparison CSV")) { - auto now = std::chrono::system_clock::now(); - auto epoch = std::chrono::duration_cast(now.time_since_epoch()).count(); - - std::string filename = _requestedRobot.empty() ? "comparison" : _requestedRobot; - filename += "_comparison_" + std::to_string(epoch) + ".csv"; - - auto outPath = paths::runs() / filename; - std::filesystem::create_directories(paths::runs()); - - FILE* f = nullptr; - if (fopen_s(&f, outPath.string().c_str(), "w") == 0 && f) { - // Header: time, then rms/max for each integrator - fprintf(f, "time_s"); - for (const auto& res : _comparisonResults) { - fprintf(f, ",%s_rms,%s_max", res.integratorName.c_str(), res.integratorName.c_str()); - } - fprintf(f, "\n"); - - // Use longest time series - size_t maxSamples = 0; - for (const auto& res : _comparisonResults) { maxSamples = std::max(maxSamples, res.time.size()); } - - for (size_t k = 0; k < maxSamples; ++k) { - // Use first result's time as reference - float t = (k < _comparisonResults[0].time.size()) ? _comparisonResults[0].time[k] : 0.0f; - fprintf(f, "%.6f", t); - for (const auto& res : _comparisonResults) { - if (k < (int)res.time.size()) { - fprintf(f, ",%.9f,%.9f", res.errRms[k], res.errMax[k]); - } else { - fprintf(f, ",,"); - } - } - fprintf(f, "\n"); - } - fclose(f); - D_SUCCESS("Comparison CSV exported to: %s", outPath.string().c_str()); - } - } - ImGui::SameLine(); - ImGui::TextDisabled("Saves to: LocalAppData/DSFE/runs/"); - } - - // --- Results Window (post-simulation) --- - - // When a simulation completes, this modal window pops up with the telemetry plots and export options - void ControlPanel::drawResultsWindow() { - if (!_showResultsWindow) return; - - // Get the latest telemetry record - const auto& rec = _sim->telemetry(); - const auto& ring = rec.ring; - - // If telemetry is too short, don't show the window - if (ring.size() < 2) { - _showResultsWindow = false; - return; - } - - // Force focus on first frame the window opens - if (_resultsFocusNeeded) { ImGui::SetNextWindowFocus(); } - // Get display size for dynamic window sizing - ImVec2 display = ImGui::GetIO().DisplaySize; - - // Use 75% of screen size, clamped to reasonable limits - ImVec2 desiredSize(display.x * 0.75f, display.y * 0.75f); - - // Safety clamp for very large monitors (>4K or ultrawides) - desiredSize.x = ImClamp(desiredSize.x, 700.0f, 1200.0f); - desiredSize.y = ImClamp(desiredSize.y, 500.0f, 900.0f); - - // Set the window size on first appearance - ImGui::SetNextWindowSize(desiredSize, ImGuiCond_Appearing); - - // Center the window on screen - ImVec2 center((display.x - desiredSize.x) * 0.5f, (display.y - desiredSize.y) * 0.5f); - ImGui::SetNextWindowPos(center, ImGuiCond_FirstUseEver); - - // Clear focus flag after using it - if (_resultsFocusNeeded) { _resultsFocusNeeded = false; } - - ImGui::PushStyleColor(ImGuiCol_WindowBg, ImVec4(0.1f, 0.1f, 0.1f, 0.95f)); - - bool open = true; - - // Rounded corners + accent title bar - ImGui::PushStyleVar(ImGuiStyleVar_WindowRounding, 8.0f); - ImGui::PushStyleVar(ImGuiStyleVar_WindowPadding, ImVec2(16, 12)); - ImGui::PushStyleVar(ImGuiStyleVar_FramePadding, ImVec2(8, 4)); - ImGui::PushStyleColor(ImGuiCol_TitleBg, ImVec4(0.08f, 0.08f, 0.12f, 1.0f)); - ImGui::PushStyleColor(ImGuiCol_TitleBgActive, ImVec4(0.15f, 0.18f, 0.28f, 1.0f)); - ImGui::Begin("Simulation Results", &open, ImGuiWindowFlags_NoTitleBar); - - if (!open) { - _showResultsWindow = false; - ImGui::End(); - ImGui::PopStyleColor(2); - ImGui::PopStyleVar(3); - ImGui::PopStyleColor(); - return; - } - - // --- Header --- - const bool runningNow = _sim->isSimRunning(); - if (runningNow) { - ImGui::TextColored(ImVec4(1.0f, 0.8f, 0.4f, 1.0f), "Running..."); - } - else if (_comparisonReady && !runningNow) { - ImGui::TextColored(ImVec4(1.0f, 0.749f, 0.0f, 1.0f), "Comparison Ready"); - } - else { - ImGui::TextColored(ImVec4(0.9f, 0.6f, 0.2f, 1.0f), "Comparison Complete"); - } - - // Show robot name if available - if (!_requestedRobot.empty()) { - ImGui::SameLine(); - ImGui::TextDisabled(" Robot: %s", _requestedRobot.c_str()); - } - - const auto& last = ring.at(ring.size() - 1); // get the latest sample for summary info - - ImGui::TextDisabled("Total Time: %.3f s | Samples: %d", last.timeSec, (int)ring.size()); - ImGui::Separator(); - ImGui::Spacing(); - - // Track the window rect for glReadPixels - static ImVec2 capturePos = { 0, 0 }; - static ImVec2 captureSize = { 0, 0 }; - - if (ImGui::Button("Export as CSV")) { - auto now = std::chrono::system_clock::now(); - auto epoch = std::chrono::duration_cast(now.time_since_epoch()).count(); - - std::string filename = _requestedRobot.empty() ? "results" : _requestedRobot; - filename += "_results_" + std::to_string(epoch) + ".csv"; - - auto outPath = paths::runs() / filename; - std::filesystem::create_directories(paths::runs()); - - exportTelemetryCSV(outPath.string().c_str()); - } - - ImGui::SameLine(); - ImGui::TextDisabled("Saves to: LocalAppData/DSFE/runs/"); - - ImGui::Spacing(); - ImGui::Separator(); - - // --- Rebuild series from ring (reuse the same helpers) --- - static std::vector rX, rRms, rMax, rCs, rCst, rCso; - static std::vector> rY; - - const size_t sampleCount = ring.size(); - const size_t jointCount = last.j.size(); - - rX.resize(sampleCount); - rRms.resize(sampleCount); - rMax.resize(sampleCount); - rCs.resize(sampleCount); - rCst.resize(sampleCount); - rCso.resize(sampleCount); - - if (rY.size() != jointCount) { rY.resize(jointCount); } - for (size_t j = 0; j < jointCount; ++j) { rY[j].resize(sampleCount); } - - for (int k = 0; k < sampleCount; ++k) { - const auto& s = ring.at(k); - rX[k] = (float)s.timeSec; - rRms[k] = (float) s.err_rms; - rMax[k] = (float)s.err_max; - rCs[k] = (float)s.clamp_sum; - rCst[k] = (float)s.clamp_theta; - rCso[k] = (float)s.clamp_omega; - - const size_t m = std::min(jointCount, s.j.size()); - for (size_t j = 0; j < m; ++j) { - rY[j][k] = (float)(s.j[j].q_ref - s.j[j].q); - } - for (size_t j = m; j < jointCount; ++j) { - rY[j][k] = 0.0f; - } - } - - float totalAvail = ImGui::GetWindowHeight() - 200.0f; - float splitterThickness = 4.0f; - - float reserved = 2.0f * splitterThickness; - float usable = totalAvail - reserved; - float minH = 150.0f; - - _plotH1 = ImClamp(_plotH1, minH, usable - minH); - _plotH2 = ImClamp(_plotH2, minH, usable - _plotH1 - minH); - - float _plotH3 = usable - _plotH1 - _plotH2; - - float _plotWidth = -1; - - // --- Error Plot --- - const ImVec2 plotSz1(_plotWidth, _plotH1); - if (ImPlot::BeginPlot("Error (RMS, Max)##results", plotSz1)) { - ImPlot::SetupAxes("t (s)", "error (rad)", ImPlotAxisFlags_AutoFit, ImPlotAxisFlags_AutoFit); - ImPlot::SetupLegend(ImPlotLocation_NorthEast); - ImPlot::PlotLine("RMS", rX.data(), rRms.data(), (int)sampleCount); - ImPlot::PlotLine("Max", rX.data(), rMax.data(), (int)sampleCount); - ImPlot::EndPlot(); - } - - // --- Splitter A --- - ImGui::hSplitter("##split1", &_plotH1, minH, usable - _plotH2 - minH, splitterThickness); - - - // --- Clamp Events --- - const ImVec2 tPlotSz2(_plotWidth, _plotH2); - if (ImPlot::BeginPlot("Clamp Events##results", tPlotSz2)) { - ImPlot::SetupAxes("t (s)", "Sum", ImPlotAxisFlags_AutoFit, ImPlotAxisFlags_AutoFit); - ImPlot::SetupLegend(ImPlotLocation_NorthEast); - ImPlot::PlotStairs("Clamp Theta", rX.data(), rCst.data(), (int)sampleCount); - ImPlot::PlotStairs("Clamp Omega", rX.data(), rCso.data(), (int)sampleCount); - - ImPlot::PushStyleColor(ImPlotCol_Line, ImVec4(0.9f, 0.1f, 0.1f, 1.0f)); - ImPlot::PlotStairs("Clamp Sum", rX.data(), rCs.data(), (int)sampleCount); - ImPlot::PopStyleColor(); - - ImPlot::EndPlot(); - } - - // --- Splitter B --- - ImGui::hSplitter("##split2", &_plotH2, minH, usable - _plotH1 - minH, splitterThickness); - - // --- Joint Error Overlay --- - const ImVec2 plotSz3(_plotWidth, _plotH3); - if (ImPlot::BeginPlot("Joint Error Overlay##results", plotSz3)) { - ImPlot::SetupAxes("t (s)", "e (rad)", ImPlotAxisFlags_AutoFit, ImPlotAxisFlags_AutoFit); - ImPlot::SetupLegend(ImPlotLocation_NorthEast); - for (size_t j = 0; j < jointCount; ++j) { - char label[16]; - snprintf(label, sizeof(label), "J%02d", (int)j + 1); - ImPlot::PlotLine(label, rX.data(), rY[j].data(), (int)sampleCount); - } - ImPlot::EndPlot(); - } - - // --- Integrator Comparison Section --- - ImGui::SectionDivider(); - ImGui::SectionHeader("Integrator Comparison:", ImVec4(0.4f, 0.8f, 1.0f, 1.0f)); - - const bool hasScript = !_sim->lastScriptText().empty(); - ImGui::BeginDisabled(!hasScript || _sim->isSimRunning()); - if (ImGui::Button("Compare All Integrators")) { runComparisonAllIntegratorsAsync(); } - ImGui::EndDisabled(); - ImGui::SameLine(); - ImGui::TextDisabled("Runs All integration methods sequentially"); - if (!hasScript) { - ImGui::SameLine(); - ImGui::TextDisabled("(run a script first)"); - } - - if (_comparisonReady) { - ImGui::Spacing(); - drawComparisonPlots(); - } - - // Capture window rect for next-frame export - capturePos = ImGui::GetWindowPos(); - captureSize = ImGui::GetWindowSize(); - - ImGui::PopStyleColor(2); - ImGui::PopStyleVar(3); - - ImGui::End(); - ImGui::PopStyleColor(); - } -} \ No newline at end of file diff --git a/DSFE_App/DSFE_GUI/src/ui/DebugPanel.cpp b/DSFE_App/DSFE_GUI/src/ui/DebugPanel.cpp deleted file mode 100644 index 3c67d17f..00000000 --- a/DSFE_App/DSFE_GUI/src/ui/DebugPanel.cpp +++ /dev/null @@ -1,317 +0,0 @@ -// DSFE_GUI DebugPanel.cpp -#include -#include "ui/DebugPanel.h" - -#include - -#include "EngineLib/LogMacros.h" - -namespace gui { - void gui::DebugPanel::render() { - ImGui::SetNextWindowPos(ImVec2(ImGui::GetIO().DisplaySize.x - 310, ImGui::GetIO().DisplaySize.y - 200), ImGuiCond_FirstUseEver); - ImGui::SetNextWindowSize(ImVec2(300, 200), ImGuiCond_FirstUseEver); - - ImGui::PushStyleColor(ImGuiCol_WindowBg, ImVec4(0.129f, 0.129f, 0.129f, 0.8f)); - ImGui::Begin("Output", nullptr, ImGuiWindowFlags_NoCollapse | ImGuiWindowFlags_NoScrollbar | ImGuiWindowFlags_NoTitleBar); - - beginDebugPanel("Output Panel"); - - if (ImGui::BeginTabBar("Debug Tabs")) { - if (ImGui::BeginTabItem("Terminal Log")) { - renderLog(); - ImGui::EndTabItem(); - } - - if (ImGui::BeginTabItem("Simulation Log")) { - renderSimLog(); - ImGui::EndTabItem(); - } - ImGui::EndTabBar(); - } - - endDebugPanel(); - - ImGui::End(); - ImGui::PopStyleColor(); - } - - void DebugPanel::renderErrorTable() { - ImGui::PushStyleColor(ImGuiCol_WindowBg, ImVec4(0.1f, 0.1f, 0.1f, 0.925f)); - ImGui::BeginChild("ErrorTableChild", ImVec2(0, 0), true, ImGuiWindowFlags_AlwaysVerticalScrollbar); - - ImGuiTableFlags tableFlags = - ImGuiTableFlags_BordersV | - ImGuiTableFlags_BordersOuterH | - ImGuiTableFlags_Resizable | - ImGuiTableFlags_RowBg | - ImGuiTableFlags_NoBordersInBody; - - if (ImGui::BeginTable("ErrorTable", 3, tableFlags)) - { - ImGui::TableSetupColumn("Type", ImGuiTableColumnFlags_WidthFixed); - ImGui::TableSetupColumn(" ", ImGuiTableColumnFlags_WidthFixed); - ImGui::TableSetupColumn("Description"); - ImGui::TableHeadersRow(); - - ImGui::TableNextRow(); - ImGui::TableSetColumnIndex(0); - - ImGuiTreeNodeFlags rootFlags = - ImGuiTreeNodeFlags_SpanAllColumns | - ImGuiTreeNodeFlags_DefaultOpen; - - bool openError = ImGui::TreeNodeEx("Errors:", rootFlags); - bool openWarn = ImGui::TreeNodeEx("Warnings:", rootFlags); - - ImGui::TableSetColumnIndex(1); - ImGui::TextUnformatted(""); - - if (openError) { - for (const auto& e : _entries) { - ImGui::TableNextRow(); - - // Column 0 -> level type - ImGui::TableSetColumnIndex(0); - ImGuiTreeNodeFlags leafFlags = - ImGuiTreeNodeFlags_Leaf | - ImGuiTreeNodeFlags_NoTreePushOnOpen | - ImGuiTreeNodeFlags_OpenOnArrow; - - ImGui::Text(" ", leafFlags); - - // Column 1 -> Description - ImGui::TableSetColumnIndex(1); - if (_entries.back().isError) - ImGui::TextColored({ 1,0,0,1 }, e.level.c_str()); - - ImGui::TableSetColumnIndex(2); - if (_entries.back().isError) - ImGui::TextUnformatted(e.desc.c_str()); - - ImGui::TableNextRow(); - } - ImGui::TreePop(); - } - - ImGui::TableNextRow(); - - if (openWarn) { - for (const auto& e : _entries) { - ImGui::TableNextRow(); - - // Column 0 -> level type - ImGui::TableSetColumnIndex(0); - ImGuiTreeNodeFlags leafFlags = - ImGuiTreeNodeFlags_Leaf | - ImGuiTreeNodeFlags_NoTreePushOnOpen | - ImGuiTreeNodeFlags_OpenOnArrow; - - ImGui::TreeNodeEx(" ", leafFlags); - - // Column 1 -> Description - ImGui::TableSetColumnIndex(1); - if (!_entries.back().isError) - ImGui::TextColored({ 1,1,0,1 }, e.level.c_str()); - - ImGui::TableSetColumnIndex(2); - if (!_entries.back().isError) - ImGui::TextUnformatted(e.desc.c_str()); - } - ImGui::TreePop(); - } - - ImGui::EndTable(); - } - - ImGui::EndChild(); - ImGui::PopStyleColor(); - } - - void DebugPanel::renderLog() { - ImGuiWindowFlags window_flags = ImGuiWindowFlags_HorizontalScrollbar - | ImGuiWindowFlags_AlwaysVerticalScrollbar; - - ImGui::PushStyleColor(ImGuiCol_WindowBg, ImVec4(0.1f, 0.1f, 0.1f, 0.925f)); - ImGui::BeginChild("LogChild", ImVec2(0, -30), true, window_flags); - - const auto& entries = gLog.Instance().Entries(); - - for (int i = 0; i < (int)entries.size(); ++i) { - const auto& e = entries[i]; - - const char* levelStr = ""; - ImVec4 levelColour{ 1, 1, 1, 1 }; // Colour associated log levelling! (I think its useful) - - // Determine log level string and colour - switch (e.level) { - // trace level for detailed debugging information - case LogLevel::Trace: levelStr = "TRACE"; levelColour = traceCol; break; - // debug level for general debugging information - case LogLevel::Debug: levelStr = "DEBUG"; levelColour = debugCol; break; - // info level for informational messages - case LogLevel::Info: levelStr = "INFO"; levelColour = infoCol; break; - // warning level for potential issues - case LogLevel::Warning: levelStr = "WARN"; levelColour = warnCol; break; - // error level for error messages - case LogLevel::Error: levelStr = "ERROR"; levelColour = errorCol; break; - // success level for successful operations - case LogLevel::Success: levelStr = "SUCCESS"; levelColour = okCol; break; - // fail level for failed operations - case LogLevel::Fail: levelStr = "FAIL"; levelColour = failCol; break; - // Runtime level for runtime specific messages - case LogLevel::Runtime: levelStr = "RUNTIME"; levelColour = runtimeCol; break; - // Output level for general output messages - case LogLevel::Output: levelStr = "OUTPUT"; levelColour = outputCol; break; - // Default level for general output messages - default: levelStr = "OUTPUT"; levelColour = outputCol; break; - } - - bool selected = selectedLines.count(i) > 0; - - ImGui::PushID(i); - if (ImGui::Selectable("##logline", selected, ImGuiSelectableFlags_AllowDoubleClick | ImGuiSelectableFlags_SpanAllColumns)) { - if (ImGui::GetIO().KeyShift && lastClickedLine != -1) { - int a = std::min(lastClickedLine, i); - int b = std::max(lastClickedLine, i); - selectedLines.clear(); - for (int j = a; j <= b; ++j) - selectedLines.insert(j); - } - else if (ImGui::GetIO().KeyCtrl) { - if (selected) selectedLines.erase(i); - else selectedLines.insert(i); - lastClickedLine = i; - } - else { - selectedLines.clear(); - selectedLines.insert(i); - lastClickedLine = i; - } - } - - // Render coloured text on top of selectable - ImGui::SameLine(); - ImGui::TextUnformatted("["); - ImGui::SameLine(0, 0); - ImGui::TextColored(levelColour, "%s", levelStr); - ImGui::SameLine(0, 0); - ImGui::TextUnformatted("]: "); - ImGui::SameLine(0, 0); - ImGui::TextWrapped("%s", e.message.c_str()); - - ImGui::PopID(); - } - - - ImGui::EndChild(); - ImGui::PopStyleColor(); - - if (ImGui::IsWindowFocused() && ImGui::GetIO().KeyCtrl && ImGui::IsKeyPressed(ImGuiKey_C)) { - std::string clip; - for (int idx : selectedLines) { - const auto& e = entries[idx]; - clip += e.message; - clip += "\n"; - } - ImGui::SetClipboardText(clip.c_str()); - } - } - - void DebugPanel::renderSimLog() { - ImGuiWindowFlags window_flags = ImGuiWindowFlags_HorizontalScrollbar - | ImGuiWindowFlags_AlwaysVerticalScrollbar; - - ImGui::PushStyleColor(ImGuiCol_WindowBg, ImVec4(0.1f, 0.1f, 0.1f, 0.925f)); - ImGui::BeginChild("simLogChild", ImVec2(0, -30), true, window_flags); - - const auto& entries = gLog.Instance().SimEntries(); - - for (int i = 0; i < (int)entries.size(); ++i) { - const auto& e = entries[i]; - - const char* levelStr = ""; - ImVec4 levelColour{ 1, 1, 1, 1 }; // Colour associated log levelling! (I think its useful) - - // Determine log level string and colour - switch (e.level) { - // debug level for general debugging information - case simLogLevel::Rotate: levelStr = "ROTATE"; levelColour = rotateCol; break; - // translate level for general debugging information - case simLogLevel::Translate: levelStr = "TRANSLATE"; levelColour = translateCol; break; - // error level for error messages - case simLogLevel::Error: levelStr = "ERROR"; levelColour = errorCol; break; - // fail level for failed operations - case simLogLevel::Fail: levelStr = "FAIL"; levelColour = failCol; break; - // success level for successful operations - case simLogLevel::Success: levelStr = "SUCCESS"; levelColour = okCol; break; - // Runtime level for runtime specific messages - case simLogLevel::Runtime: - default: levelStr = "RUNTIME"; levelColour = runtimeCol; break; - } - - bool selected = simSelectedLines.count(i) > 0; - - ImGui::PushID(i); - if (ImGui::Selectable("##logline", selected, ImGuiSelectableFlags_AllowDoubleClick | ImGuiSelectableFlags_SpanAllColumns)) { - if (ImGui::GetIO().KeyShift && lastClickedLine != -1) { - int a = std::min(lastClickedLine, i); - int b = std::max(lastClickedLine, i); - simSelectedLines.clear(); - for (int j = a; j <= b; ++j) - simSelectedLines.insert(j); - } - else if (ImGui::GetIO().KeyCtrl) { - if (selected) simSelectedLines.erase(i); - else simSelectedLines.insert(i); - lastClickedLine = i; - } - else { - simSelectedLines.clear(); - simSelectedLines.insert(i); - lastClickedLine = i; - } - } - - // Render coloured text on top of selectable - ImGui::SameLine(); - ImGui::TextUnformatted("["); - ImGui::SameLine(0, 0); - ImGui::TextColored(levelColour, "%s", levelStr); - ImGui::SameLine(0, 0); - ImGui::TextUnformatted("]: "); - ImGui::SameLine(0, 0); - ImGui::TextWrapped("%s", e.message.c_str()); - - ImGui::PopID(); - } - - ImGui::EndChild(); - ImGui::PopStyleColor(); - - if (ImGui::IsWindowFocused() && ImGui::GetIO().KeyCtrl && ImGui::IsKeyPressed(ImGuiKey_C)) { - std::string clip; - for (int idx : simSelectedLines) { - const auto& e = entries[idx]; - clip += e.message; - clip += "\n"; - } - ImGui::SetClipboardText(clip.c_str()); - } - - // button to clear simulation log - if (ImGui::Button("Clear Simulation Log")) { clearSimLog(); } - } - - void DebugPanel::beginDebugPanel(const char* id, ImVec2 size) { - ImGui::PushStyleVar(ImGuiStyleVar_WindowPadding, ImVec2(12.0f, 10.0f)); - ImGui::PushStyleVar(ImGuiStyleVar_ChildRounding, 6.0f); - ImGui::PushStyleVar(ImGuiStyleVar_FramePadding, ImVec2(8.0f, 5.0f)); - ImGui::PushStyleVar(ImGuiStyleVar_ItemSpacing, ImVec2(10.0f, 6.0f)); - ImGui::BeginChild(id, size, true, ImGuiChildFlags_AlwaysUseWindowPadding); - } - - void DebugPanel::endDebugPanel() { - ImGui::EndChild(); - ImGui::PopStyleVar(4); - } -} From 5ac799e3bf91e9de66558b19941c78a9cd1c2696 Mon Sep 17 00:00:00 2001 From: saltyjoss Date: Mon, 8 Jun 2026 15:51:20 +0100 Subject: [PATCH 133/210] feat: Added the console and simulation logging window in `ConsoleOutputWidget` --- DSFE_App/DSFE_GUI/CMakeLists.txt | 5 +- .../MainWindow/Widgets/ConsoleOutputWidget.h | 45 ++++++ .../Widgets/ConsoleOutputWidget.cpp | 141 ++++++++++++++++++ 3 files changed, 189 insertions(+), 2 deletions(-) create mode 100644 DSFE_App/DSFE_GUI/include/MainWindow/Widgets/ConsoleOutputWidget.h create mode 100644 DSFE_App/DSFE_GUI/src/MainWindow/Widgets/ConsoleOutputWidget.cpp diff --git a/DSFE_App/DSFE_GUI/CMakeLists.txt b/DSFE_App/DSFE_GUI/CMakeLists.txt index ba958df6..dcd44548 100644 --- a/DSFE_App/DSFE_GUI/CMakeLists.txt +++ b/DSFE_App/DSFE_GUI/CMakeLists.txt @@ -70,12 +70,13 @@ set(MANAGER_SRC src/Scene/Manager/SimViewport.cpp ) -set(UI_SRC +set(WIDGETS_SRC src/ui/RenderPreset.cpp src/MainWindow/Widgets/ViewportWidget.cpp src/MainWindow/Widgets/RobotSelectorWidget.cpp src/MainWindow/Widgets/ControlPanelWidget.cpp src/MainWindow/Widgets/DSLEditorWidget.cpp + src/MainWindow/Widgets/ConsoleOutputWidget.cpp src/MainWindow/Widgets/FractionSelectorWidget.cpp include/MainWindow/Widgets/FractionSelectorWidget.h ) @@ -100,7 +101,7 @@ target_sources(DSFE_GUI PRIVATE ${RENDER_SRC} ${SCENE_SRC} ${MANAGER_SRC} - ${UI_SRC} + ${WIDGETS_SRC} ${ROBOT_SRC} ${PLATFORM_SRC} ${ASSETS_SRC} diff --git a/DSFE_App/DSFE_GUI/include/MainWindow/Widgets/ConsoleOutputWidget.h b/DSFE_App/DSFE_GUI/include/MainWindow/Widgets/ConsoleOutputWidget.h new file mode 100644 index 00000000..39938b00 --- /dev/null +++ b/DSFE_App/DSFE_GUI/include/MainWindow/Widgets/ConsoleOutputWidget.h @@ -0,0 +1,45 @@ +// DSFE_GUI ConsoleOutputWidget.h +#pragma once + +#include +#include + +#include "Platform/Logger.h" + +class QTextEdit; +class QTabWidget; +class QPushButton; + +// Colours for log levels (Will eventually move to a separate file for global UI constants) +#define TRACE_COLOUR QColor(160, 160, 160) +#define DEBUG_COLOUR QColor(80, 170, 240) +#define INFO_COLOUR QColor(80, 160, 255) +#define WARN_COLOUR QColor(255, 180, 0) +#define ERROR_COLOUR QColor(220, 60, 70) +#define OK_COLOUR QColor(0, 190, 100) +#define FAIL_COLOUR QColor(170, 0, 50) +#define RUNTIME_COLOUR QColor(190, 120, 225) +#define OUTPUT_COLOUR QColor(220, 220, 220) + +#define ROTATE_COLOUR QColor(120, 180, 255) +#define TRANSLATE_COLOUR QColor(120, 255, 180) + +namespace widgets { + class ConsoleOutputWidget : public QWidget { + public: + explicit ConsoleOutputWidget(QWidget* parent = nullptr); + void clearSimLog(); + void updateLog(); + + private: + QTabWidget* _tabs = nullptr; + QTextEdit* _terminalLog = nullptr; + QTextEdit* _simLog = nullptr; + QPushButton* _clearTerminalButton = nullptr; + + size_t _lastTerminalCount = 0; + size_t _lastSimCount = 0; + + bool autoScroll = true; + }; +} // namespace widgets \ No newline at end of file diff --git a/DSFE_App/DSFE_GUI/src/MainWindow/Widgets/ConsoleOutputWidget.cpp b/DSFE_App/DSFE_GUI/src/MainWindow/Widgets/ConsoleOutputWidget.cpp new file mode 100644 index 00000000..e0faa1e1 --- /dev/null +++ b/DSFE_App/DSFE_GUI/src/MainWindow/Widgets/ConsoleOutputWidget.cpp @@ -0,0 +1,141 @@ +// DSFE_GUI ConsoleOutputWidget.cpp +#include "Widgets/ConsoleOutputWidget.h" + +#include +#include +#include +#include +#include +#include +#include +#include +#include + +#include "EngineLib/LogMacros.h" + +namespace widgets { + QColor levelColour(LogLevel level) { + switch (level) { + case LogLevel::Trace: return TRACE_COLOUR; + case LogLevel::Debug: return DEBUG_COLOUR; + case LogLevel::Info: return INFO_COLOUR; + case LogLevel::Warning: return WARN_COLOUR; + case LogLevel::Error: return ERROR_COLOUR; + case LogLevel::Success: return OK_COLOUR; + case LogLevel::Fail: return FAIL_COLOUR; + case LogLevel::Runtime: return RUNTIME_COLOUR; + case LogLevel::Output: + default: return OUTPUT_COLOUR; + } + } + QString levelString(LogLevel level) { + switch (level) { + case LogLevel::Trace: return "TRACE"; + case LogLevel::Debug: return "DEBUG"; + case LogLevel::Info: return "INFO"; + case LogLevel::Warning: return "WARN"; + case LogLevel::Error: return "ERROR"; + case LogLevel::Success: return "OK"; + case LogLevel::Fail: return "FAIL"; + case LogLevel::Runtime: return "RUNTIME"; + case LogLevel::Output: + default: return "OUTPUT"; + } + } + QColor simColour(simLogLevel level) { + switch (level) { + case simLogLevel::Rotate: return ROTATE_COLOUR; + case simLogLevel::Translate: return TRANSLATE_COLOUR; + case simLogLevel::Fail: return FAIL_COLOUR; + case simLogLevel::Error: return ERROR_COLOUR; + case simLogLevel::Success: return OK_COLOUR; + case simLogLevel::Runtime: + default: return RUNTIME_COLOUR; + } + } + + QString simString(simLogLevel level) { + switch (level) { + case simLogLevel::Rotate: return "ROTATE"; + case simLogLevel::Translate: return "TRANSLATE"; + case simLogLevel::Fail: return "FAIL"; + case simLogLevel::Error: return "ERROR"; + case simLogLevel::Success: return "SUCCESS"; + case simLogLevel::Runtime: + default: return "RUNTIME"; + } + } + + ConsoleOutputWidget::ConsoleOutputWidget(QWidget* parent) : QWidget(parent) { + auto* rootLayout = new QVBoxLayout(this); + + _tabs = new QTabWidget(this); + _terminalLog = new QTextEdit(this); + _terminalLog->setReadOnly(true); + _simLog = new QTextEdit(this); + _simLog->setReadOnly(true); + _tabs->addTab(_terminalLog, "Terminal"); + _tabs->addTab(_simLog, "Simulation"); + rootLayout->addWidget(_tabs); + + auto* buttonLayout = new QHBoxLayout(); + _clearTerminalButton = new QPushButton("Clear Terminal Log", this); + buttonLayout->addWidget(_clearTerminalButton); + rootLayout->addLayout(buttonLayout); + + connect(_clearTerminalButton, &QPushButton::clicked, this, [this]() { + gLog.Instance().clear(); + _terminalLog->clear(); + _lastTerminalCount = 0; + }); + + auto* timer = new QTimer(this); + connect(timer, &QTimer::timeout, this, &ConsoleOutputWidget::updateLog); + timer->start(100); // Update every 100 ms + } + + void ConsoleOutputWidget::clearSimLog() { + gLog.Instance().clearSimLog(); + _simLog->clear(); + _lastSimCount = 0; + } + + void ConsoleOutputWidget::updateLog() { + const auto& entries = gLog.Instance().Entries(); + while (_lastTerminalCount < entries.size()) { + const auto& e = entries[_lastTerminalCount]; + QTextCursor cursor = _terminalLog->textCursor(); + cursor.movePosition(QTextCursor::End); + QTextCharFormat lvlFmt; + lvlFmt.setForeground(levelColour(e.level)); + cursor.insertText(QString("[%1] ").arg(levelString(e.level)), lvlFmt); + QTextCharFormat msgFmt; + msgFmt.setForeground(QColor(220, 220, 220)); + cursor.insertText(QString::fromStdString(e.message), msgFmt); + cursor.insertBlock(); + ++_lastTerminalCount; + } + + const auto& simEntries = gLog.Instance().SimEntries(); + while (_lastSimCount < simEntries.size()) { + const auto& e = simEntries[_lastSimCount]; + QTextCursor cursor = _simLog->textCursor(); + cursor.movePosition(QTextCursor::End); + QTextCharFormat lvlFmt; + lvlFmt.setForeground(simColour(e.level)); + cursor.insertText(QString("[%1] ").arg(simString(e.level)), lvlFmt); + QTextCharFormat msgFmt; + msgFmt.setForeground(QColor(220, 220, 220)); + cursor.insertText(QString::fromStdString(e.message), msgFmt); + cursor.insertBlock(); + ++_lastSimCount; + } + + if (autoScroll) { + auto* tBar = _terminalLog->verticalScrollBar(); + tBar->setValue(tBar->maximum()); + auto* sBar = _simLog->verticalScrollBar(); + sBar->setValue(sBar->maximum()); + } + } +} // namespace widgets \ No newline at end of file From 87d89d65ea98c5c8455f36188e08b84cec680436 Mon Sep 17 00:00:00 2001 From: saltyjoss Date: Mon, 8 Jun 2026 15:51:34 +0100 Subject: [PATCH 134/210] feat: Linked up automatic sim log clearning with script execution --- .../include/MainWindow/Widgets/DSLEditorWidget.h | 6 +++++- .../include/MainWindow/Workspace/ProjectPage.h | 6 +++++- .../src/MainWindow/Widgets/DSLEditorWidget.cpp | 7 +++++-- .../DSFE_GUI/src/MainWindow/Workspace/ProjectPage.cpp | 10 ++++++---- 4 files changed, 21 insertions(+), 8 deletions(-) diff --git a/DSFE_App/DSFE_GUI/include/MainWindow/Widgets/DSLEditorWidget.h b/DSFE_App/DSFE_GUI/include/MainWindow/Widgets/DSLEditorWidget.h index 1ef4fd14..6af42e4b 100644 --- a/DSFE_App/DSFE_GUI/include/MainWindow/Widgets/DSLEditorWidget.h +++ b/DSFE_App/DSFE_GUI/include/MainWindow/Widgets/DSLEditorWidget.h @@ -33,9 +33,11 @@ namespace runs { } namespace widgets { + class ConsoleOutputWidget; + class DSLEditorWidget : public QWidget { public: - explicit DSLEditorWidget(gui::SimManager* sim, QWidget* parent = nullptr); + explicit DSLEditorWidget(gui::SimManager* sim, ConsoleOutputWidget* _log, QWidget* parent = nullptr); bool loadScript(const QString& fileName); bool saveScript(const QString& fileName); @@ -50,6 +52,8 @@ namespace widgets { interpreter::IStoredProgram* _program = nullptr; interpreter::RunWrapper* _wrapper = nullptr; + ConsoleOutputWidget* _log = nullptr; + void runScript(); void stopScript(); diff --git a/DSFE_App/DSFE_GUI/include/MainWindow/Workspace/ProjectPage.h b/DSFE_App/DSFE_GUI/include/MainWindow/Workspace/ProjectPage.h index bc338d3a..07e10138 100644 --- a/DSFE_App/DSFE_GUI/include/MainWindow/Workspace/ProjectPage.h +++ b/DSFE_App/DSFE_GUI/include/MainWindow/Workspace/ProjectPage.h @@ -4,7 +4,10 @@ #include namespace gui { class SimManager; } -namespace widgets { class DSLEditorWidget; } +namespace widgets { + class ConsoleOutputWidget; + class DSLEditorWidget; +} namespace Workspace { class ProjectPage : public QWidget { @@ -13,6 +16,7 @@ namespace Workspace { widgets::DSLEditorWidget* editor() const; private: + widgets::ConsoleOutputWidget* _log = nullptr; widgets::DSLEditorWidget* _editor = nullptr; }; } // namespace Workspace \ No newline at end of file diff --git a/DSFE_App/DSFE_GUI/src/MainWindow/Widgets/DSLEditorWidget.cpp b/DSFE_App/DSFE_GUI/src/MainWindow/Widgets/DSLEditorWidget.cpp index 77233689..e7d9b153 100644 --- a/DSFE_App/DSFE_GUI/src/MainWindow/Widgets/DSLEditorWidget.cpp +++ b/DSFE_App/DSFE_GUI/src/MainWindow/Widgets/DSLEditorWidget.cpp @@ -16,11 +16,13 @@ #include "Numerics/IntegrationMethods.h" #include "Interpreter/StoredProgram.h" +#include "Widgets/ConsoleOutputWidget.h" + #include "EngineLib/LogMacros.h" namespace widgets { - DSLEditorWidget::DSLEditorWidget(gui::SimManager* sim, QWidget* parent) - : QWidget(parent), _sim(sim), _scriptWorkingDir((paths::assets() / "DSLScripts").string()) + DSLEditorWidget::DSLEditorWidget(gui::SimManager* sim, ConsoleOutputWidget* log, QWidget* parent) + : QWidget(parent), _sim(sim), _log(log), _scriptWorkingDir((paths::assets() / "DSLScripts").string()) { auto* rootLayout = new QVBoxLayout(this); auto* scriptStateLayout = new QHBoxLayout(); @@ -159,6 +161,7 @@ namespace widgets { _scriptText = _scriptEditor->toPlainText().toStdString(); LOG_INFO("Script size = %zu", _scriptText.size()); + if (_log) { _log->clearSimLog(); } _sim->setActiveProgram(_program); _sim->setScriptRunning(true); LOG_INFO("DSL script started."); D_INFO("DSL script started."); diff --git a/DSFE_App/DSFE_GUI/src/MainWindow/Workspace/ProjectPage.cpp b/DSFE_App/DSFE_GUI/src/MainWindow/Workspace/ProjectPage.cpp index 70f7d168..341093c6 100644 --- a/DSFE_App/DSFE_GUI/src/MainWindow/Workspace/ProjectPage.cpp +++ b/DSFE_App/DSFE_GUI/src/MainWindow/Workspace/ProjectPage.cpp @@ -1,9 +1,11 @@ // DSFE_GUI ProjectPage.cpp #include "Workspace/ProjectPage.h" #include "Scene/SimulationManager.h" + +#include "Widgets/DSLEditorWidget.h" #include "Widgets/ViewportWidget.h" #include "Widgets/ControlPanelWidget.h" -#include "Widgets/DSLEditorWidget.h" +#include "Widgets/ConsoleOutputWidget.h" #include #include @@ -16,16 +18,16 @@ namespace Workspace { auto* rootSplitter = new QSplitter(Qt::Horizontal, this); auto* centreSplitter = new QSplitter(Qt::Vertical); auto* rightSplitter = new QSplitter(Qt::Vertical); - _editor = new widgets::DSLEditorWidget(sim, this); + _log = new widgets::ConsoleOutputWidget(this); + _editor = new widgets::DSLEditorWidget(sim, _log, this); rootSplitter->addWidget(_editor); centreSplitter->addWidget(new widgets::ViewportWidget(sim, this)); - centreSplitter->addWidget(new QLabel("Console Output (TODO)", this)); + centreSplitter->addWidget(_log); rightSplitter->addWidget(new widgets::ControlPanelWidget(sim, this)); rightSplitter->addWidget(new QLabel("Scene Object? (TODO)", this)); rootSplitter->addWidget(centreSplitter); rootSplitter->addWidget(rightSplitter); layout->addWidget(rootSplitter); - // Sizing reused from the old imgui layout rootSplitter->setSizes({ 635, 1016, 393 }); centreSplitter->setSizes({ 733, 396 }); rightSplitter->setSizes({ 733, 396 }); From 99a551a37b6e449d8894ad77cf721ff5ed827c63 Mon Sep 17 00:00:00 2001 From: saltyjoss Date: Mon, 8 Jun 2026 15:59:43 +0100 Subject: [PATCH 135/210] fixes: Removed scene objects * May revamp this at another time, but not needed for now. --- DSFE_App/DSFE_GUI/src/MainWindow/Workspace/ProjectPage.cpp | 1 - 1 file changed, 1 deletion(-) diff --git a/DSFE_App/DSFE_GUI/src/MainWindow/Workspace/ProjectPage.cpp b/DSFE_App/DSFE_GUI/src/MainWindow/Workspace/ProjectPage.cpp index 341093c6..180eb944 100644 --- a/DSFE_App/DSFE_GUI/src/MainWindow/Workspace/ProjectPage.cpp +++ b/DSFE_App/DSFE_GUI/src/MainWindow/Workspace/ProjectPage.cpp @@ -24,7 +24,6 @@ namespace Workspace { centreSplitter->addWidget(new widgets::ViewportWidget(sim, this)); centreSplitter->addWidget(_log); rightSplitter->addWidget(new widgets::ControlPanelWidget(sim, this)); - rightSplitter->addWidget(new QLabel("Scene Object? (TODO)", this)); rootSplitter->addWidget(centreSplitter); rootSplitter->addWidget(rightSplitter); layout->addWidget(rootSplitter); From 03b6979f98057cb9cd0958817475a3e63f7a6816 Mon Sep 17 00:00:00 2001 From: saltyjoss Date: Mon, 8 Jun 2026 16:04:35 +0100 Subject: [PATCH 136/210] feat: Added hard cap of terminal enteries --- .../MainWindow/Widgets/ConsoleOutputWidget.h | 26 +++++++++---------- .../Widgets/ConsoleOutputWidget.cpp | 20 +++++++------- 2 files changed, 22 insertions(+), 24 deletions(-) diff --git a/DSFE_App/DSFE_GUI/include/MainWindow/Widgets/ConsoleOutputWidget.h b/DSFE_App/DSFE_GUI/include/MainWindow/Widgets/ConsoleOutputWidget.h index 39938b00..5eec1936 100644 --- a/DSFE_App/DSFE_GUI/include/MainWindow/Widgets/ConsoleOutputWidget.h +++ b/DSFE_App/DSFE_GUI/include/MainWindow/Widgets/ConsoleOutputWidget.h @@ -11,18 +11,19 @@ class QTabWidget; class QPushButton; // Colours for log levels (Will eventually move to a separate file for global UI constants) -#define TRACE_COLOUR QColor(160, 160, 160) -#define DEBUG_COLOUR QColor(80, 170, 240) -#define INFO_COLOUR QColor(80, 160, 255) -#define WARN_COLOUR QColor(255, 180, 0) -#define ERROR_COLOUR QColor(220, 60, 70) -#define OK_COLOUR QColor(0, 190, 100) -#define FAIL_COLOUR QColor(170, 0, 50) -#define RUNTIME_COLOUR QColor(190, 120, 225) -#define OUTPUT_COLOUR QColor(220, 220, 220) - -#define ROTATE_COLOUR QColor(120, 180, 255) -#define TRANSLATE_COLOUR QColor(120, 255, 180) +constexpr QColor TRACE_COLOUR(160, 160, 160); +constexpr QColor DEBUG_COLOUR(80, 170, 240); +constexpr QColor INFO_COLOUR(80, 160, 255); +constexpr QColor WARN_COLOUR(255, 180, 0); +constexpr QColor ERROR_COLOUR(220, 60, 70); +constexpr QColor OK_COLOUR(0, 190, 100); +constexpr QColor FAIL_COLOUR(170, 0, 50); +constexpr QColor RUNTIME_COLOUR(190, 120, 225); +constexpr QColor OUTPUT_COLOUR(220, 220, 220); +constexpr QColor ROTATE_COLOUR(120, 180, 255); +constexpr QColor TRANSLATE_COLOUR(120, 255, 180); + +constexpr size_t MAX_TERMINAL_ENTRIES = 10000; // Maximum number of terminal entries to keep in memory for display namespace widgets { class ConsoleOutputWidget : public QWidget { @@ -35,7 +36,6 @@ namespace widgets { QTabWidget* _tabs = nullptr; QTextEdit* _terminalLog = nullptr; QTextEdit* _simLog = nullptr; - QPushButton* _clearTerminalButton = nullptr; size_t _lastTerminalCount = 0; size_t _lastSimCount = 0; diff --git a/DSFE_App/DSFE_GUI/src/MainWindow/Widgets/ConsoleOutputWidget.cpp b/DSFE_App/DSFE_GUI/src/MainWindow/Widgets/ConsoleOutputWidget.cpp index e0faa1e1..a1992f4f 100644 --- a/DSFE_App/DSFE_GUI/src/MainWindow/Widgets/ConsoleOutputWidget.cpp +++ b/DSFE_App/DSFE_GUI/src/MainWindow/Widgets/ConsoleOutputWidget.cpp @@ -78,17 +78,6 @@ namespace widgets { _tabs->addTab(_simLog, "Simulation"); rootLayout->addWidget(_tabs); - auto* buttonLayout = new QHBoxLayout(); - _clearTerminalButton = new QPushButton("Clear Terminal Log", this); - buttonLayout->addWidget(_clearTerminalButton); - rootLayout->addLayout(buttonLayout); - - connect(_clearTerminalButton, &QPushButton::clicked, this, [this]() { - gLog.Instance().clear(); - _terminalLog->clear(); - _lastTerminalCount = 0; - }); - auto* timer = new QTimer(this); connect(timer, &QTimer::timeout, this, &ConsoleOutputWidget::updateLog); timer->start(100); // Update every 100 ms @@ -131,6 +120,15 @@ namespace widgets { ++_lastSimCount; } + while (_terminalLog->document()->blockCount() > MAX_TERMINAL_ENTRIES) { + QTextCursor cursor = _terminalLog->textCursor(); + cursor.movePosition(QTextCursor::Start); + cursor.select(QTextCursor::BlockUnderCursor); + cursor.removeSelectedText(); + cursor.deleteChar(); // Remove the block itself + --_lastTerminalCount; // Adjust count since we've removed an entry + } + if (autoScroll) { auto* tBar = _terminalLog->verticalScrollBar(); tBar->setValue(tBar->maximum()); From 93a8e0044df4aaae3b0e96e4ba29f3c67be0dc29 Mon Sep 17 00:00:00 2001 From: saltyjoss Date: Mon, 8 Jun 2026 16:21:52 +0100 Subject: [PATCH 137/210] fixes: Corrected taskbar duplciation error --- .../src/MainWindow/DSFE_MainWindow.cpp | 50 ++++++++----------- .../Widgets/ConsoleOutputWidget.cpp | 7 ++- 2 files changed, 28 insertions(+), 29 deletions(-) diff --git a/DSFE_App/DSFE_GUI/src/MainWindow/DSFE_MainWindow.cpp b/DSFE_App/DSFE_GUI/src/MainWindow/DSFE_MainWindow.cpp index 2c978d9c..e9aa08f4 100644 --- a/DSFE_App/DSFE_GUI/src/MainWindow/DSFE_MainWindow.cpp +++ b/DSFE_App/DSFE_GUI/src/MainWindow/DSFE_MainWindow.cpp @@ -50,37 +50,31 @@ namespace window { }); }); auto* openMenu = fileMenu->addMenu("Open"); - connect(openMenu, &QMenu::aboutToShow, this, [this, openMenu]() { - LOG_INFO("Menu clicked: File -> Open"); - auto* openProjectAction = openMenu->addAction("Project"); - connect(openProjectAction, &QAction::triggered, this, []() { - LOG_INFO("Menu clicked: File -> Open -> Project"); - }); - openMenu->addSeparator(); - auto* openScriptAction = openMenu->addAction("Script (*.dsl *.txt)"); - connect(openScriptAction, &QAction::triggered, this, [this]() { - QString fileName = QFileDialog::getOpenFileName(nullptr, "Open Script", QString::fromStdString((paths::assets() / "DSLScripts").string()), "DSL Script Files (*.dsl);;Text Files (*.txt)"); - if (fileName.isEmpty()) { return; } - if (!_dslEditor) { LOG_ERROR("DSL Editor not found!"); return; } - _dslEditor->loadScript(fileName); - }); + auto* openProjectAction = openMenu->addAction("Project"); + connect(openProjectAction, &QAction::triggered, this, []() { + LOG_INFO("Menu clicked: File -> Open -> Project"); + }); + openMenu->addSeparator(); + auto* openScriptAction = openMenu->addAction("Script (*.dsl *.txt)"); + connect(openScriptAction, &QAction::triggered, this, [this]() { + QString fileName = QFileDialog::getOpenFileName(nullptr, "Open Script", QString::fromStdString((paths::assets() / "DSLScripts").string()), "DSL Script Files (*.dsl);;Text Files (*.txt)"); + if (fileName.isEmpty()) { return; } + if (!_dslEditor) { LOG_ERROR("DSL Editor not found!"); return; } + _dslEditor->loadScript(fileName); }); fileMenu->addSeparator(); auto* saveMenu = fileMenu->addMenu("Save"); - connect(saveMenu, &QMenu::aboutToShow, this, [this, saveMenu]() { - LOG_INFO("Menu clicked: File -> Save"); - auto* saveProjectAction = saveMenu->addAction("Project"); - connect(saveProjectAction, &QAction::triggered, this, []() { - LOG_INFO("Menu clicked: File -> Save -> Project"); - }); - auto* saveScriptAction = saveMenu->addAction("Script"); - connect(saveScriptAction, &QAction::triggered, this, [this]() { - LOG_INFO("Menu clicked: File -> Save -> Script"); - QString fileName = QFileDialog::getSaveFileName(nullptr, "Save Script", QString::fromStdString((paths::assets() / "DSLScripts").string()), "DSL Script Files (*.dsl);;Text Files (*.txt)"); // ARGS are - if (fileName.isEmpty()) { return; } - if (!_dslEditor) { LOG_ERROR("DSL Editor not found!"); return; } - _dslEditor->saveScript(fileName); - }); + auto* saveProjectAction = saveMenu->addAction("Project"); + connect(saveProjectAction, &QAction::triggered, this, []() { + LOG_INFO("Menu clicked: File -> Save -> Project"); + }); + auto* saveScriptAction = saveMenu->addAction("Script"); + connect(saveScriptAction, &QAction::triggered, this, [this]() { + LOG_INFO("Menu clicked: File -> Save -> Script"); + QString fileName = QFileDialog::getSaveFileName(nullptr, "Save Script", QString::fromStdString((paths::assets() / "DSLScripts").string()), "DSL Script Files (*.dsl);;Text Files (*.txt)"); // ARGS are + if (fileName.isEmpty()) { return; } + if (!_dslEditor) { LOG_ERROR("DSL Editor not found!"); return; } + _dslEditor->saveScript(fileName); }); auto* saveAsMenu = fileMenu->addMenu("Save As"); connect(saveAsMenu, &QMenu::aboutToShow, this, [this, saveAsMenu]() { diff --git a/DSFE_App/DSFE_GUI/src/MainWindow/Widgets/ConsoleOutputWidget.cpp b/DSFE_App/DSFE_GUI/src/MainWindow/Widgets/ConsoleOutputWidget.cpp index a1992f4f..3a6a6713 100644 --- a/DSFE_App/DSFE_GUI/src/MainWindow/Widgets/ConsoleOutputWidget.cpp +++ b/DSFE_App/DSFE_GUI/src/MainWindow/Widgets/ConsoleOutputWidget.cpp @@ -106,6 +106,7 @@ namespace widgets { } const auto& simEntries = gLog.Instance().SimEntries(); + if (_lastSimCount > simEntries.size()) { _lastSimCount = 0; _simLog->clear(); } while (_lastSimCount < simEntries.size()) { const auto& e = simEntries[_lastSimCount]; QTextCursor cursor = _simLog->textCursor(); @@ -126,7 +127,11 @@ namespace widgets { cursor.select(QTextCursor::BlockUnderCursor); cursor.removeSelectedText(); cursor.deleteChar(); // Remove the block itself - --_lastTerminalCount; // Adjust count since we've removed an entry + } + + if (_lastTerminalCount > entries.size()) { + _terminalLog->clear(); + _lastTerminalCount = 0; } if (autoScroll) { From 44b3d0f1e92f82ed0d7ecb4982b95c637125f66b Mon Sep 17 00:00:00 2001 From: saltyjoss Date: Mon, 8 Jun 2026 18:54:06 +0100 Subject: [PATCH 138/210] feat: Added rudamentery syntax highlighting of DSL scripts --- .../include/MainWindow/DSL/DSLSyntaxColours.h | 14 +++++ .../MainWindow/DSL/DSLSyntaxHighlighter.h | 32 ++++++++++ .../MainWindow/DSL/DSLSyntaxHighlighter.cpp | 59 +++++++++++++++++++ 3 files changed, 105 insertions(+) create mode 100644 DSFE_App/DSFE_GUI/include/MainWindow/DSL/DSLSyntaxColours.h create mode 100644 DSFE_App/DSFE_GUI/include/MainWindow/DSL/DSLSyntaxHighlighter.h create mode 100644 DSFE_App/DSFE_GUI/src/MainWindow/DSL/DSLSyntaxHighlighter.cpp diff --git a/DSFE_App/DSFE_GUI/include/MainWindow/DSL/DSLSyntaxColours.h b/DSFE_App/DSFE_GUI/include/MainWindow/DSL/DSLSyntaxColours.h new file mode 100644 index 00000000..8c66af28 --- /dev/null +++ b/DSFE_App/DSFE_GUI/include/MainWindow/DSL/DSLSyntaxColours.h @@ -0,0 +1,14 @@ +// DSFE_GUI DSLSyntaxColours.h +#pragma once + +#include + +const std::vector COMMAND_COL{ 86, 156, 214 }; +const std::vector COMMENT_COL{ 106, 153, 85 }; +const std::vector NUMBER_COL{ 220, 170, 90 }; +const std::vector TYPE_COL{ 197, 134, 192 }; +const std::vector ID_COL{ 78, 201, 176}; +const std::vector BRACKETS_COL{ 23, 159, 255 }; +const std::vector KEYWORD_COL{ 255, 215, 0 }; +const std::vector STRING_COL{ 214, 157, 133 }; +const std::vector ERROR_COL{ 255, 80, 80 }; \ No newline at end of file diff --git a/DSFE_App/DSFE_GUI/include/MainWindow/DSL/DSLSyntaxHighlighter.h b/DSFE_App/DSFE_GUI/include/MainWindow/DSL/DSLSyntaxHighlighter.h new file mode 100644 index 00000000..3742bba9 --- /dev/null +++ b/DSFE_App/DSFE_GUI/include/MainWindow/DSL/DSLSyntaxHighlighter.h @@ -0,0 +1,32 @@ +// DSFE_GUI DSLSyntaxHighlighter.h +#pragma once + +#include +#include + +class QColor; + +namespace widgets { + + class DSLSyntaxHighlighter : public QSyntaxHighlighter { + public: + explicit DSLSyntaxHighlighter(QTextDocument* parent = nullptr); + + protected: + void highlightBlock(const QString& text) override; + + private: + template + QColor makeQColor(const Container& c) { + if (c.size() < 3) return QColor(); // Returns an invalid color safely + return QColor(c[0], c[1], c[2]); + } + + struct HighlightingRule { + QRegularExpression pattern; + QTextCharFormat format; + }; + std::vector _rules; + }; + +} \ No newline at end of file diff --git a/DSFE_App/DSFE_GUI/src/MainWindow/DSL/DSLSyntaxHighlighter.cpp b/DSFE_App/DSFE_GUI/src/MainWindow/DSL/DSLSyntaxHighlighter.cpp new file mode 100644 index 00000000..f2e5bace --- /dev/null +++ b/DSFE_App/DSFE_GUI/src/MainWindow/DSL/DSLSyntaxHighlighter.cpp @@ -0,0 +1,59 @@ +// DSFE_GUI DSLSyntaxHighlighter.cpp +#include "DSL/DSLSyntaxHighlighter.h" +#include "DSL/DSLSyntaxColours.h" + +namespace widgets { + DSLSyntaxHighlighter::DSLSyntaxHighlighter(QTextDocument* parent) : QSyntaxHighlighter(parent) { + QTextCharFormat commentFmt; + commentFmt.setForeground(makeQColor(COMMENT_COL)); + _rules.push_back({ QRegularExpression("#.*$"), commentFmt }); + + QTextCharFormat cmdFmt; + cmdFmt.setForeground(makeQColor(COMMAND_COL)); + cmdFmt.setFontWeight(QFont::Bold); + + QStringList commands = { + "load", + "start", + "stop", + "trajSet", + "trajClear", + "rotateJointTo", + "rotateJointBy", + "parallel", + "set" + }; + + for (const auto& cmd : commands) { _rules.push_back({ QRegularExpression("\\b" + cmd + "\\b"), cmdFmt }); } + + QTextCharFormat numFmt; + numFmt.setForeground(makeQColor(NUMBER_COL)); + _rules.push_back({ QRegularExpression(R"(\b[-+]?[0-9]*\.?[0-9]+\b)"), numFmt }); + + QTextCharFormat trajFmt; + trajFmt.setForeground(makeQColor(TYPE_COL)); + QStringList trajTypes = { "TRAP", "SINE", "MSINE", "MULTISINE" }; + for (const auto& type : trajTypes) { _rules.push_back({ QRegularExpression("\\b" + type + "\\b"), trajFmt }); } + + QTextCharFormat idFmt; + idFmt.setForeground(makeQColor(ID_COL)); + _rules.push_back({ QRegularExpression(R"(\blink[0-9]+\b)"), idFmt }); + } + + void DSLSyntaxHighlighter::highlightBlock(const QString& text) { + int commentPos = text.indexOf('#'); + QString codeText = (commentPos >= 0) ? text.left(commentPos) : text; + + for (const auto& rule : _rules) { + auto it = rule.pattern.globalMatch(codeText); + while (it.hasNext()) { + auto match = it.next(); + setFormat(match.capturedStart(), match.capturedLength(), rule.format); + } + } + + if (commentPos >= 0) { + setFormat(commentPos, text.length() - commentPos, _rules[0].format); // Comment format is the first rule + } + } +} \ No newline at end of file From 7b48fb0099aecd1cbce7334d4264e3539a4543d0 Mon Sep 17 00:00:00 2001 From: saltyjoss Date: Mon, 8 Jun 2026 18:56:31 +0100 Subject: [PATCH 139/210] feat: Integrated DSL syntax highlighting into `DSLEditorWidget` --- DSFE_App/DSFE_GUI/include/MainWindow/Widgets/DSLEditorWidget.h | 2 ++ DSFE_App/DSFE_GUI/src/MainWindow/Widgets/DSLEditorWidget.cpp | 2 ++ 2 files changed, 4 insertions(+) diff --git a/DSFE_App/DSFE_GUI/include/MainWindow/Widgets/DSLEditorWidget.h b/DSFE_App/DSFE_GUI/include/MainWindow/Widgets/DSLEditorWidget.h index 6af42e4b..f5a03f1e 100644 --- a/DSFE_App/DSFE_GUI/include/MainWindow/Widgets/DSLEditorWidget.h +++ b/DSFE_App/DSFE_GUI/include/MainWindow/Widgets/DSLEditorWidget.h @@ -34,6 +34,7 @@ namespace runs { namespace widgets { class ConsoleOutputWidget; + class DSLSyntaxHighlighter; class DSLEditorWidget : public QWidget { public: @@ -53,6 +54,7 @@ namespace widgets { interpreter::RunWrapper* _wrapper = nullptr; ConsoleOutputWidget* _log = nullptr; + DSLSyntaxHighlighter* _highlighter = nullptr; void runScript(); void stopScript(); diff --git a/DSFE_App/DSFE_GUI/src/MainWindow/Widgets/DSLEditorWidget.cpp b/DSFE_App/DSFE_GUI/src/MainWindow/Widgets/DSLEditorWidget.cpp index e7d9b153..d77e3540 100644 --- a/DSFE_App/DSFE_GUI/src/MainWindow/Widgets/DSLEditorWidget.cpp +++ b/DSFE_App/DSFE_GUI/src/MainWindow/Widgets/DSLEditorWidget.cpp @@ -17,6 +17,7 @@ #include "Interpreter/StoredProgram.h" #include "Widgets/ConsoleOutputWidget.h" +#include "DSL/DSLSyntaxHighlighter.h" #include "EngineLib/LogMacros.h" @@ -112,6 +113,7 @@ namespace widgets { _editorTab = new QWidget(this); auto* layout = new QVBoxLayout(_editorTab); _scriptEditor = new QTextEdit(_editorTab); + _highlighter = new DSLSyntaxHighlighter(_scriptEditor->document()); layout->addWidget(_scriptEditor); _tabs->addTab(_editorTab, "Script Editor"); } From 4d79bd677bf4deae26b684d146a524aedf1a50d4 Mon Sep 17 00:00:00 2001 From: saltyjoss Date: Mon, 8 Jun 2026 19:40:01 +0100 Subject: [PATCH 140/210] feat: Added new styling format, using .qss style file utilised by `qresource` --- DSFE_App/DSFE_GUI/CMakeLists.txt | 8 + .../MainWindow/Widgets/ControlPanelWidget.h | 1 - .../include/MainWindow/style/DSFETheme.h | 7 + DSFE_App/DSFE_GUI/resources/DSFE_GUI.qrc | 5 + .../DSFE_GUI/resources/style/dsfe_dark.qss | 142 +++++++++++++ DSFE_App/DSFE_GUI/src/Application.cpp | 2 + .../MainWindow/Widgets/ControlPanelWidget.cpp | 200 ++++++++++++------ .../src/MainWindow/style/DSFETheme.cpp | 30 +++ 8 files changed, 324 insertions(+), 71 deletions(-) create mode 100644 DSFE_App/DSFE_GUI/include/MainWindow/style/DSFETheme.h create mode 100644 DSFE_App/DSFE_GUI/resources/DSFE_GUI.qrc create mode 100644 DSFE_App/DSFE_GUI/resources/style/dsfe_dark.qss create mode 100644 DSFE_App/DSFE_GUI/src/MainWindow/style/DSFETheme.cpp diff --git a/DSFE_App/DSFE_GUI/CMakeLists.txt b/DSFE_App/DSFE_GUI/CMakeLists.txt index dcd44548..5b6ce6b6 100644 --- a/DSFE_App/DSFE_GUI/CMakeLists.txt +++ b/DSFE_App/DSFE_GUI/CMakeLists.txt @@ -40,6 +40,7 @@ target_include_directories(stb INTERFACE set(MAIN_WINDOW_SRC src/MainWindow/DSFE_MainWindow.cpp src/MainWindow/Workspace/ProjectPage.cpp + src/MainWindow/style/DSFETheme.cpp ) set(RENDER_SRC @@ -79,6 +80,7 @@ set(WIDGETS_SRC src/MainWindow/Widgets/ConsoleOutputWidget.cpp src/MainWindow/Widgets/FractionSelectorWidget.cpp include/MainWindow/Widgets/FractionSelectorWidget.h + src/MainWindow/DSL/DSLSyntaxHighlighter.cpp ) set(ROBOT_SRC @@ -96,6 +98,10 @@ set(ASSETS_SRC src/Assets/importObj.cpp ) +set(QT_RESOURCES + resources/DSFE_GUI.qrc +) + target_sources(DSFE_GUI PRIVATE ${MAIN_WINDOW_SRC} ${RENDER_SRC} @@ -105,6 +111,7 @@ target_sources(DSFE_GUI PRIVATE ${ROBOT_SRC} ${PLATFORM_SRC} ${ASSETS_SRC} + ${QT_RESOURCES} ) set(imfilebrowser_ROOT ${CMAKE_CURRENT_SOURCE_DIR}/include/imfilebrowser) @@ -142,6 +149,7 @@ target_include_directories(DSFE_GUI ${CMAKE_CURRENT_SOURCE_DIR}/include ${CMAKE_CURRENT_SOURCE_DIR}/include/MainWindow ${CMAKE_CURRENT_SOURCE_DIR}/include/Scene + ${CMAKE_CURRENT_SOURCE_DIR}/include/MainWindow/style PRIVATE ${imfilebrowser_ROOT} diff --git a/DSFE_App/DSFE_GUI/include/MainWindow/Widgets/ControlPanelWidget.h b/DSFE_App/DSFE_GUI/include/MainWindow/Widgets/ControlPanelWidget.h index 15b71b1c..81f77b2c 100644 --- a/DSFE_App/DSFE_GUI/include/MainWindow/Widgets/ControlPanelWidget.h +++ b/DSFE_App/DSFE_GUI/include/MainWindow/Widgets/ControlPanelWidget.h @@ -112,7 +112,6 @@ namespace widgets { QGroupBox* _jointInfoGroup = nullptr; QSlider* _jointIdxSlider = nullptr; - QLabel* _currentSimTimeJointLabel = nullptr; static constexpr IntegratorEntry integrators[] = { diff --git a/DSFE_App/DSFE_GUI/include/MainWindow/style/DSFETheme.h b/DSFE_App/DSFE_GUI/include/MainWindow/style/DSFETheme.h new file mode 100644 index 00000000..7e870b2e --- /dev/null +++ b/DSFE_App/DSFE_GUI/include/MainWindow/style/DSFETheme.h @@ -0,0 +1,7 @@ +// DSFE_GUI DSFETheme.h +#pragma once + +class QApplication; +namespace style { + void applyTheme(QApplication& app); +} \ No newline at end of file diff --git a/DSFE_App/DSFE_GUI/resources/DSFE_GUI.qrc b/DSFE_App/DSFE_GUI/resources/DSFE_GUI.qrc new file mode 100644 index 00000000..34a1a146 --- /dev/null +++ b/DSFE_App/DSFE_GUI/resources/DSFE_GUI.qrc @@ -0,0 +1,5 @@ + + + style/dsfe_dark.qss + + \ No newline at end of file diff --git a/DSFE_App/DSFE_GUI/resources/style/dsfe_dark.qss b/DSFE_App/DSFE_GUI/resources/style/dsfe_dark.qss new file mode 100644 index 00000000..fa160758 --- /dev/null +++ b/DSFE_App/DSFE_GUI/resources/style/dsfe_dark.qss @@ -0,0 +1,142 @@ +QMainWindow { + background-color: rgb(36,36,36); +} + +QWidget { + background-color: rgb(36,36,36); + color: rgb(220,220,220); +} + +QGroupBox { + background-color: rgb(40,40,40); + border: 1px solid rgb(70,70,70); + border-radius: 4px; + margin-top: 8px; + padding-top: 10px; + font-weight: bold; +} + +QGroupBox::title { + subcontrol-origin: margin; + left: 8px; + padding: 0px 4px; + color: rgb(220,220,220); +} + +QTextEdit { + background-color: rgb(26,26,26); + border: 1px solid rgb(70,70,70); + color: rgb(220,220,220); + selection-background-color: rgb(86,156,214); + font-family: Consolas, monospace; + font-size: 12px; +} + +QPlainTextEdit { + background-color: rgb(26,26,26); + border: 1px solid rgb(70,70,70); + color: rgb(220,220,220); + selection-background-color: rgb(86,156,214); + font-family: Consolas, monospace; + font-size: 12px; +} + +QTabWidget::pane { + border: 1px solid rgb(70,70,70); + background-color: rgb(36,36,36); +} + +QTabBar::tab { + background-color: rgb(40,40,40); + border: 1px solid rgb(70,70,70); + padding: 4px 8px; +} + +QTabBar::tab:selected { + background-color: rgb(55,55,55); +} + +QMenuBar { + background-color: rgb(36,36,36); +} + +QMenuBar::item { + background: transparent; + padding: 4px 8px; +} + +QMenuBar::item:selected { + background-color: rgb(55,55,55); +} + +QMenu { + background-color: rgb(40,40,40); + border: 1px solid rgb(70,70,70); +} + +QMenu::item:selected { + background-color: rgb(86,156,214); +} + +QComboBox { + background-color: rgb(40,40,40); + border: 1px solid rgb(70,70,70); + border-radius: 3px; + padding: 2px 6px; + min-height: 20px; +} + +QComboBox::drop-down { + border: none; +} + +QPushButton { + background-color: rgb(50,50,50); + border: 1px solid rgb(70,70,70); + border-radius: 3px; + padding: 4px 10px; +} + +QPushButton:hover { + background-color: rgb(65,65,65); +} + +QPushButton:pressed { + background-color: rgb(86,156,214); +} + +QScrollBar:vertical { + background-color: rgb(30,30,30); + width: 12px; +} + +QScrollBar::handle:vertical { + background-color: rgb(80,80,80); + min-height: 20px; +} + +QSlider::groove:horizontal { + height: 4px; + background-color: rgb(70,70,70); + border-radius: 2px; +} + +QSlider::sub-page:horizontal { + background-color: rgb(86,156,214); + border-radius: 2px; +} + +QSlider::handle:horizontal { + background-color: rgb(220,220,220); + width: 12px; + margin: -5px 0px; + border-radius: 6px; +} + +QCheckBox { + spacing: 6px; +} + +QLabel { + background-color: transparent; +} \ No newline at end of file diff --git a/DSFE_App/DSFE_GUI/src/Application.cpp b/DSFE_App/DSFE_GUI/src/Application.cpp index 566b5e38..424b0761 100644 --- a/DSFE_App/DSFE_GUI/src/Application.cpp +++ b/DSFE_App/DSFE_GUI/src/Application.cpp @@ -11,6 +11,7 @@ #include #include #include +#include "MainWindow/style/DSFETheme.h" namespace fs = std::filesystem; // Initialise the static instance pointer to nullptr @@ -46,6 +47,7 @@ Application::Application(const std::string& appName) : _name(appName) { QSurfaceFormat::setDefaultFormat(format); _qtApp = std::make_unique(_qtArgc, _qtArgv.data()); + style::applyTheme(*_qtApp); _sim = std::make_unique(); int winW = 1920, winH = 1080; diff --git a/DSFE_App/DSFE_GUI/src/MainWindow/Widgets/ControlPanelWidget.cpp b/DSFE_App/DSFE_GUI/src/MainWindow/Widgets/ControlPanelWidget.cpp index 36d5b7f8..f8ebd9bb 100644 --- a/DSFE_App/DSFE_GUI/src/MainWindow/Widgets/ControlPanelWidget.cpp +++ b/DSFE_App/DSFE_GUI/src/MainWindow/Widgets/ControlPanelWidget.cpp @@ -11,8 +11,8 @@ #include #include #include - -#include +#include +#include #include "Widgets/FractionSelectorWidget.h" @@ -62,32 +62,51 @@ namespace widgets { _simPropertiesGroup = new QGroupBox("Simulation Properties"); auto* layout = new QVBoxLayout(_simPropertiesGroup); - - _useAutoDiffCheck = new QCheckBox("Enable Automatic Differentiable Integrators"); - layout->addWidget(_useAutoDiffCheck); - _integratorCombo = new QComboBox(); - layout->addWidget(_integratorCombo); - - _currentIntegratorLabel = new QLabel(); - layout->addWidget(_currentIntegratorLabel); - layout->addSpacing(5); + _useAutoDiffCheck = new QCheckBox(); + _integratorCombo = new QComboBox(); + _integratorCombo->setMaximumWidth(175); _simDtSelector = new FractionSelectorWidget(false); - layout->addWidget(new QLabel("Simulation Time Step (dt):")); - layout->addWidget(_simDtSelector); _telemetryDtSelector = new FractionSelectorWidget(true); - layout->addWidget(new QLabel("Telemetry Time Step (dt):")); - layout->addWidget(_telemetryDtSelector); + _simTimeLabel = new QLabel(); - _simTimeLabel->setTextFormat(Qt::RichText); _simTimeLabel->setWordWrap(true); - layout->addWidget(new QLabel("Simulation Time:")); - layout->addWidget(_simTimeLabel); + auto* form = new QFormLayout(); + + form->addRow("Auto Diff", _useAutoDiffCheck); + form->addRow("Integrator", _integratorCombo); + + layout->addLayout(form); + + layout->addSpacing(8); + + auto* dtHeaderRow = new QHBoxLayout(); + + auto* simDtLabel = new QLabel("Simulation dt"); + simDtLabel->setAlignment(Qt::AlignCenter); + + auto* telemetryDtLabel = new QLabel("Telemetry dt"); + telemetryDtLabel->setAlignment(Qt::AlignCenter); + + dtHeaderRow->addWidget(simDtLabel); + dtHeaderRow->addWidget(telemetryDtLabel); + + layout->addLayout(dtHeaderRow); + + auto* dtValueRow = new QHBoxLayout(); + + dtValueRow->addWidget(_simDtSelector); + dtValueRow->addWidget(_telemetryDtSelector); + layout->addLayout(dtValueRow); + layout->addSpacing(8); + + layout->addWidget(_simTimeLabel); _contentLayout->addWidget(_simPropertiesGroup); buildIntegratorCombos(); + connect(_useAutoDiffCheck, &QCheckBox::toggled, this, [this, robot](bool checked) { _useAutoDiff = checked; robot->enableAutoDiff(checked); @@ -98,19 +117,15 @@ namespace widgets { if (_useAutoDiff) { auto selectedMethod = static_cast(_integratorCombo->currentData().toInt()); _sim->setADIntegrationMethod(selectedMethod); - _currentIntegratorLabel->setWordWrap(true); - _currentIntegratorLabel->setText(QString("Current Integrator: ") + _integratorCombo->currentText()); } else { auto selectedMethod = static_cast(_integratorCombo->currentData().toInt()); _sim->setIntegrationMethod(selectedMethod); - _currentIntegratorLabel->setWordWrap(true); - _currentIntegratorLabel->setText(QString("Current Integrator: ") + _integratorCombo->currentText()); } }); connect(_simDtSelector, &FractionSelectorWidget::valueChanged, this, [this](double dt) { _sim->setFixedDt(dt); }); - connect(_telemetryDtSelector, &FractionSelectorWidget::valueChanged, this, [this](double dt) { _sim->setTelemetryHz(1.0/dt); }); + connect(_telemetryDtSelector, &FractionSelectorWidget::valueChanged, this, [this](double dt) { _sim->setTelemetryHz(1.0 / dt); }); } void ControlPanelWidget::buildIntegratorCombos() { @@ -143,13 +158,14 @@ namespace widgets { _jointInfoGroup = new QGroupBox("Robot Joint Information"); auto* layout = new QVBoxLayout(_jointInfoGroup); _jointInfoGroup->setLayout(layout); - _currentSimTimeJointLabel = new QLabel(_jointInfoGroup); - layout->addWidget(_currentSimTimeJointLabel); + layout->addWidget(new QLabel("Joint")); _jointIdxSlider = new QSlider(Qt::Horizontal, _jointInfoGroup); _jointIdxSlider->setMinimum(1); _jointIdxSlider->setValue(1); + connect(_jointIdxSlider, &QSlider::valueChanged, this, [this](int value) { selectJointAndFollow(value - 1); }); + layout->addWidget(_jointIdxSlider); buildTelemetryWidgets(layout); @@ -182,41 +198,42 @@ namespace widgets { auto& t = _telemetryLabels; const float e = static_cast(j.q_ref - j.q); - t.q->setText(QString("pos:\t%1 rad").arg(j.q)); - t.qd->setText(QString("vel:\t%1 rad/s").arg(j.qd)); - t.tau->setText(QString("torque:\t%1 Nm").arg(j.torqueNm)); + t.q->setText(QString("%1 rad").arg(j.q)); + t.qd->setText(QString("%1 rad/s").arg(j.qd)); + t.tau->setText(QString("%1 Nm").arg(j.torqueNm)); - t.qRef->setText(QString("pos_ref:\t%1 rad").arg(j.q_ref)); - t.qdRef->setText(QString("vel_ref:\t%1 rad/s").arg(j.qd_ref)); - t.qddRef->setText(QString("acc_ref:\t%1 rad/s²").arg(j.qdd_ref)); - t.err->setText(QString("error:\t%1 rad").arg(e)); + t.qRef->setText(QString("%1 rad").arg(j.q_ref)); + t.qdRef->setText(QString("%1 rad/s").arg(j.qd_ref)); + t.qddRef->setText(QString("%1 rad/s²").arg(j.qdd_ref)); + t.err->setText(QString("%1 rad").arg(e)); - t.qTraj->setText(QString("pos_traj:\t%1 rad").arg(j.traj_q)); - t.qdTraj->setText(QString("vel_traj:\t%1 rad/s").arg(j.traj_qd)); - t.qddTraj->setText(QString("acc_traj:\t%1 rad/s²").arg(j.traj_qdd)); + t.qTraj->setText(QString("%1 rad").arg(j.traj_q)); + t.qdTraj->setText(QString("%1 rad/s").arg(j.traj_qd)); + t.qddTraj->setText(QString("%1 rad/s²").arg(j.traj_qdd)); - t.qClamped->setText(QString("pos_clamped:\t%1").arg(j.clampTheta ? "true" : "false")); - t.qdClamped->setText(QString("vel_clamped:\t%1").arg(j.clampOmega ? "true" : "false")); + t.qClamped->setText(j.clampTheta ? "On" : "Off"); + t.qdClamped->setText(j.clampOmega ? "On" : "Off"); - t.damping->setText(QString("damping:\t%1 kg·m²/s").arg(j.damping)); - t.friction->setText(QString("friction:\t%1 N·m").arg(j.friction)); + t.damping->setText(QString("%1 kg·m²/s").arg(j.damping)); + t.friction->setText(QString("%1 N·m").arg(j.friction)); } void ControlPanelWidget::buildTelemetryWidgets(QVBoxLayout* layout) { auto headerFont = [](QLabel* label) { QFont font = label->font(); font.setBold(true); - font.setPointSize(font.pointSize() + 2); + font.setPointSize(font.pointSize() + 1); label->setFont(font); + label->setStyleSheet("color: rgb(220,220,220);"); }; auto& t = _telemetryLabels; - t.stateHeader = new QLabel("State:"); - t.referenceHeader = new QLabel("Reference:"); - t.trajectoryHeader = new QLabel("Trajectory:"); - t.clampedHeader = new QLabel("Clamped:"); - t.constantsHeader = new QLabel("Constants:"); + t.stateHeader = new QLabel("STATE"); + t.referenceHeader = new QLabel("REFERENCE"); + t.trajectoryHeader = new QLabel("TRAJECTORY"); + t.clampedHeader = new QLabel("LIMITS"); + t.constantsHeader = new QLabel("PHYSICAL"); headerFont(t.stateHeader); headerFont(t.referenceHeader); @@ -225,7 +242,7 @@ namespace widgets { headerFont(t.constantsHeader); t.q = new QLabel(); - t.qd= new QLabel(); + t.qd = new QLabel(); t.tau = new QLabel(); t.qRef = new QLabel(); @@ -243,39 +260,83 @@ namespace widgets { t.damping = new QLabel(); t.friction = new QLabel(); - layout->addSpacing(5); - - layout->addWidget(t.stateHeader); - layout->addWidget(t.q); - layout->addWidget(t.qd); - layout->addWidget(t.tau); + auto* stateGrid = new QGridLayout(); + stateGrid->addWidget(new QLabel("Position"), 0, 0); + stateGrid->addWidget(t.q, 0, 1); + stateGrid->addWidget(new QLabel("Velocity"), 1, 0); + stateGrid->addWidget(t.qd, 1, 1); + stateGrid->addWidget(new QLabel("Torque"), 2, 0); + stateGrid->addWidget(t.tau, 2, 1); + + stateGrid->setHorizontalSpacing(12); + stateGrid->setColumnStretch(0, 0); + stateGrid->setColumnStretch(1, 1); + + auto* refGrid = new QGridLayout(); + refGrid->addWidget(new QLabel("Target Pos"), 0, 0); + refGrid->addWidget(t.qRef, 0, 1); + refGrid->addWidget(new QLabel("Target Vel"), 1, 0); + refGrid->addWidget(t.qdRef, 1, 1); + refGrid->addWidget(new QLabel("Target Acc"), 2, 0); + refGrid->addWidget(t.qddRef, 2, 1); + refGrid->addWidget(new QLabel("Error"), 3, 0); + refGrid->addWidget(t.err, 3, 1); + + refGrid->setHorizontalSpacing(12); + refGrid->setColumnStretch(0, 0); + refGrid->setColumnStretch(1, 1); + + auto* trajGrid = new QGridLayout(); + trajGrid->addWidget(new QLabel("Position"), 0, 0); + trajGrid->addWidget(t.qTraj, 0, 1); + trajGrid->addWidget(new QLabel("Velocity"), 1, 0); + trajGrid->addWidget(t.qdTraj, 1, 1); + trajGrid->addWidget(new QLabel("Acceleration"), 2, 0); + trajGrid->addWidget(t.qddTraj, 2, 1); + + trajGrid->setHorizontalSpacing(12); + trajGrid->setColumnStretch(0, 0); + trajGrid->setColumnStretch(1, 1); + + auto* limitGrid = new QGridLayout(); + limitGrid->addWidget(new QLabel("Position Clamp"), 0, 0); + limitGrid->addWidget(t.qClamped, 0, 1); + limitGrid->addWidget(new QLabel("Velocity Clamp"), 1, 0); + limitGrid->addWidget(t.qdClamped, 1, 1); + + limitGrid->setHorizontalSpacing(12); + limitGrid->setColumnStretch(0, 0); + limitGrid->setColumnStretch(1, 1); + + auto* physicalGrid = new QGridLayout(); + physicalGrid->addWidget(new QLabel("Damping"), 0, 0); + physicalGrid->addWidget(t.damping, 0, 1); + physicalGrid->addWidget(new QLabel("Friction"), 1, 0); + physicalGrid->addWidget(t.friction, 1, 1); + + physicalGrid->setHorizontalSpacing(12); + physicalGrid->setColumnStretch(0, 0); + physicalGrid->setColumnStretch(1, 1); layout->addSpacing(5); + layout->addWidget(t.stateHeader); + layout->addLayout(stateGrid); + layout->addSpacing(8); layout->addWidget(t.referenceHeader); - layout->addWidget(t.qRef); - layout->addWidget(t.qdRef); - layout->addWidget(t.qddRef); - layout->addWidget(t.err); - - layout->addSpacing(5); + layout->addLayout(refGrid); + layout->addSpacing(8); layout->addWidget(t.trajectoryHeader); - layout->addWidget(t.qTraj); - layout->addWidget(t.qdTraj); - layout->addWidget(t.qddTraj); - - layout->addSpacing(5); + layout->addLayout(trajGrid); + layout->addSpacing(8); layout->addWidget(t.clampedHeader); - layout->addWidget(t.qClamped); - layout->addWidget(t.qdClamped); - - layout->addSpacing(5); + layout->addLayout(limitGrid); + layout->addSpacing(8); layout->addWidget(t.constantsHeader); - layout->addWidget(t.damping); - layout->addWidget(t.friction); + layout->addLayout(physicalGrid); } void ControlPanelWidget::updateTelemetryDisplay() { @@ -293,7 +354,6 @@ namespace widgets { _jointIdxSlider->setValue(jointIdx + 1); } updateTelemetryInfo(s.j[jointIdx]); - if (_currentSimTimeJointLabel) { _currentSimTimeJointLabel->setText(QString("Current Simulation Time: %1 s").arg(simTime, 0, 'f', 3)); } } void ControlPanelWidget::displayPanel() { diff --git a/DSFE_App/DSFE_GUI/src/MainWindow/style/DSFETheme.cpp b/DSFE_App/DSFE_GUI/src/MainWindow/style/DSFETheme.cpp new file mode 100644 index 00000000..a810ff68 --- /dev/null +++ b/DSFE_App/DSFE_GUI/src/MainWindow/style/DSFETheme.cpp @@ -0,0 +1,30 @@ +// DSFE_GUI DSFETheme.cpp +#include "DSFETheme.h" + +#include +#include +#include +#include + + +namespace style { + void applyTheme(QApplication& app) { + QPalette palette; + palette.setColor(QPalette::Window, QColor(30, 30, 30)); + palette.setColor(QPalette::WindowText, QColor(220, 220, 220)); + palette.setColor(QPalette::Base, QColor(30, 30, 30)); + palette.setColor(QPalette::AlternateBase, QColor(40, 40, 40)); + palette.setColor(QPalette::Text, QColor(220, 220, 220)); + palette.setColor(QPalette::Button, QColor(40, 40, 40)); + palette.setColor(QPalette::ButtonText, QColor(220, 220, 220)); + palette.setColor(QPalette::Highlight, QColor(86, 156, 214)); + palette.setColor(QPalette::HighlightedText, QColor(255, 255, 255)); + app.setPalette(palette); + + QFile file(":/style/dsfe_dark.qss"); + if (!file.open(QIODevice::ReadOnly | QIODevice::Text)) { return; } + + app.setStyleSheet(QString::fromUtf8(file.readAll())); + } + +} \ No newline at end of file From 2042d468c9c3bd1fe11886c32efe0768b96c8940 Mon Sep 17 00:00:00 2001 From: saltyjoss Date: Wed, 10 Jun 2026 17:36:07 +0100 Subject: [PATCH 141/210] chore: Updated minimum cmake version to 3.17 from 3.10 --- CMakeLists.txt | 7 +- DSFE_App/CMakeLists.txt | 2 +- DSFE_App/DSFE_Core/CMakeLists.txt | 2 +- DSFE_App/DSFE_Engine/CMakeLists.txt | 2 +- DSFE_App/DSFE_GUI/CMakeLists.txt | 2 +- .../include/MainWindow/DSFE_MainWindow.h | 13 +- DSFE_App/DSFE_GUI/src/ui/Styles.cpp | 154 ------------------ PxMLib/CMakeLists.txt | 2 +- PxMLib/MathLib/CMakeLists.txt | 3 +- .../CMakeLists.txt | 2 +- PxMLib/MathLib_Unit/CMakeLists.txt | 2 +- .../DualNumberTests/CMakeLists.txt | 2 +- .../IntegratorTests/CMakeLists.txt | 2 +- 13 files changed, 22 insertions(+), 173 deletions(-) delete mode 100644 DSFE_App/DSFE_GUI/src/ui/Styles.cpp diff --git a/CMakeLists.txt b/CMakeLists.txt index a41f29ab..a4c4881a 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -1,10 +1,5 @@ # CMakeLists.txt for DSFE project (root CMakeLists.txt) -cmake_minimum_required(VERSION 3.10) - -if (POLICY CMP0141) - cmake_policy(SET CMP0141 NEW) - set(CMAKE_MSVC_DEBUG_INFORMATION_FORMAT "$,$>,$<$:EditAndContinue>,$<$:ProgramDatabase>>") -endif() +cmake_minimum_required(VERSION 3.17) project(DSFE LANGUAGES C CXX) diff --git a/DSFE_App/CMakeLists.txt b/DSFE_App/CMakeLists.txt index 1fd34a64..6f40143c 100644 --- a/DSFE_App/CMakeLists.txt +++ b/DSFE_App/CMakeLists.txt @@ -1,5 +1,5 @@ # CMakeLists.txt for the DSFE_App project -cmake_minimum_required(VERSION 3.10) +cmake_minimum_required(VERSION 3.17) project(DSFE_App LANGUAGES C CXX) # Add projects within the DSFE_App directory diff --git a/DSFE_App/DSFE_Core/CMakeLists.txt b/DSFE_App/DSFE_Core/CMakeLists.txt index a12a6709..8cf99bf2 100644 --- a/DSFE_App/DSFE_Core/CMakeLists.txt +++ b/DSFE_App/DSFE_Core/CMakeLists.txt @@ -1,5 +1,5 @@ # CMakeLists.txt for DSFE_Core library -cmake_minimum_required(VERSION 3.10) +cmake_minimum_required(VERSION 3.17) project(DSFE_Core LANGUAGES C CXX) add_library(DSFE_Core SHARED) diff --git a/DSFE_App/DSFE_Engine/CMakeLists.txt b/DSFE_App/DSFE_Engine/CMakeLists.txt index f68e79b6..654bfe42 100644 --- a/DSFE_App/DSFE_Engine/CMakeLists.txt +++ b/DSFE_App/DSFE_Engine/CMakeLists.txt @@ -1,5 +1,5 @@ # CMakeLists.txt for the DSFE_Engine executable -cmake_minimum_required(VERSION 3.10) +cmake_minimum_required(VERSION 3.17) project(DSFE_Engine LANGUAGES C CXX) find_package(Threads REQUIRED) diff --git a/DSFE_App/DSFE_GUI/CMakeLists.txt b/DSFE_App/DSFE_GUI/CMakeLists.txt index 5b6ce6b6..5054bb97 100644 --- a/DSFE_App/DSFE_GUI/CMakeLists.txt +++ b/DSFE_App/DSFE_GUI/CMakeLists.txt @@ -1,7 +1,7 @@ # CMakeLists.txt for DSFE_GUI module set(CMAKE_POSITION_INDEPENDENT_CODE ON) -cmake_minimum_required(VERSION 3.10) +cmake_minimum_required(VERSION 3.17) project(DSFE_GUI LANGUAGES C CXX) set(CMAKE_AUTOMOC ON) diff --git a/DSFE_App/DSFE_GUI/include/MainWindow/DSFE_MainWindow.h b/DSFE_App/DSFE_GUI/include/MainWindow/DSFE_MainWindow.h index 771cad2e..aec3331b 100644 --- a/DSFE_App/DSFE_GUI/include/MainWindow/DSFE_MainWindow.h +++ b/DSFE_App/DSFE_GUI/include/MainWindow/DSFE_MainWindow.h @@ -9,6 +9,11 @@ namespace gui { class SimManager; } namespace widgets { class DSLEditorWidget; } +namespace render { + enum class ResolutionPreset; + enum class QualityPreset; +} + namespace window { class DSFE_MainWindow : public QMainWindow { public: @@ -16,9 +21,13 @@ namespace window { private: void buildMenuBar(); + void buildGraphicsMenu(QMenu* graphicsMenu); + void buildRobotMenu(QMenu* projectMenu); + + render::ResolutionPreset r; + render::QualityPreset q; + gui::SimManager* _sim; widgets::DSLEditorWidget* _dslEditor = nullptr; - - void buildRobotMenu(QMenu* projectMenu); }; } diff --git a/DSFE_App/DSFE_GUI/src/ui/Styles.cpp b/DSFE_App/DSFE_GUI/src/ui/Styles.cpp deleted file mode 100644 index 2b477560..00000000 --- a/DSFE_App/DSFE_GUI/src/ui/Styles.cpp +++ /dev/null @@ -1,154 +0,0 @@ -// DSFE_GUI Styles.cpp -#include "ui/Styles.h" -#include -#include -#include - -namespace gui { - void Styles::DarkMode() { - ImGuiStyle& style = ImGui::GetStyle(); - ImVec4* colors = style.Colors; - - // --- Rounding --- - style.WindowRounding = 0.0f; - style.FrameRounding = 0.0f; - style.ScrollbarRounding = 0.0f; - style.GrabRounding = 0.0f; - style.TabRounding = 0.0f; - - // --- Padding & spacing --- - style.WindowPadding = ImVec2(6, 6); - style.FramePadding = ImVec2(6, 4); - style.ItemSpacing = ImVec2(6, 4); - style.ItemInnerSpacing = ImVec2(4, 4); - - // --- Borders & separators --- - style.WindowBorderSize = 1.0f; - style.ChildBorderSize = 1.0f; - style.PopupBorderSize = 1.0f; - style.FrameBorderSize = 1.0f; - style.TabBorderSize = 1.0f; - style.SeparatorTextBorderSize = 1.0f; - - // --- Colors --- - colors[ImGuiCol_Text] = ImVec4(0.9f, 0.9f, 0.9f, 1.0f); - colors[ImGuiCol_WindowBg] = ImVec4(0.12f, 0.12f, 0.12f, 1.0f); - colors[ImGuiCol_ChildBg] = ImVec4(0.16f, 0.16f, 0.16f, 1.0f); - colors[ImGuiCol_PopupBg] = ImVec4(0.16f, 0.16f, 0.16f, 1.0f); - colors[ImGuiCol_Border] = ImVec4(0.29f, 0.29f, 0.29f, 1.0f); - colors[ImGuiCol_BorderShadow] = ImVec4(0, 0, 0, 0); - colors[ImGuiCol_FrameBg] = ImVec4(0.18f, 0.18f, 0.18f, 1.0f); - colors[ImGuiCol_FrameBgHovered] = ImVec4(0.28f, 0.28f, 0.28f, 1.0f); - colors[ImGuiCol_FrameBgActive] = ImVec4(0.22f, 0.22f, 0.22f, 1.0f); - colors[ImGuiCol_TitleBg] = ImVec4(0.12f, 0.12f, 0.12f, 1.0f); - colors[ImGuiCol_TitleBgActive] = ImVec4(0.18f, 0.18f, 0.18f, 1.0f); - colors[ImGuiCol_TitleBgCollapsed] = ImVec4(0.10f, 0.10f, 0.10f, 1.0f); - - colors[ImGuiCol_MenuBarBg] = ImVec4(0.16f, 0.16f, 0.16f, 1.0f); - colors[ImGuiCol_ScrollbarBg] = ImVec4(0.12f, 0.12f, 0.12f, 1.0f); - colors[ImGuiCol_ScrollbarGrab] = ImVec4(0.28f, 0.28f, 0.28f, 1.0f); - colors[ImGuiCol_ScrollbarGrabHovered] = ImVec4(0.33f, 0.33f, 0.33f, 1.0f); - colors[ImGuiCol_ScrollbarGrabActive] = ImVec4(0.38f, 0.38f, 0.38f, 1.0f); - colors[ImGuiCol_CheckMark] = ImVec4(0.25f, 0.5f, 0.9f, 1.0f); - colors[ImGuiCol_SliderGrab] = ImVec4(0.25f, 0.5f, 0.9f, 1.0f); - colors[ImGuiCol_SliderGrabActive] = ImVec4(0.20f, 0.45f, 0.85f, 1.0f); - colors[ImGuiCol_Button] = ImVec4(0.18f, 0.18f, 0.18f, 1.0f); - colors[ImGuiCol_ButtonHovered] = ImVec4(0.28f, 0.28f, 0.28f, 1.0f); - colors[ImGuiCol_ButtonActive] = ImVec4(0.22f, 0.22f, 0.22f, 1.0f); - - // Tabs like VS2022 - colors[ImGuiCol_Tab] = ImVec4(0.15f, 0.15f, 0.15f, 1.0f); - colors[ImGuiCol_TabHovered] = ImVec4(0.28f, 0.28f, 0.28f, 1.0f); - colors[ImGuiCol_TabActive] = ImVec4(0.22f, 0.22f, 0.22f, 1.0f); - colors[ImGuiCol_TabUnfocused] = ImVec4(0.14f, 0.14f, 0.14f, 1.0f); - colors[ImGuiCol_TabUnfocusedActive] = ImVec4(0.18f, 0.18f, 0.18f, 1.0f); - colors[ImGuiCol_Header] = ImVec4(0.18f, 0.18f, 0.18f, 1.0f); - colors[ImGuiCol_HeaderHovered] = ImVec4(0.28f, 0.28f, 0.28f, 1.0f); - colors[ImGuiCol_HeaderActive] = ImVec4(0.22f, 0.22f, 0.22f, 1.0f); - - // Separator - colors[ImGuiCol_Separator] = ImVec4(0.29f, 0.29f, 0.29f, 1.0f); - colors[ImGuiCol_SeparatorHovered] = ImVec4(0.35f, 0.35f, 0.35f, 1.0f); - colors[ImGuiCol_SeparatorActive] = ImVec4(0.45f, 0.45f, 0.45f, 1.0f); - - // Tooltip - colors[ImGuiCol_PopupBg] = ImVec4(0.16f, 0.16f, 0.16f, 1.0f); - - // Optional: scrollbar rounding 0 to mimic VS - style.ScrollbarRounding = 0.0f; - style.GrabRounding = 0.0f; - } - - void Styles::LightMode() { - ImGuiStyle& style = ImGui::GetStyle(); - ImVec4* colors = style.Colors; - - // --- Rounding -- - style.WindowRounding = 0.0f; - style.FrameRounding = 0.0f; - style.ScrollbarRounding = 0.0f; - style.GrabRounding = 0.0f; - style.TabRounding = 0.0f; - - // --- Padding --- - style.FramePadding = ImVec2(6, 4); - style.ItemSpacing = ImVec2(6, 4); - - // --- Borders & separators --- - style.WindowBorderSize = 1.0f; - style.ChildBorderSize = 1.0f; - style.PopupBorderSize = 1.0f; - style.FrameBorderSize = 1.0f; - style.TabBorderSize = 1.0f; - style.SeparatorTextBorderSize = 1.0f; - - // --- Colors --- - colors[ImGuiCol_Text] = ImVec4(0.0f, 0.0f, 0.0f, 1.0f); - colors[ImGuiCol_WindowBg] = ImVec4(0.94f, 0.94f, 0.94f, 1.0f); - colors[ImGuiCol_ChildBg] = ImVec4(0.96f, 0.96f, 0.96f, 1.0f); - colors[ImGuiCol_PopupBg] = ImVec4(0.96f, 0.96f, 0.96f, 1.0f); - colors[ImGuiCol_Border] = ImVec4(0.65f, 0.65f, 0.65f, 1.0f); - colors[ImGuiCol_BorderShadow] = ImVec4(0.0f, 0.0f, 0.0f, 0.0f); - colors[ImGuiCol_FrameBg] = ImVec4(0.90f, 0.90f, 0.90f, 1.0f); - colors[ImGuiCol_FrameBgHovered] = ImVec4(0.82f, 0.82f, 0.82f, 1.0f); - colors[ImGuiCol_FrameBgActive] = ImVec4(0.78f, 0.78f, 0.78f, 1.0f); - colors[ImGuiCol_TitleBg] = ImVec4(0.94f, 0.94f, 0.94f, 1.0f); - colors[ImGuiCol_TitleBgActive] = ImVec4(0.90f, 0.90f, 0.90f, 1.0f); - colors[ImGuiCol_TitleBgCollapsed] = ImVec4(0.96f, 0.96f, 0.96f, 1.0f); - - colors[ImGuiCol_MenuBarBg] = ImVec4(0.92f, 0.92f, 0.92f, 1.0f); - colors[ImGuiCol_ScrollbarBg] = ImVec4(0.94f, 0.94f, 0.94f, 1.0f); - colors[ImGuiCol_ScrollbarGrab] = ImVec4(0.82f, 0.82f, 0.82f, 1.0f); - colors[ImGuiCol_ScrollbarGrabHovered] = ImVec4(0.78f, 0.78f, 0.78f, 1.0f); - colors[ImGuiCol_ScrollbarGrabActive] = ImVec4(0.73f, 0.73f, 0.73f, 1.0f); - colors[ImGuiCol_CheckMark] = ImVec4(0.25f, 0.5f, 0.9f, 1.0f); - colors[ImGuiCol_SliderGrab] = ImVec4(0.25f, 0.5f, 0.9f, 1.0f); - colors[ImGuiCol_SliderGrabActive] = ImVec4(0.2f, 0.45f, 0.85f, 1.0f); - colors[ImGuiCol_Button] = ImVec4(0.90f, 0.90f, 0.90f, 1.0f); - colors[ImGuiCol_ButtonHovered] = ImVec4(0.82f, 0.82f, 0.82f, 1.0f); - colors[ImGuiCol_ButtonActive] = ImVec4(0.78f, 0.78f, 0.78f, 1.0f); - - // Tabs like VS2022 - colors[ImGuiCol_Tab] = ImVec4(0.92f, 0.92f, 0.92f, 1.0f); - colors[ImGuiCol_TabHovered] = ImVec4(0.82f, 0.82f, 0.82f, 1.0f); - colors[ImGuiCol_TabActive] = ImVec4(0.78f, 0.78f, 0.78f, 1.0f); - colors[ImGuiCol_TabUnfocused] = ImVec4(0.94f, 0.94f, 0.94f, 1.0f); - colors[ImGuiCol_TabUnfocusedActive] = ImVec4(0.90f, 0.90f, 0.90f, 1.0f); - colors[ImGuiCol_Header] = ImVec4(0.90f, 0.90f, 0.90f, 1.0f); - colors[ImGuiCol_HeaderHovered] = ImVec4(0.82f, 0.82f, 0.82f, 1.0f); - colors[ImGuiCol_HeaderActive] = ImVec4(0.78f, 0.78f, 0.78f, 1.0f); - - // Separator - colors[ImGuiCol_Separator] = ImVec4(0.65f, 0.65f, 0.65f, 1.0f); - colors[ImGuiCol_SeparatorHovered] = ImVec4(0.75f, 0.75f, 0.75f, 1.0f); - colors[ImGuiCol_SeparatorActive] = ImVec4(0.85f, 0.85f, 0.85f, 1.0f); - - // Tooltip - colors[ImGuiCol_PopupBg] = ImVec4(0.96f, 0.96f, 0.96f, 1.0f); - - // Optional: scrollbar rounding 0 to mimic VS - style.ScrollbarRounding = 0.0f; - style.GrabRounding = 0.0f; - } -} - diff --git a/PxMLib/CMakeLists.txt b/PxMLib/CMakeLists.txt index 73994c9f..988b2907 100644 --- a/PxMLib/CMakeLists.txt +++ b/PxMLib/CMakeLists.txt @@ -1,5 +1,5 @@ # CMakeLists.txt for PxMLib project -cmake_minimum_required(VERSION 3.10) +cmake_minimum_required(VERSION 3.17) project(PxMLib LANGUAGES CXX) # Add projects within the DSFE_App directory diff --git a/PxMLib/MathLib/CMakeLists.txt b/PxMLib/MathLib/CMakeLists.txt index 495cd98e..9059084d 100644 --- a/PxMLib/MathLib/CMakeLists.txt +++ b/PxMLib/MathLib/CMakeLists.txt @@ -1,6 +1,5 @@ # CMakeLists.txt for MathLib - -cmake_minimum_required(VERSION 3.10) +cmake_minimum_required(VERSION 3.17) project(MathLib LANGUAGES CXX) add_library(MathLib STATIC diff --git a/PxMLib/MathLib_Unit/AutomaticDifferentiationTests/CMakeLists.txt b/PxMLib/MathLib_Unit/AutomaticDifferentiationTests/CMakeLists.txt index 4652f2d8..547e2f60 100644 --- a/PxMLib/MathLib_Unit/AutomaticDifferentiationTests/CMakeLists.txt +++ b/PxMLib/MathLib_Unit/AutomaticDifferentiationTests/CMakeLists.txt @@ -1,5 +1,5 @@ # CMakeLists.txt for MathLib_Unit_Tests executable -cmake_minimum_required(VERSION 3.10) +cmake_minimum_required(VERSION 3.17) project(MathLib_ADTests LANGUAGES C CXX) enable_testing() diff --git a/PxMLib/MathLib_Unit/CMakeLists.txt b/PxMLib/MathLib_Unit/CMakeLists.txt index 0910ece5..9bf06a0e 100644 --- a/PxMLib/MathLib_Unit/CMakeLists.txt +++ b/PxMLib/MathLib_Unit/CMakeLists.txt @@ -1,5 +1,5 @@ # CMakeLists.txt for MathLib_Unit_Tests executable -cmake_minimum_required(VERSION 3.10) +cmake_minimum_required(VERSION 3.17) project(MathLib_Unit_Tests LANGUAGES C CXX) add_subdirectory(IntegratorTests) diff --git a/PxMLib/MathLib_Unit/DualNumberTests/CMakeLists.txt b/PxMLib/MathLib_Unit/DualNumberTests/CMakeLists.txt index 08c8e969..abc162ed 100644 --- a/PxMLib/MathLib_Unit/DualNumberTests/CMakeLists.txt +++ b/PxMLib/MathLib_Unit/DualNumberTests/CMakeLists.txt @@ -1,5 +1,5 @@ # CMakeLists.txt for MathLib_Unit_Tests executable -cmake_minimum_required(VERSION 3.10) +cmake_minimum_required(VERSION 3.17) project(MathLib_DualNumberTests LANGUAGES C CXX) enable_testing() diff --git a/PxMLib/MathLib_Unit/IntegratorTests/CMakeLists.txt b/PxMLib/MathLib_Unit/IntegratorTests/CMakeLists.txt index a4f82d1d..f04ac90a 100644 --- a/PxMLib/MathLib_Unit/IntegratorTests/CMakeLists.txt +++ b/PxMLib/MathLib_Unit/IntegratorTests/CMakeLists.txt @@ -1,5 +1,5 @@ # CMakeLists.txt for MathLib_Unit_Tests executable -cmake_minimum_required(VERSION 3.10) +cmake_minimum_required(VERSION 3.17) project(MathLib_IntegratorTests LANGUAGES C CXX) enable_testing() From 130ad1149d206a33f094369fcb869c052a176277 Mon Sep 17 00:00:00 2001 From: saltyjoss Date: Wed, 10 Jun 2026 17:37:05 +0100 Subject: [PATCH 142/210] feat: Introduced quality, resolution, and shader switches in the view menu bar --- .../src/MainWindow/DSFE_MainWindow.cpp | 142 +++++++++++++++++- 1 file changed, 137 insertions(+), 5 deletions(-) diff --git a/DSFE_App/DSFE_GUI/src/MainWindow/DSFE_MainWindow.cpp b/DSFE_App/DSFE_GUI/src/MainWindow/DSFE_MainWindow.cpp index e9aa08f4..af18e8ef 100644 --- a/DSFE_App/DSFE_GUI/src/MainWindow/DSFE_MainWindow.cpp +++ b/DSFE_App/DSFE_GUI/src/MainWindow/DSFE_MainWindow.cpp @@ -4,9 +4,12 @@ #include "Workspace/ProjectPage.h" #include "Widgets/DSLEditorWidget.h" +#include "ui/RenderPreset.h" + #include "Platform/SystemMap.h" #include +#include #include #include #include @@ -136,6 +139,8 @@ namespace window { } // View menu { + auto* graphicsMenu = viewMenu->addMenu("Graphics Options"); + buildGraphicsMenu(graphicsMenu); auto* resetCameraAction = viewMenu->addAction("Reset Camera"); connect(resetCameraAction, &QAction::triggered, this, []() { LOG_INFO("Menu clicked: View -> Reset Camera"); @@ -155,11 +160,6 @@ namespace window { connect(physicsDebugAction, &QAction::toggled, this, [](bool checked) { LOG_INFO("Menu toggled: Tools -> Toggle Physics Debug -> %s", checked ? "On" : "Off"); }); - auto* reloadShadersAction = toolsMenu->addAction("Reload Shaders"); - connect(reloadShadersAction, &QAction::triggered, this, [this]() { - LOG_INFO("Menu clicked: Tools -> Reload Shaders"); - _sim->reloadAllShaders(); - }); auto* diagnosticsAction = toolsMenu->addAction("Run Diagnostics"); connect(diagnosticsAction, &QAction::triggered, this, []() { LOG_INFO("Menu clicked: Tools -> Run Diagnostics"); @@ -178,6 +178,138 @@ namespace window { } } + void DSFE_MainWindow::buildGraphicsMenu(QMenu* graphicsMenu) { + // Quality submenu + auto* qualityMenu = graphicsMenu->addMenu("Quality"); + auto* lowQualityAction = qualityMenu->addAction("Low"); + auto* mediumQualityAction = qualityMenu->addAction("Medium"); + auto* highQualityAction = qualityMenu->addAction("High"); + auto* ultraQualityAction = qualityMenu->addAction("Ultra"); + lowQualityAction->setCheckable(true); + mediumQualityAction->setCheckable(true); + highQualityAction->setCheckable(true); + ultraQualityAction->setCheckable(true); + auto* qualityGroup = new QActionGroup(this); + qualityGroup->setExclusive(true); + qualityGroup->addAction(lowQualityAction); + qualityGroup->addAction(mediumQualityAction); + qualityGroup->addAction(highQualityAction); + qualityGroup->addAction(ultraQualityAction); + mediumQualityAction->setChecked(true); + // LOW + connect(lowQualityAction, &QAction::triggered, this, [this]() { + q = render::QualityPreset::Low; + auto s = render::MakeSettings(r, q); + _sim->applyRenderProfile(s, r); + LOG_INFO("Graphics Quality -> Low"); + }); + // MEDIUM + connect(mediumQualityAction, &QAction::triggered, this, [this]() { + q = render::QualityPreset::Medium; + auto s = render::MakeSettings(r, q); + _sim->applyRenderProfile(s, r); + LOG_INFO("Graphics Quality -> Medium"); + }); + // HIGH + connect(highQualityAction, &QAction::triggered, this, [this]() { + q = render::QualityPreset::High; + auto s = render::MakeSettings(r, q); + _sim->applyRenderProfile(s, r); + LOG_INFO("Graphics Quality -> High"); + }); + // ULTRA + connect(ultraQualityAction, &QAction::triggered, this, [this]() { + q = render::QualityPreset::Ultra; + auto s = render::MakeSettings(r, q); + _sim->applyRenderProfile(s, r); + LOG_INFO("Graphics Quality -> Ultra"); + }); + + // Resolution submenu + auto* resolutionMenu = graphicsMenu->addMenu("Resolution"); + auto* r720Action = resolutionMenu->addAction("720p"); + auto* r1080Action = resolutionMenu->addAction("1080p"); + auto* r1440Action = resolutionMenu->addAction("1440p"); + auto* r4kAction = resolutionMenu->addAction("4K"); + r720Action->setCheckable(true); + r1080Action->setCheckable(true); + r1440Action->setCheckable(true); + r4kAction->setCheckable(true); + auto* resolutionGroup = new QActionGroup(this); + resolutionGroup->setExclusive(true); + resolutionGroup->addAction(r720Action); + resolutionGroup->addAction(r1080Action); + resolutionGroup->addAction(r1440Action); + resolutionGroup->addAction(r4kAction); + r1080Action->setChecked(true); + // 720p + connect(r720Action, &QAction::triggered, this, [this]() { + r = render::ResolutionPreset::R_720p; + auto s = render::MakeSettings(r, q); + _sim->applyRenderProfile(s, r); + LOG_INFO("Resolution -> 720p"); + }); + // 1080p + connect(r1080Action, &QAction::triggered, this, [this]() { + r = render::ResolutionPreset::R_1080p; + auto s = render::MakeSettings(r, q); + _sim->applyRenderProfile(s, r); + LOG_INFO("Resolution -> 1080p"); + }); + // 1440p + connect(r1440Action, &QAction::triggered, this, [this]() { + r = render::ResolutionPreset::R_1440p; + auto s = render::MakeSettings(r, q); + _sim->applyRenderProfile(s, r); + LOG_INFO("Resolution -> 1440p"); + }); + // 4K + connect(r4kAction, &QAction::triggered, this, [this]() { + r = render::ResolutionPreset::R_4K; + auto s = render::MakeSettings(r, q); + _sim->applyRenderProfile(s, r); + LOG_INFO("Resolution -> 4K"); + }); + + // Shader submenu + auto* shaderMenu = graphicsMenu->addMenu("Shaders"); + auto* basicShaderAction = shaderMenu->addAction("Basic"); + auto* litShaderAction = shaderMenu->addAction("Lit"); + auto* pbrShaderAction = shaderMenu->addAction("PBR"); + basicShaderAction->setCheckable(true); + litShaderAction->setCheckable(true); + pbrShaderAction->setCheckable(true); + auto* shaderGroup = new QActionGroup(this); + shaderGroup->setExclusive(true); + shaderGroup->addAction(basicShaderAction); + shaderGroup->addAction(litShaderAction); + shaderGroup->addAction(pbrShaderAction); + pbrShaderAction->setChecked(true); + // BASIC + connect(basicShaderAction, &QAction::triggered, this, [this]() { + _sim->currentShaderMode = gui::SimManager::ShaderMode::Basic; + LOG_INFO("Shader Mode -> Basic"); + }); + // LIT + connect(litShaderAction, &QAction::triggered, this, [this]() { + _sim->currentShaderMode = gui::SimManager::ShaderMode::Lit; + LOG_INFO("Shader Mode -> Lit"); + }); + // PBR + connect(pbrShaderAction, &QAction::triggered, this, [this]() { + _sim->currentShaderMode = gui::SimManager::ShaderMode::PBR; + LOG_INFO("Shader Mode -> PBR"); + }); + + shaderMenu->addSeparator(); + + auto* reloadShadersAction = shaderMenu->addAction("Reload Shaders"); + connect(reloadShadersAction, &QAction::triggered, this, [this]() { + LOG_INFO("Menu clicked: Reload Shaders"); + _sim->reloadAllShaders(); + }); + } + void DSFE_MainWindow::buildRobotMenu(QMenu* projectMenu) { const auto& robotMap = platform::getRobotSystemMap(); std::unordered_map familyMenus; From 29598dee707ef241f33aae7497f9fc09128fee47 Mon Sep 17 00:00:00 2001 From: saltyjoss Date: Sat, 13 Jun 2026 08:38:46 +0100 Subject: [PATCH 143/210] fixes: Removed stale files referring to `imgui.h`, `implot.h`, `imfilebrowser.h`, `input.h`, and `glfw.h` --- DSFE_App/DSFE_GUI/include/Platform/Window.h | 41 - .../DSFE_GUI/include/Rendering/RenderBase.h | 9 - DSFE_App/DSFE_GUI/include/Scene/Camera.h | 35 +- DSFE_App/DSFE_GUI/include/Scene/FpsCounter.h | 35 - DSFE_App/DSFE_GUI/include/Scene/Input.h | 28 - .../include/Scene/Manager/SimImplementation.h | 1 - .../include/Scene/SimulationManager.h | 12 +- .../DSFE_GUI/include/imfilebrowser/LICENSE | 21 - .../include/imfilebrowser/imfilebrowser.h | 1347 ----------------- .../DSFE_GUI/src/Scene/AxisOrientator.cpp | 2 - DSFE_App/DSFE_GUI/src/Scene/Input.cpp | 41 - DSFE_App/DSFE_GUI/src/Scene/Manager/SimUI.cpp | 182 --- DSFE_App/DSFE_GUI/src/Scene/Object.cpp | 27 +- .../DSFE_GUI/src/Scene/SimulationManager.cpp | 21 +- 14 files changed, 43 insertions(+), 1759 deletions(-) delete mode 100644 DSFE_App/DSFE_GUI/include/Platform/Window.h delete mode 100644 DSFE_App/DSFE_GUI/include/Scene/FpsCounter.h delete mode 100644 DSFE_App/DSFE_GUI/include/Scene/Input.h delete mode 100644 DSFE_App/DSFE_GUI/include/imfilebrowser/LICENSE delete mode 100644 DSFE_App/DSFE_GUI/include/imfilebrowser/imfilebrowser.h delete mode 100644 DSFE_App/DSFE_GUI/src/Scene/Input.cpp delete mode 100644 DSFE_App/DSFE_GUI/src/Scene/Manager/SimUI.cpp diff --git a/DSFE_App/DSFE_GUI/include/Platform/Window.h b/DSFE_App/DSFE_GUI/include/Platform/Window.h deleted file mode 100644 index 861ec64b..00000000 --- a/DSFE_App/DSFE_GUI/include/Platform/Window.h +++ /dev/null @@ -1,41 +0,0 @@ -#pragma once -// File: Window.h -// GitHub: SaltyJoss -#include "EngineCore.h" - -#include -#include "Platform/Logger.h" - -//Basic window interface -namespace window { - class IWindow { - public: - // Destructor - virtual ~IWindow() = default; - - // Initialisation - virtual bool init(int width, int height, const std::string& title) = 0; - - // Core windowing - virtual bool isRunning() const = 0; - virtual bool shouldClose() const = 0; - virtual void pollEvents() = 0; - virtual void swapBuffers() = 0; - - // Native window access - virtual void* getNativeWin() = 0; - virtual void setNativeWin(void* window) = 0; - - // Callbacks - virtual void onKey(int key, int scancode, int action, int mods) = 0; - virtual void onScroll(double delta) = 0; - virtual void onResize(int width, int height) = 0; - virtual void onCursorPos(double xpos, double ypos) = 0; - virtual void onClose() = 0; - - // Getters - virtual int getWidth() const = 0; - virtual int getHeight() const = 0; - virtual const std::string& getHeader() const = 0; - }; -} // namespace window \ No newline at end of file diff --git a/DSFE_App/DSFE_GUI/include/Rendering/RenderBase.h b/DSFE_App/DSFE_GUI/include/Rendering/RenderBase.h index dde79adc..9dab8b53 100644 --- a/DSFE_App/DSFE_GUI/include/Rendering/RenderBase.h +++ b/DSFE_App/DSFE_GUI/include/Rendering/RenderBase.h @@ -1,7 +1,6 @@ // DSFE_GUI RenderBase.h #pragma once -#include "Platform/Window.h" #include "Assets/VertexHolder.h" #include @@ -65,18 +64,10 @@ namespace render { virtual ~RenderContext() = default; // Centeralised way to gain context on the renders' process -> see OpenGLContext - RenderContext() : _window(nullptr) {} - - virtual bool init(window::IWindow* win) { - _window = win; - return true; - } virtual void preRender() = 0; virtual void postRender() = 0; virtual void end() = 0; - protected: - window::IWindow* _window; }; } // namespace render \ No newline at end of file diff --git a/DSFE_App/DSFE_GUI/include/Scene/Camera.h b/DSFE_App/DSFE_GUI/include/Scene/Camera.h index bc36f9ed..e45acbb1 100644 --- a/DSFE_App/DSFE_GUI/include/Scene/Camera.h +++ b/DSFE_App/DSFE_GUI/include/Scene/Camera.h @@ -6,7 +6,6 @@ #include #include "Scene/Element.h" -#include "Scene/Input.h" #include "Platform/Logger.h" @@ -63,30 +62,30 @@ namespace scene { setDistance((float)(-delta * 0.5f)); } - void onMouseMove(double x, double y, eInputButton button) { - glm::vec2 pos2d{ x, y }; + //void onMouseMove(double x, double y) { + // glm::vec2 pos2d{ x, y }; - if (button == eInputButton::Right) { - glm::vec2 delta = (pos2d - _currentPos2D) * 0.004f; + // if () { + // glm::vec2 delta = (pos2d - _currentPos2D) * 0.004f; - float sign = getUp().y < 0 ? -1.0f : 1.0f; + // float sign = getUp().y < 0 ? -1.0f : 1.0f; - _yaw += sign * delta.x * _rotationSpeed; - _pitch += delta.y * _rotationSpeed; + // _yaw += sign * delta.x * _rotationSpeed; + // _pitch += delta.y * _rotationSpeed; - updateViewMatrix(); - } - else if (button == eInputButton::Left) { - glm::vec2 delta = (pos2d - _currentPos2D) * 0.003f; + // updateViewMatrix(); + // } + // else if (false) { + // glm::vec2 delta = (pos2d - _currentPos2D) * 0.003f; - _focus += -getRight() * delta.x * _distance; - _focus += getUp() * delta.y * _distance; + // _focus += -getRight() * delta.x * _distance; + // _focus += getUp() * delta.y * _distance; - updateViewMatrix(); - } + // updateViewMatrix(); + // } - _currentPos2D = pos2d; - } + // _currentPos2D = pos2d; + //} void startFollow(const glm::vec3& pos, const glm::quat& rot, const glm::vec3& offset); void clearFollow() { _following = false; } diff --git a/DSFE_App/DSFE_GUI/include/Scene/FpsCounter.h b/DSFE_App/DSFE_GUI/include/Scene/FpsCounter.h deleted file mode 100644 index 15569a27..00000000 --- a/DSFE_App/DSFE_GUI/include/Scene/FpsCounter.h +++ /dev/null @@ -1,35 +0,0 @@ -#pragma once -// File: FpsCounter.h -// GitHub: SaltyJoss -#include "EngineCore.h" -#include - -#include "Platform/Logger.h" - -namespace gui { - class FpsCounter { - public: - void update() { - double now = glfwGetTime(); - double dt = now - last; - - last = now; - frames++; - accum += dt; - if (accum >= 1.0) { - fps = frames / accum; - frames = 0; - accum = 0.0; - } - } - - double getFPS() const { return fps; } - - private: - bool init = false; - double last = glfwGetTime(); - double accum = 0.0; - int frames = 0; - double fps = 0.0; - }; -} // namespace gui \ No newline at end of file diff --git a/DSFE_App/DSFE_GUI/include/Scene/Input.h b/DSFE_App/DSFE_GUI/include/Scene/Input.h deleted file mode 100644 index 9373fcfe..00000000 --- a/DSFE_App/DSFE_GUI/include/Scene/Input.h +++ /dev/null @@ -1,28 +0,0 @@ -// DSFE_GUI Input.h -#pragma once - -#include -#include - -#include "Platform/Logger.h" - -namespace scene { - enum class eInputButton { - Left = 0, - Right = 1, - Middle = 2, - None = 9 - }; - - class Input { - public: - static eInputButton GetPressedButton(GLFWwindow* window); - - static bool IsKeyPressed(GLFWwindow* window, int key); - static bool IsMouseButtonPressed(GLFWwindow* window, eInputButton button); - - private: - Input() = default; - }; - -} // namespace scene \ No newline at end of file diff --git a/DSFE_App/DSFE_GUI/include/Scene/Manager/SimImplementation.h b/DSFE_App/DSFE_GUI/include/Scene/Manager/SimImplementation.h index e171722c..b0f7bed2 100644 --- a/DSFE_App/DSFE_GUI/include/Scene/Manager/SimImplementation.h +++ b/DSFE_App/DSFE_GUI/include/Scene/Manager/SimImplementation.h @@ -10,7 +10,6 @@ #include #include -#include "Scene/Input.h" #include "Scene/Camera.h" #include "Scene/Mesh.h" #include "Assets/MeshLoader.h" diff --git a/DSFE_App/DSFE_GUI/include/Scene/SimulationManager.h b/DSFE_App/DSFE_GUI/include/Scene/SimulationManager.h index fd1b5c93..f433a54b 100644 --- a/DSFE_App/DSFE_GUI/include/Scene/SimulationManager.h +++ b/DSFE_App/DSFE_GUI/include/Scene/SimulationManager.h @@ -1,6 +1,10 @@ // DSFE_GUI SimulationManager.h #pragma once +#ifdef __gl_h_ +#undef __gl_h_ +#endif +#include #include #include #include @@ -12,7 +16,6 @@ #include "Rendering/ModelGroup.h" #include "Scene/ObjectID.h" #include "ui/RenderPreset.h" -#include "FpsCounter.h" #include "Platform/KeyCode.h" @@ -31,10 +34,8 @@ namespace shaders { class Shader; } // Forward Declarations for Scene namespace scene { - enum class eInputButton; class Light; class Camera; - class Input; class Mesh; class Object; class SceneRenderer; @@ -273,7 +274,7 @@ namespace gui { void processMovementKey(int key, float delta); void handleContinuousMovement(const std::unordered_set& pressedKeys, float dt); void handleMouseLook(double xpos, double ypos, bool mouseCaptured); - void onMouseMove(double x, double y, scene::eInputButton button); + void onMouseMove(double x, double y); void onMouseWheel(double delta); void resetMouseDelta(); @@ -361,9 +362,6 @@ namespace gui { bool _shadowsInit = false; bool _hdrUserOverride = false; - // Editor & UI - gui::FpsCounter _fpsCounter; - // View management bool _isHovered = false; bool skyboxEnabled = true; diff --git a/DSFE_App/DSFE_GUI/include/imfilebrowser/LICENSE b/DSFE_App/DSFE_GUI/include/imfilebrowser/LICENSE deleted file mode 100644 index a656d732..00000000 --- a/DSFE_App/DSFE_GUI/include/imfilebrowser/LICENSE +++ /dev/null @@ -1,21 +0,0 @@ -MIT License - -Copyright (c) 2019-2025 Zhuang Guan - -Permission is hereby granted, free of charge, to any person obtaining a copy -of this software and associated documentation files (the "Software"), to deal -in the Software without restriction, including without limitation the rights -to use, copy, modify, merge, publish, distribute, sublicense, and/or sell -copies of the Software, and to permit persons to whom the Software is -furnished to do so, subject to the following conditions: - -The above copyright notice and this permission notice shall be included in all -copies or substantial portions of the Software. - -THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR -IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, -FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE -AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER -LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, -OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE -SOFTWARE. diff --git a/DSFE_App/DSFE_GUI/include/imfilebrowser/imfilebrowser.h b/DSFE_App/DSFE_GUI/include/imfilebrowser/imfilebrowser.h deleted file mode 100644 index 3b472823..00000000 --- a/DSFE_App/DSFE_GUI/include/imfilebrowser/imfilebrowser.h +++ /dev/null @@ -1,1347 +0,0 @@ -#pragma once - -#include -#include -#include -#include -#include -#include -#include -#include -#include - -#ifndef IMGUI_VERSION -# error "include imgui.h before this header" -#endif - -using ImGuiFileBrowserFlags = std::uint32_t; - -enum ImGuiFileBrowserFlags_ : std::uint32_t -{ - ImGuiFileBrowserFlags_SelectDirectory = 1 << 0, // select directory instead of regular file - ImGuiFileBrowserFlags_EnterNewFilename = 1 << 1, // allow user to enter new filename when selecting regular file - ImGuiFileBrowserFlags_NoModal = 1 << 2, // file browsing window is modal by default. specify this to use a popup window - ImGuiFileBrowserFlags_NoTitleBar = 1 << 3, // hide window title bar - ImGuiFileBrowserFlags_NoStatusBar = 1 << 4, // hide status bar at the bottom of browsing window - ImGuiFileBrowserFlags_CloseOnEsc = 1 << 5, // close file browser when pressing 'ESC' - ImGuiFileBrowserFlags_CreateNewDir = 1 << 6, // allow user to create new directory - ImGuiFileBrowserFlags_MultipleSelection = 1 << 7, // allow user to select multiple files. this will hide ImGuiFileBrowserFlags_EnterNewFilename - ImGuiFileBrowserFlags_HideRegularFiles = 1 << 8, // hide regular files when ImGuiFileBrowserFlags_SelectDirectory is enabled - ImGuiFileBrowserFlags_ConfirmOnEnter = 1 << 9, // confirm selection when pressing 'ENTER' - ImGuiFileBrowserFlags_SkipItemsCausingError = 1 << 10, // when entering a new directory, any error will interrupt the process, causing the file browser to fall back to the working directory. - // with this flag, if an error is caused by a specific item in the directory, that item will be skipped, allowing the process to continue. - ImGuiFileBrowserFlags_EditPathString = 1 << 11, // allow user to directly edit the whole path string -}; - -namespace ImGui -{ - class FileBrowser - { - public: - - explicit FileBrowser( - ImGuiFileBrowserFlags flags = 0, - std::filesystem::path defaultDirectory = std::filesystem::current_path()); - - FileBrowser(const FileBrowser ©From); - - FileBrowser &operator=(const FileBrowser ©From); - - // set the window position (in pixels) - // default is centered - void SetWindowPos(int posX, int posY) noexcept; - - // set the window size (in pixels) - // default is (700, 450) - void SetWindowSize(int width, int height) noexcept; - - // set the window title text - void SetTitle(std::string title); - - // open the browsing window - void Open(); - - // close the browsing window - void Close(); - - // the browsing window is opened or not - bool IsOpened() const noexcept; - - // display the browsing window if opened - void Display(); - - // returns true when there is a selected filename - bool HasSelected() const noexcept; - - // set current browsing directory - bool SetDirectory(const std::filesystem::path &dir = std::filesystem::current_path()); - - // legacy interface. use SetDirectory instead. - bool SetPwd(const std::filesystem::path &dir = std::filesystem::current_path()) - { - return SetDirectory(dir); - } - - // get current browsing directory - const std::filesystem::path &GetDirectory() const noexcept; - - // legacy interface. use GetDirectory instead. - const std::filesystem::path &GetPwd() const noexcept - { - return GetDirectory(); - } - - // returns selected filename. make sense only when HasSelected returns true - // when ImGuiFileBrowserFlags_MultipleSelection is enabled, only one of - // selected filename will be returned - std::filesystem::path GetSelected() const; - - // returns all selected filenames. - // when ImGuiFileBrowserFlags_MultipleSelection is enabled, use this - // instead of GetSelected - std::vector GetMultiSelected() const; - - // set selected filename to empty - void ClearSelected(); - - // (optional) set file type filters. eg. { ".h", ".cpp", ".hpp" } - // ".*" matches any file types - void SetTypeFilters(const std::vector &typeFilters); - - // set currently applied type filter - // default value is 0 (the first type filter) - void SetCurrentTypeFilterIndex(int index); - - // when ImGuiFileBrowserFlags_EnterNewFilename is set - // this function will pre-fill the input dialog with a filename. - void SetInputName(std::string_view input); - - private: - - template - struct ScopeGuard - { - ScopeGuard(Functor&& t) : func(std::move(t)) { } - - ~ScopeGuard() { func(); } - - private: - - Functor func; - }; - - struct FileRecord - { - bool isDir = false; - std::filesystem::path name; - std::string showName; - std::filesystem::path extension; - }; - - static std::string ToLower(const std::string &s); - - void ToolTip(const std::string_view &s); - - void UpdateFileRecords(); - - void SetCurrentDirectoryUncatched(const std::filesystem::path &pwd); - - bool SetCurrentDirectoryInternal( - const std::filesystem::path &dir, - const std::filesystem::path &preferredFallback); - - bool IsExtensionMatched(const std::filesystem::path &extension) const; - - void ClearRangeSelectionState(); - - static void AssignToArrayStyleString(std::vector &arr, std::string_view content); - - static int ExpandInputBuffer(ImGuiInputTextCallbackData *callbackData); - -#ifdef _WIN32 - static std::uint32_t GetDrivesBitMask(); -#endif - - // for c++17 compatibility - -#if defined(__cpp_lib_char8_t) - static std::string u8StrToStr(std::u8string s); -#endif - static std::string u8StrToStr(std::string s); - - static std::filesystem::path u8StrToPath(const char *str); - - int width_; - int height_; - int posX_; - int posY_; - ImGuiFileBrowserFlags flags_; - std::filesystem::path defaultDirectory_; - - std::string title_; - std::string openLabel_; - - bool shouldOpen_; - bool shouldClose_; - bool isOpened_; - bool isOk_; - bool isPosSet_; - - std::string statusStr_; - - std::vector typeFilters_; - unsigned int typeFilterIndex_; - bool hasAllFilter_; - - std::filesystem::path currentDirectory_; - std::vector fileRecords_; - - unsigned int rangeSelectionStart_; // enable range selection when shift is pressed - std::set selectedFilenames_; - - std::string openNewDirLabel_; - std::vector newDirNameBuffer_; - std::vector inputNameBuffer_; - std::string customizedInputName_; - - bool editDir_; - bool setFocusToEditDir_; - std::vector currDirBuffer_; - -#ifdef _WIN32 - std::uint32_t drives_; -#endif - }; -} // namespace ImGui - -inline ImGui::FileBrowser::FileBrowser(ImGuiFileBrowserFlags flags, std::filesystem::path defaultDirectory) - : width_(700) - , height_(450) - , posX_(0) - , posY_(0) - , flags_(flags) - , defaultDirectory_(std::move(defaultDirectory)) - , shouldOpen_(false) - , shouldClose_(false) - , isOpened_(false) - , isOk_(false) - , isPosSet_(false) - , rangeSelectionStart_(0) - , editDir_(false) - , setFocusToEditDir_(false) -{ - assert(!((flags_ & ImGuiFileBrowserFlags_SelectDirectory) && (flags_ & ImGuiFileBrowserFlags_EnterNewFilename)) && - "'EnterNewFilename' doesn't work when 'SelectDirectory' is enabled"); - if(flags_ & ImGuiFileBrowserFlags_CreateNewDir) - { - newDirNameBuffer_.resize(32, '\0'); - } - - SetTitle("file browser"); - SetDirectory(defaultDirectory_); - - typeFilters_.clear(); - typeFilterIndex_ = 0; - hasAllFilter_ = false; - -#ifdef _WIN32 - drives_ = GetDrivesBitMask(); -#endif -} - -inline ImGui::FileBrowser::FileBrowser(const FileBrowser ©From) - : FileBrowser() -{ - *this = copyFrom; -} - -inline ImGui::FileBrowser &ImGui::FileBrowser::operator=( - const FileBrowser ©From) -{ - width_ = copyFrom.width_; - height_ = copyFrom.height_; - - posX_ = copyFrom.posX_; - posY_ = copyFrom.posY_; - - flags_ = copyFrom.flags_; - SetTitle(copyFrom.title_); - - shouldOpen_ = copyFrom.shouldOpen_; - shouldClose_ = copyFrom.shouldClose_; - isOpened_ = copyFrom.isOpened_; - isOk_ = copyFrom.isOk_; - isPosSet_ = copyFrom.isPosSet_; - - statusStr_ = ""; - - typeFilters_ = copyFrom.typeFilters_; - typeFilterIndex_ = copyFrom.typeFilterIndex_; - hasAllFilter_ = copyFrom.hasAllFilter_; - - selectedFilenames_ = copyFrom.selectedFilenames_; - rangeSelectionStart_ = copyFrom.rangeSelectionStart_; - - currentDirectory_ = copyFrom.currentDirectory_; - fileRecords_ = copyFrom.fileRecords_; - - openNewDirLabel_ = copyFrom.openNewDirLabel_; - newDirNameBuffer_ = copyFrom.newDirNameBuffer_; - inputNameBuffer_ = copyFrom.inputNameBuffer_; - customizedInputName_ = copyFrom.customizedInputName_; - - editDir_ = copyFrom.editDir_; - currDirBuffer_ = copyFrom.currDirBuffer_; - -#ifdef _WIN32 - drives_ = copyFrom.drives_; -#endif - - return *this; -} - -inline void ImGui::FileBrowser::SetWindowPos(int posX, int posY) noexcept -{ - posX_ = posX; - posY_ = posY; - isPosSet_ = true; -} - -inline void ImGui::FileBrowser::SetWindowSize(int width, int height) noexcept -{ - assert(width > 0 && height > 0); - width_ = width; - height_ = height; -} - -inline void ImGui::FileBrowser::SetTitle(std::string title) -{ - title_ = std::move(title); - - const std::string thisPtrStr = std::to_string(reinterpret_cast(this)); - openLabel_ = title_ + "##filebrowser_" + thisPtrStr; - openNewDirLabel_ = "new dir##new_dir_" + thisPtrStr; -} - -inline void ImGui::FileBrowser::Open() -{ - UpdateFileRecords(); - ClearSelected(); - statusStr_ = std::string(); - shouldOpen_ = true; - shouldClose_ = false; - if((flags_ & ImGuiFileBrowserFlags_EnterNewFilename) && !customizedInputName_.empty()) - { - AssignToArrayStyleString(inputNameBuffer_, customizedInputName_); - selectedFilenames_ = { u8StrToPath(inputNameBuffer_.data()) }; - } -} - -inline void ImGui::FileBrowser::Close() -{ - ClearSelected(); - statusStr_ = std::string(); - shouldClose_ = true; - shouldOpen_ = false; -} - -inline bool ImGui::FileBrowser::IsOpened() const noexcept -{ - return isOpened_; -} - -inline void ImGui::FileBrowser::Display() -{ - PushID(this); - ScopeGuard exitThis([this] - { - shouldOpen_ = false; - shouldClose_ = false; - PopID(); - }); - - if(shouldOpen_) - { - OpenPopup(openLabel_.c_str()); - } - isOpened_ = false; - - // open the popup window - - if(shouldOpen_ && (flags_ & ImGuiFileBrowserFlags_NoModal)) - { - if(isPosSet_) - { - SetNextWindowPos(ImVec2(static_cast(posX_), static_cast(posY_))); - } - SetNextWindowSize(ImVec2(static_cast(width_), static_cast(height_))); - } - else - { - if(isPosSet_) - { - SetNextWindowPos(ImVec2(static_cast(posX_), static_cast(posY_)), ImGuiCond_FirstUseEver); - } - SetNextWindowSize(ImVec2(static_cast(width_), static_cast(height_)), ImGuiCond_FirstUseEver); - } - if(flags_ & ImGuiFileBrowserFlags_NoModal) - { - if(!BeginPopup(openLabel_.c_str())) - { - return; - } - } - else if(!BeginPopupModal(openLabel_.c_str(), nullptr, - flags_ & ImGuiFileBrowserFlags_NoTitleBar ? ImGuiWindowFlags_NoTitleBar : 0)) - { - return; - } - - isOpened_ = true; - ScopeGuard endPopup([] { EndPopup(); }); - - std::filesystem::path newDir; bool shouldSetNewDir = false; - - if(editDir_) - { - if(setFocusToEditDir_) // Automatically set the text box to be focused on appearing - { - SetKeyboardFocusHere(); - } - - PushItemWidth(-1); - const bool enter = InputText( - "##directory", currDirBuffer_.data(), currDirBuffer_.size(), - ImGuiInputTextFlags_CallbackResize | ImGuiInputTextFlags_EnterReturnsTrue | ImGuiInputTextFlags_AutoSelectAll, - ExpandInputBuffer, &currDirBuffer_); - PopItemWidth(); - - if(!IsItemActive() && !setFocusToEditDir_) - { - editDir_ = false; - } - setFocusToEditDir_ = false; - - if(enter) - { - std::filesystem::path enteredDir = u8StrToPath(currDirBuffer_.data()); - if(is_directory(enteredDir)) - { - newDir = std::move(enteredDir); - shouldSetNewDir = true; - } - else if(is_directory(enteredDir.parent_path())) - { - newDir = enteredDir.parent_path(); - shouldSetNewDir = true; - } - else - { - statusStr_ = "[" + std::string(currDirBuffer_.data()) + "] is not a valid directory"; - } - } - } - else - { - // display elements in pwd - -#ifdef _WIN32 - const char currentDrive = static_cast(currentDirectory_.c_str()[0]); - const char driveStr[] = { currentDrive, ':', '\0' }; - - PushItemWidth(4 * GetFontSize()); - if(BeginCombo("##select_drive", driveStr)) - { - ScopeGuard guard([&] { EndCombo(); }); - - for(int i = 0; i < 26; ++i) - { - if(!(drives_ & (1 << i))) - { - continue; - } - - const char driveCh = static_cast('A' + i); - const char selectableStr[] = { driveCh, ':', '\0' }; - const bool selected = currentDrive == driveCh; - - if(Selectable(selectableStr, selected) && !selected) - { - char newPwd[] = { driveCh, ':', '\\', '\0' }; - SetDirectory(newPwd); - } - } - } - PopItemWidth(); - - SameLine(); -#endif - - int secIdx = 0, newDirLastSecIdx = -1; - for(const auto &sec : currentDirectory_) - { -#ifdef _WIN32 - if(secIdx == 1) - { - ++secIdx; - continue; - } -#endif - - PushID(secIdx); - if(secIdx > 0) - { - SameLine(); - } - if(SmallButton(u8StrToStr(sec.u8string()).c_str())) - { - newDirLastSecIdx = secIdx; - } - PopID(); - - ++secIdx; - } - - if(newDirLastSecIdx >= 0) - { - int i = 0; - std::filesystem::path dstDir; - for(const auto &sec : currentDirectory_) - { - if(i++ > newDirLastSecIdx) - { - break; - } - dstDir /= sec; - } - -#ifdef _WIN32 - if(newDirLastSecIdx == 0) - { - dstDir /= "\\"; - } -#endif - - SetDirectory(dstDir); - } - - if(flags_ & ImGuiFileBrowserFlags_EditPathString) - { - SameLine(); - - if(SmallButton("#")) - { - const auto currDirStr = u8StrToStr(currentDirectory_.u8string()); - currDirBuffer_.resize(currDirStr.size() + 1); - std::memcpy(currDirBuffer_.data(), currDirStr.data(), currDirStr.size()); - currDirBuffer_.back() = '\0'; - - editDir_ = true; - setFocusToEditDir_ = true; - } - else - { - ToolTip("Edit the current path"); - } - } - } - - SameLine(); - if(SmallButton("*")) - { -#ifdef _WIN32 - drives_ = GetDrivesBitMask(); -#endif - - UpdateFileRecords(); - - std::set newSelectedFilenames; - for(auto &name : selectedFilenames_) - { - const auto it = std::find_if( - fileRecords_.begin(), fileRecords_.end(), [&](const FileRecord &record) - { - return name == record.name; - }); - if(it != fileRecords_.end()) - { - newSelectedFilenames.insert(name); - } - } - - if((flags_ & ImGuiFileBrowserFlags_EnterNewFilename) && !inputNameBuffer_.empty() && inputNameBuffer_[0]) - { - newSelectedFilenames.insert(u8StrToPath(inputNameBuffer_.data())); - } - } - else - { - ToolTip("Refresh"); - } - - bool focusOnInputText = false; - if(flags_ & ImGuiFileBrowserFlags_CreateNewDir) - { - SameLine(); - if(SmallButton("+")) - { - OpenPopup(openNewDirLabel_.c_str()); - newDirNameBuffer_[0] = '\0'; - } - else - { - ToolTip("Create a new directory"); - } - - if(BeginPopup(openNewDirLabel_.c_str())) - { - ScopeGuard endNewDirPopup([] { EndPopup(); }); - - InputText( - "name", newDirNameBuffer_.data(), newDirNameBuffer_.size(), - ImGuiInputTextFlags_CallbackResize, ExpandInputBuffer, &newDirNameBuffer_); - focusOnInputText |= IsItemFocused(); - SameLine(); - - if(Button("ok") && newDirNameBuffer_[0] != '\0') - { - ScopeGuard closeNewDirPopup([] { CloseCurrentPopup(); }); - if(create_directory(currentDirectory_ / u8StrToPath(newDirNameBuffer_.data()))) - { - UpdateFileRecords(); - } - else - { - statusStr_ = "failed to create " + std::string(newDirNameBuffer_.data()); - } - } - } - } - - // browse files in a child window - - float reserveHeight = GetFrameHeightWithSpacing(); - if(flags_ & ImGuiFileBrowserFlags_EnterNewFilename) - { - reserveHeight += GetFrameHeightWithSpacing(); - } - - { - BeginChild("ch", ImVec2(0, -reserveHeight), true, - (flags_ & ImGuiFileBrowserFlags_NoModal) ? ImGuiWindowFlags_AlwaysHorizontalScrollbar : 0); - ScopeGuard endChild([] { EndChild(); }); - - const bool shouldHideRegularFiles = - (flags_ & ImGuiFileBrowserFlags_HideRegularFiles) && (flags_ & ImGuiFileBrowserFlags_SelectDirectory); - - for(unsigned int rscIndex = 0; rscIndex < fileRecords_.size(); ++rscIndex) - { - const auto &rsc = fileRecords_[rscIndex]; - if(!rsc.isDir && shouldHideRegularFiles) - { - continue; - } - if(!rsc.isDir && !IsExtensionMatched(rsc.extension)) - { - continue; - } - if(!rsc.name.empty() && rsc.name.c_str()[0] == '$') - { - continue; - } - - const bool selected = selectedFilenames_.find(rsc.name) != selectedFilenames_.end(); - -#if IMGUI_VERSION_NUM >= 19100 - const ImGuiSelectableFlags selectableFlag = ImGuiSelectableFlags_NoAutoClosePopups; -#else - const ImGuiSelectableFlags selectableFlag = ImGuiSelectableFlags_DontClosePopups; -#endif - - if(Selectable(rsc.showName.c_str(), selected, selectableFlag)) - { - const bool wantDir = flags_ & ImGuiFileBrowserFlags_SelectDirectory; - const bool canSelect = rsc.name != ".." && rsc.isDir == wantDir; - const bool rangeSelect = - canSelect && GetIO().KeyShift && - rangeSelectionStart_ < fileRecords_.size() && - (flags_ & ImGuiFileBrowserFlags_MultipleSelection) && - IsWindowFocused(ImGuiFocusedFlags_RootAndChildWindows); - const bool multiSelect = - !rangeSelect && GetIO().KeyCtrl && - (flags_ & ImGuiFileBrowserFlags_MultipleSelection) && - IsWindowFocused(ImGuiFocusedFlags_RootAndChildWindows); - - if(rangeSelect) - { - const unsigned int first = (std::min)(rangeSelectionStart_, rscIndex); - const unsigned int last = (std::max)(rangeSelectionStart_, rscIndex); - selectedFilenames_.clear(); - for(unsigned int i = first; i <= last; ++i) - { - if(fileRecords_[i].isDir != wantDir) - { - continue; - } - if(!wantDir && !IsExtensionMatched(fileRecords_[i].extension)) - { - continue; - } - selectedFilenames_.insert(fileRecords_[i].name); - } - } - else if(selected) - { - if(!multiSelect) - { - selectedFilenames_ = { rsc.name }; - rangeSelectionStart_ = rscIndex; - } - else - { - selectedFilenames_.erase(rsc.name); - } - if(flags_ & ImGuiFileBrowserFlags_EnterNewFilename) - { - AssignToArrayStyleString(inputNameBuffer_, ""); - } - } - else if(canSelect) - { - if(multiSelect) - { - selectedFilenames_.insert(rsc.name); - } - else - { - selectedFilenames_ = { rsc.name }; - } - if(flags_ & ImGuiFileBrowserFlags_EnterNewFilename) - { - const auto rscName = u8StrToStr(rsc.name.u8string()); - AssignToArrayStyleString(inputNameBuffer_, rscName); - } - rangeSelectionStart_ = rscIndex; - } - } - - if(IsMouseDoubleClicked(ImGuiMouseButton_Left) && IsItemHovered(ImGuiHoveredFlags_None)) - { - if(rsc.isDir) - { - shouldSetNewDir = true; - newDir = (rsc.name != "..") ? (currentDirectory_ / rsc.name) : currentDirectory_.parent_path(); - } - else if(!(flags_ & ImGuiFileBrowserFlags_SelectDirectory)) - { - selectedFilenames_ = { rsc.name }; - isOk_ = true; - CloseCurrentPopup(); - } - } - else if(IsKeyPressed(ImGuiKey_GamepadFaceDown) && IsItemHovered()) - { - if(rsc.isDir) - { - shouldSetNewDir = true; - newDir = (rsc.name != "..") ? (currentDirectory_ / rsc.name) : currentDirectory_.parent_path(); - SetKeyboardFocusHere(-1); - } - else if(!(flags_ & ImGuiFileBrowserFlags_SelectDirectory)) - { - selectedFilenames_ = { rsc.name }; - isOk_ = true; - CloseCurrentPopup(); - } - } - } - } - - if(shouldSetNewDir) - { - SetDirectory(newDir); - } - - if(flags_ & ImGuiFileBrowserFlags_EnterNewFilename) - { - PushID(this); - ScopeGuard popTextID([] { PopID(); }); - - if(inputNameBuffer_.empty()) - { - inputNameBuffer_.resize(1, '\0'); - } - - PushItemWidth(-1); - if(InputText( - "", inputNameBuffer_.data(), inputNameBuffer_.size(), - ImGuiInputTextFlags_CallbackResize, ExpandInputBuffer, &inputNameBuffer_)) - { - if(inputNameBuffer_[0] != '\0') - { - selectedFilenames_ = { u8StrToPath(inputNameBuffer_.data()) }; - } - else - { - selectedFilenames_.clear(); - } - } - focusOnInputText |= IsItemFocused(); - PopItemWidth(); - } - - if(!focusOnInputText && !editDir_) - { - const bool selectAll = (flags_ & ImGuiFileBrowserFlags_MultipleSelection) && - IsKeyPressed(ImGuiKey_A) && (IsKeyDown(ImGuiKey_LeftCtrl) || - IsKeyDown(ImGuiKey_RightCtrl)); - if(selectAll) - { - const bool needDir = flags_ & ImGuiFileBrowserFlags_SelectDirectory; - selectedFilenames_.clear(); - for(size_t i = 1; i < fileRecords_.size(); ++i) - { - auto &record = fileRecords_[i]; - if(record.isDir == needDir && - (needDir || IsExtensionMatched(record.extension))) - { - selectedFilenames_.insert(record.name); - } - } - } - } - - const bool isEnterPressed = - (flags_ & ImGuiFileBrowserFlags_ConfirmOnEnter) && - IsWindowFocused(ImGuiFocusedFlags_RootAndChildWindows) && - IsKeyPressed(ImGuiKey_Enter); - if(!(flags_ & ImGuiFileBrowserFlags_SelectDirectory)) - { - BeginDisabled(selectedFilenames_.empty()); - const bool ok = Button("ok"); - EndDisabled(); - if((ok || isEnterPressed) && !selectedFilenames_.empty()) - { - isOk_ = true; - CloseCurrentPopup(); - } - } - else - { - if(Button(" ok ") || isEnterPressed) - { - isOk_ = true; - CloseCurrentPopup(); - } - } - - SameLine(); - - const bool shouldClose = - Button("cancel") || shouldClose_ || - ((flags_ & ImGuiFileBrowserFlags_CloseOnEsc) && - IsWindowFocused(ImGuiFocusedFlags_RootAndChildWindows) && - IsKeyPressed(ImGuiKey_Escape)); - if(shouldClose) - { - CloseCurrentPopup(); - } - - if(!statusStr_.empty() && !(flags_ & ImGuiFileBrowserFlags_NoStatusBar)) - { - SameLine(); - Text("%s", statusStr_.c_str()); - if (ImGui::IsItemHovered()) - { - ImGui::BeginTooltip(); - ImGui::PushTextWrapPos(300.0f); - ImGui::Text("%s", statusStr_.c_str()); - ImGui::PopTextWrapPos(); - ImGui::EndTooltip(); - } - } - - if(!typeFilters_.empty()) - { - SameLine(); - PushItemWidth(8 * GetFontSize()); - if(BeginCombo( - "##type_filters", typeFilters_[typeFilterIndex_].c_str())) - { - ScopeGuard guard([&] { EndCombo(); }); - - for(size_t i = 0; i < typeFilters_.size(); ++i) - { - bool selected = i == typeFilterIndex_; - if(Selectable(typeFilters_[i].c_str(), selected) && !selected) - { - typeFilterIndex_ = static_cast(i); - } - } - } - PopItemWidth(); - } -} - -inline bool ImGui::FileBrowser::HasSelected() const noexcept -{ - return isOk_; -} - -inline bool ImGui::FileBrowser::SetDirectory(const std::filesystem::path &dir) -{ - const std::filesystem::path preferredFallback = this->GetDirectory(); - return SetCurrentDirectoryInternal(dir, preferredFallback); -} - -inline const std::filesystem::path &ImGui::FileBrowser::GetDirectory() const noexcept -{ - return currentDirectory_; -} - -inline std::filesystem::path ImGui::FileBrowser::GetSelected() const -{ - // when isOk_ is true, selectedFilenames_ may be empty if SelectDirectory - // is enabled. return pwd in that case. - if(selectedFilenames_.empty()) - { - return currentDirectory_; - } - return currentDirectory_ / *selectedFilenames_.begin(); -} - -inline std::vector ImGui::FileBrowser::GetMultiSelected() const -{ - if(selectedFilenames_.empty()) - { - return { currentDirectory_ }; - } - - std::vector ret; - ret.reserve(selectedFilenames_.size()); - for(auto &s : selectedFilenames_) - { - ret.push_back(currentDirectory_ / s); - } - - return ret; -} - -inline void ImGui::FileBrowser::ClearSelected() -{ - selectedFilenames_.clear(); - if((flags_ & ImGuiFileBrowserFlags_EnterNewFilename)) - { - AssignToArrayStyleString(inputNameBuffer_, ""); - } - isOk_ = false; -} - -inline void ImGui::FileBrowser::SetTypeFilters(const std::vector &_typeFilters) -{ - typeFilters_.clear(); - - // remove duplicate filter names due to case unsensitivity on windows - -#ifdef _WIN32 - - std::vector typeFilters; - for(auto &rawFilter : _typeFilters) - { - std::string lowerFilter = ToLower(rawFilter); - const auto it = std::find(typeFilters.begin(), typeFilters.end(), lowerFilter); - if(it == typeFilters.end()) - { - typeFilters.push_back(std::move(lowerFilter)); - } - } - -#else - - auto &typeFilters = _typeFilters; - -#endif - - // insert auto-generated filter - hasAllFilter_ = false; - if(typeFilters.size() > 1) - { - hasAllFilter_ = true; - std::string allFiltersName = std::string(); - for(size_t i = 0; i < typeFilters.size(); ++i) - { - if(typeFilters[i] == std::string_view(".*")) - { - hasAllFilter_ = false; - break; - } - - if(i > 0) - { - allFiltersName += ","; - } - allFiltersName += typeFilters[i]; - } - - if(hasAllFilter_) - { - typeFilters_.push_back(std::move(allFiltersName)); - } - } - - std::copy(typeFilters.begin(), typeFilters.end(), std::back_inserter(typeFilters_)); - typeFilterIndex_ = 0; -} - -inline void ImGui::FileBrowser::SetCurrentTypeFilterIndex(int index) -{ - typeFilterIndex_ = static_cast(index); -} - -inline void ImGui::FileBrowser::SetInputName(std::string_view input) -{ - assert((flags_ & ImGuiFileBrowserFlags_EnterNewFilename) && - "SetInputName can only be called when ImGuiFileBrowserFlags_EnterNewFilename is enabled"); - customizedInputName_ = input; -} - -inline std::string ImGui::FileBrowser::ToLower(const std::string &s) -{ - std::string ret = s; - for(char &c : ret) - { - c = static_cast(std::tolower(c)); - } - return ret; -} - -inline void ImGui::FileBrowser::ToolTip(const std::string_view &s) -{ - if (!ImGui::IsItemHovered()) - { - return; - } - ImGui::SetTooltip("%s", s.data()); -} - -inline void ImGui::FileBrowser::UpdateFileRecords() -{ - fileRecords_ = { FileRecord{ true, "..", "[D] ..", "" } }; - - const auto getDirectoryIterator = [&]() -> std::filesystem::directory_iterator - { - try - { - return std::filesystem::directory_iterator(currentDirectory_); - } - catch (const std::filesystem::filesystem_error& err) - { - statusStr_ = std::string("error: ") + err.what(); - if (!(flags_ & ImGuiFileBrowserFlags_SkipItemsCausingError)) - { - throw; - } - return {}; - } - }; - - for(auto &p : getDirectoryIterator()) - { - FileRecord rcd; - try - { - if(p.is_regular_file()) - { - rcd.isDir = false; - } - else if(p.is_directory()) - { - rcd.isDir = true; - } - else - { - continue; - } - - rcd.name = p.path().filename(); - if(rcd.name.empty()) - { - continue; - } - - rcd.extension = p.path().filename().extension(); - rcd.showName = (rcd.isDir ? "[D] " : "[F] ") + u8StrToStr(p.path().filename().u8string()); - } - catch(...) - { - if(!(flags_ & ImGuiFileBrowserFlags_SkipItemsCausingError)) - { - throw; - } - continue; - } - fileRecords_.push_back(rcd); - } - - // The default lexicographical order does not meet our sorting requirements. - // We want [b0, a0, A1] to be sorted into something like [a0, A1, b0] instead of [a0, b0, A1]. - // Therefore, here we compute a custom key for each filename for sorting. - if(fileRecords_.size() > 2) - { - std::vector> keys; - keys.reserve(fileRecords_.size()); - for(auto &fileRecord : fileRecords_) - { - const auto name = u8StrToStr(fileRecord.name.u8string()); - auto& key = keys.emplace_back(); - key.reserve(name.size() + 1); - key.emplace_back(!fileRecord.isDir); - for(char c : name) - { - if('A' <= c && c <= 'Z') - { - key.emplace_back(2 * (c + 'a' - 'A') + 1); - } - else - { - key.emplace_back(2 * c); - } - } - } - - std::vector fileRecordRemapIndices; - fileRecordRemapIndices.reserve(fileRecords_.size()); - for(uint32_t i = 0; i < fileRecords_.size(); ++i) - { - fileRecordRemapIndices.push_back(i); - } - - std::sort( - fileRecordRemapIndices.begin() + 1, fileRecordRemapIndices.end(), [&](uint32_t li, uint32_t ri) - { - return keys[li] < keys[ri]; - }); - - std::vector remappedFileRecords; - remappedFileRecords.reserve(fileRecords_.size()); - for(const uint32_t index : fileRecordRemapIndices) - { - remappedFileRecords.emplace_back(std::move(fileRecords_[index])); - } - - fileRecords_ = std::move(remappedFileRecords); - } - - ClearRangeSelectionState(); -} - -inline void ImGui::FileBrowser::SetCurrentDirectoryUncatched(const std::filesystem::path &pwd) -{ - currentDirectory_ = absolute(pwd); - UpdateFileRecords(); - - bool shouldClearInputNameBuffer = true; - - if((flags_ & ImGuiFileBrowserFlags_EnterNewFilename) && - selectedFilenames_.size() == 1 && - !customizedInputName_.empty() && - !inputNameBuffer_.empty() && - std::strcmp(inputNameBuffer_.data(), customizedInputName_.data()) == 0) - { - shouldClearInputNameBuffer = false; - } - - if(shouldClearInputNameBuffer) - { - selectedFilenames_.clear(); - AssignToArrayStyleString(inputNameBuffer_, ""); - } -} - -inline bool ImGui::FileBrowser::SetCurrentDirectoryInternal( - const std::filesystem::path &dir, const std::filesystem::path &preferredFallback) -{ - try - { - SetCurrentDirectoryUncatched(dir); - return true; - } - catch(const std::exception &err) - { - statusStr_ = std::string("error: ") + err.what(); - } - catch(...) - { - statusStr_ = "unknown error"; - } - - if(preferredFallback != defaultDirectory_) - { - try - { - SetCurrentDirectoryUncatched(preferredFallback); - } - catch(...) - { - SetCurrentDirectoryUncatched(defaultDirectory_); - } - } - else - { - SetCurrentDirectoryUncatched(defaultDirectory_); - } - - return false; -} - -inline bool ImGui::FileBrowser::IsExtensionMatched(const std::filesystem::path &_extension) const -{ -#ifdef _WIN32 - std::filesystem::path extension = ToLower(u8StrToStr(_extension.u8string())); -#else - auto &extension = _extension; -#endif - - // no type filters - if(typeFilters_.empty()) - { - return true; - } - - // invalid type filter index - if(static_cast(typeFilterIndex_) >= typeFilters_.size()) - { - return true; - } - - // all type filters - if(hasAllFilter_ && typeFilterIndex_ == 0) - { - for(size_t i = 1; i < typeFilters_.size(); ++i) - { - if(extension == typeFilters_[i]) - { - return true; - } - } - return false; - } - - // universal filter - if(typeFilters_[typeFilterIndex_] == std::string_view(".*")) - { - return true; - } - - // regular filter - return extension == typeFilters_[typeFilterIndex_]; -} - -inline void ImGui::FileBrowser::ClearRangeSelectionState() -{ - rangeSelectionStart_ = 9999999; - const bool dir = flags_ & ImGuiFileBrowserFlags_SelectDirectory; - for(unsigned int i = 1; i < fileRecords_.size(); ++i) - { - if(fileRecords_[i].isDir == dir) - { - if(!dir && !IsExtensionMatched(fileRecords_[i].extension)) - { - continue; - } - rangeSelectionStart_ = i; - break; - } - } -} - -inline void ImGui::FileBrowser::AssignToArrayStyleString(std::vector &arr, std::string_view content) -{ - if(content.empty()) - { - if(!arr.empty()) - { - arr[0] = '\0'; - } - return; - } - - if(arr.size() < content.size() + 1) - { - arr.resize(content.size() + 1); - } - std::memcpy(arr.data(), content.data(), content.size()); - arr[content.size()] = '\0'; -} - -inline int ImGui::FileBrowser::ExpandInputBuffer(ImGuiInputTextCallbackData *callbackData) -{ - if(callbackData && callbackData->EventFlag & ImGuiInputTextFlags_CallbackResize) - { - auto buffer = static_cast*>(callbackData->UserData); - size_t newSize = buffer->size(); - while(newSize < static_cast(callbackData->BufSize)) - { - newSize <<= 1; - } - buffer->resize(newSize, '\0'); - callbackData->Buf = buffer->data(); - callbackData->BufDirty = true; - } - return 0; -} - -#if defined(__cpp_lib_char8_t) -inline std::string ImGui::FileBrowser::u8StrToStr(std::u8string s) -{ - std::string result; - result.resize(s.length()); - std::memcpy(result.data(), s.data(), s.length()); - return result; -} -#endif - -inline std::string ImGui::FileBrowser::u8StrToStr(std::string s) -{ - return s; -} - -inline std::filesystem::path ImGui::FileBrowser::u8StrToPath(const char *str) -{ -#if defined(__cpp_lib_char8_t) - // With C++20/23, it's impossible to efficiently convert a `char*` string to a `char8_t*` string without violating - // the strict aliasing rule. Bad joke! - const size_t len = std::strlen(str); - std::u8string u8Str; - u8Str.resize(len); - std::memcpy(u8Str.data(), str, len); - return std::filesystem::path(u8Str); -#else - // u8path is deprecated in C++20 - return std::filesystem::u8path(str); -#endif -} - -#ifdef _WIN32 - -inline std::uint32_t ImGui::FileBrowser::GetDrivesBitMask() -{ - std::uint32_t ret = 0; - for(int i = 0; i < 26; ++i) - { - const char rootName[4] = { static_cast('A' + i), ':', '\\', '\0' }; - try{ - if (std::filesystem::exists(rootName)) - { - ret |= (1 << i); - } - } - catch (const std::filesystem::filesystem_error &) - { - // Ignore invalid paths or inaccessible drives, e.g., empty CD drives or network shares - } - } - return ret; -} - -#endif diff --git a/DSFE_App/DSFE_GUI/src/Scene/AxisOrientator.cpp b/DSFE_App/DSFE_GUI/src/Scene/AxisOrientator.cpp index 8052b41d..e0bf0eac 100644 --- a/DSFE_App/DSFE_GUI/src/Scene/AxisOrientator.cpp +++ b/DSFE_App/DSFE_GUI/src/Scene/AxisOrientator.cpp @@ -8,8 +8,6 @@ #include #include -#include // needed if you keep any ImGui usage - namespace gui { // --- Static Member Definitions --- GLuint AxisOrientator::g_VAO = 0; diff --git a/DSFE_App/DSFE_GUI/src/Scene/Input.cpp b/DSFE_App/DSFE_GUI/src/Scene/Input.cpp deleted file mode 100644 index df7743bc..00000000 --- a/DSFE_App/DSFE_GUI/src/Scene/Input.cpp +++ /dev/null @@ -1,41 +0,0 @@ -#include "pch.h" -// File: Input.cpp -// GitHub: SaltyJoss -#include "Scene/Input.h" -#include - -#include "EngineLib/LogMacros.h" - -using namespace scene; - -// Get the currently pressed mouse button, if any -eInputButton Input::GetPressedButton(GLFWwindow* window) { - if (glfwGetMouseButton(window, GLFW_MOUSE_BUTTON_LEFT) == GLFW_PRESS) { return eInputButton::Left; } - if (glfwGetMouseButton(window, GLFW_MOUSE_BUTTON_RIGHT) == GLFW_PRESS) { return eInputButton::Right; } - if (glfwGetMouseButton(window, GLFW_MOUSE_BUTTON_MIDDLE) == GLFW_PRESS) { return eInputButton::Middle; } - return eInputButton::None; -} - -// Check if a specific key is currently pressed -bool Input::IsKeyPressed(GLFWwindow* window, int key) { - return glfwGetKey(window, key) == GLFW_PRESS; -} - -// Check if a specific mouse button is currently pressed -bool Input::IsMouseButtonPressed(GLFWwindow* window, eInputButton button) { - int glfwButton; - switch (button) { - case eInputButton::Left: - glfwButton = GLFW_MOUSE_BUTTON_LEFT; - break; - case eInputButton::Right: - glfwButton = GLFW_MOUSE_BUTTON_RIGHT; - break; - case eInputButton::Middle: - glfwButton = GLFW_MOUSE_BUTTON_MIDDLE; - break; - default: - return false; - } - return glfwGetMouseButton(window, glfwButton) == GLFW_PRESS; -} \ No newline at end of file diff --git a/DSFE_App/DSFE_GUI/src/Scene/Manager/SimUI.cpp b/DSFE_App/DSFE_GUI/src/Scene/Manager/SimUI.cpp deleted file mode 100644 index 36c20079..00000000 --- a/DSFE_App/DSFE_GUI/src/Scene/Manager/SimUI.cpp +++ /dev/null @@ -1,182 +0,0 @@ -// DSFE_GUI SimUI.cpp -#include "Scene/SimulationManager.h" -#ifdef __gl_h_ -#undef __gl_h_ -#endif -#include "Manager/SimImplementation.h" - -#include - -// TODO: Convert logic to Qt6 and only ever use ImGui for debugging at most (or not at all) - -namespace gui { - // Main Dockspace with Menu Bar - void SimManager::drawMainDockspace() { - ImGuiWindowFlags flags = - ImGuiWindowFlags_NoDocking | - ImGuiWindowFlags_NoTitleBar | - ImGuiWindowFlags_NoCollapse | - ImGuiWindowFlags_NoResize | - ImGuiWindowFlags_NoMove | - ImGuiWindowFlags_NoBringToFrontOnFocus | - ImGuiWindowFlags_NoNavFocus; - - const ImGuiViewport* vp = ImGui::GetMainViewport(); - ImGui::SetNextWindowPos(vp->Pos); - ImGui::SetNextWindowSize(vp->Size); - ImGui::SetNextWindowViewport(vp->ID); - - ImGui::PushStyleVar(ImGuiStyleVar_WindowRounding, 0.0f); - ImGui::PushStyleVar(ImGuiStyleVar_WindowBorderSize, 0.0f); - - // Must be a window so DockSpace has somewhere to live - ImGui::Begin("##MainDockspace", nullptr, flags); - - ImGui::PopStyleVar(2); - - beginSimManager("##MainDockspaceChild"); - - ImGuiID dock_id = ImGui::GetID("MainDockspaceID"); - ImGui::DockSpace(dock_id, ImVec2(0, 0), ImGuiDockNodeFlags_PassthruCentralNode); - - endSimManager(); - - ImGui::End(); - } - - // Viewport Window - void SimManager::drawViewportWindow() { - ImGui::Begin("Viewport", nullptr, - ImGuiWindowFlags_NoScrollbar | - ImGuiWindowFlags_NoScrollWithMouse); - - beginSimManager("##ViewportBody"); - - // --- Tabs: Single / Quad --- - if (ImGui::BeginTabBar("ViewportTabs", ImGuiTabBarFlags_None)) { - const bool singleSelected = ImGui::BeginTabItem("Single"); - if (singleSelected) { - // Restore to Manual view when switching back from Quad - if (_impl->viewMode == Impl::ViewMode::Quad) { - _impl->activeView = gui::ViewID::Manual; - } - _impl->viewMode = Impl::ViewMode::Single; - ImGui::EndTabItem(); - } - - const bool quadSelected = ImGui::BeginTabItem("Quad"); - if (quadSelected) { - _impl->viewMode = Impl::ViewMode::Quad; - ImGui::EndTabItem(); - } - - ImGui::EndTabBar(); - } - - // Everything below tabs is render output - _isHovered = ImGui::IsWindowHovered(ImGuiHoveredFlags_RootAndChildWindows); - - ImVec2 panel = ImGui::GetContentRegionAvail(); - - // Pixel size (framebuffer coords) - int vpW = (int)(panel.x); - int vpH = (int)(panel.y); - vpW = std::max(1, vpW); - vpH = std::max(1, vpH); - - // Update DISPLAY size only - if ((int)_displaySize.x != vpW || (int)_displaySize.y != vpH) { - _displaySize = { (float)vpW, (float)vpH }; - - // invalidate only display cached sizes so post buffers resize - for (auto& v : _impl->_views) { - v.displayW = 0; - v.displayH = 0; - } - //LOG_INFO("Viewport display size updated to %dx%d", vpW, vpH); - } - - // --- Render + Present --- - if (_impl->viewMode == Impl::ViewMode::Quad) { - int halfW = std::max(1, vpW / 2); - int halfH = std::max(1, vpH / 2); - - _impl->renderView(*this, _impl->_views[(size_t)gui::ViewID::Top], halfW, halfH); - _impl->renderView(*this, _impl->_views[(size_t)gui::ViewID::Front], halfW, halfH); - _impl->renderView(*this, _impl->_views[(size_t)gui::ViewID::Right], halfW, halfH); - _impl->renderView(*this, _impl->_views[(size_t)gui::ViewID::Follow], halfW, halfH); - - // Stable 2x2 layout - ImVec2 avail = ImGui::GetContentRegionAvail(); - ImVec2 cell = ImVec2(avail.x * 0.5f, avail.y * 0.5f); - - // Helper to draw each cell with the same pattern - auto drawCell = [&](const char* childId, gui::ViewID id, bool sameLine) { - if (sameLine) ImGui::SameLine(); - ImGui::BeginChild(childId, cell, false, ImGuiWindowFlags_NoScrollbar | ImGuiWindowFlags_NoScrollWithMouse); - - auto& v = _impl->_views[(size_t)id]; - ImVec2 inner = ImGui::GetContentRegionAvail(); - - ImGui::Image((ImTextureID)(intptr_t)v.post->getTexture(), inner, ImVec2(0, 1), ImVec2(1, 0)); - - // Set active view when clicking inside the quad cell - if (ImGui::IsWindowHovered() && ImGui::IsMouseClicked(ImGuiMouseButton_Left)) { - _impl->activeView = id; - } - - // Handle scroll zoom on hovered quad cell - if (ImGui::IsWindowHovered()) { - float scrollY = ImGui::GetIO().MouseWheel; - if (scrollY != 0.0f) { - v.cam->onMouseWheel((double)scrollY); - } - } - - // Visual indication: draw a border around the active cell - if (_impl->activeView == id) { - ImDrawList* dl = ImGui::GetWindowDrawList(); - ImVec2 p0 = ImGui::GetWindowPos(); - ImVec2 p1 = ImVec2(p0.x + ImGui::GetWindowSize().x, p0.y + ImGui::GetWindowSize().y); - dl->AddRect(p0, p1, IM_COL32(255, 200, 0, 180), 4.0f, 0, 2.0f); - } - - ImGui::EndChild(); - }; - - drawCell("##Top", gui::ViewID::Top, false); - drawCell("##Front", gui::ViewID::Front, true); - drawCell("##Right", gui::ViewID::Right, false); - drawCell("##Follow", gui::ViewID::Follow, true); - } - else { - auto& v = _impl->_views[static_cast(_impl->activeView)]; - _impl->renderView(*this, v, vpW, vpH); - - ImGui::Image((ImTextureID)(intptr_t)v.post->getTexture(), panel, ImVec2(0, 1), ImVec2(1, 0)); - } - - endSimManager(); - - ImGui::End(); - } - - // Begin Control Panel Helper - void SimManager::beginSimManager(const char* id) { - ImGui::PushStyleVar(ImGuiStyleVar_WindowPadding, ImVec2(12.0f, 10.0f)); - ImGui::PushStyleVar(ImGuiStyleVar_ChildRounding, 6.0f); - ImGui::PushStyleVar(ImGuiStyleVar_FramePadding, ImVec2(8.0f, 5.0f)); - ImGui::PushStyleVar(ImGuiStyleVar_ItemSpacing, ImVec2(10.0f, 8.0f)); - - ImGui::BeginChild(id, ImVec2(0, 0), true, - ImGuiChildFlags_AlwaysUseWindowPadding | - ImGuiWindowFlags_NoScrollbar | - ImGuiWindowFlags_NoScrollWithMouse); - } - - // End Control Panel Helper - void SimManager::endSimManager() { - ImGui::EndChild(); - ImGui::PopStyleVar(4); - } -} \ No newline at end of file diff --git a/DSFE_App/DSFE_GUI/src/Scene/Object.cpp b/DSFE_App/DSFE_GUI/src/Scene/Object.cpp index 8a4758e7..e2e78680 100644 --- a/DSFE_App/DSFE_GUI/src/Scene/Object.cpp +++ b/DSFE_App/DSFE_GUI/src/Scene/Object.cpp @@ -9,7 +9,6 @@ #include "Physics/PhysicsState.h" #include "Scene/ObjectID.h" -#include "Scene/Input.h" #include "Scene/Mesh.h" using namespace mathlib; @@ -45,25 +44,25 @@ namespace scene { // Update method passes material properties to the shader void Object::update(shaders::Shader* shader) { if (_mesh) _mesh->update(shader); } - // Handle mouse movement for object manipulation + // Handle mouse movement for object manipulation **DECREPATED** void Object::onMouseMove(double x, double y, eInputButton button) { glm::vec2 pos2d{ x, y }; glm::vec2 delta = pos2d - _lastMousePos; _lastMousePos = pos2d; - if (button == eInputButton::Right) { - delta *= 0.004f; - float yaw = delta.x; - float pitch = -delta.y; + //if (button == eInputButton::Right) { + // delta *= 0.004f; + // float yaw = delta.x; + // float pitch = -delta.y; - glm::quat qYaw = glm::angleAxis(yaw, glm::vec3(0.0f, 1.0f, 0.0f)); - glm::vec3 right = transform.rotQ * glm::vec3(1.0f, 0.0f, 0.0f); - glm::quat qPitch = glm::angleAxis(pitch, glm::normalize(right)); + // glm::quat qYaw = glm::angleAxis(yaw, glm::vec3(0.0f, 1.0f, 0.0f)); + // glm::vec3 right = transform.rotQ * glm::vec3(1.0f, 0.0f, 0.0f); + // glm::quat qPitch = glm::angleAxis(pitch, glm::normalize(right)); - } - else if (button == eInputButton::Left) { - delta *= 0.003f; - transform.position += glm::vec3(delta.x * _distance, -delta.y * _distance, 0.0f); - } + //} + //else if (button == eInputButton::Left) { + // delta *= 0.003f; + // transform.position += glm::vec3(delta.x * _distance, -delta.y * _distance, 0.0f); + //} } } diff --git a/DSFE_App/DSFE_GUI/src/Scene/SimulationManager.cpp b/DSFE_App/DSFE_GUI/src/Scene/SimulationManager.cpp index 3f2b6845..9487201d 100644 --- a/DSFE_App/DSFE_GUI/src/Scene/SimulationManager.cpp +++ b/DSFE_App/DSFE_GUI/src/Scene/SimulationManager.cpp @@ -11,11 +11,6 @@ extern "C" core::ISimulationCore* CreateSimulationCore_v1(); extern "C" void DestroySimulationCore(core::ISimulationCore*); -#ifdef __gl_h_ -#undef __gl_h_ -#endif -#include - #include "Manager/SimImplementation.h" #include "Platform/KeyCode.h" @@ -76,7 +71,7 @@ namespace gui { // CONSTRUCTOR & DESTRUCTOR // -------------------------------------------------- - SimManager::SimManager() : _internalSize(1280, 720), _displaySize(1.0f, 1.0f), _backgroundColour(0.18f, 0.18f, 0.20f), + SimManager::SimManager() : _internalSize(1920, 1080), _displaySize(1.0f, 1.0f), _backgroundColour(0.18f, 0.18f, 0.20f), _backgroundAlpha(1.0f), _impl(std::make_unique(*this)), _core(std::make_unique()), _studyRunner(std::make_unique(makeCoreFactory, std::thread::hardware_concurrency() > 1 ? std::thread::hardware_concurrency() - 1 : 1)) { _core->setRobotSystem(_impl->_robotSystem.get()); @@ -174,7 +169,6 @@ namespace gui { if (_impl->_robotSystem && hasRobot()) { _impl->_robotRenderer->applyTransforms(_impl->_robotSystem->model(), _impl->_robotSystem->worldTransforms()); } - _fpsCounter.update(); auto& view = _impl->_views[static_cast(_impl->activeView)]; _impl->renderView(*this, view, w, h); glBindFramebuffer(GL_FRAMEBUFFER, _presentationFBO); @@ -262,6 +256,8 @@ namespace gui { if (pressedKeys.contains(eKeyCode::LShift)) { processMovementKey((int)eKeyCode::LShift, kspd); } } +// Handle mouse look (camera rotation) based on mouse movement. **OLD LOGIC FOR IMGUI AND GLFW** +#pragma region DecrepatedInputLogic void gui::SimManager::handleMouseLook(double xpos, double ypos, bool mouseCaptured) { if (_impl->viewMode == Impl::ViewMode::Quad) { return; } // No mouse look in quad view scene::Camera* cam = _impl->_views[static_cast(_impl->activeView)].cam.get(); @@ -281,10 +277,9 @@ namespace gui { _lastMousePos = { static_cast(xpos), static_cast(ypos) }; if (ctrlMode == ControlMode::Camera) { cam->processMouseMovement(static_cast(xoffset), static_cast(yoffset)); } - else if (ctrlMode == ControlMode::Object && _impl->_selectedObject) { _impl->_selectedObject->onMouseMove(xpos, ypos, scene::eInputButton::Right); } + else if (ctrlMode == ControlMode::Object && _impl->_selectedObject) { return; /*_impl->_selectedObject->onMouseMove(xpos, ypos, scene::eInputButton::Right);*/ } } - - void SimManager::onMouseMove(double x, double y, scene::eInputButton button) { + void SimManager::onMouseMove(double x, double y) { scene::Camera* cam = _impl->_views[static_cast(_impl->activeView)].cam.get(); glm::vec2 pos2d{ x, y }; glm::vec2 delta = pos2d - _lastMousePos; @@ -298,10 +293,9 @@ namespace gui { return; } - if (ctrlMode == ControlMode::Camera) { cam->onMouseMove(x, y, button); } - else if (ctrlMode == ControlMode::Object && _impl->_selectedObject) { _impl->_selectedObject->onMouseMove(x, y, button); } + //if (ctrlMode == ControlMode::Camera) { cam->onMouseMove(x, y, button); } + //else if (ctrlMode == ControlMode::Object && _impl->_selectedObject) { _impl->_selectedObject->onMouseMove(x, y, button); } } - void SimManager::onMouseWheel(double delta) { scene::Camera* cam = _impl->_views[static_cast(_impl->activeView)].cam.get(); auto* obj = _impl->_selectedObject; @@ -317,4 +311,5 @@ namespace gui { } void gui::SimManager::resetMouseDelta() { _firstMouse = true; } +#pragma endregion } \ No newline at end of file From 45c3405a3024c26657e0b9ee0b8400470e728d40 Mon Sep 17 00:00:00 2001 From: saltyjoss Date: Sat, 13 Jun 2026 08:39:19 +0100 Subject: [PATCH 144/210] fixes: Removed CMake fetch and includes of previously removed libraries --- CMakeLists.txt | 23 --------------------- DSFE_App/DSFE_GUI/CMakeLists.txt | 35 -------------------------------- 2 files changed, 58 deletions(-) diff --git a/CMakeLists.txt b/CMakeLists.txt index a4c4881a..2a712153 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -22,13 +22,6 @@ FetchContent_Declare( ) FetchContent_MakeAvailable(eigen) -FetchContent_Declare( - glfw - GIT_REPOSITORY https://github.com/glfw/glfw.git - GIT_TAG 3.4 -) -FetchContent_MakeAvailable(glfw) - # Fetch GLM from GitHub FetchContent_Declare( glm @@ -57,22 +50,6 @@ FetchContent_Declare( set(gtest_force_shared_crt ON CACHE BOOL "" FORCE) FetchContent_MakeAvailable(googletest) -FetchContent_Declare( - imgui - GIT_REPOSITORY https://github.com/ocornut/imgui.git - GIT_TAG docking - GIT_SHALLOW TRUE -) -FetchContent_MakeAvailable(imgui) - -FetchContent_Declare( - implot - GIT_REPOSITORY https://github.com/epezent/implot.git - GIT_TAG v0.17 - GIT_SHALLOW TRUE -) -FetchContent_MakeAvailable(implot) - FetchContent_Declare( nlohmann_json GIT_REPOSITORY https://github.com/nlohmann/json.git diff --git a/DSFE_App/DSFE_GUI/CMakeLists.txt b/DSFE_App/DSFE_GUI/CMakeLists.txt index 5054bb97..f058115f 100644 --- a/DSFE_App/DSFE_GUI/CMakeLists.txt +++ b/DSFE_App/DSFE_GUI/CMakeLists.txt @@ -22,7 +22,6 @@ find_package(Qt6 REQUIRED COMPONENTS target_compile_definitions(DSFE_GUI PRIVATE DSFE_GUI_EXPORTS - IMGUI_ENABLE_DOCKING GLM_ENABLE_EXPERIMENTAL ) @@ -55,7 +54,6 @@ set(RENDER_SRC set(SCENE_SRC src/Scene/AxisOrientator.cpp src/Scene/Camera.cpp - src/Scene/Input.cpp src/Scene/Mesh.cpp src/Scene/Object.cpp src/Scene/SimulationManager.cpp @@ -67,7 +65,6 @@ set(MANAGER_SRC src/Scene/Manager/SimObjects.cpp src/Scene/Manager/SimRendering.cpp src/Scene/Manager/SimRobots.cpp - src/Scene/Manager/SimUI.cpp src/Scene/Manager/SimViewport.cpp ) @@ -114,35 +111,6 @@ target_sources(DSFE_GUI PRIVATE ${QT_RESOURCES} ) -set(imfilebrowser_ROOT ${CMAKE_CURRENT_SOURCE_DIR}/include/imfilebrowser) - -add_library(imgui STATIC - ${imgui_SOURCE_DIR}/imgui.cpp - ${imgui_SOURCE_DIR}/imgui_draw.cpp - ${imgui_SOURCE_DIR}/imgui_tables.cpp - ${imgui_SOURCE_DIR}/imgui_widgets.cpp - - ${imgui_SOURCE_DIR}/backends/imgui_impl_glfw.cpp - ${imgui_SOURCE_DIR}/backends/imgui_impl_opengl3.cpp -) - -target_include_directories(imgui PUBLIC - ${imgui_SOURCE_DIR} - ${imgui_SOURCE_DIR}/backends -) - -target_link_libraries(imgui PUBLIC glfw OpenGL::GL) - -add_library(implot STATIC - ${implot_SOURCE_DIR}/implot.cpp - ${implot_SOURCE_DIR}/implot_items.cpp -) - -target_include_directories(implot PUBLIC - ${implot_SOURCE_DIR} - ${imgui_SOURCE_DIR} -) - target_include_directories(DSFE_GUI PUBLIC ${CMAKE_CURRENT_SOURCE_DIR} @@ -163,13 +131,10 @@ target_link_libraries(DSFE_GUI Threads::Threads PRIVATE - implot stb assimp - glfw glad glm::glm - imgui OpenGL::GL Qt6::Core Qt6::Gui From 23bd1af1c0c231789c23d61ee6345dc7e60c189c Mon Sep 17 00:00:00 2001 From: saltyjoss Date: Sat, 13 Jun 2026 09:44:29 +0100 Subject: [PATCH 145/210] feat: Added new `MeshPresentationBuilder` class with a dedicated `build()` method for general objects --- DSFE_App/DSFE_GUI/CMakeLists.txt | 8 ++++--- .../include/Assets/MeshPresentationBuilder.h | 23 +++++++++++++++++++ .../src/Assets/MeshPresentationBuilder.cpp | 16 +++++++++++++ 3 files changed, 44 insertions(+), 3 deletions(-) create mode 100644 DSFE_App/DSFE_GUI/include/Assets/MeshPresentationBuilder.h create mode 100644 DSFE_App/DSFE_GUI/src/Assets/MeshPresentationBuilder.cpp diff --git a/DSFE_App/DSFE_GUI/CMakeLists.txt b/DSFE_App/DSFE_GUI/CMakeLists.txt index f058115f..031a2de9 100644 --- a/DSFE_App/DSFE_GUI/CMakeLists.txt +++ b/DSFE_App/DSFE_GUI/CMakeLists.txt @@ -85,6 +85,10 @@ set(ROBOT_SRC src/Robots/RobotRenderer.cpp ) +set(OBJECT_SRC + src/Assets/MeshPresentationBuilder.cpp +) + set(PLATFORM_SRC src/Application.cpp ) @@ -106,6 +110,7 @@ target_sources(DSFE_GUI PRIVATE ${MANAGER_SRC} ${WIDGETS_SRC} ${ROBOT_SRC} + ${OBJECT_SRC} ${PLATFORM_SRC} ${ASSETS_SRC} ${QT_RESOURCES} @@ -118,9 +123,6 @@ target_include_directories(DSFE_GUI ${CMAKE_CURRENT_SOURCE_DIR}/include/MainWindow ${CMAKE_CURRENT_SOURCE_DIR}/include/Scene ${CMAKE_CURRENT_SOURCE_DIR}/include/MainWindow/style - - PRIVATE - ${imfilebrowser_ROOT} ) # Link DSFE_Core against its dependencies diff --git a/DSFE_App/DSFE_GUI/include/Assets/MeshPresentationBuilder.h b/DSFE_App/DSFE_GUI/include/Assets/MeshPresentationBuilder.h new file mode 100644 index 00000000..f14fc7f6 --- /dev/null +++ b/DSFE_App/DSFE_GUI/include/Assets/MeshPresentationBuilder.h @@ -0,0 +1,23 @@ +// DSFE_GUI MeshPresentationBuilder.h +#pragma once + + +#include +#include + +namespace scene { + class Mesh; + class Object; +} + +namespace presentation { + struct MeshRenderBinding { + std::vector> owndObjs; + std::vector visObjs; + }; + + class MeshPresentationBuilder { + public: + MeshRenderBinding build(const std::vector>& meshes); + }; +} // namespace presentation \ No newline at end of file diff --git a/DSFE_App/DSFE_GUI/src/Assets/MeshPresentationBuilder.cpp b/DSFE_App/DSFE_GUI/src/Assets/MeshPresentationBuilder.cpp new file mode 100644 index 00000000..98ab2463 --- /dev/null +++ b/DSFE_App/DSFE_GUI/src/Assets/MeshPresentationBuilder.cpp @@ -0,0 +1,16 @@ +// DSFE_GUI MeshPresentationBuilder.cpp +#include "Assets/MeshPresentationBuilder.h" +#include "Scene/Object.h" + +namespace presentation { + MeshRenderBinding MeshPresentationBuilder::build(const std::vector>& meshes) { + MeshRenderBinding binding; + for (const auto& mesh : meshes) { + auto obj = std::make_unique(mesh); + obj->category = scene::ObjectCategory::General; + binding.visObjs.push_back(obj.get()); + binding.owndObjs.push_back(std::move(obj)); + } + return binding; + } +} // namespace presentation \ No newline at end of file From add51c7cf89395877742c6b5c5529f2cd23ec54c Mon Sep 17 00:00:00 2001 From: saltyjoss Date: Sat, 13 Jun 2026 11:58:38 +0100 Subject: [PATCH 146/210] chore: Code cleanup of unused / old code in GUI files * Need to actually RE-ADD physics logic for single objects, maybe add this into PxM rather than Core, and step the logic in Core only. --- DSFE_App/DSFE_GUI/include/Scene/Camera.h | 33 +----------- .../include/Scene/Manager/SimImplementation.h | 36 ++++++++----- DSFE_App/DSFE_GUI/include/Scene/Mesh.h | 2 +- DSFE_App/DSFE_GUI/include/Scene/Object.h | 3 -- .../include/Scene/SimulationManager.h | 1 - DSFE_App/DSFE_GUI/src/Assets/MeshLoader.cpp | 52 +++++-------------- .../src/MainWindow/DSFE_MainWindow.cpp | 4 +- DSFE_App/DSFE_GUI/src/Scene/Camera.cpp | 6 +-- DSFE_App/DSFE_GUI/src/Scene/Object.cpp | 24 +-------- .../DSFE_GUI/src/Scene/SimulationManager.cpp | 23 +------- 10 files changed, 44 insertions(+), 140 deletions(-) diff --git a/DSFE_App/DSFE_GUI/include/Scene/Camera.h b/DSFE_App/DSFE_GUI/include/Scene/Camera.h index e45acbb1..b9b05154 100644 --- a/DSFE_App/DSFE_GUI/include/Scene/Camera.h +++ b/DSFE_App/DSFE_GUI/include/Scene/Camera.h @@ -4,9 +4,7 @@ #include #include #include - #include "Scene/Element.h" - #include "Platform/Logger.h" namespace scene { @@ -57,35 +55,7 @@ namespace scene { updateViewMatrix(); } - void onMouseWheel(double delta) { - // Negative delta = scroll forward = zoom in (decrease distance) - setDistance((float)(-delta * 0.5f)); - } - - //void onMouseMove(double x, double y) { - // glm::vec2 pos2d{ x, y }; - - // if () { - // glm::vec2 delta = (pos2d - _currentPos2D) * 0.004f; - - // float sign = getUp().y < 0 ? -1.0f : 1.0f; - - // _yaw += sign * delta.x * _rotationSpeed; - // _pitch += delta.y * _rotationSpeed; - - // updateViewMatrix(); - // } - // else if (false) { - // glm::vec2 delta = (pos2d - _currentPos2D) * 0.003f; - - // _focus += -getRight() * delta.x * _distance; - // _focus += getUp() * delta.y * _distance; - - // updateViewMatrix(); - // } - - // _currentPos2D = pos2d; - //} + void onMouseWheel(double delta) { setDistance((float)(-delta * 0.5f)); } void startFollow(const glm::vec3& pos, const glm::quat& rot, const glm::vec3& offset); void clearFollow() { _following = false; } @@ -95,7 +65,6 @@ namespace scene { } void updateViewMatrix(); - void fall(); void moveForward(float delta); diff --git a/DSFE_App/DSFE_GUI/include/Scene/Manager/SimImplementation.h b/DSFE_App/DSFE_GUI/include/Scene/Manager/SimImplementation.h index b0f7bed2..2bd91cdb 100644 --- a/DSFE_App/DSFE_GUI/include/Scene/Manager/SimImplementation.h +++ b/DSFE_App/DSFE_GUI/include/Scene/Manager/SimImplementation.h @@ -109,7 +109,6 @@ namespace gui { // Robot System std::unique_ptr _robotSystem; // simulation - // Robot Rendering std::unique_ptr _robotRenderer; // rendering RobotRenderBinding _currentBinding; // current render binding @@ -292,31 +291,40 @@ namespace gui { void buildRobotPresentationFromModel(const robots::RobotModel& model, SimManager& owner) { clearRobotPresentation(); - RobotPresentationBuilder builder; RobotRenderBinding binding = builder.build(model); - - for (auto& owned : binding.ownedObjects) { - _objects.push_back(std::move(owned)); - } - + for (auto& owned : binding.ownedObjects) { _objects.push_back(std::move(owned)); } _robotRenderer->bind(binding); - for (const auto& [linkName, visuals] : binding.linkVisuals) { auto& target = _linkToObjects[linkName]; - for (auto* obj : visuals) { if (!obj) { continue; } - target.push_back(obj); - - if (!_primaryLinkObject.contains(linkName)) { - _primaryLinkObject[linkName] = obj; - } + if (!_primaryLinkObject.contains(linkName)) { _primaryLinkObject[linkName] = obj; } } } } + //void clearObjectPresentation() { _objects.clear(); } + //void buildObjectPresentation(const std::vector>& meshes) { + // clearObjectPresentation(); + // presentation::MeshPresentationBuilder builder; + // presentation::MeshRenderBinding binding = builder.build(meshes); + // for (auto& owned : binding.owndObjs) { + // owned->state.q = mathlib::Quat(1.0, 0.0, 0.0, 0.0); + // owned->state.linearVelocity = mathlib::Vec3::Zero(); + // owned->state.angularVelocity = mathlib::Vec3::Zero(); + // owned->state.forces = mathlib::Vec3::Zero(); + // owned->state.torques = mathlib::Vec3::Zero(); + // owned->state.mass = 1.0; + // owned->state.damping = 0.0; + // owned->state.inertia = mathlib::Mat3::Identity(); + // owned->visible = true; + // owned->internal = false; + // _objects.push_back(std::move(owned)); + // } + //} + // Random number generation for SSAO kernel and noise std::mt19937 _rng{ std::random_device{}() }; std::uniform_real_distribution _uni{ -1.0f, 1.0f }; diff --git a/DSFE_App/DSFE_GUI/include/Scene/Mesh.h b/DSFE_App/DSFE_GUI/include/Scene/Mesh.h index 11b1831e..57251122 100644 --- a/DSFE_App/DSFE_GUI/include/Scene/Mesh.h +++ b/DSFE_App/DSFE_GUI/include/Scene/Mesh.h @@ -114,7 +114,7 @@ namespace scene { std::unique_ptr _rndrBffrMngr; int id = 0; - std::string _name = "obj" + id; + std::string _name = "obj_" + id; // Default material properties glm::vec3 _albedo = glm::vec3(0.4, 0.4, 0.4); diff --git a/DSFE_App/DSFE_GUI/include/Scene/Object.h b/DSFE_App/DSFE_GUI/include/Scene/Object.h index 3fb3966c..e2172bc7 100644 --- a/DSFE_App/DSFE_GUI/include/Scene/Object.h +++ b/DSFE_App/DSFE_GUI/include/Scene/Object.h @@ -15,8 +15,6 @@ #include "EngineLib/LogMacros.h" namespace scene { - enum class eInputButton; - class Input; class Mesh; enum class ObjectCategory { General, RobotLink }; @@ -125,7 +123,6 @@ namespace scene { } void onMouseWheel(double delta) { _distance += (float)delta * 0.5f; } - void onMouseMove(double x, double y, eInputButton button); void setLastMousePos(const glm::vec2& pos) { _lastMousePos = pos; } glm::vec2 getLastMousePos() const { return _lastMousePos; } diff --git a/DSFE_App/DSFE_GUI/include/Scene/SimulationManager.h b/DSFE_App/DSFE_GUI/include/Scene/SimulationManager.h index f433a54b..4da0d6cf 100644 --- a/DSFE_App/DSFE_GUI/include/Scene/SimulationManager.h +++ b/DSFE_App/DSFE_GUI/include/Scene/SimulationManager.h @@ -274,7 +274,6 @@ namespace gui { void processMovementKey(int key, float delta); void handleContinuousMovement(const std::unordered_set& pressedKeys, float dt); void handleMouseLook(double xpos, double ypos, bool mouseCaptured); - void onMouseMove(double x, double y); void onMouseWheel(double delta); void resetMouseDelta(); diff --git a/DSFE_App/DSFE_GUI/src/Assets/MeshLoader.cpp b/DSFE_App/DSFE_GUI/src/Assets/MeshLoader.cpp index 7187af35..d321f4c6 100644 --- a/DSFE_App/DSFE_GUI/src/Assets/MeshLoader.cpp +++ b/DSFE_App/DSFE_GUI/src/Assets/MeshLoader.cpp @@ -13,7 +13,6 @@ namespace assets { // Load a mesh from the specified file path and return a vector of shared pointers to Mesh objects std::vector> MeshLoader::load(const std::string& filepath) { _imported.clear(); - const uint32_t importFlags = aiProcess_Triangulate | aiProcess_JoinIdenticalVertices | @@ -30,10 +29,8 @@ namespace assets { LOG_ERROR("Failed to load mesh from %s: %s", filepath.c_str(), importer.GetErrorString()); return {}; } - glm::mat4 rootTransform(1.0f); processNode(scene->mRootNode, scene, rootTransform); - return _imported; } @@ -48,22 +45,18 @@ namespace assets { a.a3, a.b3, a.c3, a.d3, a.a4, a.b4, a.c4, a.d4 ); - glm::mat4 globalTransform = parentTransform * nodeTransform; // Process meshes in this node - for (unsigned int i = 0; i < node->mNumMeshes; i++) - { + for (unsigned int i = 0; i < node->mNumMeshes; i++) { aiMesh* mesh = scene->mMeshes[node->mMeshes[i]]; - auto m = processMesh(mesh, scene); + auto m = processMesh(mesh, scene); m->localTransform = globalTransform; _imported.push_back(std::move(m)); } // Recursively process child nodes - for (unsigned int i = 0; i < node->mNumChildren; i++) { - processNode(node->mChildren[i], scene, globalTransform); - } + for (unsigned int i = 0; i < node->mNumChildren; i++) { processNode(node->mChildren[i], scene, globalTransform); } } // Helper method to process an Assimp mesh and convert it into a shared pointer to a scene::Mesh object @@ -79,21 +72,15 @@ namespace assets { ? glm::vec3(mesh->mNormals[i].x, mesh->mNormals[i].y, mesh->mNormals[i].z) : glm::vec3(0.0f, 1.0f, 0.0f); - if (mesh->mTextureCoords[0]) { - vh._texCoord = glm::vec2(mesh->mTextureCoords[0][i].x, - mesh->mTextureCoords[0][i].y); - } + if (mesh->mTextureCoords[0]) { vh._texCoord = glm::vec2(mesh->mTextureCoords[0][i].x, mesh->mTextureCoords[0][i].y); } else { vh._texCoord = glm::vec2(0.0f); } - result->addVertex(vh); } // Get the indices for the faces of the mesh and add them to the result mesh, applying the index offset to account for previously added vertices for (unsigned int i = 0; i < mesh->mNumFaces; ++i) { const aiFace& face = mesh->mFaces[i]; - for (unsigned int j = 0; j < face.mNumIndices; ++j) { - result->addVertexIndex(face.mIndices[j] + indexOffset); - } + for (unsigned int j = 0; j < face.mNumIndices; ++j) { result->addVertexIndex(face.mIndices[j] + indexOffset); } } // Extract material properties from Assimp @@ -102,42 +89,31 @@ namespace assets { // Albedo / diffuse colour aiColor4D diffuse(0.4f, 0.4f, 0.4f, 1.0f); - if (mat->Get(AI_MATKEY_COLOR_DIFFUSE, diffuse) == AI_SUCCESS || - mat->Get(AI_MATKEY_BASE_COLOR, diffuse) == AI_SUCCESS) { + if (mat->Get(AI_MATKEY_COLOR_DIFFUSE, diffuse) == AI_SUCCESS || mat->Get(AI_MATKEY_BASE_COLOR, diffuse) == AI_SUCCESS) { result->setAlbedo(glm::vec3(diffuse.r, diffuse.g, diffuse.b)); } // Metallic (GLTF/PBR key, fallback to reflectivity) float metallic = 0.0f; - if (mat->Get(AI_MATKEY_METALLIC_FACTOR, metallic) == AI_SUCCESS) { - result->setMetallic(metallic); - } else { + if (mat->Get(AI_MATKEY_METALLIC_FACTOR, metallic) == AI_SUCCESS) { result->setMetallic(metallic); } else { float reflectivity = 0.0f; - if (mat->Get(AI_MATKEY_REFLECTIVITY, reflectivity) == AI_SUCCESS) { - result->setMetallic(glm::clamp(reflectivity, 0.0f, 1.0f)); - } + if (mat->Get(AI_MATKEY_REFLECTIVITY, reflectivity) == AI_SUCCESS) { result->setMetallic(glm::clamp(reflectivity, 0.0f, 1.0f)); } } // Roughness (GLTF/PBR key, fallback from shininess) float roughness = 0.5f; - if (mat->Get(AI_MATKEY_ROUGHNESS_FACTOR, roughness) == AI_SUCCESS) { - result->setRoughness(roughness); - } else { + if (mat->Get(AI_MATKEY_ROUGHNESS_FACTOR, roughness) == AI_SUCCESS) { result->setRoughness(roughness); } + else { float shininess = 0.0f; - if (mat->Get(AI_MATKEY_SHININESS, shininess) == AI_SUCCESS && shininess > 0.0f) { - // Convert Phong shininess to roughness (approximate) - result->setRoughness(glm::clamp(1.0f - glm::sqrt(shininess / 1000.0f), 0.05f, 1.0f)); + if (mat->Get(AI_MATKEY_SHININESS, shininess) == AI_SUCCESS && shininess > 0.0f) { + result->setRoughness(glm::clamp(1.0f - glm::sqrt(shininess / 1000.0f), 0.05f, 1.0f)); // Convert Phong shininess to roughness (approximate) } } } // Set mesh name from Assimp - if (mesh->mName.length > 0) { - result->setName(std::string(mesh->mName.C_Str())); - } - - // Update the index offset for the next mesh - result->init(); + if (mesh->mName.length > 0) { result->setName(std::string(mesh->mName.C_Str())); } + result->init(); // Initialise the mesh (create GPU buffers, etc.) return result; } } \ No newline at end of file diff --git a/DSFE_App/DSFE_GUI/src/MainWindow/DSFE_MainWindow.cpp b/DSFE_App/DSFE_GUI/src/MainWindow/DSFE_MainWindow.cpp index af18e8ef..4efb9e31 100644 --- a/DSFE_App/DSFE_GUI/src/MainWindow/DSFE_MainWindow.cpp +++ b/DSFE_App/DSFE_GUI/src/MainWindow/DSFE_MainWindow.cpp @@ -58,7 +58,7 @@ namespace window { LOG_INFO("Menu clicked: File -> Open -> Project"); }); openMenu->addSeparator(); - auto* openScriptAction = openMenu->addAction("Script (*.dsl *.txt)"); + auto* openScriptAction = openMenu->addAction("Script"); connect(openScriptAction, &QAction::triggered, this, [this]() { QString fileName = QFileDialog::getOpenFileName(nullptr, "Open Script", QString::fromStdString((paths::assets() / "DSLScripts").string()), "DSL Script Files (*.dsl);;Text Files (*.txt)"); if (fileName.isEmpty()) { return; } @@ -125,7 +125,7 @@ namespace window { auto* loadMeshAction = projectMenu->addAction("Load Mesh"); connect(loadMeshAction, &QAction::triggered, this, [this]() { LOG_INFO("Menu clicked: Project -> Load Mesh"); - QString path = QFileDialog::getOpenFileName(nullptr, "Select Mesh File", QString::fromStdString((paths::assets() / "objects" / "Shapes").string()), "Mesh Files (*.obj *.fbx *.gltf *.dae *.stl"); + QString path = QFileDialog::getOpenFileName(nullptr, "Select Mesh File", QString::fromStdString((paths::assets() / "objects" / "Shapes").string()), "Mesh Files(*.obj * .fbx * .gltf * .dae * .stl)"); if (path.isEmpty()) { return; } _sim->loadMesh(path.toStdString()); }); diff --git a/DSFE_App/DSFE_GUI/src/Scene/Camera.cpp b/DSFE_App/DSFE_GUI/src/Scene/Camera.cpp index 5ec66667..dffa27a9 100644 --- a/DSFE_App/DSFE_GUI/src/Scene/Camera.cpp +++ b/DSFE_App/DSFE_GUI/src/Scene/Camera.cpp @@ -6,9 +6,7 @@ #endif #include #include "Scene/Camera.h" - #include "Platform/KeyCode.h" - #include "EngineLib/LogMacros.h" namespace scene { @@ -187,8 +185,8 @@ namespace scene { if (len < 1e-6f) return; dir /= len; - // Match your yaw/pitch convention: - // Your default: _yaw = -pi/2 gives forward (0,0,-1). + // Match yaw/pitch convention: + // Default: _yaw = -pi/2 gives forward (0,0,-1). // That corresponds to: // forward.x = cos(yaw)*cos(pitch) // forward.y = sin(pitch) diff --git a/DSFE_App/DSFE_GUI/src/Scene/Object.cpp b/DSFE_App/DSFE_GUI/src/Scene/Object.cpp index e2e78680..db849f79 100644 --- a/DSFE_App/DSFE_GUI/src/Scene/Object.cpp +++ b/DSFE_App/DSFE_GUI/src/Scene/Object.cpp @@ -25,7 +25,7 @@ namespace scene { // --- Object Implementation --- - // Constructor initializes transform and physics state + // Constructor initialises transform and physics state Object::Object(std::shared_ptr mesh) : _mesh(std::move(mesh)) { transform.position = glm::vec3(0.0f); transform.rotQ = glm::quat{ 1.0f, 0.0f, 0.0f, 0.0f }; @@ -43,26 +43,4 @@ namespace scene { // Update method passes material properties to the shader void Object::update(shaders::Shader* shader) { if (_mesh) _mesh->update(shader); } - - // Handle mouse movement for object manipulation **DECREPATED** - void Object::onMouseMove(double x, double y, eInputButton button) { - glm::vec2 pos2d{ x, y }; - glm::vec2 delta = pos2d - _lastMousePos; - _lastMousePos = pos2d; - - //if (button == eInputButton::Right) { - // delta *= 0.004f; - // float yaw = delta.x; - // float pitch = -delta.y; - - // glm::quat qYaw = glm::angleAxis(yaw, glm::vec3(0.0f, 1.0f, 0.0f)); - // glm::vec3 right = transform.rotQ * glm::vec3(1.0f, 0.0f, 0.0f); - // glm::quat qPitch = glm::angleAxis(pitch, glm::normalize(right)); - - //} - //else if (button == eInputButton::Left) { - // delta *= 0.003f; - // transform.position += glm::vec3(delta.x * _distance, -delta.y * _distance, 0.0f); - //} - } } diff --git a/DSFE_App/DSFE_GUI/src/Scene/SimulationManager.cpp b/DSFE_App/DSFE_GUI/src/Scene/SimulationManager.cpp index 9487201d..281ee456 100644 --- a/DSFE_App/DSFE_GUI/src/Scene/SimulationManager.cpp +++ b/DSFE_App/DSFE_GUI/src/Scene/SimulationManager.cpp @@ -201,11 +201,9 @@ namespace gui { const glm::mat4& world = toGlm(T[i]); for (scene::Object* obj : it->second) { - if (!obj) continue; - + if (!obj) { continue; } glm::vec3 pos = glm::vec3(world[3]); glm::quat q = glm::quat_cast(world); - obj->transform.position = pos; obj->transform.rotQ = q; } @@ -257,7 +255,6 @@ namespace gui { } // Handle mouse look (camera rotation) based on mouse movement. **OLD LOGIC FOR IMGUI AND GLFW** -#pragma region DecrepatedInputLogic void gui::SimManager::handleMouseLook(double xpos, double ypos, bool mouseCaptured) { if (_impl->viewMode == Impl::ViewMode::Quad) { return; } // No mouse look in quad view scene::Camera* cam = _impl->_views[static_cast(_impl->activeView)].cam.get(); @@ -279,23 +276,6 @@ namespace gui { if (ctrlMode == ControlMode::Camera) { cam->processMouseMovement(static_cast(xoffset), static_cast(yoffset)); } else if (ctrlMode == ControlMode::Object && _impl->_selectedObject) { return; /*_impl->_selectedObject->onMouseMove(xpos, ypos, scene::eInputButton::Right);*/ } } - void SimManager::onMouseMove(double x, double y) { - scene::Camera* cam = _impl->_views[static_cast(_impl->activeView)].cam.get(); - glm::vec2 pos2d{ x, y }; - glm::vec2 delta = pos2d - _lastMousePos; - _lastMousePos = pos2d; - - if (_impl->viewMode == Impl::ViewMode::Quad) { return; } // No mouse drag in quad view - - if (!_isHovered) { - cam->setCurrentPos2D(pos2d); - _impl->_selectedObject->setLastMousePos(pos2d); - return; - } - - //if (ctrlMode == ControlMode::Camera) { cam->onMouseMove(x, y, button); } - //else if (ctrlMode == ControlMode::Object && _impl->_selectedObject) { _impl->_selectedObject->onMouseMove(x, y, button); } - } void SimManager::onMouseWheel(double delta) { scene::Camera* cam = _impl->_views[static_cast(_impl->activeView)].cam.get(); auto* obj = _impl->_selectedObject; @@ -311,5 +291,4 @@ namespace gui { } void gui::SimManager::resetMouseDelta() { _firstMouse = true; } -#pragma endregion } \ No newline at end of file From 1533216567ebdc21dd2a5e80c02346ce24343d93 Mon Sep 17 00:00:00 2001 From: saltyjoss Date: Sat, 13 Jun 2026 12:00:01 +0100 Subject: [PATCH 147/210] fixes: Updated `buildRobotMenu` to directly call the core `loadRobot()` rather than the internal simMananger version, as it eliminates crashes from uninitialise GL pointers --- DSFE_App/DSFE_GUI/src/MainWindow/DSFE_MainWindow.cpp | 3 ++- 1 file changed, 2 insertions(+), 1 deletion(-) diff --git a/DSFE_App/DSFE_GUI/src/MainWindow/DSFE_MainWindow.cpp b/DSFE_App/DSFE_GUI/src/MainWindow/DSFE_MainWindow.cpp index 4efb9e31..afd39d0a 100644 --- a/DSFE_App/DSFE_GUI/src/MainWindow/DSFE_MainWindow.cpp +++ b/DSFE_App/DSFE_GUI/src/MainWindow/DSFE_MainWindow.cpp @@ -1,6 +1,7 @@ //DSFE_GUI DSFE_MainWindow.cpp #include "MainWindow/DSFE_MainWindow.h" #include "Scene/SimulationManager.h" +#include "Scene/SimulationCore.h" #include "Workspace/ProjectPage.h" #include "Widgets/DSLEditorWidget.h" @@ -322,7 +323,7 @@ namespace window { QAction* robotAction = familyMenus[family]->addAction(robotName); connect(robotAction, &QAction::triggered, this, [this, robotName]() { LOG_INFO("Menu clicked: Project -> Load Robot -> %s", robotName.toStdString().c_str()); - _sim->loadRobot(robotName.toStdString()); + _sim->simCore()->loadRobot(robotName.toStdString()); }); } } From 45ce847387e9ae1779eb6f0549f54de1925214f1 Mon Sep 17 00:00:00 2001 From: saltyjoss Date: Sat, 13 Jun 2026 12:18:08 +0100 Subject: [PATCH 148/210] feat: Introduced new internal library in PxM, `PhysLib` * This will see some more work now, potentially moving over some of the "Robot" -> to be named `Multibody or something less specific but equally appropriate (or moving it into a multibody sub dir)" -> system methods --- PxMLib/CMakeLists.txt | 1 + PxMLib/MathLib/framework.h | 2 +- PxMLib/MathLib/pch.h | 1 - PxMLib/PhysLib/CMakeLists.txt | 22 ++++++++++++++++++++++ PxMLib/PhysLib/dllmain.cpp | 19 +++++++++++++++++++ PxMLib/PhysLib/framework.h | 6 ++++++ PxMLib/PhysLib/pch.cpp | 2 ++ PxMLib/PhysLib/pch.h | 24 ++++++++++++++++++++++++ 8 files changed, 75 insertions(+), 2 deletions(-) create mode 100644 PxMLib/PhysLib/CMakeLists.txt create mode 100644 PxMLib/PhysLib/dllmain.cpp create mode 100644 PxMLib/PhysLib/framework.h create mode 100644 PxMLib/PhysLib/pch.cpp create mode 100644 PxMLib/PhysLib/pch.h diff --git a/PxMLib/CMakeLists.txt b/PxMLib/CMakeLists.txt index 988b2907..3f67732c 100644 --- a/PxMLib/CMakeLists.txt +++ b/PxMLib/CMakeLists.txt @@ -4,4 +4,5 @@ project(PxMLib LANGUAGES CXX) # Add projects within the DSFE_App directory add_subdirectory(MathLib) +add_subdirectory(PhysLib)` add_subdirectory(MathLib_Unit) diff --git a/PxMLib/MathLib/framework.h b/PxMLib/MathLib/framework.h index dca3ebf1..a2c75c3c 100644 --- a/PxMLib/MathLib/framework.h +++ b/PxMLib/MathLib/framework.h @@ -1,6 +1,6 @@ #pragma once -#define WIN32_LEAN_AND_MEAN // Exclude rarely-used stuff from Windows headers +#define WIN32_LEAN_AND_MEAN // Exclude rarely-used stuff from Windows headers // Windows Header Files #define NOMINMAX #include diff --git a/PxMLib/MathLib/pch.h b/PxMLib/MathLib/pch.h index 49d41c41..35608d93 100644 --- a/PxMLib/MathLib/pch.h +++ b/PxMLib/MathLib/pch.h @@ -7,7 +7,6 @@ #ifndef PCH_H #define PCH_H -// add headers that you want to pre-compile here #include "framework.h" // Standard Libraries diff --git a/PxMLib/PhysLib/CMakeLists.txt b/PxMLib/PhysLib/CMakeLists.txt new file mode 100644 index 00000000..e3231b64 --- /dev/null +++ b/PxMLib/PhysLib/CMakeLists.txt @@ -0,0 +1,22 @@ +# CMakeLists.txt for PhysLib +cmake_minimum_required(VERSION 3.17) +project(PhysLib LANGUAGES CXX) + +add_library(PhysLib STATIC + ${CMAKE_CURRENT_SOURCE_DIR}/pch.cpp +) + +target_precompile_headers(PhysLib PRIVATE + ${CMAKE_CURRENT_SOURCE_DIR}/pch.h +) + +target_include_directories(PhysLib + PUBLIC + $ + PRIVATE + ${CMAKE_CURRENT_SOURCE_DIR}/ +) + +target_link_libraries(PhysLib PUBLIC + Eigen3::Eigen +) \ No newline at end of file diff --git a/PxMLib/PhysLib/dllmain.cpp b/PxMLib/PhysLib/dllmain.cpp new file mode 100644 index 00000000..f2665971 --- /dev/null +++ b/PxMLib/PhysLib/dllmain.cpp @@ -0,0 +1,19 @@ +// dllmain.cpp : Defines the entry point for the DLL application. +#include "pch.h" + +BOOL APIENTRY DllMain( HMODULE hModule, + DWORD ul_reason_for_call, + LPVOID lpReserved + ) +{ + switch (ul_reason_for_call) + { + case DLL_PROCESS_ATTACH: + case DLL_THREAD_ATTACH: + case DLL_THREAD_DETACH: + case DLL_PROCESS_DETACH: + break; + } + return TRUE; +} + diff --git a/PxMLib/PhysLib/framework.h b/PxMLib/PhysLib/framework.h new file mode 100644 index 00000000..a2c75c3c --- /dev/null +++ b/PxMLib/PhysLib/framework.h @@ -0,0 +1,6 @@ +#pragma once + +#define WIN32_LEAN_AND_MEAN // Exclude rarely-used stuff from Windows headers +// Windows Header Files +#define NOMINMAX +#include diff --git a/PxMLib/PhysLib/pch.cpp b/PxMLib/PhysLib/pch.cpp new file mode 100644 index 00000000..c94d6873 --- /dev/null +++ b/PxMLib/PhysLib/pch.cpp @@ -0,0 +1,2 @@ +// pch.cpp: source file corresponding to the pre-compiled header +#include "pch.h" \ No newline at end of file diff --git a/PxMLib/PhysLib/pch.h b/PxMLib/PhysLib/pch.h new file mode 100644 index 00000000..35608d93 --- /dev/null +++ b/PxMLib/PhysLib/pch.h @@ -0,0 +1,24 @@ +// pch.h: This is a precompiled header file. +// Files listed below are compiled only once, improving build performance for future builds. +// This also affects IntelliSense performance, including code completion and many code browsing features. +// However, files listed here are ALL re-compiled if any one of them is updated between builds. +// Do not add files here that you will be updating frequently as this negates the performance advantage. + +#ifndef PCH_H +#define PCH_H + +#include "framework.h" + +// Standard Libraries +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + +#endif //PCH_H From c41e1064220b95ff39b40992c19db9d11970f2f2 Mon Sep 17 00:00:00 2001 From: saltyjoss Date: Sat, 13 Jun 2026 12:18:54 +0100 Subject: [PATCH 149/210] feat: Initialised new `SingleBodySystem` method to allow PROPER modelling on single bodies * This is in preparation for something, but did not want to reuse the old logic from imgui days. --- DSFE_App/DSFE_Core/CMakeLists.txt | 38 ++++++++++++------- .../SingleBodySystem/SingleBodySystem.h | 38 +++++++++++++++++++ .../src/SingleBodySystem/SingleBodySystem.cpp | 1 + 3 files changed, 63 insertions(+), 14 deletions(-) create mode 100644 DSFE_App/DSFE_Core/include/SingleBodySystem/SingleBodySystem.h create mode 100644 DSFE_App/DSFE_Core/src/SingleBodySystem/SingleBodySystem.cpp diff --git a/DSFE_App/DSFE_Core/CMakeLists.txt b/DSFE_App/DSFE_Core/CMakeLists.txt index 8cf99bf2..bec3de84 100644 --- a/DSFE_App/DSFE_Core/CMakeLists.txt +++ b/DSFE_App/DSFE_Core/CMakeLists.txt @@ -12,18 +12,34 @@ if (MSVC) target_compile_options(DSFE_Core PRIVATE /bigobj) endif() -set(CORE_SRC +set(DEP_CORE_SRC src/EngineCore.cpp - src/Numerics/IntegrationService.cpp +) + +set(SINGLE_BODY_SYS_SRC + src/SingleBodySystem/SingleBodySystem.cpp +) +set(MULTI_BODY_SYS_SRC src/Robots/RobotDynamics.cpp src/Robots/RobotKinematics.cpp src/Robots/RobotLoader.cpp src/Robots/RobotSystem.cpp src/Robots/TrajectoryManager.cpp src/Robots/RobotSimSnapshot.cpp +) + +set(PLATFORM_SRC + src/Platform/DataManager.cpp + src/Platform/Logger.cpp + src/Platform/Paths.cpp + src/Platform/StudyRunner.cpp + src/Analysis/Telemetry.cpp +) + +set(DSL_SRC src/Interpreter/Command.cpp src/Interpreter/CommandContext.cpp src/Interpreter/CommandFactory.cpp @@ -50,22 +66,16 @@ set(CORE_SRC src/Interpreter/Commands/WaitCmd.cpp ) -set(PLATFORM_SRC - src/Platform/DataManager.cpp - src/Platform/Logger.cpp - src/Platform/Paths.cpp - src/Platform/StudyRunner.cpp - - src/Analysis/Telemetry.cpp -) - -set(SCENE_SRC src/Scene/SimulationCore.cpp) +set(SIM_CORE_SRC src/Scene/SimulationCore.cpp) # Add sources for DSFE_Core, including ImGui files target_sources(DSFE_Core PRIVATE - ${CORE_SRC} + ${DEP_CORE_SRC} + ${SINGLE_BODY_SYS_SRC} + ${MULTI_BODY_SYS_SRC} ${PLATFORM_SRC} - ${SCENE_SRC} + ${DSL_SRC} + ${SIM_CORE_SRC} ) # Add Windows-specific source files diff --git a/DSFE_App/DSFE_Core/include/SingleBodySystem/SingleBodySystem.h b/DSFE_App/DSFE_Core/include/SingleBodySystem/SingleBodySystem.h new file mode 100644 index 00000000..1b458a5e --- /dev/null +++ b/DSFE_App/DSFE_Core/include/SingleBodySystem/SingleBodySystem.h @@ -0,0 +1,38 @@ +// DSFE_CORE SingleBodySystem.h +#pragma once + +#include "EngineCore.h" +#include +#include "Numerics/IntegrationService.h" + +namespace single_body_system { + class SingleBodySystem { + public: + SingleBodySystem() = default; + ~SingleBodySystem() = default; + + + + void step(double dt, double t); + + + integration::eIntegrationMethod getIntegrationMethod() const { return _curIntMethod; } + std::string getIntegratorName() const { return _integrator->IntegratorName(_curIntMethod); } + void setStandardIntegrator(integration::eIntegrationMethod m) { _curIntMethod = m; } + + integration::eAutoDiffIntegrationMethod AD_IntegrationMethod() const { return _curIntMethod_AD; } + std::string AD_integratorName() const { return _AD_integrator->IntegratorName(_curIntMethod_AD); } + void setADIntegrator(integration::eAutoDiffIntegrationMethod m) { _curIntMethod_AD = m; } + + private: + double _mass; + + /*std::unique_ptr _dynamics;*/ + + std::unique_ptr _integrator; + integration::eIntegrationMethod _curIntMethod{}; + + std::unique_ptr _AD_integrator; + integration::eAutoDiffIntegrationMethod _curIntMethod_AD{}; + }; +} \ No newline at end of file diff --git a/DSFE_App/DSFE_Core/src/SingleBodySystem/SingleBodySystem.cpp b/DSFE_App/DSFE_Core/src/SingleBodySystem/SingleBodySystem.cpp new file mode 100644 index 00000000..218a7124 --- /dev/null +++ b/DSFE_App/DSFE_Core/src/SingleBodySystem/SingleBodySystem.cpp @@ -0,0 +1 @@ +// DSFE_CORE SingleBodySystem.cpp \ No newline at end of file From 8c7334943d21877e8450fcf5d01ac88770e88aad Mon Sep 17 00:00:00 2001 From: saltyjoss Date: Sat, 13 Jun 2026 12:19:23 +0100 Subject: [PATCH 150/210] fixes: Removed accidental ` "`" ` --- PxMLib/CMakeLists.txt | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/PxMLib/CMakeLists.txt b/PxMLib/CMakeLists.txt index 3f67732c..b2857283 100644 --- a/PxMLib/CMakeLists.txt +++ b/PxMLib/CMakeLists.txt @@ -4,5 +4,5 @@ project(PxMLib LANGUAGES CXX) # Add projects within the DSFE_App directory add_subdirectory(MathLib) -add_subdirectory(PhysLib)` +add_subdirectory(PhysLib) add_subdirectory(MathLib_Unit) From dd96bf26253040dfc64d8abb356a849357e6b69b Mon Sep 17 00:00:00 2001 From: saltyjoss Date: Sat, 13 Jun 2026 13:14:54 +0100 Subject: [PATCH 151/210] chore: Code cleanup --- DSFE_App/DSFE_Core/include/Robots/RobotModel.h | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/DSFE_App/DSFE_Core/include/Robots/RobotModel.h b/DSFE_App/DSFE_Core/include/Robots/RobotModel.h index a3b6c4b7..3da80a1e 100644 --- a/DSFE_App/DSFE_Core/include/Robots/RobotModel.h +++ b/DSFE_App/DSFE_Core/include/Robots/RobotModel.h @@ -46,7 +46,7 @@ namespace robots { // Inertial properties of a link struct Inertial { - double mass = 0.0f; + double mass = 0.0; mathlib::Vec3 com_xyz{ 0.0,0.0,0.0 }; Inertia inertia{}; }; From d7287285ae406ff885c84fa4d90c122e68a1385e Mon Sep 17 00:00:00 2001 From: saltyjoss Date: Sat, 13 Jun 2026 13:15:25 +0100 Subject: [PATCH 152/210] feat: Added `SingleBodySystem` struct into PhysLib --- PxMLib/CMakeLists.txt | 2 +- PxMLib/PhysLib/CMakeLists.txt | 1 + .../SingleBodySystems/SingleBodySystem.h | 41 +++++++++++++++++++ PxMLib/PhysLib/pch.h | 2 + 4 files changed, 45 insertions(+), 1 deletion(-) create mode 100644 PxMLib/PhysLib/Include/SingleBodySystems/SingleBodySystem.h diff --git a/PxMLib/CMakeLists.txt b/PxMLib/CMakeLists.txt index b2857283..43f5cd6a 100644 --- a/PxMLib/CMakeLists.txt +++ b/PxMLib/CMakeLists.txt @@ -4,5 +4,5 @@ project(PxMLib LANGUAGES CXX) # Add projects within the DSFE_App directory add_subdirectory(MathLib) -add_subdirectory(PhysLib) add_subdirectory(MathLib_Unit) +add_subdirectory(PhysLib) diff --git a/PxMLib/PhysLib/CMakeLists.txt b/PxMLib/PhysLib/CMakeLists.txt index e3231b64..0bb22ebc 100644 --- a/PxMLib/PhysLib/CMakeLists.txt +++ b/PxMLib/PhysLib/CMakeLists.txt @@ -19,4 +19,5 @@ target_include_directories(PhysLib target_link_libraries(PhysLib PUBLIC Eigen3::Eigen + MathLib ) \ No newline at end of file diff --git a/PxMLib/PhysLib/Include/SingleBodySystems/SingleBodySystem.h b/PxMLib/PhysLib/Include/SingleBodySystems/SingleBodySystem.h new file mode 100644 index 00000000..b17dd5da --- /dev/null +++ b/PxMLib/PhysLib/Include/SingleBodySystems/SingleBodySystem.h @@ -0,0 +1,41 @@ +// PxM/PhysLib SingleBodySystems/SingleBodySystem.h +#pragma once + +#include +#include + +using namespace constants; + +namespace single_body_system { + // Inertia struct representing the mass and inertia tensor of a rigid body + struct BodyInertia { + double mass = 0.0; + mathlib::Vec3 com_xyz{ 0.0, 0.0, 0.0 }; // Center of mass position in the body frame + mathlib::Mat3 inertiaTensor; // 3x3 inertia tensor matrix + }; + + struct DynamicsState { + mathlib::Vec3 x; // Position of the body in 3D space + mathlib::Vec3 xd; // Velocity of the body in 3D space + mathlib::Vec3 xdd; // Acceleration of the body in 3D space + mathlib::Quat q; // Orientation of the body (quaternion) + mathlib::Vec3 w; // Angular velocity of the body + mathlib::Vec3 wd; // Angular acceleration of the body + }; + + struct BodyLimits { + mathlib::Vec3 xdMin{ 0.0 }; // Minimum velocity limits (xd, yd, zd) + mathlib::Vec3 xdMax{ constants::c_0 }; // Maximum velocity limits (xd, yd, zd) + mathlib::Vec3 xddMin{ 0.0 }; // Minimum acceleration limits (xdd, ydd, zdd) + mathlib::Vec3 xddMax; // Maximum acceleration limits (xdd, ydd, zdd) + }; + + struct SingleBodySystem { + std::string name = "UnnamedBody"; + float scale = 1.0f; + + BodyInertia inertia; + DynamicsState state; + BodyLimits limits; + }; +} \ No newline at end of file diff --git a/PxMLib/PhysLib/pch.h b/PxMLib/PhysLib/pch.h index 35608d93..818dce77 100644 --- a/PxMLib/PhysLib/pch.h +++ b/PxMLib/PhysLib/pch.h @@ -21,4 +21,6 @@ #include #include +#include // All of PhysLib depends on MathLib, necessary here to avoid circular dependencies + #endif //PCH_H From 03ea214e44758c7ed5c8dc0be974be696d45a103 Mon Sep 17 00:00:00 2001 From: saltyjoss Date: Sat, 13 Jun 2026 13:18:48 +0100 Subject: [PATCH 153/210] feat: Added basic `SingleBodyDynamics` class with methods to `computeDynamics` and `computeDerivatives` * This is insanely rushed, and I have not checked my math, I want to basically see if I can get the object recognised first, then worry about physical correctness after * I doubt ill need to compute the derivatives to begin with but we shall see. --- .../Include/SingleBodySystems/Dynamics.h | 36 +++++++++++++++++++ 1 file changed, 36 insertions(+) create mode 100644 PxMLib/PhysLib/Include/SingleBodySystems/Dynamics.h diff --git a/PxMLib/PhysLib/Include/SingleBodySystems/Dynamics.h b/PxMLib/PhysLib/Include/SingleBodySystems/Dynamics.h new file mode 100644 index 00000000..0571a126 --- /dev/null +++ b/PxMLib/PhysLib/Include/SingleBodySystems/Dynamics.h @@ -0,0 +1,36 @@ +// PxM/PhysLib SingleBodySystems/Dynamics.h +#pragma once +#include "SingleBodySystem.h" + +namespace single_body_system::dynamics { + class SingleBodyDynamics { + public: + void computeDynamics(SingleBodySystem& body, mathlib::Vec3& externalForce, mathlib::Vec3& externalTorque, double dt) { + // Compute linear acceleration + body.state.xdd = (externalForce / body.inertia.mass).eval(); + // Compute angular acceleration + mathlib::Mat3 inertiaInv = body.inertia.inertiaTensor.inverse(); + body.state.wd = inertiaInv * (externalTorque - body.state.w.cross(body.inertia.inertiaTensor * body.state.w)); + // Update linear velocity and position + body.state.xd += body.state.xdd * dt; + body.state.x += body.state.xd * dt; + // Update angular velocity and orientation (using quaternion integration) + body.state.w += body.state.wd * dt; + mathlib::Quat dq = mathlib::Quat(0.0, body.state.w * dt); + body.state.q = (body.state.q.coeffs() + (0.5 * dq.coeffs() * body.state.q.coeffs())).normalized(); + } + + void derivatives(const SingleBodySystem& body, mathlib::Vec3& externalForce, mathlib::Vec3& externalTorque, mathlib::Mat3& dxdot_dx, mathlib::Mat3& dxdot_dxdot) { + // Compute the Jacobian of the dynamics with respect to state variables + dxdot_dx.setZero(); + dxdot_dxdot.setZero(); + // Linear acceleration derivatives + dxdot_dx.block<3, 3>(0, 0) = mathlib::Mat3::Zero(); // dx/dx = 0 + dxdot_dxdot.block<3, 3>(0, 3) = mathlib::Mat3::Identity() / body.inertia.mass; // dx/dxdot = 1/mass + // Angular acceleration derivatives + mathlib::Mat3 inertiaInv = body.inertia.inertiaTensor.inverse(); + dxdot_dx.block<3, 3>(3, 6) = -inertiaInv * skewSymmetric(body.state.w) * body.inertia.inertiaTensor; // dw/dq + dxdot_dxdot.block<3, 3>(3, 9) = inertiaInv; // dw/dwd = I^-1 + } + }; +} \ No newline at end of file From 3f6f42a78942ee058e9312cf68613f094df8dc2d Mon Sep 17 00:00:00 2001 From: saltyjoss Date: Sat, 13 Jun 2026 13:52:35 +0100 Subject: [PATCH 154/210] fixes: Updated SinlgeBody naming to `Body` --- DSFE_App/DSFE_Core/CMakeLists.txt | 8 ++++---- .../{SingleBodySystem.h => Body.h} | 18 ++++++++++-------- .../DSFE_Core/src/SingleBodySystem/Body.cpp | 16 ++++++++++++++++ .../src/SingleBodySystem/SingleBodySystem.cpp | 1 - .../Include/SingleBodySystems/Dynamics.h | 4 ++-- .../SingleBodySystems/SingleBodySystem.h | 2 +- 6 files changed, 33 insertions(+), 16 deletions(-) rename DSFE_App/DSFE_Core/include/SingleBodySystem/{SingleBodySystem.h => Body.h} (78%) create mode 100644 DSFE_App/DSFE_Core/src/SingleBodySystem/Body.cpp delete mode 100644 DSFE_App/DSFE_Core/src/SingleBodySystem/SingleBodySystem.cpp diff --git a/DSFE_App/DSFE_Core/CMakeLists.txt b/DSFE_App/DSFE_Core/CMakeLists.txt index bec3de84..d8f452b8 100644 --- a/DSFE_App/DSFE_Core/CMakeLists.txt +++ b/DSFE_App/DSFE_Core/CMakeLists.txt @@ -18,7 +18,7 @@ set(DEP_CORE_SRC ) set(SINGLE_BODY_SYS_SRC - src/SingleBodySystem/SingleBodySystem.cpp + src/SingleBodySystem/Body.cpp ) set(MULTI_BODY_SYS_SRC @@ -35,7 +35,6 @@ set(PLATFORM_SRC src/Platform/Logger.cpp src/Platform/Paths.cpp src/Platform/StudyRunner.cpp - src/Analysis/Telemetry.cpp ) @@ -96,9 +95,10 @@ target_precompile_headers(DSFE_Core PRIVATE "${CMAKE_CURRENT_SOURCE_DIR}/pch.h") # Link DSFE_Core against its dependencies target_link_libraries(DSFE_Core - PUBLIC - MathLib + PUBLIC Eigen3::Eigen + MathLib + PhysLib hdf5::hdf5-shared Threads::Threads diff --git a/DSFE_App/DSFE_Core/include/SingleBodySystem/SingleBodySystem.h b/DSFE_App/DSFE_Core/include/SingleBodySystem/Body.h similarity index 78% rename from DSFE_App/DSFE_Core/include/SingleBodySystem/SingleBodySystem.h rename to DSFE_App/DSFE_Core/include/SingleBodySystem/Body.h index 1b458a5e..9d8c3809 100644 --- a/DSFE_App/DSFE_Core/include/SingleBodySystem/SingleBodySystem.h +++ b/DSFE_App/DSFE_Core/include/SingleBodySystem/Body.h @@ -1,8 +1,9 @@ -// DSFE_CORE SingleBodySystem.h +// DSFE_CORE Body.h #pragma once #include "EngineCore.h" -#include +#include +#include "SingleBodySystems/Dynamics.h" #include "Numerics/IntegrationService.h" namespace single_body_system { @@ -11,11 +12,8 @@ namespace single_body_system { SingleBodySystem() = default; ~SingleBodySystem() = default; - - void step(double dt, double t); - integration::eIntegrationMethod getIntegrationMethod() const { return _curIntMethod; } std::string getIntegratorName() const { return _integrator->IntegratorName(_curIntMethod); } void setStandardIntegrator(integration::eIntegrationMethod m) { _curIntMethod = m; } @@ -25,14 +23,18 @@ namespace single_body_system { void setADIntegrator(integration::eAutoDiffIntegrationMethod m) { _curIntMethod_AD = m; } private: - double _mass; + void packState(mathlib::VecX& x) const; + void unpackState(const mathlib::VecX& x); - /*std::unique_ptr _dynamics;*/ + Body* _body; + std::unique_ptr _dynamics; std::unique_ptr _integrator; integration::eIntegrationMethod _curIntMethod{}; - std::unique_ptr _AD_integrator; integration::eAutoDiffIntegrationMethod _curIntMethod_AD{}; + + double _mass = 1.0; + mathlib::Vec3 _g{ 0.0, 0.0, 0.0 }; }; } \ No newline at end of file diff --git a/DSFE_App/DSFE_Core/src/SingleBodySystem/Body.cpp b/DSFE_App/DSFE_Core/src/SingleBodySystem/Body.cpp new file mode 100644 index 00000000..b889b500 --- /dev/null +++ b/DSFE_App/DSFE_Core/src/SingleBodySystem/Body.cpp @@ -0,0 +1,16 @@ +// DSFE_CORE Body.cpp +#include "pch.h" +#include "SingleBodySystem/Body.h" +#include "EngineLib/LogMacros.h" + +namespace single_body_system { + // Step the simulation forward by dt seconds at time t + void SingleBodySystem::step(double dt, double t) { + if (!_dynamics) { + _dynamics = std::make_unique(); + } + mathlib::Vec3 externalForce{ 0.0, 0.0, 0.0 }; // Placeholder for external forces + mathlib::Vec3 externalTorque{ 0.0, 0.0, 0.0 }; // Placeholder for external torques + _dynamics->computeDynamics(*this, externalForce, externalTorque, dt); + } +} \ No newline at end of file diff --git a/DSFE_App/DSFE_Core/src/SingleBodySystem/SingleBodySystem.cpp b/DSFE_App/DSFE_Core/src/SingleBodySystem/SingleBodySystem.cpp deleted file mode 100644 index 218a7124..00000000 --- a/DSFE_App/DSFE_Core/src/SingleBodySystem/SingleBodySystem.cpp +++ /dev/null @@ -1 +0,0 @@ -// DSFE_CORE SingleBodySystem.cpp \ No newline at end of file diff --git a/PxMLib/PhysLib/Include/SingleBodySystems/Dynamics.h b/PxMLib/PhysLib/Include/SingleBodySystems/Dynamics.h index 0571a126..75125274 100644 --- a/PxMLib/PhysLib/Include/SingleBodySystems/Dynamics.h +++ b/PxMLib/PhysLib/Include/SingleBodySystems/Dynamics.h @@ -5,7 +5,7 @@ namespace single_body_system::dynamics { class SingleBodyDynamics { public: - void computeDynamics(SingleBodySystem& body, mathlib::Vec3& externalForce, mathlib::Vec3& externalTorque, double dt) { + void computeDynamics(Body& body, mathlib::Vec3& externalForce, mathlib::Vec3& externalTorque, double dt) { // Compute linear acceleration body.state.xdd = (externalForce / body.inertia.mass).eval(); // Compute angular acceleration @@ -20,7 +20,7 @@ namespace single_body_system::dynamics { body.state.q = (body.state.q.coeffs() + (0.5 * dq.coeffs() * body.state.q.coeffs())).normalized(); } - void derivatives(const SingleBodySystem& body, mathlib::Vec3& externalForce, mathlib::Vec3& externalTorque, mathlib::Mat3& dxdot_dx, mathlib::Mat3& dxdot_dxdot) { + void derivatives(const Body& body, mathlib::Vec3& externalForce, mathlib::Vec3& externalTorque, mathlib::Mat3& dxdot_dx, mathlib::Mat3& dxdot_dxdot) { // Compute the Jacobian of the dynamics with respect to state variables dxdot_dx.setZero(); dxdot_dxdot.setZero(); diff --git a/PxMLib/PhysLib/Include/SingleBodySystems/SingleBodySystem.h b/PxMLib/PhysLib/Include/SingleBodySystems/SingleBodySystem.h index b17dd5da..f6ece898 100644 --- a/PxMLib/PhysLib/Include/SingleBodySystems/SingleBodySystem.h +++ b/PxMLib/PhysLib/Include/SingleBodySystems/SingleBodySystem.h @@ -30,7 +30,7 @@ namespace single_body_system { mathlib::Vec3 xddMax; // Maximum acceleration limits (xdd, ydd, zdd) }; - struct SingleBodySystem { + struct Body { std::string name = "UnnamedBody"; float scale = 1.0f; From 791a5caca8ef530ebf30dc977a200dd5ddd0793c Mon Sep 17 00:00:00 2001 From: saltyjoss Date: Sat, 13 Jun 2026 13:53:52 +0100 Subject: [PATCH 155/210] fixes: Removed unused and inappropriate MathLib files * Inappropriate meaning should not be within the library, not that the content is socially incorrect or misplaced. --- PxMLib/MathLib/include/dynamics/FrictionModels.h | 15 --------------- PxMLib/MathLib/include/dynamics/RigidBody.h | 14 -------------- 2 files changed, 29 deletions(-) delete mode 100644 PxMLib/MathLib/include/dynamics/FrictionModels.h delete mode 100644 PxMLib/MathLib/include/dynamics/RigidBody.h diff --git a/PxMLib/MathLib/include/dynamics/FrictionModels.h b/PxMLib/MathLib/include/dynamics/FrictionModels.h deleted file mode 100644 index 969ec102..00000000 --- a/PxMLib/MathLib/include/dynamics/FrictionModels.h +++ /dev/null @@ -1,15 +0,0 @@ -// PxM/MathLib FrictionModels.h - -#include - -namespace dynamics { - template - inline Scalar computeKarnoppFriction(const Scalar qd, const Scalar tau_active, Scalar c, Scalar b) { - Scalar Dv = Scalar(1e-4); // Small vel deadband - if (mathlib::abs(qd) > Dv) { return c * mathlib::sgn(qd) + b * qd; } // Viscous friction - else { - Scalar max_static_friction = c * Scalar(1.2); // Max static friction (20 percent higher than dynamic friction) - return mathlib::clamp(tau_active, -max_static_friction, max_static_friction); // Static friction with saturation - } - } -} // namespace dynamics \ No newline at end of file diff --git a/PxMLib/MathLib/include/dynamics/RigidBody.h b/PxMLib/MathLib/include/dynamics/RigidBody.h deleted file mode 100644 index a2c119ec..00000000 --- a/PxMLib/MathLib/include/dynamics/RigidBody.h +++ /dev/null @@ -1,14 +0,0 @@ -#pragma once - -#include - -using namespace mathlib; - -namespace dynamics { - template - struct LinkInertia { - Scalar mass; - Vec3_T com; // center of mass - Mat3_T inertia; // inertia tensor - }; -} \ No newline at end of file From d8929ef26248dda4abac2a537b631db909e7b3ab Mon Sep 17 00:00:00 2001 From: saltyjoss Date: Sat, 13 Jun 2026 13:54:07 +0100 Subject: [PATCH 156/210] feat: Added new central `PhysLib` header --- PxMLib/PhysLib/Include/PhysLib.h | 9 +++++++++ 1 file changed, 9 insertions(+) create mode 100644 PxMLib/PhysLib/Include/PhysLib.h diff --git a/PxMLib/PhysLib/Include/PhysLib.h b/PxMLib/PhysLib/Include/PhysLib.h new file mode 100644 index 00000000..2741d1ad --- /dev/null +++ b/PxMLib/PhysLib/Include/PhysLib.h @@ -0,0 +1,9 @@ +// PxM/PhysLib PhysLib.h + +#ifdef PHYS_LIB_H +#define PHYS_LIB_H + +#include +#include "SingleBodySystems/SingleBodySystem.h" + +#endif // PHYS_LIB_H \ No newline at end of file From d54bfbd4bbbf706b6674c1e046933f6251e45803 Mon Sep 17 00:00:00 2001 From: saltyjoss Date: Sat, 13 Jun 2026 15:12:05 +0100 Subject: [PATCH 157/210] feat: Added a simple stepping method into `SingleBodySystem` --- .../DSFE_Core/include/SingleBodySystem/Body.h | 20 +++- .../DSFE_Core/src/SingleBodySystem/Body.cpp | 98 +++++++++++++++++-- 2 files changed, 107 insertions(+), 11 deletions(-) diff --git a/DSFE_App/DSFE_Core/include/SingleBodySystem/Body.h b/DSFE_App/DSFE_Core/include/SingleBodySystem/Body.h index 9d8c3809..afc400d2 100644 --- a/DSFE_App/DSFE_Core/include/SingleBodySystem/Body.h +++ b/DSFE_App/DSFE_Core/include/SingleBodySystem/Body.h @@ -6,14 +6,19 @@ #include "SingleBodySystems/Dynamics.h" #include "Numerics/IntegrationService.h" +#include "Analysis/MetricLogger.h" + namespace single_body_system { - class SingleBodySystem { + class SingleBodySystem { // FOR NOW, this represents a single RIGID BODY, but in the near-near-near-future I will extend this for particles (or make a separate ParticleSystem class, changing this to RigidBodySystem) public: - SingleBodySystem() = default; - ~SingleBodySystem() = default; + SingleBodySystem(); + ~SingleBodySystem(); void step(double dt, double t); + Body* body() { return _body; } // return pointer to the body + const Body* body() const { return _body; } // return const pointer to the body + integration::eIntegrationMethod getIntegrationMethod() const { return _curIntMethod; } std::string getIntegratorName() const { return _integrator->IntegratorName(_curIntMethod); } void setStandardIntegrator(integration::eIntegrationMethod m) { _curIntMethod = m; } @@ -23,8 +28,8 @@ namespace single_body_system { void setADIntegrator(integration::eAutoDiffIntegrationMethod m) { _curIntMethod_AD = m; } private: - void packState(mathlib::VecX& x) const; - void unpackState(const mathlib::VecX& x); + mathlib::VecX packState() const; + void unpackState(const mathlib::VecX& s); Body* _body; std::unique_ptr _dynamics; @@ -34,6 +39,11 @@ namespace single_body_system { std::unique_ptr _AD_integrator; integration::eAutoDiffIntegrationMethod _curIntMethod_AD{}; + // precomputed clamp lookup tables + mutable std::vector _clampVel; + mutable std::vector _clampAngVel; + + double _simTime = 0.0; double _mass = 1.0; mathlib::Vec3 _g{ 0.0, 0.0, 0.0 }; }; diff --git a/DSFE_App/DSFE_Core/src/SingleBodySystem/Body.cpp b/DSFE_App/DSFE_Core/src/SingleBodySystem/Body.cpp index b889b500..ebe00b8a 100644 --- a/DSFE_App/DSFE_Core/src/SingleBodySystem/Body.cpp +++ b/DSFE_App/DSFE_Core/src/SingleBodySystem/Body.cpp @@ -4,13 +4,99 @@ #include "EngineLib/LogMacros.h" namespace single_body_system { + SingleBodySystem::SingleBodySystem() + : _body(new Body()), _dynamics(std::make_unique()), + _integrator(std::make_unique()), _AD_integrator(std::make_unique()) + { + } + + mathlib::VecX SingleBodySystem::packState() const { + if (!_body) { LOG_ERROR("Body not initialised!"); return; } + mathlib::VecX x(13); + x.segment<3>(0) = _body->state.p; // position + x.segment<3>(3) = _body->state.pd; // linear velocity + x.segment<4>(6) = _body->state.w; // angular velocity + x.segment<3>(9) = _body->state.q.coeffs(); // quaternion coefficients <- included because this should work for various single bodies, but in the case of a particle we can just use a different state representation via particle_packState() + return x; + } + + void SingleBodySystem::unpackState(const mathlib::VecX& x) { + if (!_body) { LOG_ERROR("Body not initialised!"); return; } + if (x.size() != 13) { LOG_ERROR("State vector size mismatch! Expected 13, got %d", (int)x.size()); return; } + + if (_clampVel.size() != 3) { _clampVel.resize(3, 0); } + if (_clampAngVel.size() != 3) { _clampAngVel.resize(3, 0); } + + auto& p_in = x.segment<3>(0); + auto& pd_in = x.segment<3>(3); + auto& w_in = x.segment<3>(6); + + for (int i = 0; i < 3; ++i) { + double pd_i = pd_in[i]; // Store the original value before clamping + double w_i = w_in[i]; // Store the original value before clamping + + double pdMax_hpd = std::abs(_body->limits.pdMax[i]); + double pd_i_out = pd_i; + double wMax_hw = std::abs(_body->limits.wMax[i]); + double w_i_out = w_i; + + const double eps = 0.05; // 5 % tolerance for clamping + // TODO: Check the correct way to clamp the velocities close the S.o.L + // * Not really sure if I have done this correctly + if (pdMax_hpd > 0.0) { + if (pdMax_hpd == constants::c_0) { std::clamp(pd_i, -pdMax_hpd, pdMax_hpd); } + else if (std::abs(pd_i) > (1.0 + eps) * pdMax_hpd) { pd_i_out = std::clamp(pd_i, -pdMax_hpd, pdMax_hpd); } + } + if (wMax_hw > 0.0) { + if (wMax_hw == constants::c_0) { std::clamp(w_i, -wMax_hw, wMax_hw); } + else if (std::abs(w_i) > (1.0 + eps) * wMax_hw) { w_i_out = std::clamp(w_i, -wMax_hw, wMax_hw); } + } + + _clampVel[i] = (pd_i_out != pd_i) ? 1 : 0; + _clampAngVel[i] = (w_i_out != w_i) ? 1 : 0; + + _body->state.p[i] = p_in[i]; + _body->state.pd[i] = pd_i_out; + _body->state.w[i] = w_i_out; + } + _body->state.q.coeffs() = x.segment<4>(9); + + + if (_clampVel[0] || _clampVel[1] || _clampVel[2]) { LOG_WARN("Linear velocity clamping applied: pd = [%f, %f, %f]", pd_in[0], pd_in[1], pd_in[2]); } + if (_clampAngVel[0] || _clampAngVel[1] || _clampAngVel[2]) { LOG_WARN("Angular velocity clamping applied: w = [%f, %f, %f]", w_in[0], w_in[1], w_in[2]); } + } + // Step the simulation forward by dt seconds at time t void SingleBodySystem::step(double dt, double t) { - if (!_dynamics) { - _dynamics = std::make_unique(); - } - mathlib::Vec3 externalForce{ 0.0, 0.0, 0.0 }; // Placeholder for external forces - mathlib::Vec3 externalTorque{ 0.0, 0.0, 0.0 }; // Placeholder for external torques - _dynamics->computeDynamics(*this, externalForce, externalTorque, dt); + if (!_body) { LOG_ERROR("Body not initialised!"); return; } + if (!_dynamics) { LOG_ERROR("Dynamics system not initialised!"); return; } + + _simTime = t; + VecX x = packState(); + // Not going to use the dynamics methods initially, just want a straight cut test first. + auto func = [this](const mathlib::VecX& x, mathlib::VecX& dxdt) { + // Unpack the state vector into the body state + unpackState(x); + // Compute the derivatives of the state (dx/dt) + mathlib::Vec3 externalForce{ 0.0, 0.0, 0.0 }; // Placeholder for external forces + mathlib::Vec3 externalTorque{ 0.0, 0.0, 0.0 }; // Placeholder for external torques + // Compute linear acceleration + if (externalForce == mathlib::Vec3(0.0, 0.0, 0.0)) { _body->state.pdd = mathlib::Vec3(0.0, 0.0, 0.0); } + else { _body->state.pdd = (externalForce / _body->inertia.mass).eval(); } + // Compute angular acceleration + mathlib::Mat3 inertiaInv = _body->inertia.inertiaTensor.inverse(); + _body->state.wd = inertiaInv * (externalTorque - _body->state.w.cross(_body->inertia.inertiaTensor * _body->state.w)); + // Fill in the derivative vector dxdt + dxdt.segment<3>(0) = _body->state.pd; // dp/dt = pd + dxdt.segment<3>(3) = _body->state.pdd; // dpd/dt = pdd + dxdt.segment<4>(6) = _body->state.w; // dw/dt = w + dxdt.segment<3>(10) = _body->state.wd; // dwd/dt = wd + }; + + auto step = _integrator->step(_curIntMethod, x, t, dt, func, nullptr); // Wont work with Implicit since no jacobian provided, but will work with RK4 and other explicit methods + // Also will not with adaptive (probably) since no rtol/atol provided, but will work with fixed step methods + VecX x_next = step.x_next; + + unpackState(x); } } \ No newline at end of file From acdddd283c9a973323aea3b1e9cf49931ca4742e Mon Sep 17 00:00:00 2001 From: saltyjoss Date: Sat, 13 Jun 2026 15:12:09 +0100 Subject: [PATCH 158/210] fixes --- DSFE_App/DSFE_Core/include/Robots/RobotDynamics.h | 1 - 1 file changed, 1 deletion(-) diff --git a/DSFE_App/DSFE_Core/include/Robots/RobotDynamics.h b/DSFE_App/DSFE_Core/include/Robots/RobotDynamics.h index ca7b0e92..c9a850b3 100644 --- a/DSFE_App/DSFE_Core/include/Robots/RobotDynamics.h +++ b/DSFE_App/DSFE_Core/include/Robots/RobotDynamics.h @@ -3,7 +3,6 @@ #include "EngineCore.h" #include -#include #include "Robots/DynamicsTypes.h" #include "Robots/RobotMetrics.h" From 25395a37d772ea667a10919200ff506e49aa5179 Mon Sep 17 00:00:00 2001 From: saltyjoss Date: Sat, 13 Jun 2026 15:12:45 +0100 Subject: [PATCH 159/210] refactor: Updated `SingleBodyDynamics` and `Body` to reflect new pos, vel, and acc naming. --- .../Include/SingleBodySystems/Dynamics.h | 25 ++++++++++++++++--- .../SingleBodySystems/SingleBodySystem.h | 22 ++++++++-------- 2 files changed, 34 insertions(+), 13 deletions(-) diff --git a/PxMLib/PhysLib/Include/SingleBodySystems/Dynamics.h b/PxMLib/PhysLib/Include/SingleBodySystems/Dynamics.h index 75125274..d8a04311 100644 --- a/PxMLib/PhysLib/Include/SingleBodySystems/Dynamics.h +++ b/PxMLib/PhysLib/Include/SingleBodySystems/Dynamics.h @@ -5,15 +5,34 @@ namespace single_body_system::dynamics { class SingleBodyDynamics { public: + void buildInertiaTensor(Body& body) { + // Compute the inertia tensor based on the body's mass and center of mass + double m = body.inertia.mass; + mathlib::Vec3 com = body.inertia.com_xyz; + // Inertia tensor for a point mass at the center of mass + mathlib::Mat3 I = mathlib::Mat3::Zero(); // Local inertia tensor + I << + (1.0 / 12.0) * m * (com.y() * com.y() + com.z() * com.z()), 0, 0, + 0, (1.0 / 12.0)* m * (com.x() * com.x() + com.z() * com.z()), 0, + 0, 0, (1.0 / 12.0)* m * (com.x() * com.x() + com.y() * com.y()); + body.inertia.inertiaTensor = I; // Assign the computed inertia tensor to the body + } + + void buildParticleInertia(Body& body) { + // For a particle, in the simple case the body is treated as a point mass, so the inertia tensor is zero + body.inertia.inertiaTensor = mathlib::Mat3::Zero(); + } + void computeDynamics(Body& body, mathlib::Vec3& externalForce, mathlib::Vec3& externalTorque, double dt) { // Compute linear acceleration - body.state.xdd = (externalForce / body.inertia.mass).eval(); + if (externalForce == mathlib::Vec3(0.0, 0.0, 0.0)) { body.state.pdd = mathlib::Vec3(0.0, 0.0, 0.0); } + else { body.state.pdd = (externalForce / body.inertia.mass).eval(); } // Compute angular acceleration mathlib::Mat3 inertiaInv = body.inertia.inertiaTensor.inverse(); body.state.wd = inertiaInv * (externalTorque - body.state.w.cross(body.inertia.inertiaTensor * body.state.w)); // Update linear velocity and position - body.state.xd += body.state.xdd * dt; - body.state.x += body.state.xd * dt; + body.state.pd += body.state.pdd * dt; + body.state.p += body.state.pd * dt; // Update angular velocity and orientation (using quaternion integration) body.state.w += body.state.wd * dt; mathlib::Quat dq = mathlib::Quat(0.0, body.state.w * dt); diff --git a/PxMLib/PhysLib/Include/SingleBodySystems/SingleBodySystem.h b/PxMLib/PhysLib/Include/SingleBodySystems/SingleBodySystem.h index f6ece898..1606d0b4 100644 --- a/PxMLib/PhysLib/Include/SingleBodySystems/SingleBodySystem.h +++ b/PxMLib/PhysLib/Include/SingleBodySystems/SingleBodySystem.h @@ -15,19 +15,21 @@ namespace single_body_system { }; struct DynamicsState { - mathlib::Vec3 x; // Position of the body in 3D space - mathlib::Vec3 xd; // Velocity of the body in 3D space - mathlib::Vec3 xdd; // Acceleration of the body in 3D space - mathlib::Quat q; // Orientation of the body (quaternion) - mathlib::Vec3 w; // Angular velocity of the body - mathlib::Vec3 wd; // Angular acceleration of the body + mathlib::Vec3 p{ 0.0 }; // Position of the body in 3D space (meters) + mathlib::Vec3 pd{ 0.0 }; // Velocity of the body in 3D space (meters/second) + mathlib::Vec3 pdd{ 0.0 }; // Acceleration of the body in 3D space (meters/second^2) + mathlib::Quat q{ 1.0, 0.0, 0.0, 0.0 }; // Orientation of the body (quaternion) + mathlib::Vec3 w{ 0.0 }; // Angular velocity of the body (rads/second) + mathlib::Vec3 wd{ 0.0 }; // Angular acceleration of the body (rads/second^2) }; struct BodyLimits { - mathlib::Vec3 xdMin{ 0.0 }; // Minimum velocity limits (xd, yd, zd) - mathlib::Vec3 xdMax{ constants::c_0 }; // Maximum velocity limits (xd, yd, zd) - mathlib::Vec3 xddMin{ 0.0 }; // Minimum acceleration limits (xdd, ydd, zdd) - mathlib::Vec3 xddMax; // Maximum acceleration limits (xdd, ydd, zdd) + mathlib::Vec3 pdMin{ 0.0 }; // Minimum velocity limits (xd, yd, zd) + mathlib::Vec3 pdMax{ constants::c_0 }; // Maximum velocity limits (xd, yd, zd) + mathlib::Vec3 pddMin{ 0.0 }; // Minimum acceleration limits (xdd, ydd, zdd) + mathlib::Vec3 pddMax; // Maximum acceleration limits (xdd, ydd, zdd) + mathlib::Vec3 wMin{ 0.0 }; // Minimum angular velocity limits (wx, wy, wz) + mathlib::Vec3 wMax{ constants::c_0 }; // Maximum angular velocity limits (wx, wy, wz) }; struct Body { From 650e3d8b6e44c75e1a9f31235f38d91073bb26eb Mon Sep 17 00:00:00 2001 From: saltyjoss Date: Sat, 13 Jun 2026 17:10:38 +0100 Subject: [PATCH 160/210] fixes: Updated `Dynamics` to have correct matrix and vector computations * AGAIN NOT ENTIRELY SURE IF THIS IS EVEN CORRECT AT A SIMPLE LEVEL, IM NOT TRYING TO CORRECT IT RIGHT NOW, IM TRYING TO CHECK IF IT FUNCTIONS AS EXPECTED IN C++, THEN CORRECT. --- .../DSFE_Core/include/SingleBodySystem/Body.h | 3 + .../DSFE_Core/src/SingleBodySystem/Body.cpp | 36 +++------ .../Include/SingleBodySystems/Dynamics.h | 77 +++++++++++++------ .../SingleBodySystems/SingleBodySystem.h | 28 +++---- 4 files changed, 81 insertions(+), 63 deletions(-) diff --git a/DSFE_App/DSFE_Core/include/SingleBodySystem/Body.h b/DSFE_App/DSFE_Core/include/SingleBodySystem/Body.h index afc400d2..72b1c0df 100644 --- a/DSFE_App/DSFE_Core/include/SingleBodySystem/Body.h +++ b/DSFE_App/DSFE_Core/include/SingleBodySystem/Body.h @@ -39,6 +39,9 @@ namespace single_body_system { std::unique_ptr _AD_integrator; integration::eAutoDiffIntegrationMethod _curIntMethod_AD{}; + mathlib::Vec3 _F_ext{ 0.0, 0.0, 0.0 }; + mathlib::Vec3 _tau_ext{ 0.0, 0.0, 0.0 }; + // precomputed clamp lookup tables mutable std::vector _clampVel; mutable std::vector _clampAngVel; diff --git a/DSFE_App/DSFE_Core/src/SingleBodySystem/Body.cpp b/DSFE_App/DSFE_Core/src/SingleBodySystem/Body.cpp index ebe00b8a..00b91dd0 100644 --- a/DSFE_App/DSFE_Core/src/SingleBodySystem/Body.cpp +++ b/DSFE_App/DSFE_Core/src/SingleBodySystem/Body.cpp @@ -11,12 +11,12 @@ namespace single_body_system { } mathlib::VecX SingleBodySystem::packState() const { - if (!_body) { LOG_ERROR("Body not initialised!"); return; } mathlib::VecX x(13); x.segment<3>(0) = _body->state.p; // position x.segment<3>(3) = _body->state.pd; // linear velocity - x.segment<4>(6) = _body->state.w; // angular velocity - x.segment<3>(9) = _body->state.q.coeffs(); // quaternion coefficients <- included because this should work for various single bodies, but in the case of a particle we can just use a different state representation via particle_packState() + x.segment<4>(6) = _body->state.q.coeffs(); + x.segment<3>(10) = _body->state.w; // angular velocity + return x; } @@ -29,7 +29,7 @@ namespace single_body_system { auto& p_in = x.segment<3>(0); auto& pd_in = x.segment<3>(3); - auto& w_in = x.segment<3>(6); + auto& w_in = x.segment<3>(10); for (int i = 0; i < 3; ++i) { double pd_i = pd_in[i]; // Store the original value before clamping @@ -59,7 +59,7 @@ namespace single_body_system { _body->state.pd[i] = pd_i_out; _body->state.w[i] = w_i_out; } - _body->state.q.coeffs() = x.segment<4>(9); + _body->state.q.coeffs() = x.segment<4>(6); if (_clampVel[0] || _clampVel[1] || _clampVel[2]) { LOG_WARN("Linear velocity clamping applied: pd = [%f, %f, %f]", pd_in[0], pd_in[1], pd_in[2]); } @@ -73,27 +73,11 @@ namespace single_body_system { _simTime = t; VecX x = packState(); - // Not going to use the dynamics methods initially, just want a straight cut test first. - auto func = [this](const mathlib::VecX& x, mathlib::VecX& dxdt) { - // Unpack the state vector into the body state - unpackState(x); - // Compute the derivatives of the state (dx/dt) - mathlib::Vec3 externalForce{ 0.0, 0.0, 0.0 }; // Placeholder for external forces - mathlib::Vec3 externalTorque{ 0.0, 0.0, 0.0 }; // Placeholder for external torques - // Compute linear acceleration - if (externalForce == mathlib::Vec3(0.0, 0.0, 0.0)) { _body->state.pdd = mathlib::Vec3(0.0, 0.0, 0.0); } - else { _body->state.pdd = (externalForce / _body->inertia.mass).eval(); } - // Compute angular acceleration - mathlib::Mat3 inertiaInv = _body->inertia.inertiaTensor.inverse(); - _body->state.wd = inertiaInv * (externalTorque - _body->state.w.cross(_body->inertia.inertiaTensor * _body->state.w)); - // Fill in the derivative vector dxdt - dxdt.segment<3>(0) = _body->state.pd; // dp/dt = pd - dxdt.segment<3>(3) = _body->state.pdd; // dpd/dt = pdd - dxdt.segment<4>(6) = _body->state.w; // dw/dt = w - dxdt.segment<3>(10) = _body->state.wd; // dwd/dt = wd - }; - - auto step = _integrator->step(_curIntMethod, x, t, dt, func, nullptr); // Wont work with Implicit since no jacobian provided, but will work with RK4 and other explicit methods + // Not going to use the computeDynamics methods initially, just want a straight cut test first. + auto f_deriv = [&](auto t, const auto& x) { return _dynamics->derivatives(*_body, x, _F_ext, _tau_ext, dt); }; + auto f_jac = [&](const auto& x, auto& J_out) { _dynamics->jacobian(*_body, x, J_out); }; + + auto step = _integrator->step(_curIntMethod, x, t, dt, f_deriv, f_jac); // Wont work with Implicit since no jacobian provided, but will work with RK4 and other explicit methods // Also will not with adaptive (probably) since no rtol/atol provided, but will work with fixed step methods VecX x_next = step.x_next; diff --git a/PxMLib/PhysLib/Include/SingleBodySystems/Dynamics.h b/PxMLib/PhysLib/Include/SingleBodySystems/Dynamics.h index d8a04311..d2ca1c70 100644 --- a/PxMLib/PhysLib/Include/SingleBodySystems/Dynamics.h +++ b/PxMLib/PhysLib/Include/SingleBodySystems/Dynamics.h @@ -23,33 +23,64 @@ namespace single_body_system::dynamics { body.inertia.inertiaTensor = mathlib::Mat3::Zero(); } - void computeDynamics(Body& body, mathlib::Vec3& externalForce, mathlib::Vec3& externalTorque, double dt) { - // Compute linear acceleration - if (externalForce == mathlib::Vec3(0.0, 0.0, 0.0)) { body.state.pdd = mathlib::Vec3(0.0, 0.0, 0.0); } - else { body.state.pdd = (externalForce / body.inertia.mass).eval(); } - // Compute angular acceleration - mathlib::Mat3 inertiaInv = body.inertia.inertiaTensor.inverse(); - body.state.wd = inertiaInv * (externalTorque - body.state.w.cross(body.inertia.inertiaTensor * body.state.w)); + mathlib::VecX derivatives(Body& body, const mathlib::VecX& x, mathlib::Vec3& F_ext, mathlib::Vec3& tau_ext, double dt) { + mathlib::VecX dxdt(13); + + mathlib::Vec3 p = x.block<3, 1>(0, 0); + mathlib::Vec3 pd = x.block<3, 1>(3, 0); + mathlib::Vec4 v = x.block<4, 1>(6, 0); + mathlib::Quat q_coeffs(v); + mathlib::Vec3 w = x.block<3, 1>(10, 0); + + if (F_ext == mathlib::Vec3::Zero()) { body.state.pdd = F_ext; } + else { body.state.pdd = (F_ext / body.inertia.mass).eval(); } + // Update linear velocity and position - body.state.pd += body.state.pdd * dt; - body.state.p += body.state.pd * dt; - // Update angular velocity and orientation (using quaternion integration) - body.state.w += body.state.wd * dt; - mathlib::Quat dq = mathlib::Quat(0.0, body.state.w * dt); - body.state.q = (body.state.q.coeffs() + (0.5 * dq.coeffs() * body.state.q.coeffs())).normalized(); + body.state.pd = body.state.pdd * dt; + body.state.p = body.state.pd * dt; + + if (tau_ext == mathlib::Vec3::Zero()) { + body.state.wd = tau_ext; + body.state.w = tau_ext; + } + else { + mathlib::Mat3 inertiaInv = body.inertia.inertiaTensor.inverse(); + body.state.wd = inertiaInv * (tau_ext - body.state.w.cross(body.inertia.inertiaTensor * body.state.w)); + body.state.w = body.state.wd * dt; + } + + // Update orientation quaternion + if (q_coeffs.norm() > 1e-12) { + body.state.q = mathlib::Quat(q_coeffs).normalized(); + } + else { + body.state.q.coeffs() = mathlib::Quat(1.0, 0.0, 0.0, 0.0).coeffs(); + } + + double theta = body.state.w.norm() * dt; + if (theta > 1e-12) { + mathlib::Quat dq(Eigen::AngleAxis(theta, body.state.w.normalized())); + body.state.q = (body.state.q * dq).normalized(); + } + + dxdt.block<3, 1>(0, 0) = body.state.pd; // dp/dt = pd + dxdt.block<3, 1>(3, 0) = body.state.pdd; // dpd/dt = pdd + dxdt.block<4, 1>(6, 0) = body.state.q.coeffs(); // dq/dt = q (quaternion) + dxdt.block<3, 1>(10, 0) = body.state.w; // dw/dt = w + + return dxdt; } - void derivatives(const Body& body, mathlib::Vec3& externalForce, mathlib::Vec3& externalTorque, mathlib::Mat3& dxdot_dx, mathlib::Mat3& dxdot_dxdot) { - // Compute the Jacobian of the dynamics with respect to state variables - dxdot_dx.setZero(); - dxdot_dxdot.setZero(); - // Linear acceleration derivatives - dxdot_dx.block<3, 3>(0, 0) = mathlib::Mat3::Zero(); // dx/dx = 0 - dxdot_dxdot.block<3, 3>(0, 3) = mathlib::Mat3::Identity() / body.inertia.mass; // dx/dxdot = 1/mass - // Angular acceleration derivatives + void jacobian(Body& body, const mathlib::VecX& x, mathlib::MatX& J_out) { + J_out.setZero(13, 13); + // Partial derivatives for position and velocity + J_out.block<3, 3>(0, 3) = mathlib::Mat3::Identity(); // dp/dt = pd + J_out.block<3, 3>(3, 10) = mathlib::Mat3::Identity(); // dpd/dt = pdd + // Partial derivatives for orientation (quaternion) + J_out.block<4, 4>(6, 6) = mathlib::Mat4::Identity(); // dq/dt = q + // Partial derivatives for angular velocity mathlib::Mat3 inertiaInv = body.inertia.inertiaTensor.inverse(); - dxdot_dx.block<3, 3>(3, 6) = -inertiaInv * skewSymmetric(body.state.w) * body.inertia.inertiaTensor; // dw/dq - dxdot_dxdot.block<3, 3>(3, 9) = inertiaInv; // dw/dwd = I^-1 + J_out.block<3, 3>(10, 10) = inertiaInv; // dw/dt = inertiaInv * (tau_ext - w x (I * w)) } }; } \ No newline at end of file diff --git a/PxMLib/PhysLib/Include/SingleBodySystems/SingleBodySystem.h b/PxMLib/PhysLib/Include/SingleBodySystems/SingleBodySystem.h index 1606d0b4..d218396d 100644 --- a/PxMLib/PhysLib/Include/SingleBodySystems/SingleBodySystem.h +++ b/PxMLib/PhysLib/Include/SingleBodySystems/SingleBodySystem.h @@ -10,26 +10,26 @@ namespace single_body_system { // Inertia struct representing the mass and inertia tensor of a rigid body struct BodyInertia { double mass = 0.0; - mathlib::Vec3 com_xyz{ 0.0, 0.0, 0.0 }; // Center of mass position in the body frame - mathlib::Mat3 inertiaTensor; // 3x3 inertia tensor matrix + mathlib::Vec3 com_xyz = mathlib::Vec3::Zero();; // Center of mass position in the body frame + mathlib::Mat3 inertiaTensor = mathlib::Mat3::Zero();; // Inertia tensor in the body frame }; struct DynamicsState { - mathlib::Vec3 p{ 0.0 }; // Position of the body in 3D space (meters) - mathlib::Vec3 pd{ 0.0 }; // Velocity of the body in 3D space (meters/second) - mathlib::Vec3 pdd{ 0.0 }; // Acceleration of the body in 3D space (meters/second^2) - mathlib::Quat q{ 1.0, 0.0, 0.0, 0.0 }; // Orientation of the body (quaternion) - mathlib::Vec3 w{ 0.0 }; // Angular velocity of the body (rads/second) - mathlib::Vec3 wd{ 0.0 }; // Angular acceleration of the body (rads/second^2) + mathlib::Vec3 p = mathlib::Vec3::Zero(); // Position of the body in 3D space (meters) + mathlib::Vec3 pd = mathlib::Vec3::Zero(); // Velocity of the body in 3D space (meters/second) + mathlib::Vec3 pdd = mathlib::Vec3::Zero(); // Acceleration of the body in 3D space (meters/second^2) + mathlib::Quat q = mathlib::Quat::Identity(); // Orientation of the body as a quaternion + mathlib::Vec3 w = mathlib::Vec3::Zero(); // Angular velocity of the body (rads/second) + mathlib::Vec3 wd = mathlib::Vec3::Zero(); // Angular acceleration of the body (rads/second^2) }; struct BodyLimits { - mathlib::Vec3 pdMin{ 0.0 }; // Minimum velocity limits (xd, yd, zd) - mathlib::Vec3 pdMax{ constants::c_0 }; // Maximum velocity limits (xd, yd, zd) - mathlib::Vec3 pddMin{ 0.0 }; // Minimum acceleration limits (xdd, ydd, zdd) - mathlib::Vec3 pddMax; // Maximum acceleration limits (xdd, ydd, zdd) - mathlib::Vec3 wMin{ 0.0 }; // Minimum angular velocity limits (wx, wy, wz) - mathlib::Vec3 wMax{ constants::c_0 }; // Maximum angular velocity limits (wx, wy, wz) + mathlib::Vec3 pdMin = mathlib::Vec3::Zero(); // Minimum velocity limits (xd, yd, zd) + mathlib::Vec3 pdMax = mathlib::Vec3(c_0, c_0, c_0); // Maximum velocity limits (xd, yd, zd) + mathlib::Vec3 pddMin = mathlib::Vec3::Zero(); // Minimum acceleration limits (xdd, ydd, zdd) + mathlib::Vec3 pddMax = mathlib::Vec3::Zero(); // Maximum acceleration limits (xdd, ydd, zdd) + mathlib::Vec3 wMin = mathlib::Vec3::Zero(); // Minimum angular velocity limits (wx, wy, wz) + mathlib::Vec3 wMax = mathlib::Vec3(c_0, c_0, c_0); // Maximum angular velocity limits (wx, wy, wz) }; struct Body { From d8ae172ebb82c9c97251e52947a6e5097e68af40 Mon Sep 17 00:00:00 2001 From: saltyjoss Date: Sat, 13 Jun 2026 17:35:10 +0100 Subject: [PATCH 161/210] feat: Added basic set of unit tests to test the `Dynamics.h` methods --- PxMLib/CMakeLists.txt | 1 + PxMLib/PhysLib_Unit/CMakeLists.txt | 5 + PxMLib/PhysLib_Unit/Common/TestHarness.h | 99 +++++++++++++++ .../SingleBodySystemTests/CMakeLists.txt | 27 +++++ .../SingleBodySystemDynamicsTests.cpp | 113 ++++++++++++++++++ .../SingleBodySystemTests/main.cpp | 32 +++++ 6 files changed, 277 insertions(+) create mode 100644 PxMLib/PhysLib_Unit/CMakeLists.txt create mode 100644 PxMLib/PhysLib_Unit/Common/TestHarness.h create mode 100644 PxMLib/PhysLib_Unit/SingleBodySystemTests/CMakeLists.txt create mode 100644 PxMLib/PhysLib_Unit/SingleBodySystemTests/SingleBodySystemDynamicsTests.cpp create mode 100644 PxMLib/PhysLib_Unit/SingleBodySystemTests/main.cpp diff --git a/PxMLib/CMakeLists.txt b/PxMLib/CMakeLists.txt index 43f5cd6a..af62eede 100644 --- a/PxMLib/CMakeLists.txt +++ b/PxMLib/CMakeLists.txt @@ -6,3 +6,4 @@ project(PxMLib LANGUAGES CXX) add_subdirectory(MathLib) add_subdirectory(MathLib_Unit) add_subdirectory(PhysLib) +add_subdirectory(PhysLib_Unit) diff --git a/PxMLib/PhysLib_Unit/CMakeLists.txt b/PxMLib/PhysLib_Unit/CMakeLists.txt new file mode 100644 index 00000000..7cc7e428 --- /dev/null +++ b/PxMLib/PhysLib_Unit/CMakeLists.txt @@ -0,0 +1,5 @@ +# CMakeLists.txt for PhysLib_Unit_Tests executable +cmake_minimum_required(VERSION 3.17) +project(PhysLib_Unit_Tests LANGUAGES C CXX) + +add_subdirectory(SingleBodySystemTests) \ No newline at end of file diff --git a/PxMLib/PhysLib_Unit/Common/TestHarness.h b/PxMLib/PhysLib_Unit/Common/TestHarness.h new file mode 100644 index 00000000..32665440 --- /dev/null +++ b/PxMLib/PhysLib_Unit/Common/TestHarness.h @@ -0,0 +1,99 @@ +#pragma once +// File: TestHarness.h +// GitHub: SaltyJoss +// Pretty basic console test runner for integration tests +#include +#include +#include +#include +#include +#include +#include + +namespace test { + // ANSI colour codes for console output + inline constexpr const char* GREEN = "\033[32m"; + inline constexpr const char* RED = "\033[31m"; + inline constexpr const char* YELLOW = "\033[33m"; + inline constexpr const char* CYAN = "\033[36m"; + inline constexpr const char* RESET = "\033[0m"; + + // assertTrue checks a condition and throws a runtime_error with the given message if it fails. + inline void assertTrue(bool condition, const char* msg) { + if (!condition) { throw std::runtime_error(msg); } + } + + // TestCase represents a single test with its group, name, and function to execute. + struct TestCase { + std::string group; + std::string name; + std::function fn; + }; + + // registry() returns a reference to the static vector of registered test cases. + inline std::vector& registry() { + static std::vector tests; + return tests; + } + + // AutoRegister is a helper that registers a test case at static initialization time. + struct AutoRegister { + AutoRegister(const char* group, const char* name, std::function fn) { + registry().push_back({ group, name, std::move(fn) }); + } + }; + + // Runs all registered tests, printing results and timing. Returns number of failed tests. + inline int runAll() { + int passed = 0, failed = 0; + std::string lastGroup; + // Iterate through all registered test cases, grouped by their group name. + for (const auto& tc : registry()) { + if (tc.group != lastGroup) { + std::cout << "\n" << CYAN << "=== " << tc.group << " ===" << RESET << "\n"; + lastGroup = tc.group; + } + // Time the test execution and catch any exceptions to report failures. + auto t0 = std::chrono::high_resolution_clock::now(); + try { + tc.fn(); + auto t1 = std::chrono::high_resolution_clock::now(); + double ms = std::chrono::duration(t1 - t0).count(); + std::cout << " " << GREEN << "PASS" << RESET + << " " << tc.name + << " (" << static_cast(ms) << " ms)\n"; + ++passed; + } + // If an exception is thrown, catch it and report as a failure with the error message. + catch (const std::exception& e) { + auto t1 = std::chrono::high_resolution_clock::now(); + double ms = std::chrono::duration(t1 - t0).count(); + std::cout << " " << RED << "FAIL" << RESET + << " " << tc.name + << " (" << static_cast(ms) << " ms)" + << " -> " << e.what() << "\n"; + ++failed; + } + } + + // Print a summary of the test results, showing total, passed, and failed counts with color coding. + std::cout << "\n" << std::string(50, '-') << "\n"; + std::cout << "Total: " << (passed + failed) + << " " << GREEN << "Passed: " << passed << RESET + << " " << (failed ? RED : GREEN) << "Failed: " << failed << RESET << "\n"; + + return failed; + } +} + +// TEST(group, name) registers a free function as a test case. +#define TEST(groupName, testName) \ + static void testName##_impl(); \ + namespace { static test::AutoRegister _reg_##testName( \ + groupName, #testName, testName##_impl); } \ + static void testName##_impl() + +#define ASSERT_TRUE(cond, msg) test::assertTrue((cond), (msg)) +#define ASSERT_EQ(a, b, tol, msg) test::assertTrue(std::abs((a) - (b)) < (tol)), (msg) +#define SUCCEED() return +#define ASSERT_NEAR(a, b, tol) test::assertTrue(std::abs((a) - (b)) < (tol), "Expected " #a " and " #b " to be within " #tol) \ No newline at end of file diff --git a/PxMLib/PhysLib_Unit/SingleBodySystemTests/CMakeLists.txt b/PxMLib/PhysLib_Unit/SingleBodySystemTests/CMakeLists.txt new file mode 100644 index 00000000..c9779dbe --- /dev/null +++ b/PxMLib/PhysLib_Unit/SingleBodySystemTests/CMakeLists.txt @@ -0,0 +1,27 @@ +# CMakeLists.txt for MathLib_Unit_Tests executable +cmake_minimum_required(VERSION 3.17) +project(PhysLib_SingleBodySystemTests LANGUAGES C CXX) + +enable_testing() + +add_executable(PhysLib_SingleBodySystemTests + main.cpp + SingleBodySystemDynamicsTests.cpp +) + +target_link_libraries(PhysLib_SingleBodySystemTests PRIVATE + PhysLib + gtest_main +) + +target_include_directories(PhysLib_SingleBodySystemTests PRIVATE + ${CMAKE_CURRENT_SOURCE_DIR}/../Common +) + +include(GoogleTest) + +gtest_discover_tests( + PhysLib_SingleBodySystemTests + DISCOVERY_MODE PRE_TEST + DISCOVERY_TIMEOUT 10 +) diff --git a/PxMLib/PhysLib_Unit/SingleBodySystemTests/SingleBodySystemDynamicsTests.cpp b/PxMLib/PhysLib_Unit/SingleBodySystemTests/SingleBodySystemDynamicsTests.cpp new file mode 100644 index 00000000..0263345e --- /dev/null +++ b/PxMLib/PhysLib_Unit/SingleBodySystemTests/SingleBodySystemDynamicsTests.cpp @@ -0,0 +1,113 @@ +// PxM/PhysLib_Unit SingleBodySystemDynamicsTests.cpp +#include "TestHarness.h" + +#include "PhysLib.h" +#include "SingleBodySystems/Dynamics.h" + +#include "core/MathLib.h" +#include "integrators/numerical_integrators.h" + +namespace { + auto f = [](const mathlib::VecX& x, double t) -> mathlib::VecX { + mathlib::VecX dxdt(13); + dxdt.setZero(); + dxdt[3] = 1.0; // Constant velocity in x-direction + return dxdt; + }; +} + +// Test case for the derivatives function in SingleBodyDynamics +TEST("Single Body Dynamics Derivatives Method Test", SingleBodyDynamics_Derivatives) { + single_body_system::Body body; + body.inertia.mass = 1.0; + body.state.p = mathlib::Vec3(0.0, 0.0, 0.0); + body.state.pd = mathlib::Vec3(1.0, 0.0, 0.0); + body.state.q = mathlib::Quat::Identity(); + body.state.w = mathlib::Vec3(0.0, 0.0, 0.0); + mathlib::VecX x(13); + x.segment<3>(0) = body.state.p; + x.segment<3>(3) = body.state.pd; + x.segment<4>(6) = body.state.q.coeffs(); + x.segment<3>(10) = body.state.w; + mathlib::Vec3 F_ext(1.0, 0.0, 0.0); // External force in x-direction + mathlib::Vec3 tau_ext(0.0, 0.0, 1.0); // External torque around z-axis + double dt = 1.0; + single_body_system::dynamics::SingleBodyDynamics dynamics; + mathlib::VecX dxdt = dynamics.derivatives(body, x, F_ext, tau_ext, dt); + // Check that the derivatives are as expected + ASSERT_NEAR(dxdt[3], 1.0, 1e-6); // Velocity in x-direction should be constant + ASSERT_NEAR(dxdt[4], 0.0, 1e-6); // Velocity in y-direction should be zero + ASSERT_NEAR(dxdt[5], 0.0, 1e-6); // Velocity in z-direction should be zero +} +// Test case for the buildInertiaTensor function in SingleBodyDynamics +TEST("Single Body Dynamics Build Inertia Tensor Method Test", SingleBodyDynamics_BuildInertiaTensor) { + single_body_system::Body body; + body.inertia.mass = 2.0; + body.inertia.com_xyz = mathlib::Vec3(1.0, 1.0, 1.0); + single_body_system::dynamics::SingleBodyDynamics dynamics; + dynamics.buildInertiaTensor(body); + // Check that the inertia tensor is computed correctly + mathlib::Mat3 expectedInertia; + expectedInertia << (1.0 / 12.0) * body.inertia.mass * (body.inertia.com_xyz.y() * body.inertia.com_xyz.y() + body.inertia.com_xyz.z() * body.inertia.com_xyz.z()), 0, 0, + 0, (1.0 / 12.0)* body.inertia.mass* (body.inertia.com_xyz.x() * body.inertia.com_xyz.x() + body.inertia.com_xyz.z() * body.inertia.com_xyz.z()), 0, + 0, 0, (1.0 / 12.0)* body.inertia.mass* (body.inertia.com_xyz.x() * body.inertia.com_xyz.x() + body.inertia.com_xyz.y() * body.inertia.com_xyz.y()); + ASSERT_TRUE(body.inertia.inertiaTensor.isApprox(expectedInertia, 1e-6), "Inertia tensor is not computed correctly"); +} +// Test case for the buildParticleInertia function in SingleBodyDynamics +TEST("Single Body Dynamics Build Particle Inertia Method Test", SingleBodyDynamics_BuildParticleInertia) { + single_body_system::Body body; + body.inertia.mass = 1.0; + single_body_system::dynamics::SingleBodyDynamics dynamics; + dynamics.buildParticleInertia(body); + // Check that the inertia tensor is zero for a particle + ASSERT_TRUE(body.inertia.inertiaTensor.isZero(1e-6), "Inertia tensor for a particle should be zero"); +} +// Test case for the jacobian function in SingleBodyDynamics +TEST("Single Body Dynamics Jacobian Method Test", SingleBodyDynamics_Jacobian) { + single_body_system::Body body; + body.inertia.mass = 1.0; + body.state.p = mathlib::Vec3(0.0, 0.0, 0.0); + body.state.pd = mathlib::Vec3(1.0, 0.0, 0.0); + body.state.q = mathlib::Quat::Identity(); + body.state.w = mathlib::Vec3(0.0, 0.0, 0.0); + mathlib::VecX x(13); + x.segment<3>(0) = body.state.p; + x.segment<3>(3) = body.state.pd; + x.segment<4>(6) = body.state.q.coeffs(); + x.segment<3>(10) = body.state.w; + mathlib::MatX J_out(13, 13); + single_body_system::dynamics::SingleBodyDynamics dynamics; + dynamics.jacobian(body, x, J_out); + int rows = J_out.rows(); + int cols = J_out.cols(); + + // Check that the Jacobian is computed correctly (this is a simple check for the structure of the Jacobian) + ASSERT_TRUE(rows == 13, "Jacobian should have 13 rows"); + ASSERT_TRUE(cols == 13, "Jacobian should have 13 columns"); +} +// Test case for the derivatives function with zero external forces and torques +TEST("Single Body Dynamics Derivatives Method Test with Zero External Forces and Torques", SingleBodyDynamics_Derivatives_ZeroForcesTorques) { + single_body_system::Body body; + body.inertia.mass = 1.0; + body.state.p = mathlib::Vec3(0.0, 0.0, 0.0); + body.state.pd = mathlib::Vec3(1.0, 0.0, 0.0); + body.state.q = mathlib::Quat::Identity(); + body.state.w = mathlib::Vec3(0.0, 0.0, 0.0); + mathlib::VecX x(13); + x.segment<3>(0) = body.state.p; + x.segment<3>(3) = body.state.pd; + x.segment<4>(6) = body.state.q.coeffs(); + x.segment<3>(10) = body.state.w; + mathlib::Vec3 F_ext(0.0, 0.0, 0.0); // No external force + mathlib::Vec3 tau_ext(0.0, 0.0, 0.0); // No external torque + double dt = 1.0; + single_body_system::dynamics::SingleBodyDynamics dynamics; + mathlib::VecX dxdt = dynamics.derivatives(body, x, F_ext, tau_ext, dt); + // Check that the derivatives are as expected (no change in velocity or angular velocity) + ASSERT_NEAR(dxdt[3], 1.0, 1e-6); // Velocity in x-direction should remain constant + ASSERT_NEAR(dxdt[4], 0.0, 1e-6); // Velocity in y-direction should be zero + ASSERT_NEAR(dxdt[5], 0.0, 1e-6); // Velocity in z-direction should be zero + ASSERT_NEAR(dxdt[10], 0.0, 1e-6); // Angular velocity in x-direction should be zero + ASSERT_NEAR(dxdt[11], 0.0, 1e-6); // Angular velocity in y-direction should be zero + ASSERT_NEAR(dxdt[12], 0.0, 1e-6); // Angular velocity in z-direction should be zero +} diff --git a/PxMLib/PhysLib_Unit/SingleBodySystemTests/main.cpp b/PxMLib/PhysLib_Unit/SingleBodySystemTests/main.cpp new file mode 100644 index 00000000..505041d1 --- /dev/null +++ b/PxMLib/PhysLib_Unit/SingleBodySystemTests/main.cpp @@ -0,0 +1,32 @@ +// File: main.cpp +// GitHub: SaltyJoss +// Entry point for the integration test runner + +#include "TestHarness.h" + +#ifdef _WIN32 + #define NOMINMAX + #include +#endif + +int main() { +#ifdef _WIN32 + // Enable ANSI escape codes on Windows 10+ + HANDLE hOut = GetStdHandle(STD_OUTPUT_HANDLE); + DWORD mode = 0; + if (GetConsoleMode(hOut, &mode)) { + SetConsoleMode(hOut, mode | ENABLE_VIRTUAL_TERMINAL_PROCESSING); + } +#endif + + std::cout << "========================================\n"; + std::cout << " Single Body System Unit Tests\n"; + std::cout << "========================================\n"; + + int failures = test::runAll(); + + std::cout << "\nPress Enter to exit..."; + std::cin.get(); + + return failures; +} From 6f2a84bb43baca0eb55f0c9bb26b6555068aabdd Mon Sep 17 00:00:00 2001 From: saltyjoss Date: Sat, 13 Jun 2026 18:23:00 +0100 Subject: [PATCH 162/210] fixes: Updated logic to correctly compute derivatives in single body systems --- .../Include/SingleBodySystems/Dynamics.h | 53 ++++++++----------- .../SingleBodySystems/SingleBodySystem.h | 2 +- .../SingleBodySystemDynamicsTests.cpp | 4 +- 3 files changed, 26 insertions(+), 33 deletions(-) diff --git a/PxMLib/PhysLib/Include/SingleBodySystems/Dynamics.h b/PxMLib/PhysLib/Include/SingleBodySystems/Dynamics.h index d2ca1c70..db38c6aa 100644 --- a/PxMLib/PhysLib/Include/SingleBodySystems/Dynamics.h +++ b/PxMLib/PhysLib/Include/SingleBodySystems/Dynamics.h @@ -25,48 +25,39 @@ namespace single_body_system::dynamics { mathlib::VecX derivatives(Body& body, const mathlib::VecX& x, mathlib::Vec3& F_ext, mathlib::Vec3& tau_ext, double dt) { mathlib::VecX dxdt(13); + dxdt.setZero(); mathlib::Vec3 p = x.block<3, 1>(0, 0); mathlib::Vec3 pd = x.block<3, 1>(3, 0); - mathlib::Vec4 v = x.block<4, 1>(6, 0); - mathlib::Quat q_coeffs(v); + mathlib::Vec4 qv = x.block<4, 1>(6, 0); + mathlib::Quat q(qv); mathlib::Vec3 w = x.block<3, 1>(10, 0); - if (F_ext == mathlib::Vec3::Zero()) { body.state.pdd = F_ext; } - else { body.state.pdd = (F_ext / body.inertia.mass).eval(); } + const double m = body.inertia.mass; - // Update linear velocity and position - body.state.pd = body.state.pdd * dt; - body.state.p = body.state.pd * dt; + mathlib::Vec3 pdd; + if (m <= 1e-12) { pdd = mathlib::Vec3(0.0, 0.0, 0.0); } + else { pdd = F_ext / m; } - if (tau_ext == mathlib::Vec3::Zero()) { - body.state.wd = tau_ext; - body.state.w = tau_ext; - } - else { - mathlib::Mat3 inertiaInv = body.inertia.inertiaTensor.inverse(); - body.state.wd = inertiaInv * (tau_ext - body.state.w.cross(body.inertia.inertiaTensor * body.state.w)); - body.state.w = body.state.wd * dt; - } + mathlib::Vec3 wd = mathlib::Vec3::Zero(); + mathlib::Mat3 I = body.inertia.inertiaTensor; + mathlib::Mat3 I_inv = I.inverse(); - // Update orientation quaternion - if (q_coeffs.norm() > 1e-12) { - body.state.q = mathlib::Quat(q_coeffs).normalized(); - } - else { - body.state.q.coeffs() = mathlib::Quat(1.0, 0.0, 0.0, 0.0).coeffs(); + if (I.determinant() > 1e-12) { + wd = I_inv * (tau_ext - w.cross(I * w)); } - double theta = body.state.w.norm() * dt; - if (theta > 1e-12) { - mathlib::Quat dq(Eigen::AngleAxis(theta, body.state.w.normalized())); - body.state.q = (body.state.q * dq).normalized(); - } + dxdt.block<3, 1>(0, 0) = pd; // dp/dt = v + dxdt.block<3, 1>(3, 0) = pdd; // dpd/dt = a + + mathlib::Quat dq; + mathlib::Vec3 omega = w; + + mathlib::Quat omega_quat(0.0, omega.x(), omega.y(), omega.z()); + dq.coeffs() = 0.5 * (omega_quat * q).coeffs(); - dxdt.block<3, 1>(0, 0) = body.state.pd; // dp/dt = pd - dxdt.block<3, 1>(3, 0) = body.state.pdd; // dpd/dt = pdd - dxdt.block<4, 1>(6, 0) = body.state.q.coeffs(); // dq/dt = q (quaternion) - dxdt.block<3, 1>(10, 0) = body.state.w; // dw/dt = w + dxdt.block<4, 1>(6, 0) = dq.coeffs(); // dq/dt = 0.5 * q * w + dxdt.block<3, 1>(10, 0) = wd; return dxdt; } diff --git a/PxMLib/PhysLib/Include/SingleBodySystems/SingleBodySystem.h b/PxMLib/PhysLib/Include/SingleBodySystems/SingleBodySystem.h index d218396d..a9eca26b 100644 --- a/PxMLib/PhysLib/Include/SingleBodySystems/SingleBodySystem.h +++ b/PxMLib/PhysLib/Include/SingleBodySystems/SingleBodySystem.h @@ -9,7 +9,7 @@ using namespace constants; namespace single_body_system { // Inertia struct representing the mass and inertia tensor of a rigid body struct BodyInertia { - double mass = 0.0; + double mass = 1.0; mathlib::Vec3 com_xyz = mathlib::Vec3::Zero();; // Center of mass position in the body frame mathlib::Mat3 inertiaTensor = mathlib::Mat3::Zero();; // Inertia tensor in the body frame }; diff --git a/PxMLib/PhysLib_Unit/SingleBodySystemTests/SingleBodySystemDynamicsTests.cpp b/PxMLib/PhysLib_Unit/SingleBodySystemTests/SingleBodySystemDynamicsTests.cpp index 0263345e..7900dca3 100644 --- a/PxMLib/PhysLib_Unit/SingleBodySystemTests/SingleBodySystemDynamicsTests.cpp +++ b/PxMLib/PhysLib_Unit/SingleBodySystemTests/SingleBodySystemDynamicsTests.cpp @@ -7,6 +7,8 @@ #include "core/MathLib.h" #include "integrators/numerical_integrators.h" +#include + namespace { auto f = [](const mathlib::VecX& x, double t) -> mathlib::VecX { mathlib::VecX dxdt(13); @@ -104,7 +106,7 @@ TEST("Single Body Dynamics Derivatives Method Test with Zero External Forces and single_body_system::dynamics::SingleBodyDynamics dynamics; mathlib::VecX dxdt = dynamics.derivatives(body, x, F_ext, tau_ext, dt); // Check that the derivatives are as expected (no change in velocity or angular velocity) - ASSERT_NEAR(dxdt[3], 1.0, 1e-6); // Velocity in x-direction should remain constant + ASSERT_NEAR(dxdt[3], 0.0, 1e-6); // Velocity in x-direction should remain constant ASSERT_NEAR(dxdt[4], 0.0, 1e-6); // Velocity in y-direction should be zero ASSERT_NEAR(dxdt[5], 0.0, 1e-6); // Velocity in z-direction should be zero ASSERT_NEAR(dxdt[10], 0.0, 1e-6); // Angular velocity in x-direction should be zero From 5453401f07c2fec8b5c799fa5c79c6303f008aab Mon Sep 17 00:00:00 2001 From: saltyjoss Date: Tue, 16 Jun 2026 09:29:56 +0100 Subject: [PATCH 163/210] feat: Integrated basic pointer referencing of `SingleBodySystem` into `SimulationCore` and `SimManager` * I am thinking that a `Systems` header (and compiler?) may be beneficial to centralise ALL systems, then compute which internally using state callbacks. --- .../include/Platform/ISimulationCore.h | 5 + .../DSFE_Core/include/Scene/SimulationCore.h | 12 +++ .../DSFE_Core/include/SingleBodySystem/Body.h | 23 ++++- .../DSFE_Core/src/Scene/SimulationCore.cpp | 92 +++++++++++++------ .../DSFE_Core/src/SingleBodySystem/Body.cpp | 33 ++++++- .../include/Scene/Manager/SimImplementation.h | 4 + .../include/Scene/SimulationManager.h | 7 +- .../src/MainWindow/DSFE_MainWindow.cpp | 3 + .../DSFE_GUI/src/Scene/Manager/SimRobots.cpp | 5 + .../DSFE_GUI/src/Scene/SimulationManager.cpp | 20 ++++ 10 files changed, 170 insertions(+), 34 deletions(-) diff --git a/DSFE_App/DSFE_Core/include/Platform/ISimulationCore.h b/DSFE_App/DSFE_Core/include/Platform/ISimulationCore.h index 27b7b6da..dec47dd0 100644 --- a/DSFE_App/DSFE_Core/include/Platform/ISimulationCore.h +++ b/DSFE_App/DSFE_Core/include/Platform/ISimulationCore.h @@ -9,6 +9,7 @@ // Forward Declarations namespace integration { enum class eIntegrationMethod; enum class eAutoDiffIntegrationMethod; } namespace robots { class RobotSystem; } +namespace single_body_system { class SingleBodySystem; } namespace control { class TrajectoryManager; } namespace diagnostics { class TelemetryRecorder; } namespace interpreter { class IStoredProgram; } @@ -47,7 +48,11 @@ namespace core { virtual void setRunTag(const std::string& tag) = 0; // Subsystems virtual robots::RobotSystem* robotSystem() = 0; + virtual single_body_system::SingleBodySystem* singleBodySystem() = 0; virtual control::TrajectoryManager* trajectoryManager() = 0; + // Body management + virtual bool hasSingleBody() const = 0; + virtual void loadSingleBody(const std::string& name) = 0; // Robot management virtual bool hasRobot() const = 0; virtual void loadRobot(const std::string& name) = 0; diff --git a/DSFE_App/DSFE_Core/include/Scene/SimulationCore.h b/DSFE_App/DSFE_Core/include/Scene/SimulationCore.h index 22d6bc17..2da971e7 100644 --- a/DSFE_App/DSFE_Core/include/Scene/SimulationCore.h +++ b/DSFE_App/DSFE_Core/include/Scene/SimulationCore.h @@ -22,6 +22,7 @@ // Forward Declarations namespace control { class TrajectoryManager; } namespace robots { class RobotSystem; } +namespace single_body_system { class SingleBodySystem; } namespace interpreter { class IStoredProgram; } namespace core { @@ -77,9 +78,16 @@ namespace core { // Subsystems access robots::RobotSystem* robotSystem() override; const robots::RobotSystem* robotSystem() const; + single_body_system::SingleBodySystem* singleBodySystem() override; + const single_body_system::SingleBodySystem* singleBodySystem() const; control::TrajectoryManager* trajectoryManager() override; const control::TrajectoryManager* trajectoryManager() const; + // Body management + bool hasSingleBody() const override; + void loadSingleBody(const std::string& name) override; + void loadSingleBodyInternal(const std::string& name); // Internal method that assumes ownership + // Robot management bool hasRobot() const override; void loadRobot(const std::string& name) override; @@ -95,6 +103,7 @@ namespace core { // Setters for subsystems and scene objects void setRobotSystem(robots::RobotSystem* robot); + void setSingleBodySystem(single_body_system::SingleBodySystem* singleBody); void setTrajectoryManager(control::TrajectoryManager* traj); void setJointLogBuffer(robots::JointLogBuffer* buffer); void setTrajRefBuffer(robots::TrajRefBuffer* buffer); @@ -146,11 +155,13 @@ namespace core { // Owning storage (used only in owning mode) // std::unique_ptr>> _objectsOwned; std::unique_ptr _robotOwned; + std::unique_ptr _singleBodyOwned; std::unique_ptr _trajOwned; // Non-owning access (always used by logic) // std::vector>* _objects = nullptr; robots::RobotSystem* _robot = nullptr; + single_body_system::SingleBodySystem* _singleBody = nullptr; control::TrajectoryManager* _traj = nullptr; mutable std::mutex _stateMutex; @@ -175,6 +186,7 @@ namespace core { // Active Script Program interpreter::IStoredProgram* _activeProgram = nullptr; bool _robotPresentationDirty = false; + bool _singleBodyPresentationDirty = false; // Telemetry diagnostics::TelemetryRecorder _telemetry; // Dynamic telemetry recorder diff --git a/DSFE_App/DSFE_Core/include/SingleBodySystem/Body.h b/DSFE_App/DSFE_Core/include/SingleBodySystem/Body.h index 72b1c0df..3b4224e0 100644 --- a/DSFE_App/DSFE_Core/include/SingleBodySystem/Body.h +++ b/DSFE_App/DSFE_Core/include/SingleBodySystem/Body.h @@ -9,16 +9,21 @@ #include "Analysis/MetricLogger.h" namespace single_body_system { - class SingleBodySystem { // FOR NOW, this represents a single RIGID BODY, but in the near-near-near-future I will extend this for particles (or make a separate ParticleSystem class, changing this to RigidBodySystem) + class DSFE_API SingleBodySystem { // FOR NOW, this represents a single RIGID BODY, but in the near-near-near-future I will extend this for particles (or make a separate ParticleSystem class, changing this to RigidBodySystem) public: SingleBodySystem(); - ~SingleBodySystem(); + ~SingleBodySystem() = default; void step(double dt, double t); Body* body() { return _body; } // return pointer to the body const Body* body() const { return _body; } // return const pointer to the body + void loadBody(const std::string& name); // this does not load the actual graphical model, just the physical properties of the body (mass, inertia, etc.) from the stl file or other source + void resetBody(); // reset the body to its initial state (position, orientation, velocity, etc.) + + bool hasBody() const { return _body != nullptr; } + integration::eIntegrationMethod getIntegrationMethod() const { return _curIntMethod; } std::string getIntegratorName() const { return _integrator->IntegratorName(_curIntMethod); } void setStandardIntegrator(integration::eIntegrationMethod m) { _curIntMethod = m; } @@ -27,6 +32,18 @@ namespace single_body_system { std::string AD_integratorName() const { return _AD_integrator->IntegratorName(_curIntMethod_AD); } void setADIntegrator(integration::eAutoDiffIntegrationMethod m) { _curIntMethod_AD = m; } + integration::IntegrationService* getIntegrator(); + const integration::IntegrationService* getIntegrator() const; + + integration::DifferentiableIntegrator* getADIntegrator(); + const integration::DifferentiableIntegrator* getADIntegrator() const; + + bool autoDiffEnabled() const { return _useAutoDiff; } + void enableAutoDiff(bool enable) { _useAutoDiff = enable; } + + std::shared_ptr runtimeIntegratorState(); + std::shared_ptr runtimeIntegratorState() const; + private: mathlib::VecX packState() const; void unpackState(const mathlib::VecX& s); @@ -42,6 +59,8 @@ namespace single_body_system { mathlib::Vec3 _F_ext{ 0.0, 0.0, 0.0 }; mathlib::Vec3 _tau_ext{ 0.0, 0.0, 0.0 }; + bool _useAutoDiff = false; + // precomputed clamp lookup tables mutable std::vector _clampVel; mutable std::vector _clampAngVel; diff --git a/DSFE_App/DSFE_Core/src/Scene/SimulationCore.cpp b/DSFE_App/DSFE_Core/src/Scene/SimulationCore.cpp index f0d18b8f..c7f9880b 100644 --- a/DSFE_App/DSFE_Core/src/Scene/SimulationCore.cpp +++ b/DSFE_App/DSFE_Core/src/Scene/SimulationCore.cpp @@ -5,6 +5,7 @@ #include "Robots/RobotSystem.h" #include "Robots/RobotModel.h" #include "Robots/TrajectoryManager.h" +#include "SingleBodySystem/Body.h" #include "Interpreter/StoredProgram.h" #include "Interpreter/Parser.h" @@ -15,10 +16,12 @@ namespace core { // Owned constructed subsystems (default) SimulationCore::SimulationCore() - : _trajOwned(std::make_unique()), _robotOwned(std::make_unique()) + : _trajOwned(std::make_unique()), _robotOwned(std::make_unique()), + _singleBodyOwned(std::make_unique()) { _traj = _trajOwned.get(); _robot = _robotOwned.get(); + _singleBody = _singleBodyOwned.get(); startExportThread(); } @@ -30,15 +33,23 @@ namespace core { // Non-owning constructor (used when subsystems are managed externally, e.g. by the SimulationManager) SimulationCore::SimulationCore(robots::RobotSystem& robot, control::TrajectoryManager& traj) - : _robot(&robot), _traj(&traj) { + : _robot(&robot), _traj(&traj), _singleBody(nullptr) { startExportThread(); } // Simulation System void SimulationCore::setupSimulationIntegrator() { - if (!_robot) { return; } - auto* intgr = _robot->getIntegrator(); - auto* adIntgr = _robot->getADIntegrator(); + if (!_robot && !_singleBody) { return; } + integration::IntegrationService* intgr; + integration::DifferentiableIntegrator* adIntgr; + if (_robot) { + intgr = _robot->getIntegrator(); + adIntgr = _robot->getADIntegrator(); + } + if (_singleBody) { + intgr = _singleBody->getIntegrator(); + adIntgr = _singleBody->getADIntegrator(); + } intgr->resetAdaptiveState(); intgr->setAdaptiveTolerances(1e-3, 1e-6); intgr->setMaxStep(_dt); @@ -47,37 +58,39 @@ namespace core { } // Set the integration method for the simulation (also updates the robot's integrator if it exists) void SimulationCore::setIntegrationMethod(integration::eIntegrationMethod method) { - if (!_robot) { return; } - _robot->setStandardIntegrator(method); + if (!_robot && !_singleBody) { return; } + if (_singleBody) { _singleBody->setStandardIntegrator(method); } + if (_robot) { _robot->setStandardIntegrator(method); } } // Set the auto-diff integration method for the simulation (also updates the robot's AD integrator if it exists) void SimulationCore::setADIntegrationMethod(integration::eAutoDiffIntegrationMethod method) { - if (!_robot) { return; } - _robot->setADIntegrator(method); + if (!_robot && !_singleBody) { return; } + if (_singleBody) { _singleBody->setADIntegrator(method); } + if (_robot) { _robot->setADIntegrator(method); } } // Get the name of the current integration method (returns "no_robot" if no robot is loaded) std::string SimulationCore::integrationMethodName() const { - if (!_robot) { return "no_robot"; } std::string intName; - if (_robot->autoDiffEnabled()) { - intName = _robot->getIntegratorName(); - } - else { - intName = _robot->AD_integratorName(); + if (_robot) { + if (_robot->autoDiffEnabled()) { intName = _robot->getIntegratorName(); } + else { intName = _robot->AD_integratorName(); } } + else if(_singleBody) { intName = _singleBody->getIntegratorName(); } + else { intName = "no_system"; } LOG_INFO("Integration Method: %s", intName.c_str()); - return intName; } // Get the current integration method integration::eIntegrationMethod SimulationCore::integrationMethod() const { - if (!_robot) { return integration::eIntegrationMethod::RK4; } - return _robot->getIntegrationMethod(); + if (_robot) { return _robot->getIntegrationMethod(); } + if (_singleBody) { return _singleBody->getIntegrationMethod(); } + return integration::eIntegrationMethod::RK4; } // Get the current auto-diff integration method integration::eAutoDiffIntegrationMethod SimulationCore::autoDiffIntegrationMethod() const { - if (!_robot) { return integration::eAutoDiffIntegrationMethod::AD_ImplicitEuler; } - return _robot->AD_IntegrationMethod(); + if (_robot) { return _robot->AD_IntegrationMethod(); } + if (_singleBody) { return _singleBody->AD_IntegrationMethod(); } + return integration::eAutoDiffIntegrationMethod::AD_ImplicitEuler; } void SimulationCore::enableAutoDiff(bool enable) { if (!_robot) { return; } @@ -135,6 +148,7 @@ namespace core { } _telemetry.update(simTime, *_robot, _traj, diagnostics::eTelemetryLevel::FULL); } + if (hasSingleBody()) { _singleBody->step(_dt, simTime); } } _accum -= _dt; // decrease accumulator by fixed timestep until we catch up to the current frame time } @@ -157,6 +171,8 @@ namespace core { _simTime.store(0.0, std::memory_order_relaxed); _accum = 0.0; + std::string intName; + // Reset simulation system if (_robot) { _robot->resetRobot(); @@ -182,11 +198,16 @@ namespace core { _trajRefBuffer.clear(); _trajRefBuffer.reserve(std::max(1024, total / (26 / 5))); // 26 to 5 entries, so reserving 1/(26/5) of total steps as a heuristic for ref buffer size _robot->setRefBuffer(&_trajRefBuffer); - } - - const auto state = _robot->runtimeIntegratorState(); - std::string intName = (_robot->autoDiffEnabled()) ? _robot->AD_integratorName() : _robot->getIntegratorName(); + const auto state = _robot->runtimeIntegratorState(); + intName = (_robot->autoDiffEnabled()) ? _robot->AD_integratorName() : _robot->getIntegratorName(); + } + if (_singleBody) { + _singleBody->resetBody(); + const auto state = _singleBody->runtimeIntegratorState(); + intName = (_singleBody->autoDiffEnabled()) ? _singleBody->AD_integratorName() : _singleBody->getIntegratorName(); + } + _data.setParentFolder(paths::runs().string()); // Ensure reference sim system have their integrators configured for the new run @@ -412,12 +433,31 @@ namespace core { loadRobotInternal(name); _robotPresentationDirty = true; } - + // Internal method to load a robot, assumes ownership of the robot system void SimulationCore::loadRobotInternal(const std::string& name) { if (!_robot) { LOG_ERROR("Cannot load robot: RobotSystem not set"); return; } _robot->loadRobot(name); } + // Accessor for the single body system (non-const and const versions) + single_body_system::SingleBodySystem* SimulationCore::singleBodySystem() { return _singleBody; } + const single_body_system::SingleBodySystem* SimulationCore::singleBodySystem() const { return _singleBody; } + + // Setter and checker for Single Body System + void SimulationCore::setSingleBodySystem(single_body_system::SingleBodySystem* singleBody) { _singleBody = singleBody; } + bool SimulationCore::hasSingleBody() const { return _singleBody && _singleBody->hasBody(); } + + // Loads a single body into the single body system by name + void SimulationCore::loadSingleBody(const std::string& name) { + loadSingleBodyInternal(name); + _singleBodyPresentationDirty = true; + } + // Internal method to load a single body, assumes ownership of the single body system + void SimulationCore::loadSingleBodyInternal(const std::string& name) { + if (!_singleBody) { LOG_ERROR("Cannot load single body: SingleBodySystem not set"); return; } + _singleBody->loadBody(name); + } + // Setter for the trajectory manager void SimulationCore::setTrajectoryManager(control::TrajectoryManager* traj) { _traj = traj; } @@ -440,8 +480,6 @@ namespace core { void SimulationCore::setActiveProgram(interpreter::IStoredProgram* p) { _activeProgram = p; } interpreter::IStoredProgram* SimulationCore::activeProgram() const { return _activeProgram; } - // Setter for the - // Thread-based methods // Starts the export thread if it's not already running diff --git a/DSFE_App/DSFE_Core/src/SingleBodySystem/Body.cpp b/DSFE_App/DSFE_Core/src/SingleBodySystem/Body.cpp index 00b91dd0..60fabd9e 100644 --- a/DSFE_App/DSFE_Core/src/SingleBodySystem/Body.cpp +++ b/DSFE_App/DSFE_Core/src/SingleBodySystem/Body.cpp @@ -44,11 +44,11 @@ namespace single_body_system { // TODO: Check the correct way to clamp the velocities close the S.o.L // * Not really sure if I have done this correctly if (pdMax_hpd > 0.0) { - if (pdMax_hpd == constants::c_0) { std::clamp(pd_i, -pdMax_hpd, pdMax_hpd); } + if (pdMax_hpd == constants::c_0) { pd_i_out = std::clamp(pd_i, -pdMax_hpd, pdMax_hpd); } else if (std::abs(pd_i) > (1.0 + eps) * pdMax_hpd) { pd_i_out = std::clamp(pd_i, -pdMax_hpd, pdMax_hpd); } } if (wMax_hw > 0.0) { - if (wMax_hw == constants::c_0) { std::clamp(w_i, -wMax_hw, wMax_hw); } + if (wMax_hw == constants::c_0) { w_i_out = std::clamp(w_i, -wMax_hw, wMax_hw); } else if (std::abs(w_i) > (1.0 + eps) * wMax_hw) { w_i_out = std::clamp(w_i, -wMax_hw, wMax_hw); } } @@ -76,11 +76,36 @@ namespace single_body_system { // Not going to use the computeDynamics methods initially, just want a straight cut test first. auto f_deriv = [&](auto t, const auto& x) { return _dynamics->derivatives(*_body, x, _F_ext, _tau_ext, dt); }; auto f_jac = [&](const auto& x, auto& J_out) { _dynamics->jacobian(*_body, x, J_out); }; - auto step = _integrator->step(_curIntMethod, x, t, dt, f_deriv, f_jac); // Wont work with Implicit since no jacobian provided, but will work with RK4 and other explicit methods - // Also will not with adaptive (probably) since no rtol/atol provided, but will work with fixed step methods VecX x_next = step.x_next; unpackState(x); + + for (int i = 0; i < x_next.size(); ++i) { + const auto v = mathlib::real(x_next[i]); + if (std::isnan(v) || std::isinf(v)) { LOG_ERROR("Non-finite x_next[%d] = %f", i, (double)v); } + } + } + + void SingleBodySystem::loadBody(const std::string& name) { + if (!_body) { LOG_ERROR("Body not initialised!"); return; } + _body->name = name; } + + void SingleBodySystem::resetBody() { + if (!_body) { LOG_ERROR("Body not initialised!"); return; } + _body->state.p = mathlib::Vec3::Zero(); + _body->state.pd = mathlib::Vec3::Zero(); + _body->state.q = mathlib::Quat::Identity(); + _body->state.w = mathlib::Vec3::Zero(); + } + + integration::IntegrationService* SingleBodySystem::getIntegrator() { return _integrator.get(); } + const integration::IntegrationService* SingleBodySystem::getIntegrator() const { return _integrator.get(); } + + integration::DifferentiableIntegrator* SingleBodySystem::getADIntegrator() { return _AD_integrator.get(); } + const integration::DifferentiableIntegrator* SingleBodySystem::getADIntegrator() const { return _AD_integrator.get(); } + + std::shared_ptr SingleBodySystem::runtimeIntegratorState() { return _useAutoDiff ? _AD_integrator->runtimeState() : _integrator->runtimeState(); } + std::shared_ptr SingleBodySystem::runtimeIntegratorState() const { return _useAutoDiff ? _AD_integrator->runtimeState() : _integrator->runtimeState(); } } \ No newline at end of file diff --git a/DSFE_App/DSFE_GUI/include/Scene/Manager/SimImplementation.h b/DSFE_App/DSFE_GUI/include/Scene/Manager/SimImplementation.h index 2bd91cdb..0437666a 100644 --- a/DSFE_App/DSFE_GUI/include/Scene/Manager/SimImplementation.h +++ b/DSFE_App/DSFE_GUI/include/Scene/Manager/SimImplementation.h @@ -19,6 +19,7 @@ #include "Robots/RobotSystem.h" #include "Robots/RobotModel.h" #include "Robots/TrajectoryManager.h" +#include "SingleBodySystem/Body.h" #include "Rendering/SkyboxRenderer.h" #include "Rendering/ShaderUtil.h" @@ -109,6 +110,8 @@ namespace gui { // Robot System std::unique_ptr _robotSystem; // simulation + // Single Body System + std::unique_ptr _singleBody; // simulation // Robot Rendering std::unique_ptr _robotRenderer; // rendering RobotRenderBinding _currentBinding; // current render binding @@ -135,6 +138,7 @@ namespace gui { // Robot system with mesh loading (for normal simulation) _robotSystem = std::make_unique(); + _singleBody = std::make_unique(); } void initGLResources(SimManager& owner) { diff --git a/DSFE_App/DSFE_GUI/include/Scene/SimulationManager.h b/DSFE_App/DSFE_GUI/include/Scene/SimulationManager.h index 4da0d6cf..535ba764 100644 --- a/DSFE_App/DSFE_GUI/include/Scene/SimulationManager.h +++ b/DSFE_App/DSFE_GUI/include/Scene/SimulationManager.h @@ -176,6 +176,7 @@ namespace gui { void setPresentationFBO(GLuint fbo) { _presentationFBO = fbo; } void syncRobotToScene(); + void syncBodyToScene(); // Scene Objects Management void setSelectedObject(scene::Object* obj); @@ -194,16 +195,20 @@ namespace gui { void clearRobot(); const bool hasRobot() const; + const bool hasBody() const; + // Setters for robot joint states (angle in radians) void setRobotLinkRotation(const std::string& linkName, double angle); void setRobotRootPose(const mathlib::Vec3& pos, mathlib::Quat& rot); void setRobotRootHome(const mathlib::Vec3& pos, mathlib::Quat& rot); - // Accesors for the robot system (non-const and const versions) robots::RobotSystem* robotSystem(); const robots::RobotSystem* robotSystem() const; + single_body_system::SingleBodySystem* singleBodySystem(); + const single_body_system::SingleBodySystem* singleBodySystem() const; + // Accessors for the trajectory manager (non-const and const versions) control::TrajectoryManager* traj(); const control::TrajectoryManager* traj() const; diff --git a/DSFE_App/DSFE_GUI/src/MainWindow/DSFE_MainWindow.cpp b/DSFE_App/DSFE_GUI/src/MainWindow/DSFE_MainWindow.cpp index afd39d0a..2fa785e9 100644 --- a/DSFE_App/DSFE_GUI/src/MainWindow/DSFE_MainWindow.cpp +++ b/DSFE_App/DSFE_GUI/src/MainWindow/DSFE_MainWindow.cpp @@ -128,6 +128,9 @@ namespace window { LOG_INFO("Menu clicked: Project -> Load Mesh"); QString path = QFileDialog::getOpenFileName(nullptr, "Select Mesh File", QString::fromStdString((paths::assets() / "objects" / "Shapes").string()), "Mesh Files(*.obj * .fbx * .gltf * .dae * .stl)"); if (path.isEmpty()) { return; } + // Covert path name to just the file name without extension or path + std::string bodyName = QFileInfo(path).baseName().toStdString(); + _sim->simCore()->loadSingleBody(bodyName); _sim->loadMesh(path.toStdString()); }); auto* loadHDRAction = projectMenu->addAction("Load HDRI"); diff --git a/DSFE_App/DSFE_GUI/src/Scene/Manager/SimRobots.cpp b/DSFE_App/DSFE_GUI/src/Scene/Manager/SimRobots.cpp index 4a0555aa..51c977e9 100644 --- a/DSFE_App/DSFE_GUI/src/Scene/Manager/SimRobots.cpp +++ b/DSFE_App/DSFE_GUI/src/Scene/Manager/SimRobots.cpp @@ -68,11 +68,16 @@ namespace gui { _impl->eeObject = nullptr; } const bool SimManager::hasRobot() const { return _impl->_robotSystem && _impl->_robotSystem->hasRobot(); } + const bool SimManager::hasBody() const { return _impl->_singleBody && _impl->_singleBody->hasBody(); } // Access the robot system (non-const and const versions) robots::RobotSystem* SimManager::robotSystem() { return _core->robotSystem(); } const robots::RobotSystem* SimManager::robotSystem() const { return _core->robotSystem(); } + // Access the single body system (non-const and const versions) + single_body_system::SingleBodySystem* SimManager::singleBodySystem() { return _core->singleBodySystem(); } + const single_body_system::SingleBodySystem* SimManager::singleBodySystem() const { return _core->singleBodySystem(); } + // Access the trajectory manager (non-const and const versions) control::TrajectoryManager* SimManager::traj() { return _core->trajectoryManager(); } const control::TrajectoryManager* SimManager::traj() const { return _core->trajectoryManager(); } diff --git a/DSFE_App/DSFE_GUI/src/Scene/SimulationManager.cpp b/DSFE_App/DSFE_GUI/src/Scene/SimulationManager.cpp index 281ee456..58c1dbb6 100644 --- a/DSFE_App/DSFE_GUI/src/Scene/SimulationManager.cpp +++ b/DSFE_App/DSFE_GUI/src/Scene/SimulationManager.cpp @@ -12,6 +12,7 @@ extern "C" core::ISimulationCore* CreateSimulationCore_v1(); extern "C" void DestroySimulationCore(core::ISimulationCore*); #include "Manager/SimImplementation.h" +#include "SingleBodySystems/SingleBodySystem.h" #include "Platform/KeyCode.h" #include @@ -210,6 +211,25 @@ namespace gui { } } + void SimManager::syncBodyToScene() { + if (!hasBody()) { return; } + auto* sys = singleBodySystem(); + const auto& body = sys->body(); + ; + auto it = _impl->_linkToObjects.find(body->name); + if (it == _impl->_linkToObjects.end()) return; + + // For a single body, world transform are redundant, in fact the body->transform is already in world space so we can + auto& state = body->state; + for (scene::Object* obj : it->second) { + if (!obj) { continue; } + glm::vec3 pos = toGlm(state.p); + glm::quat q = toGlm(state.q); + obj->transform.position = pos; + obj->transform.rotQ = q; + } + } + void SimManager::resize(int32_t width, int32_t height) { if (width <= 0 || height <= 0) return; _internalSize = { (float)width, (float)height }; From beccf8ee1198afa8ebfb922d2e7a965f1671b14b Mon Sep 17 00:00:00 2001 From: saltyjoss Date: Tue, 16 Jun 2026 11:42:32 +0100 Subject: [PATCH 164/210] refactor: Moved to `CMakePresets` focused build system, preparing for eventual extension to linux-based OS operation. --- .gitignore | 3 + CMakeLists.txt | 9 +- CMakePresets.json | 106 +++++++++--------- DSFE_App/CMakeLists.txt | 6 +- DSFE_App/DSFE_Core/CMakeLists.txt | 3 +- DSFE_App/DSFE_Engine/CMakeLists.txt | 2 - DSFE_App/DSFE_GUI/CMakeLists.txt | 6 +- .../DSFE_GUI/src/Scene/SimulationManager.cpp | 3 - .../CMakeLists.txt | 3 - PxMLib/MathLib_Unit/CMakeLists.txt | 3 - .../DualNumberTests/CMakeLists.txt | 3 - .../IntegratorTests/CMakeLists.txt | 3 - PxMLib/PhysLib_Unit/CMakeLists.txt | 3 - .../SingleBodySystemTests/CMakeLists.txt | 3 - 14 files changed, 68 insertions(+), 88 deletions(-) diff --git a/.gitignore b/.gitignore index a58b3aa9..6cdcb229 100644 --- a/.gitignore +++ b/.gitignore @@ -476,3 +476,6 @@ vcpkg_downloads/ !CMakeLists.txt !**/imgui.ini +/build-ninja +/CMakeFiles +/_deps diff --git a/CMakeLists.txt b/CMakeLists.txt index 2a712153..427e7beb 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -1,16 +1,15 @@ # CMakeLists.txt for DSFE project (root CMakeLists.txt) cmake_minimum_required(VERSION 3.17) - project(DSFE LANGUAGES C CXX) set(CMAKE_CXX_STANDARD 23) set(CMAKE_CXX_STANDARD_REQUIRED ON) set(CMAKE_MSVC_RUNTIME_LIBRARY "MultiThreaded$<$:Debug>DLL") -# Set common output directories for all targets -set(CMAKE_ARCHIVE_OUTPUT_DIRECTORY ${CMAKE_BINARY_DIR}/lib) -set(CMAKE_LIBRARY_OUTPUT_DIRECTORY ${CMAKE_BINARY_DIR}/lib) -set(CMAKE_RUNTIME_OUTPUT_DIRECTORY ${CMAKE_BINARY_DIR}/bin) +set(OUTPUT_BASE ${CMAKE_BINARY_DIR}) +set(CMAKE_RUNTIME_OUTPUT_DIRECTORY ${OUTPUT_BASE}/bin) +set(CMAKE_LIBRARY_OUTPUT_DIRECTORY ${OUTPUT_BASE}/lib) +set(CMAKE_ARCHIVE_OUTPUT_DIRECTORY ${OUTPUT_BASE}/lib) include(FetchContent) diff --git a/CMakePresets.json b/CMakePresets.json index 6c6c0187..6780ead5 100644 --- a/CMakePresets.json +++ b/CMakePresets.json @@ -1,81 +1,87 @@ { - "version": 3, + "version": 8, "configurePresets": [ { - "name": "windows-base", + "name": "base", "hidden": true, - "generator": "Ninja", "binaryDir": "${sourceDir}/out/build/${presetName}", - "installDir": "${sourceDir}/out/install/${presetName}", "cacheVariables": { - "CMAKE_C_COMPILER": "cl.exe", - "CMAKE_CXX_COMPILER": "cl.exe" - }, + "CMAKE_EXPORT_COMPILE_COMMANDS": "ON" + } + }, + { + "name": "windows-x64", + "displayName": "Windows x64", + "inherits": "base", "condition": { "type": "equals", "lhs": "${hostSystemName}", "rhs": "Windows" - } - }, - { - "name": "x64-debug", - "displayName": "x64 Debug", - "inherits": "windows-base", + }, + "generator": "Visual Studio 18 2026", + "cacheVariables": { + "CMAKE_PREFIX_PATH": "C:/Qt/6.11.1/msvc2022_64" + }, "architecture": { "value": "x64", "strategy": "external" - }, - "cacheVariables": { - "CMAKE_BUILD_TYPE": "Debug" } }, { - "name": "x64-release", - "displayName": "x64 Release", - "inherits": "x64-debug", - "cacheVariables": { - "CMAKE_BUILD_TYPE": "Release" - } - }, - { - "name": "x86-debug", - "displayName": "x86 Debug", - "inherits": "windows-base", - "architecture": { - "value": "x86", - "strategy": "external" + "name": "linux-x64-debug", + "displayName": "Linux x64 Debug", + "inherits": "base", + "condition": { + "type": "equals", + "lhs": "${hostSystemName}", + "rhs": "Linux" }, + "generator": "Ninja", "cacheVariables": { "CMAKE_BUILD_TYPE": "Debug" + }, + "architecture": { + "value": "x64", + "strategy": "external" } }, { - "name": "x86-release", - "displayName": "x86 Release", - "inherits": "x86-debug", - "cacheVariables": { - "CMAKE_BUILD_TYPE": "Release" - } - }, - { - "name": "linux-debug", - "displayName": "Linux Debug", - "generator": "Ninja", - "binaryDir": "${sourceDir}/out/build/${presetName}", - "installDir": "${sourceDir}/out/install/${presetName}", - "cacheVariables": { - "CMAKE_BUILD_TYPE": "Debug" - }, + "name": "linux-x64-release", + "displayName": "Linux x64 Release", + "inherits": "base", "condition": { "type": "equals", "lhs": "${hostSystemName}", "rhs": "Linux" }, - "vendor": { - "microsoft.com/VisualStudioRemoteSettings/CMake/1.0": { - "sourceDir": "$env{HOME}/.vs/$ms{projectDirName}" - } + "generator": "Ninja", + "cacheVariables": { + "CMAKE_BUILD_TYPE": "Release" + }, + "architecture": { + "value": "x64", + "strategy": "external" } } + ], + "buildPresets": [ + { + "name": "windows-x64-debug", + "configurePreset": "windows-x64", + "configuration": "Debug" + }, + { + "name": "windows-x64-release", + "configurePreset": "windows-x64", + "configuration": "Release" + }, + { + "name": "linux-x64-debug", + "configurePreset": "linux-x64-debug" + }, + { + "name": "linux-x64-release", + "configurePreset": "linux-x64-release" + } ] } diff --git a/DSFE_App/CMakeLists.txt b/DSFE_App/CMakeLists.txt index 6f40143c..6c33a113 100644 --- a/DSFE_App/CMakeLists.txt +++ b/DSFE_App/CMakeLists.txt @@ -1,8 +1,4 @@ -# CMakeLists.txt for the DSFE_App project -cmake_minimum_required(VERSION 3.17) -project(DSFE_App LANGUAGES C CXX) - -# Add projects within the DSFE_App directory +# CMakeLists.txt for the DSFE_App add_subdirectory(DSFE_Core) add_subdirectory(DSFE_GUI) add_subdirectory(DSFE_Engine) diff --git a/DSFE_App/DSFE_Core/CMakeLists.txt b/DSFE_App/DSFE_Core/CMakeLists.txt index d8f452b8..60357fd8 100644 --- a/DSFE_App/DSFE_Core/CMakeLists.txt +++ b/DSFE_App/DSFE_Core/CMakeLists.txt @@ -9,7 +9,7 @@ find_package(HDF5 CONFIG REQUIRED) target_compile_definitions(DSFE_Core PRIVATE DSFE_CORE_EXPORTS) if (MSVC) -target_compile_options(DSFE_Core PRIVATE /bigobj) + target_compile_options(DSFE_Core PRIVATE /bigobj) endif() set(DEP_CORE_SRC @@ -86,7 +86,6 @@ endif() target_include_directories(DSFE_Core PUBLIC ${CMAKE_CURRENT_SOURCE_DIR} ${CMAKE_CURRENT_SOURCE_DIR}/include - ${HDF5_INCLUDE_DIRS} ) diff --git a/DSFE_App/DSFE_Engine/CMakeLists.txt b/DSFE_App/DSFE_Engine/CMakeLists.txt index 654bfe42..50beccb0 100644 --- a/DSFE_App/DSFE_Engine/CMakeLists.txt +++ b/DSFE_App/DSFE_Engine/CMakeLists.txt @@ -1,6 +1,4 @@ # CMakeLists.txt for the DSFE_Engine executable -cmake_minimum_required(VERSION 3.17) -project(DSFE_Engine LANGUAGES C CXX) find_package(Threads REQUIRED) diff --git a/DSFE_App/DSFE_GUI/CMakeLists.txt b/DSFE_App/DSFE_GUI/CMakeLists.txt index 031a2de9..92c52f02 100644 --- a/DSFE_App/DSFE_GUI/CMakeLists.txt +++ b/DSFE_App/DSFE_GUI/CMakeLists.txt @@ -1,13 +1,13 @@ # CMakeLists.txt for DSFE_GUI module -set(CMAKE_POSITION_INDEPENDENT_CODE ON) - cmake_minimum_required(VERSION 3.17) project(DSFE_GUI LANGUAGES C CXX) +set(CMAKE_POSITION_INDEPENDENT_CODE ON) + set(CMAKE_AUTOMOC ON) set(CMAKE_AUTOUIC ON) set(CMAKE_AUTORCC ON) -set(CMAKE_PREFIX_PATH "C:/Qt/6.11.1/msvc2022_64") +#set(CMAKE_PREFIX_PATH "C:/Qt/6.11.1/msvc2022_64") add_library(DSFE_GUI SHARED) diff --git a/DSFE_App/DSFE_GUI/src/Scene/SimulationManager.cpp b/DSFE_App/DSFE_GUI/src/Scene/SimulationManager.cpp index 58c1dbb6..b2f372f3 100644 --- a/DSFE_App/DSFE_GUI/src/Scene/SimulationManager.cpp +++ b/DSFE_App/DSFE_GUI/src/Scene/SimulationManager.cpp @@ -8,9 +8,6 @@ #include #include -extern "C" core::ISimulationCore* CreateSimulationCore_v1(); -extern "C" void DestroySimulationCore(core::ISimulationCore*); - #include "Manager/SimImplementation.h" #include "SingleBodySystems/SingleBodySystem.h" #include "Platform/KeyCode.h" diff --git a/PxMLib/MathLib_Unit/AutomaticDifferentiationTests/CMakeLists.txt b/PxMLib/MathLib_Unit/AutomaticDifferentiationTests/CMakeLists.txt index 547e2f60..9c86dbff 100644 --- a/PxMLib/MathLib_Unit/AutomaticDifferentiationTests/CMakeLists.txt +++ b/PxMLib/MathLib_Unit/AutomaticDifferentiationTests/CMakeLists.txt @@ -1,7 +1,4 @@ # CMakeLists.txt for MathLib_Unit_Tests executable -cmake_minimum_required(VERSION 3.17) -project(MathLib_ADTests LANGUAGES C CXX) - enable_testing() add_executable(MathLib_ADTests diff --git a/PxMLib/MathLib_Unit/CMakeLists.txt b/PxMLib/MathLib_Unit/CMakeLists.txt index 9bf06a0e..d8ede159 100644 --- a/PxMLib/MathLib_Unit/CMakeLists.txt +++ b/PxMLib/MathLib_Unit/CMakeLists.txt @@ -1,7 +1,4 @@ # CMakeLists.txt for MathLib_Unit_Tests executable -cmake_minimum_required(VERSION 3.17) -project(MathLib_Unit_Tests LANGUAGES C CXX) - add_subdirectory(IntegratorTests) add_subdirectory(DualNumberTests) add_subdirectory(AutomaticDifferentiationTests) \ No newline at end of file diff --git a/PxMLib/MathLib_Unit/DualNumberTests/CMakeLists.txt b/PxMLib/MathLib_Unit/DualNumberTests/CMakeLists.txt index abc162ed..5af97497 100644 --- a/PxMLib/MathLib_Unit/DualNumberTests/CMakeLists.txt +++ b/PxMLib/MathLib_Unit/DualNumberTests/CMakeLists.txt @@ -1,7 +1,4 @@ # CMakeLists.txt for MathLib_Unit_Tests executable -cmake_minimum_required(VERSION 3.17) -project(MathLib_DualNumberTests LANGUAGES C CXX) - enable_testing() add_executable(MathLib_DualNumberTests diff --git a/PxMLib/MathLib_Unit/IntegratorTests/CMakeLists.txt b/PxMLib/MathLib_Unit/IntegratorTests/CMakeLists.txt index f04ac90a..c50df7ce 100644 --- a/PxMLib/MathLib_Unit/IntegratorTests/CMakeLists.txt +++ b/PxMLib/MathLib_Unit/IntegratorTests/CMakeLists.txt @@ -1,7 +1,4 @@ # CMakeLists.txt for MathLib_Unit_Tests executable -cmake_minimum_required(VERSION 3.17) -project(MathLib_IntegratorTests LANGUAGES C CXX) - enable_testing() add_executable(MathLib_IntegratorTests diff --git a/PxMLib/PhysLib_Unit/CMakeLists.txt b/PxMLib/PhysLib_Unit/CMakeLists.txt index 7cc7e428..1629f64c 100644 --- a/PxMLib/PhysLib_Unit/CMakeLists.txt +++ b/PxMLib/PhysLib_Unit/CMakeLists.txt @@ -1,5 +1,2 @@ # CMakeLists.txt for PhysLib_Unit_Tests executable -cmake_minimum_required(VERSION 3.17) -project(PhysLib_Unit_Tests LANGUAGES C CXX) - add_subdirectory(SingleBodySystemTests) \ No newline at end of file diff --git a/PxMLib/PhysLib_Unit/SingleBodySystemTests/CMakeLists.txt b/PxMLib/PhysLib_Unit/SingleBodySystemTests/CMakeLists.txt index c9779dbe..0ed8ef16 100644 --- a/PxMLib/PhysLib_Unit/SingleBodySystemTests/CMakeLists.txt +++ b/PxMLib/PhysLib_Unit/SingleBodySystemTests/CMakeLists.txt @@ -1,7 +1,4 @@ # CMakeLists.txt for MathLib_Unit_Tests executable -cmake_minimum_required(VERSION 3.17) -project(PhysLib_SingleBodySystemTests LANGUAGES C CXX) - enable_testing() add_executable(PhysLib_SingleBodySystemTests From 40debc1a17718cf3f5da857b8917f2937670f6cd Mon Sep 17 00:00:00 2001 From: saltyjoss Date: Tue, 16 Jun 2026 11:57:58 +0100 Subject: [PATCH 165/210] refactor: Updated `build.yml` to use preset logic * This is a test, if it works, I will move the toolchain logic into the presets too! --- .github/workflows/build.yml | 13 +++++++++---- 1 file changed, 9 insertions(+), 4 deletions(-) diff --git a/.github/workflows/build.yml b/.github/workflows/build.yml index 4a70e721..d18d3d3d 100644 --- a/.github/workflows/build.yml +++ b/.github/workflows/build.yml @@ -25,20 +25,25 @@ jobs: run: | .\vcpkg\vcpkg.exe install hdf5:x64-windows + - name: Install Qt + uses: jurplel/install-qt-action@v4 + with: + version: 6.11.1 + arch: win64_msvc2022_64 + - name: Configure shell: pwsh run: | - cmake -S . -B build ` - -A x64 ` + cmake --preset windows-x64 ` -DVCPKG_TARGET_TRIPLET=x64-windows ` -DCMAKE_TOOLCHAIN_FILE="${{ github.workspace }}/vcpkg/scripts/buildsystems/vcpkg.cmake" - name: Build shell: pwsh run: | - cmake --build build --config Release --verbose + cmake --build --preset windows-x64-release --verbose - name: Run tests shell: pwsh run: | - ctest --test-dir build --output-on-failure + ctest --preset windows-x64-release --output-on-failure From cf0dd241895f69f08daa147d1bd8d246e72002a4 Mon Sep 17 00:00:00 2001 From: saltyjoss Date: Tue, 16 Jun 2026 12:08:26 +0100 Subject: [PATCH 166/210] fixes: Try different Qt6 version for workflow * Added a temporary CI step for checking the available versions --- .github/workflows/build.yml | 8 +++++++- 1 file changed, 7 insertions(+), 1 deletion(-) diff --git a/.github/workflows/build.yml b/.github/workflows/build.yml index d18d3d3d..56ccfe0c 100644 --- a/.github/workflows/build.yml +++ b/.github/workflows/build.yml @@ -25,10 +25,16 @@ jobs: run: | .\vcpkg\vcpkg.exe install hdf5:x64-windows + - name: List Qt versions (debug) + shell: pwsh + run: | + python -m pip install aqtinstall + python -m aqt list-qt windows desktop + - name: Install Qt uses: jurplel/install-qt-action@v4 with: - version: 6.11.1 + version: 6.6.2 arch: win64_msvc2022_64 - name: Configure From a473b311fcb8a10f52e617098afb24ec23fd9480 Mon Sep 17 00:00:00 2001 From: saltyjoss Date: Tue, 16 Jun 2026 12:15:31 +0100 Subject: [PATCH 167/210] fixes --- .github/workflows/build.yml | 12 ------------ 1 file changed, 12 deletions(-) diff --git a/.github/workflows/build.yml b/.github/workflows/build.yml index 56ccfe0c..ed31bb47 100644 --- a/.github/workflows/build.yml +++ b/.github/workflows/build.yml @@ -25,18 +25,6 @@ jobs: run: | .\vcpkg\vcpkg.exe install hdf5:x64-windows - - name: List Qt versions (debug) - shell: pwsh - run: | - python -m pip install aqtinstall - python -m aqt list-qt windows desktop - - - name: Install Qt - uses: jurplel/install-qt-action@v4 - with: - version: 6.6.2 - arch: win64_msvc2022_64 - - name: Configure shell: pwsh run: | From 4bad85f7f10cb7f7da6f8b064dd2cf294b76f05f Mon Sep 17 00:00:00 2001 From: saltyjoss Date: Tue, 16 Jun 2026 12:28:18 +0100 Subject: [PATCH 168/210] fixes --- .github/workflows/build.yml | 5 +++++ 1 file changed, 5 insertions(+) diff --git a/.github/workflows/build.yml b/.github/workflows/build.yml index ed31bb47..fc7523c2 100644 --- a/.github/workflows/build.yml +++ b/.github/workflows/build.yml @@ -25,6 +25,11 @@ jobs: run: | .\vcpkg\vcpkg.exe install hdf5:x64-windows + - name: Install Qt + shell: pwsh + run: | + .\vcpkg\vcpkg.exe install qtbase:x64-windows + - name: Configure shell: pwsh run: | From 941c9ada428d09e0112c22111e38dad94a2bae3d Mon Sep 17 00:00:00 2001 From: saltyjoss Date: Tue, 16 Jun 2026 12:45:15 +0100 Subject: [PATCH 169/210] feat: Added `DSFE_ENABLE_GUI` compile time flag, influencing the inclusion of `DSFE_GUI` at runtime. --- DSFE_App/DSFE_Engine/CMakeLists.txt | 8 +++-- DSFE_App/DSFE_Engine/src/Main.cpp | 56 +++++++++++++++++------------ 2 files changed, 39 insertions(+), 25 deletions(-) diff --git a/DSFE_App/DSFE_Engine/CMakeLists.txt b/DSFE_App/DSFE_Engine/CMakeLists.txt index 50beccb0..9778b916 100644 --- a/DSFE_App/DSFE_Engine/CMakeLists.txt +++ b/DSFE_App/DSFE_Engine/CMakeLists.txt @@ -1,5 +1,5 @@ # CMakeLists.txt for the DSFE_Engine executable - +option(DSFE_ENABLE_GUI "Enable GUI module" ON) find_package(Threads REQUIRED) add_executable(DSFE_Engine @@ -12,11 +12,15 @@ target_include_directories(DSFE_Engine PRIVATE ) target_link_libraries(DSFE_Engine PRIVATE - DSFE_GUI DSFE_Core Threads::Threads ) +if (DSFE_ENABLE_GUI) + target_link_libraries(DSFE_Engine PRIVATE DSFE_GUI) + target_compile_definitions(DSFE_Engine PRIVATE DSFE_ENABLE_GUI) +endif() + add_custom_command(TARGET DSFE_Engine POST_BUILD COMMAND ${CMAKE_COMMAND} -E copy_if_different "$" diff --git a/DSFE_App/DSFE_Engine/src/Main.cpp b/DSFE_App/DSFE_Engine/src/Main.cpp index 571390d0..c477399a 100644 --- a/DSFE_App/DSFE_Engine/src/Main.cpp +++ b/DSFE_App/DSFE_Engine/src/Main.cpp @@ -4,10 +4,13 @@ #include #include #include -#include #include "BatchEntry.h" #include "BatchArgs.h" +#ifdef DSFE_ENABLE_GUI + #include +#endif + // Helper function to split a comma-separated string into a vector of strings, trimming whitespace static std::vector splitComma(const std::string& input) { std::vector result; @@ -46,40 +49,47 @@ int main(int argc, char** argv) { batchMode = true; break; } + else if (arg == "--about") { + std::cout + << "DSFE (Dynamic Systems Framework Engine)\n" + << " > A research-focused simulation engine for numerically modelling dynamic systems with different integration methods (explicit and implicit).\n" + << " > Version 0.9.1-alpha\n" + << " > Developed by Joss Salton\n"; + return 0; + } + else if (arg == "--help" || arg == "-h") { + std::cout + << "Usage:\n" + << " --batch -t