+ 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 );
+}
+
+static int rb_sphere_vs_box( rigidbody *rba, rigidbody *rbb, rb_ct *buf )
+{
+ v3f co, delta;
+
+ closest_point_obb( rba->co, rbb, 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;
+ return 1;
+ }
+
+ return 0;
+}
+
+static int rb_sphere_vs_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->p = r-d;
+ ct->rba = rba;
+ ct->rbb = rbb;
+ return 1;
+ }
+
+ return 0;
+}
+
+/* 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 );
+}
+
+static int rb_box_vs_sphere( rigidbody *rba, rigidbody *rbb, rb_ct *buf )
+{
+ return rb_sphere_vs_box( rbb, rba, buf );
+}
+
+static int rb_scene_vs_box( rigidbody *rba, rigidbody *rbb, rb_ct *buf )
+{
+ return rb_box_vs_scene( rbb, rba, buf );
+}
+
+static int (*rb_jump_table[4][4])( rigidbody *rba, rigidbody *rbb, rb_ct *buf )
+= {
+ /* box */ /* Sphere */ /* Capsule */ /* Mesh */
+/*box */ { RB_MATRIX_ERROR, rb_box_vs_sphere, rb_box_vs_capsule, rb_box_vs_scene },
+/*sphere */ { rb_sphere_vs_box, rb_sphere_vs_sphere, rb_sphere_vs_capsule, rb_sphere_vs_scene },
+/*capsule*/ { rb_capsule_vs_box,rb_capsule_vs_sphere,rb_capsule_vs_capsule,RB_MATRIX_ERROR },
+/*mesh */ { rb_scene_vs_box, RB_MATRIX_ERROR, RB_MATRIX_ERROR, RB_MATRIX_ERROR }
+};
+
+static int rb_collide( rigidbody *rba, rigidbody *rbb )
+{
+ int (*collider_jump)(rigidbody *rba, rigidbody *rbb, rb_ct *buf )
+ = rb_jump_table[rba->type][rbb->type];
+
+ /*
+ * 12 is the maximum manifold size we can generate, so we are forced to abort
+ * potentially checking any more.
+ */
+ if( rb_contact_count + 12 > vg_list_size(rb_contact_buffer) )
+ {
+ vg_warn( "Too many contacts made in global collider buffer (%d of %d\n)",
+ rb_contact_count, vg_list_size(rb_contact_buffer) );
+ return 0;
+ }
+