4 * Setup basic IK system (2d, 2 bones)
25 static void ik_basic( struct ik_basic
*ik
, m4x3f m1
, m4x3f m2
)
27 /* Localize the system into 2d */
30 vg_line( ik
->base
, ik
->pole
, 0xffffff00 );
31 vg_line( ik
->end
, ik
->pole
, 0xffffff00 );
33 v3_sub( ik
->base
, ik
->pole
, v0
);
34 v3_sub( ik
->end
, ik
->pole
, v1
);
36 v3_cross( v0
, v1
, axis
);
39 v3_cross( axis
, v0
, v1
);
42 v3_muladds( ik
->base
, axis
, 0.2f
, p0
);
43 v3_muladds( ik
->base
, v0
, 0.2f
, p1
);
44 v3_muladds( ik
->base
, v1
, 0.2f
, p2
);
45 vg_line( ik
->base
, p0
, 0xffff0000 );
46 vg_line( ik
->base
, p1
, 0xff00ff00 );
47 vg_line( ik
->base
, p2
, 0xff0000ff );
49 v2f base
= { v3_dot( v0
, ik
->base
), v3_dot( v1
, ik
->base
) },
50 end
= { v3_dot( v0
, ik
->end
), v3_dot( v1
, ik
->end
) },
55 v2_sub( end
, base
, delta
);
58 d
= vg_clampf( v2_length(delta
), fabsf(ik
->l1
- ik
->l2
),
59 ik
->l1
+ik
->l2
-0.00001f
),
60 c
= acosf( (ik
->l1
*ik
->l1
+ d
*d
- ik
->l2
*ik
->l2
) / (2.0f
*ik
->l1
*d
) ),
61 rot
= atan2f( delta
[1], delta
[0] ) + c
- VG_PIf
/2.0f
;
63 knee
[0] = sinf(-rot
) * ik
->l1
;
64 knee
[1] = cosf(-rot
) * ik
->l1
;
66 /* Project back into world coords */
68 v3_muladds( ik
->base
, v0
, knee
[0], world_knee
);
69 v3_muladds( world_knee
, v1
, knee
[1], world_knee
);
72 m4x3_lookat( m1
, ik
->base
, world_knee
, axis
);
73 m4x3_lookat( m2
, world_knee
, ik
->end
, axis
);
75 vg_line( ik
->base
, world_knee
, 0xff0000ff );
76 vg_line( world_knee
, ik
->end
, 0xff00ff40 );