+VG_STATIC void rb_debug_swingtwist_constraints( rb_constr_swingtwist *buf,
+ int len ){
+ float size = 0.12f;
+
+ for( int i=0; i<len; i++ ){
+ rb_constr_swingtwist *st = &buf[ i ];
+
+ v3f vx, vxb, vy, va, axis, center;
+
+ m3x3_mulv( st->rba->to_world, st->conevx, vx );
+ m3x3_mulv( st->rbb->to_world, st->conevxb, vxb );
+ m3x3_mulv( st->rba->to_world, st->conevy, vy );
+ m3x3_mulv( st->rbb->to_world, st->coneva, va );
+ m4x3_mulv( st->rba->to_world, st->view_offset, center );
+ v3_cross( vy, vx, axis );
+
+ float rx = st->conevx[3], /* elipse radii */
+ ry = st->conevy[3];
+
+ v3f p0, p1;
+ v3_muladds( center, va, size, p1 );
+ vg_line( center, p1, 0xffffffff );
+ vg_line_point( p1, 0.00025f, 0xffffffff );
+
+ if( st->tangent_violation ){
+ v3_muladds( center, st->tangent_target, size, p0 );
+
+ vg_line( center, p0, 0xff00ff00 );
+ vg_line_point( p0, 0.00025f, 0xff00ff00 );
+ vg_line( p1, p0, 0xff000000 );
+ }
+
+ for( int x=0; x<32; x++ ){
+ float t0 = ((float)x * (1.0f/32.0f)) * VG_TAUf,
+ t1 = (((float)x+1.0f) * (1.0f/32.0f)) * VG_TAUf,
+ c0 = cosf( t0 ),
+ s0 = sinf( t0 ),
+ c1 = cosf( t1 ),
+ s1 = sinf( t1 );
+
+ v3f v0, v1;
+ v3_muladds( axis, vx, c0*rx, v0 );
+ v3_muladds( v0, vy, s0*ry, v0 );
+ v3_muladds( axis, vx, c1*rx, v1 );
+ v3_muladds( v1, vy, s1*ry, v1 );
+
+ v3_normalize( v0 );
+ v3_normalize( v1 );
+
+ v3_muladds( center, v0, size, p0 );
+ v3_muladds( center, v1, size, p1 );
+
+ u32 col0r = fabsf(c0) * 255.0f,
+ col0g = fabsf(s0) * 255.0f,
+ col1r = fabsf(c1) * 255.0f,
+ col1g = fabsf(s1) * 255.0f,
+ col = st->tangent_violation? 0xff0000ff: 0xff000000,
+ col0 = col | (col0r<<16) | (col0g << 8),
+ col1 = col | (col1r<<16) | (col1g << 8);
+
+ vg_line2( center, p0, VG__NONE, col0 );
+ vg_line2( p0, p1, col0, col1 );
+ }
+
+ /* Draw twist */
+ v3_muladds( center, va, size, p0 );
+ v3_muladds( p0, vxb, size, p1 );
+
+ vg_line( p0, p1, 0xff0000ff );
+
+ if( st->axis_violation ){
+ v3_muladds( p0, st->axis_target, size*1.25f, p1 );
+ vg_line( p0, p1, 0xffffff00 );
+ vg_line_point( p1, 0.0025f, 0xffffff80 );
+ }
+
+ v3f refaxis;
+ v3_cross( vy, va, refaxis ); /* our default rotation */
+ v3_normalize( refaxis );
+ v3f refaxis_up;
+ v3_cross( va, refaxis, refaxis_up );
+ float newang = acosf(st->conet-0.0001f);
+
+ v3_muladds( p0, refaxis_up, sinf(newang)*size, p1 );
+ v3_muladds( p1, refaxis, -cosf(newang)*size, p1 );
+ vg_line( p0, p1, 0xff000000 );
+
+ v3_muladds( p0, refaxis_up, sinf(-newang)*size, p1 );
+ v3_muladds( p1, refaxis, -cosf(-newang)*size, p1 );
+ vg_line( p0, p1, 0xff404040 );
+ }
+}
+
+/*
+ * Solve a list of positional constraints
+ */
+VG_STATIC void rb_solve_position_constraints( rb_constr_pos *buf, int len ){
+ for( int i=0; i<len; i++ ){
+ rb_constr_pos *constr = &buf[i];
+ rigidbody *rba = constr->rba, *rbb = constr->rbb;
+
+ v3f wa, wb;
+ m3x3_mulv( rba->to_world, constr->lca, wa );
+ m3x3_mulv( rbb->to_world, constr->lcb, wb );
+
+ m3x3f ssra, ssrat, ssrb, ssrbt;
+
+ m3x3_skew_symetric( ssrat, wa );
+ m3x3_skew_symetric( ssrbt, wb );
+ m3x3_transpose( ssrat, ssra );
+ m3x3_transpose( ssrbt, ssrb );
+
+ v3f b, b_wa, b_wb, b_a, b_b;
+ m3x3_mulv( ssra, rba->w, b_wa );
+ m3x3_mulv( ssrb, rbb->w, b_wb );
+ v3_add( rba->v, b_wa, b );
+ v3_sub( b, rbb->v, b );
+ v3_sub( b, b_wb, b );
+ v3_muls( b, -1.0f, b );
+
+ m3x3f invMa, invMb;
+ m3x3_diagonal( invMa, rba->inv_mass );
+ m3x3_diagonal( invMb, rbb->inv_mass );
+
+ m3x3f ia, ib;
+ m3x3_mul( ssra, rba->iIw, ia );
+ m3x3_mul( ia, ssrat, ia );
+ m3x3_mul( ssrb, rbb->iIw, ib );
+ m3x3_mul( ib, ssrbt, ib );
+
+ m3x3f cma, cmb;
+ m3x3_add( invMa, ia, cma );
+ m3x3_add( invMb, ib, cmb );
+
+ m3x3f A;
+ m3x3_add( cma, cmb, A );
+
+ /* Solve Ax = b ( A^-1*b = x ) */
+ v3f impulse;
+ m3x3f invA;
+ m3x3_inv( A, invA );
+ m3x3_mulv( invA, b, impulse );
+
+ v3f delta_va, delta_wa, delta_vb, delta_wb;
+ m3x3f iwa, iwb;
+ m3x3_mul( rba->iIw, ssrat, iwa );
+ m3x3_mul( rbb->iIw, ssrbt, iwb );
+
+ m3x3_mulv( invMa, impulse, delta_va );
+ m3x3_mulv( invMb, impulse, delta_vb );
+ m3x3_mulv( iwa, impulse, delta_wa );
+ m3x3_mulv( iwb, impulse, delta_wb );
+
+ v3_add( rba->v, delta_va, rba->v );
+ v3_add( rba->w, delta_wa, rba->w );
+ v3_sub( rbb->v, delta_vb, rbb->v );
+ v3_sub( rbb->w, delta_wb, rbb->w );
+ }
+}
+
+VG_STATIC void rb_solve_swingtwist_constraints( rb_constr_swingtwist *buf,
+ int len ){
+ float size = 0.12f;
+
+ for( int i=0; i<len; i++ ){
+ rb_constr_swingtwist *st = &buf[ i ];
+
+ if( !st->axis_violation )
+ continue;
+
+ float rv = v3_dot( st->axis, st->rbb->w ) -
+ v3_dot( st->axis, st->rba->w );
+
+ if( rv * (float)st->axis_violation > 0.0f )
+ continue;
+
+ v3f impulse, wa, wb;
+ v3_muls( st->axis, rv*st->axis_mass, impulse );
+ m3x3_mulv( st->rba->iIw, impulse, wa );
+ v3_add( st->rba->w, wa, st->rba->w );
+
+ v3_muls( impulse, -1.0f, impulse );
+ m3x3_mulv( st->rbb->iIw, impulse, wb );
+ v3_add( st->rbb->w, wb, st->rbb->w );
+
+ float rv2 = v3_dot( st->axis, st->rbb->w ) -
+ v3_dot( st->axis, st->rba->w );
+ }
+
+ for( int i=0; i<len; i++ ){
+ rb_constr_swingtwist *st = &buf[ i ];
+
+ if( !st->tangent_violation )
+ continue;
+
+ float rv = v3_dot( st->tangent_axis, st->rbb->w ) -
+ v3_dot( st->tangent_axis, st->rba->w );
+
+ if( rv > 0.0f )
+ continue;
+
+ v3f impulse, wa, wb;
+ v3_muls( st->tangent_axis, rv*st->tangent_mass, impulse );
+ m3x3_mulv( st->rba->iIw, impulse, wa );
+ v3_add( st->rba->w, wa, st->rba->w );
+
+ v3_muls( impulse, -1.0f, impulse );
+ m3x3_mulv( st->rbb->iIw, impulse, wb );
+ v3_add( st->rbb->w, wb, st->rbb->w );
+
+ float rv2 = v3_dot( st->tangent_axis, st->rbb->w ) -
+ v3_dot( st->tangent_axis, st->rba->w );
+ }
+}
+
+VG_STATIC void rb_solve_constr_angle( rigidbody *rba, rigidbody *rbb,
+ v3f ra, v3f rb ){
+ m3x3f ssra, ssrb, ssrat, ssrbt;
+ m3x3f cma, cmb;
+
+ m3x3_skew_symetric( ssrat, ra );
+ m3x3_skew_symetric( ssrbt, rb );
+ m3x3_transpose( ssrat, ssra );
+ m3x3_transpose( ssrbt, ssrb );
+
+ m3x3_mul( ssra, rba->iIw, cma );
+ m3x3_mul( cma, ssrat, cma );
+ m3x3_mul( ssrb, rbb->iIw, cmb );
+ m3x3_mul( cmb, ssrbt, cmb );
+
+ m3x3f A, invA;
+ m3x3_add( cma, cmb, A );
+ m3x3_inv( A, invA );
+
+ v3f b_wa, b_wb, b;
+ m3x3_mulv( ssra, rba->w, b_wa );
+ m3x3_mulv( ssrb, rbb->w, b_wb );
+ v3_add( b_wa, b_wb, b );
+ v3_negate( b, b );
+
+ v3f impulse;
+ m3x3_mulv( invA, b, impulse );
+
+ v3f delta_wa, delta_wb;
+ m3x3f iwa, iwb;
+ m3x3_mul( rba->iIw, ssrat, iwa );
+ m3x3_mul( rbb->iIw, ssrbt, iwb );
+ m3x3_mulv( iwa, impulse, delta_wa );
+ m3x3_mulv( iwb, impulse, delta_wb );
+ v3_add( rba->w, delta_wa, rba->w );
+ v3_sub( rbb->w, delta_wb, rbb->w );
+}
+
+/*
+ * Correct position constraint drift errors
+ * [ 0.0 <= amt <= 1.0 ]: the correction amount
+ */
+VG_STATIC void rb_correct_position_constraints( rb_constr_pos *buf, int len,
+ float amt ){
+ for( int i=0; i<len; i++ ){
+ rb_constr_pos *constr = &buf[i];
+ rigidbody *rba = constr->rba, *rbb = constr->rbb;
+
+ v3f p0, p1, d;
+ m3x3_mulv( rba->to_world, constr->lca, p0 );
+ m3x3_mulv( rbb->to_world, constr->lcb, p1 );
+ v3_add( rba->co, p0, p0 );
+ v3_add( rbb->co, p1, p1 );
+ v3_sub( p1, p0, d );
+
+ v3_muladds( rbb->co, d, -1.0f * amt, rbb->co );
+ rb_update_transform( rbb );
+ }
+}
+
+VG_STATIC void rb_correct_swingtwist_constraints( rb_constr_swingtwist *buf,
+ int len, float amt ){
+ for( int i=0; i<len; i++ ){
+ rb_constr_swingtwist *st = &buf[i];
+
+ if( !st->tangent_violation )
+ continue;
+
+ v3f va;
+ m3x3_mulv( st->rbb->to_world, st->coneva, va );
+
+ float angle = v3_dot( va, st->tangent_target );
+
+ if( fabsf(angle) < 0.9999f ){
+ v3f axis;
+ v3_cross( va, st->tangent_target, axis );
+
+ v4f correction;
+ q_axis_angle( correction, axis, acosf(angle) * amt );
+ q_mul( correction, st->rbb->q, st->rbb->q );
+ rb_update_transform( st->rbb );
+ }
+ }
+
+ for( int i=0; i<len; i++ ){
+ rb_constr_swingtwist *st = &buf[i];
+
+ if( !st->axis_violation )
+ continue;
+
+ v3f vxb;
+ m3x3_mulv( st->rbb->to_world, st->conevxb, vxb );
+
+ float angle = v3_dot( vxb, st->axis_target );
+
+ if( fabsf(angle) < 0.9999f ){
+ v3f axis;
+ v3_cross( vxb, st->axis_target, axis );
+
+ v4f correction;
+ q_axis_angle( correction, axis, acosf(angle) * amt );
+ q_mul( correction, st->rbb->q, st->rbb->q );
+ rb_update_transform( st->rbb );
+ }
+ }
+}
+
+VG_STATIC void rb_correct_contact_constraints( rb_ct *buf, int len, float amt ){
+ for( int i=0; i<len; i++ ){
+ rb_ct *ct = &buf[i];
+ rigidbody *rba = ct->rba,
+ *rbb = ct->rbb;
+
+ float mass_total = 1.0f / (rba->inv_mass + rbb->inv_mass);
+
+ v3_muladds( rba->co, ct->n, -mass_total * rba->inv_mass, rba->co );
+ v3_muladds( rbb->co, ct->n, mass_total * rbb->inv_mass, rbb->co );
+ }
+}
+
+
+/*
+ * Effectors
+ */
+
+VG_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 * k_rb_delta, ra->v );
+
+ if( depth < 0.0f )
+ v3_muls( ra->v, 1.0f-(drag*k_rb_delta), ra->v );
+}
+
+/* apply a spring&dampener force to match ra(worldspace) on rigidbody, to
+ * rt(worldspace)
+ */
+VG_STATIC void rb_effect_spring_target_vector( rigidbody *rba, v3f ra, v3f rt,
+ float spring, float dampening,
+ float timestep ){
+ float d = v3_dot( rt, ra );
+ float a = acosf( vg_clampf( d, -1.0f, 1.0f ) );
+
+ v3f axis;
+ v3_cross( rt, ra, axis );
+
+ float Fs = -a * spring,
+ Fd = -v3_dot( rba->w, axis ) * dampening;
+
+ v3_muladds( rba->w, axis, (Fs+Fd) * timestep, rba->w );