X-Git-Url: https://harrygodden.com/git/?a=blobdiff_plain;f=rigidbody.h;h=eeed74500a8db526484d788bd935d2bcf276fc6d;hb=6d66c67945f84476d6ac75a0497007cc30bcf58c;hp=82e793437994f2bbf0616bd79e6af3d356ca667b;hpb=e36cb7e44611855d39ba84710a7007ca659e9cd5;p=carveJwlIkooP6JGAAIwe30JlM.git

diff --git a/rigidbody.h b/rigidbody.h
index 82e7934..eeed745 100644
--- a/rigidbody.h
+++ b/rigidbody.h
@@ -3,22 +3,35 @@
  *            qu3e  - Randy Gaul
  */
 
-#include "vg/vg.h"
+#include "common.h"
+#include "bvh.h"
+
 static void rb_tangent_basis( v3f n, v3f tx, v3f ty );
+static bh_system bh_system_rigidbodies;
 
 #ifndef RIGIDBODY_H
 #define RIGIDBODY_H
 
 #define RB_DEPR 
-
-#define k_rb_delta (1.0f/60.0f)
+#define k_rb_rate  60.0f
+#define k_rb_delta (1.0f/k_rb_rate)
 
 typedef struct rigidbody rigidbody;
 struct rigidbody
 {
    v3f co, v, I;
    v4f q;
-   boxf bbx;
+
+   enum rb_shape
+   {
+      k_rb_shape_box,
+      k_rb_shape_capsule
+   } 
+   type;
+   v3f top, bottom;
+   float radius;
+
+   boxf bbx, bbx_world;
    float inv_mass;
 
    struct contact
@@ -27,7 +40,7 @@ struct rigidbody
       v3f t[2];
       float bias, norm_impulse, tangent_impulse[2];
    }
-   manifold[4];
+   manifold[12];
    int manifold_count;
 
    v3f delta;  /* where is the origin of this in relation to a parent body */
@@ -41,6 +54,9 @@ static void rb_update_transform( rigidbody *rb )
    v3_copy( rb->co, rb->to_world[3] );
 
    m4x3_invert_affine( rb->to_world, rb->to_local );
+
+   box_copy( rb->bbx, rb->bbx_world );
+   m4x3_transform_aabb( rb->to_world, rb->bbx_world );
 }
 
 static void rb_init( rigidbody *rb )
@@ -64,7 +80,6 @@ static void rb_iter( rigidbody *rb )
 
    /* intergrate velocity */
    v3_muladds( rb->co, rb->v, k_rb_delta, rb->co );
-
    v3_lerp( rb->I, (v3f){0.0f,0.0f,0.0f}, 0.0025f, rb->I );
 
    /* inegrate inertia */
@@ -108,7 +123,12 @@ static void rb_tangent_basis( v3f n, v3f tx, v3f ty )
 
 #include "world.h"
 
-static void rb_build_manifold( rigidbody *rb )
+static void rb_manifold_reset( rigidbody *rb )
+{
+   rb->manifold_count = 0;
+}
+
+static void rb_build_manifold_terrain( rigidbody *rb )
 {
    v3f *box = rb->bbx;
    v3f pts[8]; 
@@ -134,7 +154,7 @@ static void rb_build_manifold( rigidbody *rb )
    m4x3_mulv( rb->to_world, p110, p110 );
    m4x3_mulv( rb->to_world, p111, p111 );
 
-   rb->manifold_count = 0;
+   int count = 0;
 
    for( int i=0; i<8; i++ )
    {
@@ -171,7 +191,8 @@ static void rb_build_manifold( rigidbody *rb )
          ct->tangent_impulse[1] = 0.0f;
 
          rb->manifold_count ++;
-         if( rb->manifold_count == 4 )
+         count ++;
+         if( count == 4 )
             break;
       }
    }
@@ -242,8 +263,7 @@ struct rb_angle_limit
    float impulse, bias;
 };
 
-static int rb_angle_limit_force( 
-                                  rigidbody *rba, v3f va, 
+static int rb_angle_limit_force(  rigidbody *rba, v3f va, 
                                   rigidbody *rbb, v3f vb,
                                   float max )
 {
@@ -279,7 +299,6 @@ static void rb_constraint_angle_limit( struct rb_angle_limit *limit )
 
 }
 
-
 RB_DEPR
 static void rb_constraint_angle( rigidbody *rba, v3f va,
                                  rigidbody *rbb, v3f vb, 
@@ -374,6 +393,10 @@ static void rb_constraint_position( rigidbody *ra, v3f lca,
    v3_add( impulse, ra->I, ra->I );
 
 #if 0
+   /*
+    * this could be used for spring joints
+    * its not good for position constraint
+    */
    v3f impulse;
    v3_muls( delta, 0.5f*spring, impulse );
 
@@ -392,44 +415,7 @@ static void rb_constraint_position( rigidbody *ra, v3f lca,
 static void rb_debug( rigidbody *rb, u32 colour )
 {
    v3f *box = rb->bbx;
-   v3f p000, p001, p010, p011, p100, p101, p110, p111;
-
-   p000[0]=box[0][0];p000[1]=box[0][1];p000[2]=box[0][2];
-   p001[0]=box[0][0];p001[1]=box[0][1];p001[2]=box[1][2];
-   p010[0]=box[0][0];p010[1]=box[1][1];p010[2]=box[0][2];
-   p011[0]=box[0][0];p011[1]=box[1][1];p011[2]=box[1][2];
-
-   p100[0]=box[1][0];p100[1]=box[0][1];p100[2]=box[0][2];
-   p101[0]=box[1][0];p101[1]=box[0][1];p101[2]=box[1][2];
-   p110[0]=box[1][0];p110[1]=box[1][1];p110[2]=box[0][2];
-   p111[0]=box[1][0];p111[1]=box[1][1];p111[2]=box[1][2];
-
-   m4x3_mulv( rb->to_world, p000, p000 );
-   m4x3_mulv( rb->to_world, p001, p001 );
-   m4x3_mulv( rb->to_world, p010, p010 );
-   m4x3_mulv( rb->to_world, p011, p011 );
-   m4x3_mulv( rb->to_world, p100, p100 );
-   m4x3_mulv( rb->to_world, p101, p101 );
-   m4x3_mulv( rb->to_world, p110, p110 );
-   m4x3_mulv( rb->to_world, p111, p111 );
-   
-   vg_line( p000, p001, colour );
-   vg_line( p001, p011, colour );
-   vg_line( p011, p010, colour );
-   vg_line( p010, p000, colour );
-
-   vg_line( p100, p101, colour );
-   vg_line( p101, p111, colour );
-   vg_line( p111, p110, colour );
-   vg_line( p110, p100, colour );
-
-   vg_line( p100, p000, colour );
-   vg_line( p101, p001, colour );
-   vg_line( p110, p010, colour );
-   vg_line( p111, p011, colour );
-
-   vg_line( p000, p110, colour );
-   vg_line( p100, p010, colour );
+   vg_line_boxf_transformed( rb->to_world, rb->bbx, colour );
 }
 
 /*
@@ -481,4 +467,362 @@ static int rb_point_in_body( rigidbody *rb, v3f pos, float *pen, v3f normal )
    return 0;
 }
 
+static void rb_build_manifold_rb_static( rigidbody *ra, rigidbody *rb_static )
+{
+   v3f verts[8];
+
+   v3f a, b;
+   v3_copy( ra->bbx[0], a );
+   v3_copy( ra->bbx[1], b );
+   
+	m4x3_mulv( ra->to_world, (v3f){ a[0], a[1], a[2] }, verts[0] );
+	m4x3_mulv( ra->to_world, (v3f){ a[0], b[1], a[2] }, verts[1] );
+	m4x3_mulv( ra->to_world, (v3f){ b[0], b[1], a[2] }, verts[2] );
+	m4x3_mulv( ra->to_world, (v3f){ b[0], a[1], a[2] }, verts[3] );
+	m4x3_mulv( ra->to_world, (v3f){ a[0], a[1], b[2] }, verts[4] );
+	m4x3_mulv( ra->to_world, (v3f){ a[0], b[1], b[2] }, verts[5] );
+	m4x3_mulv( ra->to_world, (v3f){ b[0], b[1], b[2] }, verts[6] );
+	m4x3_mulv( ra->to_world, (v3f){ b[0], a[1], b[2] }, verts[7] );
+
+   vg_line_boxf_transformed( rb_static->to_world, rb_static->bbx, 0xff0000ff );
+
+   int count = 0;
+
+   for( int i=0; i<8; i++ )
+   {
+      if( ra->manifold_count == vg_list_size(ra->manifold) )
+         return;
+
+      struct contact *ct = &ra->manifold[ ra->manifold_count ];
+      
+      float p;
+      v3f normal;
+
+      if( rb_point_in_body( rb_static, verts[i], &p, normal ))
+      {
+         v3_copy( normal, ct->n );
+         v3_muladds( verts[i], ct->n, p*0.5f, ct->co );
+         v3_sub( ct->co, ra->co, ct->delta );
+
+         vg_line_pt3( ct->co, 0.0125f, 0xffff00ff );
+         
+         ct->bias = -0.2f * (1.0f/k_rb_delta) * vg_minf( 0.0f, -p+0.04f );
+         rb_tangent_basis( ct->n, ct->t[0], ct->t[1] );
+
+         ct->norm_impulse = 0.0f;
+         ct->tangent_impulse[0] = 0.0f;
+         ct->tangent_impulse[1] = 0.0f;
+
+         ra->manifold_count ++;
+         count ++;
+         if( count == 4 )
+            return;
+      }
+   }
+}
+
+/* 
+ * Capsule phyics
+ */
+
+static float closest_segment_segment( v3f p1, v3f q1, v3f p2, v3f q2, 
+   float *s, float *t, v3f c1, v3f c2)
+{
+   v3f d1,d2,r;
+   v3_sub( q1, p1, d1 );
+   v3_sub( q2, p2, d2 );
+   v3_sub( p1, p2, r );
+
+   float a = v3_length2( d1 ),
+         e = v3_length2( d2 ),
+         f = v3_dot( d2, r );
+
+   const float kEpsilon = 0.0001f;
+
+   if( a <= kEpsilon && e <= kEpsilon )
+   {
+      *s = 0.0f;
+      *t = 0.0f;
+      v3_copy( p1, c1 );
+      v3_copy( p2, c2 );
+
+      v3f v0;
+      v3_sub( c1, c2, v0 );
+
+      return v3_length2( v0 );
+   }
+   
+   if( a<= kEpsilon )
+   {
+      *s = 0.0f;
+      *t = vg_clampf( f / e, 0.0f, 1.0f );
+   }
+   else
+   {
+      float c = v3_dot( d1, r );
+      if( e <= kEpsilon )
+      {
+         *t = 0.0f;
+         *s = vg_clampf( -c / a, 0.0f, 1.0f );
+      }
+      else
+      {
+         float b = v3_dot(d1,d2),
+               d = a*e-b*b;
+
+         if( d != 0.0f )
+         {
+            *s = vg_clampf((b*f - c*e)/d, 0.0f, 1.0f);
+         }
+         else
+         {
+            *s = 0.0f;
+         }
+
+         *t = (b*(*s)+f) / e;
+
+         if( *t < 0.0f )
+         {
+            *t = 0.0f;
+            *s = vg_clampf( -c / a, 0.0f, 1.0f );
+         }
+         else if( *t > 1.0f )
+         {
+            *t = 1.0f;
+            *s = vg_clampf((b-c)/a,0.0f,1.0f);
+         }
+      }
+   }
+
+   v3_muladds( p1, d1, *s, c1 );
+   v3_muladds( p2, d2, *t, c2 );
+
+   v3f v0;
+   v3_sub( c1, c2, v0 );
+   return v3_length2( v0 );
+}
+
+static void closest_point_segment( v3f a, v3f b, v3f point, v3f dest )
+{
+   v3f v0, v1;
+   v3_sub( b, a, v0 );
+   v3_sub( point, a, v1 );
+
+   float t = v3_dot( v1, v0 ) / v3_length2(v0);
+   v3_muladds( a, v0, vg_clampf(t,0.0f,1.0f), dest );
+}
+
+/* Real-Time Collision Detection */
+static void closest_on_triangle( v3f p, v3f tri[3], v3f dest )
+{
+   v3f ab, ac, ap;
+   float d1, d2;
+
+   /* Region outside A */
+   v3_sub( tri[1], tri[0], ab );
+   v3_sub( tri[2], tri[0], ac );
+   v3_sub( p, tri[0], ap );
+   
+   d1 = v3_dot(ab,ap);
+   d2 = v3_dot(ac,ap);
+   if( d1 <= 0.0f && d2 <= 0.0f ) 
+   {
+      v3_copy( tri[0], dest );
+      v3_copy( (v3f){INFINITY,INFINITY,INFINITY}, dest );
+      return;
+   }
+
+   /* Region outside B */
+   v3f bp;
+   float d3, d4;
+
+   v3_sub( p, tri[1], bp );
+   d3 = v3_dot( ab, bp );
+   d4 = v3_dot( ac, bp );
+
+   if( d3 >= 0.0f && d4 <= d3 )
+   {
+      v3_copy( tri[1], dest );
+      v3_copy( (v3f){INFINITY,INFINITY,INFINITY}, dest );
+      return;
+   }
+   
+   /* Edge region of AB */
+   float vc = d1*d4 - d3*d2;
+   if( vc <= 0.0f && d1 >= 0.0f && d3 <= 0.0f )
+   {
+      float v = d1 / (d1-d3);
+      v3_muladds( tri[0], ab, v, dest );
+      v3_copy( (v3f){INFINITY,INFINITY,INFINITY}, dest );
+      return;
+   }
+
+   /* Region outside C */
+   v3f cp;
+   float d5, d6;
+   v3_sub( p, tri[2], cp );
+   d5 = v3_dot(ab, cp);
+   d6 = v3_dot(ac, cp);
+   
+   if( d6 >= 0.0f && d5 <= d6 )
+   {
+      v3_copy( tri[2], dest );
+      v3_copy( (v3f){INFINITY,INFINITY,INFINITY}, dest );
+      return;
+   }
+
+   /* Region of AC */
+   float vb = d5*d2 - d1*d6;
+   if( vb <= 0.0f && d2 >= 0.0f && d6 <= 0.0f )
+   {
+      float w = d2 / (d2-d6);
+      v3_muladds( tri[0], ac, w, dest );
+      v3_copy( (v3f){INFINITY,INFINITY,INFINITY}, dest );
+      return;
+   }
+
+   /* Region of BC */
+   float va = d3*d6 - d5*d4;
+   if( va <= 0.0f && (d4-d3) >= 0.0f && (d5-d6) >= 0.0f )
+   {
+      float w = (d4-d3) / ((d4-d3) + (d5-d6));
+      v3f bc;
+      v3_sub( tri[2], tri[1], bc );
+      v3_muladds( tri[1], bc, w, dest );
+      v3_copy( (v3f){INFINITY,INFINITY,INFINITY}, dest );
+      return;
+   }
+
+   /* P inside region, Q via barycentric coordinates uvw */
+   float d = 1.0f/(va+vb+vc),
+         v = vb*d,
+         w = vc*d;
+
+   v3_muladds( tri[0], ab, v, dest );
+   v3_muladds( dest, ac, w, dest );
+}
+
+static int sphere_vs_triangle( v3f c, float r, v3f tri[3], 
+      v3f co, v3f norm, float *p )
+{
+   v3f delta;
+   closest_on_triangle( c, tri, co );
+
+   v3_sub( c, co, delta );
+
+
+   float d = v3_length2( delta );
+   if( d < r*r )
+   {
+      v3f ab, ac, tn;
+      v3_sub( tri[1], tri[0], ab );
+      v3_sub( tri[2], tri[0], ac );
+      v3_cross( ac, ab, tn );
+
+      if( v3_dot( delta, tn ) > 0.0f )
+         v3_muls( delta, -1.0f, delta );
+
+      vg_line_pt3( co, 0.05f, 0xff00ff00 );
+
+      d = sqrtf(d);
+      v3_muls( delta, 1.0f/d, norm );
+
+      *p = r-d;
+      return 1;
+   }
+
+   return 0;
+}
+
+static void debug_capsule( m4x3f m, float height, float radius, u32 colour )
+{
+   v3f last = { 0.0f, 0.0f, radius };
+   m4x3f lower, upper;
+   m3x3_copy( m, lower );
+   m3x3_copy( m, upper );
+   m4x3_mulv( m, (v3f){0.0f,-height*0.5f+radius,0.0f}, lower[3] );
+   m4x3_mulv( m, (v3f){0.0f, height*0.5f-radius,0.0f}, upper[3] );
+   
+   for( int i=0; i<16; i++ )
+   {
+      float t = ((float)(i+1) * (1.0f/16.0f)) * VG_PIf * 2.0f,
+            s = sinf(t),
+            c = cosf(t);
+
+      v3f p = { s*radius, 0.0f, c*radius };
+
+      v3f p0, p1;
+      m4x3_mulv( lower, p, p0 );
+      m4x3_mulv( lower, last, p1 );
+      vg_line( p0, p1, colour );
+
+      m4x3_mulv( upper, p, p0 );
+      m4x3_mulv( upper, last, p1 );
+      vg_line( p0, p1, colour );
+
+      v3_copy( p, last );
+   }
+
+   for( int i=0; i<4; i++ )
+   {
+      float t = ((float)(i) * (1.0f/4.0f)) * VG_PIf * 2.0f,
+            s = sinf(t),
+            c = cosf(t);
+
+      v3f p = { s*radius, 0.0f, c*radius };
+
+      v3f p0, p1;
+      m4x3_mulv( lower, p, p0 );
+      m4x3_mulv( upper, p, p1 );
+      vg_line( p0, p1, colour );
+
+      m4x3_mulv( lower, (v3f){0.0f,-radius,0.0f}, p0 );
+      m4x3_mulv( upper, (v3f){0.0f, radius,0.0f}, p1 );
+      vg_line( p0, p1, colour );
+   }
+}
+
+/*
+ * BVH implementation, this is ONLY for static rigidbodies, its to slow for
+ * realtime use.
+ */
+
+static void rb_bh_expand_bound( void *user, boxf bound, u32 item_index )
+{
+   rigidbody *rb = &((rigidbody *)user)[ item_index ];
+   box_concat( bound, rb->bbx_world );
+}
+
+static float rb_bh_centroid( void *user, u32 item_index, int axis )
+{
+   rigidbody *rb = &((rigidbody *)user)[ item_index ];
+   return (rb->bbx_world[axis][0] + rb->bbx_world[1][axis]) * 0.5f;
+}
+
+static void rb_bh_swap( void *user, u32 ia, u32 ib )
+{
+   rigidbody temp, *rba, *rbb;
+   rba = &((rigidbody *)user)[ ia ];
+   rbb = &((rigidbody *)user)[ ib ];
+
+   temp = *rba;
+   *rba = *rbb;
+   *rbb = temp;
+}
+
+static void rb_bh_debug( void *user, u32 item_index )
+{
+   rigidbody *rb = &((rigidbody *)user)[ item_index ];
+   rb_debug( rb, 0xff00ffff );
+}
+
+static bh_system bh_system_rigidbodies =
+{
+   .expand_bound = rb_bh_expand_bound,
+   .item_centroid = rb_bh_centroid,
+   .item_swap = rb_bh_swap,
+   .item_debug = rb_bh_debug,
+   .cast_ray = NULL
+};
+
 #endif /* RIGIDBODY_H */