- 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;
- }
-
- /*
- * TODO: Replace this with a more dedicated broad phase pass
- */
- if( box_overlap( rba->bbx_world, rbb->bbx_world ) )
- {
- int count = collider_jump( rba, rbb, rb_contact_buffer+rb_contact_count);
- rb_contact_count += count;
- return count;
- }
- else
- return 0;
-}
-
-/*
- * Generic functions
- */
-
-#ifdef RB_DEPR
-/*
- * This function does not accept triangle as a dynamic object, it is assumed
- * to always be static.
- *
- * The triangle is also assumed to be one sided for better detection
- */
-static int rb_sphere_vs_triangle( rigidbody *rba, v3f tri[3], rb_ct *buf )
-{
- v3f delta, co;
-
- closest_on_triangle( rba->co, tri, co );
- v3_sub( rba->co, co, delta );
-
- float d2 = v3_length2( delta ),
- r = rba->inf.sphere.radius;
-
- if( d2 < r*r )
- {
- v3f ab, ac, tn;
- v3_sub( tri[1], tri[0], ab );
- v3_sub( tri[2], tri[0], ac );
- v3_cross( ac, ab, tn );
-
- if( v3_dot( delta, tn ) > 0.0f )
- v3_muls( delta, -1.0f, delta );
-
- float d = sqrtf(d2);
-
- rb_ct *ct = buf;
- v3_muls( delta, 1.0f/d, ct->n );
- v3_copy( co, ct->co );
- ct->p = r-d;
- ct->rba = rba;
- ct->rbb = &rb_static_null;
- return 1;
- }
-
- return 0;
-}
-
-static int sphere_vs_triangle( v3f c, float r, v3f tri[3],
- v3f co, v3f norm, float *p )
-{
- v3f delta;
- closest_on_triangle( c, tri, co );
-
- v3_sub( c, co, delta );
-
-
- float d = v3_length2( delta );
- if( d < r*r )
- {
- v3f ab, ac, tn;
- v3_sub( tri[1], tri[0], ab );
- v3_sub( tri[2], tri[0], ac );
- v3_cross( ac, ab, tn );
-
- if( v3_dot( delta, tn ) > 0.0f )
- v3_muls( delta, -1.0f, delta );
-
- vg_line_pt3( co, 0.05f, 0xff00ff00 );
-
- d = sqrtf(d);
- v3_muls( delta, 1.0f/d, norm );
-
- *p = r-d;
- return 1;
- }
-
- return 0;
-}
-
-#include "world.h"
-#endif
-
-static void rb_solver_reset(void)
-{
- rb_contact_count = 0;
-}
-
-static rb_ct *rb_global_ct(void)
-{
- return rb_contact_buffer + rb_contact_count;
-}
-
-#ifdef RB_DEPR
-static struct contact *rb_start_contact(void)
-{
- if( rb_contact_count == vg_list_size(rb_contact_buffer) )
- {
- vg_error( "rigidbody: too many contacts generated (%u)\n",
- rb_contact_count );
- return NULL;
- }
-
- return &rb_contact_buffer[ rb_contact_count ];
-}
-
-static void rb_commit_contact( struct contact *ct, float p )
-{
- ct->bias = -0.2f*k_rb_rate*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;
-
- rb_contact_count ++;
-}
-
-static void rb_build_manifold_terrain_sphere( rigidbody *rb )
-{
- u32 geo[256];
- v3f tri[3];
- int len = bh_select( &world.geo.bhtris, rb->bbx_world, geo, 256 );
-
- for( int i=0; i<len; i++ )
- {
- u32 *ptri = &world.geo.indices[ geo[i]*3 ];
-
- for( int j=0; j<3; j++ )
- v3_copy( world.geo.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 );
-
- v3f co, norm;
- float p;
-
- for( int j=0; j<2; j++ )
- {
- if( sphere_vs_triangle( rb->co, rb->inf.sphere.radius, tri,co,norm,&p))
- {
- struct contact *ct = rb_start_contact();
-
- if( !ct )
- return;
-
- v3f p1;
- v3_muladds( rb->co, norm, p, p1 );
- vg_line( rb->co, p1, 0xffffffff );
-
- ct->rba = rb;
- v3_copy( co, ct->co );
- v3_copy( norm, ct->n );
- rb_commit_contact( ct, p );
- }
- }
- }
-
-}
-
-static void rb_build_manifold_terrain( rigidbody *rb )
-{
- v3f *box = rb->bbx;
- 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]=box[0][0];p000[1]=box[0][1];p000[2]=box[0][2];
- p001[0]=box[0][0];p001[1]=box[0][1];p001[2]=box[1][2];
- p010[0]=box[0][0];p010[1]=box[1][1];p010[2]=box[0][2];
- p011[0]=box[0][0];p011[1]=box[1][1];p011[2]=box[1][2];
-
- p100[0]=box[1][0];p100[1]=box[0][1];p100[2]=box[0][2];
- p101[0]=box[1][0];p101[1]=box[0][1];p101[2]=box[1][2];
- p110[0]=box[1][0];p110[1]=box[1][1];p110[2]=box[0][2];
- p111[0]=box[1][0];p111[1]=box[1][1];p111[2]=box[1][2];
-
- m4x3_mulv( rb->to_world, p000, p000 );
- m4x3_mulv( rb->to_world, p001, p001 );
- m4x3_mulv( rb->to_world, p010, p010 );
- m4x3_mulv( rb->to_world, p011, p011 );
- m4x3_mulv( rb->to_world, p100, p100 );
- m4x3_mulv( rb->to_world, p101, p101 );
- m4x3_mulv( rb->to_world, p110, p110 );
- m4x3_mulv( rb->to_world, p111, p111 );
-
- int count = 0;
-
- for( int i=0; i<8; i++ )
- {
- float *point = pts[i];
- struct contact *ct = rb_start_contact();
-
- if( !ct )
- return;
-
- ct->rba = rb;
-
- v3f surface;
- v3_copy( point, surface );
- surface[1] += 4.0f;
-
- ray_hit hit;
- hit.dist = INFINITY;
- if( !ray_world( surface, (v3f){0.0f,-1.0f,0.0f}, &hit ))
- continue;