stuff

[carveJwlIkooP6JGAAIwe30JlM.git] / rigidbody.h

diff --git a/rigidbody.h b/rigidbody.h
index 82e793437994f2bbf0616bd79e6af3d356ca667b..845237f2e5e7b3f15ca7b15576da890d5e4557a8 100644 (file)
--- a/rigidbody.h
+++ b/rigidbody.h
@@ -3,14 +3,16 @@
  *            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)
 
 typedef struct rigidbody rigidbody;
@@ -18,7 +20,7 @@ struct rigidbody
 {
    v3f co, v, I;
    v4f q;
-   boxf bbx;
+   boxf bbx, bbx_world;
    float inv_mass;
 
    struct contact
@@ -27,7 +29,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 +43,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 )
@@ -108,7 +113,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 +144,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 +181,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 +253,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 +289,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 +383,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 );
 
@@ -481,4 +494,99 @@ 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] );
+
+   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;
+      }
+   }
+}
+
+/*
+ * 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 */