ik.h

   1 #ifndef IK_H
   2 #define IK_H
   3
   4 #include "vg/vg.h"
   5
   6 /*
   7  *  Setup basic IK system (2d, 2 bones)
   8  *
   9  *  + base (m1)
  10  *   \
  11  *    \ l1
  12  *     \
  13  *      *(m2)   + pole
  14  *     /
  15  *    / l2
  16  *   +
  17  *   end
  18  */
  19 struct ik_basic
  20 {
  21    v3f base,
  22        pole,
  23        end;
  24
  25    float l1, l2;
  26 };
  27
  28 typedef enum ik_dir ik_dir;
  29 enum ik_dir
  30 {
  31    k_ikX,
  32    k_iknX,
  33    k_ikY,
  34    k_iknY,
  35    k_ikZ,
  36    k_iknZ
  37 };
  38
  39 static void ik_track_to( m4x3f m, v3f pos, v3f target, v3f axis,
  40       ik_dir fwd, ik_dir up )
  41 {
  42    v3f dir, other;
  43    v3_sub( target, pos, dir );
  44    v3_normalize( dir );
  45    v3_cross( dir, axis, other );
  46
  47    if( fwd == k_ikX && up == k_ikY )
  48    {
  49       v3_copy( axis,  m[0] );
  50       v3_copy( dir,   m[1] );
  51       v3_copy( other, m[2] );
  52    }
  53
  54    if( fwd == k_ikY && up == k_ikX )
  55    {
  56       v3_negate( axis, m[2] );
  57       v3_copy( dir,    m[1] );
  58       v3_negate( other,m[0] );
  59    }
  60
  61    if( fwd == k_ikY && up == k_iknX )
  62    {
  63       v3_negate( axis, m[2] );
  64       v3_negate( dir,    m[1] );
  65       v3_copy( other,m[0] );
  66    }
  67
  68    if( fwd == k_ikZ && up == k_ikY )
  69    {
  70       v3_copy( axis,  m[1] );
  71       v3_copy( dir,   m[2] );
  72       v3_negate( other, m[0] );
  73    }
  74
  75    if( fwd == k_iknZ && up == k_ikY )
  76    {
  77       v3_negate( axis,  m[1] );
  78       v3_negate( dir,   m[2] );
  79       v3_negate( other, m[0] );
  80    }
  81
  82    v3_copy( pos, m[3] );
  83 }
  84
  85 static void ik_basic( struct ik_basic *ik, m4x3f m1, m4x3f m2,
  86       enum ik_dir fwd, enum ik_dir up )
  87 {
  88    /* Localize the system into 2d */
  89    v3f local[3];
  90    float *v0 = local[0],
  91          *v1 = local[1],
  92          *axis = local[2];
  93
  94    vg_line( ik->base, ik->pole, 0x6fffff00 );
  95    vg_line( ik->end, ik->pole, 0x6fffff00 );
  96
  97    v3_sub( ik->base, ik->pole, v0 );
  98    v3_sub( ik->end, ik->pole, v1 );
  99
 100    v3_cross( v0, v1, axis );
 101    v3_normalize( v0 );
 102    v3_normalize( axis );
 103    v3_cross( axis, v0, v1 );
 104
 105    v3f p0, p1, p2;
 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 );
 112
 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  ) },
 115        knee;
 116
 117    /* Compute angles */
 118    v2f delta;
 119    v2_sub( end, base, delta );
 120
 121    float
 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;
 126
 127    knee[0] = sinf(-rot) * ik->l1;
 128    knee[1] = cosf(-rot) * ik->l1;
 129
 130    /* Project back into world coords */
 131    v3f world_knee;
 132    v3_muladds( ik->base, v0, knee[0], world_knee );
 133    v3_muladds( world_knee, v1, knee[1], world_knee );
 134
 135    /* Create matrices */
 136
 137    ik_track_to( m1, ik->base, world_knee, axis, fwd, up );
 138    ik_track_to( m2, world_knee, ik->end, axis, fwd, up );
 139
 140    vg_line( ik->base, world_knee, 0xa00000ff );
 141    vg_line( world_knee, ik->end, 0xa000ff40 );
 142 }
 143
 144 #endif