skeleton.h

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