+static void rb_build_manifold_rb_static( rigidbody *ra, rigidbody *rb_static )
+{
+ v3f verts[8];
+
+ v3f a, b;
+ v3_copy( ra->bbx[0], a );
+ v3_copy( ra->bbx[1], b );
+
+ m4x3_mulv( ra->to_world, (v3f){ a[0], a[1], a[2] }, verts[0] );
+ m4x3_mulv( ra->to_world, (v3f){ a[0], b[1], a[2] }, verts[1] );
+ m4x3_mulv( ra->to_world, (v3f){ b[0], b[1], a[2] }, verts[2] );
+ m4x3_mulv( ra->to_world, (v3f){ b[0], a[1], a[2] }, verts[3] );
+ m4x3_mulv( ra->to_world, (v3f){ a[0], a[1], b[2] }, verts[4] );
+ m4x3_mulv( ra->to_world, (v3f){ a[0], b[1], b[2] }, verts[5] );
+ m4x3_mulv( ra->to_world, (v3f){ b[0], b[1], b[2] }, verts[6] );
+ m4x3_mulv( ra->to_world, (v3f){ b[0], a[1], b[2] }, verts[7] );
+
+ int count = 0;
+
+ for( int i=0; i<8; i++ )
+ {
+ if( ra->manifold_count == vg_list_size(ra->manifold) )
+ return;
+
+ struct contact *ct = &ra->manifold[ ra->manifold_count ];
+
+ float p;
+ v3f normal;
+
+ if( rb_point_in_body( rb_static, verts[i], &p, normal ))
+ {
+ v3_copy( normal, ct->n );
+ v3_muladds( verts[i], ct->n, p*0.5f, ct->co );
+ v3_sub( ct->co, ra->co, ct->delta );
+
+ vg_line_pt3( ct->co, 0.0125f, 0xffff00ff );
+
+ ct->bias = -0.2f * (1.0f/k_rb_delta) * vg_minf( 0.0f, -p+0.04f );
+ rb_tangent_basis( ct->n, ct->t[0], ct->t[1] );
+
+ ct->norm_impulse = 0.0f;
+ ct->tangent_impulse[0] = 0.0f;
+ ct->tangent_impulse[1] = 0.0f;
+
+ ra->manifold_count ++;
+ count ++;
+ if( count == 4 )
+ return;
+ }
+ }
+}
+
+/*
+ * BVH implementation, this is ONLY for static rigidbodies, its to slow for
+ * realtime use.
+ */
+
+static void rb_bh_expand_bound( void *user, boxf bound, u32 item_index )
+{
+ rigidbody *rb = &((rigidbody *)user)[ item_index ];
+ box_concat( bound, rb->bbx_world );
+}
+
+static float rb_bh_centroid( void *user, u32 item_index, int axis )
+{
+ rigidbody *rb = &((rigidbody *)user)[ item_index ];
+ return (rb->bbx_world[axis][0] + rb->bbx_world[1][axis]) * 0.5f;
+}
+
+static void rb_bh_swap( void *user, u32 ia, u32 ib )
+{
+ rigidbody temp, *rba, *rbb;
+ rba = &((rigidbody *)user)[ ia ];
+ rbb = &((rigidbody *)user)[ ib ];
+
+ temp = *rba;
+ *rba = *rbb;
+ *rbb = temp;
+}
+
+static void rb_bh_debug( void *user, u32 item_index )
+{
+ rigidbody *rb = &((rigidbody *)user)[ item_index ];
+ rb_debug( rb, 0xff00ffff );
+}
+
+static bh_system bh_system_rigidbodies =
+{
+ .expand_bound = rb_bh_expand_bound,
+ .item_centroid = rb_bh_centroid,
+ .item_swap = rb_bh_swap,
+ .item_debug = rb_bh_debug,
+ .cast_ray = NULL
+};
+