skeleton.h

   1 #ifndef SKELETON_H
   2 #define SKELETON_H
   3
   4 #include "model.h"
   5
   6 struct skeleton
   7 {
   8    struct skeleton_bone
   9    {
  10       v3f co, end;
  11       u32 parent;
  12
  13       /* info, not real */
  14       int deform, ik;
  15
  16       mdl_keyframe kf;
  17    }
  18    *bones;
  19    m4x3f *final_transforms;
  20
  21    struct skeleton_ik
  22    {
  23       u32 lower, upper, target, pole;
  24    }
  25    *ik;
  26
  27    struct skeleton_anim
  28    {
  29       float rate;
  30       u32 length;
  31       struct mdl_keyframe *anim_data;
  32       char name[32];
  33    }
  34    *anims;
  35
  36    u32 bone_count,
  37        ik_count,
  38        anim_count,
  39        bindable_count;  /* TODO: try to place IK last in the rig from export
  40                                  so that we dont always upload transforms for
  41                                  useless cpu IK bones. */
  42 };
  43
  44 static void skeleton_apply_frame( m4x3f transform,
  45                                   struct skeleton *skele,
  46                                   struct skeleton_anim *anim,
  47                                   float time )
  48 {
  49    float animtime = time*anim->rate;
  50
  51    u32 frame = ((u32)animtime) % anim->length,
  52        next  = (frame+1) % anim->length;
  53
  54    float t = vg_fractf( animtime );
  55
  56    mdl_keyframe *base  = anim->anim_data + (skele->bone_count-1)*frame,
  57                 *nbase = anim->anim_data + (skele->bone_count-1)*next;
  58
  59    m4x3_copy( transform, skele->final_transforms[0] );
  60
  61    for( int i=1; i<skele->bone_count; i++ )
  62    {
  63       struct skeleton_bone *sb = &skele->bones[i];
  64
  65       /* process pose */
  66       m4x3f posemtx;
  67
  68       v3f temp_delta;
  69       v3_sub( skele->bones[i].co, skele->bones[sb->parent].co, temp_delta );
  70
  71       /* pose matrix */
  72       mdl_keyframe *kf  = base+i-1,
  73                    *nkf = nbase+i-1;
  74
  75       v3f co;
  76       v4f q;
  77       v3f s;
  78
  79       v3_lerp( kf->co, nkf->co, t, co );
  80       q_nlerp( kf->q, nkf->q, t, q );
  81       v3_lerp( kf->s, nkf->s, t, s );
  82
  83       q_m3x3( q, posemtx );
  84       v3_copy( co, posemtx[3] );
  85       v3_add( temp_delta, posemtx[3], posemtx[3] );
  86
  87       /* final matrix */
  88       m4x3_mul( skele->final_transforms[ sb->parent ], posemtx,
  89                         skele->final_transforms[i] );
  90    }
  91
  92    /* armature space -> bone space matrix ( for verts ) */
  93    for( int i=1; i<skele->bone_count; i++ )
  94    {
  95       m4x3f abmtx;
  96       m3x3_identity( abmtx );
  97       v3_negate( skele->bones[i].co, abmtx[3] );
  98       m4x3_mul( skele->final_transforms[i], abmtx,
  99                 skele->final_transforms[i] );
 100    }
 101 }
 102
 103 /*
 104  * Get transformed position of bone
 105  */
 106 static void skeleton_bone_posepos( struct skeleton *skele, u32 id, v3f co )
 107 {
 108    m4x3_mulv( skele->final_transforms[id], skele->bones[id].co, co );
 109 }
 110
 111 /*
 112  * creates the reference inverse matrix for an IK bone, as it has an initial
 113  * intrisic rotation based on the direction that the IK is setup..
 114  */
 115 static void skeleton_inverse_for_ik( struct skeleton *skele,
 116                                      v3f ivaxis,
 117                                      u32 id, m4x3f inverse )
 118 {
 119    v3_copy( ivaxis, inverse[0] );
 120    v3_copy( skele->bones[id].end, inverse[1] );
 121    v3_normalize( inverse[1] );
 122    v3_cross( inverse[0], inverse[1], inverse[2] );
 123    v3_copy( skele->bones[id].co, inverse[3] );
 124    m4x3_invert_affine( inverse, inverse );
 125 }
 126
 127 /*
 128  * Apply all IK modifiers (2 bone ik reference from blender is supported)
 129  */
 130 static void skeleton_apply_ik_pass( struct skeleton *skele )
 131 {
 132    for( int i=0; i<skele->ik_count; i++ )
 133    {
 134       struct skeleton_ik *ik = &skele->ik[i];
 135
 136       v3f v0, /* base -> target */
 137           v1, /* base -> pole */
 138           vaxis;
 139
 140       v3f co_base,
 141           co_target,
 142           co_pole;
 143
 144       skeleton_bone_posepos( skele, ik->lower, co_base );
 145       skeleton_bone_posepos( skele, ik->target, co_target );
 146       skeleton_bone_posepos( skele, ik->pole, co_pole );
 147
 148       v3_sub( co_target, co_base, v0 );
 149       v3_sub( co_pole, co_base, v1 );
 150       v3_cross( v0, v1, vaxis );
 151       v3_normalize( vaxis );
 152       v3_normalize( v0 );
 153       v3_cross( vaxis, v0, v1 );
 154
 155       /* localize problem into [x:v0,y:v1] 2d plane */
 156       v2f base = { v3_dot( v0, co_base   ), v3_dot( v1, co_base   ) },
 157           end  = { v3_dot( v0, co_target ), v3_dot( v1, co_target ) },
 158           knee;
 159
 160       /* Compute angles (basic trig)*/
 161       v2f delta;
 162       v2_sub( end, base, delta );
 163
 164       float
 165          l1 = v3_length( skele->bones[ik->lower].end ),
 166          l2 = v3_length( skele->bones[ik->upper].end ),
 167          d = vg_clampf( v2_length(delta), fabsf(l1 - l2), l1+l2-0.00001f ),
 168          c = acosf( (l1*l1 + d*d - l2*l2) / (2.0f*l1*d) ),
 169          rot = atan2f( delta[1], delta[0] ) + c - VG_PIf/2.0f;
 170
 171       knee[0] = sinf(-rot) * l1;
 172       knee[1] = cosf(-rot) * l1;
 173
 174       m4x3_identity( skele->final_transforms[ik->lower] );
 175       m4x3_identity( skele->final_transforms[ik->upper] );
 176
 177       /* inverse matrix axis '(^axis,^bone,...)[base] */
 178       m4x3f inverse;
 179       v3f iv0, iv1, ivaxis;
 180       v3_sub( skele->bones[ik->target].co, skele->bones[ik->lower].co, iv0 );
 181       v3_sub( skele->bones[ik->pole].co, skele->bones[ik->lower].co, iv1 );
 182       v3_cross( iv0, iv1, ivaxis );
 183       v3_normalize( ivaxis );
 184
 185       skeleton_inverse_for_ik( skele, ivaxis, ik->lower, inverse );
 186
 187       /* create rotation matrix */
 188       v3f co_knee;
 189       v3_muladds( co_base, v0, knee[0], co_knee );
 190       v3_muladds( co_knee, v1, knee[1], co_knee );
 191       vg_line( co_base, co_knee, 0xff00ff00 );
 192
 193       m4x3f transform;
 194       v3_copy( vaxis, transform[0] );
 195       v3_muls( v0, knee[0], transform[1] );
 196       v3_muladds( transform[1], v1, knee[1], transform[1] );
 197       v3_normalize( transform[1] );
 198       v3_cross( transform[0], transform[1], transform[2] );
 199       v3_copy( co_base, transform[3] );
 200
 201       m4x3_mul( transform, inverse, skele->final_transforms[ik->lower] );
 202
 203       /* 'upper' or knee bone */
 204       skeleton_inverse_for_ik( skele, ivaxis, ik->upper, inverse );
 205
 206       v3_copy( vaxis, transform[0] );
 207       v3_sub( co_target, co_knee, transform[1] );
 208       v3_normalize( transform[1] );
 209       v3_cross( transform[0], transform[1], transform[2] );
 210       v3_copy( co_knee, transform[3] );
 211
 212       m4x3_mul( transform, inverse, skele->final_transforms[ik->upper] );
 213    }
 214 }
 215
 216 static struct skeleton_anim *skeleton_get_anim( struct skeleton *skele,
 217                                                 const char *name )
 218 {
 219    for( int i=0; i<skele->anim_count; i++ )
 220    {
 221       struct skeleton_anim *anim = &skele->anims[i];
 222
 223       if( !strcmp( anim->name, name ) )
 224          return anim;
 225    }
 226
 227    return NULL;
 228 }
 229
 230 /* Setup an animated skeleton from model */
 231 static int skeleton_setup( struct skeleton *skele, mdl_header *mdl )
 232 {
 233    u32 bone_count = 1, skeleton_root = 0, ik_count = 0;
 234    skele->bone_count = 0;
 235    skele->bones = NULL;
 236    skele->final_transforms = NULL;
 237    skele->anims = NULL;
 238
 239    struct classtype_skeleton *inf = NULL;
 240
 241    for( u32 i=0; i<mdl->node_count; i++ )
 242    {
 243       mdl_node *pnode = mdl_node_from_id( mdl, i );
 244
 245       if( pnode->classtype == k_classtype_skeleton )
 246       {
 247          inf = mdl_get_entdata( mdl, pnode );
 248          if( skele->bone_count )
 249          {
 250             vg_error( "Multiple skeletons in model file\n" );
 251             goto error_dealloc;
 252          }
 253
 254          skele->bone_count = inf->channels;
 255          skele->ik_count = inf->ik_count;
 256          skele->bones = malloc(sizeof(struct skeleton_bone)*skele->bone_count);
 257          skele->ik = malloc(sizeof(struct skeleton_ik)*skele->ik_count);
 258          skeleton_root = i;
 259       }
 260       else if( skele->bone_count )
 261       {
 262          int is_ik   =  pnode->classtype == k_classtype_ik_bone,
 263              is_bone = (pnode->classtype == k_classtype_bone) || is_ik;
 264
 265          if( is_bone )
 266          {
 267             if( bone_count == skele->bone_count )
 268             {
 269                vg_error( "too many bones (%u/%u) @%s!\n",
 270                            bone_count, skele->bone_count,
 271                            mdl_pstr( mdl, pnode->pstr_name ));
 272
 273                goto error_dealloc;
 274             }
 275
 276             struct skeleton_bone *sb = &skele->bones[bone_count];
 277
 278             v3_copy( pnode->co, sb->co );
 279             v3_copy( pnode->s, sb->end );
 280             sb->parent = pnode->parent-skeleton_root;
 281
 282             if( is_ik )
 283             {
 284                struct classtype_ik_bone *ik_inf = mdl_get_entdata( mdl, pnode );
 285                sb->deform = ik_inf->deform;
 286                sb->ik = 1; /* TODO: place into new IK array */
 287                skele->bones[ sb->parent ].ik = 1;
 288
 289                if( ik_count == skele->ik_count )
 290                {
 291                   vg_error( "Too many ik bones, corrupt model file\n" );
 292                   goto error_dealloc;
 293                }
 294
 295                struct skeleton_ik *ik = &skele->ik[ ik_count ++ ];
 296                ik->upper = bone_count;
 297                ik->lower = sb->parent;
 298                ik->target = ik_inf->target;
 299                ik->pole = ik_inf->pole;
 300             }
 301             else
 302             {
 303                struct classtype_bone *bone_inf = mdl_get_entdata( mdl, pnode );
 304                sb->deform = bone_inf->deform;
 305                sb->ik = 0;
 306             }
 307
 308             bone_count ++;
 309          }
 310          else
 311          {
 312             break;
 313          }
 314       }
 315    }
 316
 317    if( !inf )
 318    {
 319       vg_error( "No skeleton in model\n" );
 320       return 0;
 321    }
 322
 323    if( bone_count != skele->bone_count )
 324    {
 325       vg_error( "Loaded %u bones out of %u\n", bone_count, skele->bone_count );
 326       goto error_dealloc;
 327    }
 328
 329    if( ik_count != skele->ik_count )
 330    {
 331       vg_error( "Loaded %u ik bones out of %u\n", ik_count, skele->ik_count );
 332       goto error_dealloc;
 333    }
 334
 335    /* fill in implicit root bone */
 336    v3_zero( skele->bones[0].co );
 337    v3_copy( (v3f){0.0f,1.0f,0.0f}, skele->bones[0].end );
 338    skele->bones[0].parent = 0xffffffff;
 339
 340    skele->final_transforms = malloc( sizeof(m4x3f) * skele->bone_count );
 341    skele->anim_count = inf->anim_count;
 342    skele->anims = malloc( sizeof(struct skeleton_anim) * inf->anim_count);
 343
 344    for( int i=0; i<inf->anim_count; i++ )
 345    {
 346       mdl_animation *anim =
 347          mdl_animation_from_id( mdl, inf->anim_start+i );
 348
 349       skele->anims[i].rate = anim->rate;
 350       skele->anims[i].length = anim->length;
 351       strncpy( skele->anims[i].name, mdl_pstr(mdl, anim->pstr_name), 32 );
 352
 353       u32 total_keyframes = (skele->bone_count-1)*anim->length;
 354       size_t block_size = sizeof(mdl_keyframe) * total_keyframes;
 355       mdl_keyframe *dst = malloc( block_size );
 356
 357       skele->anims[i].anim_data = dst;
 358       memcpy( dst, mdl_get_animdata( mdl, anim ), block_size );
 359    }
 360
 361    vg_success( "Loaded skeleton with %u bones\n", skele->bone_count );
 362    return 1;
 363
 364 error_dealloc:
 365    free( skele->bones );
 366    free( skele->ik );
 367    return 0;
 368 }
 369
 370 static void skeleton_debug( struct skeleton *skele )
 371 {
 372    for( int i=0; i<skele->bone_count; i ++ )
 373    {
 374       struct skeleton_bone *sb = &skele->bones[i];
 375
 376       v3f p0, p1;
 377       v3_copy( sb->co, p0 );
 378       v3_add( p0, sb->end, p1 );
 379       //vg_line( p0, p1, 0xffffffff );
 380
 381       m4x3_mulv( skele->final_transforms[i], p0, p0 );
 382       m4x3_mulv( skele->final_transforms[i], p1, p1 );
 383
 384       if( sb->deform )
 385       {
 386          if( sb->ik )
 387          {
 388             vg_line( p0, p1, 0xff0000ff );
 389          }
 390          else
 391          {
 392             vg_line( p0, p1, 0xffcccccc );
 393          }
 394       }
 395       else
 396          vg_line( p0, p1, 0xff00ffff );
 397    }
 398 }
 399
 400 #endif /* SKELETON_H */