fix long standing grind bug

[carveJwlIkooP6JGAAIwe30JlM.git] / rigidbody.h

diff --git a/rigidbody.h b/rigidbody.h
index c10f4f8b40edbddaa9356a14e403538f461d1dc8..b1dc834a76ab710d046ec99f729de9da1703d955 100644 (file)
--- a/rigidbody.h
+++ b/rigidbody.h
@@ -44,25 +44,10 @@ VG_STATIC float
 
 VG_STATIC void rb_register_cvar(void)
 {
-   vg_var_push( (struct vg_var){
-      .name = "k_limit_bias", .data = &k_limit_bias,
-      .data_type = k_var_dtype_f32, .opt_f32 = {.clamp = 0}, .persistent = 1
-   });
-
-   vg_var_push( (struct vg_var){
-      .name = "k_joint_bias", .data = &k_joint_bias,
-      .data_type = k_var_dtype_f32, .opt_f32 = {.clamp = 0}, .persistent = 1
-   });
-
-   vg_var_push( (struct vg_var){
-      .name = "k_joint_correction", .data = &k_joint_correction,
-      .data_type = k_var_dtype_f32, .opt_f32 = {.clamp = 0}, .persistent = 1
-   });
-
-   vg_var_push( (struct vg_var){
-      .name = "k_joint_impulse", .data = &k_joint_impulse,
-      .data_type = k_var_dtype_f32, .opt_f32 = {.clamp = 0}, .persistent = 1
-   });
+   VG_VAR_F32( k_limit_bias, flags=VG_VAR_CHEAT );
+   VG_VAR_F32( k_joint_bias, flags=VG_VAR_CHEAT );
+   VG_VAR_F32( k_joint_correction, flags=VG_VAR_CHEAT );
+   VG_VAR_F32( k_joint_impulse, flags=VG_VAR_CHEAT );
 }
 
 /* 
@@ -78,23 +63,19 @@ typedef struct rb_sphere rb_sphere;
 typedef struct rb_capsule rb_capsule;
 typedef struct rb_scene rb_scene;
 
-struct rb_sphere
-{
+struct rb_sphere{
    float radius;
 };
 
-struct rb_capsule
-{
+struct rb_capsule{
    float height, radius;
 };
 
-struct rb_scene
-{
+struct rb_scene{
    bh_tree *bh_scene;
 };
 
-struct rigidbody
-{
+struct rigidbody{
    v3f co, v, w;
    v4f q;
 
@@ -108,8 +89,7 @@ struct rigidbody
 };
 
 /* simple objects */
-struct rb_object
-{
+struct rb_object{
    rigidbody rb;
    enum rb_shape{
       k_rb_shape_box     = 0,
@@ -119,8 +99,7 @@ struct rb_object
    } 
    type;
    
-   union
-   {
+   union{
       struct rb_sphere sphere;
       struct rb_capsule capsule;
       struct rb_scene scene;
@@ -128,8 +107,7 @@ struct rb_object
    inf;
 };
 
-VG_STATIC struct contact
-{
+VG_STATIC struct contact{
    rigidbody *rba, *rbb;
    v3f co, n;
    v3f t[2];
@@ -146,14 +124,12 @@ VG_STATIC int rb_contact_count = 0;
 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 */
@@ -167,8 +143,7 @@ struct rb_constr_swingtwist
    float tangent_mass, axis_mass;
 };
 
-struct rb_constr_spring
-{
+struct rb_constr_spring{
    int nothing;
 };
 
@@ -187,14 +162,12 @@ VG_STATIC float sphere_volume( float radius )
 VG_STATIC void rb_tangent_basis( v3f n, v3f tx, v3f ty )
 {
    /* Compute tangent basis (box2d) */
-   if( fabsf( n[0] ) >= 0.57735027f )
-   {
+   if( fabsf( n[0] ) >= 0.57735027f ){
       tx[0] =  n[1];
       tx[1] = -n[0];
       tx[2] =  0.0f;
    }
-   else
-   {
+   else{
       tx[0] =  0.0f;
       tx[1] =  n[2];
       tx[2] = -n[1];
@@ -215,13 +188,11 @@ VG_STATIC void rb_debug_contact( rb_ct *ct )
    v3f p1;
    v3_muladds( ct->co, ct->n, 0.05f, p1 );
 
-   if( ct->type == k_contact_type_default )
-   {
+   if( ct->type == k_contact_type_default ){
       vg_line_pt3( ct->co, 0.0125f, 0xff0000ff );
       vg_line( ct->co, p1, 0xffffffff );
    }
-   else if( ct->type == k_contact_type_edge )
-   {
+   else if( ct->type == k_contact_type_edge ){
       vg_line_pt3( ct->co, 0.0125f, 0xff00ffc0 );
       vg_line( ct->co, p1, 0xffffffff );
    }
@@ -233,8 +204,7 @@ VG_STATIC void debug_sphere( m4x3f m, float radius, u32 colour )
        lx = { 0.0f, radius, 0.0f },
        lz = { 0.0f, 0.0f, radius };
    
-   for( int i=0; i<16; i++ )
-   {
+   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);
@@ -292,8 +262,7 @@ VG_STATIC void debug_capsule( m4x3f m, float radius, float h, u32 colour )
    vg_line( a0, a1, colour );
    vg_line( b0, b1, colour );
    
-   for( int i=0; i<16; i++ )
-   {
+   for( int i=0; i<16; i++ ){
       float t = ((float)(i+1) * (1.0f/16.0f)) * VG_PIf * 2.0f,
             s1 = sinf(t)*radius,
             c1 = cosf(t)*radius;
@@ -320,15 +289,13 @@ VG_STATIC void debug_capsule( m4x3f m, float radius, float h, u32 colour )
       vg_line( a0, a1, colour );
       vg_line( b0, b1, colour );
 
-      if( c0 < 0.0f )
-      {
+      if( c0 < 0.0f ){
          v3_add( p0, e2, a0 );
          v3_add( p0, e3, a1 );
          v3_add( p0, e4, b0 );
          v3_add( p0, e5, b1 );
       }
-      else
-      {
+      else{
          v3_add( p1, e2, a0 );
          v3_add( p1, e3, a1 );
          v3_add( p1, e4, b0 );
@@ -408,8 +375,7 @@ VG_STATIC void rb_update_transform( rigidbody *rb )
  */
 VG_STATIC void rb_extrapolate( rigidbody *rb, v3f co, v4f q )
 {
-   float substep = vg_clampf( vg.accumulator / k_rb_delta, 0.0f, 1.0f );
-
+   float substep = vg.time_fixed_extrapolate;
    v3_muladds( rb->co, rb->v, k_rb_delta*substep, co );
 
    if( v3_length2( rb->w ) > 0.0f ){
@@ -500,7 +466,7 @@ VG_STATIC void rb_iter( rigidbody *rb )
        !vg_validf( rb->v[1] ) ||
        !vg_validf( rb->v[2] ) )
    {
-      vg_fatal_exit_loop( "NaN velocity" );
+      vg_fatal_error( "NaN velocity" );
    }
 
    v3f gravity = { 0.0f, -9.8f, 0.0f };
@@ -1265,11 +1231,7 @@ VG_STATIC int rb_sphere__triangle( m4x3f mtxA, rb_sphere *b,
 VG_STATIC int rb_sphere__scene( m4x3f mtxA, rb_sphere *b,
                                 m4x3f mtxB, rb_scene *s, rb_ct *buf )
 {
-   scene *sc = s->bh_scene->user;
-
-   bh_iter it;
-   bh_iter_init( 0, &it );
-   int idx;
+   scene_context *sc = s->bh_scene->user;
 
    int count = 0;
 
@@ -1277,8 +1239,12 @@ VG_STATIC int rb_sphere__scene( m4x3f mtxA, rb_sphere *b,
    boxf box;
    v3_sub( mtxA[3], (v3f){ r,r,r }, box[0] );
    v3_add( mtxA[3], (v3f){ r,r,r }, box[1] );
+
+   bh_iter it;
+   i32 idx;
+   bh_iter_init_box( 0, &it, box );
    
-   while( bh_next( s->bh_scene, &it, box, &idx ) ){
+   while( bh_next( s->bh_scene, &it, &idx ) ){
       u32 *ptri = &sc->arrindices[ idx*3 ];
       v3f tri[3];
 
@@ -1307,7 +1273,7 @@ VG_STATIC int rb_box__scene( m4x3f mtxA, boxf bbx,
                              m4x3f mtxB, rb_scene *s, rb_ct *buf )
 {
 #if 1
-   scene *sc = s->bh_scene->user;
+   scene_context *sc = s->bh_scene->user;
    v3f tri[3];
 
    v3f extent, center;
@@ -1328,13 +1294,13 @@ VG_STATIC int rb_box__scene( m4x3f mtxA, boxf bbx,
    m4x3_invert_affine( mtxA, to_local );
 
    bh_iter it;
-   bh_iter_init( 0, &it );
+   bh_iter_init_box( 0, &it, world_bbx );
    int idx;
    int count = 0;
 
    vg_line_boxf( world_bbx, VG__RED );
    
-   while( bh_next( s->bh_scene, &it, world_bbx, &idx ) ){
+   while( bh_next( s->bh_scene, &it, &idx ) ){
       u32 *ptri = &sc->arrindices[ idx*3 ];
 
       for( int j=0; j<3; j++ )
@@ -1551,18 +1517,18 @@ VG_STATIC int rb_capsule__scene( m4x3f mtxA, rb_capsule *c,
                                  m4x3f mtxB, rb_scene *s, 
                                  rb_ct *buf )
 {
-   bh_iter it;
-   bh_iter_init( 0, &it );
-   int idx;
    int count = 0;
 
    boxf bbx;
    v3_sub( mtxA[3], (v3f){ c->height, c->height, c->height }, bbx[0] );
    v3_add( mtxA[3], (v3f){ c->height, c->height, c->height }, bbx[1] );
    
-   scene *sc = s->bh_scene->user;
+   scene_context *sc = s->bh_scene->user;
    
-   while( bh_next( s->bh_scene, &it, bbx, &idx ) ){
+   bh_iter it;
+   bh_iter_init_box( 0, &it, bbx );
+   i32 idx;
+   while( bh_next( s->bh_scene, &it, &idx ) ){
       u32 *ptri = &sc->arrindices[ idx*3 ];
       v3f tri[3];
 
@@ -2263,7 +2229,7 @@ VG_STATIC void rb_effect_spring_target_vector( rigidbody *rba, v3f ra, v3f rt,
                                                float timestep )
 {
    float d = v3_dot( rt, ra );
-   float a = vg_signf( d ) * acosf( vg_clampf( d, -1.0f, 1.0f ) );
+   float a = acosf( vg_clampf( d, -1.0f, 1.0f ) );
 
    v3f axis;
    v3_cross( rt, ra, axis );