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