-/*
- * -----------------------------------------------------------------------------
- * Constraints
- * -----------------------------------------------------------------------------
- */
-
-static void rb_debug_position_constraints( rb_constr_pos *buffer, int len ){
- for( int i=0; i<len; i++ ){
- rb_constr_pos *constr = &buffer[i];
- rigidbody *rba = constr->rba, *rbb = constr->rbb;
-
- v3f wca, wcb;
- m3x3_mulv( rba->to_world, constr->lca, wca );
- m3x3_mulv( rbb->to_world, constr->lcb, wcb );
-
- v3f p0, p1;
- v3_add( wca, rba->co, p0 );
- v3_add( wcb, rbb->co, p1 );
- vg_line_point( p0, 0.0025f, 0xff000000 );
- vg_line_point( p1, 0.0025f, 0xffffffff );
- vg_line2( p0, p1, 0xff000000, 0xffffffff );
- }
-}
-
-static void rb_presolve_swingtwist_constraints( rb_constr_swingtwist *buf,
- int len ){
- for( int i=0; i<len; i++ ){
- rb_constr_swingtwist *st = &buf[ i ];
-
- v3f vx, vy, va, vxb, 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 );
-
- /* Constraint violated ? */
- float fx = v3_dot( vx, va ), /* projection world */
- fy = v3_dot( vy, va ),
- fn = v3_dot( va, axis ),
-
- rx = st->conevx[3], /* elipse radii */
- ry = st->conevy[3],
-
- lx = fx/rx, /* projection local (fn==lz) */
- ly = fy/ry;
-
- st->tangent_violation = ((lx*lx + ly*ly) > fn*fn) || (fn <= 0.0f);
- if( st->tangent_violation ){
- /* Calculate a good position and the axis to solve on */
- v2f closest, tangent,
- p = { fx/fabsf(fn), fy/fabsf(fn) };
-
- closest_point_elipse( p, (v2f){rx,ry}, closest );
- tangent[0] = -closest[1] / (ry*ry);
- tangent[1] = closest[0] / (rx*rx);
- v2_normalize( tangent );
-
- v3f v0, v1;
- v3_muladds( axis, vx, closest[0], v0 );
- v3_muladds( v0, vy, closest[1], v0 );
- v3_normalize( v0 );
-
- v3_muls( vx, tangent[0], v1 );
- v3_muladds( v1, vy, tangent[1], v1 );
-
- v3_copy( v0, st->tangent_target );
- v3_copy( v1, st->tangent_axis );
-
- /* calculate mass */
- v3f aIw, bIw;
- m3x3_mulv( st->rba->iIw, st->tangent_axis, aIw );
- m3x3_mulv( st->rbb->iIw, st->tangent_axis, bIw );
- st->tangent_mass = 1.0f / (v3_dot( st->tangent_axis, aIw ) +
- v3_dot( st->tangent_axis, bIw ));
-
- float angle = v3_dot( va, st->tangent_target );
- }
-
- v3f refaxis;
- v3_cross( vy, va, refaxis ); /* our default rotation */
- v3_normalize( refaxis );
-
- float angle = v3_dot( refaxis, vxb );
- st->axis_violation = fabsf(angle) < st->conet;
-
- if( st->axis_violation ){
- v3f dir_test;
- v3_cross( refaxis, vxb, dir_test );
-
- if( v3_dot(dir_test, va) < 0.0f )
- st->axis_violation = -st->axis_violation;
-
- float newang = (float)st->axis_violation * acosf(st->conet-0.0001f);
-
- v3f refaxis_up;
- v3_cross( va, refaxis, refaxis_up );
- v3_muls( refaxis_up, sinf(newang), st->axis_target );
- v3_muladds( st->axis_target, refaxis, -cosf(newang), st->axis_target );
-
- /* calculate mass */
- v3_copy( va, st->axis );
- v3f aIw, bIw;
- m3x3_mulv( st->rba->iIw, st->axis, aIw );
- m3x3_mulv( st->rbb->iIw, st->axis, bIw );
- st->axis_mass = 1.0f / (v3_dot( st->axis, aIw ) +
- v3_dot( st->axis, bIw ));
- }
- }
-}
-
-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 );
- }
-}
-