Added own camera object and its vectorial dependency.

src/Camera.c

@@ -0,0 +1,158 @@
+#include "Camera.h"
+
+#include <assert.h>
+
+#include "string.h"
+#include "vectorial/simd4x4f.h"
+
+inline void Camera_Init(Camera* camera) {
+  // clang-format off
+  static float mtx_identity[16] = {
+      1.f, 0.f, 0.f, 0.f,
+      0.f, 1.f, 0.f, 0.f,
+      0.f, 0.f, 1.f, 0.f,
+      0.f, 0.f, 0.f, 1.f
+  };
+  // clang-format on
+  camera->near = 0.01;
+  camera->far = 1000.0;
+  camera->fov = 90 * M_PI / 180.f;
+
+  camera->forward[0] = -1.f;
+  camera->forward[1] = 0.f;
+  camera->forward[2] = -1.f;
+
+  camera->right[0] = 1.f;
+  camera->right[1] = 0.f;
+  camera->right[2] = 0.f;
+
+  camera->up[0] = 0.f;
+  camera->up[1] = 1.f;
+  camera->up[2] = 0.f;
+
+  camera->pos[0] = 2.f;
+  camera->pos[1] = 1.2f;
+  camera->pos[2] = 2.f;
+
+  camera->vel[0] = 0.f;
+  camera->vel[1] = 0.f;
+  camera->vel[2] = 0.f;
+
+  camera->heading = -45.0 * M_PI / 180.0f;
+  camera->pitch = 10 * M_PI / 180.0f;
+
+  memcpy(&camera->mtxView, &mtx_identity, sizeof(camera->mtxView));
+  Camera_CalcToMatrix(camera, &camera->mtxView);
+  Camera_CalcFromMatrix(camera, &camera->mtxView);
+}
+
+void Camera_CalcFromMatrix(Camera* camera, float* mat) {
+  simd4x4f mtx;
+  simd4x4f_uload(&mtx, mat);
+
+  camera->forward[0] = mtx.x[2];
+  camera->forward[1] = mtx.y[2];
+  camera->forward[2] = mtx.z[2];
+
+  camera->right[0] = mtx.x[0];
+  camera->right[1] = mtx.y[0];
+  camera->right[2] = mtx.z[0];
+
+  camera->heading = atan2(-camera->forward[2], camera->forward[0]);
+  camera->pitch = asin(camera->forward[1]);
+
+  simd4x4f rot_mat = mtx;
+  rot_mat.w = simd4f_create(0.f, 0.f, 0.f, 1.f);
+  simd4x4f_transpose_inplace(&rot_mat);
+
+  simd4f eye;
+  simd4x4f_matrix_point3_mul(&rot_mat, &mtx.w, &eye);
+
+  camera->pos[0] = -simd4f_get_x(eye);
+  camera->pos[1] = -simd4f_get_y(eye);
+  camera->pos[2] = -simd4f_get_z(eye);
+
+//  gLog ("ViewMat");
+//  gLog ("%f, %f, %f, %f", mtx->x[0], mtx->x[1], mtx->x[2], mtx->x[3]);
+//  gLog ("%f, %f, %f, %f", mtx->y[0], mtx->y[1], mtx->y[2], mtx->y[3]);
+//  gLog ("%f, %f, %f, %f", mtx->z[0], mtx->z[1], mtx->z[2], mtx->z[3]);
+//  gLog ("%f, %f, %f, %f", mtx->w[0], mtx->w[1], mtx->w[2], mtx->w[3]);
+}
+
+void Camera_CalcToMatrix(Camera* camera, float* mat) {
+  float sp = sin(camera->pitch);
+  float cp = cos(camera->pitch);
+  float ch = cos(camera->heading);
+  float sh = sin(camera->heading);
+
+  const float d = 10.0f;
+
+  simd4f eye = simd4f_create (camera->pos[0], camera->pos[1], camera->pos[2], 1.f);
+  simd4f forward = simd4f_create (-cp * ch, -sp, cp * sh, 0.f);
+  simd4f right = simd4f_cross3 (forward, simd4f_create (0.f, 1.f, 0.f, 1.f));
+  simd4f up = simd4f_cross3(right, forward);
+  simd4f center = simd4f_add(simd4f_mul(forward, simd4f_splat(d)), eye);
+
+  camera->forward[0] = -simd4f_get_x(forward);
+  camera->forward[1] = -simd4f_get_y(forward);
+  camera->forward[2] = -simd4f_get_z(forward);
+
+  camera->right[0] = simd4f_get_x(right);
+  camera->right[1] = simd4f_get_y(right);
+  camera->right[2] = simd4f_get_z(right);
+
+  simd4x4f mtx;
+  simd4x4f_lookat(&mtx, eye, center, up);
+
+  simd4f_ustore4(mtx.x, mat);
+  simd4f_ustore4(mtx.y, mat +4);
+  simd4f_ustore4(mtx.z, mat +8);
+  simd4f_ustore4(mtx.w, mat +12);
+}
+
+inline void Camera_Update(
+    Camera* camera,
+    int width,
+    int height,
+    float dt,
+    float mouse_dx,
+    float mouse_dy,
+    float accel[3]) {
+  assert(camera);
+  assert((width > 0) && (height > 0));
+  const float w = (float) width;
+  const float h = (float) height;
+  simd4x4f proj;
+  simd4x4f_perspective(&proj, camera->fov, w/h, camera->near, camera->far);
+  simd4f_ustore4(proj.x, camera->mtxProj);
+  simd4f_ustore4(proj.y, camera->mtxProj +4);
+  simd4f_ustore4(proj.z, camera->mtxProj +8);
+  simd4f_ustore4(proj.w, camera->mtxProj +12);
+
+  if (mouse_dx != 0.f || mouse_dy != 0.f || accel != NULL) {
+    const float mouse_sensitivity = 20.0f;
+
+    camera->heading -= dt * mouse_dx * mouse_sensitivity * M_PI / 180.f;
+    if (camera->heading < -M_PI) {
+      camera->heading += M_PI * 2.f;
+    } else if (camera->heading > M_PI) {
+      camera->heading -= M_PI * 2.f;
+    }
+    camera->pitch += dt * mouse_dy * mouse_sensitivity * M_PI / 180.f;
+    if (camera->pitch < -M_PI * 0.49) {
+      camera->pitch = -M_PI * 0.49;
+    } else if (camera->pitch > M_PI * 0.49) {
+      camera->pitch = M_PI * 0.49;
+    }
+
+    for (int i = 0; i < 3; i++) {
+      camera->vel[i] += dt * accel[0] * camera->forward[i]
+                        + dt * accel[2] * camera->right[i]
+                        + dt * accel[1] * camera->up[i];
+      camera->pos[i] += dt * camera->vel[i];
+      camera->vel[i] = camera->vel[i] * 0.1;
+    }
+
+    Camera_CalcToMatrix(camera, &camera->mtxView);
+  }
+}

src/Camera.h

@@ -0,0 +1,43 @@
+//
+// Created by martin on 19.10.21.
+//
+
+#ifndef RBDLSIM_RENDER_UTILS_H
+#define RBDLSIM_RENDER_UTILS_H
+
+#ifdef __cplusplus
+extern "C" {
+#endif
+
+typedef struct {
+  float mtxProj[16];
+  float mtxView[16];
+  float near;
+  float far;
+  float fov;
+  float heading;
+  float pitch;
+  float vel[3];
+  float pos[3];
+  float forward[3];
+  float right[3];
+  float up[3];
+} Camera;
+
+void Camera_Init(Camera* camera);
+void Camera_CalcFromMatrix(Camera* camera, float* mtx);
+void Camera_CalcToMatrix(Camera* camera, float* mtx);
+void Camera_Update(
+    Camera* camera,
+    int width,
+    int height,
+    float dt,
+    float mouse_dx,
+    float mouse_dy,
+    float accel[3]);
+
+#ifdef __cplusplus
+}
+#endif
+
+#endif  //RBDLSIM_RENDER_UTILS_H

src/main.cc

@@ -14,6 +14,7 @@
 #define GLFW_INCLUDE_NONE
 #include <iostream>
 
+#include "Camera.h"
 #include "GLFW/glfw3.h"
 
 const int Width = 1024;
@@ -68,7 +69,7 @@ typedef struct {
 static struct {
   std::unique_ptr<ozz_t> ozz;
   sg_pass_action pass_action;
-  camera_t camera;
+  Camera camera;
   struct {
     bool skeleton;
     bool animation;
@@ -85,6 +86,25 @@ static struct {
   } time;
 } state;
 
+typedef struct {
+  int32_t mousedX;
+  int32_t mousedY;
+  int32_t mouseX;
+  int32_t mouseY;
+  uint8_t mouseButton;
+  int32_t mouseScroll;
+  char key;
+} GuiInputState;
+
+GuiInputState gGuiInputState = {0, 0, 0, 0, 0, 0, 0};
+
+enum class ControlMode {
+  ControlModeNone,
+  ControlModeFPS
+};
+
+ControlMode gControlMode = ControlMode::ControlModeNone;
+
 // io buffers for skeleton and animation data files, we know the max file size upfront
 static uint8_t skel_data_buffer[4 * 1024];
 static uint8_t anim_data_buffer[32 * 1024];
@@ -93,11 +113,35 @@ static void load_skeleton(void);
 static void load_animation(void);
 static void eval_animation(void);
 static void draw_skeleton(void);
+static void draw_grid(void);
 static void draw_ui(void);
 // static void skeleton_data_loaded(const sfetch_response_t* response);
 // static void animation_data_loaded(const sfetch_response_t* response);
 static void frame(void);
 
+void handle_mouse(GLFWwindow* w, GuiInputState* io_input_state) {
+  if (!glfwGetWindowAttrib(w, GLFW_FOCUSED)) {
+    return;
+  }
+
+  double mouse_x, mouse_y;
+  glfwGetCursorPos(w, &mouse_x, &mouse_y);
+
+  if (io_input_state->mouseButton) {
+    io_input_state->mousedX = int32_t(mouse_x) - io_input_state->mouseX;
+    io_input_state->mousedY = int32_t(mouse_y) - io_input_state->mouseY;
+  } else {
+    io_input_state->mousedX = 0;
+    io_input_state->mousedY = 0;
+  }
+  io_input_state->mouseX = int32_t(mouse_x);
+  io_input_state->mouseY = int32_t(mouse_y);
+
+  io_input_state->mouseButton = glfwGetMouseButton(w, 0)
+                                + (glfwGetMouseButton(w, 1) << 1)
+                                + (glfwGetMouseButton(w, 2) << 2);
+}
+
 int main() {
   // window and GL context via GLFW and flextGL
   glfwInit();
@@ -106,7 +150,7 @@ int main() {
   glfwWindowHint(GLFW_OPENGL_FORWARD_COMPAT, GLFW_TRUE);
   glfwWindowHint(GLFW_OPENGL_PROFILE, GLFW_OPENGL_CORE_PROFILE);
   glfwWindowHint(GLFW_COCOA_RETINA_FRAMEBUFFER, GLFW_FALSE);
-  GLFWwindow* w = glfwCreateWindow(Width, Height, "Sokol+ImGui+GLFW", 0, 0);
+  GLFWwindow* w = glfwCreateWindow(Width, Height, "AnimTestbed", 0, 0);
   glfwMakeContextCurrent(w);
   glfwSwapInterval(1);
 
@@ -154,16 +198,7 @@ int main() {
   state.ozz = std::make_unique<ozz_t>();
   state.time.factor = 1.0f;
 
-  // initialize camera helper
-  camera_desc_t camdesc = {};
-  camdesc.min_dist = 1.0f;
-  camdesc.max_dist = 100.0f;
-	camdesc.farz = 1000.0f;
-  camdesc.center.Y = 1.0f;
-  camdesc.distance = 30.0f;
-  camdesc.latitude = 10.0f;
-  camdesc.longitude = 20.0f;
-  cam_init(&state.camera, &camdesc);
+  Camera_Init(&state.camera);
 
   // setup Dear Imgui
   ImGui::CreateContext();
@@ -265,7 +300,7 @@ int main() {
 
   // initial clear color
   pass_action.colors[0].action = SG_ACTION_CLEAR;
-  pass_action.colors[0].value = {0.0f, 0.5f, 0.7f, 1.0f};
+  pass_action.colors[0].value = {0.1f, 0.1f, 0.1f, 1.0f};
 
   load_skeleton();
   load_animation();
@@ -278,14 +313,81 @@ int main() {
     int cur_width, cur_height;
     glfwGetFramebufferSize(w, &cur_width, &cur_height);
 
-    cam_update(&state.camera, cur_width, cur_height);
-
     // this is standard ImGui demo code
     ImGuiIO& io = ImGui::GetIO();
     io.DisplaySize = ImVec2(float(cur_width), float(cur_height));
     io.DeltaTime = (float)stm_sec(stm_laptime(&last_time));
     ImGui::NewFrame();
 
+    ImGui::Begin("Camera");
+    ImGui::SliderFloat3("pos", state.camera.pos, -100.f, 100.f);
+    ImGui::SliderFloat("near", &state.camera.near, 0.001f, 10.f);
+    ImGui::SliderFloat("far", &state.camera.far, 1.0f, 10000.f);
+    ImGui::SliderFloat("heading", &state.camera.heading, -180.0f, 180.f);
+    ImGui::SliderFloat("pitch", &state.camera.pitch, -179.0f, 179.f);
+    ImGui::End();
+
+    // handle input
+    handle_mouse (w, &gGuiInputState);
+
+    if (glfwGetMouseButton(w, GLFW_MOUSE_BUTTON_RIGHT)) {
+      if (gControlMode == ControlMode::ControlModeNone) {
+        gControlMode = ControlMode::ControlModeFPS;
+        Camera_CalcFromMatrix(&state.camera, &state.camera.mtxView[0]);
+        glfwSetInputMode(w, GLFW_CURSOR, GLFW_CURSOR_DISABLED);
+      }
+    } else {
+      gControlMode = ControlMode::ControlModeNone;
+      glfwSetInputMode(w, GLFW_CURSOR, GLFW_CURSOR_NORMAL);
+      Camera_Update(
+          &state.camera,
+          cur_width,
+          cur_height,
+          state.time.frame,
+          0,
+          0,
+          nullptr);
+    }
+
+    if (gControlMode == ControlMode::ControlModeFPS) {
+      float camera_accel[3] = {0.f, 0.f, 0.f};
+      float accel_scale = 100.0;
+
+      if (glfwGetKey(w, GLFW_KEY_LEFT_SHIFT)) {
+        accel_scale *= 3.;
+      } else if (glfwGetKey(w, GLFW_KEY_LEFT_CONTROL)) {
+        accel_scale /= 3.;
+      }
+
+      if (glfwGetKey(w, GLFW_KEY_W)) {
+        camera_accel[0] -= accel_scale;
+      }
+      if (glfwGetKey(w, GLFW_KEY_S)) {
+        camera_accel[0] += accel_scale;
+      }
+      if (glfwGetKey(w, GLFW_KEY_C)) {
+        camera_accel[1] -= accel_scale;
+      }
+      if (glfwGetKey(w, GLFW_KEY_SPACE)) {
+        camera_accel[1] += accel_scale;
+      }
+      if (glfwGetKey(w, GLFW_KEY_A)) {
+        camera_accel[2] -= accel_scale;
+      }
+      if (glfwGetKey(w, GLFW_KEY_D)) {
+        camera_accel[2] += accel_scale;
+      }
+
+      Camera_Update(
+          &state.camera,
+          cur_width,
+          cur_height,
+          state.time.frame,
+          gGuiInputState.mousedX,
+          gGuiInputState.mousedY,
+          camera_accel);
+    }
+
     if (ImGui::BeginMainMenuBar()) {
       ImGui::Text("AnimTestbed");
       ImGui::Checkbox("ImGui Demo", &show_imgui_demo_window);
@@ -301,14 +403,6 @@ int main() {
       ImGui::ShowDemoWindow();
     }
 
-    ImGui::Begin("Camera");
-    ImGui::SliderFloat("min_dist", &state.camera.min_dist, 1.0f, 100.f);
-    ImGui::SliderFloat("max_dist", &state.camera.max_dist, 1.0f, 100.f);
-    ImGui::SliderFloat("center.Y", &state.camera.center.Y, 1.0f, 100.f);
-    ImGui::SliderFloat("distance", &state.camera.distance, 1.0f, 1000.f);
-    ImGui::SliderFloat("latitude", &state.camera.latitude, 1.0f, 100.f);
-    ImGui::SliderFloat("longitude", &state.camera.longitude, -179.0f, 179.f);
-    ImGui::End();
 
     // the sokol_gfx draw pass
     sg_begin_default_pass(&pass_action, cur_width, cur_height);
@@ -433,6 +527,8 @@ static void draw_ui() {
 }
 
 static void frame() {
+  draw_grid();
+
   if (state.loaded.animation && state.loaded.skeleton) {
     if (!state.time.paused) {
       state.time.absolute += state.time.frame * state.time.factor;
@@ -516,14 +612,50 @@ static void draw_joint(int joint_index, int parent_joint_index) {
   draw_line(p5, p2);
 }
 
+static void draw_grid(void) {
+  sgl_defaults();
+  sgl_matrix_mode_projection();
+  sgl_load_matrix((const float*)&state.camera.mtxProj);
+  sgl_matrix_mode_modelview();
+  sgl_load_matrix((const float*)&state.camera.mtxView);
+
+  const int grid_size = 10;
+
+  sgl_begin_lines();
+  sgl_c3f(0.4f, 0.4f, 0.4f);
+  for (int i = -grid_size; i <= grid_size; i++) {
+    if (i == 0) {
+      continue;
+    }
+    ozz::math::SimdFloat4 p0 = ozz::math::simd_float4::Load(i * 1.0f, 0.f, -grid_size * 1.0f, 1.f);
+    ozz::math::SimdFloat4 p1 = ozz::math::simd_float4::Load(i * 1.0f, 0.f, grid_size * 1.0f, 1.f);
+    draw_line(p0, p1);
+
+    p0 = ozz::math::simd_float4::Load(-grid_size * 1.0f, 0.f, i * 1.0f, 1.f);
+    p1 = ozz::math::simd_float4::Load(grid_size * 1.0f, 0.f, i * 1.0f, 1.f);
+    draw_line(p0, p1);
+  }
+  sgl_c3f (0.7f, 0.4f, 0.2f);
+  ozz::math::SimdFloat4 p0 = ozz::math::simd_float4::Load(0, 0.f, -grid_size * 1.0f, 1.f);
+  ozz::math::SimdFloat4 p1 = ozz::math::simd_float4::Load(0, 0.f, grid_size * 1.0f, 1.f);
+  draw_line(p0, p1);
+
+  sgl_c3f (0.2f, 0.4f, 0.7f);
+  p0 = ozz::math::simd_float4::Load(-grid_size * 1.0f, 0.f, 0.f, 1.f);
+  p1 = ozz::math::simd_float4::Load(grid_size * 1.0f, 0.f, 0.f, 1.f);
+  draw_line(p0, p1);
+
+  sgl_end();
+}
+
 static void draw_skeleton(void) {
   sgl_defaults();
   sgl_matrix_mode_projection();
-  sgl_load_matrix((const float*)&state.camera.proj);
+  sgl_load_matrix((const float*)&state.camera.mtxProj);
   sgl_matrix_mode_modelview();
-	hmm_mat4 scale_mat = HMM_Scale (HMM_Vec3(0.1f, 0.1f, 0.1f));
-  sgl_load_matrix((const float*)&state.camera.view);
-	sgl_mult_matrix((const float*)&scale_mat);
+  hmm_mat4 scale_mat = HMM_Scale(HMM_Vec3(0.01f, 0.01f, 0.01f));
+  sgl_load_matrix((const float*)&state.camera.mtxView);
+  sgl_mult_matrix((const float*)&scale_mat);
 
   const int num_joints = state.ozz->skeleton.num_joints();
   ozz::span<const int16_t> joint_parents = state.ozz->skeleton.joint_parents();