7 * Setup basic IK system (2d, 2 bones)
28 typedef enum ik_dir ik_dir
;
39 static void ik_track_to( m4x3f m
, v3f pos
, v3f target
, v3f axis
,
40 ik_dir fwd
, ik_dir up
)
43 v3_sub( target
, pos
, dir
);
45 v3_cross( dir
, axis
, other
);
47 if( fwd
== k_ikX
&& up
== k_ikY
)
49 v3_copy( axis
, m
[0] );
51 v3_copy( other
, m
[2] );
54 if( fwd
== k_ikY
&& up
== k_ikX
)
56 v3_negate( axis
, m
[2] );
58 v3_negate( other
,m
[0] );
61 if( fwd
== k_ikY
&& up
== k_iknX
)
63 v3_negate( axis
, m
[2] );
64 v3_negate( dir
, m
[1] );
65 v3_copy( other
,m
[0] );
68 if( fwd
== k_ikZ
&& up
== k_ikY
)
70 v3_copy( axis
, m
[1] );
72 v3_negate( other
, m
[0] );
75 if( fwd
== k_iknZ
&& up
== k_ikY
)
77 v3_negate( axis
, m
[1] );
78 v3_negate( dir
, m
[2] );
79 v3_negate( other
, m
[0] );
85 static void ik_basic( struct ik_basic
*ik
, m4x3f m1
, m4x3f m2
,
86 enum ik_dir fwd
, enum ik_dir up
)
88 /* Localize the system into 2d */
94 vg_line( ik
->base
, ik
->pole
, 0x6fffff00 );
95 vg_line( ik
->end
, ik
->pole
, 0x6fffff00 );
97 v3_sub( ik
->base
, ik
->pole
, v0
);
98 v3_sub( ik
->end
, ik
->pole
, v1
);
100 v3_cross( v0
, v1
, axis
);
102 v3_normalize( axis
);
103 v3_cross( axis
, v0
, v1
);
106 v3_muladds( ik
->base
, axis
, 0.05f
, p0
);
107 v3_muladds( ik
->base
, v0
, 0.05f
, p1
);
108 v3_muladds( ik
->base
, v1
, 0.05f
, p2
);
109 vg_line( ik
->base
, p0
, 0xffff0000 );
110 vg_line( ik
->base
, p1
, 0xff00ff00 );
111 vg_line( ik
->base
, p2
, 0xff0000ff );
113 v2f base
= { v3_dot( v0
, ik
->base
), v3_dot( v1
, ik
->base
) },
114 end
= { v3_dot( v0
, ik
->end
), v3_dot( v1
, ik
->end
) },
119 v2_sub( end
, base
, delta
);
122 d
= vg_clampf( v2_length(delta
), fabsf(ik
->l1
- ik
->l2
),
123 ik
->l1
+ik
->l2
-0.00001f
),
124 c
= acosf( (ik
->l1
*ik
->l1
+ d
*d
- ik
->l2
*ik
->l2
) / (2.0f
*ik
->l1
*d
) ),
125 rot
= atan2f( delta
[1], delta
[0] ) + c
- VG_PIf
/2.0f
;
127 knee
[0] = sinf(-rot
) * ik
->l1
;
128 knee
[1] = cosf(-rot
) * ik
->l1
;
130 /* Project back into world coords */
132 v3_muladds( ik
->base
, v0
, knee
[0], world_knee
);
133 v3_muladds( world_knee
, v1
, knee
[1], world_knee
);
135 /* Create matrices */
137 ik_track_to( m1
, ik
->base
, world_knee
, axis
, fwd
, up
);
138 ik_track_to( m2
, world_knee
, ik
->end
, axis
, fwd
, up
);
140 vg_line( ik
->base
, world_knee
, 0xa00000ff );
141 vg_line( world_knee
, ik
->end
, 0xa000ff40 );