X-Git-Url: https://harrygodden.com/git/?a=blobdiff_plain;f=rigidbody.h;h=a69e4afd69cb5c2b41089b65b3dbdd31f248bc70;hb=7b4f1548e85202dd34e4f849e56e042d73dbb6f2;hp=d19b7862b174a007efa6df04702a5c518eb3f601;hpb=35b57a341eb37d863ec69e4f011a88b7bfba5c01;p=carveJwlIkooP6JGAAIwe30JlM.git

diff --git a/rigidbody.h b/rigidbody.h
index d19b786..a69e4af 100644
--- a/rigidbody.h
+++ b/rigidbody.h
@@ -105,7 +105,7 @@ struct rb_object{
    inf;
 };
 
-VG_STATIC struct contact{
+static struct contact{
    rigidbody *rba, *rbb;
    v3f co, n;
    v3f t[2];
@@ -117,7 +117,7 @@ VG_STATIC struct contact{
    enum contact_type type;
 }
 rb_contact_buffer[256];
-VG_STATIC int rb_contact_count = 0;
+static int rb_contact_count = 0;
 
 typedef struct rb_constr_pos rb_constr_pos;
 typedef struct rb_constr_swingtwist rb_constr_swingtwist;
@@ -147,7 +147,7 @@ struct rb_constr_swingtwist{
  * -----------------------------------------------------------------------------
  */
 
-VG_STATIC void rb_debug_contact( rb_ct *ct ){
+static void rb_debug_contact( rb_ct *ct ){
    v3f p1;
    v3_muladds( ct->co, ct->n, 0.05f, p1 );
 
@@ -162,7 +162,7 @@ VG_STATIC void rb_debug_contact( rb_ct *ct ){
 }
 
 
-VG_STATIC void rb_object_debug( rb_object *obj, u32 colour ){
+static void rb_object_debug( rb_object *obj, u32 colour ){
    if( obj->type == k_rb_shape_box ){
       v3f *box = obj->rb.bbx;
       vg_line_boxf_transformed( obj->rb.to_world, obj->rb.bbx, colour );
@@ -191,7 +191,7 @@ VG_STATIC void rb_object_debug( rb_object *obj, u32 colour ){
 /*
  * Update world space bounding box based on local one
  */
-VG_STATIC void rb_update_bounds( rigidbody *rb ){
+static void rb_update_bounds( rigidbody *rb ){
    box_init_inf( rb->bbx_world );
    m4x3_expand_aabb_aabb( rb->to_world, rb->bbx_world, rb->bbx );
 }
@@ -199,7 +199,7 @@ VG_STATIC void rb_update_bounds( rigidbody *rb ){
 /*
  * Commit transform to rigidbody. Updates matrices
  */
-VG_STATIC void rb_update_transform( rigidbody *rb )
+static void rb_update_transform( rigidbody *rb )
 {
    q_normalize( rb->q );
    q_m3x3( rb->q, rb->to_world );
@@ -216,7 +216,7 @@ VG_STATIC void rb_update_transform( rigidbody *rb )
  * Extrapolate rigidbody into a transform based on vg accumulator.
  * Useful for rendering
  */
-VG_STATIC void rb_extrapolate( rigidbody *rb, v3f co, v4f q )
+static void rb_extrapolate( rigidbody *rb, v3f co, v4f q )
 {
    float substep = vg.time_fixed_extrapolate;
    v3_muladds( rb->co, rb->v, k_rb_delta*substep, co );
@@ -240,7 +240,7 @@ VG_STATIC void rb_extrapolate( rigidbody *rb, v3f co, v4f q )
 /*
  * Initialize rigidbody and calculate masses, inertia
  */
-VG_STATIC void rb_init_object( rb_object *obj ){
+static void rb_init_object( rb_object *obj ){
    float volume = 1.0f;
    int inert = 0;
 
@@ -302,7 +302,7 @@ VG_STATIC void rb_init_object( rb_object *obj ){
    rb_update_transform( &obj->rb );
 }
 
-VG_STATIC void rb_iter( rigidbody *rb ){
+static void rb_iter( rigidbody *rb ){
    if( !vg_validf( rb->v[0] ) ||
        !vg_validf( rb->v[1] ) ||
        !vg_validf( rb->v[2] ) )
@@ -330,9 +330,11 @@ VG_STATIC void rb_iter( rigidbody *rb ){
       q_mul( rotation, rb->q, rb->q );
    }
 
+#if 0
    /* damping */
    v3_muls( rb->v, 1.0f/(1.0f+k_rb_delta*k_damp_linear), rb->v );
    v3_muls( rb->w, 1.0f/(1.0f+k_rb_delta*k_damp_angular), rb->w );
+#endif
 }
 
 
@@ -345,7 +347,7 @@ VG_STATIC void rb_iter( rigidbody *rb ){
 /* 
  * Project AABB, and triangle interval onto axis to check if they overlap
  */
-VG_STATIC int rb_box_triangle_interval( v3f extent, v3f axis, v3f tri[3] ){
+static int rb_box_triangle_interval( v3f extent, v3f axis, v3f tri[3] ){
    float 
 
    r = extent[0] * fabsf(axis[0]) +
@@ -365,7 +367,7 @@ VG_STATIC int rb_box_triangle_interval( v3f extent, v3f axis, v3f tri[3] ){
 /*
  * Seperating axis test box vs triangle
  */
-VG_STATIC int rb_box_triangle_sat( v3f extent, v3f center,
+static int rb_box_triangle_sat( v3f extent, v3f center,
                                    m4x3f to_local, v3f tri_src[3] ){
    v3f tri[3];
 
@@ -412,7 +414,7 @@ VG_STATIC int rb_box_triangle_sat( v3f extent, v3f center,
  * -----------------------------------------------------------------------------
  */
 
-VG_STATIC int rb_manifold_apply_filtered( rb_ct *man, int len ){
+static int rb_manifold_apply_filtered( rb_ct *man, int len ){
    int k = 0;
 
    for( int i=0; i<len; i++ ){
@@ -430,7 +432,7 @@ VG_STATIC int rb_manifold_apply_filtered( rb_ct *man, int len ){
 /*
  * Merge two contacts if they are within radius(r) of eachother
  */
-VG_STATIC void rb_manifold_contact_weld( rb_ct *ci, rb_ct *cj, float r ){
+static void rb_manifold_contact_weld( rb_ct *ci, rb_ct *cj, float r ){
    if( v3_dist2( ci->co, cj->co ) < r*r ){
       cj->type = k_contact_type_disabled;
       ci->p = (ci->p + cj->p) * 0.5f;
@@ -461,7 +463,7 @@ VG_STATIC void rb_manifold_contact_weld( rb_ct *ci, rb_ct *cj, float r ){
 /*
  * 
  */
-VG_STATIC void rb_manifold_filter_joint_edges( rb_ct *man, int len, float r ){
+static void rb_manifold_filter_joint_edges( rb_ct *man, int len, float r ){
    for( int i=0; i<len-1; i++ ){
       rb_ct *ci = &man[i];
       if( ci->type != k_contact_type_edge ) 
@@ -480,7 +482,7 @@ VG_STATIC void rb_manifold_filter_joint_edges( rb_ct *man, int len, float r ){
 /*
  * Resolve overlapping pairs
  */
-VG_STATIC void rb_manifold_filter_pairs( rb_ct *man, int len, float r ){
+static void rb_manifold_filter_pairs( rb_ct *man, int len, float r ){
    for( int i=0; i<len-1; i++ ){
       rb_ct *ci = &man[i];
       int similar = 0;
@@ -516,7 +518,7 @@ VG_STATIC void rb_manifold_filter_pairs( rb_ct *man, int len, float r ){
 /* 
  * Remove contacts that are facing away from A
  */
-VG_STATIC void rb_manifold_filter_backface( rb_ct *man, int len ){
+static void rb_manifold_filter_backface( rb_ct *man, int len ){
    for( int i=0; i<len; i++ ){
       rb_ct *ct = &man[i];
       if( ct->type == k_contact_type_disabled ) 
@@ -533,7 +535,7 @@ VG_STATIC void rb_manifold_filter_backface( rb_ct *man, int len ){
 /*
  * Filter out duplicate coplanar results. Good for spheres.
  */
-VG_STATIC void rb_manifold_filter_coplanar( rb_ct *man, int len, float w ){
+static void rb_manifold_filter_coplanar( rb_ct *man, int len, float w ){
    for( int i=0; i<len; i++ ){
       rb_ct *ci = &man[i];
       if( ci->type == k_contact_type_disabled ||
@@ -597,7 +599,7 @@ struct capsule_manifold{
  * Expand a line manifold with a new pair. t value is the time along segment
  * on the oriented object which created this pair.
  */
-VG_STATIC void rb_capsule_manifold( v3f pa, v3f pb, float t, float r, 
+static void rb_capsule_manifold( v3f pa, v3f pb, float t, float r, 
                                     capsule_manifold *manifold ){
    v3f delta;
    v3_sub( pa, pb, delta );
@@ -617,12 +619,12 @@ VG_STATIC void rb_capsule_manifold( v3f pa, v3f pb, float t, float r,
    }
 }
 
-VG_STATIC void rb_capsule_manifold_init( capsule_manifold *manifold ){
+static void rb_capsule_manifold_init( capsule_manifold *manifold ){
    manifold->t0 =  INFINITY;
    manifold->t1 = -INFINITY;
 }
 
-VG_STATIC int rb_capsule__manifold_done( m4x3f mtx, rb_capsule *c,
+static int rb_capsule__manifold_done( m4x3f mtx, rb_capsule *c,
                                          capsule_manifold *manifold,
                                          rb_ct *buf ){
    v3f p0, p1;
@@ -673,7 +675,7 @@ VG_STATIC int rb_capsule__manifold_done( m4x3f mtx, rb_capsule *c,
    return count;
 }
 
-VG_STATIC int rb_capsule_sphere( rb_object *obja, rb_object *objb, rb_ct *buf ){
+static int rb_capsule_sphere( rb_object *obja, rb_object *objb, rb_ct *buf ){
    rigidbody *rba = &obja->rb, *rbb = &objb->rb;
    float h = obja->inf.capsule.height,
         ra = obja->inf.capsule.radius,
@@ -713,7 +715,7 @@ VG_STATIC int rb_capsule_sphere( rb_object *obja, rb_object *objb, rb_ct *buf ){
    return 0;
 }
 
-VG_STATIC int rb_capsule__capsule( m4x3f mtxA, rb_capsule *ca,
+static int rb_capsule__capsule( m4x3f mtxA, rb_capsule *ca,
                                    m4x3f mtxB, rb_capsule *cb, rb_ct *buf ){
    float ha = ca->height,
          hb = cb->height,
@@ -748,7 +750,7 @@ VG_STATIC int rb_capsule__capsule( m4x3f mtxA, rb_capsule *ca,
    return rb_capsule__manifold_done( mtxA, ca, &manifold, buf );
 }
 
-VG_STATIC int rb_sphere_box( rb_object *obja, rb_object *objb, rb_ct *buf ){
+static int rb_sphere_box( rb_object *obja, rb_object *objb, rb_ct *buf ){
    v3f co, delta;
    rigidbody *rba = &obja->rb, *rbb = &objb->rb;
 
@@ -803,7 +805,7 @@ VG_STATIC int rb_sphere_box( rb_object *obja, rb_object *objb, rb_ct *buf ){
    return 0;
 }
 
-VG_STATIC int rb_sphere_sphere( rb_object *obja, rb_object *objb, rb_ct *buf ){
+static int rb_sphere_sphere( rb_object *obja, rb_object *objb, rb_ct *buf ){
    rigidbody *rba = &obja->rb, *rbb = &objb->rb;
    v3f delta;
    v3_sub( rba->co, rbb->co, delta );
@@ -832,7 +834,7 @@ VG_STATIC int rb_sphere_sphere( rb_object *obja, rb_object *objb, rb_ct *buf ){
    return 0;
 }
 
-VG_STATIC int rb_sphere__triangle( m4x3f mtxA, rb_sphere *b,
+static int rb_sphere__triangle( m4x3f mtxA, rb_sphere *b,
                                    v3f tri[3], rb_ct *buf ){
    v3f delta, co;
    enum contact_type type = closest_on_triangle_1( mtxA[3], tri, co );
@@ -871,7 +873,7 @@ VG_STATIC int rb_sphere__triangle( m4x3f mtxA, rb_sphere *b,
    return 0;
 }
 
-VG_STATIC int rb_sphere__scene( m4x3f mtxA, rb_sphere *b,
+static int rb_sphere__scene( m4x3f mtxA, rb_sphere *b,
                                 m4x3f mtxB, rb_scene *s, rb_ct *buf, 
                                 u16 ignore ){
    scene_context *sc = s->bh_scene->user;
@@ -914,7 +916,7 @@ VG_STATIC int rb_sphere__scene( m4x3f mtxA, rb_sphere *b,
    return count;
 }
 
-VG_STATIC int rb_box__scene( m4x3f mtxA, boxf bbx,
+static int rb_box__scene( m4x3f mtxA, boxf bbx,
                              m4x3f mtxB, rb_scene *s, rb_ct *buf, u16 ignore ){
    scene_context *sc = s->bh_scene->user;
    v3f tri[3];
@@ -1068,7 +1070,7 @@ VG_STATIC int rb_box__scene( m4x3f mtxA, boxf bbx,
    return count;
 }
 
-VG_STATIC int rb_capsule__triangle( m4x3f mtxA, rb_capsule *c,
+static int rb_capsule__triangle( m4x3f mtxA, rb_capsule *c,
                                     v3f tri[3], rb_ct *buf ){
    v3f pc, p0w, p1w;
    v3_muladds( mtxA[3], mtxA[1], -c->height*0.5f+c->radius, p0w );
@@ -1076,6 +1078,48 @@ VG_STATIC int rb_capsule__triangle( m4x3f mtxA, rb_capsule *c,
 
    capsule_manifold manifold;
    rb_capsule_manifold_init( &manifold );
+
+   v3f v0, v1, n;
+   v3_sub( tri[1], tri[0], v0 );
+   v3_sub( tri[2], tri[0], v1 );
+   v3_cross( v0, v1, n );
+
+   if( v3_length2( n ) <= 0.00001f ){
+#ifdef RIGIDBODY_CRY_ABOUT_EVERYTHING
+      vg_error( "Zero area triangle!\n" );
+#endif
+      return 0;
+   }
+
+   v3_normalize( n );
+
+#if 1
+   /* deep penetration recovery. for when we clip through the triangles. so its
+    * not very 'correct' */
+   f32 dist;
+   if( ray_tri( tri, p0w, mtxA[1], &dist, 1 ) ){
+      f32 l = c->height - c->radius*2.0f;
+      if( (dist >= 0.0f) && (dist < l) ){
+         v3f co;
+         v3_muladds( p0w, mtxA[1], dist, co );
+         vg_line_point( co, 0.02f, 0xffffff00 );
+
+         v3f d0, d1;
+         v3_sub( p0w, co, d0 );
+         v3_sub( p1w, co, d1 );
+
+         f32 p = vg_minf( v3_dot( n, d0 ), v3_dot( n, d1 ) ) - c->radius;
+         
+         rb_ct *ct = buf;
+         ct->p = -p;
+         ct->type = k_contact_type_default;
+         v3_copy( n, ct->n );
+         v3_muladds( co, n, p, ct->co );
+
+         return 1;
+      }
+   }
+#endif
    
    v3f c0, c1;
    closest_on_triangle_1( p0w, tri, c0 );
@@ -1090,12 +1134,13 @@ VG_STATIC int rb_capsule__triangle( m4x3f mtxA, rb_capsule *c,
    v3_normalize(d1);
    v3_normalize(da);
 
+   /* the two balls at the ends */
    if( v3_dot( da, d0 ) <= 0.01f )
       rb_capsule_manifold( p0w, c0, 0.0f, c->radius, &manifold );
-
    if( v3_dot( da, d1 ) >= -0.01f )
       rb_capsule_manifold( p1w, c1, 1.0f, c->radius, &manifold );
 
+   /* the edges to edges */
    for( int i=0; i<3; i++ ){
       int i0 = i,
           i1 = (i+1)%3;
@@ -1106,20 +1151,6 @@ VG_STATIC int rb_capsule__triangle( m4x3f mtxA, rb_capsule *c,
       rb_capsule_manifold( ca, cb, ta, c->radius, &manifold );
    }
 
-   v3f v0, v1, n;
-   v3_sub( tri[1], tri[0], v0 );
-   v3_sub( tri[2], tri[0], v1 );
-   v3_cross( v0, v1, n );
-
-   if( v3_length2( n ) <= 0.00001f ){
-#ifdef RIGIDBODY_CRY_ABOUT_EVERYTHING
-      vg_error( "Zero area triangle!\n" );
-#endif
-      return 0;
-   }
-
-   v3_normalize( n );
-
    int count = rb_capsule__manifold_done( mtxA, c, &manifold, buf );
    for( int i=0; i<count; i++ )
       v3_copy( n, buf[i].n );
@@ -1128,7 +1159,7 @@ VG_STATIC int rb_capsule__triangle( m4x3f mtxA, rb_capsule *c,
 }
 
 /* mtxB is defined only for tradition; it is not used currently */
-VG_STATIC int rb_capsule__scene( m4x3f mtxA, rb_capsule *c,
+static int rb_capsule__scene( m4x3f mtxA, rb_capsule *c,
                                  m4x3f mtxB, rb_scene *s, 
                                  rb_ct *buf, u16 ignore ){
    int count = 0;
@@ -1164,14 +1195,14 @@ VG_STATIC int rb_capsule__scene( m4x3f mtxA, rb_capsule *c,
    return count;
 }
 
-VG_STATIC int rb_global_has_space( void ){
+static int rb_global_has_space( void ){
    if( rb_contact_count + 16 > vg_list_size(rb_contact_buffer) )
       return 0;
 
    return 1;
 }
 
-VG_STATIC rb_ct *rb_global_buffer( void ){
+static rb_ct *rb_global_buffer( void ){
    return &rb_contact_buffer[ rb_contact_count ];
 }
 
@@ -1181,15 +1212,15 @@ VG_STATIC rb_ct *rb_global_buffer( void ){
  * -----------------------------------------------------------------------------
  */
 
-VG_STATIC void rb_solver_reset(void){
+static void rb_solver_reset(void){
    rb_contact_count = 0;
 }
 
-VG_STATIC rb_ct *rb_global_ct(void){
+static rb_ct *rb_global_ct(void){
    return rb_contact_buffer + rb_contact_count;
 }
 
-VG_STATIC void rb_prepare_contact( rb_ct *ct, float timestep ){
+static void rb_prepare_contact( rb_ct *ct, float timestep ){
    ct->bias = -0.2f * (timestep*3600.0f) 
                * vg_minf( 0.0f, -ct->p+k_penetration_slop );
    
@@ -1200,7 +1231,7 @@ VG_STATIC void rb_prepare_contact( rb_ct *ct, float timestep ){
 }
 
 /* calculate total move. manifold should belong to ONE object only */
-VG_STATIC void rb_depenetrate( rb_ct *manifold, int len, v3f dt ){
+static void rb_depenetrate( rb_ct *manifold, int len, v3f dt ){
    v3_zero( dt );
 
    for( int j=0; j<7; j++ )
@@ -1221,7 +1252,7 @@ VG_STATIC void rb_depenetrate( rb_ct *manifold, int len, v3f dt ){
 /*
  * Initializing things like tangent vectors
  */
-VG_STATIC void rb_presolve_contacts( rb_ct *buffer, int len ){
+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 );
@@ -1262,7 +1293,7 @@ VG_STATIC void rb_presolve_contacts( rb_ct *buffer, int len ){
 /*
  * Creates relative contact velocity vector
  */
-VG_STATIC void rb_rcv( rigidbody *rba, rigidbody *rbb, v3f ra, v3f rb, v3f rv ){
+static void rb_rcv( rigidbody *rba, rigidbody *rbb, v3f ra, v3f rb, v3f rv ){
    v3f rva, rvb;
    v3_cross( rba->w, ra, rva );
    v3_add(   rba->v, rva, rva );
@@ -1272,7 +1303,7 @@ VG_STATIC void rb_rcv( rigidbody *rba, rigidbody *rbb, v3f ra, v3f rb, v3f rv ){
    v3_sub( rva, rvb, rv );
 }
 
-VG_STATIC void rb_contact_restitution( rb_ct *ct, float cr ){
+static void rb_contact_restitution( rb_ct *ct, float cr ){
    v3f rv, ra, rb;
    v3_sub( ct->co, ct->rba->co, ra );
    v3_sub( ct->co, ct->rbb->co, rb );
@@ -1288,7 +1319,7 @@ VG_STATIC void rb_contact_restitution( rb_ct *ct, float cr ){
 /*
  * Apply impulse to object
  */
-VG_STATIC void rb_linear_impulse( rigidbody *rb, v3f delta, v3f impulse ){
+static void rb_linear_impulse( rigidbody *rb, v3f delta, v3f impulse ){
    /* linear */
    v3_muladds( rb->v, impulse,  rb->inv_mass, rb->v );
    
@@ -1303,7 +1334,7 @@ VG_STATIC void rb_linear_impulse( rigidbody *rb, v3f delta, v3f impulse ){
 /*
  * One iteration to solve the contact constraint
  */
-VG_STATIC void rb_solve_contacts( rb_ct *buf, int len ){
+static void rb_solve_contacts( rb_ct *buf, int len ){
    for( int i=0; i<len; i++ ){
       struct contact *ct = &buf[i];
 
@@ -1354,7 +1385,7 @@ VG_STATIC void rb_solve_contacts( rb_ct *buf, int len ){
  * -----------------------------------------------------------------------------
  */
 
-VG_STATIC void rb_debug_position_constraints( rb_constr_pos *buffer, int len ){
+static void rb_debug_position_constraints( rb_constr_pos *buffer, int len ){
    for( int i=0; i<len; i++ ){
       rb_constr_pos *constr = &buffer[i];
       rigidbody *rba = constr->rba, *rbb = constr->rbb;
@@ -1372,7 +1403,7 @@ VG_STATIC void rb_debug_position_constraints( rb_constr_pos *buffer, int len ){
    }
 }
 
-VG_STATIC void rb_presolve_swingtwist_constraints( rb_constr_swingtwist *buf,
+static void rb_presolve_swingtwist_constraints( rb_constr_swingtwist *buf,
                                                    int len ){
    float size = 0.12f;
 
@@ -1463,7 +1494,7 @@ VG_STATIC void rb_presolve_swingtwist_constraints( rb_constr_swingtwist *buf,
    }
 }
 
-VG_STATIC void rb_debug_swingtwist_constraints( rb_constr_swingtwist *buf, 
+static void rb_debug_swingtwist_constraints( rb_constr_swingtwist *buf, 
                                                 int len ){
    float size = 0.12f;
 
@@ -1559,7 +1590,7 @@ VG_STATIC void rb_debug_swingtwist_constraints( rb_constr_swingtwist *buf,
 /*
  * Solve a list of positional constraints
  */
-VG_STATIC void rb_solve_position_constraints( rb_constr_pos *buf, int len ){
+static void rb_solve_position_constraints( rb_constr_pos *buf, int len ){
    for( int i=0; i<len; i++ ){
       rb_constr_pos *constr = &buf[i];
       rigidbody *rba = constr->rba, *rbb = constr->rbb;
@@ -1623,7 +1654,7 @@ VG_STATIC void rb_solve_position_constraints( rb_constr_pos *buf, int len ){
    }
 }
 
-VG_STATIC void rb_solve_swingtwist_constraints( rb_constr_swingtwist *buf, 
+static void rb_solve_swingtwist_constraints( rb_constr_swingtwist *buf, 
                                                 int len ){
    float size = 0.12f;
 
@@ -1678,7 +1709,7 @@ VG_STATIC void rb_solve_swingtwist_constraints( rb_constr_swingtwist *buf,
    }
 }
 
-VG_STATIC void rb_solve_constr_angle( rigidbody *rba, rigidbody *rbb,
+static void rb_solve_constr_angle( rigidbody *rba, rigidbody *rbb,
                                       v3f ra, v3f rb ){
    m3x3f ssra, ssrb, ssrat, ssrbt;
    m3x3f cma, cmb;
@@ -1720,7 +1751,7 @@ VG_STATIC void rb_solve_constr_angle( rigidbody *rba, rigidbody *rbb,
  * Correct position constraint drift errors
  * [ 0.0 <= amt <= 1.0 ]: the correction amount
  */
-VG_STATIC void rb_correct_position_constraints( rb_constr_pos *buf, int len, 
+static void rb_correct_position_constraints( rb_constr_pos *buf, int len, 
                                                 float amt ){
    for( int i=0; i<len; i++ ){
       rb_constr_pos *constr = &buf[i];
@@ -1738,7 +1769,7 @@ VG_STATIC void rb_correct_position_constraints( rb_constr_pos *buf, int len,
    }
 }
 
-VG_STATIC void rb_correct_swingtwist_constraints( rb_constr_swingtwist *buf, 
+static void rb_correct_swingtwist_constraints( rb_constr_swingtwist *buf, 
                                                   int len, float amt ){
    for( int i=0; i<len; i++ ){
       rb_constr_swingtwist *st = &buf[i];
@@ -1785,16 +1816,17 @@ VG_STATIC void rb_correct_swingtwist_constraints( rb_constr_swingtwist *buf,
    }
 }
 
-VG_STATIC void rb_correct_contact_constraints( rb_ct *buf, int len, float amt ){
+static void rb_correct_contact_constraints( rb_ct *buf, int len, float amt ){
    for( int i=0; i<len; i++ ){
       rb_ct *ct = &buf[i];
       rigidbody *rba = ct->rba,
                 *rbb = ct->rbb;
 
-      float mass_total = 1.0f / (rba->inv_mass + rbb->inv_mass);
+      f32 mass_total = 1.0f / (rba->inv_mass + rbb->inv_mass),
+          d = ct->p*mass_total*amt;
 
-      v3_muladds( rba->co, ct->n, -mass_total * rba->inv_mass, rba->co );
-      v3_muladds( rbb->co, ct->n,  mass_total * rbb->inv_mass, rbb->co );
+      v3_muladds( rba->co, ct->n, -d * rba->inv_mass, rba->co );
+      v3_muladds( rbb->co, ct->n,  d * rbb->inv_mass, rbb->co );
    }
 }
 
@@ -1803,7 +1835,7 @@ VG_STATIC void rb_correct_contact_constraints( rb_ct *buf, int len, float amt ){
  * Effectors
  */
 
-VG_STATIC void rb_effect_simple_bouyency( rigidbody *ra, v4f plane, 
+static void rb_effect_simple_bouyency( rigidbody *ra, v4f plane, 
                                           float amt, float drag ){
    /* float */
    float depth  = v3_dot( plane, ra->co ) - plane[3],
@@ -1818,7 +1850,7 @@ VG_STATIC void rb_effect_simple_bouyency( rigidbody *ra, v4f plane,
 /* apply a spring&dampener force to match ra(worldspace) on rigidbody, to 
  * rt(worldspace)
  */
-VG_STATIC void rb_effect_spring_target_vector( rigidbody *rba, v3f ra, v3f rt,
+static void rb_effect_spring_target_vector( rigidbody *rba, v3f ra, v3f rt,
                                                float spring, float dampening,
                                                float timestep ){
    float d = v3_dot( rt, ra );