- 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 );
+ 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_pt3( p1, 0.00025f, 0xffffffff );
+
+ if( st->tangent_violation ){
+ v3_muladds( center, st->tangent_target, size, p0 );
+
+ vg_line( center, p0, 0xff00ff00 );
+ vg_line_pt3( 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_pt3( 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 );
+ }