Basic Simulator with editing of scene.

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/simulator.cc
 	)
 
 # Tests

include/simulator.h

@@ -0,0 +1,15 @@
+//
+// Created by martin on 02.11.20.
+//
+
+#ifndef RBDLSIM_SIMULATOR_H
+#define RBDLSIM_SIMULATOR_H
+
+// Forward declarations for visualization
+typedef struct srcmdbuf srcmdbuf;
+
+void simulator_init();
+void simulator_update(double dt);
+void simulator_draw(srcmdbuf* cmdbuf);
+
+#endif  //RBDLSIM_SIMULATOR_H

src/simulator.cc

@@ -0,0 +1,109 @@
+// clang-format off
+// glad must be included before any other OpenGL libraries
+#include <glad/gl.h>
+// clang-format on
+
+#include <iostream>
+
+#include "rbdlsim.h"
+#include "simulator.h"
+#include "srender.h"
+#include "utils.h"
+#include "imgui.h"
+
+using namespace std;
+using namespace RBDLSim;
+
+static World sWorld;
+static SimShape sGroundShape;
+static SimBody sSphereBody;
+
+typedef SimpleMath::Matrix<float, 3, 3> Matrix33f;
+typedef SimpleMath::Matrix<float, 3, 1> Vector3f;
+typedef SimpleMath::Matrix<float, 4, 4> Matrix44f;
+typedef SimpleMath::Matrix<float, 4, 1> Vector4f;
+
+
+void simulator_init() {
+  gLog ("Initializing Simulator");
+
+  sGroundShape.mType = SimShape::Plane;
+  sGroundShape.pos.set(0., 0., 0.);
+  sGroundShape.orientation.set(0., 0., 0., 1.);
+  sGroundShape.scale.set(1.0, 1.0, 1.0);
+
+  sWorld.mStaticShapes.push_back(sGroundShape);
+
+  sSphereBody =
+      CreateSphereBody(10., 1., Vector3d(0., 5.405, 0.), Vector3d::Zero());
+  sWorld.mBodies.push_back(sSphereBody);
+
+  sWorld.mSimTime = 0.;
+}
+
+void simulator_update(double dt) {
+  gLog ("Updating Simulator");
+
+  ImGui::Begin("Simulator");
+  ImGui::Text("Ground Plane");
+  Vector3f ground_pos = sGroundShape.pos;
+  ImGui::DragFloat3("Position", ground_pos.data(), 0.1f, -5.0f, 5.0f);
+  sGroundShape.pos = ground_pos;
+
+  Vector4f orientation = sGroundShape.orientation;
+  ImGui::DragFloat4("Normal", orientation.data(), 0.1f, -1.0f, 1.0f);
+  orientation.normalize();
+  sGroundShape.orientation.set(orientation[0], orientation[1], orientation[2], orientation[3]);
+
+  ImGui::Text("Bodies");
+  for (int i = 0; i < sWorld.mBodies.size(); i++) {
+    SimBody& body = sWorld.mBodies[i];
+    Vector3f body_pos = body.q.block(0, 0, 3, 1);
+    ImGui::DragFloat3("Pos", body_pos.data(), 0.01f, -5.0f, 5.0f);
+    body.q.block(0, 0, 3, 1) = body_pos;
+    body.updateCollisionShapes();
+  }
+
+  ImGui::End();
+}
+
+void simulator_draw(srcmdbuf* cmdbuf) {
+  gLog ("Drawing Simulator");
+
+  srcmd rcmd;
+  srcmd_clear(&rcmd);
+
+  // Ground Plane
+  Vector3f ground_pos = sGroundShape.pos;
+
+  rcmd.type = SRndrCmdTypeGrid;
+  for (int i = -1; i <= 1; i++) {
+    for (int j = -1; j <= 1; j++) {
+      simd4x4f_translation(&rcmd.mat, ground_pos[0] + i * 10.0f, ground_pos[1], ground_pos[2] + j * 10.f);
+      srcmdbuf_add(cmdbuf, &rcmd);
+    }
+  }
+
+  // World Bodies
+  for (int i = 0; i < sWorld.mBodies.size(); i++) {
+    const SimBody& body = sWorld.mBodies[i];
+    for (int j = 0; j < body.mCollisionShapes.size(); j++) {
+      const SimBody::BodyCollisionInfo& cinfo = body.mCollisionShapes[j];
+      switch (cinfo.second.mType) {
+        case SimShape::Box: rcmd.type = SRndrCmdTypeCube; break;
+        case SimShape::Sphere: rcmd.type = SRndrCmdTypeSphere; break;
+        default: gLog ("Error: cannot render shape of type %d", cinfo.second.mType);
+      }
+
+      rcmd.mat = simd4x4f_create(
+          // clang-format off
+          simd4f_create(cinfo.second.scale[0] * 0.5f, 0.f, 0.f, 0.f),
+          simd4f_create(0.f, cinfo.second.scale[1] * 0.5f, 0.f, 0.f),
+          simd4f_create(0.f, 0.f, cinfo.second.scale[2] * 0.5f, 0.f),
+          simd4f_create(cinfo.second.pos[0], cinfo.second.pos[1], cinfo.second.pos[2], 1.f)
+          // clang-format on
+      );
+      srcmdbuf_add(cmdbuf, &rcmd);
+    }
+  }
+}

src/vissim.cc

@@ -19,6 +19,7 @@
 #include "imgui.h"
 #include "utils.h"
 #include "srender.h"
+#include "simulator.h"
 
 struct Timer {
   float mCurrentTime = 0.0f;
@@ -197,31 +198,7 @@ void DoRender() {
   rcmd.type = SRndrCmdTypeFrame;
   srcmdbuf_add(gRndrCmds, &rcmd);
 
-  rcmd.type = SRndrCmdTypeGrid;
-  simd4x4f_translation(&rcmd.mat, 10.f, 0.f, 10.f);
-  srcmdbuf_add(gRndrCmds, &rcmd);
-  simd4x4f_translation(&rcmd.mat, 10.f, 0.f, 0.f);
-  srcmdbuf_add(gRndrCmds, &rcmd);
-  simd4x4f_translation(&rcmd.mat, 10.f, 0.f, -10.f);
-  srcmdbuf_add(gRndrCmds, &rcmd);
-
-  simd4x4f_translation(&rcmd.mat, 0.f, 0.f, 10.f);
-  srcmdbuf_add(gRndrCmds, &rcmd);
-  simd4x4f_translation(&rcmd.mat, 0.f, 0.f, 0.f);
-  srcmdbuf_add(gRndrCmds, &rcmd);
-  simd4x4f_translation(&rcmd.mat, 0.f, 0.f, -10.f);
-  srcmdbuf_add(gRndrCmds, &rcmd);
-
-  simd4x4f_translation(&rcmd.mat, -10.f, 0.f, 10.f);
-  srcmdbuf_add(gRndrCmds, &rcmd);
-  simd4x4f_translation(&rcmd.mat, -10.f, 0.f, 0.f);
-  srcmdbuf_add(gRndrCmds, &rcmd);
-  simd4x4f_translation(&rcmd.mat, -10.f, 0.f, -10.f);
-  srcmdbuf_add(gRndrCmds, &rcmd);
-
-  srcmd_clear(&rcmd);
-  rcmd.type = SRndrCmdTypeSphere;
-  srcmdbuf_add(gRndrCmds, &rcmd);
+  simulator_draw(gRndrCmds);
 
   // Perform the actual render
   srndr_render(gRndr, gView, gRndrCmds);
@@ -318,6 +295,7 @@ int main(void) {
   gRndr = srndr_create();
   gView = srview_create();
   gRndrCmds = srcmdbuf_create(1024);
+  simulator_init();
 
   while (!glfwWindowShouldClose(gWindow)) {
     frame_counter++;
@@ -361,6 +339,8 @@ int main(void) {
 
     DoRender();
 
+    simulator_update(gTimer->mDeltaTime);
+
     glBindFramebuffer(GL_DRAW_FRAMEBUFFER, 0);
 
     // Rendering