diff --git a/CMakeLists.txt b/CMakeLists.txt index d396161..e66cf10 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/simulator.cc ) # Tests diff --git a/include/simulator.h b/include/simulator.h new file mode 100644 index 0000000..e328053 --- /dev/null +++ b/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 \ No newline at end of file diff --git a/src/simulator.cc b/src/simulator.cc new file mode 100644 index 0000000..3886c62 --- /dev/null +++ b/src/simulator.cc @@ -0,0 +1,109 @@ +// clang-format off +// glad must be included before any other OpenGL libraries +#include +// clang-format on + +#include + +#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 Matrix33f; +typedef SimpleMath::Matrix Vector3f; +typedef SimpleMath::Matrix Matrix44f; +typedef SimpleMath::Matrix 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); + } + } +} \ No newline at end of file diff --git a/src/vissim.cc b/src/vissim.cc index 6435baf..1643106 100644 --- a/src/vissim.cc +++ b/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