X-Git-Url: https://harrygodden.com/git/?p=vg.git;a=blobdiff_plain;f=vg_rigidbody_constraints.h;fp=vg_rigidbody_constraints.h;h=92ec26d9e30e1388b9e664ae6b69c6e2e275dfb2;hp=410b14fe84a6c0d083c848c86f6073b5408ffa1d;hb=3b14f3dcd5bf9dd3c85144f2123d667bfa4bb63f;hpb=fce86711735b15bff37de0f70716808410fcf269

diff --git a/vg_rigidbody_constraints.h b/vg_rigidbody_constraints.h
index 410b14f..92ec26d 100644
--- a/vg_rigidbody_constraints.h
+++ b/vg_rigidbody_constraints.h
@@ -1,15 +1,16 @@
 #pragma once
-#include "vg_rigidbody.h"
 
 typedef struct rb_constr_pos rb_constr_pos;
 typedef struct rb_constr_swingtwist rb_constr_swingtwist;
 
-struct rb_constr_pos{
+struct rb_constr_pos
+{
    rigidbody *rba, *rbb;
    v3f lca, lcb;
 };
 
-struct rb_constr_swingtwist{
+struct rb_constr_swingtwist
+{
    rigidbody *rba, *rbb;
 
    v4f conevx, conevy; /* relative to rba */
@@ -25,498 +26,22 @@ struct rb_constr_swingtwist{
    f32 conv_tangent, conv_axis;
 };
 
-/*
- * -----------------------------------------------------------------------------
- *                               Constraints
- * -----------------------------------------------------------------------------
- */
-
-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;
-
-      v3f wca, wcb;
-      m3x3_mulv( rba->to_world, constr->lca, wca );
-      m3x3_mulv( rbb->to_world, constr->lcb, wcb );
-
-      v3f p0, p1;
-      v3_add( wca, rba->co, p0 );
-      v3_add( wcb, rbb->co, p1 );
-      vg_line_point( p0, 0.0025f, 0xff000000 );
-      vg_line_point( p1, 0.0025f, 0xffffffff );
-      vg_line2( p0, p1, 0xff000000, 0xffffffff );
-   }
-}
-
-static void rb_presolve_swingtwist_constraints( rb_constr_swingtwist *buf,
-                                                   int len ){
-   for( int i=0; i<len; i++ ){
-      rb_constr_swingtwist *st = &buf[ i ];
-      
-      v3f vx, vy, va, vxb, axis, center;
-
-      m3x3_mulv( st->rba->to_world, st->conevx, vx );
-      m3x3_mulv( st->rbb->to_world, st->conevxb, vxb );
-      m3x3_mulv( st->rba->to_world, st->conevy, vy );
-      m3x3_mulv( st->rbb->to_world, st->coneva, va );
-      m4x3_mulv( st->rba->to_world, st->view_offset, center );
-      v3_cross( vy, vx, axis );
-
-      /* Constraint violated ? */
-      float fx = v3_dot( vx, va ),     /* projection world */
-            fy = v3_dot( vy, va ),
-            fn = v3_dot( va, axis ),
-
-            rx = st->conevx[3],        /* elipse radii */
-            ry = st->conevy[3],
-
-            lx = fx/rx,                /* projection local (fn==lz) */
-            ly = fy/ry;
-
-      st->tangent_violation = ((lx*lx + ly*ly) > fn*fn) || (fn <= 0.0f);
-      if( st->tangent_violation ){
-         /* Calculate a good position and the axis to solve on */
-         v2f closest, tangent, 
-             p = { fx/fabsf(fn), fy/fabsf(fn) };
-
-         closest_point_elipse( p, (v2f){rx,ry}, closest );
-         tangent[0] = -closest[1] / (ry*ry);
-         tangent[1] =  closest[0] / (rx*rx);
-         v2_normalize( tangent );
-
-         v3f v0, v1;
-         v3_muladds( axis, vx, closest[0], v0 );
-         v3_muladds( v0, vy, closest[1], v0 );
-         v3_normalize( v0 );
-
-         v3_muls( vx, tangent[0], v1 );
-         v3_muladds( v1, vy, tangent[1], v1 );
-
-         v3_copy( v0, st->tangent_target );
-         v3_copy( v1, st->tangent_axis );
-
-         /* calculate mass */
-         v3f aIw, bIw;
-         m3x3_mulv( st->rba->iIw, st->tangent_axis, aIw );
-         m3x3_mulv( st->rbb->iIw, st->tangent_axis, bIw );
-         st->tangent_mass = 1.0f / (v3_dot( st->tangent_axis, aIw ) +
-                                    v3_dot( st->tangent_axis, bIw ));
-
-         float angle = v3_dot( va, st->tangent_target );
-      }
-
-      v3f refaxis;
-      v3_cross( vy, va, refaxis );  /* our default rotation */
-      v3_normalize( refaxis );
-
-      float angle = v3_dot( refaxis, vxb );
-      st->axis_violation = fabsf(angle) < st->conet;
-
-      if( st->axis_violation ){
-         v3f dir_test;
-         v3_cross( refaxis, vxb, dir_test );
-
-         if( v3_dot(dir_test, va) < 0.0f )
-            st->axis_violation = -st->axis_violation;
-
-         float newang = (float)st->axis_violation * acosf(st->conet-0.0001f);
-
-         v3f refaxis_up;
-         v3_cross( va, refaxis, refaxis_up );
-         v3_muls( refaxis_up, sinf(newang), st->axis_target );
-         v3_muladds( st->axis_target, refaxis, -cosf(newang), st->axis_target );
-
-         /* calculate mass */
-         v3_copy( va, st->axis );
-         v3f aIw, bIw;
-         m3x3_mulv( st->rba->iIw, st->axis, aIw );
-         m3x3_mulv( st->rbb->iIw, st->axis, bIw );
-         st->axis_mass = 1.0f / (v3_dot( st->axis, aIw ) +
-                                 v3_dot( st->axis, bIw ));
-      }
-   }
-}
-
-static void rb_debug_swingtwist_constraints( rb_constr_swingtwist *buf, 
-                                                int len ){
-   float size = 0.12f;
-
-   for( int i=0; i<len; i++ ){
-      rb_constr_swingtwist *st = &buf[ i ];
-      
-      v3f vx, vxb, vy, va, axis, center;
-
-      m3x3_mulv( st->rba->to_world, st->conevx, vx );
-      m3x3_mulv( st->rbb->to_world, st->conevxb, vxb );
-      m3x3_mulv( st->rba->to_world, st->conevy, vy );
-      m3x3_mulv( st->rbb->to_world, st->coneva, va );
-      m4x3_mulv( st->rba->to_world, st->view_offset, center );
-      v3_cross( vy, vx, axis );
-
-      float rx = st->conevx[3],        /* elipse radii */
-            ry = st->conevy[3];
-
-      v3f p0, p1;
-      v3_muladds( center, va, size, p1 );
-      vg_line( center, p1, 0xffffffff );
-      vg_line_point( p1, 0.00025f, 0xffffffff );
-
-      if( st->tangent_violation ){
-         v3_muladds( center, st->tangent_target, size, p0 );
-
-         vg_line( center, p0, 0xff00ff00 );
-         vg_line_point( p0, 0.00025f, 0xff00ff00 );
-         vg_line( p1, p0, 0xff000000 );
-      }
-      
-      for( int x=0; x<32; x++ ){
-         float t0 = ((float)x * (1.0f/32.0f)) * VG_TAUf,
-               t1 = (((float)x+1.0f) * (1.0f/32.0f)) * VG_TAUf,
-               c0 = cosf( t0 ),
-               s0 = sinf( t0 ),
-               c1 = cosf( t1 ),
-               s1 = sinf( t1 );
-
-         v3f v0, v1;
-         v3_muladds( axis, vx, c0*rx, v0 );
-         v3_muladds( v0,   vy, s0*ry, v0 );
-         v3_muladds( axis, vx, c1*rx, v1 );
-         v3_muladds( v1,   vy, s1*ry, v1 );
-
-         v3_normalize( v0 );
-         v3_normalize( v1 );
-
-         v3_muladds( center, v0, size, p0 );
-         v3_muladds( center, v1, size, p1 );
-
-         u32 col0r = fabsf(c0) * 255.0f,
-             col0g = fabsf(s0) * 255.0f,
-             col1r = fabsf(c1) * 255.0f,
-             col1g = fabsf(s1) * 255.0f,
-             col   = st->tangent_violation? 0xff0000ff: 0xff000000,
-             col0  = col | (col0r<<16) | (col0g << 8),
-             col1  = col | (col1r<<16) | (col1g << 8);
-
-         vg_line2( center, p0, VG__NONE, col0 );
-         vg_line2( p0, p1, col0, col1 );
-      }
-
-      /* Draw twist */
-      v3_muladds( center, va, size, p0 );
-      v3_muladds( p0, vxb, size, p1 );
-
-      vg_line( p0, p1, 0xff0000ff );
-
-      if( st->axis_violation ){
-         v3_muladds( p0, st->axis_target, size*1.25f, p1 );
-         vg_line( p0, p1, 0xffffff00 );
-         vg_line_point( p1, 0.0025f, 0xffffff80 );
-      }
-
-      v3f refaxis;
-      v3_cross( vy, va, refaxis );  /* our default rotation */
-      v3_normalize( refaxis );
-      v3f refaxis_up;
-      v3_cross( va, refaxis, refaxis_up );
-      float newang = acosf(st->conet-0.0001f);
-
-      v3_muladds( p0, refaxis_up, sinf(newang)*size, p1 );
-      v3_muladds( p1, refaxis, -cosf(newang)*size, p1 );
-      vg_line( p0, p1, 0xff000000 );
-
-      v3_muladds( p0, refaxis_up, sinf(-newang)*size, p1 );
-      v3_muladds( p1, refaxis, -cosf(-newang)*size, p1 );
-      vg_line( p0, p1, 0xff404040 );
-   }
-}
-
+void rb_debug_position_constraints( rb_constr_pos *buffer, int len );
+void rb_presolve_swingtwist_constraints( rb_constr_swingtwist *buf, int len );
+void rb_debug_swingtwist_constraints( rb_constr_swingtwist *buf, int len );
+   
 /*
  * Solve a list of positional constraints
  */
-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;
-
-      v3f wa, wb;
-      m3x3_mulv( rba->to_world, constr->lca, wa );
-      m3x3_mulv( rbb->to_world, constr->lcb, wb );
-
-      m3x3f ssra, ssrat, ssrb, ssrbt;
-      
-      m3x3_skew_symetric( ssrat, wa );
-      m3x3_skew_symetric( ssrbt, wb );
-      m3x3_transpose( ssrat, ssra );
-      m3x3_transpose( ssrbt, ssrb );
-
-      v3f b, b_wa, b_wb, b_a, b_b;
-      m3x3_mulv( ssra, rba->w, b_wa );
-      m3x3_mulv( ssrb, rbb->w, b_wb );
-      v3_add( rba->v, b_wa, b );
-      v3_sub( b, rbb->v, b );
-      v3_sub( b, b_wb, b );
-      v3_muls( b, -1.0f, b );
-
-      m3x3f invMa, invMb;
-      m3x3_diagonal( invMa, rba->inv_mass );
-      m3x3_diagonal( invMb, rbb->inv_mass );
-
-      m3x3f ia, ib;
-      m3x3_mul( ssra, rba->iIw, ia );
-      m3x3_mul( ia, ssrat, ia );
-      m3x3_mul( ssrb, rbb->iIw, ib );
-      m3x3_mul( ib, ssrbt, ib );
-
-      m3x3f cma, cmb;
-      m3x3_add( invMa, ia, cma );
-      m3x3_add( invMb, ib, cmb );
-
-      m3x3f A;
-      m3x3_add( cma, cmb, A );
-
-      /* Solve Ax = b ( A^-1*b = x ) */
-      v3f impulse;
-      m3x3f invA;
-      m3x3_inv( A, invA );
-      m3x3_mulv( invA, b, impulse );
-
-      v3f delta_va, delta_wa, delta_vb, delta_wb;
-      m3x3f iwa, iwb;
-      m3x3_mul( rba->iIw, ssrat, iwa );
-      m3x3_mul( rbb->iIw, ssrbt, iwb );
-
-      m3x3_mulv( invMa, impulse, delta_va );
-      m3x3_mulv( invMb, impulse, delta_vb );
-      m3x3_mulv( iwa, impulse, delta_wa );
-      m3x3_mulv( iwb, impulse, delta_wb );
-
-      v3_add( rba->v, delta_va, rba->v );
-      v3_add( rba->w, delta_wa, rba->w );
-      v3_sub( rbb->v, delta_vb, rbb->v );
-      v3_sub( rbb->w, delta_wb, rbb->w );
-   }
-}
-
-static void rb_solve_swingtwist_constraints( rb_constr_swingtwist *buf, 
-                                                int len ){
-   for( int i=0; i<len; i++ ){
-      rb_constr_swingtwist *st = &buf[ i ];
-
-      if( !st->axis_violation )
-         continue;
-
-      float rv = v3_dot( st->axis, st->rbb->w ) - 
-                 v3_dot( st->axis, st->rba->w );
-
-      if( rv * (float)st->axis_violation > 0.0f )
-         continue;
-
-      v3f impulse, wa, wb;
-      v3_muls( st->axis, rv*st->axis_mass, impulse );
-      m3x3_mulv( st->rba->iIw, impulse, wa );
-      v3_add( st->rba->w, wa, st->rba->w );
-
-      v3_muls( impulse, -1.0f, impulse );
-      m3x3_mulv( st->rbb->iIw, impulse, wb );
-      v3_add( st->rbb->w, wb, st->rbb->w );
-
-      float rv2 = v3_dot( st->axis, st->rbb->w ) - 
-                  v3_dot( st->axis, st->rba->w );
-   }
-
-   for( int i=0; i<len; i++ ){
-      rb_constr_swingtwist *st = &buf[ i ];
-
-      if( !st->tangent_violation )
-         continue;
-
-      float rv = v3_dot( st->tangent_axis, st->rbb->w ) - 
-                 v3_dot( st->tangent_axis, st->rba->w );
-
-      if( rv > 0.0f )
-         continue;
-
-      v3f impulse, wa, wb;
-      v3_muls( st->tangent_axis, rv*st->tangent_mass, impulse );
-      m3x3_mulv( st->rba->iIw, impulse, wa );
-      v3_add( st->rba->w, wa, st->rba->w );
-
-      v3_muls( impulse, -1.0f, impulse );
-      m3x3_mulv( st->rbb->iIw, impulse, wb );
-      v3_add( st->rbb->w, wb, st->rbb->w );
-
-      float rv2 = v3_dot( st->tangent_axis, st->rbb->w ) - 
-                  v3_dot( st->tangent_axis, st->rba->w );
-   }
-}
-
-/* debugging */
-static void rb_postsolve_swingtwist_constraints( rb_constr_swingtwist *buf, 
-                                                 u32 len ){
-   for( int i=0; i<len; i++ ){
-      rb_constr_swingtwist *st = &buf[ i ];
-
-      if( !st->axis_violation ){
-         st->conv_axis = 0.0f;
-         continue;
-      }
-
-      f32 rv = v3_dot( st->axis, st->rbb->w ) - 
-               v3_dot( st->axis, st->rba->w );
-
-      if( rv * (f32)st->axis_violation > 0.0f )
-         st->conv_axis = 0.0f;
-      else
-         st->conv_axis = rv;
-   }
-
-   for( int i=0; i<len; i++ ){
-      rb_constr_swingtwist *st = &buf[ i ];
-
-      if( !st->tangent_violation ){
-         st->conv_tangent = 0.0f;
-         continue;
-      }
-
-      f32 rv = v3_dot( st->tangent_axis, st->rbb->w ) - 
-               v3_dot( st->tangent_axis, st->rba->w );
-
-      if( rv > 0.0f )
-         st->conv_tangent = 0.0f;
-      else
-         st->conv_tangent = rv;
-   }
-}
-
-static void rb_solve_constr_angle( rigidbody *rba, rigidbody *rbb,
-                                      v3f ra, v3f rb ){
-   m3x3f ssra, ssrb, ssrat, ssrbt;
-   m3x3f cma, cmb;
-
-   m3x3_skew_symetric( ssrat, ra );
-   m3x3_skew_symetric( ssrbt, rb );
-   m3x3_transpose( ssrat, ssra );
-   m3x3_transpose( ssrbt, ssrb );
-
-   m3x3_mul( ssra, rba->iIw, cma );
-   m3x3_mul( cma, ssrat, cma );
-   m3x3_mul( ssrb, rbb->iIw, cmb );
-   m3x3_mul( cmb, ssrbt, cmb );
-
-   m3x3f A, invA;
-   m3x3_add( cma, cmb, A );
-   m3x3_inv( A, invA );
-
-   v3f b_wa, b_wb, b;
-   m3x3_mulv( ssra, rba->w, b_wa );
-   m3x3_mulv( ssrb, rbb->w, b_wb );
-   v3_add( b_wa, b_wb, b );
-   v3_negate( b, b );
-
-   v3f impulse;
-   m3x3_mulv( invA, b, impulse );
-
-   v3f delta_wa, delta_wb;
-   m3x3f iwa, iwb;
-   m3x3_mul( rba->iIw, ssrat, iwa );
-   m3x3_mul( rbb->iIw, ssrbt, iwb );
-   m3x3_mulv( iwa, impulse, delta_wa );
-   m3x3_mulv( iwb, impulse, delta_wb );
-   v3_add( rba->w, delta_wa, rba->w );
-   v3_sub( rbb->w, delta_wb, rbb->w );
-}
+void rb_solve_position_constraints( rb_constr_pos *buf, int len );
+void rb_solve_swingtwist_constraints( rb_constr_swingtwist *buf, int len );
+void rb_postsolve_swingtwist_constraints( rb_constr_swingtwist *buf, u32 len );
+void rb_solve_constr_angle( rigidbody *rba, rigidbody *rbb, v3f ra, v3f rb );
 
 /*
  * Correct position constraint drift errors
  * [ 0.0 <= amt <= 1.0 ]: the correction amount
  */
-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];
-      rigidbody *rba = constr->rba, *rbb = constr->rbb;
-
-      v3f p0, p1, d;
-      m3x3_mulv( rba->to_world, constr->lca, p0 );
-      m3x3_mulv( rbb->to_world, constr->lcb, p1 );
-      v3_add( rba->co, p0, p0 );
-      v3_add( rbb->co, p1, p1 );
-      v3_sub( p1, p0, d );
-
-#if 1
-      v3_muladds( rbb->co, d, -1.0f * amt, rbb->co );
-      rb_update_matrices( rbb );
-#else
-      f32 mt = 1.0f/(rba->inv_mass+rbb->inv_mass),
-          a  = mt * (k_phys_baumgarte/k_rb_delta);
-
-      v3_muladds( rba->v, d, a* rba->inv_mass, rba->v );
-      v3_muladds( rbb->v, d, a*-rbb->inv_mass, rbb->v );
-#endif
-   }
-}
-
-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];
-
-      if( !st->tangent_violation )
-         continue;
-
-      v3f va;
-      m3x3_mulv( st->rbb->to_world, st->coneva, va );
-
-      f32 angle = v3_dot( va, st->tangent_target );
-
-      if( fabsf(angle) < 0.9999f ){
-         v3f axis;
-         v3_cross( va, st->tangent_target, axis );
-#if 1
-         angle = acosf(angle) * amt;
-         v4f correction;
-         q_axis_angle( correction, axis, angle );
-         q_mul( correction, st->rbb->q, st->rbb->q );
-         q_normalize( st->rbb->q );
-         rb_update_matrices( st->rbb );
-#else
-         f32 mt = 1.0f/(st->rba->inv_mass+st->rbb->inv_mass),
-             wa = mt * acosf(angle) * (k_phys_baumgarte/k_rb_delta);
-         //v3_muladds( st->rba->w, axis, wa*-st->rba->inv_mass, st->rba->w );
-         v3_muladds( st->rbb->w, axis, wa* st->rbb->inv_mass, st->rbb->w );
-#endif
-      }
-   }
-
-   for( int i=0; i<len; i++ ){
-      rb_constr_swingtwist *st = &buf[i];
-
-      if( !st->axis_violation )
-         continue;
-
-      v3f vxb;
-      m3x3_mulv( st->rbb->to_world, st->conevxb, vxb );
-
-      f32 angle = v3_dot( vxb, st->axis_target );
-
-      if( fabsf(angle) < 0.9999f ){
-         v3f axis;
-         v3_cross( vxb, st->axis_target, axis );
-
-#if 1
-         angle = acosf(angle) * amt;
-         v4f correction;
-         q_axis_angle( correction, axis, angle );
-         q_mul( correction, st->rbb->q, st->rbb->q );
-         q_normalize( st->rbb->q );
-         rb_update_matrices( st->rbb );
-#else
-         f32 mt = 1.0f/(st->rba->inv_mass+st->rbb->inv_mass),
-             wa = mt * acosf(angle) * (k_phys_baumgarte/k_rb_delta);
-         //v3_muladds( st->rba->w, axis, wa*-0.5f, st->rba->w );
-         v3_muladds( st->rbb->w, axis, wa* st->rbb->inv_mass, st->rbb->w );
-#endif
-      }
-   }
-}
+void rb_correct_position_constraints( rb_constr_pos *buf, int len, f32 amt );
+void rb_correct_swingtwist_constraints( rb_constr_swingtwist *buf, 
+                                        int len, float amt );