+ 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