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