+ struct skeleton_ik *ik = &skele->ik[i];
+
+ v3f v0, /* base -> target */
+ v1, /* base -> pole */
+ vaxis;
+
+ v3f co_base,
+ co_target,
+ co_pole;
+
+ v3_copy( skele->final_mtx[ik->lower][3], co_base );
+ v3_copy( skele->final_mtx[ik->target][3], co_target );
+ v3_copy( skele->final_mtx[ik->pole][3], co_pole );
+
+ v3_sub( co_target, co_base, v0 );
+ v3_sub( co_pole, co_base, v1 );
+ v3_cross( v0, v1, vaxis );
+ v3_normalize( vaxis );
+ v3_normalize( v0 );
+ v3_cross( vaxis, v0, v1 );
+
+ /* localize problem into [x:v0,y:v1] 2d plane */
+ v2f base = { v3_dot( v0, co_base ), v3_dot( v1, co_base ) },
+ end = { v3_dot( v0, co_target ), v3_dot( v1, co_target ) },
+ knee;
+
+ /* Compute angles (basic trig)*/
+ v2f delta;
+ v2_sub( end, base, delta );
+
+ float
+ l1 = v3_length( skele->bones[ik->lower].end ),
+ l2 = v3_length( skele->bones[ik->upper].end ),
+ d = vg_clampf( v2_length(delta), fabsf(l1 - l2), l1+l2-0.00001f ),
+ c = acosf( (l1*l1 + d*d - l2*l2) / (2.0f*l1*d) ),
+ rot = atan2f( delta[1], delta[0] ) + c - VG_PIf/2.0f;
+
+ knee[0] = sinf(-rot) * l1;
+ knee[1] = cosf(-rot) * l1;
+
+ m4x3_identity( skele->final_mtx[ik->lower] );
+ m4x3_identity( skele->final_mtx[ik->upper] );
+
+ /* create rotation matrix */
+ v3f co_knee;
+ v3_muladds( co_base, v0, knee[0], co_knee );
+ v3_muladds( co_knee, v1, knee[1], co_knee );
+ vg_line( co_base, co_knee, 0xff00ff00 );
+
+ m4x3f transform;
+ v3_copy( vaxis, transform[0] );
+ v3_muls( v0, knee[0], transform[1] );
+ v3_muladds( transform[1], v1, knee[1], transform[1] );
+ v3_normalize( transform[1] );
+ v3_cross( transform[0], transform[1], transform[2] );
+ v3_copy( co_base, transform[3] );
+
+ m3x3_mul( transform, ik->ia, transform );
+ m4x3_copy( transform, skele->final_mtx[ik->lower] );
+
+ /* upper/knee bone */
+ v3_copy( vaxis, transform[0] );
+ v3_sub( co_target, co_knee, transform[1] );
+ v3_normalize( transform[1] );
+ v3_cross( transform[0], transform[1], transform[2] );
+ v3_copy( co_knee, transform[3] );
+
+ m3x3_mul( transform, ik->ib, transform );
+ m4x3_copy( transform, skele->final_mtx[ik->upper] );