diff --git a/MathLib/MathLib/include/core/Utils.h b/MathLib/MathLib/include/core/Utils.h index 16e2415f..e4e4d4b2 100644 --- a/MathLib/MathLib/include/core/Utils.h +++ b/MathLib/MathLib/include/core/Utils.h @@ -1,5 +1,4 @@ #pragma once -#pragma warning(disable : 4251) #include "MathLibAPI.h" #include "core/Types.h" @@ -11,21 +10,21 @@ using namespace constants; namespace mathlib { // Converts a non-eigen vector to Vec3 (if it has 3 elements) template - Vec3 toVec3(const T& vec) { + 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 - Vec4 toVec4(const T& vec) { + 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 - Mat3 toMat3(const T& mat) { + 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], @@ -35,7 +34,7 @@ namespace mathlib { // Converts a non-eigen 4x4 matrix to Mat4 (if it has 4 rows and 4 columns) template - Mat4 toMat4(const T& mat) { + 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], @@ -45,48 +44,62 @@ namespace mathlib { } // Convert degrees to radians - double deg2rad(double degrees) { return degrees * (PI_d / 180.0); } + inline double deg2rad(double degrees) { return degrees * (PI_d / 180.0); } // Convert radians to degrees - double rad2deg(double radians) { return radians * (180.0 / PI_d); } + inline double rad2deg(double radians) { return radians * (180.0 / PI_d); } // Clamp a value between min and max - double clamp(double value, double minVal, double maxVal) { + inline double clamp(double value, double minVal, double maxVal) { if (value < minVal) { return minVal; } if (value > maxVal) { return maxVal; } return value; } + // 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] + } + + // 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; } + return angleRad; // [rad] + } + // Linear interpolation between a and b by factor t (0 <= t <= 1) - double lerp(double a, double b, double t) { return a + t * (b - a); } + inline double lerp(double a, double b, double t) { return a + t * (b - a); } // Check if two doubles are approximately equal within a tolerance - bool approximatelyEqual(double a, double b, double tolerance = 1e-9) { return std::fabs(a - b) <= tolerance; } + 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] - bool isInRange(double value, double minVal, double maxVal) { return (value >= minVal) && (value <= maxVal); } + 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 - double sgn(double val) { return (val > 0) - (val < 0); } + inline double sgn(double val) { return (val > 0) - (val < 0); } // Returns a quadratic function f(x) = a*x^2 + b*x + c - std::function quadratic(double a, double b, double c) { return [a, b, c](double x) { return a * x * x + 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 - 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; }; } + 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 - double dot(const Vec3& v1, const Vec3& v2) { return v1.x() * v2.x() + v1.y() * v2.y() + v1.z() * v2.z(); } + inline double dot(const Vec3& v1, const Vec3& v2) { return v1.x() * v2.x() + v1.y() * v2.y() + v1.z() * v2.z(); } // Natural frequency of a mass-spring system - double natural_freq(double k_p, double I) { + 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); } // Damping ratio of a mass-spring-damper system - double damping_ratio(double k_d, double I, double k_p) { + 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); @@ -95,33 +108,33 @@ namespace mathlib { // Overloads for std::array and C arrays for Vec3 conversion template - std::enable_if_t toVec3(const std::array& arr) { + inline std::enable_if_t toVec3(const std::array& arr) { return Vec3(arr[0], arr[1], arr[2]); } template - Vec3 toVec3(const T arr[3]) { + inline Vec3 toVec3(const T arr[3]) { return Vec3(arr[0], arr[1], arr[2]); } // Overloads for std::array and C arrays for Vec4 conversion template - std::enable_if_t toVec4(const std::array& arr) { + inline std::enable_if_t toVec4(const std::array& arr) { return Vec4(arr[0], arr[1], arr[2], arr[3]); } template - Vec4 toVec4(const T arr[4]) { + inline Vec4 toVec4(const T arr[4]) { return Vec4(arr[0], arr[1], arr[2], arr[3]); } // Overloads for std::array and C arrays for Mat3 conversion template - std::enable_if_t toMat3(const std::array, N>& mat) { + inline std::enable_if_t toMat3(const std::array, N>& mat) { 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]); } template - Mat3 toMat3(const T mat[3][3]) { + inline Mat3 toMat3(const T mat[3][3]) { 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]); @@ -129,14 +142,14 @@ namespace mathlib { // Overloads for std::array and C arrays for Mat4 conversion template - std::enable_if_t toMat4(const std::array, N>& mat) { + 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], 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]); } template - Mat4 toMat4(const T mat[4][4]) { + inline Mat4 toMat4(const T mat[4][4]) { 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], diff --git a/README.md b/README.md index f74d36b2..06afccb3 100644 --- a/README.md +++ b/README.md @@ -86,21 +86,21 @@ Download the latest release of DSFE from `Release` in the `DSFE GitHub Repositor > Therefore, I most likely will not make any updates until that period is over.
### Todo List: - * Implement `collision meshes` with existin dynamics pipeline - * Improve UI, making it `Research-Oriented` - * Add workspace layouts - * Support multiple concurrent sessions - * Move simulation data output to a `dedicated Data-specific thread` **(IMPORTANT)** - * Replace the current diagonal Coriolis approximation to the `full Coriolis matrix formulation` - * Support `multiple articulated systems` within a single simulation instance - * Extend DSFE to support `other classes of dynamical systems` outside robotic manipulators - * Further `extend physcial modelling` for different robot models (humanoid, legged) - * Seperate physics/mathematics core from the GUI and visualisation layers - * Explore migration to a `CMake-only` build system - * Explore `DX11` and `Vulkan` alternatives - * Get DSFE to work on `Linux` - * Explore `non-x64 systesm` support - * Explore the adoption of `standardised URDF XML` in place of the current DSFE json format + * [ ] Implement `collision meshes` with existin dynamics pipeline + * [ ] Improve UI, making it `Research-Oriented` + * [ ] Add workspace layouts + * [ ] Support multiple concurrent sessions + * [ ] Move simulation data output to a `dedicated Data-specific thread` **(IMPORTANT)** + * [x] Replace the current diagonal model with the standarised `full-matrix rigid-body model` + * [ ] Support `multiple articulated systems` within a single simulation instance + * [ ] Extend DSFE to support `other classes of dynamical systems` outside robotic manipulators + * [ ] Further `extend physcial modelling` for different robot models (humanoid, legged) + * [ ] Seperate physics/mathematics core from the GUI and visualisation layers + * [ ] Explore migration to a `CMake-only` build system + * [ ] Explore `DX11` and `Vulkan` alternatives + * [ ] Get DSFE to work on `Linux` + * [ ] Explore `non-x64 systesm` support + * [ ] Explore the adoption of `standardised URDF XML` in place of the current DSFE json format
diff --git a/Sim_Engine/Engine.vcxproj b/Sim_Engine/Engine.vcxproj index bdc17788..35777830 100644 --- a/Sim_Engine/Engine.vcxproj +++ b/Sim_Engine/Engine.vcxproj @@ -57,7 +57,7 @@ true _DEBUG;_CONSOLE;%(PreprocessorDefinitions) true - $(SolutionDir)EngineLib;$(SolutionDir)EngineLib\include;$SolutionDir)External\GLFW;$(SolutionDir)External\GLAD;$(SolutionDir)External\GLM;$(SolutionDir)External\assimp;$(SolutionDir)External\MathLib\include;%(AdditionalIncludeDirectories) + $(SolutionDir)EngineLib;$(SolutionDir)EngineLib\include;$SolutionDir)External\GLFW;$(SolutionDir)External\GLAD;$(SolutionDir)External\GLM;$(SolutionDir)External\assimp;$(SolutionDir)External\MathLib\include;$(SolutionDir)Engine\include;$(SolutionDir)External\Eigen;%(AdditionalIncludeDirectories) stdcpp20 $(IntDir)vc$(PlatformToolsetVersion).pdb $(IntDir)%(FileName).obj @@ -87,7 +87,7 @@ echo D | xcopy /E /Y /D "$(ProjectDir)Engine\configs\*" "$(TargetDir)configs\" true NDEBUG;_CONSOLE;%(PreprocessorDefinitions) true - $(SolutionDir)EngineLib;$(SolutionDir)EngineLib\include;$SolutionDir)External\GLFW;$(SolutionDir)External\GLAD;$(SolutionDir)External\GLM;$(SolutionDir)External\assimp;$(SolutionDir)External\MathLib\include;%(AdditionalIncludeDirectories) + $(SolutionDir)EngineLib;$(SolutionDir)EngineLib\include;$SolutionDir)External\GLFW;$(SolutionDir)External\GLAD;$(SolutionDir)External\GLM;$(SolutionDir)External\assimp;$(SolutionDir)External\MathLib\include;$(SolutionDir)Engine\include;$(SolutionDir)External\Eigen;%(AdditionalIncludeDirectories) stdcpp20 false $(IntDir)vc$(PlatformToolsetVersion).pdb @@ -111,6 +111,7 @@ echo D | xcopy /E /Y /D "$(ProjectDir)Engine\configs\*" "$(TargetDir)configs\" + @@ -138,6 +139,10 @@ echo D | xcopy /E /Y /D "$(ProjectDir)Engine\configs\*" "$(TargetDir)configs\" + + + + diff --git a/Sim_Engine/Engine.vcxproj.filters b/Sim_Engine/Engine.vcxproj.filters index a4e06fa2..c65e0d64 100644 --- a/Sim_Engine/Engine.vcxproj.filters +++ b/Sim_Engine/Engine.vcxproj.filters @@ -7,6 +7,9 @@ src + + src + @@ -15,6 +18,9 @@ {383daa6b-14af-40d3-b6df-31732d4d7a6a} + + {146d8cfd-be4d-41a3-bc59-546deb084dc0} + @@ -84,4 +90,12 @@ Shaders + + + include + + + include + + \ No newline at end of file diff --git a/Sim_Engine/Engine/include/BatchArgs.h b/Sim_Engine/Engine/include/BatchArgs.h new file mode 100644 index 00000000..ec49d476 --- /dev/null +++ b/Sim_Engine/Engine/include/BatchArgs.h @@ -0,0 +1,19 @@ +#pragma once +#include +#include +#include + +// Struct to hold command-line arguments for batch mode +struct BatchArgs { + std::string testPath; + // Base configuration for batch runs (if not provided, defaults will be used) + std::optional baseDtRaw; + std::optional baseDt; + std::optional baseInt; + // Lists for sweep parameters (if not provided, defaults will be used) + std::vector sweepDtRaw; + std::vector sweepDt; + std::vector sweepInt; + // Run name for output organisation + std::optional runName; +}; \ No newline at end of file diff --git a/Sim_Engine/Engine/include/BatchEntry.h b/Sim_Engine/Engine/include/BatchEntry.h new file mode 100644 index 00000000..4a24b60d --- /dev/null +++ b/Sim_Engine/Engine/include/BatchEntry.h @@ -0,0 +1,3 @@ +#pragma once +struct BatchArgs; +int runBatchMode(const BatchArgs& args); \ No newline at end of file diff --git a/Sim_Engine/Engine/src/BatchMain.cpp b/Sim_Engine/Engine/src/BatchMain.cpp new file mode 100644 index 00000000..4e9048f7 --- /dev/null +++ b/Sim_Engine/Engine/src/BatchMain.cpp @@ -0,0 +1,132 @@ +// BatchMain.cpp +#include "pch.h" +#include +#include "BatchEntry.h" +#include "BatchArgs.h" +#include "Platform/StudyRunner.h" +#include "Numerics/IntegrationMethods.h" +#include "Platform/Paths.h" + +#include +#include +#include +#include +#include + +// Integration method parser from string (throws if unknown) +static integration::eIntegrationMethod parseMethod(const std::string& name) { + if (name == "euler") return integration::eIntegrationMethod::Euler; + if (name == "midpoint") return integration::eIntegrationMethod::Midpoint; + if (name == "heun") return integration::eIntegrationMethod::Heun; + if (name == "ralston") return integration::eIntegrationMethod::Ralston; + if (name == "rk4") return integration::eIntegrationMethod::RK4; + if (name == "rk45") return integration::eIntegrationMethod::RK45; + throw std::runtime_error("Unknown integrator: " + name); +} + +// Helper to format a timestep (dt) as a string for use in tags and filenames +static std::string formatDtForFile(const std::string& raw) { + std::string out; + out.reserve(raw.size() + 4); + for (char c : raw) { + if (c == '.') { out += "p"; } + if (c == '/') { out += "over"; } + else { out += c; } + } + return out; +} + +// Core factory function for StudyRunner +static CorePtr makeCoreFactory() { + core::ISimulationCore* raw = CreateSimulationCore_v1(); + if (!raw) { return CorePtr(nullptr, [](core::ISimulationCore*) {}); } + return CorePtr(raw, [](core::ISimulationCore* p) { DestroySimulationCore(p); }); +} + +// Batch mode entry point +int runBatchMode(const BatchArgs& args) { + paths::init(); + fprintf(stdout, "BATCH MODE ENTERED\n"); + fflush(stdout); + // Basic validation and setup + try { + // Validate required baseline + if (!args.baseDt.has_value() || !args.baseInt.has_value()) { throw std::runtime_error("Baseline (--basedt and --baseint) required."); } + if (args.testPath.empty()) { throw std::runtime_error("No test script provided."); } + + // Load script + std::ifstream file(args.testPath); + if (!file) { throw std::runtime_error("Failed to open test file: " + args.testPath); } + std::ostringstream buffer; + buffer << file.rdbuf(); + std::string scriptText = buffer.str(); + + // Build configs + std::vector configs; + + // Baseline (always first) + { + StudyRunner::config base; + base.method = parseMethod(args.baseInt.value()); + base.dt = args.baseDt.value(); + base.len_min = 1.0; // Fixed simulation duration (consistent across runs) + base.tag = + (args.runName.has_value() ? args.runName.value() + "_" : "") + + "base_dt" + formatDtForFile(args.baseDtRaw.value()) + + "_int_" + args.baseInt.value(); + configs.push_back(base); + } + + // Sweep grid + for (size_t i = 0; i < args.sweepDt.size(); ++i) { + double dt = args.sweepDt[i]; + const std::string& dtRaw = args.sweepDtRaw[i]; + for (const auto& intName : args.sweepInt) { + StudyRunner::config c; + c.method = parseMethod(intName); + c.dt = dt; + c.len_min = 1.0; + c.tag = + (args.runName.has_value() ? args.runName.value() + "_" : "") + + "dt" + formatDtForFile(dtRaw) + + "_int_" + intName; + configs.push_back(c); + } + } + + // Worker configuration + size_t workers = 1; // stable mode (parallel logging disabled) + StudyRunner runner(makeCoreFactory, workers); + + // Execute + auto T0 = std::chrono::high_resolution_clock::now(); + auto results = runner.runStudies(configs, scriptText); + auto T1 = std::chrono::high_resolution_clock::now(); + + double total = std::chrono::duration(T1 - T0).count(); + fprintf(stderr, "TOTAL BATCH TIME = %.3f sec\n", total); + + // Sort results by tag + std::sort(results.begin(), results.end(), + [](const StudyResult& a, const StudyResult& b) { + return a.tag < b.tag; + }); + + // Print summary + for (const auto& r : results) { + std::cout + << "tag=" << r.tag + << " success=" << (r.success ? "yes" : "no") + << " int=" << r.intName + << " time=" << r.simTime + << " samples=" << r.samples + << "\n"; + } + } + // Catch any exceptions that escaped from the batch processing and log them + catch (const std::exception& e) { + std::cerr << "Batch mode exception: " << e.what() << std::endl; + return EXIT_FAILURE; + } + return EXIT_SUCCESS; +} \ No newline at end of file diff --git a/Sim_Engine/Engine/src/Main.cpp b/Sim_Engine/Engine/src/Main.cpp index 86cd4cae..490db367 100644 --- a/Sim_Engine/Engine/src/Main.cpp +++ b/Sim_Engine/Engine/src/Main.cpp @@ -1,9 +1,166 @@ #include "pch.h" +// File: Main.cpp #include #include +#include "BatchEntry.h" +#include "BatchArgs.h" -int main() { - Application app("DSFE"); - app.run(); - return 0; +// 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; + std::stringstream ss(input); + std::string item; + // Split the input string by commas and trim whitespace from each item + while (std::getline(ss, item, ',')) + if (!item.empty()) + result.push_back(item); + + return result; +} + +// Helper function to parse a timestep (dt) from a string, supporting both plain decimal and fractional formats (e.g., "1/180") +static double parseDt(const std::string& input) { + // Case: "1/180" + auto slashPos = input.find('/'); + if (slashPos != std::string::npos) { + double num = std::stod(input.substr(0, slashPos)); + double den = std::stod(input.substr(slashPos + 1)); + return num / den; + } + + // Case: plain decimal + return std::stod(input); +} + +// Main entry point for the application, handling both GUI and batch modes based on command-line arguments +int main(int argc, char** argv) { + bool batchMode = false; + + // First pass to check for batch mode flag + for (int i = 1; i < argc; ++i) { + std::string arg(argv[i]); + if (arg == "--batch" || arg == "-b") { + batchMode = true; + break; + } + } + + // Non-batch mode: simple pass to handle GUI-specific options + if (!batchMode) { + for (int i = 1; i < argc; ++i) { + std::string arg(argv[i]); + if (arg == "--about") { + std::cout << "DSFE - Dynamic Systems Framework Engine\n"; + return 0; + } + else if (arg == "--help" || arg == "-h") { + std::cout << "Batch mode usage:\n"; + std::cout << " --batch -t