3d orientation

main.cpp

@@ -13,8 +13,10 @@
 
 #include <SDL3/SDL.h>
 
-#include "math/vec2.hpp"
+#include "math/vec3.hpp"
-#include "math/mat2x2.hpp"
+#include "math/vec4.hpp"
+#include "math/mat3x3.hpp"
+#include "math/mat4x4.hpp"
 
 struct glyph {
   int32_t width;
@@ -158,26 +160,33 @@ int32_t render_text(SDL_Renderer * renderer, int32_t x, int32_t y, const char *
   return x_advance;
 }
 
-using vec2 = vec<2, float>;
+using vec3 = vec<3, float>;
-using mat2x2 = mat<2, 2, float>;
+using vec4 = vec<4, float>;
+using mat3x3 = mat<3, 3, float>;
+using mat4x4 = mat<4, 4, float>;
 
 struct line {
-  vec2 a;
+  vec3 a;
-  vec2 b;
+  vec3 b;
 };
 
+constexpr int num_lines = 5;
+
 struct state {
-  struct line line;
+  struct line line[num_lines];
-  vec2 normal;
+  vec3 normal;
-  vec2 mouse_position;
+  vec3 mouse_position;
 };
 
 struct state state = {
   .line = {
-    { 0, -0.5 },
+    {{ 0, -0.5,    0 }, { 0,  0.5,    0 }},
-    { 0,  0.5 },
+    {{ 0, -0.5, -0.5 }, { 0,  0.5, -0.5 }},
+    {{ 0, -0.5,  0.5 }, { 0,  0.5,  0.5 }},
+    {{ 0,  0.5,  0.5 }, { 0,  0.5, -0.5 }},
+    {{ 0, -0.5,  0.5 }, { 0, -0.5, -0.5 }},
   },
-  .normal = { -1, 0 },
+  .normal = { -1, 0, 0 },
 };
 
 static int window_width = 1;
@@ -188,23 +197,48 @@ static inline int min(int a, int b)
   return (a > b) ? b : a;
 }
 
-vec2 transform_vertex(vec2 v, float scale)
+const float deg = 0.017453292519943295;
+float deg45 = 0.7853981633974483;
+static float vtheta = 0;
+
+vec3 transform_vertex(vec3 v, float scale)
 {
   float dim = ((float)min(window_height, window_width)) / 2.0f;
 
-  float x = v.x;
+  v = v * scale;
-  float y = v.y;
+
+  mat3x3 rot1 = {
+    cos(vtheta), -sin(vtheta), 0,
+    sin(vtheta), cos(vtheta), 0,
+    0, 0, 1,
+  };
+  mat3x3 rot2 = {
+    1, 0, 0,
+    0, cos(deg45), -sin(deg45),
+    0, sin(deg45), cos(deg45),
+  };
+  mat3x3 rot3 = {
+    cos(-deg45), 0, sin(-deg45),
+    0, 1, 0,
+
+    sin(-deg45), 0, cos(-deg45),
+  };
+  (void)rot1;   (void)rot2;   (void)rot3;
+
+  v = rot1 * v;
+  //v = rot3 * v;
+  v = rot2 * v;
+  //
 
-  x *= scale;
-  y *= scale;
 
   return {
-    x * dim + window_width / 2.0f,
+    v.x * dim + window_width / 2.0f,
-    y * dim + window_height / 2.0f,
+    v.y * dim + window_height / 2.0f,
+    v.z,
   };
 }
 
-vec2 inverse_transform(float x, float y)
+vec3 inverse_transform(float x, float y)
 {
   float dim = ((float)min(window_height, window_width)) / 2.0f;
   assert(dim != 0);
@@ -212,6 +246,7 @@ vec2 inverse_transform(float x, float y)
   return {
     -(window_width - 2 * x) / (2 * dim),
     -(window_height - 2 * y) / (2 * dim),
+    0,
   };
 }
 
@@ -228,42 +263,119 @@ static inline void render_line(SDL_Renderer * renderer, struct line line)
   assert(SDL_RenderLine(renderer, line.a.x, line.a.y, line.b.x, line.b.y));
 }
 
+void render_basis(SDL_Renderer * renderer)
+{
+  // magenta: Z
+  assert(SDL_SetRenderDrawColorFloat(renderer, 1, 0, 1, 1));
+  render_line(renderer, transform_line({{0, 0, 0}, {0, 0, 1}}, 1.0f));
+
+  // yellow: Y
+  assert(SDL_SetRenderDrawColorFloat(renderer, 1, 1, 0, 1));
+  render_line(renderer, transform_line({{0, 0, 0}, {0, 1, 0}}, 1.0f));
+
+  // cyan: X
+  assert(SDL_SetRenderDrawColorFloat(renderer, 0, 1, 1, 1));
+  render_line(renderer, transform_line({{0, 0, 0}, {1, 0, 0}}, 1.0f));
+}
+
+mat3x3 rotate_to(vec3 old_normal, vec3 new_normal)
+{
+  vec3 s = cross(old_normal, new_normal);
+  float c = dot(old_normal, new_normal);
+  mat3x3 i = mat3x3();
+
+  mat3x3 v = {
+    0,    -s.z,  s.y,
+    s.z,     0, -s.x,
+    -s.y,  s.x,    0,
+  };
+
+  float mag = magnitude(s);
+
+  mat3x3 v2 = v * v;
+
+  float c_mag2 = (1 - c) / (mag * mag);
+
+  mat3x3 v2_c_mag2 = (v2 * c_mag2);
+
+  mat3x3 t = i + v + v2_c_mag2;
+
+  return t;
+}
+
+mat4x4 look_at(vec3 eye, vec3 center, vec3 up)
+{
+  vec3 x;
+  vec3 y;
+  vec3 z;
+
+  z = eye - center;
+  z = z / magnitude(z);
+
+  y = up;
+
+  x = cross(y, z);
+  y = cross(z, x);
+
+  x = x / magnitude(x);
+  y = y / magnitude(y);
+
+  mat4x4 t = {
+    x.x, x.y, x.z, -dot(x, eye),
+    y.x, y.y, y.z, -dot(y, eye),
+    z.x, z.y, z.z, -dot(z, eye),
+    0,   0,   0,   1.0f
+  };
+
+  return t;
+}
+
 void render_lines(SDL_Renderer * renderer)
 {
+  render_basis(renderer);
+
   // line
   assert(SDL_SetRenderDrawColorFloat(renderer, 1, 0.5, 0.5, 1));
-  struct line tl = transform_line(state.line, 1.0f);
+  for (int i = 0; i < num_lines; i++) {
+    struct line tl = transform_line(state.line[i], 1.0f);
     render_line(renderer, tl);
+  }
 
   // normal
   assert(SDL_SetRenderDrawColorFloat(renderer, 1, 0, 0, 1));
-  struct line normal_line = {{0, 0}, state.normal};
+  struct line normal_line = {{0, 0, 0}, state.normal};
   render_line(renderer, transform_line(normal_line, 0.5f));
 
   // mouse
   assert(SDL_SetRenderDrawColorFloat(renderer, 0, 0, 1, 1));
-  struct line mouse_line = {{0, 0}, state.mouse_position};
+  struct line mouse_line = {{0, 0, 0}, state.mouse_position};
   render_line(renderer, transform_line(mouse_line, 0.5f));
 
   // foo
-  float s = cross(state.normal, -state.mouse_position);
+  mat3x3 t = rotate_to(state.normal, -state.mouse_position);
-  float c = dot(state.normal, -state.mouse_position);
+  vec3 nr = t * state.normal;
-  mat2x2 r = {
+  //mat4x4 t = look_at({0, 0, 0}, state.mouse_position - state.normal, {0, 0, 1});
-    c, -s,
+  //vec4 nr4 = t * (vec4){state.normal.x, state.normal.y, state.normal.z, 0};
-    s, c,
+  //vec3 nr = {nr.x, nr.y, nr.z};
-  };
-
-  vec2 nr = r * state.normal;
   assert(SDL_SetRenderDrawColorFloat(renderer, 0, 1, 0, 1));
-  struct line nr_line = {{0, 0}, nr};
+  struct line nr_line = {{0, 0, 0}, nr};
   render_line(renderer, transform_line(nr_line, 0.5f));
 
   {
       assert(SDL_SetRenderDrawColorFloat(renderer, 0.5, 1, 0.5, 1));
-      vec2 ar = r * state.line.a;
+      for (int i = 0; i < num_lines; i++) {
-      vec2 br = r * state.line.b;
+        struct line l = state.line[i];
-      struct line tl = transform_line({ar, br}, 1.0f);
+        /*
-      render_line(renderer, tl);
+        vec4 a4 = t * l.a;
+        vec4 b4 = t * l.b;
+        vec3 a = {a4.x, a4.y, a4.z};
+        vec3 b = {b4.x, b4.y, b4.z};
+        */
+        vec3 a = t * l.a;
+        vec3 b = t * l.b;
+        struct line tl = {a, b};
+        render_line(renderer, transform_line(tl, 1.0f));
+      }
   }
 }
 
@@ -275,10 +387,36 @@ void render_text_state(SDL_Renderer * renderer)
 
 
   //float d = dot(state.normal, state.mouse_position);
+  /*
   float d = dot({0, 1}, state.mouse_position);
   char buf[64];
   int len = snprintf(buf, 64, "%.03f", d);
   x_advance = render_text(renderer, x_advance, y_advance, buf, len);
+  */
+  vec3 z = transform_vertex({0, 0, 1}, 1.1);
+  vec3 y = transform_vertex({0, 1, 0}, 1.1);
+  vec3 x = transform_vertex({1, 0, 0}, 1.1);
+
+  render_text(renderer, ((z.x - 30) * 64), ((z.y + 10) * 64), "+z", 2);
+  render_text(renderer, ((y.x - 30) * 64), ((y.y + 10) * 64), "+y", 2);
+  render_text(renderer, ((x.x - 30) * 64), ((x.y + 10) * 64), "+x", 2);
+}
+
+static float theta = 0;
+
+void update_mouse_position()
+{
+  vec3 pos = {0, 1, 1};
+  pos = pos / magnitude(pos);
+
+  mat3x3 rot1 = {
+    cos(theta), -sin(theta), 0,
+    sin(theta), cos(theta), 0,
+    0, 0, 1,
+  };
+
+  state.mouse_position = rot1 * pos;
+  theta += deg;
 }
 
 int main()
@@ -301,7 +439,7 @@ int main()
     const char * s = SDL_GetRenderDriver(i);
     printf("  %s\n", s);
   }
-  renderer = SDL_CreateRenderer(window, "software");
+  renderer = SDL_CreateRenderer(window, "opengles2");
   SDL_SetRenderDrawColor(renderer, 0, 0, 0, 255);
 
   SDL_PropertiesID props = SDL_GetRendererProperties(renderer);
@@ -344,15 +482,17 @@ int main()
 	break;
       case SDL_EVENT_MOUSE_BUTTON_DOWN:
         if (event.button.button == 1) {
-          vec2 mv = inverse_transform(event.button.x, event.button.y);
+          //vec3 mv = inverse_transform(event.button.x, event.button.y);
-          float m = magnitude(mv);
+          //float m = magnitude(mv);
-          state.mouse_position = mv / m;
+          //state.mouse_position = mv / m;
         }
 	break;
       default:
 	break;
       }
     }
+    update_mouse_position();
+    vtheta += deg / 10;
   }
 
  exit: