skeleton.h

   1 /*
   2  * Copyright (C) Mount0 Software, Harry Godden - All Rights Reserved
   3  */
   4
   5 #ifndef SKELETON_H
   6 #define SKELETON_H
   7
   8 #include "model.h"
   9
  10 struct skeleton
  11 {
  12    struct skeleton_bone
  13    {
  14       v3f co, end;
  15       u32 parent;
  16
  17       int deform, ik;
  18       int defer;
  19
  20       mdl_keyframe kf;
  21
  22       u32 orig_node;
  23
  24       int collider;
  25       boxf hitbox;
  26
  27       char name[16];
  28    }
  29    *bones;
  30    m4x3f *final_mtx;
  31
  32    struct skeleton_ik
  33    {
  34       u32 lower, upper, target, pole;
  35       m3x3f ia, ib;
  36    }
  37    *ik;
  38
  39    struct skeleton_anim
  40    {
  41       float rate;
  42       u32 length;
  43       struct mdl_keyframe *anim_data;
  44       char name[32];
  45    }
  46    *anims;
  47
  48    u32 bone_count,
  49        ik_count,
  50        collider_count,
  51        anim_count,
  52        bindable_count;  /* TODO: try to place IK last in the rig from export
  53                                  so that we dont always upload transforms for
  54                                  useless cpu IK bones. */
  55 };
  56
  57 static u32 skeleton_bone_id( struct skeleton *skele, const char *name )
  58 {
  59    for( u32 i=0; i<skele->bone_count; i++ )
  60    {
  61       if( !strcmp( skele->bones[i].name, name ))
  62          return i;
  63    }
  64    return 0;
  65 }
  66
  67 static void keyframe_copy_pose( mdl_keyframe *kfa, mdl_keyframe *kfb, int num )
  68 {
  69    for( int i=0; i<num; i++ )
  70       kfb[i] = kfa[i];
  71 }
  72
  73 /*
  74  * Lerp between two sets of keyframes and store in dest. Rotations use Nlerp.
  75  */
  76 static void keyframe_lerp_pose( mdl_keyframe *kfa, mdl_keyframe *kfb, float t,
  77                                 mdl_keyframe *kfd, int count )
  78 {
  79    if( t <= 0.01f )
  80    {
  81       keyframe_copy_pose( kfa, kfd, count );
  82       return;
  83    }
  84    else if( t >= 0.99f )
  85    {
  86       keyframe_copy_pose( kfb, kfd, count );
  87       return;
  88    }
  89
  90    for( int i=0; i<count; i++ )
  91    {
  92       v3_lerp( kfa[i].co, kfb[i].co, t, kfd[i].co );
  93       q_nlerp( kfa[i].q,  kfb[i].q,  t, kfd[i].q );
  94       v3_lerp( kfa[i].s,  kfb[i].s,  t, kfd[i].s );
  95    }
  96 }
  97
  98 static void skeleton_lerp_pose( struct skeleton *skele,
  99                                 mdl_keyframe *kfa, mdl_keyframe *kfb, float t,
 100                                 mdl_keyframe *kfd )
 101 {
 102    keyframe_lerp_pose( kfa, kfb, t, kfd, skele->bone_count-1 );
 103 }
 104
 105 /*
 106  * Sample animation between 2 closest frames using time value. Output is a
 107  * keyframe buffer that is allocated with an appropriate size
 108  */
 109 static void skeleton_sample_anim( struct skeleton *skele,
 110                                   struct skeleton_anim *anim,
 111                                   float time,
 112                                   mdl_keyframe *output )
 113 {
 114    float animtime = time*anim->rate;
 115
 116    u32 frame = ((u32)animtime) % anim->length,
 117        next  = (frame+1) % anim->length;
 118
 119    float t = vg_fractf( animtime );
 120
 121    mdl_keyframe *base  = anim->anim_data + (skele->bone_count-1)*frame,
 122                 *nbase = anim->anim_data + (skele->bone_count-1)*next;
 123
 124    skeleton_lerp_pose( skele, base, nbase, t, output );
 125 }
 126
 127 static int skeleton_sample_anim_clamped( struct skeleton *skele,
 128                                          struct skeleton_anim *anim,
 129                                          float time,
 130                                          mdl_keyframe *output )
 131 {
 132    float end = (float)(anim->length-1) / anim->rate;
 133    skeleton_sample_anim( skele, anim, vg_minf( end, time ), output );
 134
 135    if( time > end )
 136       return 0;
 137    else
 138       return 1;
 139 }
 140
 141 typedef enum anim_apply
 142 {
 143    k_anim_apply_always,
 144    k_anim_apply_defer_ik,
 145    k_anim_apply_deffered_only
 146 }
 147 anim_apply;
 148
 149 static int should_apply_bone( struct skeleton *skele, u32 id, anim_apply type )
 150 {
 151    struct skeleton_bone *sb = &skele->bones[ id ],
 152                         *sp = &skele->bones[ sb->parent ];
 153
 154    if( type == k_anim_apply_defer_ik )
 155    {
 156       if( (sp->ik && !sb->ik) || sp->defer )
 157       {
 158          sb->defer = 1;
 159          return 0;
 160       }
 161       else
 162       {
 163          sb->defer = 0;
 164          return 1;
 165       }
 166    }
 167    else if( type == k_anim_apply_deffered_only )
 168    {
 169       if( sb->defer )
 170          return 1;
 171       else
 172          return 0;
 173    }
 174
 175    return 1;
 176 }
 177
 178 /*
 179  * Apply block of keyframes to skeletons final pose
 180  */
 181 static void skeleton_apply_pose( struct skeleton *skele, mdl_keyframe *pose,
 182                                  anim_apply passtype )
 183 {
 184    m4x3_identity( skele->final_mtx[0] );
 185    skele->bones[0].defer = 0;
 186    skele->bones[0].ik = 0;
 187
 188    for( int i=1; i<skele->bone_count; i++ )
 189    {
 190       struct skeleton_bone *sb = &skele->bones[i],
 191                            *sp = &skele->bones[ sb->parent ];
 192
 193       if( !should_apply_bone( skele, i, passtype ) )
 194          continue;
 195
 196       sb->defer = 0;
 197
 198       /* process pose */
 199       m4x3f posemtx;
 200
 201       v3f temp_delta;
 202       v3_sub( skele->bones[i].co, skele->bones[sb->parent].co, temp_delta );
 203
 204       /* pose matrix */
 205       mdl_keyframe *kf = &pose[i-1];
 206       q_m3x3( kf->q, posemtx );
 207       v3_copy( kf->co, posemtx[3] );
 208       v3_add( temp_delta, posemtx[3], posemtx[3] );
 209
 210       /* final matrix */
 211       m4x3_mul( skele->final_mtx[ sb->parent ], posemtx, skele->final_mtx[i] );
 212    }
 213 }
 214
 215 /*
 216  * creates the reference inverse matrix for an IK bone, as it has an initial
 217  * intrisic rotation based on the direction that the IK is setup..
 218  */
 219 static void skeleton_inverse_for_ik( struct skeleton *skele,
 220                                      v3f ivaxis,
 221                                      u32 id, m3x3f inverse )
 222 {
 223    v3_copy( ivaxis, inverse[0] );
 224    v3_copy( skele->bones[id].end, inverse[1] );
 225    v3_normalize( inverse[1] );
 226    v3_cross( inverse[0], inverse[1], inverse[2] );
 227    m3x3_transpose( inverse, inverse );
 228 }
 229
 230 /*
 231  * Creates inverse rotation matrices which the IK system uses.
 232  */
 233 static void skeleton_create_inverses( struct skeleton *skele )
 234 {
 235    /* IK: inverse 'plane-bone space' axis '(^axis,^bone,...)[base] */
 236    for( int i=0; i<skele->ik_count; i++ )
 237    {
 238       struct skeleton_ik *ik = &skele->ik[i];
 239
 240       m4x3f inverse;
 241       v3f iv0, iv1, ivaxis;
 242       v3_sub( skele->bones[ik->target].co, skele->bones[ik->lower].co, iv0 );
 243       v3_sub( skele->bones[ik->pole].co, skele->bones[ik->lower].co, iv1 );
 244       v3_cross( iv0, iv1, ivaxis );
 245       v3_normalize( ivaxis );
 246
 247       skeleton_inverse_for_ik( skele, ivaxis, ik->lower, ik->ia );
 248       skeleton_inverse_for_ik( skele, ivaxis, ik->upper, ik->ib );
 249    }
 250 }
 251
 252 /*
 253  * Apply a model matrix to all bones, should be done last
 254  */
 255 static void skeleton_apply_transform( struct skeleton *skele, m4x3f transform )
 256 {
 257    for( int i=0; i<skele->bone_count; i++ )
 258    {
 259       struct skeleton_bone *sb = &skele->bones[i];
 260       m4x3_mul( transform, skele->final_mtx[i], skele->final_mtx[i] );
 261    }
 262 }
 263
 264 /*
 265  * Apply an inverse matrix to all bones which maps vertices from bind space into
 266  * bone relative positions
 267  */
 268 static void skeleton_apply_inverses( struct skeleton *skele )
 269 {
 270    for( int i=0; i<skele->bone_count; i++ )
 271    {
 272       struct skeleton_bone *sb = &skele->bones[i];
 273       m4x3f inverse;
 274       m3x3_identity( inverse );
 275       v3_negate( sb->co, inverse[3] );
 276
 277       m4x3_mul( skele->final_mtx[i], inverse, skele->final_mtx[i] );
 278    }
 279 }
 280
 281 /*
 282  * Apply all IK modifiers (2 bone ik reference from blender is supported)
 283  */
 284 static void skeleton_apply_ik_pass( struct skeleton *skele )
 285 {
 286    for( int i=0; i<skele->ik_count; i++ )
 287    {
 288       struct skeleton_ik *ik = &skele->ik[i];
 289
 290       v3f v0, /* base -> target */
 291           v1, /* base -> pole */
 292           vaxis;
 293
 294       v3f co_base,
 295           co_target,
 296           co_pole;
 297
 298       v3_copy( skele->final_mtx[ik->lower][3], co_base );
 299       v3_copy( skele->final_mtx[ik->target][3], co_target );
 300       v3_copy( skele->final_mtx[ik->pole][3], co_pole );
 301
 302       v3_sub( co_target, co_base, v0 );
 303       v3_sub( co_pole, co_base, v1 );
 304       v3_cross( v0, v1, vaxis );
 305       v3_normalize( vaxis );
 306       v3_normalize( v0 );
 307       v3_cross( vaxis, v0, v1 );
 308
 309       /* localize problem into [x:v0,y:v1] 2d plane */
 310       v2f base = { v3_dot( v0, co_base   ), v3_dot( v1, co_base   ) },
 311           end  = { v3_dot( v0, co_target ), v3_dot( v1, co_target ) },
 312           knee;
 313
 314       /* Compute angles (basic trig)*/
 315       v2f delta;
 316       v2_sub( end, base, delta );
 317
 318       float
 319          l1 = v3_length( skele->bones[ik->lower].end ),
 320          l2 = v3_length( skele->bones[ik->upper].end ),
 321          d = vg_clampf( v2_length(delta), fabsf(l1 - l2), l1+l2-0.00001f ),
 322          c = acosf( (l1*l1 + d*d - l2*l2) / (2.0f*l1*d) ),
 323          rot = atan2f( delta[1], delta[0] ) + c - VG_PIf/2.0f;
 324
 325       knee[0] = sinf(-rot) * l1;
 326       knee[1] = cosf(-rot) * l1;
 327
 328       m4x3_identity( skele->final_mtx[ik->lower] );
 329       m4x3_identity( skele->final_mtx[ik->upper] );
 330
 331       /* create rotation matrix */
 332       v3f co_knee;
 333       v3_muladds( co_base, v0, knee[0], co_knee );
 334       v3_muladds( co_knee, v1, knee[1], co_knee );
 335       vg_line( co_base, co_knee, 0xff00ff00 );
 336
 337       m4x3f transform;
 338       v3_copy( vaxis, transform[0] );
 339       v3_muls( v0, knee[0], transform[1] );
 340       v3_muladds( transform[1], v1, knee[1], transform[1] );
 341       v3_normalize( transform[1] );
 342       v3_cross( transform[0], transform[1], transform[2] );
 343       v3_copy( co_base, transform[3] );
 344
 345       m3x3_mul( transform, ik->ia, transform );
 346       m4x3_copy( transform, skele->final_mtx[ik->lower] );
 347
 348       /* upper/knee bone */
 349       v3_copy( vaxis, transform[0] );
 350       v3_sub( co_target, co_knee, transform[1] );
 351       v3_normalize( transform[1] );
 352       v3_cross( transform[0], transform[1], transform[2] );
 353       v3_copy( co_knee, transform[3] );
 354
 355       m3x3_mul( transform, ik->ib, transform );
 356       m4x3_copy( transform, skele->final_mtx[ik->upper] );
 357    }
 358 }
 359
 360 /*
 361  * Applies the typical operations that you want for an IK rig:
 362  *    Pose, IK, Pose(deferred), Inverses, Transform
 363  */
 364 static void skeleton_apply_standard( struct skeleton *skele, mdl_keyframe *pose,
 365                                      m4x3f transform )
 366 {
 367    skeleton_apply_pose( skele, pose, k_anim_apply_defer_ik );
 368    skeleton_apply_ik_pass( skele );
 369    skeleton_apply_pose( skele, pose, k_anim_apply_deffered_only );
 370    skeleton_apply_inverses( skele );
 371    skeleton_apply_transform( skele, transform );
 372 }
 373
 374 /*
 375  * Get an animation by name
 376  */
 377 static struct skeleton_anim *skeleton_get_anim( struct skeleton *skele,
 378                                                 const char *name )
 379 {
 380    for( int i=0; i<skele->anim_count; i++ )
 381    {
 382       struct skeleton_anim *anim = &skele->anims[i];
 383
 384       if( !strcmp( anim->name, name ) )
 385          return anim;
 386    }
 387
 388    return NULL;
 389 }
 390
 391 /* Setup an animated skeleton from model */
 392 static int skeleton_setup( struct skeleton *skele, mdl_header *mdl )
 393 {
 394    u32 bone_count = 1, skeleton_root = 0, ik_count = 0, collider_count = 0;
 395    skele->bone_count = 0;
 396    skele->bones = NULL;
 397    skele->final_mtx = NULL;
 398    skele->anims = NULL;
 399
 400    struct classtype_skeleton *inf = NULL;
 401
 402    for( u32 i=0; i<mdl->node_count; i++ )
 403    {
 404       mdl_node *pnode = mdl_node_from_id( mdl, i );
 405
 406       if( pnode->classtype == k_classtype_skeleton )
 407       {
 408          inf = mdl_get_entdata( mdl, pnode );
 409          if( skele->bone_count )
 410          {
 411             vg_error( "Multiple skeletons in model file\n" );
 412             goto error_dealloc;
 413          }
 414
 415          skele->bone_count = inf->channels;
 416          skele->ik_count = inf->ik_count;
 417          skele->collider_count = inf->collider_count;
 418          skele->bones = malloc(sizeof(struct skeleton_bone)*skele->bone_count);
 419          skele->ik = malloc(sizeof(struct skeleton_ik)*skele->ik_count);
 420          skeleton_root = i;
 421       }
 422       else if( skele->bone_count )
 423       {
 424          int is_bone = pnode->classtype == k_classtype_bone;
 425
 426          if( is_bone )
 427          {
 428             if( bone_count == skele->bone_count )
 429             {
 430                vg_error( "too many bones (%u/%u) @%s!\n",
 431                            bone_count, skele->bone_count,
 432                            mdl_pstr( mdl, pnode->pstr_name ));
 433
 434                goto error_dealloc;
 435             }
 436
 437             struct skeleton_bone *sb = &skele->bones[bone_count];
 438             struct classtype_bone *bone_inf = mdl_get_entdata( mdl, pnode );
 439             int is_ik = bone_inf->ik_target;
 440
 441             v3_copy( pnode->co, sb->co );
 442             v3_copy( pnode->s, sb->end );
 443             sb->parent = pnode->parent-skeleton_root;
 444             strncpy( sb->name, mdl_pstr(mdl,pnode->pstr_name), 15 );
 445             sb->deform = bone_inf->deform;
 446
 447             if( is_ik )
 448             {
 449                sb->ik = 1; /* TODO: place into new IK array */
 450                skele->bones[ sb->parent ].ik = 1;
 451
 452                if( ik_count == skele->ik_count )
 453                {
 454                   vg_error( "Too many ik bones, corrupt model file\n" );
 455                   goto error_dealloc;
 456                }
 457
 458                struct skeleton_ik *ik = &skele->ik[ ik_count ++ ];
 459                ik->upper = bone_count;
 460                ik->lower = sb->parent;
 461                ik->target = bone_inf->ik_target;
 462                ik->pole = bone_inf->ik_pole;
 463             }
 464             else
 465             {
 466                sb->ik = 0;
 467             }
 468
 469             sb->collider = bone_inf->collider;
 470             sb->orig_node = i;
 471             box_copy( bone_inf->hitbox, sb->hitbox );
 472
 473             if( bone_inf->collider )
 474             {
 475                if( collider_count == skele->collider_count )
 476                {
 477                   vg_error( "Too many collider bones\n" );
 478                   goto error_dealloc;
 479                }
 480
 481                collider_count ++;
 482             }
 483
 484             bone_count ++;
 485          }
 486          else
 487          {
 488             break;
 489          }
 490       }
 491    }
 492
 493    if( !inf )
 494    {
 495       vg_error( "No skeleton in model\n" );
 496       return 0;
 497    }
 498
 499    if( collider_count != skele->collider_count )
 500    {
 501       vg_error( "Loaded %u colliders out of %u\n", collider_count,
 502                                                       skele->collider_count );
 503       goto error_dealloc;
 504    }
 505
 506    if( bone_count != skele->bone_count )
 507    {
 508       vg_error( "Loaded %u bones out of %u\n", bone_count, skele->bone_count );
 509       goto error_dealloc;
 510    }
 511
 512    if( ik_count != skele->ik_count )
 513    {
 514       vg_error( "Loaded %u ik bones out of %u\n", ik_count, skele->ik_count );
 515       goto error_dealloc;
 516    }
 517
 518    /* fill in implicit root bone */
 519    v3_zero( skele->bones[0].co );
 520    v3_copy( (v3f){0.0f,1.0f,0.0f}, skele->bones[0].end );
 521    skele->bones[0].parent = 0xffffffff;
 522    skele->bones[0].collider = 0;
 523
 524    skele->final_mtx = malloc( sizeof(m4x3f) * skele->bone_count );
 525    skele->anim_count = inf->anim_count;
 526    skele->anims = malloc( sizeof(struct skeleton_anim) * inf->anim_count);
 527
 528    for( int i=0; i<inf->anim_count; i++ )
 529    {
 530       mdl_animation *anim =
 531          mdl_animation_from_id( mdl, inf->anim_start+i );
 532
 533       skele->anims[i].rate = anim->rate;
 534       skele->anims[i].length = anim->length;
 535       strncpy( skele->anims[i].name, mdl_pstr(mdl, anim->pstr_name), 32 );
 536
 537       u32 total_keyframes = (skele->bone_count-1)*anim->length;
 538       size_t block_size = sizeof(mdl_keyframe) * total_keyframes;
 539       mdl_keyframe *dst = malloc( block_size );
 540
 541       skele->anims[i].anim_data = dst;
 542       memcpy( dst, mdl_get_animdata( mdl, anim ), block_size );
 543    }
 544
 545    skeleton_create_inverses( skele );
 546    vg_success( "Loaded skeleton with %u bones\n", skele->bone_count );
 547    vg_success( "                     %u colliders\n", skele->collider_count );
 548    return 1;
 549
 550 error_dealloc:
 551    free( skele->bones );
 552    free( skele->ik );
 553    return 0;
 554 }
 555
 556 static void skeleton_debug( struct skeleton *skele )
 557 {
 558    for( int i=0; i<skele->bone_count; i ++ )
 559    {
 560       struct skeleton_bone *sb = &skele->bones[i];
 561
 562       v3f p0, p1;
 563       v3_copy( sb->co, p0 );
 564       v3_add( p0, sb->end, p1 );
 565       //vg_line( p0, p1, 0xffffffff );
 566
 567       m4x3_mulv( skele->final_mtx[i], p0, p0 );
 568       m4x3_mulv( skele->final_mtx[i], p1, p1 );
 569
 570       if( sb->deform )
 571       {
 572          if( sb->ik )
 573          {
 574             vg_line( p0, p1, 0xff0000ff );
 575          }
 576          else
 577          {
 578             vg_line( p0, p1, 0xffcccccc );
 579          }
 580       }
 581       else
 582          vg_line( p0, p1, 0xff00ffff );
 583    }
 584 }
 585
 586 #endif /* SKELETON_H */