sat

[carveJwlIkooP6JGAAIwe30JlM.git] / rigidbody.h

diff --git a/rigidbody.h b/rigidbody.h
index f2f36cb2ba063f2aa0ee061b2a0686117ebd1cd6..c10f4f8b40edbddaa9356a14e403538f461d1dc8 100644 (file)
--- a/rigidbody.h
+++ b/rigidbody.h
@@ -569,40 +569,35 @@ VG_STATIC int rb_box_triangle_sat( v3f extent, v3f center,
       v3_sub( tri[i], center, tri[i] );
    }
 
+   v3f f0,f1,f2,n;
+   v3_sub( tri[1], tri[0], f0 );
+   v3_sub( tri[2], tri[1], f1 );
+   v3_sub( tri[0], tri[2], f2 );
+
+
+   v3f axis[9];
+   v3_cross( (v3f){1.0f,0.0f,0.0f}, f0, axis[0] );
+   v3_cross( (v3f){1.0f,0.0f,0.0f}, f1, axis[1] );
+   v3_cross( (v3f){1.0f,0.0f,0.0f}, f2, axis[2] );
+   v3_cross( (v3f){0.0f,1.0f,0.0f}, f0, axis[3] );
+   v3_cross( (v3f){0.0f,1.0f,0.0f}, f1, axis[4] );
+   v3_cross( (v3f){0.0f,1.0f,0.0f}, f2, axis[5] );
+   v3_cross( (v3f){0.0f,0.0f,1.0f}, f0, axis[6] );
+   v3_cross( (v3f){0.0f,0.0f,1.0f}, f1, axis[7] );
+   v3_cross( (v3f){0.0f,0.0f,1.0f}, f2, axis[8] );
+   
+   for( int i=0; i<9; i++ )
+      if(!rb_box_triangle_interval( extent, axis[i], tri )) return 0;
+
    /* u0, u1, u2 */
    if(!rb_box_triangle_interval( extent, (v3f){1.0f,0.0f,0.0f}, tri )) return 0;
    if(!rb_box_triangle_interval( extent, (v3f){0.0f,1.0f,0.0f}, tri )) return 0;
    if(!rb_box_triangle_interval( extent, (v3f){0.0f,0.0f,1.0f}, tri )) return 0;
 
-   v3f v0,v1,v2,n, e0,e1,e2;
-   v3_sub( tri[1], tri[0], v0 );
-   v3_sub( tri[2], tri[0], v1 );
-   v3_sub( tri[2], tri[1], v2 );
-   v3_normalize( v0 );
-   v3_normalize( v1 );
-   v3_normalize( v2 );
-   v3_cross( v0, v1, n );
-   v3_cross( v0, n, e0 );
-   v3_cross( n, v1, e1 );
-   v3_cross( v2, n, e2 );
-
    /* normal */
+   v3_cross( f0, f1, n );
    if(!rb_box_triangle_interval( extent, n, tri )) return 0;
 
-   v3f axis[9];
-   v3_cross( e0, (v3f){1.0f,0.0f,0.0f}, axis[0] );
-   v3_cross( e0, (v3f){0.0f,1.0f,0.0f}, axis[1] );
-   v3_cross( e0, (v3f){0.0f,0.0f,1.0f}, axis[2] );
-   v3_cross( e1, (v3f){1.0f,0.0f,0.0f}, axis[3] );
-   v3_cross( e1, (v3f){0.0f,1.0f,0.0f}, axis[4] );
-   v3_cross( e1, (v3f){0.0f,0.0f,1.0f}, axis[5] );
-   v3_cross( e2, (v3f){1.0f,0.0f,0.0f}, axis[6] );
-   v3_cross( e2, (v3f){0.0f,1.0f,0.0f}, axis[7] );
-   v3_cross( e2, (v3f){0.0f,0.0f,1.0f}, axis[8] );
-   
-   for( int i=0; i<9; i++ )
-      if(!rb_box_triangle_interval( extent, axis[i], tri )) return 0;
-
    return 1;
 }
 
@@ -1311,6 +1306,7 @@ VG_STATIC int rb_sphere__scene( m4x3f mtxA, rb_sphere *b,
 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;
    v3f tri[3];
 
@@ -1452,6 +1448,50 @@ VG_STATIC int rb_box__scene( m4x3f mtxA, boxf bbx,
       }
    }
    return count;
+#else
+
+   scene *sc = s->bh_scene->user;
+   v3f tri[3];
+
+   v3f extent, center;
+   v3_sub( bbx[1], bbx[0], extent );
+   v3_muls( extent, 0.5f, extent );
+   v3_add( bbx[0], extent, center );
+
+   float r = v3_length(extent);
+   boxf world_bbx;
+   v3_fill( world_bbx[0], -r );
+   v3_fill( world_bbx[1],  r );
+   for( int i=0; i<2; i++ ){
+      v3_add( center, world_bbx[i], world_bbx[i] );
+      v3_add( mtxA[3], world_bbx[i], world_bbx[i] );
+   }
+
+   m4x3f to_local;
+   m4x3_invert_affine( mtxA, to_local );
+
+   bh_iter it;
+   bh_iter_init( 0, &it );
+   int idx;
+   int count = 0;
+
+   vg_line_boxf( world_bbx, VG__RED );
+   
+   while( bh_next( s->bh_scene, &it, world_bbx, &idx ) ){
+      u32 *ptri = &sc->arrindices[ idx*3 ];
+
+      for( int j=0; j<3; j++ )
+         v3_copy( sc->arrvertices[ptri[j]].co, tri[j] );
+
+      vg_line( tri[0],tri[1],VG__BLACK );
+      vg_line( tri[1],tri[2],VG__BLACK );
+      vg_line( tri[2],tri[0],VG__BLACK );
+
+      v3f clip[2][8];
+      u32 clip_length = 0;
+   }
+
+#endif
 }
 
 VG_STATIC int rb_capsule__triangle( m4x3f mtxA, rb_capsule *c,
@@ -1659,6 +1699,20 @@ 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 )
+{
+   v3f rv, ra, rb;
+   v3_sub( ct->co, ct->rba->co, ra );
+   v3_sub( ct->co, ct->rbb->co, rb );
+   rb_rcv( ct->rba, ct->rbb, ra, rb, rv );
+
+   float v = v3_dot( rv, ct->n );
+
+   if( v < -1.0f ){
+      ct->bias += -cr * v;
+   }
+}
+
 /*
  * Apply impulse to object
  */