Implemented state history buffer and scrubbing.

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 $<BUILD_INTERFACE:${CMAKE_CURRENT_SOURCE_DIR}/include>
@@ -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
 	)
 

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) {

src/sthstry.c

@@ -0,0 +1,6 @@
+//
+// Created by martin on 13.11.20.
+//
+
+#define STHSTRY_IMPLEMENTATION
+#include "sthstry.h"

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 <malloc.h>
+#include <stdbool.h>
+#include <stddef.h>
+#include <stdlib.h>
+#include <string.h>
+
+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