From 48b72c7e157596f99e656e6b8b29365197bf404b Mon Sep 17 00:00:00 2001 From: Martin Felis Date: Fri, 13 Nov 2020 13:26:52 +0100 Subject: [PATCH] Implemented state history buffer and scrubbing. --- CMakeLists.txt | 3 +- src/simulator.cc | 52 ++++++++++++++-- src/sthstry.c | 6 ++ src/sthstry.h | 156 +++++++++++++++++++++++++++++++++++++++++++++++ 4 files changed, 212 insertions(+), 5 deletions(-) create mode 100644 src/sthstry.c create mode 100644 src/sthstry.h diff --git a/CMakeLists.txt b/CMakeLists.txt index c5ce1f9..7be5820 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -44,7 +44,7 @@ target_link_libraries(runsim ${PROJECT_NAME}) target_sources(runsim PRIVATE src/main.cc) # Visualization -add_executable(vissim ) +add_executable(vissim) target_include_directories( vissim PUBLIC $ @@ -67,6 +67,7 @@ target_sources(vissim PRIVATE 3rdparty/imgui/backends/imgui_impl_glfw.cpp 3rdparty/imgui/backends/imgui_impl_opengl3.cpp src/vissim.cc + src/sthstry.c src/simulator.cc ) diff --git a/src/simulator.cc b/src/simulator.cc index b0dc241..c9a2ba0 100644 --- a/src/simulator.cc +++ b/src/simulator.cc @@ -10,6 +10,7 @@ #include "srender.h" #include "utils.h" #include "imgui.h" +#include "sthstry.h" using namespace std; using namespace RBDLSim; @@ -29,9 +30,12 @@ static bool nSteps = 0; static double sSimTime = 0.; static double sSimTimeAccumulator = 0.; static double sSimTimeStep = 1.0e-2; +static sthstry* sStateHistory = nullptr; +static int sStateHistoryCurrent = 0; void simulator_init() { gLog("Initializing Simulator"); + sStateHistory = sthstry_create(); sGroundShape.mType = SimShape::Plane; sGroundShape.pos.set(0., 0., 0.); @@ -59,6 +63,17 @@ void simulator_init() { Vector3d::Zero()); //sWorld.mBodies.push_back(sSphereBody2); + sthstry_register( + sStateHistory, + sWorld.mBodies[0].q.data(), + sSphereBody.q.size() * sizeof(double)); + sthstry_register( + sStateHistory, + sWorld.mBodies[0].qdot.data(), + sSphereBody.qdot.size() * sizeof(double)); + // sthstry_register(sStateHistory, sWorld.mBodies[1].q.data(), sSphereBody2.q.size() * sizeof(double)); + // sthstry_register(sStateHistory, sWorld.mBodies[1].qdot.data(), sSphereBody2.qdot.size() * sizeof(double)); + sWorld.mSimTime = 0.; simulator_reset(); @@ -81,13 +96,14 @@ void simulator_reset() { sWorld.mBodies[0].q[0] = 0.0; sWorld.mBodies[0].q[1] = 1.50; -// sWorld.mBodies[1].q[0] = 0.0; -// sWorld.mBodies[1].q[1] = 5.50; + sthstry_reset_storage(sStateHistory); + sthstry_store(sStateHistory); + + // sWorld.mBodies[1].q[0] = 0.0; + // sWorld.mBodies[1].q[1] = 5.50; } void simulator_update(double dt) { - gLog("dt = %f", dt); - ImGui::Begin("Simulator"); if (ImGui::Button("Reset")) { simulator_reset(); @@ -97,6 +113,31 @@ void simulator_update(double dt) { } ImGui::Checkbox("Paused", &sIsPaused); + if (ImGui::Button("<")) { + if (sStateHistoryCurrent > 0) { + sStateHistoryCurrent--; + } + sthstry_restore(sStateHistory, sStateHistoryCurrent); + } + ImGui::SameLine(); + if (ImGui::Button(">")) { + sStateHistoryCurrent++; + + if (sStateHistoryCurrent >= sthstry_get_num_states(sStateHistory)) { + simulator_step(sSimTimeStep); + } else { + sthstry_restore(sStateHistory, sStateHistoryCurrent); + } + } + ImGui::SameLine(); + if (ImGui::SliderInt( + "Step", + &sStateHistoryCurrent, + 0, + sthstry_get_num_states(sStateHistory) - 1)) { + sthstry_restore(sStateHistory, sStateHistoryCurrent); + } + ImGui::Text("Ground Plane"); Vector3f ground_pos = sGroundShape.pos; ImGui::DragFloat3("Position", ground_pos.data(), 0.1f, -5.0f, 5.0f); @@ -136,6 +177,9 @@ void simulator_step(double dt) { sWorld.resolveCollisions(dt); sWorld.integrateWorld(dt); + + sthstry_store(sStateHistory); + sStateHistoryCurrent = sthstry_get_num_states(sStateHistory); } void simulator_draw(srcmdbuf* cmdbuf) { diff --git a/src/sthstry.c b/src/sthstry.c new file mode 100644 index 0000000..35a39fa --- /dev/null +++ b/src/sthstry.c @@ -0,0 +1,6 @@ +// +// Created by martin on 13.11.20. +// + +#define STHSTRY_IMPLEMENTATION +#include "sthstry.h" diff --git a/src/sthstry.h b/src/sthstry.h new file mode 100644 index 0000000..7ed5e86 --- /dev/null +++ b/src/sthstry.h @@ -0,0 +1,156 @@ +// +// Created by martin on 13.11.20. +// + +#ifndef STHSTRY_H +#define STHSTRY_H + +#ifdef __cplusplus +extern "C" { +#endif + +#include +#include +#include +#include +#include + +typedef struct sthstry sthstry; + +sthstry* sthstry_create(); +void sthstry_destroy(sthstry* sh); + +void sthstry_register(sthstry* sh, void* ptr, size_t size); +void sthstry_store(sthstry* sh); +void sthstry_restore(sthstry* sh, int index); +int sthstry_get_num_states(sthstry* sh); +void sthstry_reset_storage(sthstry* sh); + +#define STHSTRY_BUFFER_SIZE 1024 * 1024 * 100 + +#ifdef STHSTRY_IMPLEMENTATION + +typedef struct stentry { + void* ptr; + size_t size; + size_t offset; +} stentry; + +typedef struct sthstry { + char* storage; + size_t storage_size; + size_t state_block_size; + int num_state_blocks; + + stentry* registry; + int registry_capacity; + int num_registry_entries; + bool registry_locked; +} sthstry; + +// Forward declarations +bool sthstry_resize_registry(sthstry* sh, int count); + +sthstry* sthstry_create() { + sthstry* result = (sthstry*)calloc(1, sizeof(sthstry)); + result->storage = (char*)calloc(1, STHSTRY_BUFFER_SIZE); + result->storage_size = STHSTRY_BUFFER_SIZE; + result->state_block_size = 0; + result->num_state_blocks = 0; + + result->registry_capacity = 0; + result->num_registry_entries = 0; + result->registry_locked = false; + sthstry_resize_registry(result, 10); +} + +void sthstry_destroy(sthstry* sh) { + free(sh->storage); + free(sh->registry); + free(sh); +} + +bool sthstry_resize_registry(sthstry* sh, int count) { + if (count == 0) { + free(sh->registry); + sh->registry = NULL; + sh->registry_capacity = 0; + return true; + } + + stentry* result = (stentry*)calloc(count, sizeof(stentry)); + if (result == NULL) { + fprintf(stderr, "Error: could not allocate memory for registry!\n"); + } + memcpy(result, sh->registry, sizeof(stentry) * sh->num_registry_entries); + free(sh->registry); + sh->registry = result; + sh->registry_capacity = count; +} + +void sthstry_register(sthstry* sh, void* ptr, size_t size) { + if (sh->registry_locked) { + fprintf( + stderr, + "Error: cannot register entry to locked state history registry!\n"); + abort(); + } + + if (sh->registry_capacity == sh->num_registry_entries) { + sthstry_resize_registry(sh, sh->registry_capacity * 2); + } + + stentry* entry = &sh->registry[sh->num_registry_entries++]; + entry->offset = sh->state_block_size; + entry->ptr = ptr; + entry->size = size; + sh->state_block_size += entry->size; +} + +void sthstry_store(sthstry* sh) { + sh->registry_locked = true; + + size_t needed_size = (sh->num_state_blocks + 1) * sh->state_block_size; + if (needed_size > sh->storage_size) { + fprintf( + stderr, + "Error: cannot store another block. Storage exceeded by %ld bytes.\n", + needed_size - sh->storage_size); + abort(); + } + + char* state_block = sh->storage + sh->num_state_blocks * sh->state_block_size; + for (int i = 0; i < sh->num_registry_entries; i++) { + stentry* entry = &sh->registry[i]; + memcpy((void*)(state_block + entry->offset), entry->ptr, entry->size); + } + + sh->num_state_blocks++; +} + +int sthstry_get_num_states(sthstry* sh) { return sh->num_state_blocks; } + +void sthstry_restore(sthstry* sh, int index) { + if (index >= sh->num_state_blocks) { + fprintf( + stderr, + "Error: cannot restore state block %d only have %d blocks stored.\n", + index, + sh->num_state_blocks); + } + char* state_block = sh->storage + index * sh->state_block_size; + for (int i = 0; i < sh->num_registry_entries; i++) { + stentry* entry = &sh->registry[i]; + memcpy(entry->ptr, (void*)(state_block + entry->offset), entry->size); + } +} + +void sthstry_reset_storage(sthstry* sh) { sh->num_state_blocks = 0; } + +#endif // STHSTRY_IMPLEMENTATION + +#ifdef __cplusplus +} +#endif + +#endif // STHSTRY_H