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