+#if 0
+ for( int i=0; i<4; i++ )
+ {
+ v3f q0, q1;
+ int i0 = i,
+ i1 = (i+1)%4;
+
+ v3_add( quad[i0], centroid, q0 );
+ v3_add( quad[i1], centroid, q1 );
+
+ m4x3_mulv( rbb->to_world, q0, q0 );
+ m4x3_mulv( rbb->to_world, q1, q1 );
+
+ vg_line( q0, q1, 0xffffffff );
+ }
+#endif
+
+ return rb_capsule_manifold_done( rba, rbb, &manifold, buf );
+}
+#endif
+
+VG_STATIC int rb_sphere_box( rigidbody *rba, rigidbody *rbb, rb_ct *buf )
+{
+ v3f co, delta;
+
+ closest_point_obb( rba->co, rbb->bbx, rbb->to_world, rbb->to_local, co );
+ v3_sub( rba->co, co, delta );
+
+ float d2 = v3_length2(delta),
+ r = rba->inf.sphere.radius;
+
+ if( d2 <= r*r )
+ {
+ float d;
+
+ rb_ct *ct = buf;
+ if( d2 <= 0.0001f )
+ {
+ v3_sub( rba->co, rbb->co, delta );
+
+ /*
+ * some extra testing is required to find the best axis to push the
+ * object back outside the box. Since there isnt a clear seperating
+ * vector already, especially on really high aspect boxes.
+ */
+ float lx = v3_dot( rbb->right, delta ),
+ ly = v3_dot( rbb->up, delta ),
+ lz = v3_dot( rbb->forward, delta ),
+ px = rbb->bbx[1][0] - fabsf(lx),
+ py = rbb->bbx[1][1] - fabsf(ly),
+ pz = rbb->bbx[1][2] - fabsf(lz);
+
+ if( px < py && px < pz )
+ v3_muls( rbb->right, vg_signf(lx), ct->n );
+ else if( py < pz )
+ v3_muls( rbb->up, vg_signf(ly), ct->n );
+ else
+ v3_muls( rbb->forward, vg_signf(lz), ct->n );
+
+ 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
+
+#if 0
+__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;
+}
+#endif
+
+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 + 0.1f;
+ 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;
+}
+
+#if 0
+__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;
+}
+#endif
+
+VG_STATIC int rb_box__scene( m4x3f mtxA, boxf bbx,
+ m4x3f mtxB, rb_scene *s, rb_ct *buf )
+{
+ 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] );
+
+ 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 );
+ v3_normalize( n );
+
+ /* find best feature */
+ float best = v3_dot( mtxA[0], n );
+ int axis = 0;
+
+ for( int i=1; i<3; i++ ){
+ float c = v3_dot( mtxA[i], n );
+
+ if( fabsf(c) > fabsf(best) ){
+ best = c;
+ axis = i;
+ }
+ }
+
+ v3f manifold[4];
+
+ if( axis == 0 ){
+ float px = best > 0.0f? bbx[0][0]: bbx[1][0];
+ manifold[0][0] = px;
+ manifold[0][1] = bbx[0][1];
+ manifold[0][2] = bbx[0][2];
+ manifold[1][0] = px;
+ manifold[1][1] = bbx[1][1];
+ manifold[1][2] = bbx[0][2];
+ manifold[2][0] = px;
+ manifold[2][1] = bbx[1][1];
+ manifold[2][2] = bbx[1][2];
+ manifold[3][0] = px;
+ manifold[3][1] = bbx[0][1];
+ manifold[3][2] = bbx[1][2];
+ }
+ else if( axis == 1 ){
+ float py = best > 0.0f? bbx[0][1]: bbx[1][1];
+ manifold[0][0] = bbx[0][0];
+ manifold[0][1] = py;
+ manifold[0][2] = bbx[0][2];
+ manifold[1][0] = bbx[1][0];
+ manifold[1][1] = py;
+ manifold[1][2] = bbx[0][2];
+ manifold[2][0] = bbx[1][0];
+ manifold[2][1] = py;
+ manifold[2][2] = bbx[1][2];
+ manifold[3][0] = bbx[0][0];
+ manifold[3][1] = py;
+ manifold[3][2] = bbx[1][2];
+ }
+ else{
+ float pz = best > 0.0f? bbx[0][2]: bbx[1][2];
+ manifold[0][0] = bbx[0][0];
+ manifold[0][1] = bbx[0][1];
+ manifold[0][2] = pz;
+ manifold[1][0] = bbx[1][0];
+ manifold[1][1] = bbx[0][1];
+ manifold[1][2] = pz;
+ manifold[2][0] = bbx[1][0];
+ manifold[2][1] = bbx[1][1];
+ manifold[2][2] = pz;
+ manifold[3][0] = bbx[0][0];
+ manifold[3][1] = bbx[1][1];
+ manifold[3][2] = pz;
+ }
+
+ for( int j=0; j<4; j++ )
+ m4x3_mulv( mtxA, manifold[j], manifold[j] );
+
+ vg_line( manifold[0], manifold[1], 0xffffffff );
+ vg_line( manifold[1], manifold[2], 0xffffffff );
+ vg_line( manifold[2], manifold[3], 0xffffffff );
+ vg_line( manifold[3], manifold[0], 0xffffffff );
+
+ for( int j=0; j<4; j++ ){
+ rb_ct *ct = buf+count;
+
+ v3_copy( manifold[j], ct->co );
+ v3_copy( n, ct->n );
+
+ float l0 = v3_dot( tri[0], n ),
+ l1 = v3_dot( manifold[j], n );
+
+ ct->p = (l0-l1)*0.5f;
+ if( ct->p < 0.0f )
+ continue;
+
+ ct->type = k_contact_type_default;
+ count ++;
+
+ if( count >= 12 )
+ return count;
+ }
+ }
+ return count;
+}
+
+#if 0
+__attribute__ ((deprecated))
+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];
+ }
+ else
+ {
+ float pz = best > 0.0f? rba->bbx[0][2]: rba->bbx[1][2];
+ manifold[0][0] = rba->bbx[0][0];
+ manifold[0][1] = rba->bbx[0][1];
+ manifold[0][2] = pz;
+ manifold[1][0] = rba->bbx[1][0];
+ manifold[1][1] = rba->bbx[0][1];
+ manifold[1][2] = pz;
+ manifold[2][0] = rba->bbx[1][0];
+ manifold[2][1] = rba->bbx[1][1];
+ manifold[2][2] = pz;
+ manifold[3][0] = rba->bbx[0][0];
+ manifold[3][1] = rba->bbx[1][1];
+ manifold[3][2] = pz;
+ }
+
+ for( int j=0; j<4; j++ )
+ m4x3_mulv( rba->to_world, manifold[j], manifold[j] );
+
+ vg_line( manifold[0], manifold[1], 0xffffffff );
+ vg_line( manifold[1], manifold[2], 0xffffffff );
+ vg_line( manifold[2], manifold[3], 0xffffffff );
+ vg_line( manifold[3], manifold[0], 0xffffffff );
+
+ for( int j=0; j<4; j++ )
+ {
+ rb_ct *ct = buf+count;
+
+ v3_copy( manifold[j], ct->co );
+ v3_copy( n, ct->n );
+
+ float l0 = v3_dot( tri[0], n ),
+ l1 = v3_dot( manifold[j], n );
+
+ ct->p = (l0-l1)*0.5f;
+ if( ct->p < 0.0f )
+ continue;
+
+ ct->type = k_contact_type_default;
+ ct->rba = rba;
+ ct->rbb = rbb;
+ count ++;
+
+ if( count >= 12 )
+ return count;
+ }
+ }
+ return count;
+}
+#endif
+
+VG_STATIC int rb_capsule__triangle( m4x3f mtxA, rb_capsule *c,
+ v3f tri[3], rb_ct *buf )
+{
+ v3f pc, p0w, p1w;
+ v3_muladds( mtxA[3], mtxA[1], -c->height*0.5f+c->radius, p0w );
+ v3_muladds( mtxA[3], mtxA[1], c->height*0.5f-c->radius, p1w );
+
+ capsule_manifold manifold;
+ rb_capsule_manifold_init( &manifold );
+
+ v3f c0, c1;
+ closest_on_triangle_1( p0w, tri, c0 );
+ closest_on_triangle_1( p1w, tri, c1 );
+
+ v3f d0, d1, da;
+ v3_sub( c0, p0w, d0 );
+ v3_sub( c1, p1w, d1 );
+ v3_sub( p1w, p0w, da );
+
+ v3_normalize(d0);
+ v3_normalize(d1);
+ v3_normalize(da);
+
+ if( v3_dot( da, d0 ) <= 0.01f )
+ rb_capsule_manifold( p0w, c0, 0.0f, c->radius, &manifold );
+
+ if( v3_dot( da, d1 ) >= -0.01f )
+ rb_capsule_manifold( p1w, c1, 1.0f, c->radius, &manifold );
+
+ for( int i=0; i<3; i++ )
+ {
+ int i0 = i,
+ i1 = (i+1)%3;
+
+ v3f ca, cb;
+ float ta, tb;
+ closest_segment_segment( p0w, p1w, tri[i0], tri[i1], &ta, &tb, ca, cb );
+ rb_capsule_manifold( ca, cb, ta, c->radius, &manifold );
+ }
+
+ 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 );
+
+ int count = rb_capsule__manifold_done( mtxA, c, &manifold, buf );
+ for( int i=0; i<count; i++ )
+ v3_copy( n, buf[i].n );
+
+ return count;
+}
+
+/*
+ * Generates up to two contacts; optimised for the most stable manifold
+ */
+#if 0
+__attribute__ ((deprecated))
+VG_STATIC int rb_capsule_triangle( rigidbody *rba, rigidbody *rbb,
+ v3f tri[3], rb_ct *buf )
+{
+ float h = rba->inf.capsule.height,
+ r = rba->inf.capsule.radius;
+
+ v3f pc, p0w, p1w;
+ v3_muladds( rba->co, rba->up, -h*0.5f+r, p0w );
+ v3_muladds( rba->co, rba->up, h*0.5f-r, p1w );
+
+ capsule_manifold manifold;
+ rb_capsule_manifold_init( &manifold );
+
+ v3f c0, c1;
+ closest_on_triangle_1( p0w, tri, c0 );
+ closest_on_triangle_1( p1w, tri, c1 );
+
+ v3f d0, d1, da;
+ v3_sub( c0, p0w, d0 );
+ v3_sub( c1, p1w, d1 );
+ v3_sub( p1w, p0w, da );
+
+ v3_normalize(d0);
+ v3_normalize(d1);
+ v3_normalize(da);
+
+ if( v3_dot( da, d0 ) <= 0.01f )
+ rb_capsule_manifold( p0w, c0, 0.0f, r, &manifold );
+
+ if( v3_dot( da, d1 ) >= -0.01f )
+ rb_capsule_manifold( p1w, c1, 1.0f, r, &manifold );
+
+ for( int i=0; i<3; i++ )
+ {
+ int i0 = i,
+ i1 = (i+1)%3;
+
+ v3f ca, cb;
+ float ta, tb;
+ closest_segment_segment( p0w, p1w, tri[i0], tri[i1], &ta, &tb, ca, cb );
+ rb_capsule_manifold( ca, cb, ta, r, &manifold );
+ }
+
+ 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 );
+
+ int count = rb_capsule_manifold_done( rba, rbb, &manifold, buf );
+ for( int i=0; i<count; i++ )
+ v3_copy( n, buf[i].n );
+
+ return count;
+}
+#endif
+
+/* mtxB is defined only for tradition; it is not used currently */
+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;
+
+ while( bh_next( s->bh_scene, &it, bbx, &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];
+
+ int contact = rb_capsule__triangle( mtxA, c, tri, &buf[count] );
+ count += contact;
+
+ if( count >= 16 )
+ {
+ vg_warn("Exceeding capsule_vs_scene capacity. Geometry too dense!\n");
+ return count;
+ }
+ }
+
+ return count;
+}
+
+#if 0
+__attribute__ ((deprecated))
+VG_STATIC int rb_capsule_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];
+
+#if 0
+ vg_line( tri[0],tri[1],0x70ff6000 );
+ vg_line( tri[1],tri[2],0x70ff6000 );
+ vg_line( tri[2],tri[0],0x70ff6000 );
+#endif
+
+ int contact = rb_capsule_triangle( rba, rbb, tri, buf+count );
+ count += contact;
+
+ if( count == 16 )
+ {
+ vg_warn("Exceeding capsule_vs_scene capacity. Geometry too dense!\n");
+ return count;
+ }
+ }
+
+ return count;
+}
+
+VG_STATIC int rb_scene_capsule( rigidbody *rba, rigidbody *rbb, rb_ct *buf )
+{
+ return rb_capsule_scene( rbb, rba, buf );
+}
+#endif
+
+VG_STATIC int RB_MATRIX_ERROR( rigidbody *rba, rigidbody *rbb, rb_ct *buf )
+{
+#if 0
+ vg_error( "Collision type is unimplemented between types %d and %d\n",
+ rba->type, rbb->type );
+#endif
+
+ return 0;
+}
+
+VG_STATIC int rb_sphere_capsule( rigidbody *rba, rigidbody *rbb, rb_ct *buf )
+{
+ return rb_capsule_sphere( rbb, rba, buf );
+}
+
+#if 0
+VG_STATIC int rb_box_capsule( rigidbody *rba, rigidbody *rbb, rb_ct *buf )
+{
+ return rb_capsule_box( rbb, rba, buf );
+}
+#endif
+
+VG_STATIC int rb_box_sphere( rigidbody *rba, rigidbody *rbb, rb_ct *buf )
+{
+ return rb_sphere_box( rbb, rba, buf );
+}
+
+#if 0
+VG_STATIC int rb_scene_box( rigidbody *rba, rigidbody *rbb, rb_ct *buf )
+{
+ return rb_box_scene( rbb, rba, buf );
+}
+#endif