2 files changed, 319 insertions, 77 deletions

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 );