skeleton.h

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