X-Git-Url: https://harrygodden.com/git/?p=vg.git;a=blobdiff_plain;f=vg_rigidbody_collision.h;fp=vg_rigidbody_collision.h;h=c08baa536a5e55714d98fd7a39393d8e2098ed7f;hp=31bd1bb3e3fd9c5ffa55e8d04cb444191913667e;hb=fce86711735b15bff37de0f70716808410fcf269;hpb=1c305409e8eca9cf8449d681df73208956ce14df

diff --git a/vg_rigidbody_collision.h b/vg_rigidbody_collision.h
index 31bd1bb..c08baa5 100644
--- a/vg_rigidbody_collision.h
+++ b/vg_rigidbody_collision.h
@@ -1,6 +1,10 @@
 #pragma once
 #include "vg_rigidbody.h"
 
+#ifndef VG_MAX_CONTACTS
+#define VG_MAX_CONTACTS 256
+#endif
+
 typedef struct rb_ct rb_ct;
 static struct rb_ct{
    rigidbody *rba, *rbb;
@@ -13,7 +17,7 @@ static struct rb_ct{
 
    enum contact_type type;
 }
-rb_contact_buffer[256];
+rb_contact_buffer[VG_MAX_CONTACTS];
 static int rb_contact_count = 0;
 
 /*
@@ -117,29 +121,28 @@ static int rb_capsule__manifold_done( m4x3f mtx, rb_capsule *c,
     * Debugging
     */
 
+#if 0
    if( count == 2 )
       vg_line( buf[0].co, buf[1].co, 0xff0000ff );
+#endif
 
    return count;
 }
 
-#if 0
-static int rb_capsule_sphere( rb_object *obja, rb_object *objb, rb_ct *buf ){
-   rigidbody *rba = &obja->rb, *rbb = &objb->rb;
-   f32 h = obja->inf.capsule.h,
-        ra = obja->inf.capsule.r,
-        rb = objb->inf.sphere.r;
+static int rb_capsule__sphere( m4x3f mtxA, rb_capsule *ca,
+                               v3f coB, f32 rb, rb_ct *buf ){
+   f32 ha = ca->h,
+       ra = ca->r,
+       r  = ra + rb;
 
    v3f p0, p1;
-   v3_muladds( rba->co, rba->to_world[1], -h*0.5f+ra, p0 );
-   v3_muladds( rba->co, rba->to_world[1],  h*0.5f-ra, p1 );
+   v3_muladds( mtxA[3], mtxA[1], -ha*0.5f+ra, p0 );
+   v3_muladds( mtxA[3], mtxA[1],  ha*0.5f-ra, p1 );
 
    v3f c, delta;
-   closest_point_segment( p0, p1, rbb->co, c );
-   v3_sub( c, rbb->co, delta );
-
-   f32 d2 = v3_length2(delta),
-          r = ra + rb;
+   closest_point_segment( p0, p1, coB, c );
+   v3_sub( c, coB, delta );
+   f32 d2 = v3_length2(delta);
 
    if( d2 < r*r ){
       f32 d = sqrtf(d2);
@@ -150,20 +153,14 @@ static int rb_capsule_sphere( rb_object *obja, rb_object *objb, rb_ct *buf ){
 
       v3f p0, p1;
       v3_muladds( c, ct->n, -ra, p0 );
-      v3_muladds( rbb->co, ct->n, rb,  p1 );
+      v3_muladds( coB, ct->n, rb,  p1 );
       v3_add( p0, p1, ct->co );
       v3_muls( ct->co, 0.5f, ct->co );
-
-      ct->rba = rba;
-      ct->rbb = rbb;
       ct->type = k_contact_type_default;
-
       return 1;
    }
-
-   return 0;
+   else return 0;
 }
-#endif
 
 static int rb_capsule__capsule( m4x3f mtxA, rb_capsule *ca,
                                 m4x3f mtxB, rb_capsule *cb, rb_ct *buf ){
@@ -200,71 +197,170 @@ static int rb_capsule__capsule( m4x3f mtxA, rb_capsule *ca,
    return rb_capsule__manifold_done( mtxA, ca, &manifold, buf );
 }
 
-#if 0
-static int rb_sphere_box( rb_object *obja, rb_object *objb, rb_ct *buf ){
-   v3f co, delta;
-   rigidbody *rba = &obja->rb, *rbb = &objb->rb;
+/*
+ * Generates up to two contacts; optimised for the most stable manifold
+ */
+static int rb_capsule__box( m4x3f mtxA, rb_capsule *ca,
+                            m4x3f mtxB, m4x3f mtxB_inverse, boxf box, 
+                            rb_ct *buf ){
+   f32 h = ca->h, r = ca->r;
 
-   closest_point_obb( rba->co, rbb->bbx, rbb->to_world, rbb->to_local, co );
-   v3_sub( rba->co, co, delta );
+   /*
+    * Solving this in symetric local space of the cube saves us some time and a 
+    * couple branches when it comes to the quad stage.
+    */
+   v3f centroid;
+   v3_add( box[0], box[1], centroid );
+   v3_muls( centroid, 0.5f, centroid );
 
-   f32 d2 = v3_length2(delta),
-          r = obja->inf.sphere.radius;
+   boxf bbx;
+   v3_sub( box[0], centroid, bbx[0] );
+   v3_sub( box[1], centroid, bbx[1] );
+   
+   v3f pc, p0w, p1w, p0, p1;
+   v3_muladds( mtxA[3], mtxA[1], -h*0.5f+r, p0w );
+   v3_muladds( mtxA[3], mtxA[1],  h*0.5f-r, p1w );
+
+   m4x3_mulv( mtxB_inverse, p0w, p0 );
+   m4x3_mulv( mtxB_inverse, p1w, p1 );
+   v3_sub( p0, centroid, p0 );
+   v3_sub( p1, centroid, p1 );
+   v3_add( p0, p1, pc );
+   v3_muls( pc, 0.5f, pc );
+
+   /* 
+    * Finding an appropriate quad to collide lines with
+    */
+   v3f region;
+   v3_div( pc, bbx[1], region );
+   
+   v3f quad[4];
+   if( (fabsf(region[0]) > fabsf(region[1])) && 
+       (fabsf(region[0]) > fabsf(region[2])) )
+   {
+      f32 px = vg_signf(region[0]) * bbx[1][0];
+      v3_copy( (v3f){ px, bbx[0][1], bbx[0][2] }, quad[0] );
+      v3_copy( (v3f){ px, bbx[1][1], bbx[0][2] }, quad[1] );
+      v3_copy( (v3f){ px, bbx[1][1], bbx[1][2] }, quad[2] );
+      v3_copy( (v3f){ px, bbx[0][1], bbx[1][2] }, quad[3] );
+   }
+   else if( fabsf(region[1]) > fabsf(region[2]) )
+   {
+      f32 py = vg_signf(region[1]) * bbx[1][1];
+      v3_copy( (v3f){ bbx[0][0], py, bbx[0][2] }, quad[0] );
+      v3_copy( (v3f){ bbx[1][0], py, bbx[0][2] }, quad[1] );
+      v3_copy( (v3f){ bbx[1][0], py, bbx[1][2] }, quad[2] );
+      v3_copy( (v3f){ bbx[0][0], py, bbx[1][2] }, quad[3] );
+   }
+   else
+   {
+      f32 pz = vg_signf(region[2]) * bbx[1][2];
+      v3_copy( (v3f){ bbx[0][0], bbx[0][1], pz }, quad[0] );
+      v3_copy( (v3f){ bbx[1][0], bbx[0][1], pz }, quad[1] );
+      v3_copy( (v3f){ bbx[1][0], bbx[1][1], pz }, quad[2] );
+      v3_copy( (v3f){ bbx[0][0], bbx[1][1], pz }, quad[3] );
+   }
 
-   if( d2 <= r*r ){
+   capsule_manifold manifold;
+   rb_capsule_manifold_init( &manifold );
+   
+   v3f c0, c1;
+   closest_point_aabb( p0, bbx, c0 );
+   closest_point_aabb( p1, bbx, c1 );
+
+   v3f d0, d1, da;
+   v3_sub( c0, p0, d0 );
+   v3_sub( c1, p1, d1 );
+   v3_sub( p1, p0, da );
+   
+   /* TODO: ? */
+   v3_normalize(d0);
+   v3_normalize(d1);
+   v3_normalize(da);
+
+   if( v3_dot( da, d0 ) <= 0.01f )
+      rb_capsule_manifold( p0, c0, 0.0f, r, &manifold );
+
+   if( v3_dot( da, d1 ) >= -0.01f )
+      rb_capsule_manifold( p1, c1, 1.0f, r, &manifold );
+
+   for( i32 i=0; i<4; i++ ){
+      i32 i0 = i,
+          i1 = (i+1)%4;
+
+      v3f ca, cb;
+      f32 ta, tb;
+      closest_segment_segment( p0, p1, quad[i0], quad[i1], &ta, &tb, ca, cb );
+      rb_capsule_manifold( ca, cb, ta, r, &manifold );
+   }
+
+   /*
+    * Create final contacts based on line manifold
+    */
+   m3x3_mulv( mtxB, manifold.d0, manifold.d0 );
+   m3x3_mulv( mtxB, manifold.d1, manifold.d1 );
+   return rb_capsule__manifold_done( mtxA, ca, &manifold, buf );
+}
+
+static int rb_sphere__box( v3f coA, f32 ra, 
+                           m4x3f mtxB, m4x3f mtxB_inverse, boxf box,
+                           rb_ct *buf ){
+   v3f co, delta;
+   closest_point_obb( coA, box, mtxB, mtxB_inverse, co );
+   v3_sub( coA, co, delta );
+
+   f32 d2 = v3_length2(delta);
+
+   if( d2 <= ra*ra ){
       f32 d;
 
       rb_ct *ct = buf;
       if( d2 <= 0.0001f ){
-         v3_sub( rba->co, rbb->co, delta );
+         v3f e, coB;
+         v3_sub( box[1], box[0], e );
+         v3_muls( e, 0.5f, e );
+         v3_add( box[0], e, coB );
+         v3_sub( coA, coB, 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.
           */
-         f32 lx = v3_dot( rbb->to_world[0], delta ),
-               ly = v3_dot( rbb->to_world[1], delta ),
-               lz = v3_dot( rbb->to_world[2], 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->to_world[0], vg_signf(lx), ct->n );
-         else if( py < pz )
-            v3_muls( rbb->to_world[1], vg_signf(ly), ct->n );
-         else
-            v3_muls( rbb->to_world[2], vg_signf(lz), ct->n );
-
-         v3_muladds( rba->co, ct->n, -r, ct->co );
-         ct->p = r;
+         f32 lx = v3_dot( mtxB[0], delta ),
+             ly = v3_dot( mtxB[1], delta ),
+             lz = v3_dot( mtxB[2], delta ),
+             px = e[0] - fabsf(lx),
+             py = e[1] - fabsf(ly),
+             pz = e[2] - fabsf(lz);
+
+         if( px < py && px < pz ) v3_muls( mtxB[0], vg_signf(lx), ct->n );
+         else if( py < pz )       v3_muls( mtxB[1], vg_signf(ly), ct->n );
+         else                     v3_muls( mtxB[2], vg_signf(lz), ct->n );
+
+         v3_muladds( coA, ct->n, -ra, ct->co );
+         ct->p = ra;
       }
       else{
          d = sqrtf(d2);
          v3_muls( delta, 1.0f/d, ct->n );
-         ct->p = r-d;
+         ct->p = ra-d;
          v3_copy( co, ct->co );
       }
 
-      ct->rba = rba;
-      ct->rbb = rbb;
       ct->type = k_contact_type_default;
       return 1;
    }
-
-   return 0;
+   else return 0;
 }
-#endif
 
-#if 0
-static int rb_sphere_sphere( rb_object *obja, rb_object *objb, rb_ct *buf ){
-   rigidbody *rba = &obja->rb, *rbb = &objb->rb;
+static int rb_sphere__sphere( v3f coA, f32 ra, 
+                              v3f coB, f32 rb, rb_ct *buf ){
    v3f delta;
-   v3_sub( rba->co, rbb->co, delta );
+   v3_sub( coA, coB, delta );
 
    f32 d2 = v3_length2(delta),
-          r = obja->inf.sphere.radius + objb->inf.sphere.radius;
+        r = ra+rb;
 
    if( d2 < r*r ){
       f32 d = sqrtf(d2);
@@ -273,20 +369,16 @@ static int rb_sphere_sphere( rb_object *obja, rb_object *objb, rb_ct *buf ){
       v3_muls( delta, 1.0f/d, ct->n );
 
       v3f p0, p1;
-      v3_muladds( rba->co, ct->n,-obja->inf.sphere.radius, p0 );
-      v3_muladds( rbb->co, ct->n, objb->inf.sphere.radius, p1 );
+      v3_muladds( coA, ct->n,-ra, p0 );
+      v3_muladds( coB, ct->n, rb, p1 );
       v3_add( p0, p1, ct->co );
       v3_muls( ct->co, 0.5f, ct->co );
       ct->type = k_contact_type_default;
       ct->p = r-d;
-      ct->rba = rba;
-      ct->rbb = rbb;
       return 1;
    }
-
-   return 0;
+   else return 0;
 }
-#endif
 
 static int rb_sphere__triangle( m4x3f mtxA, f32 r,
                                 v3f tri[3], rb_ct *buf ){
@@ -669,8 +761,8 @@ static rb_ct *rb_global_ct(void){
    return rb_contact_buffer + rb_contact_count;
 }
 
-static void rb_prepare_contact( rb_ct *ct, float timestep ){
-   ct->bias = -k_phys_baumgarte * (timestep*3600.0f) 
+static void rb_prepare_contact( rb_ct *ct ){
+   ct->bias = -k_phys_baumgarte * (vg.time_fixed_delta*3600.0f) 
                * vg_minf( 0.0f, -ct->p+k_penetration_slop );
    
    v3_tangent_basis( ct->n, ct->t[0], ct->t[1] );
@@ -705,7 +797,7 @@ static void rb_depenetrate( rb_ct *manifold, int len, v3f dt ){
 static void rb_presolve_contacts( rb_ct *buffer, int len ){
    for( int i=0; i<len; i++ ){
       rb_ct *ct = &buffer[i];
-      rb_prepare_contact( ct, k_rb_delta );
+      rb_prepare_contact( ct );
 
       v3f ra, rb, raCn, rbCn, raCt, rbCt;
       v3_sub( ct->co, ct->rba->co, ra );
@@ -735,8 +827,6 @@ static void rb_presolve_contacts( rb_ct *buffer, int len ){
          ct->tangent_mass[j] += v3_dot( rbCt, rbCtI );
          ct->tangent_mass[j]  = 1.0f/ct->tangent_mass[j];
       }
-
-      rb_debug_contact( ct );
    }
 }