+/* TODO: these guys */
+
+static int rb_capsule_vs_scene( rigidbody *rba, rigidbody *rbb, rb_ct *buf )
+{
+ u32 geo[128];
+ v3f tri[3];
+ int len = bh_select( &rbb->inf.scene.pscene->bhtris,
+ rba->bbx_world, geo, 128 );
+
+ return 0;
+}
+
+static int rb_sphere_vs_triangle( rigidbody *rba, rigidbody *rbb,
+ v3f tri[3], rb_ct *buf )
+{
+ v3f delta, co;
+
+ closest_on_triangle( rba->co, tri, co );
+ v3_sub( rba->co, co, delta );
+
+ vg_line( rba->co, co, 0xffff0000 );
+ vg_line_pt3( rba->co, 0.1f, 0xff00ffff );
+
+ 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 );
+ v3_normalize( ct->n );
+
+ float d = sqrtf(d2);
+
+ v3_copy( co, ct->co );
+ ct->p = r-d;
+ ct->rba = rba;
+ ct->rbb = rbb;
+ return 1;
+ }
+
+ return 0;
+}
+
+static int rb_sphere_vs_scene( rigidbody *rba, rigidbody *rbb, rb_ct *buf )
+{
+ scene *sc = rbb->inf.scene.pscene;
+
+ u32 geo[128];
+ v3f tri[3];
+ int len = bh_select( &sc->bhtris, rba->bbx_world, geo, 128 );
+
+ int count = 0;
+
+ for( int i=0; i<len; i++ )
+ {
+ u32 *ptri = &sc->indices[ geo[i]*3 ];
+
+ for( int j=0; j<3; j++ )
+ v3_copy( sc->verts[ptri[j]].co, tri[j] );
+
+ vg_line(tri[0],tri[1],0xff00ff00 );
+ vg_line(tri[1],tri[2],0xff00ff00 );
+ vg_line(tri[2],tri[0],0xff00ff00 );
+
+ buf[count].element_id = ptri[0];
+ count += rb_sphere_vs_triangle( rba, rbb, tri, buf+count );
+
+ if( count == 12 )
+ {
+ vg_warn( "Exceeding sphere_vs_scene capacity. Geometry too dense!\n" );
+ return count;
+ }
+ }
+
+ return count;
+}
+
+static float rb_box_plane_interval( rigidbody *rba, v4f p )
+{
+ /* TODO: Make boxes COG aligned as is every other shape.
+ * or create COG vector.
+ * TODO: Make forward actually point in the right fucking direction. */
+ v3f e,c;
+ v3_sub( rba->bbx[1], rba->bbx[0], e );
+ v3_muls( e, 0.5f, e );
+ v3_add( rba->bbx[0], e, c );
+ m4x3_mulv( rba->to_world, c, c );
+
+ float r =
+ e[0]*fabsf( v3_dot(p, rba->right)) +
+ e[1]*fabsf( v3_dot(p, rba->up)) +
+ e[2]*fabsf(-v3_dot(p, rba->forward)),
+ s = v3_dot( p, c ) - p[3];
+
+ return r-s;
+}
+
+static int rb_box_vs_scene( rigidbody *rba, rigidbody *rbb, rb_ct *buf )
+{
+#if 1
+ scene *sc = rbb->inf.scene.pscene;
+
+ u32 geo[128];
+ v3f tri[3];
+ int len = bh_select( &sc->bhtris, rba->bbx_world, geo, 128 );
+
+ vg_info( "%d\n", len );
+
+ int count = 0;
+
+ for( int i=0; i<len; i++ )
+ {
+ u32 *ptri = &sc->indices[ geo[i]*3 ];
+
+ for( int j=0; j<3; j++ )
+ v3_copy( sc->verts[ptri[j]].co, tri[j] );
+
+ vg_line(tri[0],tri[1],0xff00ff00 );
+ vg_line(tri[1],tri[2],0xff00ff00 );
+ vg_line(tri[2],tri[0],0xff00ff00 );
+
+ //count += rb_sphere_vs_triangle( rba, rbb, tri, buf+count );
+ //
+
+ /* TODO: SAT test first */
+
+ /*
+ * each pair of faces on the box vs triangle normal
+ */
+
+ v3f v0, v1;
+ v4f tn;
+
+ v3_sub( tri[1],tri[0], v0 );
+ v3_sub( tri[2],tri[0], v1 );
+ v3_cross( v0, v1, tn );
+ v3_normalize( tn );
+
+ tn[3] = v3_dot( tn, tri[0] );
+
+ v4f pa, pb, pc, pd, pe, pf;
+ v3_copy( rba->right, pa );
+ v3_muls( rba->right, -1.0f, pb );
+ v3_copy( rba->up, pc );
+ v3_muls( rba->up, -1.0f, pd );
+ v3_copy( rba->forward, pf );
+ v3_muls( rba->forward, -1.0f, pe );
+
+ float dx = v3_dot( rba->co, rba->right ),
+ dy = v3_dot( rba->co, rba->up ),
+ dz = -v3_dot( rba->co, rba->forward );
+
+ pa[3] = dx + rba->bbx[1][0];
+ pb[3] = -dx - rba->bbx[0][0];
+ pc[3] = dy + rba->bbx[1][1];
+ pd[3] = -dy - rba->bbx[0][1];
+ pe[3] = dz + rba->bbx[1][2];
+ pf[3] = -dz - rba->bbx[0][2];
+
+ float *pairs[][2] = { {pc, pa}, {pc,pe}, {pc,pb}, {pc,pf},
+ {pd, pa}, {pd,pe}, {pd,pb}, {pd,pf},
+ {pf, pa}, {pa,pe}, {pe,pb}, {pb,pf}};
+ for( int j=0; j<vg_list_size(pairs); j++ )
+ {
+ v3f p;
+
+ if( rb_intersect_planes_1( pairs[j][0], pairs[j][1], tn, p ))
+ {
+ v3f p_tri, p_box;
+ closest_point_obb( p, rba, p_box );
+ closest_on_triangle_1( p, tri, p_tri );
+
+ //vg_line_pt3( p, 0.1f, 0xffeeaaff );
+
+ if( v3_dist( p_tri, p ) < 0.001f && v3_dist( p_box, p ) < 0.001f )
+ {
+ if( count == 12 )
+ {
+ vg_warn( "Exceeding box_vs_scene capacity."
+ "Geometry too dense!\n" );
+ return count;
+ }
+
+ rb_ct *ct = buf+count;
+
+ v3_copy( tn, ct->n );
+ v3_copy( p_box, ct->co );
+
+ ct->p = rb_box_plane_interval( rba, tn );
+ ct->rba = rba;
+ ct->rbb = rbb;
+ count ++;
+ }
+ }
+ }
+ }
+#else
+
+ v3f pts[8];
+ float *p000 = pts[0], *p001 = pts[1], *p010 = pts[2], *p011 = pts[3],
+ *p100 = pts[4], *p101 = pts[5], *p110 = pts[6], *p111 = pts[7];
+
+ p000[0]=rba->bbx[0][0];p000[1]=rba->bbx[0][1];p000[2]=rba->bbx[0][2];
+ p001[0]=rba->bbx[0][0];p001[1]=rba->bbx[0][1];p001[2]=rba->bbx[1][2];
+ p010[0]=rba->bbx[0][0];p010[1]=rba->bbx[1][1];p010[2]=rba->bbx[0][2];
+ p011[0]=rba->bbx[0][0];p011[1]=rba->bbx[1][1];p011[2]=rba->bbx[1][2];
+ p100[0]=rba->bbx[1][0];p100[1]=rba->bbx[0][1];p100[2]=rba->bbx[0][2];
+ p101[0]=rba->bbx[1][0];p101[1]=rba->bbx[0][1];p101[2]=rba->bbx[1][2];
+ p110[0]=rba->bbx[1][0];p110[1]=rba->bbx[1][1];p110[2]=rba->bbx[0][2];
+ p111[0]=rba->bbx[1][0];p111[1]=rba->bbx[1][1];p111[2]=rba->bbx[1][2];
+
+ int count = 0;
+ for( int i=0; i<8; i++ )
+ {
+ m4x3_mulv( rba->to_world, pts[i], pts[i] );
+
+ vg_line_pt3( pts[i], 0.1f, 0xffff00ff );
+
+ if( pts[i][1] < 0.0f )
+ {
+ rb_ct *ct = buf+count;
+
+ v3_copy( (v3f){0.0f,1.0f,0.0f}, ct->n );
+ v3_copy( pts[i], ct->co );
+
+ ct->p = 0.0f-pts[i][1];
+ ct->rba = rba;
+ ct->rbb = rbb;
+ count ++;
+ }
+ }
+
+#endif
+
+ return count;
+}
+
+static int RB_MATRIX_ERROR( rigidbody *rba, rigidbody *rbb, rb_ct *buf )
+{
+ vg_error( "Collision type is unimplemented between types %d and %d\n",
+ rba->type, rbb->type );
+
+ return 0;
+}
+
+static int rb_sphere_vs_capsule( rigidbody *rba, rigidbody *rbb, rb_ct *buf )
+{
+ return rb_capsule_vs_sphere( rbb, rba, buf );
+}
+
+static int rb_box_vs_capsule( rigidbody *rba, rigidbody *rbb, rb_ct *buf )
+{
+ return rb_capsule_vs_box( rbb, rba, buf );
+}
+