skeleton.h

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