+ v3_muladds( rba->co, ct->n, -r, ct->co );
+ ct->p = r;
+ }
+ else
+ {
+ d = sqrtf(d2);
+ v3_muls( delta, 1.0f/d, ct->n );
+ ct->p = r-d;
+ v3_copy( co, ct->co );
+ }
+
+ ct->rba = rba;
+ ct->rbb = rbb;
+ ct->type = k_contact_type_default;
+ return 1;
+ }
+
+ return 0;
+}
+
+VG_STATIC int rb_sphere_sphere( rigidbody *rba, rigidbody *rbb, rb_ct *buf )
+{
+ v3f delta;
+ v3_sub( rba->co, rbb->co, delta );
+
+ float d2 = v3_length2(delta),
+ r = rba->inf.sphere.radius + rbb->inf.sphere.radius;
+
+ if( d2 < r*r )
+ {
+ float d = sqrtf(d2);
+
+ rb_ct *ct = buf;
+ v3_muls( delta, 1.0f/d, ct->n );
+
+ v3f p0, p1;
+ v3_muladds( rba->co, ct->n,-rba->inf.sphere.radius, p0 );
+ v3_muladds( rbb->co, ct->n, rbb->inf.sphere.radius, p1 );
+ v3_add( p0, p1, ct->co );
+ v3_muls( ct->co, 0.5f, ct->co );
+ ct->type = k_contact_type_default;
+ ct->p = r-d;
+ ct->rba = rba;
+ ct->rbb = rbb;
+ return 1;
+ }
+
+ return 0;
+}
+
+//#define RIGIDBODY_DYNAMIC_MESH_EDGES
+
+__attribute__ ((deprecated))
+VG_STATIC int rb_sphere_triangle( rigidbody *rba, rigidbody *rbb,
+ v3f tri[3], rb_ct *buf )
+{
+ v3f delta, co;
+
+#ifdef RIGIDBODY_DYNAMIC_MESH_EDGES
+ closest_on_triangle_1( rba->co, tri, co );
+#else
+ enum contact_type type = closest_on_triangle_1( rba->co, tri, co );
+#endif
+
+ v3_sub( rba->co, co, delta );
+
+ float d2 = v3_length2( delta ),
+ r = rba->inf.sphere.radius;
+
+ if( d2 < r*r )
+ {
+ rb_ct *ct = buf;
+
+ v3f ab, ac, tn;
+ v3_sub( tri[2], tri[0], ab );
+ v3_sub( tri[1], tri[0], ac );
+ v3_cross( ac, ab, tn );
+ v3_copy( tn, ct->n );
+
+ if( v3_length2( ct->n ) <= 0.00001f )
+ {
+ vg_error( "Zero area triangle!\n" );
+ return 0;
+ }
+
+ v3_normalize( ct->n );
+
+ float d = sqrtf(d2);
+
+ v3_copy( co, ct->co );
+ ct->type = type;
+ ct->p = r-d;
+ ct->rba = rba;
+ ct->rbb = rbb;
+ return 1;
+ }
+
+ return 0;
+}
+
+VG_STATIC int rb_sphere__triangle( m4x3f mtxA, rb_sphere *b,
+ v3f tri[3], rb_ct *buf )
+{
+ v3f delta, co;
+ enum contact_type type = closest_on_triangle_1( mtxA[3], tri, co );
+
+ v3_sub( mtxA[3], co, delta );
+
+ float d2 = v3_length2( delta ),
+ r = b->radius;
+
+ if( d2 < r*r )
+ {
+ rb_ct *ct = buf;
+
+ v3f ab, ac, tn;
+ v3_sub( tri[2], tri[0], ab );
+ v3_sub( tri[1], tri[0], ac );
+ v3_cross( ac, ab, tn );
+ v3_copy( tn, ct->n );
+
+ if( v3_length2( ct->n ) <= 0.00001f )
+ {
+ vg_error( "Zero area triangle!\n" );
+ return 0;
+ }
+
+ v3_normalize( ct->n );
+
+ float d = sqrtf(d2);
+
+ v3_copy( co, ct->co );
+ ct->type = type;
+ ct->p = r-d;
+ return 1;
+ }
+
+ return 0;
+}
+
+VG_STATIC void rb_debug_sharp_scene_edges( rigidbody *rbb, float sharp_ang,
+ boxf box, u32 colour )
+{
+ sharp_ang = cosf( sharp_ang );
+
+ scene *sc = rbb->inf.scene.bh_scene->user;
+ vg_line_boxf( box, 0xff00ff00 );
+
+ bh_iter it;
+ bh_iter_init( 0, &it );
+ int idx;
+
+ while( bh_next( rbb->inf.scene.bh_scene, &it, box, &idx ) )
+ {
+ u32 *ptri = &sc->arrindices[ idx*3 ];
+ v3f tri[3];
+
+ for( int j=0; j<3; j++ )
+ v3_copy( sc->arrvertices[ptri[j]].co, tri[j] );
+
+ for( int j=0; j<3; j++ )
+ {
+#if 0
+ v3f edir;
+ v3_sub( tri[(j+1)%3], tri[j], edir );
+
+ if( v3_dot( edir, (v3f){ 0.5184758473652127f,
+ 0.2073903389460850f,
+ -0.8295613557843402f } ) < 0.0f )
+ continue;
+#endif
+
+ bh_iter jt;
+ bh_iter_init( 0, &jt );
+
+ boxf region;
+ float const k_r = 0.02f;
+ v3_add( (v3f){ k_r, k_r, k_r }, tri[j], region[1] );
+ v3_add( (v3f){ -k_r, -k_r, -k_r }, tri[j], region[0] );
+
+ int jdx;
+ while( bh_next( rbb->inf.scene.bh_scene, &jt, region, &jdx ) )
+ {
+ if( idx <= jdx )
+ continue;
+
+ u32 *ptrj = &sc->arrindices[ jdx*3 ];
+ v3f trj[3];
+
+ for( int k=0; k<3; k++ )
+ v3_copy( sc->arrvertices[ptrj[k]].co, trj[k] );
+
+ for( int k=0; k<3; k++ )
+ {
+ if( v3_dist2( tri[j], trj[k] ) <= k_r*k_r )
+ {
+ int jp1 = (j+1)%3,
+ jp2 = (j+2)%3,
+ km1 = (k+3-1)%3,
+ km2 = (k+3-2)%3;
+
+ if( v3_dist2( tri[jp1], trj[km1] ) <= k_r*k_r )
+ {
+ v3f b0, b1, b2;
+ v3_sub( tri[jp1], tri[j], b0 );
+ v3_sub( tri[jp2], tri[j], b1 );
+ v3_sub( trj[km2], tri[j], b2 );
+
+ v3f cx0, cx1;
+ v3_cross( b0, b1, cx0 );
+ v3_cross( b2, b0, cx1 );
+
+ float polarity = v3_dot( cx0, b2 );
+
+ if( polarity < 0.0f )
+ {
+#if 0
+ vg_line( tri[j], tri[jp1], 0xff00ff00 );
+ float ang = v3_dot(cx0,cx1) /
+ (v3_length(cx0)*v3_length(cx1));
+ if( ang < sharp_ang )
+ {
+ vg_line( tri[j], tri[jp1], 0xff00ff00 );
+ }
+#endif
+ }
+ }
+ }
+ }
+ }
+ }
+ }
+}
+
+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;
+
+ int count = 0;
+
+ float r = b->radius;
+ boxf box;
+ v3_sub( mtxA[3], (v3f){ r,r,r }, box[0] );
+ v3_add( mtxA[3], (v3f){ r,r,r }, box[1] );
+
+ while( bh_next( s->bh_scene, &it, box, &idx ) )
+ {
+ u32 *ptri = &sc->arrindices[ idx*3 ];
+ v3f tri[3];
+
+ 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;
+}
+
+__attribute__ ((deprecated))
+VG_STATIC int rb_sphere_scene( rigidbody *rba, rigidbody *rbb, rb_ct *buf )
+{
+ scene *sc = rbb->inf.scene.bh_scene->user;
+
+ bh_iter it;
+ bh_iter_init( 0, &it );
+ int idx;
+
+ int count = 0;
+
+ while( bh_next( rbb->inf.scene.bh_scene, &it, rba->bbx_world, &idx ) )
+ {
+ u32 *ptri = &sc->arrindices[ idx*3 ];
+ v3f tri[3];
+
+ 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( rba, rbb, tri, buf+count );
+ count += contact;
+
+ if( count == 16 )
+ {
+ vg_warn( "Exceeding sphere_vs_scene capacity. Geometry too dense!\n" );
+ return count;
+ }
+ }
+
+ return count;
+}
+
+VG_STATIC int rb_box_scene( rigidbody *rba, rigidbody *rbb, rb_ct *buf )
+{
+ scene *sc = rbb->inf.scene.bh_scene->user;
+
+ v3f tri[3];
+
+ bh_iter it;
+ bh_iter_init( 0, &it );
+ int idx;
+
+ int count = 0;
+
+ while( bh_next( rbb->inf.scene.bh_scene, &it, rba->bbx_world, &idx ) )
+ {
+ u32 *ptri = &sc->arrindices[ idx*3 ];
+
+ for( int j=0; j<3; j++ )
+ v3_copy( sc->arrvertices[ptri[j]].co, tri[j] );
+
+ if( rb_box_triangle_sat( rba, 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 );
+ v3_normalize( n );
+
+ /* find best feature */
+ float best = v3_dot( rba->right, n );
+ int axis = 0;
+
+ float cy = v3_dot( rba->up, n );
+ if( fabsf(cy) > fabsf(best) )
+ {
+ best = cy;
+ axis = 1;
+ }
+
+ float cz = -v3_dot( rba->forward, n );
+ if( fabsf(cz) > fabsf(best) )
+ {
+ best = cz;
+ axis = 2;
+ }
+
+ v3f manifold[4];
+
+ if( axis == 0 )
+ {
+ float px = best > 0.0f? rba->bbx[0][0]: rba->bbx[1][0];
+ manifold[0][0] = px;
+ manifold[0][1] = rba->bbx[0][1];
+ manifold[0][2] = rba->bbx[0][2];
+ manifold[1][0] = px;
+ manifold[1][1] = rba->bbx[1][1];
+ manifold[1][2] = rba->bbx[0][2];
+ manifold[2][0] = px;
+ manifold[2][1] = rba->bbx[1][1];
+ manifold[2][2] = rba->bbx[1][2];
+ manifold[3][0] = px;
+ manifold[3][1] = rba->bbx[0][1];
+ manifold[3][2] = rba->bbx[1][2];
+ }
+ else if( axis == 1 )
+ {
+ float py = best > 0.0f? rba->bbx[0][1]: rba->bbx[1][1];
+ manifold[0][0] = rba->bbx[0][0];
+ manifold[0][1] = py;
+ manifold[0][2] = rba->bbx[0][2];
+ manifold[1][0] = rba->bbx[1][0];
+ manifold[1][1] = py;
+ manifold[1][2] = rba->bbx[0][2];
+ manifold[2][0] = rba->bbx[1][0];
+ manifold[2][1] = py;
+ manifold[2][2] = rba->bbx[1][2];
+ manifold[3][0] = rba->bbx[0][0];
+ manifold[3][1] = py;
+ manifold[3][2] = rba->bbx[1][2];