+ v3_copy( co, ct->co );
+ ct->type = type;
+ ct->p = r-d;
+ return 1;
+ }
+
+ return 0;
+}
+
+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;
+
+ int count = 0;
+
+ float r = b->radius + 0.1f;
+ 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, &idx ) ){
+ u32 *ptri = &sc->arrindices[ idx*3 ];
+ v3f tri[3];
+
+ if( sc->arrvertices[ptri[0]].flags & ignore ) continue;
+
+ for( int j=0; j<3; j++ )
+ v3_copy( sc->arrvertices[ptri[j]].co, tri[j] );
+
+ buf[ count ].element_id = ptri[0];
+
+ vg_line( tri[0],tri[1],0x70ff6000 );
+ vg_line( tri[1],tri[2],0x70ff6000 );
+ vg_line( tri[2],tri[0],0x70ff6000 );
+
+ int contact = rb_sphere__triangle( mtxA, b, tri, &buf[count] );
+ count += contact;
+
+ if( count == 16 ){
+ vg_warn( "Exceeding sphere_vs_scene capacity. Geometry too dense!\n" );
+ return count;
+ }
+ }
+
+ return count;
+}
+
+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];
+
+ 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_box( 0, &it, world_bbx );
+ int idx;
+ int count = 0;
+
+ vg_line_boxf( world_bbx, VG__RED );
+
+ while( bh_next( s->bh_scene, &it, &idx ) ){
+ u32 *ptri = &sc->arrindices[ idx*3 ];
+ if( sc->arrvertices[ptri[0]].flags & ignore ) continue;
+
+ for( int j=0; j<3; j++ )
+ v3_copy( sc->arrvertices[ptri[j]].co, tri[j] );
+
+ if( rb_box_triangle_sat( extent, center, to_local, tri ) ){
+ vg_line(tri[0],tri[1],0xff50ff00 );
+ vg_line(tri[1],tri[2],0xff50ff00 );
+ vg_line(tri[2],tri[0],0xff50ff00 );
+ }
+ else{
+ vg_line(tri[0],tri[1],0xff0000ff );
+ vg_line(tri[1],tri[2],0xff0000ff );
+ vg_line(tri[2],tri[0],0xff0000ff );
+ continue;
+ }
+
+ 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 );