X-Git-Url: https://harrygodden.com/git/?a=blobdiff_plain;f=rigidbody.h;h=b9b863aad2db7c12ddbdb07ae5d15d4c95998204;hb=15beb60ade240af4e00b0d204f7e89a4d35dca36;hp=05d2db1100fe78310d8a548691d99138c90a9d3a;hpb=4a883ac1b2506032f9dddab342712de46f2ca734;p=carveJwlIkooP6JGAAIwe30JlM.git

diff --git a/rigidbody.h b/rigidbody.h
index 05d2db1..b9b863a 100644
--- a/rigidbody.h
+++ b/rigidbody.h
@@ -1,3 +1,7 @@
+/*
+ * Copyright (C) 2021-2022 Mt.ZERO Software, Harry Godden - All Rights Reserved
+ */
+
 /* 
  * Resources: Box2D - Erin Catto
  *            qu3e  - Randy Gaul
@@ -20,8 +24,8 @@ static bh_system bh_system_rigidbodies;
  */
 
 static const float 
-   k_rb_rate          = 60.0f,
-   k_rb_delta         = (1.0f/k_rb_rate),
+   k_rb_rate          = (1.0/VG_TIMESTEP_FIXED),
+   k_rb_delta         = (1.0/k_rb_rate),
    k_friction         = 0.6f,
    k_damp_linear      = 0.05f,               /* scale velocity 1/(1+x) */
    k_damp_angular     = 0.1f,                /* scale angular  1/(1+x) */
@@ -326,6 +330,40 @@ static void rb_update_transform( rigidbody *rb )
    rb_update_bounds( rb );
 }
 
+/* 
+ * Extrapolate rigidbody into a transform based on vg accumulator.
+ * Useful for rendering
+ */
+static void rb_extrapolate_transform( rigidbody *rb, m4x3f transform )
+{
+   float substep = vg_clampf( vg.accumulator / k_rb_delta, 0.0f, 1.0f );
+
+   v3f co;
+   v4f q;
+
+   v3_muladds( rb->co, rb->v, k_rb_delta*substep, co );
+
+   if( v3_length2( rb->w ) > 0.0f )
+   {
+      v4f rotation;
+      v3f axis;
+      v3_copy( rb->w, axis );
+      
+      float mag = v3_length( axis );
+      v3_divs( axis, mag, axis );
+      q_axis_angle( rotation, axis, mag*k_rb_delta*substep );
+      q_mul( rotation, rb->q, q );
+      q_normalize( q );
+   }
+   else
+   {
+      v4_copy( rb->q, q );
+   }
+
+   q_m3x3( q, transform );
+   v3_copy( co, transform[3] );
+}
+
 /*
  * Initialize rigidbody and calculate masses, inertia
  */
@@ -338,17 +376,12 @@ static void rb_init( rigidbody *rb )
       v3f dims;
       v3_sub( rb->bbx[1], rb->bbx[0], dims );
       volume = dims[0]*dims[1]*dims[2];
-      
-      if( !rb->is_world )
-         vg_info( "Box volume: %f\n", volume );
    }
    else if( rb->type == k_rb_shape_sphere )
    {
       volume = sphere_volume( rb->inf.sphere.radius );
       v3_fill( rb->bbx[0], -rb->inf.sphere.radius );
       v3_fill( rb->bbx[1],  rb->inf.sphere.radius );
-
-      vg_info( "Sphere volume: %f\n", volume );
    }
    else if( rb->type == k_rb_shape_capsule )
    {
@@ -634,7 +667,6 @@ static void closest_on_triangle( v3f p, v3f tri[3], v3f dest )
    v3_muladds( dest, ac, w, dest );
 }
 
-/* TODO */
 static void closest_on_triangle_1( v3f p, v3f tri[3], v3f dest )
 {
    v3f ab, ac, ap;
@@ -1078,7 +1110,6 @@ static int rb_capsule_box( rigidbody *rba, rigidbody *rbb, rb_ct *buf )
    v3_sub( c1, p1, d1 );
    v3_sub( p1, p0, da );
    
-   /* TODO: ? */
    v3_normalize(d0);
    v3_normalize(d1);
    v3_normalize(da);
@@ -1336,7 +1367,6 @@ static int rb_box_scene( rigidbody *rba, rigidbody *rbb, rb_ct *buf )
          axis = 1;
       }
       
-      /* TODO: THIS IS WRONG DIRECTION */
       float cz = -v3_dot( rba->forward, n );
       if( fabsf(cz) > fabsf(best) )
       {
@@ -1482,7 +1512,7 @@ static int rb_collide( rigidbody *rba, rigidbody *rbb )
    }
 
    /*
-    * TODO: Replace this with a more dedicated broad phase pass
+    * FUTURE: Replace this with a more dedicated broad phase pass
     */
    if( box_overlap( rba->bbx_world, rbb->bbx_world ) )
    {
@@ -1743,7 +1773,6 @@ static void rb_limit_cure( rigidbody *ra, rigidbody *rb, v3f axis, float d )
 static void rb_constraint_limits( rigidbody *ra, v3f lca, 
                                   rigidbody *rb, v3f lcb, v3f limits[2] )
 {
-   /* TODO: Code dupe remover */
    v3f ax, ay, az, bx, by, bz;
    m3x3_mulv( ra->to_world, (v3f){1.0f,0.0f,0.0f}, ax );
    m3x3_mulv( ra->to_world, (v3f){0.0f,1.0f,0.0f}, ay );
@@ -1860,6 +1889,23 @@ static void rb_constraint_position( rigidbody *ra, v3f lca,
    }
 }
 
+/* 
+ * Effectors
+ */
+
+static void rb_effect_simple_bouyency( rigidbody *ra, v4f plane, 
+                                       float amt, float drag )
+{
+   /* float */
+   float depth  = v3_dot( plane, ra->co ) - plane[3],
+         lambda = vg_clampf( -depth, 0.0f, 1.0f ) * amt;
+
+   v3_muladds( ra->v, plane, lambda * k_rb_delta, ra->v );
+
+   if( depth < 0.0f )
+      v3_muls( ra->v, 1.0f-(drag*k_rb_delta), ra->v );
+}
+
 /*
  * -----------------------------------------------------------------------------
  * BVH implementation, this is ONLY for static rigidbodies, its to slow for