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       u32 flags;
  18       int defer;
  19
  20       mdl_keyframe kf;
  21       mdl_bone *orig_bone;
  22
  23       u32 collider;
  24       boxf hitbox;
  25       const char *name;
  26    }
  27    *bones;
  28    u32 bone_count;
  29
  30    struct skeleton_anim
  31    {
  32       const char *name;
  33       u32 length;
  34
  35       float rate;
  36       mdl_keyframe *anim_data;
  37    }
  38    *anims;
  39    u32 anim_count;
  40
  41    m4x3f *final_mtx;
  42
  43    struct skeleton_ik
  44    {
  45       u32 lower, upper, target, pole;
  46       m3x3f ia, ib;
  47    }
  48    *ik;
  49    u32 ik_count;
  50
  51    u32
  52        collider_count,
  53        bindable_count;
  54 };
  55
  56 VG_STATIC u32 skeleton_bone_id( struct skeleton *skele, const char *name )
  57 {
  58    for( u32 i=1; i<skele->bone_count; i++ ){
  59       if( !strcmp( skele->bones[i].name, name ))
  60          return i;
  61    }
  62
  63    vg_error( "skeleton_bone_id( *, \"%s\" );\n", name );
  64    vg_fatal_exit_loop( "Bone does not exist\n" );
  65
  66    return 0;
  67 }
  68
  69 VG_STATIC void keyframe_copy_pose( mdl_keyframe *kfa, mdl_keyframe *kfb,
  70                                    int num )
  71 {
  72    for( int i=0; i<num; i++ )
  73       kfb[i] = kfa[i];
  74 }
  75
  76 /*
  77  * Lerp between two sets of keyframes and store in dest. Rotations use Nlerp.
  78  */
  79 VG_STATIC void keyframe_lerp_pose( mdl_keyframe *kfa, mdl_keyframe *kfb,
  80                                    float t, mdl_keyframe *kfd, int count )
  81 {
  82    if( t <= 0.0001f ){
  83       keyframe_copy_pose( kfa, kfd, count );
  84       return;
  85    }
  86    else if( t >= 0.9999f ){
  87       keyframe_copy_pose( kfb, kfd, count );
  88       return;
  89    }
  90
  91    for( int i=0; i<count; i++ ){
  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 VG_STATIC
  99 void skeleton_lerp_pose( struct skeleton *skele,
 100                          mdl_keyframe *kfa, mdl_keyframe *kfb, float t,
 101                          mdl_keyframe *kfd )
 102 {
 103    keyframe_lerp_pose( kfa, kfb, t, kfd, skele->bone_count-1 );
 104 }
 105
 106 VG_STATIC void skeleton_copy_pose( struct skeleton *skele,
 107                                    mdl_keyframe *kfa, mdl_keyframe *kfd )
 108 {
 109    keyframe_copy_pose( kfa, kfd, skele->bone_count-1 );
 110 }
 111
 112 /*
 113  * Sample animation between 2 closest frames using time value. Output is a
 114  * keyframe buffer that is allocated with an appropriate size
 115  */
 116 VG_STATIC void skeleton_sample_anim( struct skeleton *skele,
 117                                   struct skeleton_anim *anim,
 118                                   float time,
 119                                   mdl_keyframe *output )
 120 {
 121    float animtime = time*anim->rate;
 122
 123    u32 frame = ((u32)animtime) % anim->length,
 124        next  = (frame+1) % anim->length;
 125
 126    float t = vg_fractf( animtime );
 127
 128    mdl_keyframe *base  = anim->anim_data + (skele->bone_count-1)*frame,
 129                 *nbase = anim->anim_data + (skele->bone_count-1)*next;
 130
 131    skeleton_lerp_pose( skele, base, nbase, t, output );
 132 }
 133
 134 VG_STATIC int skeleton_sample_anim_clamped( struct skeleton *skele,
 135                                          struct skeleton_anim *anim,
 136                                          float time,
 137                                          mdl_keyframe *output )
 138 {
 139    float end = (float)(anim->length-1) / anim->rate;
 140    skeleton_sample_anim( skele, anim, vg_minf( end, time ), output );
 141
 142    if( time > end )
 143       return 0;
 144    else
 145       return 1;
 146 }
 147
 148 typedef enum anim_apply
 149 {
 150    k_anim_apply_always,
 151    k_anim_apply_defer_ik,
 152    k_anim_apply_deffered_only
 153 }
 154 anim_apply;
 155
 156 VG_STATIC
 157 int should_apply_bone( struct skeleton *skele, u32 id, anim_apply type )
 158 {
 159    struct skeleton_bone *sb = &skele->bones[ id ],
 160                         *sp = &skele->bones[ sb->parent ];
 161
 162    if( type == k_anim_apply_defer_ik ){
 163       if( ((sp->flags & k_bone_flag_ik) && !(sb->flags & k_bone_flag_ik))
 164           || sp->defer )
 165       {
 166          sb->defer = 1;
 167          return 0;
 168       }
 169       else{
 170          sb->defer = 0;
 171          return 1;
 172       }
 173    }
 174    else if( type == k_anim_apply_deffered_only ){
 175       if( sb->defer )
 176          return 1;
 177       else
 178          return 0;
 179    }
 180
 181    return 1;
 182 }
 183
 184 /*
 185  * Apply block of keyframes to skeletons final pose
 186  */
 187 VG_STATIC void skeleton_apply_pose( struct skeleton *skele, mdl_keyframe *pose,
 188                                     anim_apply passtype )
 189 {
 190    m4x3_identity( skele->final_mtx[0] );
 191    skele->bones[0].defer = 0;
 192    skele->bones[0].flags &= ~k_bone_flag_ik;
 193
 194    for( u32 i=1; i<skele->bone_count; i++ ){
 195       struct skeleton_bone *sb = &skele->bones[i],
 196                            *sp = &skele->bones[sb->parent];
 197
 198       if( !should_apply_bone( skele, i, passtype ) )
 199          continue;
 200
 201       sb->defer = 0;
 202
 203       /* process pose */
 204       m4x3f posemtx;
 205
 206       v3f temp_delta;
 207       v3_sub( skele->bones[i].co, skele->bones[sb->parent].co, temp_delta );
 208
 209       /* pose matrix */
 210       mdl_keyframe *kf = &pose[i-1];
 211       q_m3x3( kf->q, posemtx );
 212       v3_copy( kf->co, posemtx[3] );
 213       v3_add( temp_delta, posemtx[3], posemtx[3] );
 214
 215       /* final matrix */
 216       m4x3_mul( skele->final_mtx[ sb->parent ], posemtx, skele->final_mtx[i] );
 217    }
 218 }
 219
 220 /*
 221  * creates the reference inverse matrix for an IK bone, as it has an initial
 222  * intrisic rotation based on the direction that the IK is setup..
 223  */
 224 VG_STATIC void skeleton_inverse_for_ik( struct skeleton *skele,
 225                                      v3f ivaxis,
 226                                      u32 id, m3x3f inverse )
 227 {
 228    v3_copy( ivaxis, inverse[0] );
 229    v3_copy( skele->bones[id].end, inverse[1] );
 230    v3_normalize( inverse[1] );
 231    v3_cross( inverse[0], inverse[1], inverse[2] );
 232    m3x3_transpose( inverse, inverse );
 233 }
 234
 235 /*
 236  * Creates inverse rotation matrices which the IK system uses.
 237  */
 238 VG_STATIC void skeleton_create_inverses( struct skeleton *skele )
 239 {
 240    /* IK: inverse 'plane-bone space' axis '(^axis,^bone,...)[base] */
 241    for( u32 i=0; i<skele->ik_count; i++ ){
 242       struct skeleton_ik *ik = &skele->ik[i];
 243
 244       m4x3f inverse;
 245       v3f iv0, iv1, ivaxis;
 246       v3_sub( skele->bones[ik->target].co, skele->bones[ik->lower].co, iv0 );
 247       v3_sub( skele->bones[ik->pole].co,   skele->bones[ik->lower].co, iv1 );
 248       v3_cross( iv0, iv1, ivaxis );
 249       v3_normalize( ivaxis );
 250
 251       skeleton_inverse_for_ik( skele, ivaxis, ik->lower, ik->ia );
 252       skeleton_inverse_for_ik( skele, ivaxis, ik->upper, ik->ib );
 253    }
 254 }
 255
 256 /*
 257  * Apply a model matrix to all bones, should be done last
 258  */
 259 VG_STATIC
 260 void skeleton_apply_transform( struct skeleton *skele, m4x3f transform )
 261 {
 262    for( u32 i=0; i<skele->bone_count; i++ ){
 263       struct skeleton_bone *sb = &skele->bones[i];
 264       m4x3_mul( transform, skele->final_mtx[i], skele->final_mtx[i] );
 265    }
 266 }
 267
 268 /*
 269  * Apply an inverse matrix to all bones which maps vertices from bind space into
 270  * bone relative positions
 271  */
 272 VG_STATIC void skeleton_apply_inverses( struct skeleton *skele )
 273 {
 274    for( u32 i=0; i<skele->bone_count; i++ ){
 275       struct skeleton_bone *sb = &skele->bones[i];
 276       m4x3f inverse;
 277       m3x3_identity( inverse );
 278       v3_negate( sb->co, inverse[3] );
 279
 280       m4x3_mul( skele->final_mtx[i], inverse, skele->final_mtx[i] );
 281    }
 282 }
 283
 284 /*
 285  * Apply all IK modifiers (2 bone ik reference from blender is supported)
 286  */
 287 VG_STATIC void skeleton_apply_ik_pass( struct skeleton *skele )
 288 {
 289    for( u32 i=0; i<skele->ik_count; i++ ){
 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( u32 i=0; i<skele->anim_count; i++ ){
 383       struct skeleton_anim *anim = &skele->anims[i];
 384
 385       if( !strcmp( anim->name, name ) )
 386          return anim;
 387    }
 388
 389    vg_error( "skeleton_get_anim( *, \"%s\" )\n", name );
 390    vg_fatal_exit_loop( "Invalid animation name\n" );
 391
 392    return NULL;
 393 }
 394
 395 VG_STATIC void skeleton_alloc_from( struct skeleton *skele,
 396                                     void *lin_alloc,
 397                                     mdl_context *mdl,
 398                                     mdl_armature *armature )
 399 {
 400    skele->bone_count     = armature->bone_count+1;
 401    skele->anim_count     = armature->anim_count;
 402    skele->ik_count       = 0;
 403    skele->collider_count = 0;
 404
 405    for( u32 i=0; i<armature->bone_count; i++ ){
 406       mdl_bone *bone = mdl_arritm( &mdl->bones, armature->bone_start+i );
 407
 408       if( bone->flags & k_bone_flag_ik )
 409          skele->ik_count ++;
 410
 411       if( bone->collider )
 412          skele->collider_count ++;
 413    }
 414
 415    u32 bone_size = sizeof(struct skeleton_bone) * skele->bone_count,
 416        ik_size   = sizeof(struct skeleton_ik)   * skele->ik_count,
 417        mtx_size  = sizeof(m4x3f)                * skele->bone_count,
 418        anim_size = sizeof(struct skeleton_anim) * skele->anim_count;
 419
 420    skele->bones      = vg_linear_alloc( lin_alloc, bone_size );
 421    skele->ik         = vg_linear_alloc( lin_alloc, ik_size );
 422    skele->final_mtx  = vg_linear_alloc( lin_alloc, mtx_size );
 423    skele->anims      = vg_linear_alloc( lin_alloc, anim_size );
 424 }
 425
 426 VG_STATIC void skeleton_fatal_err(void)
 427 {
 428    vg_fatal_exit_loop( "Skeleton setup failed" );
 429 }
 430
 431 /* Setup an animated skeleton from model. mdl's metadata should stick around */
 432 VG_STATIC void skeleton_setup( struct skeleton *skele,
 433                                void *lin_alloc, mdl_context *mdl )
 434 {
 435    u32 ik_count = 0, collider_count = 0;
 436    skele->bone_count = 0;
 437    skele->bones = NULL;
 438    skele->final_mtx = NULL;
 439    skele->anims = NULL;
 440
 441    if( !mdl->armatures.count ){
 442       vg_error( "No skeleton in model\n" );
 443       skeleton_fatal_err();
 444    }
 445
 446    mdl_armature *armature = mdl_arritm( &mdl->armatures, 0 );
 447    skeleton_alloc_from( skele, lin_alloc, mdl, armature );
 448
 449    for( u32 i=0; i<armature->bone_count; i++ ){
 450       mdl_bone *bone = mdl_arritm( &mdl->bones, armature->bone_start+i );
 451       struct skeleton_bone *sb = &skele->bones[i+1];
 452
 453       v3_copy( bone->co, sb->co );
 454       v3_copy( bone->end, sb->end );
 455
 456       sb->parent = bone->parent;
 457       sb->name   = mdl_pstr( mdl, bone->pstr_name );
 458       sb->flags  = bone->flags;
 459       sb->collider = bone->collider;
 460       sb->orig_bone = bone;
 461
 462       if( sb->flags & k_bone_flag_ik ){
 463          skele->bones[ sb->parent ].flags |= k_bone_flag_ik;
 464
 465          if( ik_count == skele->ik_count ){
 466             vg_error( "Too many ik bones, corrupt model file\n" );
 467             skeleton_fatal_err();
 468          }
 469
 470          struct skeleton_ik *ik = &skele->ik[ ik_count ++ ];
 471          ik->upper = i+1;
 472          ik->lower = bone->parent;
 473          ik->target = bone->ik_target;
 474          ik->pole = bone->ik_pole;
 475       }
 476
 477       box_copy( bone->hitbox, sb->hitbox );
 478
 479       if( bone->collider ){
 480          if( collider_count == skele->collider_count ){
 481             vg_error( "Too many collider bones\n" );
 482             skeleton_fatal_err();
 483          }
 484
 485          collider_count ++;
 486       }
 487    }
 488
 489    /* fill in implicit root bone */
 490    v3_zero( skele->bones[0].co );
 491    v3_copy( (v3f){0.0f,1.0f,0.0f}, skele->bones[0].end );
 492    skele->bones[0].parent = 0xffffffff;
 493    skele->bones[0].flags = 0;
 494    skele->bones[0].name = "[root]";
 495
 496    /* process animation quick refs */
 497    for( u32 i=0; i<skele->anim_count; i++ ){
 498       mdl_animation *anim =
 499          mdl_arritm( &mdl->animations, armature->anim_start+i );
 500
 501       skele->anims[i].rate       = anim->rate;
 502       skele->anims[i].length     = anim->length;
 503       skele->anims[i].name       = mdl_pstr(mdl, anim->pstr_name);
 504       skele->anims[i].anim_data  =
 505          mdl_arritm( &mdl->keyframes, anim->offset );
 506
 507       vg_info( "animation[ %f, %u ] '%s'\n", anim->rate,
 508                                              anim->length,
 509                                              skele->anims[i].name );
 510    }
 511
 512    skeleton_create_inverses( skele );
 513    vg_success( "Loaded skeleton with %u bones\n", skele->bone_count );
 514    vg_success( "                     %u colliders\n", skele->collider_count );
 515 }
 516
 517 VG_STATIC void skeleton_debug( struct skeleton *skele )
 518 {
 519    for( u32 i=1; i<skele->bone_count; i ++ ){
 520       struct skeleton_bone *sb = &skele->bones[i];
 521
 522       v3f p0, p1;
 523       v3_copy( sb->co, p0 );
 524       v3_add( p0, sb->end, p1 );
 525
 526       m4x3_mulv( skele->final_mtx[i], p0, p0 );
 527       m4x3_mulv( skele->final_mtx[i], p1, p1 );
 528
 529       if( sb->flags & k_bone_flag_deform ){
 530          if( sb->flags & k_bone_flag_ik ){
 531             vg_line( p0, p1, 0xff0000ff );
 532          }
 533          else{
 534             vg_line( p0, p1, 0xffcccccc );
 535          }
 536       }
 537       else
 538          vg_line( p0, p1, 0xff00ffff );
 539    }
 540 }
 541
 542 #endif /* SKELETON_H */