/apps/demos/scenery: vehicle...

apps/demos/scenery/scenes/ingame.c

@@ -19,10 +19,144 @@
 #define PLAYER_HEIGHT 0.6f
 #define TREE_DENSITY 0.03f
 
+#define G_CONST 0.1f
+
 /* TODO: pregenerate grid of levels of detail */
 static float heightmap[TERRAIN_DISTANCE][TERRAIN_DISTANCE];
 
 
+/* vehicle sim ! */
+/* https://www.youtube.com/watch?v=pwbwFdWBkU0 */
+/* representation is a "jelly" box with spring configuration trying to make it coherent */
+
+/* == springs == */
+/* damped spring: F = -kx - cv */
+/* x = length(p1-p0) - at_rest_length */
+/* v = dot(v1 - v0, unit(p1-p0)) */
+/* F = (-kx - cv) * unit(p1-p0) */
+/* v += (F/m)*t */
+/* one points gains positive F, other negative F, to come together */
+
+/* == ground interaction == */
+/* if point is under terrain, then apply this: */
+/* x = y difference on point and ground */
+/* -x(n) = x * normal */
+/* -v(n) = dot(v, normal) */
+/* -F = (-kx(n)-cv(n)) * normal */
+
+/* == friction == */
+/* v(o)/F(o) are perpendicular to slope (x - x(n)) */
+/* if v(o) == 0, then */
+/*     -- at rest, static friction overcomes */
+/*     if F(o) <= f(s)*x(n), F(o) = 0 */
+/*     else, F(o) -= f(k) * x(n) */
+/* else if length(v(o) + (F(o)/m)*t) <= (f(k)*x(n)*t), v(o) = 0 */
+/*     else, F = -unit(v(o)*f(k)*x(n)) */
+
+#define VEHICLE_MASS 10.0f
+#define VEHICLE_LENGTH 3.0f
+#define VEHICLE_WIDTH 1.7f
+#define VEHICLE_HEIGHT 1.3f
+/* spring constant */
+#define VEHICLE_SPRING_K 20.0f
+/* damping constant */
+#define VEHICLE_SPRING_C 50.0f
+
+/* initial, ideal corner positions */
+static const Vec3 vbpi[8] = {
+    [0] = { 0, 0, 0 },
+    [1] = { VEHICLE_LENGTH, 0, 0 },
+    [2] = { VEHICLE_LENGTH, 0, VEHICLE_WIDTH },
+    [3] = { 0, 0, VEHICLE_WIDTH },
+    [4] = { 0, VEHICLE_HEIGHT, 0 },
+    [5] = { VEHICLE_LENGTH, VEHICLE_HEIGHT, 0 },
+    [6] = { VEHICLE_LENGTH, VEHICLE_HEIGHT, VEHICLE_WIDTH },
+    [7] = { 0, VEHICLE_HEIGHT, VEHICLE_WIDTH },
+};
+/* corner positions in simulation */
+static Vec3 vbp[8] = { vbpi[0], vbpi[1], vbpi[2], vbpi[3],
+                       vbpi[4], vbpi[5], vbpi[6], vbpi[7], };
+/* corner velocities */
+static Vec3 vbv[8];
+/* springs */
+static uint8_t vbs[28][2] = {
+    {0, 1}, {4, 5}, {0, 4},
+    {1, 2}, {5, 6}, {1, 5},
+    {2, 3}, {6, 7}, {2, 6},
+    {3, 0}, {7, 4}, {3, 7},
+
+    {0, 2}, {0, 5}, {0, 7},
+    {1, 3}, {1, 6}, {1, 4},
+    {4, 6}, {2, 7}, {2, 5},
+    {5, 7}, {3, 4}, {3, 6},
+
+    {0, 6}, {1, 7}, {2, 4}, {3, 5},
+};
+
+
+static float height_at(SceneIngame *scn, Vec2 position);
+static Vec3 normal_at(SceneIngame *scn, Vec2 position);
+
+
+static void draw_vehicle(SceneIngame *scn) {
+    for (size_t i = 0; i < 12; ++i)
+        draw_line_3d(vbp[vbs[i][0]], vbp[vbs[i][1]], 1, (Color){255, 255, 255, 255});
+    for (size_t i = 12; i < 24; ++i)
+        draw_line_3d(vbp[vbs[i][0]], vbp[vbs[i][1]], 1, (Color){200, 200, 200, 255});
+    for (size_t i = 24; i < 28; ++i)
+        draw_line_3d(vbp[vbs[i][0]], vbp[vbs[i][1]], 1, (Color){255, 125, 125, 255});
+}
+
+
+static void process_vehicle(SceneIngame *scn) {
+    /* apply gravity */
+    Vec3 const Fg = { .y = -VEHICLE_MASS * G_CONST };
+    for (size_t i = 0; i < 8; ++i) {
+        Vec3 const vd = vec3_scale(Fg, (1.0f / VEHICLE_MASS) * ctx.frame_duration);
+        vbv[i] = vec3_add(vbv[i], vd);
+    }
+
+    /* apply springs */
+    for (size_t i = 0; i < 24; ++i) {
+        Vec3 const p0 = vbp[vbs[i][0]];
+        Vec3 const p1 = vbp[vbs[i][1]];
+        Vec3 const v0 = vbv[vbs[i][0]];
+        Vec3 const v1 = vbv[vbs[i][1]];
+        Vec3 const pd = vec3_sub(p1, p0);
+        Vec3 const pn = vec3_norm(pd);
+        /* TODO: length at rest could be precalculated */
+        float const lar = vec3_length(vec3_sub(vbpi[vbs[i][1]], vbpi[vbs[i][0]]));
+        float const x = vec3_length(pd) - lar;
+        float const v = vec3_dot(vec3_sub(v1, v0), pn);
+        Vec3 const Fs = vec3_scale(pn, -VEHICLE_SPRING_K * x - VEHICLE_SPRING_C * v);
+        Vec3 const vd = vec3_scale(Fs, (1.0f / VEHICLE_MASS) * ctx.frame_duration);
+
+        vbv[vbs[i][0]] = vec3_sub(vbv[vbs[i][0]], vd);
+        vbv[vbs[i][1]] = vec3_add(vbv[vbs[i][1]], vd);
+    }
+
+    /* spring out points that are underground */
+    for (size_t i = 0; i < 8; ++i) {
+        Vec3 const p = vbp[i];
+        Vec3 const v = vbv[i];
+        float const h = height_at(scn, (Vec2){ p.x, p.z });
+        if (h > p.y) {
+            Vec3 const n = normal_at(scn, (Vec2){ p.x, p.z });
+            float const xn = (h - p.y) * n.y;
+            float const vn = vec3_dot(v, n);
+            Vec3 const Fd = vec3_scale(n, -VEHICLE_SPRING_K * xn - VEHICLE_SPRING_C * vn);
+            vbv[i] = vec3_add(vbv[i], vec3_scale(Fd, -(1.0f / VEHICLE_MASS) * ctx.frame_duration));
+        }
+    }
+
+    /* apply velocity */
+    for (size_t i = 0; i < 8; ++i) {
+        vbp[i] = vec3_add(vbp[i], vbv[i]);
+        vbv[i] = vec3_scale(vbv[i], 0.99f); /* friction... */
+    }
+}
+
+
 static void process_fly_mode(State *state) {
     SceneIngame *scn = (SceneIngame *)state->scene;
 
@@ -52,7 +186,22 @@ static void process_fly_mode(State *state) {
         scn->pos.y -= speed;
 }
 
+/* TODO: could be baked in map format */
+static Vec3 normal_at(SceneIngame *scn, Vec2 position) {
+    int const x = (int)(HALF_TERRAIN_DISTANCE + (position.x - scn->pos.x));
+    int const y = (int)(HALF_TERRAIN_DISTANCE + (position.y - scn->pos.z));
 
+    float const height0 = heightmap[x][y];
+    float const height1 = heightmap[x + 1][y];
+    float const height2 = heightmap[x][y + 1];
+
+    Vec3 const a = { .x = 1, .y = height0 - height1, .z = 0 };
+    Vec3 const b = { .x = 0, .y = height0 - height2, .z = -1 };
+
+    return vec3_norm(vec3_cross(a, b));
+}
+
+/* TODO: don't operate on triangles, instead interpolate on quads */
 static float height_at(SceneIngame *scn, Vec2 position) {
     float height0, height1, height2, weight0, weight1, weight2;
 
@@ -276,10 +425,13 @@ static void ingame_tick(State *state) {
     if (input_action_just_pressed("mouse_capture_toggle"))
         scn->mouse_captured = !scn->mouse_captured;
 
+    process_vehicle(scn);
+
     ctx.mouse_capture = scn->mouse_captured;
 
     generate_terrain(scn);
     draw_terrain(scn);
+    draw_vehicle(scn);
 
     draw_skybox("/assets/miramar/miramar_*.tga");