ik.h

   1 #include "vg/vg.h"
   2
   3 /*
   4  *  Setup basic IK system (2d, 2 bones)
   5  *
   6  *  + base (m1)
   7  *   \
   8  *    \ l1
   9  *     \
  10  *      *(m2)   + pole
  11  *     /
  12  *    / l2
  13  *   +
  14  *   end
  15  */
  16 struct ik_basic
  17 {
  18    v3f base,
  19        pole,
  20        end;
  21
  22    float l1, l2;
  23 };
  24
  25 static void ik_basic( struct ik_basic *ik, m4x3f m1, m4x3f m2 )
  26 {
  27    /* Localize the system into 2d */
  28    v3f v0, v1, axis;
  29
  30    vg_line( ik->base, ik->pole, 0xffffff00 );
  31    vg_line( ik->end, ik->pole, 0xffffff00 );
  32
  33    v3_sub( ik->base, ik->pole, v0 );
  34    v3_sub( ik->end, ik->pole, v1 );
  35
  36    v3_cross( v0, v1, axis );
  37    v3_normalize( v0 );
  38    v3_normalize( axis );
  39    v3_cross( axis, v0, v1 );
  40
  41    v3f p0, p1, p2;
  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 );
  48
  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  ) },
  51        knee;
  52
  53    /* Compute angles */
  54    v2f delta;
  55    v2_sub( end, base, delta );
  56
  57    float
  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;
  62
  63    knee[0] = sinf(-rot) * ik->l1;
  64    knee[1] = cosf(-rot) * ik->l1;
  65
  66    /* Project back into world coords */
  67    v3f world_knee;
  68    v3_muladds( ik->base, v0, knee[0], world_knee );
  69    v3_muladds( world_knee, v1, knee[1], world_knee );
  70
  71    /* Create matrices */
  72    m4x3_lookat( m1, ik->base, world_knee, axis );
  73    m4x3_lookat( m2, world_knee, ik->end, axis );
  74
  75    vg_line( ik->base, world_knee, 0xff0000ff );
  76    vg_line( world_knee, ik->end, 0xff00ff40 );
  77 }