skeleton.h

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