src/common/physics.c

   1 #include <math.h>
   2 #include "common/physics.h"
   3
   4 static aabb3f64 move_box(aabb3f32 box, v3f64 pos)
   5 {
   6         return (aabb3f64) {
   7                 {pos.x + box.min.x, pos.y + box.min.y, pos.z + box.min.z},
   8                 {pos.x + box.max.x, pos.y + box.max.y, pos.z + box.max.z},
   9         };
  10 }
  11
  12 static aabb3s32 round_box(aabb3f64 box)
  13 {
  14         return (aabb3s32) {
  15                 {floor(box.min.x + 0.5), floor(box.min.y + 0.5), floor(box.min.z + 0.5)},
  16                 { ceil(box.max.x - 0.5),  ceil(box.max.y - 0.5),  ceil(box.max.z - 0.5)},
  17         };
  18 }
  19
  20 static bool is_solid(Terrain *terrain, s32 x, s32 y, s32 z)
  21 {
  22         NodeType node = terrain_get_node(terrain, (v3s32) {x, y, z}).type;
  23         return node == COUNT_NODE || node_def[node].solid;
  24 }
  25
  26 bool physics_ground(Terrain *terrain, bool collide, aabb3f32 box, v3f64 *pos, v3f64 *vel)
  27 {
  28         if (!collide)
  29                 return false;
  30
  31         if (vel->y != 0.0)
  32                 return false;
  33
  34         aabb3f64 mbox = move_box(box, *pos);
  35         mbox.min.y -= 0.5;
  36
  37         aabb3s32 rbox = round_box(mbox);
  38
  39         if (mbox.min.y - (f64) rbox.min.y > 0.01)
  40                 return false;
  41
  42         for (s32 x = rbox.min.x; x <= rbox.max.x; x++)
  43                 for (s32 z = rbox.min.z; z <= rbox.max.z; z++)
  44                         if (is_solid(terrain, x, rbox.min.y, z))
  45                                 return true;
  46
  47         return false;
  48 }
  49
  50 bool physics_step(Terrain *terrain, bool collide, aabb3f32 box, v3f64 *pos, v3f64 *vel, v3f64 *acc, f64 t)
  51 {
  52         v3f64 old_pos = *pos;
  53
  54         f64 *x = &pos->x;
  55         f64 *v = &vel->x;
  56         f64 *a = &acc->x;
  57
  58         f32 *min = &box.min.x;
  59         f32 *max = &box.max.x;
  60
  61         static u8 idx[3][3] = {
  62                 {0, 1, 2},
  63                 {1, 0, 2},
  64                 {2, 0, 1},
  65         };
  66
  67         for (u8 i = 0; i < 3; i++) {
  68                 f64 v_old = v[i];
  69                 v[i] += a[i] * t;
  70                 f64 v_cur = (v[i] + v_old) / 2.0;
  71                 if (v_cur == 0.0)
  72                         continue;
  73
  74                 f64 x_old = x[i];
  75                 x[i] += v_cur * t;
  76                 if (!collide)
  77                         continue;
  78
  79                 aabb3s32 box_rnd = round_box(move_box(box, *pos));
  80
  81                 s32 dir;
  82                 f32 off;
  83                 s32 *min_rnd = &box_rnd.min.x;
  84                 s32 *max_rnd = &box_rnd.max.x;
  85
  86                 if (v[i] > 0.0) {
  87                         dir = +1;
  88                         off = max[i];
  89
  90                         min_rnd[i] = ceil(x_old + off + 0.5);
  91                         max_rnd[i] = floor(x[i] + off + 0.5);
  92                 } else {
  93                         dir = -1;
  94                         off = min[i];
  95
  96                         min_rnd[i] = floor(x_old + off - 0.5);
  97                         max_rnd[i] =   ceil(x[i] + off - 0.5);
  98                 }
  99
 100                 max_rnd[i] += dir;
 101
 102                 u8 i_a = idx[i][0]; // = i
 103                 u8 i_b = idx[i][1];
 104                 u8 i_c = idx[i][2];
 105
 106                 for (s32 a = min_rnd[i_a]; a != max_rnd[i_a]; a += dir)
 107                 for (s32 b = min_rnd[i_b]; b <= max_rnd[i_b]; b++)
 108                 for (s32 c = min_rnd[i_c]; c <= max_rnd[i_c]; c++)      {
 109                         s32 p[3];
 110                         p[i_a] = a;
 111                         p[i_b] = b;
 112                         p[i_c] = c;
 113
 114                         if (is_solid(terrain, p[0], p[1], p[2])) {
 115                                 x[i] = (f64) a - off - 0.5 * (f64) dir;
 116                                 v[i] = 0.0;
 117                                 goto done;
 118                         }
 119                 }
 120
 121                 done: continue;
 122         }
 123
 124         return !v3f64_equals(*pos, old_pos);
 125 }