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

diff --git a/rigidbody.h b/rigidbody.h
index aa74a84..eeed745 100644
--- a/rigidbody.h
+++ b/rigidbody.h
@@ -4,22 +4,33 @@
  */
 
 #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
 
-#include "bvh.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;
+
+   enum rb_shape
+   {
+      k_rb_shape_box,
+      k_rb_shape_capsule
+   } 
+   type;
+   v3f top, bottom;
+   float radius;
+
    boxf bbx, bbx_world;
    float inv_mass;
 
@@ -69,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 */
@@ -289,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, 
@@ -384,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 );
 
@@ -402,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 );
 }
 
 /*
@@ -508,6 +484,8 @@ static void rb_build_manifold_rb_static( rigidbody *ra, rigidbody *rb_static )
 	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++ )
@@ -543,6 +521,267 @@ static void rb_build_manifold_rb_static( rigidbody *ra, rigidbody *rb_static )
    }
 }
 
+/* 
+ * 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.