sat

[carveJwlIkooP6JGAAIwe30JlM.git] / rigidbody.h

diff --git a/rigidbody.h b/rigidbody.h
index 60fc88ce59a18275f59c6b36aba67ed88f9fbb89..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,