more movement

10 files changed, 410 insertions, 115 deletions

diff --git a/src/editor/view3d.cpp b/src/editor/view3d.cpp
index 656eda9..3729823 100644
--- a/src/editor/view3d.cpp
+++ b/src/editor/view3d.cpp
@@ -13,14 +13,15 @@ void gui_editor_3dview_draw_showpos( GUI_EDITOR_3DVIEW* view ) {
   I32 y = gui_rely( view ) + EDITORVIEW_TITLE_OFFSET;
 
   VEC3 pos = objl->pl->pos;
+  F32 speed = vec_len( objl->pl->velocity );
 
   gui_draw_str(
     x + view->w - 2, y + 2,
     ALIGN_R,
     FNT_JPN12,
     ui_clr.txt,
-    "pos: %.02f %.02f %.02f",
-    pos.x, pos.y, pos.z
+    "pos: %.02f %.02f %.02f (%.02f)",
+    pos.x, pos.y, pos.z, speed
   );
 
   gui_draw_str(
diff --git a/src/game/object.h b/src/game/object.h
index 8191ab7..80ade03 100644
--- a/src/game/object.h
+++ b/src/game/object.h
@@ -42,6 +42,10 @@ struct OBJECT_PROP {
   type name; \
   OBJECT_PROP __##name##_props{ #name, (void*)&name, sizeof(type), &this->props, this }
 
+#define OBJVARV( type, name, val ) \
+  type name{ type( val ) }; \
+  OBJECT_PROP __##name##_props{ #name, (void*)&name, sizeof(type), &this->props, this }
+
 #define OBJVAR_STR( name, len ) \
   char name##[len]; \
   OBJECT_PROP __##name##_props{ #name, (void*)name, len, &this->props, this }
@@ -61,6 +65,9 @@ struct OBJECT {
   OBJVAR( VEC3, pos );
   OBJVAR( VEC3, rot );
   OBJVAR( I32, idx );
+  OBJVAR( VEC3, velocity );
+  OBJVAR( VEC3, mins );
+  OBJVAR( VEC3, maxs );
 };
 
 inline const char* obj_classid_to_name( U32 classid ) {
diff --git a/src/game/physics/movement.cpp b/src/game/physics/movement.cpp
index ecee539..31ae63f 100644
--- a/src/game/physics/movement.cpp
+++ b/src/game/physics/movement.cpp
@@ -3,14 +3,19 @@
 #include "../../util/math.h"
 #include "../../util/string.h"
 #include "../../game.h"
+#include "../../game/objlist.h"
 
 GAME_MOVEMENT* gmove;
-CVAR* mv_gravity = var_new( "mv_gravity", 5.5f );
-CVAR* mv_friction = var_new( "mv_friction", 5.f );
-CVAR* mv_accelerate = var_new( "mv_accelerate", 5.5f );
+CVAR* mv_gravity = var_new( "mv_gravity", 800.f );
+CVAR* mv_friction = var_new( "mv_friction", 4.f );
+CVAR* mv_accelerate = var_new( "mv_accelerate", 5.f );
 CVAR* mv_airaccelerate = var_new( "mv_airaccelerate", 12.f );
 CVAR* mv_wallboost = var_new( "mv_wallboost", 0 );
 CVAR* mv_maxspeed = var_new( "mv_maxspeed", 3500.f );
+CVAR* mv_stopspeed = var_new( "mv_stopspeed", 80.f );
+CVAR* mv_jump_impulse = var_new( "mv_jump_impulse", 301.993377f ); // csgo - sqrt(2 * 800 * 57 - jump height)
+
+const U32 NON_JUMP_VELOCITY = 140.f;
 
 void gmove_init( GAME_DATA* game ) {
   gmove = new GAME_MOVEMENT;
@@ -60,6 +65,7 @@ void gmove_start_tick() {
   gmove->walkspeed = p->walkspeed;
   gmove->aabb.min = p->mins;
   gmove->aabb.max = p->maxs;
+  gmove->ground = p->ground_ent;
   gmove->bsp = gmove->game->state.map->bsp;
 }
 
@@ -67,6 +73,11 @@ void gmove_end_tick() {
   PLAYER* p = gmove->pl;
   p->velocity = gmove->velocity;
   p->pos = gmove->pos;
+  p->ground_ent = gmove->ground;
+  if( p->ground_ent )
+    p->flags |= PFL_ONGROUND;
+  else
+    p->flags &= ~PFL_ONGROUND;
 }
 
 VEC3 gmove_clip_velocity( VEC3 in, VEC3 norm, F32 overbounce ) {
@@ -106,51 +117,92 @@ void gmove_start_gravity() {
 }
 
 void gmove_end_gravity() {
-  
+  F32 gravity = gmove->pl->gravity * var_getf( mv_gravity );
+
+  gmove->velocity.z -= gravity * 0.5f * TICK_INTERVAL;
+  gmove_check_velocity();
 }
 
-void gmove_accelerate( VEC3& wishdir, F32 wishspeed, F32 accel ) {
-  F32 addspeed, accelspeed, currentspeed;
-  F32 surf_friction = 1.f;
+U8 gmove_touch_ground( BSP_TRACE* tr, VEC3 start, VEC3 end ) {
+  return bsp_trace( tr, gmove->bsp, gmove->aabb, start, end );
+}
 
-  currentspeed = vec_dot( gmove->pl->velocity, wishdir );
-  addspeed = wishspeed - currentspeed;
-  if( addspeed <= 0 )
-    return;
+U8 gmove_touch_ground_quadrants( BSP_TRACE* tr, VEC3 start, VEC3 end ) {
+  F32 frac = tr->frac;
+  VEC3 point = tr->point;
+  U8 hit = tr->hit;
 
-  accelspeed = accel * TICK_INTERVAL * wishspeed * surf_friction;
-  if( accelspeed > addspeed )
-    accelspeed = addspeed;
+  VEC3 mins, maxs;
+  AABB aabb = gmove->aabb;
 
-  gmove->velocity += wishdir * accelspeed;
+  defer( {
+    tr->frac = frac;
+    tr->point = point;
+    tr->hit = hit;
+  } );
+
+  mins = aabb.min;
+  maxs = { min( 0.f, aabb.max.x ), min( 0.f, aabb.max.y ), aabb.max.z };
+  bsp_trace( tr, gmove->bsp, AABB{ mins, maxs }, start, end );
+  if( tr->hit && tr->normal.z > 0.7f )
+    return 1;
+
+  mins = { max( 0.f, aabb.min.x ), max( 0.f, aabb.min.y ), aabb.min.z };
+  maxs = aabb.max;
+  bsp_trace( tr, gmove->bsp, AABB{ mins, maxs }, start, end );
+  if( tr->hit && tr->normal.z > 0.7f )
+    return 1;
+
+  mins = { aabb.min.x, max( 0.f, aabb.min.y ), aabb.min.z };
+  maxs = { min( 0.f, aabb.max.x ), aabb.max.y, aabb.max.z };
+  bsp_trace( tr, gmove->bsp, AABB{ mins, maxs }, start, end );
+  if( tr->hit && tr->normal.z > 0.7f )
+    return 1;
+
+  mins = { max( 0.f, aabb.min.x ), aabb.min.y, aabb.min.z };
+  maxs = { aabb.max.x, min( 0.f, aabb.max.y ), aabb.max.z };
+  bsp_trace( tr, gmove->bsp, AABB{ mins, maxs }, start, end );
+  if( tr->hit && tr->normal.z > 0.7f )
+    return 1;
+
+  return 0;
 }
 
-void gmove_stay_on_ground() {
+void gmove_categorize_pos() {
+  gmove->pl->surf_friction = 1.f;
+  F32 offset = 2.f;
   BSP_TRACE tr{};
-  VEC3 start = gmove->pos;
-  VEC3 end = gmove->pos;
 
-  start.z += 2.f;
-  end.z -= gmove->pl->stepsize;
+  VEC3 point = gmove->pos;
+  point.z -= offset;
 
-  bsp_trace( &tr, gmove->bsp, gmove->aabb, start, end );
-  start = tr.point;
+  VEC3 bump = gmove->pos;
 
-  bsp_trace( &tr, gmove->bsp, gmove->aabb, start, end );
-  if( tr.frac > 0.f && tr.frac < 1.f && !tr.startsolid && tr.normal.z >= 0.7f ) {
-    F32 delta = fabs( gmove->pos.z - tr.point.z );
+  F32 zvel = gmove->velocity.z;
+  U8 moveup_rapid = zvel > NON_JUMP_VELOCITY;
 
-    if( delta > 0.5f * BSP_TRACE_EPSILON )
-      gmove->pos = tr.point;
-  }
-}
+  // todo : ladders
 
-void gmove_airaccelerate() {
-  
-}
+  if( moveup_rapid ) {
+    gmove->ground = 0;
+  } else {
+    gmove_touch_ground( &tr, bump, point );
+    if( !tr.hit || tr.normal.z < 0.7f ) {
+      gmove_touch_ground_quadrants( &tr, bump, point );
+        if( !tr.hit || tr.normal.z < 0.7f ) {
+        gmove->ground = 0;
 
-void gmove_categorize_pos() {
-  
+        if( gmove->velocity.z > 0.f ) {
+          gmove->pl->surf_friction = 0.25f;
+        }
+      } else {
+        // todo : set actual ground entity when we get entity traces
+        gmove->ground = objl->world;
+      }
+    } else {
+      gmove->ground = objl->world;
+    }
+  }
 }
 
 VEC3 gmove_clip_planes( VEC3 vel, VEC3* pos, LIST<VEC3>* planes, F32 overbounce ) {
@@ -183,43 +235,6 @@ VEC3 gmove_clip_planes( VEC3 vel, VEC3* pos, LIST<VEC3>* planes, F32 overbounce
   return vel;
 }
 
-void gmove_check_stuck( VEC3* pos ) {
-  BSP_TRACE t;
-  bsp_trace( &t, gmove->bsp, gmove->aabb, *pos, *pos );
-  if( !t.hit )
-    return;
-
-  VEC3 p1 = *pos;
-
-  // try nudge away from wall
-  *pos = p1 + t.normal * BSP_TRACE_EPSILON * 2.f;
-  bsp_trace( &t, gmove->bsp, gmove->aabb, *pos, *pos );
-  if( !t.hit )
-    return;
-
-  // nudge +90deg away horizontal
-  *pos = p1 + VEC3{ -t.normal.y, t.normal.x, t.normal.z } * BSP_TRACE_EPSILON * 2.f;
-  bsp_trace( &t, gmove->bsp, gmove->aabb, *pos, *pos );
-  if( !t.hit )
-    return;
-
-  // nudge -90deg away horizontal
-  *pos = p1 + VEC3{ t.normal.y, -t.normal.x, t.normal.z } * BSP_TRACE_EPSILON * 2.f;
-  bsp_trace( &t, gmove->bsp, gmove->aabb, *pos, *pos );
-  if( !t.hit )
-    return;
-
-  // nudge -180deg away horizontal
-  *pos = p1 + VEC3{ t.normal.y, t.normal.x, -t.normal.z } * BSP_TRACE_EPSILON * 2.f;
-  bsp_trace( &t, gmove->bsp, gmove->aabb, *pos, *pos );
-  if( !t.hit )
-    return;
-
-  // todo : loop over every dir from current pos and incrementally increase distance.
-  // kinda like source
-  *pos = p1; // restore original pos - failed to unstuck
-}
-
 F32 gmove_try_move( BSP_TRACE* t, VEC3* pos, VEC3* vel ) {
   F32        dt = TICK_INTERVAL;
   AABB       aabb = gmove->aabb;
@@ -250,6 +265,142 @@ F32 gmove_try_move( BSP_TRACE* t, VEC3* pos, VEC3* vel ) {
   return dt < 0 ? 0 : dt;
 }
 
+void gmove_check_stuck( VEC3* in_pos ) {
+  BSP_TRACE t;
+  VEC3 pos = *in_pos;
+  if( vec_len( gmove->unstuck_vel ) < 0.f )
+    gmove->unstuck_vel = { 0, 0, 1.f };
+
+  if( gmove->ground )
+    pos.z += 2.f;
+  bsp_trace( &t, gmove->bsp, gmove->aabb, pos, pos );
+  if( !t.hit ) {
+    if( vec_len( gmove->velocity ) > 1.f ) {
+      gmove->unstuck_pos = pos;
+      gmove->unstuck_vel = gmove->velocity;
+    }
+    return;
+  }
+
+  VEC3 p1 = pos;
+  defer( {
+    if( !t.hit ) *in_pos = pos;
+  } );
+
+  for( U32 i = 1; i <= 32; i *= 2 ) {
+    // try nudge away from wall
+    pos = p1 + t.normal * BSP_TRACE_EPSILON * (F32)i;
+    bsp_trace( &t, gmove->bsp, gmove->aabb, pos, pos );
+    if( !t.hit )
+      return;
+
+    // nudge +90deg away horizontal
+    pos = p1 + VEC3{ -t.normal.y, t.normal.x, t.normal.z } * (F32)i;
+    bsp_trace( &t, gmove->bsp, gmove->aabb, pos, pos );
+    if( !t.hit )
+      return;
+
+    // nudge -90deg away horizontal
+    pos = p1 + VEC3{ t.normal.y, -t.normal.x, t.normal.z } * (F32)i;
+    bsp_trace( &t, gmove->bsp, gmove->aabb, pos, pos );
+    if( !t.hit )
+      return;
+
+    // nudge -180deg away horizontal
+    pos = p1 + VEC3{ t.normal.y, t.normal.x, -t.normal.z } * (F32)i;
+    bsp_trace( &t, gmove->bsp, gmove->aabb, pos, pos );
+    if( !t.hit )
+      return;
+
+    pos = p1 + (gmove->unstuck_vel * -1.f) * TICK_INTERVAL * (F32)i;
+    bsp_trace( &t, gmove->bsp, gmove->aabb, pos, pos );
+    if( !t.hit )
+      return;
+  }
+
+  // todo : loop over every dir from current pos and incrementally increase distance.
+  // kinda like source
+}
+
+void gmove_accelerate( VEC3& wishdir, F32 wishspeed, F32 accel ) {
+  F32 addspeed, accelspeed, currentspeed;
+  F32 surf_friction = 1.f;
+
+  currentspeed = vec_dot( gmove->velocity, wishdir );
+  addspeed = wishspeed - currentspeed;
+  if( addspeed <= 0 )
+    return;
+
+  accelspeed = accel * TICK_INTERVAL * wishspeed * surf_friction;
+  if( accelspeed > addspeed )
+    accelspeed = addspeed;
+
+  gmove->velocity += wishdir * accelspeed;
+}
+
+void gmove_airaccelerate( VEC3 wishdir, F32 wishspeed, F32 accel ) {
+  F32 addspeed, accelspeed, currentspeed;
+  F32 wishspd;
+
+  wishspd = wishspeed;
+
+  if( wishspd > 30.f )
+    wishspd = 30.f;
+
+  currentspeed = vec_dot( gmove->velocity, wishdir );
+  addspeed = wishspd - currentspeed;
+
+  if( addspeed <= 0.f )
+    return;
+
+  accelspeed = accel * wishspeed * TICK_INTERVAL * gmove->pl->surf_friction;
+  if( accelspeed > addspeed )
+    accelspeed = addspeed;
+
+  gmove->velocity += wishdir * accelspeed;
+}
+
+void gmove_friction() {
+  F32 speed = vec_len2d( gmove->velocity );
+  if( speed < 0.1f )
+    return;
+
+  F32 friction = var_getf( mv_friction ) /* * surface_friction */;
+  F32 stopspeed = var_getf( mv_stopspeed );
+  F32 control = (speed < stopspeed)? stopspeed : speed;
+
+  F32 drop = control * friction * TICK_INTERVAL;
+  F32 newspeed = speed - drop;
+  if( newspeed < 0.f )
+    newspeed = 0.f;
+
+  if( newspeed != speed ) {
+    newspeed /= speed;
+    gmove->velocity *= newspeed;
+  }
+}
+
+void gmove_stay_on_ground() {
+  BSP_TRACE tr{};
+  VEC3 start = gmove->pos;
+  VEC3 end = gmove->pos;
+
+  start.z += 2.f;
+  end.z -= gmove->pl->stepsize;
+
+  bsp_trace( &tr, gmove->bsp, gmove->aabb, start, end );
+  start = tr.point;
+
+  bsp_trace( &tr, gmove->bsp, gmove->aabb, start, end );
+  if( tr.frac > 0.f && tr.frac < 1.f && !tr.startsolid && tr.normal.z >= 0.7f ) {
+    F32 delta = fabs( gmove->pos.z - tr.point.z );
+
+    if( delta > 0.5f * BSP_TRACE_EPSILON )
+      gmove->pos = tr.point;
+  }
+}
+
+
 void gmove_step_move( VEC3 dest, BSP_TRACE* tr ) {
   VEC3 endpos = dest;
   VEC3 pos = gmove->pos;
@@ -307,15 +458,18 @@ void gmove_walk_move() {
     0
   };
 
+  if( vec_len( wishdir ) > 0.1f )
+    vec_normalize( &wishdir );
+
   VEC3 vel = wishdir * gmove->walkspeed;
   F32 speed = vec_len( vel );
-  if( speed > gmove->maxspeed ) {
+  if( speed != 0.f && speed > gmove->maxspeed ) {
     vel *= gmove->maxspeed / speed;
     speed = gmove->maxspeed;
   }
 
   gmove_accelerate( wishdir, speed, var_getf( mv_accelerate ) );
-  speed = vec_len( vel );
+  speed = vec_len( gmove->velocity );
 
   defer( gmove_check_stuck( &gmove->pos ) );
   if( speed < 1.f ) {
@@ -336,12 +490,94 @@ void gmove_walk_move() {
   gmove_step_move( dest, &tr );
 }
 
-void gmove_full_walk_move() {
+
+void gmove_air_move() {
+  VEC2 move = gmove->input->move;
+  F32  yawrad = m_deg2rad( gmove->angle.y );
+  VEC3 wishdir = {
+    move.x * cosf( yawrad ) - move.y * sinf( yawrad ),
+    move.y * cosf( yawrad ) + move.x * sinf( yawrad ),
+    0
+  };
+
+  if( vec_len( wishdir ) > 0.1f )
+    vec_normalize( &wishdir );
+
+  VEC3 vel = wishdir * gmove->walkspeed;
+  F32 speed = vec_len( vel );
+
+  if( speed != 0 && speed > gmove->maxspeed ) {
+    speed = gmove->maxspeed;
+  }
+
+  gmove_airaccelerate( wishdir, speed, var_getf( mv_airaccelerate ) );
+
+  BSP_TRACE tr{};
+  gmove_try_move( &tr, &gmove->pos, &gmove->velocity );
+  gmove_check_stuck( &gmove->pos );
+}
+
+void gmove_check_falling() {
+
+}
+
+void gmove_check_jump() {
+  if( !gmove->ground )
+    return;
+
+  gmove->ground = 0;
+  // todo : play jump sound
   
+  F32 mul = var_getf( mv_jump_impulse );
+  if( gmove->pl->flags & PFL_DUCK ) {
+    gmove->velocity.z = mul;
+  }
+  else {
+    gmove->velocity.z += mul;
+  }
+
+  gmove_end_gravity();
+
+  gmove->out_step += 0.15f;
+}
+
+void gmove_full_walk_move() {
+  // todo: inwater, watermove
+  gmove_start_gravity();
+
+  // todo:
+  // jump,
+  // categorizepos
+  // more
+
+  if( gmove->input->jump )
+    gmove_check_jump();
+
+  if( gmove->ground ) {
+    gmove_friction();
+    gmove_check_velocity();
+    gmove_walk_move();
+  }
+  else
+    gmove_air_move();
+
+  gmove_categorize_pos();
+
+  gmove_end_gravity();
+  gmove_check_falling();
+}
+
+void gmove_player_move() {
+  if( gmove->velocity.z > 250.f )
+    gmove->ground = 0;
+
+  switch( gmove->pl->movetype ) {
+    case PMT_WALK: return gmove_full_walk_move();
+  }
 }
 
 void gmove_process_move() {
-  gmove_walk_move();
+  gmove_player_move();
 }
 
 void gmove_tick() {
diff --git a/src/game/physics/movement.h b/src/game/physics/movement.h
index 47aa97d..e1f27cb 100644
--- a/src/game/physics/movement.h
+++ b/src/game/physics/movement.h
@@ -11,7 +11,8 @@ extern CVAR* mv_accelerate;
 extern CVAR* mv_airaccelerate;
 extern CVAR* mv_wallboost;
 extern CVAR* mv_maxspeed;
-
+extern CVAR* mv_stopspeed;
+extern CVAR* mv_jump_impulse;
 
 struct GAME_MOVEMENT {
   PLAYER* pl;
@@ -24,9 +25,14 @@ struct GAME_MOVEMENT {
   F32  maxspeed;
   F32  walkspeed;
   AABB aabb;
-  BSP* bsp;
+  OBJECT* ground{};
+
+  BSP* bsp{};
 
   F32  out_step;
+
+  VEC3 unstuck_vel{};
+  VEC3 unstuck_pos{};
 };
 
 extern void gmove_init( GAME_DATA* game );
diff --git a/src/game/player.cpp b/src/game/player.cpp
index c6bf385..e4c0390 100644
--- a/src/game/player.cpp
+++ b/src/game/player.cpp
@@ -1,9 +1,6 @@
 #include "../game.h"
-#include "SDL_scancode.h"
 #include "objlist.h"
 #include "physics/movement.h"
-#include "world/trace.h"
-#include <cmath>
 #include "player.h"
 
 F32 PLAYER_HULL_HEIGHT = 48.f;
@@ -16,6 +13,7 @@ PLAYER* player_create( VEC3 origin, F32 yaw ) {
   p->health = 100;
   p->keeponlevel = 1;
   p->velocity = {};
+  p->flags = {};
 
   p->mins = {
     -PLAYER_HULL_WIDTH * .5f,
@@ -57,6 +55,7 @@ void capture_mouse( PLAYER* p ) {
 
 void capture_move_keys( PLAYER* p ) {
   VEC2* move = &p->input.move;
+  p->input.jump = input.keys[input.binds.jump];
   if( input.keys[input.binds.fwd] ) {
     if( !p->input.fwd_held ) move->x = 1.f;
      p->input.fwd_held = 1;
diff --git a/src/game/player.h b/src/game/player.h
index 7dfd009..27b423d 100644
--- a/src/game/player.h
+++ b/src/game/player.h
@@ -2,32 +2,51 @@
 #include "object.h"
 #include "camera.h"
 
+enum PlayerFlags_t {
+  PFL_NONE = 0,
+  PFL_ONGROUND = 1 << 0,
+  PFL_INVULNERABLE = 1 << 1,
+  PFL_NOCLIP = 1 << 2,
+  PFL_DUCK = 1 << 3,
+};
+
+enum PlayerMoveType_t {
+  PMT_NONE = 0,
+  PMT_WALK = 1,
+  PMT_FLY = 2,
+  PMT_LADDER = 3,
+  PMT_NOCLIP = 4
+};
+
 struct PLAYER_INPUT {
   PLAYER_CAMERA cam;
   struct PLAYER* pobj;
 
   // for nulls
   U8 fwd_held{}, bk_held{}, left_held{}, right_held{};
+  U8 jump{};
 
   VEC2 move;
 };
 
 struct PLAYER : OBJECT {
   static const U32 CLASSID = OBJCLASS_PLAYER;
-  U32 health;
-  F32 fov{72.f};
-  F32 eyeoffset{40.f};
+  OBJVAR( U32, health );
+  OBJVARV( F32, fov, 72.f );
+  OBJVARV( F32, eyeoffset, 40.f );
+
+  OBJVAR( I32, flags );
 
   PLAYER_INPUT input;
-  VEC3 mins;
-  VEC3 maxs;
 
-  F32 gravity{1.f};
-  F32 maxspeed{70.f};
-  F32 walkspeed{70.f};
+  OBJVARV( F32, gravity, 1.f );
+  F32 maxspeed{250.f};
+  F32 walkspeed{250.f};
   F32 stepsize{16.f};
+  OBJVARV( U8, movetype, PMT_WALK );
 
-  VEC3 velocity;
+  OBJECT* ground_ent;
+  F32 surf_friction{};
 };
 
 extern PLAYER* player_create( VEC3 origin, F32 yaw );
diff --git a/src/game/world/trace.cpp b/src/game/world/trace.cpp
index ba945f0..1c28d78 100644
--- a/src/game/world/trace.cpp
+++ b/src/game/world/trace.cpp
@@ -1,5 +1,3 @@
-#include <cfloat>
-
 #include "trace.h"
 #include "../../util/math.h"
 #include "bsp.h"
@@ -205,7 +203,7 @@ inline U8 point_in_inflated_poly(
 
     F32 dist   = vec_dot( point - a, in );
     F32 expand = aabb_support_radius( hull, in );
-    if( dist < -( expand - BSP_TRACE_EPSILON * 4.f ) )
+    if( dist < -( expand - BSP_EDGE_TOLERANCE ) )
       return 0;
   }
   return 1;
@@ -393,17 +391,22 @@ U8 bsp_trace( BSP_TRACE* trace, BSP* bsp, AABB hull ) {
   VEC3 start = trace->in_start;
   VEC3 end = trace->in_end;
 
-  // start hull at origin rather than in middle
-  trace->in_start.z += ( hull.max.z - hull.min.z );
-  trace->in_end.z += ( hull.max.z - hull.min.z );
+  // move trace up by half of the hull - extend at feet instead of center
+  VEC3 off = VEC3{ 0, 0, hull.max.z - hull.min.z } * 0.5f;
+
+  trace->in_start += off;
+  trace->in_end += off;
 
   U8 ret = bsp_trace_sweep_aabb( trace, bsp, hull, trace->in_start, trace->in_end, bsp->root );
 
-  trace->in_start = start;
-  trace->in_end = end;
+  trace->in_start -= off;
+  trace->in_end -= off;
 
   if( !ret )
     trace->point = trace->in_end;
+  else
+    trace->point -= off;
+
   return ret;
 }
 
diff --git a/src/game/world/trace.h b/src/game/world/trace.h
index 717fc8f..d04ed6b 100644
--- a/src/game/world/trace.h
+++ b/src/game/world/trace.h
@@ -5,6 +5,7 @@
 
 // quake uses 1 / 32
 const F32 BSP_TRACE_EPSILON = 1.f / 64.f;
+const F32 BSP_EDGE_TOLERANCE = 1.f / 16.f;
 
 struct BSP_TRACE {
   VEC3 in_start;
diff --git a/src/util/aabb.h b/src/util/aabb.h
index 4d74c88..96e40bf 100644
--- a/src/util/aabb.h
+++ b/src/util/aabb.h
@@ -13,19 +13,3 @@ inline F32 aabb_support_radius( const AABB& aabb, const VEC3& dir ) {
   VEC3 half = aabb_half( aabb );
   return fabsf(dir.x) * half.x + fabsf(dir.y) * half.y + fabsf(dir.z) * half.z;
 }
-
-// returns positive radius in X and negative in Y
-inline VEC2 aabb_extend_at_feet( const AABB& aabb, const VEC3& dir ) {
-  VEC3 half = aabb_half( aabb );
-
-  F32 hx = half.x;
-  F32 hy = half.y;
-  F32 hz = half.z * 2;
-
-  F32 lateral = fabsf( dir.x ) * hx + fabsf( dir.y ) * hy;
-
-  F32 pos_z = dir.z > 0 ? dir.z * hz : 0.f;
-  F32 neg_z = dir.z < 0 ? (-dir.z) * hz : 0.f;
-
-  return { lateral + pos_z, lateral + neg_z };
-}
diff --git a/src/util/vector.h b/src/util/vector.h
index 33f6551..314131c 100644
--- a/src/util/vector.h
+++ b/src/util/vector.h
@@ -190,3 +190,42 @@ inline F32 vec_angle( VEC2 v1, VEC2 v2 ) {
   VEC2 d = v2 - v1;
   return remainderf( atan2f( d.y, d.x ) * RADPI, 360.f );
 }
+
+inline VEC3 angle_vectors( VEC3 angles ) {
+  F32 sp, sy, cp, cy;
+  sp = sinf( angles.x * PIRAD );
+  cp = cosf( angles.x * PIRAD );
+  sy = sinf( angles.y * PIRAD );
+  cy = cosf( angles.y * PIRAD );
+
+  return VEC3{ cp * cy, cp * sy, -sp };
+}
+
+inline void angle_vectors( const VEC3& angles, VEC3* forward, VEC3* right, VEC3* up ) {
+  F32 sr, sp, sy, cr, cp, cy;
+
+  sp = sinf( angles.x * PIRAD );
+  cp = cosf( angles.x * PIRAD );
+  sy = sinf( angles.y * PIRAD );
+  cy = cosf( angles.y * PIRAD );
+  sr = sinf( angles.z * PIRAD );
+  cr = cosf( angles.z * PIRAD );
+
+  if ( forward ) {
+    forward->x = cp * cy;
+    forward->y = cp * sy;
+    forward->z = -sp;
+  }
+
+  if ( right ) {
+    right->x = -1 * sr * sp * cy + -1 * cr * -sy;
+    right->y = -1 * sr * sp * sy + -1 * cr * cy;
+    right->z = -1 * sr * cp;
+  }
+
+  if ( up ) {
+    up->x = cr * sp * cy + -sr * -sy;
+    up->y = cr * sp * sy + -sr * cy;
+    up->z = cr * cp;
+  }
+}