From: hgn <hgodden00@gmail.com>
Date: Wed, 20 Jul 2022 18:17:18 +0000 (+0100)
Subject: f
X-Git-Url: https://harrygodden.com/git/?a=commitdiff_plain;h=55faf7fcd56fad190f53e6da4417c5bb1c2234d7;p=carveJwlIkooP6JGAAIwe30JlM.git

f
---

diff --git a/character.h b/character.h
index 94b181e..71299e8 100644
--- a/character.h
+++ b/character.h
@@ -888,7 +888,7 @@ static void character_ragdoll_iter( struct character *ch )
    {
       rb_build_manifold_terrain( &ch->ragdoll[i] );
 
-      /* TODO: Cars */
+      /* TODO: Cars, piece-to-piece collision */
 #if 0
       u32 colliders[16];
       int len = bh_select( &world.bhcubes, ch->ragdoll[i].bbx_world, 
@@ -900,6 +900,8 @@ static void character_ragdoll_iter( struct character *ch )
 #endif
    }
 
+   rb_presolve_contacts( rb_contact_buffer, rb_contact_count );
+
    v3f rv;
 
    float shoe_vel[2];
@@ -912,7 +914,7 @@ static void character_ragdoll_iter( struct character *ch )
    {
       float const k_springfactor = 1.0f/20.0f;
       
-      rb_solve_contacts();
+      rb_solve_contacts( rb_contact_buffer, rb_contact_count );
       
       for( int j=0; j<vg_list_size(rd_joints); j++ )
       {
diff --git a/physics_test.h b/physics_test.h
index 8187b91..bd8bc07 100644
--- a/physics_test.h
+++ b/physics_test.h
@@ -9,6 +9,16 @@ rigidbody ground = { .type = k_rb_shape_box,
                      .co = {0.0f, 0.0f, 0.0f},
                      .q = {0.0f,0.0f,0.0f,1.0f},
                      .is_world = 1 };
+
+rigidbody blocky = 
+   {
+      .type = k_rb_shape_box,
+      .bbx = {{-2.0f,-1.0f,-3.0f},{2.0f,1.0f,2.0f}},
+      .co = {30.0f,2.0f,30.0f},
+      .q = {0.0f,0.0f,0.0f,1.0f},
+      .is_world = 1
+   };
+
 rigidbody funnel[4] = {
    {
       .type = k_rb_shape_box,
@@ -36,6 +46,11 @@ rigidbody funnel[4] = {
    }
 };
 
+rigidbody jeff1 = { .type = k_rb_shape_capsule,
+                    .inf.capsule = { .radius = 0.75f, .height = 3.0f },
+                    .co = {30.0f, 4.0f, 30.0f },
+                    .q = {0.0f,0.0f,0.0f,1.0f}
+};
 
 rigidbody ball = { .type = k_rb_shape_sphere,
                    .inf.sphere = { .radius = 2.0f },
@@ -60,6 +75,8 @@ static void physics_test_start(void)
    rb_init( &ground );
    rb_init( &ball );
    rb_init( &ball1 );
+   rb_init( &jeff1 );
+   rb_init( &blocky );
 }
 
 static void physics_test_update(void)
@@ -71,6 +88,7 @@ static void physics_test_update(void)
 
    rb_iter( &ball );
    rb_iter( &ball1 );
+   rb_iter( &jeff1 );
 
    rb_solver_reset();
 
@@ -84,21 +102,25 @@ static void physics_test_update(void)
    rb_contact_count += rb_sphere_vs_box( &ball1, &ground, rb_global_ct() );
    rb_contact_count += rb_sphere_vs_sphere( &ball, &ball1, rb_global_ct() );
 
-   rb_presolve_contacts();
+   rb_contact_count += rb_capsule_vs_box( &jeff1, &ground, rb_global_ct() );
+   rb_contact_count += rb_capsule_vs_box( &jeff1, &blocky, rb_global_ct() );
+
+   rb_presolve_contacts( rb_contact_buffer, rb_contact_count );
 
    for( int i=0; i<5; i++ )
-      rb_solve_contacts();
+      rb_solve_contacts( rb_contact_buffer, rb_contact_count );
 
    rb_update_transform( &ball );
    rb_update_transform( &ball1 );
+   rb_update_transform( &jeff1 );
 
    }
 
    if(glfwGetKey( vg_window, GLFW_KEY_L ))
    {
-      v3_copy( player.camera_pos, ball.co );
-      v3_zero( ball.v );
-      v3_zero( ball.w );
+      v3_copy( player.camera_pos, jeff1.co );
+      v3_zero( jeff1.v );
+      v3_zero( jeff1.w );
    }
    
    for( int i=0; i<4; i++ )
@@ -106,6 +128,9 @@ static void physics_test_update(void)
    rb_debug( &ground, 0xff00ff00 );
    rb_debug( &ball, 0xffe00040 );
    rb_debug( &ball1, 0xff00e050 );
+
+   rb_debug( &blocky, 0xffcccccc );
+   rb_debug( &jeff1, 0xff00ffff );
 }
 
 static void physics_test_render(void)
@@ -113,7 +138,7 @@ static void physics_test_render(void)
    m4x4f world_4x4;
    m4x3_expand( player.camera_inverse, world_4x4 );
 
-   gpipeline.fov = freecam? 60.0f: 135.0f; /* 120 */
+   gpipeline.fov = 60.0f;
    m4x4_projection( vg_pv, gpipeline.fov, 
          (float)vg_window_x / (float)vg_window_y, 
          0.1f, 2100.0f );
diff --git a/player.h b/player.h
index a0ac858..a378f72 100644
--- a/player.h
+++ b/player.h
@@ -439,34 +439,26 @@ static void player_physics(void)
       vg_line(tri[1],tri[2],0xff00ff00 );
       vg_line(tri[2],tri[0],0xff00ff00 );
 
-      v3f co, norm;
-      float p;
+      v3f temp;
+      v3_copy( player.rb.co, temp );
 
       for( int j=0; j<2; j++ )
       {
-         if( sphere_vs_triangle( poles[j], k_board_radius, tri,co,norm,&p) )
+         if(manifold_count >= vg_list_size(manifold))
          {
-            if(manifold_count >= vg_list_size(manifold))
-            {
-               vg_error("Manifold overflow!\n");
-               break;
-            }
-
-            v3f p1;
-            v3_muladds( poles[j], norm, p, p1 );
-            vg_line( poles[j], p1, 0xffffffff );
-
-            struct contact *ct = &manifold[manifold_count ++];
-            v3_copy( co, ct->co );
-            v3_copy( norm, ct->n );
+            vg_error("Manifold overflow!\n");
+            break;
+         }
 
-            ct->bias = -0.2f*k_rb_rate*vg_minf(0.0f,-p+k_board_allowance);
-            ct->norm_impulse = 0.0f;
+         rb_ct *ct = &manifold[manifold_count];
+         v3_copy( poles[j], player.rb.co );
 
-            v3_add( norm, surface_avg, surface_avg );
-         }
+         manifold_count += rb_sphere_vs_triangle( &player.rb, tri, ct );
       }
+
+      v3_copy( temp, player.rb.co );
    }
+   rb_presolve_contacts( manifold, manifold_count );
 
    if( !manifold_count )
    {
diff --git a/rigidbody.h b/rigidbody.h
index c00b0f0..58c70d7 100644
--- a/rigidbody.h
+++ b/rigidbody.h
@@ -197,7 +197,8 @@ static void rb_tangent_basis( v3f n, v3f tx, v3f ty )
 static void rb_solver_reset(void);
 static void rb_build_manifold_terrain( rigidbody *rb );
 static void rb_build_manifold_terrain_sphere( rigidbody *rb );
-static void rb_solve_contacts(void);
+static void rb_solve_contacts( rb_ct *buf, int len );
+static void rb_presolve_contacts( rb_ct *buffer, int len );
 
 /* 
  * These closest point tests were learned from Real-Time Collision Detection by 
@@ -411,6 +412,70 @@ static void rb_debug_contact( rb_ct *ct )
    vg_line( ct->co, p1, 0xffffffff );
 }
 
+static void rb_box_incident_dir( v3f p, boxf box, v3f dir )
+{
+   
+}
+
+/*
+ * Generates up to two contacts; optimised for the most stable manifold
+ */
+static int rb_capsule_vs_box( rigidbody *rba, rigidbody *rbb, rb_ct *buf )
+{
+   float h = rba->inf.capsule.height,
+         r = rba->inf.capsule.radius;
+   
+   v3f p0, p1;
+   v3_muladds( rba->co, rba->up,  h*0.5f-r*0.5f, p1 );
+   v3_muladds( rba->co, rba->up, -h*0.5f+r*0.5f, p0 );
+
+   m4x3_mulv( rbb->to_local, p0, p0 );
+   m4x3_mulv( rbb->to_local, p1, p1 );
+
+   /* Clip zone +y */
+   
+   v3f c0, c1;
+   closest_point_aabb( p0, rbb->bbx, c0 );
+   closest_point_aabb( p1, rbb->bbx, c1 );
+
+   v3f vis0;
+   m4x3_mulv( rbb->to_world, c0, vis0 );
+   vg_line_pt3( vis0, 0.1f, 0xffff00ff );
+   m4x3_mulv( rbb->to_world, c1, vis0 );
+   vg_line_pt3( vis0, 0.1f, 0xffff00ff );
+   
+   v3f d0, d1;
+   v3_sub( p0, c0, d0 );
+   v3_sub( p1, c1, d1 );
+
+   float d02 = v3_length2(d0),
+         d12 = v3_length2(d1);
+
+   int count = 0;
+
+   if( d02 <= r*r )
+   {
+      rb_ct *ct = buf+count;
+      float d = sqrtf(d02);
+      
+      vg_info( "d: %.4f\n", d );
+
+      v3_muls( d0, -1.0f/d, ct->n );
+      ct->p = r-d;
+      v3_add( c0, p0, ct->co );
+      v3_muls( ct->co, 0.5f, ct->co );
+      
+      m3x3_mulv( rbb->to_world, ct->n, ct->n );
+      m4x3_mulv( rbb->to_world, ct->co, ct->co );
+
+      ct->rba = rba;
+      ct->rbb = rbb;
+      count ++;
+   }
+
+   return count;
+}
+
 static int rb_sphere_vs_box( rigidbody *rba, rigidbody *rbb, rb_ct *buf )
 {
    v3f co, delta;
@@ -424,18 +489,43 @@ static int rb_sphere_vs_box( rigidbody *rba, rigidbody *rbb, rb_ct *buf )
    if( d2 <= r*r )
    {
       float d;
+
+      rb_ct *ct = buf;
       if( d2 <= 0.0001f )
       {
-         v3_sub( rbb->co, rba->co, delta );
-         d2 = v3_length2(delta);
-      }
+         v3_sub( rba->co, rbb->co, delta );
+
+         /* 
+          * some extra testing is required to find the best axis to push the
+          * object back outside the box. Since there isnt a clear seperating
+          * vector already, especially on really high aspect boxes.
+          */
+         float lx = v3_dot( rbb->right, delta ),
+               ly = v3_dot( rbb->up, delta ),
+               lz = v3_dot( rbb->forward, delta ),
+               px = rbb->bbx[1][0] - fabsf(lx),
+               py = rbb->bbx[1][1] - fabsf(ly),
+               pz = rbb->bbx[1][2] - fabsf(lz);
+
+         if( px < py && px < pz )
+            v3_muls( rbb->right, vg_signf(lx), ct->n );
+         else if( py < pz )
+            v3_muls( rbb->up, vg_signf(ly), ct->n );
+         else
+            v3_muls( rbb->forward, vg_signf(lz), ct->n );
 
-      d = sqrtf(d2);
+         v3_muladds( rba->co, ct->n, -r, ct->co );
+         ct->p = r;
+      }
+      else
+      {
+         d = sqrtf(d2);
+         v3_muls( delta, 1.0f/d, ct->n );
+         ct->p = r-d;
+         v3_add( co, rba->co, ct->co );
+         v3_muls( ct->co, 0.5f, ct->co );
+      }
 
-      rb_ct *ct = buf;
-      v3_muls( delta, 1.0f/d, ct->n );
-      v3_copy( co, ct->co );
-      ct->p = r-d;
       ct->rba = rba;
       ct->rbb = rbb;
       return 1;
@@ -710,12 +800,12 @@ static void rb_build_manifold_terrain( rigidbody *rb )
 /*
  * Initializing things like tangent vectors
  */
-static void rb_presolve_contacts(void)
+
+static void rb_presolve_contacts( rb_ct *buffer, int len )
 {
-   for( int i=0; i<rb_contact_count; i++ )
+   for( int i=0; i<len; i++ )
    {
-      rb_ct *ct = &rb_contact_buffer[i];
-
+      rb_ct *ct = &buffer[i];
       ct->bias = -0.2f * k_rb_rate * vg_minf(0.0f,-ct->p+0.04f);
       rb_tangent_basis( ct->n, ct->t[0], ct->t[1] );
 
@@ -729,7 +819,8 @@ static void rb_presolve_contacts(void)
 }
 
 /*
- * Creates relative contact velocity vector, and offsets between each body */
+ * Creates relative contact velocity vector, and offsets between each body
+ */
 static void rb_rcv( rb_ct *ct, v3f rv, v3f da, v3f db )
 {
    rigidbody *rba = ct->rba,
@@ -747,6 +838,9 @@ static void rb_rcv( rb_ct *ct, v3f rv, v3f da, v3f db )
    v3_add( rva, rvb, rv );
 }
 
+/*
+ * Apply regular and angular velocity impulses to objects involved in contact
+ */
 static void rb_standard_impulse( rb_ct *ct, v3f da, v3f db, v3f impulse )
 {
    rigidbody *rba = ct->rba,
@@ -764,17 +858,17 @@ static void rb_standard_impulse( rb_ct *ct, v3f da, v3f db, v3f impulse )
    v3_muladds( rbb->w, wb, ct->mass_total * rbb->inv_mass, rbb->w );
 }
 
-static void rb_solve_contacts(void)
+/*
+ * One iteration to solve the contact constraint
+ */
+static void rb_solve_contacts( rb_ct *buf, int len )
 {
    float k_friction = 0.1f;
 
-   /* TODO: second object
-    *       Static objects route to static element */
-
    /* Friction Impulse */
-   for( int i=0; i<rb_contact_count; i++ )
+   for( int i=0; i<len; i++ )
    {
-      struct contact *ct = &rb_contact_buffer[i];
+      struct contact *ct = &buf[i];
       rigidbody *rb = ct->rba;
 
       v3f rv, da, db;
@@ -796,9 +890,9 @@ static void rb_solve_contacts(void)
    }
 
    /* Normal Impulse */
-   for( int i=0; i<rb_contact_count; i++ )
+   for( int i=0; i<len; i++ )
    {
-      struct contact *ct = &rb_contact_buffer[i];
+      struct contact *ct = &buf[i];
       rigidbody *rba = ct->rba,
                 *rbb = ct->rbb;
 
@@ -818,6 +912,10 @@ static void rb_solve_contacts(void)
    }
 }
 
+/*
+ * The following ventures into not really very sophisticated at all maths
+ */
+
 struct rb_angle_limit
 {
    rigidbody *rba, *rbb;
@@ -1019,6 +1117,22 @@ static void rb_debug( rigidbody *rb, u32 colour )
    {
       debug_sphere( rb->to_world, rb->inf.sphere.radius, colour );
    }
+   else if( rb->type == k_rb_shape_capsule )
+   {
+      m4x3f m0, m1;
+      float h = rb->inf.capsule.height,
+            r = rb->inf.capsule.radius;
+
+      m3x3_copy( rb->to_world, m0 );
+      m3x3_copy( rb->to_world, m1 );
+      
+      v3_muladds( rb->co, rb->up, -h*0.5f, m0[3] );
+      v3_muladds( rb->co, rb->up,  h*0.5f, m1[3] );
+
+      debug_sphere( m0, r, colour );
+      debug_sphere( m1, r, colour );
+      vg_line( m0[3], m1[3], colour );
+   }
 }
 
 /*
@@ -1130,6 +1244,7 @@ static void rb_build_manifold_rb_static( rigidbody *ra, rigidbody *rb_static )
  * Capsule phyics
  */
 
+RB_DEPR
 static void debug_capsule( m4x3f m, float height, float radius, u32 colour )
 {
    v3f last = { 0.0f, 0.0f, radius };
diff --git a/world.h b/world.h
index 079d6d3..9242023 100644
--- a/world.h
+++ b/world.h
@@ -409,7 +409,7 @@ static void world_update(void)
    rb_build_manifold_terrain_sphere( &world.mr_ball );
    
    for( int i=0; i<5; i++ )
-      rb_solve_contacts();
+      rb_solve_contacts( rb_contact_buffer, rb_contact_count );
 
    rb_iter( &world.mr_ball );
    rb_update_transform( &world.mr_ball );