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