+
+ /* Normal */
+ rb_rcv( ct, rv, da, db );
+ float vn = v3_dot( rv, ct->n ),
+ lambda = ct->normal_mass * (-vn + ct->bias);
+
+ float temp = ct->norm_impulse;
+ ct->norm_impulse = vg_maxf( temp + lambda, 0.0f );
+ lambda = ct->norm_impulse - temp;
+
+ v3f impulse;
+ v3_muls( ct->n, lambda, impulse );
+ rb_linear_impulse( ct->rba, da, impulse );
+
+ v3_muls( ct->n, -lambda, impulse );
+ rb_linear_impulse( ct->rbb, db, impulse );
+ }
+}
+
+/*
+ * -----------------------------------------------------------------------------
+ * Constraints
+ * -----------------------------------------------------------------------------
+ */
+
+VG_STATIC void draw_angle_limit( v3f c, v3f major, v3f minor,
+ float amin, float amax, float measured,
+ u32 colour )
+{
+ float f = 0.05f;
+ v3f ay, ax;
+ v3_muls( major, f, ay );
+ v3_muls( minor, f, ax );
+
+ for( int x=0; x<16; x++ )
+ {
+ float t0 = (float)x / 16.0f,
+ t1 = (float)(x+1) / 16.0f,
+ a0 = vg_lerpf( amin, amax, t0 ),
+ a1 = vg_lerpf( amin, amax, t1 );
+
+ v3f p0, p1;
+ v3_muladds( c, ay, cosf(a0), p0 );
+ v3_muladds( p0, ax, sinf(a0), p0 );
+ v3_muladds( c, ay, cosf(a1), p1 );
+ v3_muladds( p1, ax, sinf(a1), p1 );
+
+ vg_line( p0, p1, colour );
+
+ if( x == 0 )
+ vg_line( c, p0, colour );
+ if( x == 15 )
+ vg_line( c, p1, colour );
+ }
+
+ 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 );
+}
+
+VG_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 );
+}
+
+VG_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 );
+ }
+}
+
+VG_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 );
+}
+
+VG_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 );
+}
+
+VG_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 );