+ ray_hit hit;
+ hit.dist = INFINITY;
+ if( !ray_world( surface, (v3f){0.0f,-1.0f,0.0f}, &hit ))
+ continue;
+
+ v3_copy( hit.pos, surface );
+
+ float p = vg_minf( surface[1] - point[1], 1.0f );
+
+ if( p > 0.0f )
+ {
+ v3_copy( hit.normal, ct->n );
+ v3_add( point, surface, ct->co );
+ v3_muls( ct->co, 0.5f, ct->co );
+
+ rb_commit_contact( ct, p );
+ count ++;
+ if( count == 4 )
+ break;
+ }
+ }
+}
+
+/*
+ * Initializing things like tangent vectors
+ */
+
+static void rb_presolve_contacts( rb_ct *buffer, int len )
+{
+ for( int i=0; i<len; i++ )
+ {
+ rb_ct *ct = &buffer[i];
+ ct->bias = -0.2f * k_rb_rate * vg_minf(0.0f,-ct->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;
+ ct->mass_total = 1.0f/(ct->rba->inv_mass + ct->rbb->inv_mass);
+
+ rb_debug_contact( ct );
+ }
+}
+
+/*
+ * Creates relative contact velocity vector, and offsets between each body
+ */
+static void rb_rcv( rb_ct *ct, v3f rv, v3f da, v3f db )
+{
+ rigidbody *rba = ct->rba,
+ *rbb = ct->rbb;
+
+ v3_sub( ct->co, rba->co, da );
+ v3_sub( ct->co, rbb->co, db );
+
+ v3f rva, rvb;
+ v3_cross( rba->w, da, rva );
+ v3_add( rba->v, rva, rva );
+ v3_cross( rbb->w, db, rvb );
+ v3_add( rbb->v, rvb, rvb );
+
+ v3_sub( rva, rvb, rv );
+}
+
+/*
+ * Apply regular and angular velocity impulses to objects involved in contact
+ */
+static void rb_standard_impulse( rb_ct *ct, v3f da, v3f db, v3f impulse )
+{
+ rigidbody *rba = ct->rba,
+ *rbb = ct->rbb;
+
+ v3f ia, ib;
+ v3_muls( impulse, ct->mass_total*rba->inv_mass, ia );
+ v3_muls( impulse, -ct->mass_total*rbb->inv_mass, ib );
+
+ /* response */
+ v3_add( rba->v, ia, rba->v );
+ v3_add( rbb->v, ib, rbb->v );
+
+ /* Angular velocity */
+ v3f wa, wb;
+ v3_cross( da, ia, wa );
+ v3_cross( db, ib, wb );
+ v3_add( rba->w, wa, rba->w );
+ v3_add( rbb->w, wb, rbb->w );
+}
+
+/*
+ * One iteration to solve the contact constraint
+ */
+static void rb_solve_contacts( rb_ct *buf, int len )
+{
+ float k_friction = 0.1f;
+
+ /* Friction Impulse */
+ for( int i=0; i<len; i++ )
+ {
+ struct contact *ct = &buf[i];
+ rigidbody *rb = ct->rba;
+
+ v3f rv, da, db;
+ rb_rcv( ct, rv, da, db );
+
+ for( int j=0; j<2; j++ )
+ {
+ float f = k_friction * ct->norm_impulse,
+ vt = -v3_dot( rv, ct->t[j] );
+
+ float temp = ct->tangent_impulse[j];
+ ct->tangent_impulse[j] = vg_clampf( temp+vt, -f, f );
+ vt = ct->tangent_impulse[j] - temp;
+
+ v3f impulse;
+ v3_muls( ct->t[j], vt, impulse );
+ rb_standard_impulse( ct, da, db, impulse );
+ }
+ }
+
+ /* Normal Impulse */
+ for( int i=0; i<len; i++ )
+ {
+ struct contact *ct = &buf[i];
+ rigidbody *rba = ct->rba,
+ *rbb = ct->rbb;
+
+ v3f rv, da, db;
+ rb_rcv( ct, rv, da, db );
+
+ float vn = -v3_dot( rv, ct->n ) + ct->bias;
+
+ float temp = ct->norm_impulse;
+ ct->norm_impulse = vg_maxf( temp + vn, 0.0f );
+ vn = ct->norm_impulse - temp;
+
+ v3f impulse;
+ v3_muls( ct->n, vn, impulse );
+ rb_standard_impulse( ct, da, db, impulse );
+ }
+}
+
+/*
+ * The following ventures into not really very sophisticated at all maths
+ */
+
+struct rb_angle_limit
+{
+ rigidbody *rba, *rbb;
+ v3f axis;
+ float impulse, bias;
+};
+
+static int rb_angle_limit_force( rigidbody *rba, v3f va,
+ rigidbody *rbb, v3f vb,
+ float max )
+{
+ v3f wva, wvb;
+ m3x3_mulv( rba->to_world, va, wva );
+ m3x3_mulv( rbb->to_world, vb, wvb );
+
+ float dt = v3_dot(wva,wvb)*0.999f,
+ ang = fabsf(dt);
+ ang = acosf( dt );
+ if( ang > max )
+ {
+ float correction = max-ang;
+
+ v3f axis;
+ v3_cross( wva, wvb, axis );
+
+ v4f rotation;
+ q_axis_angle( rotation, axis, -correction*0.25f );
+ q_mul( rotation, rba->q, rba->q );
+
+ q_axis_angle( rotation, axis, correction*0.25f );
+ q_mul( rotation, rbb->q, rbb->q );
+
+ return 1;
+ }
+
+ return 0;
+}
+
+static void rb_constraint_angle_limit( struct rb_angle_limit *limit )
+{
+
+}
+
+RB_DEPR
+static void rb_constraint_angle( rigidbody *rba, v3f va,
+ rigidbody *rbb, v3f vb,
+ float max, float spring )
+{
+ v3f wva, wvb;
+ m3x3_mulv( rba->to_world, va, wva );
+ m3x3_mulv( rbb->to_world, vb, wvb );
+
+ float dt = v3_dot(wva,wvb)*0.999f,
+ ang = fabsf(dt);
+
+ v3f axis;
+ v3_cross( wva, wvb, axis );
+ v3_muladds( rba->w, axis, ang*spring*0.5f, rba->w );
+ v3_muladds( rbb->w, axis, -ang*spring*0.5f, rbb->w );
+
+ return;
+
+ /* TODO: convert max into the dot product value so we dont have to always
+ * evaluate acosf, only if its greater than the angle specified */
+ ang = acosf( dt );
+ if( ang > max )
+ {
+ float correction = max-ang;
+
+ v4f rotation;
+ q_axis_angle( rotation, axis, -correction*0.125f );
+ q_mul( rotation, rba->q, rba->q );
+
+ q_axis_angle( rotation, axis, correction*0.125f );
+ q_mul( rotation, rbb->q, rbb->q );
+ }
+}
+
+static void rb_relative_velocity( rigidbody *ra, v3f lca,
+ rigidbody *rb, v3f lcb, v3f rcv )
+{
+ v3f wca, wcb;
+ m3x3_mulv( ra->to_world, lca, wca );
+ m3x3_mulv( rb->to_world, lcb, wcb );
+
+ v3_sub( ra->v, rb->v, rcv );
+
+ v3f rcv_Ra, rcv_Rb;
+ v3_cross( ra->w, wca, rcv_Ra );
+ v3_cross( rb->w, wcb, rcv_Rb );
+ v3_add( rcv_Ra, rcv, rcv );
+ v3_sub( rcv, rcv_Rb, rcv );
+}
+
+static void rb_constraint_position( rigidbody *ra, v3f lca,
+ rigidbody *rb, v3f lcb )
+{
+ /* C = (COa + Ra*LCa) - (COb + Rb*LCb) = 0 */
+ v3f wca, wcb;
+ m3x3_mulv( ra->to_world, lca, wca );
+ m3x3_mulv( rb->to_world, lcb, wcb );
+
+ v3f delta;
+ v3_add( wcb, rb->co, delta );
+ v3_sub( delta, wca, delta );
+ v3_sub( delta, ra->co, delta );
+
+ v3_muladds( ra->co, delta, 0.5f, ra->co );
+ v3_muladds( rb->co, delta, -0.5f, rb->co );
+
+ v3f rcv;
+ v3_sub( ra->v, rb->v, rcv );
+
+ v3f rcv_Ra, rcv_Rb;
+ v3_cross( ra->w, wca, rcv_Ra );
+ v3_cross( rb->w, wcb, rcv_Rb );
+ v3_add( rcv_Ra, rcv, rcv );
+ v3_sub( rcv, rcv_Rb, rcv );
+
+ float nm = 0.5f/(rb->inv_mass + ra->inv_mass);
+
+ float mass_a = 1.0f/ra->inv_mass,
+ mass_b = 1.0f/rb->inv_mass,
+ total_mass = mass_a+mass_b;
+
+ v3f impulse;
+ v3_muls( rcv, 1.0f, impulse );
+ v3_muladds( rb->v, impulse, mass_b/total_mass, rb->v );
+ v3_cross( wcb, impulse, impulse );
+ v3_add( impulse, rb->w, rb->w );
+
+ v3_muls( rcv, -1.0f, impulse );
+ v3_muladds( ra->v, impulse, mass_a/total_mass, ra->v );
+ v3_cross( wca, impulse, impulse );
+ v3_add( impulse, ra->w, ra->w );
+
+#if 0
+ /*
+ * this could be used for spring joints
+ * its not good for position constraint
+ */
+ v3f impulse;
+ v3_muls( delta, 0.5f*spring, impulse );
+
+ v3_add( impulse, ra->v, ra->v );
+ v3_cross( wca, impulse, impulse );
+ v3_add( impulse, ra->w, ra->w );
+
+ v3_muls( delta, -0.5f*spring, impulse );
+
+ v3_add( impulse, rb->v, rb->v );
+ v3_cross( wcb, impulse, impulse );
+ v3_add( impulse, rb->w, rb->w );
+#endif
+}
+
+static void debug_sphere( m4x3f m, float radius, u32 colour )
+{
+ v3f ly = { 0.0f, 0.0f, radius },
+ lx = { 0.0f, radius, 0.0f },
+ lz = { 0.0f, 0.0f, radius };
+
+ for( int i=0; i<16; i++ )
+ {
+ float t = ((float)(i+1) * (1.0f/16.0f)) * VG_PIf * 2.0f,
+ s = sinf(t),
+ c = cosf(t);
+
+ v3f py = { s*radius, 0.0f, c*radius },
+ px = { s*radius, c*radius, 0.0f },
+ pz = { 0.0f, s*radius, c*radius };
+
+ v3f p0, p1, p2, p3, p4, p5;
+ m4x3_mulv( m, py, p0 );
+ m4x3_mulv( m, ly, p1 );
+ m4x3_mulv( m, px, p2 );
+ m4x3_mulv( m, lx, p3 );
+ m4x3_mulv( m, pz, p4 );
+ m4x3_mulv( m, lz, p5 );
+
+ vg_line( p0, p1, colour == 0x00? 0xff00ff00: colour );
+ vg_line( p2, p3, colour == 0x00? 0xff0000ff: colour );
+ vg_line( p4, p5, colour == 0x00? 0xffff0000: colour );
+
+ v3_copy( py, ly );
+ v3_copy( px, lx );
+ v3_copy( pz, lz );
+ }
+}
+
+static void debug_capsule( m4x3f m, float radius, float h, u32 colour )
+{
+ v3f ly = { 0.0f, 0.0f, radius },
+ lx = { 0.0f, radius, 0.0f },
+ lz = { 0.0f, 0.0f, radius };
+
+ float s0 = sinf(0.0f)*radius,
+ c0 = cosf(0.0f)*radius;
+
+ v3f p0, p1, up, right, forward;
+ m3x3_mulv( m, (v3f){0.0f,1.0f,0.0f}, up );
+ m3x3_mulv( m, (v3f){1.0f,0.0f,0.0f}, right );
+ m3x3_mulv( m, (v3f){0.0f,0.0f,-1.0f}, forward );
+ v3_muladds( m[3], up, -h*0.5f+radius, p0 );
+ v3_muladds( m[3], up, h*0.5f-radius, p1 );
+
+ v3f a0, a1, b0, b1;
+ v3_muladds( p0, right, radius, a0 );
+ v3_muladds( p1, right, radius, a1 );
+ v3_muladds( p0, forward, radius, b0 );
+ v3_muladds( p1, forward, radius, b1 );
+ vg_line( a0, a1, colour );
+ vg_line( b0, b1, colour );
+
+ v3_muladds( p0, right, -radius, a0 );
+ v3_muladds( p1, right, -radius, a1 );
+ v3_muladds( p0, forward, -radius, b0 );
+ v3_muladds( p1, forward, -radius, b1 );
+ vg_line( a0, a1, colour );
+ vg_line( b0, b1, colour );
+
+ for( int i=0; i<16; i++ )
+ {
+ float t = ((float)(i+1) * (1.0f/16.0f)) * VG_PIf * 2.0f,
+ s1 = sinf(t)*radius,
+ c1 = cosf(t)*radius;
+
+ v3f e0 = { s0, 0.0f, c0 },
+ e1 = { s1, 0.0f, c1 },
+ e2 = { s0, c0, 0.0f },
+ e3 = { s1, c1, 0.0f },
+ e4 = { 0.0f, c0, s0 },
+ e5 = { 0.0f, c1, s1 };
+
+ m3x3_mulv( m, e0, e0 );
+ m3x3_mulv( m, e1, e1 );
+ m3x3_mulv( m, e2, e2 );
+ m3x3_mulv( m, e3, e3 );
+ m3x3_mulv( m, e4, e4 );
+ m3x3_mulv( m, e5, e5 );
+
+ v3_add( p0, e0, a0 );
+ v3_add( p0, e1, a1 );
+ v3_add( p1, e0, b0 );
+ v3_add( p1, e1, b1 );
+
+ vg_line( a0, a1, colour );
+ vg_line( b0, b1, colour );
+
+ if( c0 < 0.0f )
+ {
+ v3_add( p0, e2, a0 );
+ v3_add( p0, e3, a1 );
+ v3_add( p0, e4, b0 );
+ v3_add( p0, e5, b1 );
+ }
+ else
+ {
+ v3_add( p1, e2, a0 );
+ v3_add( p1, e3, a1 );
+ v3_add( p1, e4, b0 );
+ v3_add( p1, e5, b1 );
+ }
+
+ vg_line( a0, a1, colour );
+ vg_line( b0, b1, colour );
+
+ s0 = s1;
+ c0 = c1;
+ }
+}
+
+static void rb_debug( rigidbody *rb, u32 colour )
+{
+ if( rb->type == k_rb_shape_box )
+ {
+ v3f *box = rb->bbx;
+ vg_line_boxf_transformed( rb->to_world, rb->bbx, colour );
+ }
+ else if( rb->type == k_rb_shape_sphere )
+ {
+ debug_sphere( rb->to_world, rb->inf.sphere.radius, colour );
+ }
+ else if( rb->type == k_rb_shape_capsule )
+ {
+ m4x3f m0, m1;
+ float h = rb->inf.capsule.height,
+ r = rb->inf.capsule.radius;
+
+ debug_capsule( rb->to_world, r, h, colour );
+ }
+}
+
+/*
+ * out penetration distance, normal
+ */
+static int rb_point_in_body( rigidbody *rb, v3f pos, float *pen, v3f normal )
+{
+ v3f local;
+ m4x3_mulv( rb->to_local, pos, local );
+
+ if( local[0] > rb->bbx[0][0] && local[0] < rb->bbx[1][0] &&
+ local[1] > rb->bbx[0][1] && local[1] < rb->bbx[1][1] &&
+ local[2] > rb->bbx[0][2] && local[2] < rb->bbx[1][2] )
+ {
+ v3f area, com, comrel;
+ v3_add( rb->bbx[0], rb->bbx[1], com );
+ v3_muls( com, 0.5f, com );
+
+ v3_sub( rb->bbx[1], rb->bbx[0], area );
+ v3_sub( local, com, comrel );
+ v3_div( comrel, area, comrel );
+
+ int axis = 0;
+ float max_mag = fabsf(comrel[0]);
+
+ if( fabsf(comrel[1]) > max_mag )
+ {
+ axis = 1;
+ max_mag = fabsf(comrel[1]);
+ }
+ if( fabsf(comrel[2]) > max_mag )
+ {
+ axis = 2;
+ max_mag = fabsf(comrel[2]);
+ }
+
+ v3_zero( normal );
+ normal[axis] = vg_signf(comrel[axis]);
+
+ if( normal[axis] < 0.0f )
+ *pen = local[axis] - rb->bbx[0][axis];
+ else
+ *pen = rb->bbx[1][axis] - local[axis];
+
+ m3x3_mulv( rb->to_world, normal, normal );
+ return 1;
+ }
+
+ return 0;