+/* SHite fphysics */
+
+#ifndef RIGIDBODY_H
+#define RIGIDBODY_H
+
+#include "vg/vg.h"
+#include "scene.h"
+
+#define k_rb_delta (1.0f/60.0f)
+
+typedef struct rigidbody rigidbody;
+struct rigidbody
+{
+ v3f co, v, I;
+ v4f q;
+ boxf bbx;
+
+ struct contact
+ {
+ v3f co, n, delta;
+ v3f t[2];
+ float bias, norm_impulse, tangent_impulse[2];
+ }
+ manifold[4];
+ int manifold_count;
+
+ m4x3f to_world, to_local;
+};
+
+static void rb_update_transform( rigidbody *rb )
+{
+ q_normalize( rb->q );
+ q_m3x3( rb->q, rb->to_world );
+ v3_copy( rb->co, rb->to_world[3] );
+
+ m4x3_invert_affine( rb->to_world, rb->to_local );
+}
+
+static void rb_init( rigidbody *rb )
+{
+ q_identity( rb->q );
+ v3_zero( rb->v );
+ v3_zero( rb->I );
+
+ rb_update_transform( rb );
+}
+
+static void rb_iter( rigidbody *rb )
+{
+ v3f gravity = { 0.0f, -9.6f, 0.0f };
+ v3_muladds( rb->v, gravity, k_rb_delta, rb->v );
+
+ /* intergrate velocity */
+ v3_muladds( rb->co, rb->v, k_rb_delta, rb->co );
+
+ v3_lerp( rb->I, (v3f){0.0f,0.0f,0.0f}, 0.0025f, rb->I );
+
+ /* inegrate inertia */
+ if( v3_length2( rb->I ) > 0.0f )
+ {
+ v4f rotation;
+ v3f axis;
+ v3_copy( rb->I, axis );
+
+ float mag = v3_length( axis );
+ v3_divs( axis, mag, axis );
+ q_axis_angle( rotation, axis, mag*k_rb_delta );
+ q_mul( rotation, rb->q, rb->q );
+ }
+}
+
+static void rb_torque( rigidbody *rb, v3f axis, float mag )
+{
+ v3_muladds( rb->I, axis, mag*k_rb_delta, rb->I );
+}
+
+static void rb_tangent_basis( v3f n, v3f tx, v3f ty )
+{
+ /* Compute tangent basis (box2d) */
+ if( fabsf( n[0] ) >= 0.57735027f )
+ {
+ tx[0] = n[1];
+ tx[1] = -n[0];
+ tx[2] = 0.0f;
+ }
+ else
+ {
+ tx[0] = 0.0f;
+ tx[1] = n[2];
+ tx[2] = -n[1];
+ }
+
+ v3_normalize( tx );
+ v3_cross( n, tx, ty );
+}
+
+static void rb_build_manifold( rigidbody *rb, scene *sc )
+{
+ 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 );
+
+ rb->manifold_count = 0;
+
+ for( int i=0; i<8; i++ )
+ {
+ float *point = pts[i];
+ struct contact *ct = &rb->manifold[rb->manifold_count];
+
+ v3f surface;
+
+ v3_copy( point, surface );
+ bvh_scene_sample( sc, surface, ct->n );
+
+ float p = vg_minf( surface[1] - point[1], 1.0f );
+
+ if( p > 0.0f )
+ {
+ v3_add( point, surface, ct->co );
+ v3_muls( ct->co, 0.5f, ct->co );
+
+ //vg_line_pt3( ct->co, 0.0125f, 0xff0000ff );
+
+ v3_sub( ct->co, rb->co, ct->delta );
+ 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;
+
+ rb->manifold_count ++;
+ if( rb->manifold_count == 4 )
+ break;
+ }
+ }
+}
+
+static void rb_constraint_manifold( rigidbody *rb )
+{
+ float k_friction = 0.07f;
+
+ /* Friction Impulse */
+ for( int i=0; i<rb->manifold_count; i++ )
+ {
+ struct contact *ct = &rb->manifold[i];
+
+ v3f dv;
+ v3_cross( rb->I, ct->delta, dv );
+ v3_add( rb->v, dv, dv );
+
+ for( int j=0; j<2; j++ )
+ {
+ float vt = vg_clampf( -v3_dot( dv, ct->t[j] ),
+ -k_friction, k_friction );
+
+ vt = -v3_dot( dv, ct->t[j] );
+
+ float temp = ct->tangent_impulse[j];
+ ct->tangent_impulse[j] = vg_clampf( temp+vt, -k_friction, k_friction );
+ vt = ct->tangent_impulse[j] - temp;
+
+ v3f impulse;
+
+ v3_muls( ct->t[j], vt, impulse );
+ v3_add( impulse, rb->v, rb->v );
+ v3_cross( ct->delta, impulse, impulse );
+ v3_add( impulse, rb->I, rb->I );
+ }
+ }
+
+ /* Normal Impulse */
+ for( int i=0; i<rb->manifold_count; i++ )
+ {
+ struct contact *ct = &rb->manifold[i];
+
+ v3f dv;
+ v3_cross( rb->I, ct->delta, dv );
+ v3_add( rb->v, dv, dv );
+
+ float vn = -v3_dot( dv, ct->n );
+ vn += 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 );
+ v3_add( impulse, rb->v, rb->v );
+ v3_cross( ct->delta, impulse, impulse );
+ v3_add( impulse, rb->I, rb->I );
+ }
+}
+
+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->I, axis, ang*spring*0.5f, rba->I );
+ v3_muladds( rbb->I, axis, -ang*spring*0.5f, rbb->I );
+
+ 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_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->I, wca, rcv_Ra );
+ v3_cross( rb->I, wcb, rcv_Rb );
+ v3_add( rcv_Ra, rcv, rcv );
+ v3_sub( rcv, rcv_Rb, rcv );
+
+ v3f impulse;
+ v3_muls( rcv, 0.5f, impulse );
+ v3_add( impulse, rb->v, rb->v );
+ v3_cross( wcb, impulse, impulse );
+ v3_add( impulse, rb->I, rb->I );
+
+ v3_muls( rcv, -0.5f, impulse );
+ v3_add( impulse, ra->v, ra->v );
+ v3_cross( wca, impulse, impulse );
+ v3_add( impulse, ra->I, ra->I );
+
+
+#if 0
+ 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->I, ra->I );
+
+ v3_muls( delta, -0.5f*spring, impulse );
+
+ v3_add( impulse, rb->v, rb->v );
+ v3_cross( wcb, impulse, impulse );
+ v3_add( impulse, rb->I, rb->I );
+#endif
+}
+
+static void rb_debug( rigidbody *rb, u32 colour )
+{
+ v3f *box = rb->bbx;
+ v3f p000, p001, p010, p011, p100, p101, p110, p111;
+
+ 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 );
+
+ vg_line( p000, p001, colour );
+ vg_line( p001, p011, colour );
+ vg_line( p011, p010, colour );
+ vg_line( p010, p000, colour );
+
+ vg_line( p100, p101, colour );
+ vg_line( p101, p111, colour );
+ vg_line( p111, p110, colour );
+ vg_line( p110, p100, colour );
+
+ vg_line( p100, p000, colour );
+ vg_line( p101, p001, colour );
+ vg_line( p110, p010, colour );
+ vg_line( p111, p011, colour );
+
+ vg_line( p000, p110, colour );
+ vg_line( p100, p010, colour );
+}
+
+#endif /* RIGIDBODY_H */