+
+ v3f p2;
+ v3_muladds( c, ay, cosf(measured)*1.2f, p2 );
+ v3_muladds( p2, ax, sinf(measured)*1.2f, p2 );
+ vg_line( c, p2, colour );
+}
+
+static void rb_debug_constraint_limits( rigidbody *ra, rigidbody *rb, v3f lca,
+ v3f limits[2] )
+{
+ v3f ax, ay, az, bx, by, bz;
+ m3x3_mulv( ra->to_world, (v3f){1.0f,0.0f,0.0f}, ax );
+ m3x3_mulv( ra->to_world, (v3f){0.0f,1.0f,0.0f}, ay );
+ m3x3_mulv( ra->to_world, (v3f){0.0f,0.0f,1.0f}, az );
+ m3x3_mulv( rb->to_world, (v3f){1.0f,0.0f,0.0f}, bx );
+ m3x3_mulv( rb->to_world, (v3f){0.0f,1.0f,0.0f}, by );
+ m3x3_mulv( rb->to_world, (v3f){0.0f,0.0f,1.0f}, bz );
+
+ v2f px, py, pz;
+ px[0] = v3_dot( ay, by );
+ px[1] = v3_dot( az, by );
+
+ py[0] = v3_dot( az, bz );
+ py[1] = v3_dot( ax, bz );
+
+ pz[0] = v3_dot( ax, bx );
+ pz[1] = v3_dot( ay, bx );
+
+ float r0 = atan2f( px[1], px[0] ),
+ r1 = atan2f( py[1], py[0] ),
+ r2 = atan2f( pz[1], pz[0] );
+
+ v3f c;
+ m4x3_mulv( ra->to_world, lca, c );
+ draw_angle_limit( c, ay, az, limits[0][0], limits[1][0], r0, 0xff0000ff );
+ draw_angle_limit( c, az, ax, limits[0][1], limits[1][1], r1, 0xff00ff00 );
+ draw_angle_limit( c, ax, ay, limits[0][2], limits[1][2], r2, 0xffff0000 );
+}
+
+static void rb_limit_cure( rigidbody *ra, rigidbody *rb, v3f axis, float d )
+{
+ if( d != 0.0f )
+ {
+ float avx = v3_dot( ra->w, axis ) - v3_dot( rb->w, axis );
+ float joint_mass = rb->inv_mass + ra->inv_mass;
+ joint_mass = 1.0f/joint_mass;
+
+ float bias = (k_limit_bias * k_rb_rate) * d,
+ lambda = -(avx + bias) * joint_mass;
+
+ /* Angular velocity */
+ v3f wa, wb;
+ v3_muls( axis, lambda * ra->inv_mass, wa );
+ v3_muls( axis, -lambda * rb->inv_mass, wb );
+
+ v3_add( ra->w, wa, ra->w );
+ v3_add( rb->w, wb, rb->w );
+ }
+}
+
+static void rb_constraint_limits( rigidbody *ra, v3f lca,
+ rigidbody *rb, v3f lcb, v3f limits[2] )
+{
+ v3f ax, ay, az, bx, by, bz;
+ m3x3_mulv( ra->to_world, (v3f){1.0f,0.0f,0.0f}, ax );
+ m3x3_mulv( ra->to_world, (v3f){0.0f,1.0f,0.0f}, ay );
+ m3x3_mulv( ra->to_world, (v3f){0.0f,0.0f,1.0f}, az );
+ m3x3_mulv( rb->to_world, (v3f){1.0f,0.0f,0.0f}, bx );
+ m3x3_mulv( rb->to_world, (v3f){0.0f,1.0f,0.0f}, by );
+ m3x3_mulv( rb->to_world, (v3f){0.0f,0.0f,1.0f}, bz );
+
+ v2f px, py, pz;
+ px[0] = v3_dot( ay, by );
+ px[1] = v3_dot( az, by );
+
+ py[0] = v3_dot( az, bz );
+ py[1] = v3_dot( ax, bz );
+
+ pz[0] = v3_dot( ax, bx );
+ pz[1] = v3_dot( ay, bx );
+
+ float r0 = atan2f( px[1], px[0] ),
+ r1 = atan2f( py[1], py[0] ),
+ r2 = atan2f( pz[1], pz[0] );
+
+ /* calculate angle deltas */
+ float dx = 0.0f, dy = 0.0f, dz = 0.0f;
+
+ if( r0 < limits[0][0] ) dx = limits[0][0] - r0;
+ if( r0 > limits[1][0] ) dx = limits[1][0] - r0;
+ if( r1 < limits[0][1] ) dy = limits[0][1] - r1;
+ if( r1 > limits[1][1] ) dy = limits[1][1] - r1;
+ if( r2 < limits[0][2] ) dz = limits[0][2] - r2;
+ if( r2 > limits[1][2] ) dz = limits[1][2] - r2;
+
+ v3f wca, wcb;
+ m3x3_mulv( ra->to_world, lca, wca );
+ m3x3_mulv( rb->to_world, lcb, wcb );
+
+ rb_limit_cure( ra, rb, ax, dx );
+ rb_limit_cure( ra, rb, ay, dy );
+ rb_limit_cure( ra, rb, az, dz );
+}
+
+static void rb_debug_constraint_position( rigidbody *ra, v3f lca,
+ rigidbody *rb, v3f lcb )
+{
+ v3f wca, wcb;
+ m3x3_mulv( ra->to_world, lca, wca );
+ m3x3_mulv( rb->to_world, lcb, wcb );
+
+ v3f p0, p1;
+ v3_add( wca, ra->co, p0 );
+ v3_add( wcb, rb->co, p1 );
+ vg_line_pt3( p0, 0.005f, 0xffffff00 );
+ vg_line_pt3( p1, 0.005f, 0xffffff00 );
+ vg_line( p0, p1, 0xffffff00 );
+}
+
+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 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 );
+
+ v3f delta;
+ v3f p0, p1;
+ v3_add( wca, ra->co, p0 );
+ v3_add( wcb, rb->co, p1 );
+ v3_sub( p1, p0, delta );
+
+ float dist2 = v3_length2( delta );
+
+ if( dist2 > 0.00001f )
+ {
+ float dist = sqrtf(dist2);
+ v3_muls( delta, 1.0f/dist, delta );
+
+ float joint_mass = rb->inv_mass + ra->inv_mass;
+
+ v3f raCn, rbCn, raCt, rbCt;
+ v3_cross( wca, delta, raCn );
+ v3_cross( wcb, delta, rbCn );
+
+ /* orient inverse inertia tensors */
+ v3f raCnI, rbCnI;
+ m3x3_mulv( ra->iIw, raCn, raCnI );
+ m3x3_mulv( rb->iIw, rbCn, rbCnI );
+ joint_mass += v3_dot( raCn, raCnI );
+ joint_mass += v3_dot( rbCn, rbCnI );
+ joint_mass = 1.0f/joint_mass;
+
+ float vd = v3_dot( rcv, delta ),
+ bias = -(k_joint_bias * k_rb_rate) * dist,
+ lambda = -(vd + bias) * joint_mass;
+
+ v3f impulse;
+ v3_muls( delta, lambda, impulse );
+ rb_linear_impulse( ra, wca, impulse );
+ v3_muls( delta, -lambda, impulse );
+ rb_linear_impulse( rb, wcb, impulse );
+
+ /* 'fake' snap */
+ v3_muladds( ra->co, delta, dist * k_joint_correction, ra->co );
+ v3_muladds( rb->co, delta, -dist * k_joint_correction, rb->co );
+ }
+}
+
+/*
+ * Effectors
+ */
+
+static void rb_effect_simple_bouyency( rigidbody *ra, v4f plane,
+ float amt, float drag )
+{
+ /* float */
+ float depth = v3_dot( plane, ra->co ) - plane[3],
+ lambda = vg_clampf( -depth, 0.0f, 1.0f ) * amt;
+
+ v3_muladds( ra->v, plane, lambda * ktimestep, ra->v );
+
+ if( depth < 0.0f )
+ v3_muls( ra->v, 1.0f-(drag*ktimestep), ra->v );