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 */
 484
 485
 486
 487
 488
 489
 490
 491
 492
 493
 494
 495
 496
 497
 498
 499
 500
 501
 502
 503
 504
 505
 506
 507
 508
 509
 510
 511
 512
 513
 514
 515
 516
 517
 518
 519
 520
 521
 522
 523
 524
 525
 526
 527
 528
 529
 530
 531
 532
 533
 534
 535
 536
 537 #if 0
 538 #ifndef SKELETON_H
 539 #define SKELETON_H
 540
 541 #include "model.h"
 542
 543 struct skeleton
 544 {
 545    struct skeleton_bone
 546    {
 547       v3f co, end;
 548       u32 parent;
 549
 550       /* info, not real */
 551       int deform, ik;
 552       int defer;
 553
 554       mdl_keyframe kf;
 555    }
 556    *bones;
 557    m4x3f *final_transforms;
 558
 559    struct skeleton_ik
 560    {
 561       u32 lower, upper, target, pole;
 562    }
 563    *ik;
 564
 565    struct skeleton_anim
 566    {
 567       float rate;
 568       u32 length;
 569       struct mdl_keyframe *anim_data;
 570       char name[32];
 571    }
 572    *anims;
 573
 574    u32 bone_count,
 575        ik_count,
 576        anim_count,
 577        bindable_count;  /* TODO: try to place IK last in the rig from export
 578                                  so that we dont always upload transforms for
 579                                  useless cpu IK bones. */
 580 };
 581
 582 /*
 583  * Lerp between two sets of keyframes and store in dest. Rotations use Nlerp.
 584  */
 585 static void keyframe_lerp_pose( mdl_keyframe *kfa, mdl_keyframe *kfb, float t,
 586                                 mdl_keyframe *kfd, int count )
 587 {
 588    for( int i=0; i<count; i++ )
 589    {
 590       v3_lerp( kfa[i].co, kfb[i].co, t, kfd[i].co );
 591       q_nlerp( kfa[i].q,  kfb[i].q,  t, kfd[i].q );
 592       v3_lerp( kfa[i].s,  kfb[i].s,  t, kfd[i].s );
 593    }
 594 }
 595
 596 static void skeleton_lerp_pose( struct skeleton *skele,
 597                                 mdl_keyframe *kfa, mdl_keyframe *kfb, float t,
 598                                 mdl_keyframe *kfd )
 599 {
 600    keyframe_lerp_pose( kfa, kfb, t, kfd, skele->bone_count-1 );
 601 }
 602
 603 /*
 604  * Sample animation between 2 closest frames using time value. Output is a
 605  * keyframe buffer that is allocated with an appropriate size
 606  */
 607 static void skeleton_sample_anim( struct skeleton *skele,
 608                                   struct skeleton_anim *anim,
 609                                   float time,
 610                                   mdl_keyframe *output )
 611 {
 612    float animtime = time*anim->rate;
 613
 614    u32 frame = ((u32)animtime) % anim->length,
 615        next  = (frame+1) % anim->length;
 616
 617    float t = vg_fractf( animtime );
 618
 619    mdl_keyframe *base  = anim->anim_data + (skele->bone_count-1)*frame,
 620                 *nbase = anim->anim_data + (skele->bone_count-1)*next;
 621
 622    skeleton_lerp_pose( skele, base, nbase, t, output );
 623 }
 624
 625 typedef enum anim_apply
 626 {
 627    k_anim_apply_always,
 628    k_anim_apply_defer_ik,
 629    k_anim_apply_deffered_only
 630 }
 631 anim_apply;
 632
 633 static int should_apply_bone( struct skeleton *skele, u32 id, anim_apply type )
 634 {
 635    struct skeleton_bone *sb = &skele->bones[ id ],
 636                         *sp = &skele->bones[ sb->parent ];
 637
 638    if( type == k_anim_apply_defer_ik )
 639    {
 640       if( sp->ik || sp->defer )
 641       {
 642          sb->defer = 1;
 643          return 0;
 644       }
 645    }
 646    else if( type == k_anim_apply_deffered_only )
 647    {
 648       if( !sp->defer )
 649          return 0;
 650    }
 651
 652    return 1;
 653 }
 654
 655 /*
 656  * Apply block of keyframes to skeletons final pose
 657  */
 658 static void skeleton_apply_pose( m4x3f transform,
 659                                  struct skeleton *skele, mdl_keyframe *pose,
 660                                  anim_apply passtype )
 661 {
 662    m4x3_copy( transform, skele->final_transforms[0] );
 663    skele->bones[0].defer = 0;
 664    skele->bones[0].ik = 0;
 665
 666    for( int i=1; i<skele->bone_count; i++ )
 667    {
 668       struct skeleton_bone *sb = &skele->bones[i],
 669                            *sp = &skele->bones[ sb->parent ];
 670
 671       if( !should_apply_bone( skele, i, passtype ) )
 672          continue;
 673
 674       sb->defer = 0;
 675
 676       /* process pose */
 677       m4x3f posemtx;
 678
 679       v3f temp_delta;
 680       v3_sub( skele->bones[i].co, skele->bones[sb->parent].co, temp_delta );
 681
 682       /* pose matrix */
 683       mdl_keyframe *kf = &pose[i-1];
 684       q_m3x3( kf->q, posemtx );
 685       v3_copy( kf->co, posemtx[3] );
 686       v3_add( temp_delta, posemtx[3], posemtx[3] );
 687
 688       /* final matrix */
 689       m4x3_mul( skele->final_transforms[ sb->parent ], posemtx,
 690                 skele->final_transforms[i] );
 691    }
 692
 693    /* bone space inverse matrix ( for verts ) TODO: move to seperate pass */
 694    for( int i=1; i<skele->bone_count; i++ )
 695    {
 696       if( !should_apply_bone( skele, i, passtype ) )
 697          continue;
 698
 699       m4x3f abmtx;
 700       m3x3_identity( abmtx );
 701       v3_negate( skele->bones[i].co, abmtx[3] );
 702       m4x3_mul( skele->final_transforms[i], abmtx, skele->final_transforms[i] );
 703    }
 704 }
 705
 706 static void skeleton_apply_frame( m4x3f transform,
 707                                   struct skeleton *skele,
 708                                   struct skeleton_anim *anim,
 709                                   float time )
 710 {
 711    float animtime = time*anim->rate;
 712
 713    u32 frame = ((u32)animtime) % anim->length,
 714        next  = (frame+1) % anim->length;
 715
 716    float t = vg_fractf( animtime );
 717
 718    mdl_keyframe *base  = anim->anim_data + (skele->bone_count-1)*frame,
 719                 *nbase = anim->anim_data + (skele->bone_count-1)*next;
 720
 721    m4x3_copy( transform, skele->final_transforms[0] );
 722
 723    for( int i=1; i<skele->bone_count; i++ )
 724    {
 725       struct skeleton_bone *sb = &skele->bones[i];
 726
 727       /* process pose */
 728       m4x3f posemtx;
 729
 730       v3f temp_delta;
 731       v3_sub( skele->bones[i].co, skele->bones[sb->parent].co, temp_delta );
 732
 733       /* pose matrix */
 734       mdl_keyframe *kf  = base+i-1,
 735                    *nkf = nbase+i-1;
 736
 737       v3f co;
 738       v4f q;
 739       v3f s;
 740
 741       v3_lerp( kf->co, nkf->co, t, co );
 742       q_nlerp( kf->q, nkf->q, t, q );
 743       v3_lerp( kf->s, nkf->s, t, s );
 744
 745       q_m3x3( q, posemtx );
 746       v3_copy( co, posemtx[3] );
 747       v3_add( temp_delta, posemtx[3], posemtx[3] );
 748
 749       /* final matrix */
 750       m4x3_mul( skele->final_transforms[ sb->parent ], posemtx,
 751                         skele->final_transforms[i] );
 752    }
 753
 754    /* armature space -> bone space matrix ( for verts ) */
 755    for( int i=1; i<skele->bone_count; i++ )
 756    {
 757       m4x3f abmtx;
 758       m3x3_identity( abmtx );
 759       v3_negate( skele->bones[i].co, abmtx[3] );
 760       m4x3_mul( skele->final_transforms[i], abmtx,
 761                 skele->final_transforms[i] );
 762    }
 763 }
 764
 765 /*
 766  * Get transformed position of bone
 767  */
 768 static void skeleton_bone_posepos( struct skeleton *skele, u32 id, v3f co )
 769 {
 770    m4x3_mulv( skele->final_transforms[id], skele->bones[id].co, co );
 771 }
 772
 773 /*
 774  * creates the reference inverse matrix for an IK bone, as it has an initial
 775  * intrisic rotation based on the direction that the IK is setup..
 776  */
 777 static void skeleton_inverse_for_ik( struct skeleton *skele,
 778                                      v3f ivaxis,
 779                                      u32 id, m4x3f inverse )
 780 {
 781    v3_copy( ivaxis, inverse[0] );
 782    v3_copy( skele->bones[id].end, inverse[1] );
 783    v3_normalize( inverse[1] );
 784    v3_cross( inverse[0], inverse[1], inverse[2] );
 785    v3_copy( skele->bones[id].co, inverse[3] );
 786    m4x3_invert_affine( inverse, inverse );
 787 }
 788
 789 /*
 790  * Apply all IK modifiers (2 bone ik reference from blender is supported)
 791  */
 792 static void skeleton_apply_ik_pass( struct skeleton *skele )
 793 {
 794    for( int i=0; i<skele->ik_count; i++ )
 795    {
 796       struct skeleton_ik *ik = &skele->ik[i];
 797
 798       v3f v0, /* base -> target */
 799           v1, /* base -> pole */
 800           vaxis;
 801
 802       v3f co_base,
 803           co_target,
 804           co_pole;
 805
 806       skeleton_bone_posepos( skele, ik->lower, co_base );
 807       skeleton_bone_posepos( skele, ik->target, co_target );
 808       skeleton_bone_posepos( skele, ik->pole, co_pole );
 809
 810       v3_sub( co_target, co_base, v0 );
 811       v3_sub( co_pole, co_base, v1 );
 812       v3_cross( v0, v1, vaxis );
 813       v3_normalize( vaxis );
 814       v3_normalize( v0 );
 815       v3_cross( vaxis, v0, v1 );
 816
 817       /* localize problem into [x:v0,y:v1] 2d plane */
 818       v2f base = { v3_dot( v0, co_base   ), v3_dot( v1, co_base   ) },
 819           end  = { v3_dot( v0, co_target ), v3_dot( v1, co_target ) },
 820           knee;
 821
 822       /* Compute angles (basic trig)*/
 823       v2f delta;
 824       v2_sub( end, base, delta );
 825
 826       float
 827          l1 = v3_length( skele->bones[ik->lower].end ),
 828          l2 = v3_length( skele->bones[ik->upper].end ),
 829          d = vg_clampf( v2_length(delta), fabsf(l1 - l2), l1+l2-0.00001f ),
 830          c = acosf( (l1*l1 + d*d - l2*l2) / (2.0f*l1*d) ),
 831          rot = atan2f( delta[1], delta[0] ) + c - VG_PIf/2.0f;
 832
 833       knee[0] = sinf(-rot) * l1;
 834       knee[1] = cosf(-rot) * l1;
 835
 836       m4x3_identity( skele->final_transforms[ik->lower] );
 837       m4x3_identity( skele->final_transforms[ik->upper] );
 838
 839       /* inverse matrix axis '(^axis,^bone,...)[base] */
 840       m4x3f inverse;
 841       v3f iv0, iv1, ivaxis;
 842       v3_sub( skele->bones[ik->target].co, skele->bones[ik->lower].co, iv0 );
 843       v3_sub( skele->bones[ik->pole].co, skele->bones[ik->lower].co, iv1 );
 844       v3_cross( iv0, iv1, ivaxis );
 845       v3_normalize( ivaxis );
 846
 847       skeleton_inverse_for_ik( skele, ivaxis, ik->lower, inverse );
 848
 849       /* create rotation matrix */
 850       v3f co_knee;
 851       v3_muladds( co_base, v0, knee[0], co_knee );
 852       v3_muladds( co_knee, v1, knee[1], co_knee );
 853       vg_line( co_base, co_knee, 0xff00ff00 );
 854
 855       m4x3f transform;
 856       v3_copy( vaxis, transform[0] );
 857       v3_muls( v0, knee[0], transform[1] );
 858       v3_muladds( transform[1], v1, knee[1], transform[1] );
 859       v3_normalize( transform[1] );
 860       v3_cross( transform[0], transform[1], transform[2] );
 861       v3_copy( co_base, transform[3] );
 862
 863       m4x3_mul( transform, inverse, skele->final_transforms[ik->lower] );
 864
 865       /* 'upper' or knee bone */
 866       skeleton_inverse_for_ik( skele, ivaxis, ik->upper, inverse );
 867
 868       v3_copy( vaxis, transform[0] );
 869       v3_sub( co_target, co_knee, transform[1] );
 870       v3_normalize( transform[1] );
 871       v3_cross( transform[0], transform[1], transform[2] );
 872       v3_copy( co_knee, transform[3] );
 873
 874       m4x3_mul( transform, inverse, skele->final_transforms[ik->upper] );
 875    }
 876 }
 877
 878 static struct skeleton_anim *skeleton_get_anim( struct skeleton *skele,
 879                                                 const char *name )
 880 {
 881    for( int i=0; i<skele->anim_count; i++ )
 882    {
 883       struct skeleton_anim *anim = &skele->anims[i];
 884
 885       if( !strcmp( anim->name, name ) )
 886          return anim;
 887    }
 888
 889    return NULL;
 890 }
 891
 892 /* Setup an animated skeleton from model */
 893 static int skeleton_setup( struct skeleton *skele, mdl_header *mdl )
 894 {
 895    u32 bone_count = 1, skeleton_root = 0, ik_count = 0;
 896    skele->bone_count = 0;
 897    skele->bones = NULL;
 898    skele->final_transforms = NULL;
 899    skele->anims = NULL;
 900
 901    struct classtype_skeleton *inf = NULL;
 902
 903    for( u32 i=0; i<mdl->node_count; i++ )
 904    {
 905       mdl_node *pnode = mdl_node_from_id( mdl, i );
 906
 907       if( pnode->classtype == k_classtype_skeleton )
 908       {
 909          inf = mdl_get_entdata( mdl, pnode );
 910          if( skele->bone_count )
 911          {
 912             vg_error( "Multiple skeletons in model file\n" );
 913             goto error_dealloc;
 914          }
 915
 916          skele->bone_count = inf->channels;
 917          skele->ik_count = inf->ik_count;
 918          skele->bones = malloc(sizeof(struct skeleton_bone)*skele->bone_count);
 919          skele->ik = malloc(sizeof(struct skeleton_ik)*skele->ik_count);
 920          skeleton_root = i;
 921       }
 922       else if( skele->bone_count )
 923       {
 924          int is_ik   =  pnode->classtype == k_classtype_ik_bone,
 925              is_bone = (pnode->classtype == k_classtype_bone) || is_ik;
 926
 927          if( is_bone )
 928          {
 929             if( bone_count == skele->bone_count )
 930             {
 931                vg_error( "too many bones (%u/%u) @%s!\n",
 932                            bone_count, skele->bone_count,
 933                            mdl_pstr( mdl, pnode->pstr_name ));
 934
 935                goto error_dealloc;
 936             }
 937
 938             struct skeleton_bone *sb = &skele->bones[bone_count];
 939
 940             v3_copy( pnode->co, sb->co );
 941             v3_copy( pnode->s, sb->end );
 942             sb->parent = pnode->parent-skeleton_root;
 943
 944             if( is_ik )
 945             {
 946                struct classtype_ik_bone *ik_inf = mdl_get_entdata( mdl, pnode );
 947                sb->deform = ik_inf->deform;
 948                sb->ik = 1; /* TODO: place into new IK array */
 949                skele->bones[ sb->parent ].ik = 1;
 950
 951                if( ik_count == skele->ik_count )
 952                {
 953                   vg_error( "Too many ik bones, corrupt model file\n" );
 954                   goto error_dealloc;
 955                }
 956
 957                struct skeleton_ik *ik = &skele->ik[ ik_count ++ ];
 958                ik->upper = bone_count;
 959                ik->lower = sb->parent;
 960                ik->target = ik_inf->target;
 961                ik->pole = ik_inf->pole;
 962             }
 963             else
 964             {
 965                struct classtype_bone *bone_inf = mdl_get_entdata( mdl, pnode );
 966                sb->deform = bone_inf->deform;
 967                sb->ik = 0;
 968             }
 969
 970             bone_count ++;
 971          }
 972          else
 973          {
 974             break;
 975          }
 976       }
 977    }
 978
 979    if( !inf )
 980    {
 981       vg_error( "No skeleton in model\n" );
 982       return 0;
 983    }
 984
 985    if( bone_count != skele->bone_count )
 986    {
 987       vg_error( "Loaded %u bones out of %u\n", bone_count, skele->bone_count );
 988       goto error_dealloc;
 989    }
 990
 991    if( ik_count != skele->ik_count )
 992    {
 993       vg_error( "Loaded %u ik bones out of %u\n", ik_count, skele->ik_count );
 994       goto error_dealloc;
 995    }
 996
 997    /* fill in implicit root bone */
 998    v3_zero( skele->bones[0].co );
 999    v3_copy( (v3f){0.0f,1.0f,0.0f}, skele->bones[0].end );
1000    skele->bones[0].parent = 0xffffffff;
1001
1002    skele->final_transforms = malloc( sizeof(m4x3f) * skele->bone_count );
1003    skele->anim_count = inf->anim_count;
1004    skele->anims = malloc( sizeof(struct skeleton_anim) * inf->anim_count);
1005
1006    for( int i=0; i<inf->anim_count; i++ )
1007    {
1008       mdl_animation *anim =
1009          mdl_animation_from_id( mdl, inf->anim_start+i );
1010
1011       skele->anims[i].rate = anim->rate;
1012       skele->anims[i].length = anim->length;
1013       strncpy( skele->anims[i].name, mdl_pstr(mdl, anim->pstr_name), 32 );
1014
1015       u32 total_keyframes = (skele->bone_count-1)*anim->length;
1016       size_t block_size = sizeof(mdl_keyframe) * total_keyframes;
1017       mdl_keyframe *dst = malloc( block_size );
1018
1019       skele->anims[i].anim_data = dst;
1020       memcpy( dst, mdl_get_animdata( mdl, anim ), block_size );
1021    }
1022
1023    vg_success( "Loaded skeleton with %u bones\n", skele->bone_count );
1024    return 1;
1025
1026 error_dealloc:
1027    free( skele->bones );
1028    free( skele->ik );
1029    return 0;
1030 }
1031
1032 static void skeleton_debug( struct skeleton *skele )
1033 {
1034    for( int i=0; i<skele->bone_count; i ++ )
1035    {
1036       struct skeleton_bone *sb = &skele->bones[i];
1037
1038       v3f p0, p1;
1039       v3_copy( sb->co, p0 );
1040       v3_add( p0, sb->end, p1 );
1041       //vg_line( p0, p1, 0xffffffff );
1042
1043       m4x3_mulv( skele->final_transforms[i], p0, p0 );
1044       m4x3_mulv( skele->final_transforms[i], p1, p1 );
1045
1046       if( sb->deform )
1047       {
1048          if( sb->ik )
1049          {
1050             vg_line( p0, p1, 0xff0000ff );
1051          }
1052          else
1053          {
1054             vg_line( p0, p1, 0xffcccccc );
1055          }
1056       }
1057       else
1058          vg_line( p0, p1, 0xff00ffff );
1059    }
1060 }
1061
1062 #endif /* SKELETON_H */
1063 #endif