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