From: hgn <hgodden00@gmail.com>
Date: Fri, 9 Sep 2022 00:04:37 +0000 (+0100)
Subject: Ik revision
X-Git-Url: https://harrygodden.com/git/?a=commitdiff_plain;h=4b222b13c504e5c21a743b7aeb02a692ce16da01;hp=3d5597be2fd4b5d9ea3bf8863e15cc9bc8123755;p=carveJwlIkooP6JGAAIwe30JlM.git

Ik revision
---

diff --git a/anim_test.h b/anim_test.h
index c2f3eae..86c6853 100644
--- a/anim_test.h
+++ b/anim_test.h
@@ -8,7 +8,7 @@
 static struct 
 {
    struct skeleton skele;
-   struct skeleton_anim *yay;
+   struct skeleton_anim *anim_stand, *anim_highg;
 
    glmesh mesh;
 }
@@ -18,7 +18,8 @@ static void anim_test_start(void)
 {
    mdl_header *johannes = mdl_load( "models/ch_new.mdl" );
    skeleton_setup( &animtest.skele, johannes );
-   animtest.yay = skeleton_get_anim( &animtest.skele, "pose_stand" );
+   animtest.anim_stand = skeleton_get_anim( &animtest.skele, "pose_stand" );
+   animtest.anim_highg = skeleton_get_anim( &animtest.skele, "pose_highg" );
    mdl_unpack_glmesh( johannes, &animtest.mesh );
 
    free( johannes );
@@ -36,8 +37,21 @@ static void anim_test_update(void)
    q_axis_angle( qt, (v3f){0.0f,1.0f,0.0f}, vg_time*1.2f );
    q_m3x3( qt, transform );
 
-   skeleton_apply_frame( transform, &animtest.skele, animtest.yay, vg_time );
+   mdl_keyframe apose[32],
+                bpose[32];
+
+   float a = sinf(vg_time)*0.5f+0.5f,
+         b = (sinf(vg_time*2.0f)*0.5f+0.5f)*(15.0f/30.0f);
+
+   skeleton_sample_anim( &animtest.skele, animtest.anim_stand, b, apose );
+   skeleton_sample_anim( &animtest.skele, animtest.anim_highg, b, bpose );
+
+   skeleton_lerp_pose( &animtest.skele, apose, bpose, a, apose );
+   skeleton_apply_pose( &animtest.skele, apose, k_anim_apply_defer_ik );
    skeleton_apply_ik_pass( &animtest.skele );
+   skeleton_apply_pose( &animtest.skele, apose, k_anim_apply_deffered_only );
+   skeleton_apply_inverses( &animtest.skele );
+   skeleton_apply_transform( &animtest.skele, transform );
 
    skeleton_debug( &animtest.skele );
 }
@@ -62,7 +76,7 @@ static void anim_test_render( vg_tex2d *tex )
    glUniformMatrix4x3fv( _uniform_viewchar_uTransforms, 
                          animtest.skele.bone_count,
                          0,
-                         (float *)animtest.skele.final_transforms );
+                         (float *)animtest.skele.final_mtx );
    
    mesh_bind( &animtest.mesh );
    mesh_draw( &animtest.mesh );
diff --git a/models_src/ch_new.mdl b/models_src/ch_new.mdl
index d4cf5c7..234f50c 100644
Binary files a/models_src/ch_new.mdl and b/models_src/ch_new.mdl differ
diff --git a/skeleton.h b/skeleton.h
index ace80e6..ac368cf 100644
--- a/skeleton.h
+++ b/skeleton.h
@@ -3,6 +3,543 @@
 
 #include "model.h"
 
+struct skeleton
+{
+   struct skeleton_bone
+   {
+      v3f co, end;
+      u32 parent;
+
+      int deform, ik;
+      int defer;
+
+      mdl_keyframe kf;
+   }
+   *bones;
+   m4x3f *final_mtx;
+
+   struct skeleton_ik
+   {
+      u32 lower, upper, target, pole;
+      m3x3f ia, ib;
+   }
+   *ik;
+
+   struct skeleton_anim
+   {
+      float rate;
+      u32 length;
+      struct mdl_keyframe *anim_data;
+      char name[32];
+   }
+   *anims;
+
+   u32 bone_count,
+       ik_count,
+       anim_count,
+       bindable_count;  /* TODO: try to place IK last in the rig from export
+                                 so that we dont always upload transforms for
+                                 useless cpu IK bones. */
+};
+
+/*
+ * Lerp between two sets of keyframes and store in dest. Rotations use Nlerp.
+ */
+static void keyframe_lerp_pose( mdl_keyframe *kfa, mdl_keyframe *kfb, float t,
+                                mdl_keyframe *kfd, int count )
+{
+   for( int i=0; i<count; i++ )
+   {
+      v3_lerp( kfa[i].co, kfb[i].co, t, kfd[i].co );
+      q_nlerp( kfa[i].q,  kfb[i].q,  t, kfd[i].q );
+      v3_lerp( kfa[i].s,  kfb[i].s,  t, kfd[i].s );
+   }
+}
+
+static void skeleton_lerp_pose( struct skeleton *skele,
+                                mdl_keyframe *kfa, mdl_keyframe *kfb, float t,
+                                mdl_keyframe *kfd )
+{
+   keyframe_lerp_pose( kfa, kfb, t, kfd, skele->bone_count-1 );
+}
+
+/*
+ * Sample animation between 2 closest frames using time value. Output is a
+ * keyframe buffer that is allocated with an appropriate size
+ */
+static void skeleton_sample_anim( struct skeleton *skele,
+                                  struct skeleton_anim *anim,
+                                  float time,
+                                  mdl_keyframe *output )
+{
+   float animtime = time*anim->rate;
+
+   u32 frame = ((u32)animtime) % anim->length,
+       next  = (frame+1) % anim->length;
+
+   float t = vg_fractf( animtime );
+
+   mdl_keyframe *base  = anim->anim_data + (skele->bone_count-1)*frame,
+                *nbase = anim->anim_data + (skele->bone_count-1)*next;
+
+   skeleton_lerp_pose( skele, base, nbase, t, output );
+}
+
+typedef enum anim_apply
+{
+   k_anim_apply_always,
+   k_anim_apply_defer_ik,
+   k_anim_apply_deffered_only
+}
+anim_apply;
+
+static int should_apply_bone( struct skeleton *skele, u32 id, anim_apply type )
+{
+   struct skeleton_bone *sb = &skele->bones[ id ],
+                        *sp = &skele->bones[ sb->parent ];
+
+   if( type == k_anim_apply_defer_ik )
+   {
+      if( (sp->ik && !sb->ik) || sp->defer )
+      {
+         sb->defer = 1;
+         return 0;
+      }
+      else
+      {
+         sb->defer = 0;
+         return 1;
+      }
+   }
+   else if( type == k_anim_apply_deffered_only )
+   {
+      if( sb->defer )
+         return 1;
+      else
+         return 0;
+   }
+
+   return 1;
+}
+
+/*
+ * Apply block of keyframes to skeletons final pose
+ */
+static void skeleton_apply_pose( struct skeleton *skele, mdl_keyframe *pose,
+                                 anim_apply passtype )
+{
+   m4x3_identity( skele->final_mtx[0] );
+   skele->bones[0].defer = 0;
+   skele->bones[0].ik = 0;
+
+   for( int i=1; i<skele->bone_count; i++ )
+   {
+      struct skeleton_bone *sb = &skele->bones[i],
+                           *sp = &skele->bones[ sb->parent ];
+      
+      if( !should_apply_bone( skele, i, passtype ) )
+         continue;
+
+      sb->defer = 0;
+
+      /* process pose */
+      m4x3f posemtx;
+
+      v3f temp_delta;
+      v3_sub( skele->bones[i].co, skele->bones[sb->parent].co, temp_delta );
+
+      /* pose matrix */
+      mdl_keyframe *kf = &pose[i-1];
+      q_m3x3( kf->q, posemtx );
+      v3_copy( kf->co, posemtx[3] );
+      v3_add( temp_delta, posemtx[3], posemtx[3] );
+
+      /* final matrix */
+      m4x3_mul( skele->final_mtx[ sb->parent ], posemtx, skele->final_mtx[i] );
+   }
+}
+
+/* 
+ * creates the reference inverse matrix for an IK bone, as it has an initial 
+ * intrisic rotation based on the direction that the IK is setup..
+ */
+static void skeleton_inverse_for_ik( struct skeleton *skele,
+                                     v3f ivaxis,
+                                     u32 id, m3x3f inverse )
+{
+   v3_copy( ivaxis, inverse[0] );
+   v3_copy( skele->bones[id].end, inverse[1] );
+   v3_normalize( inverse[1] );
+   v3_cross( inverse[0], inverse[1], inverse[2] );
+   m3x3_transpose( inverse, inverse );
+}
+
+static void skeleton_create_inverses( struct skeleton *skele )
+{
+   /* IK: inverse 'plane-bone space' axis '(^axis,^bone,...)[base] */
+   for( int i=0; i<skele->ik_count; i++ )
+   {
+      struct skeleton_ik *ik = &skele->ik[i];
+
+      m4x3f inverse;
+      v3f iv0, iv1, ivaxis;
+      v3_sub( skele->bones[ik->target].co, skele->bones[ik->lower].co, iv0 );
+      v3_sub( skele->bones[ik->pole].co, skele->bones[ik->lower].co, iv1 );
+      v3_cross( iv0, iv1, ivaxis );
+      v3_normalize( ivaxis );
+
+      skeleton_inverse_for_ik( skele, ivaxis, ik->lower, ik->ia );
+      skeleton_inverse_for_ik( skele, ivaxis, ik->upper, ik->ib );
+   }
+}
+
+static void skeleton_apply_transform( struct skeleton *skele, m4x3f transform )
+{
+   /* bone space inverse matrix */
+   for( int i=0; i<skele->bone_count; i++ )
+   {
+      struct skeleton_bone *sb = &skele->bones[i];
+      m4x3_mul( transform, skele->final_mtx[i], skele->final_mtx[i] );
+   }
+}
+
+static void skeleton_apply_inverses( struct skeleton *skele )
+{
+   for( int i=0; i<skele->bone_count; i++ )
+   {
+      struct skeleton_bone *sb = &skele->bones[i];
+      m4x3f inverse;
+      m3x3_identity( inverse );
+      v3_negate( sb->co, inverse[3] );
+
+      m4x3_mul( skele->final_mtx[i], inverse, skele->final_mtx[i] );
+   }
+}
+
+/*
+ * Apply all IK modifiers (2 bone ik reference from blender is supported)
+ */
+static void skeleton_apply_ik_pass( struct skeleton *skele )
+{
+   for( int i=0; i<skele->ik_count; i++ )
+   {
+      struct skeleton_ik *ik = &skele->ik[i];
+      
+      v3f v0, /* base -> target */
+          v1, /* base -> pole */
+          vaxis;
+
+      v3f co_base,
+          co_target,
+          co_pole;
+
+      v3_copy( skele->final_mtx[ik->lower][3], co_base );
+      v3_copy( skele->final_mtx[ik->target][3], co_target );
+      v3_copy( skele->final_mtx[ik->pole][3], co_pole );
+
+      v3_sub( co_target, co_base, v0 );
+      v3_sub( co_pole, co_base, v1 );
+      v3_cross( v0, v1, vaxis );
+      v3_normalize( vaxis );
+      v3_normalize( v0 );
+      v3_cross( vaxis, v0, v1 );
+
+      /* localize problem into [x:v0,y:v1] 2d plane */
+      v2f base = { v3_dot( v0, co_base   ), v3_dot( v1, co_base   ) },
+          end  = { v3_dot( v0, co_target ), v3_dot( v1, co_target ) },
+          knee;
+
+      /* Compute angles (basic trig)*/
+      v2f delta;
+      v2_sub( end, base, delta );
+
+      float 
+         l1 = v3_length( skele->bones[ik->lower].end ),
+         l2 = v3_length( skele->bones[ik->upper].end ),
+         d = vg_clampf( v2_length(delta), fabsf(l1 - l2), l1+l2-0.00001f ),
+         c = acosf( (l1*l1 + d*d - l2*l2) / (2.0f*l1*d) ),
+         rot = atan2f( delta[1], delta[0] ) + c - VG_PIf/2.0f;
+
+      knee[0] = sinf(-rot) * l1;
+      knee[1] = cosf(-rot) * l1;
+
+      m4x3_identity( skele->final_mtx[ik->lower] );
+      m4x3_identity( skele->final_mtx[ik->upper] );
+
+      /* create rotation matrix */
+      v3f co_knee;
+      v3_muladds( co_base, v0, knee[0], co_knee );
+      v3_muladds( co_knee, v1, knee[1], co_knee );
+      vg_line( co_base, co_knee, 0xff00ff00 );
+
+      m4x3f transform;
+      v3_copy( vaxis, transform[0] );
+      v3_muls( v0, knee[0], transform[1] );
+      v3_muladds( transform[1], v1, knee[1], transform[1] );
+      v3_normalize( transform[1] );
+      v3_cross( transform[0], transform[1], transform[2] );
+      v3_copy( co_base, transform[3] );
+
+      m3x3_mul( transform, ik->ia, transform );
+      m4x3_copy( transform, skele->final_mtx[ik->lower] );
+
+      /* upper/knee bone */
+      v3_copy( vaxis, transform[0] );
+      v3_sub( co_target, co_knee, transform[1] );
+      v3_normalize( transform[1] );
+      v3_cross( transform[0], transform[1], transform[2] );
+      v3_copy( co_knee, transform[3] );
+
+      m3x3_mul( transform, ik->ib, transform );
+      m4x3_copy( transform, skele->final_mtx[ik->upper] );
+   }
+}
+
+static struct skeleton_anim *skeleton_get_anim( struct skeleton *skele,
+                                                const char *name )
+{
+   for( int i=0; i<skele->anim_count; i++ )
+   {
+      struct skeleton_anim *anim = &skele->anims[i];
+
+      if( !strcmp( anim->name, name ) )
+         return anim;
+   }
+
+   return NULL;
+}
+
+/* Setup an animated skeleton from model */
+static int skeleton_setup( struct skeleton *skele, mdl_header *mdl )
+{
+   u32 bone_count = 1, skeleton_root = 0, ik_count = 0;
+   skele->bone_count = 0;
+   skele->bones = NULL;
+   skele->final_mtx = NULL;
+   skele->anims = NULL;
+
+   struct classtype_skeleton *inf = NULL;
+
+   for( u32 i=0; i<mdl->node_count; i++ )
+   {
+      mdl_node *pnode = mdl_node_from_id( mdl, i );
+
+      if( pnode->classtype == k_classtype_skeleton )
+      {
+         inf = mdl_get_entdata( mdl, pnode );
+         if( skele->bone_count )
+         {
+            vg_error( "Multiple skeletons in model file\n" );
+            goto error_dealloc;
+         }
+         
+         skele->bone_count = inf->channels;
+         skele->ik_count = inf->ik_count;
+         skele->bones = malloc(sizeof(struct skeleton_bone)*skele->bone_count);
+         skele->ik = malloc(sizeof(struct skeleton_ik)*skele->ik_count);
+         skeleton_root = i;
+      }
+      else if( skele->bone_count )
+      {
+         int is_ik   =  pnode->classtype == k_classtype_ik_bone,
+             is_bone = (pnode->classtype == k_classtype_bone) || is_ik;
+
+         if( is_bone )
+         {
+            if( bone_count == skele->bone_count )
+            {
+               vg_error( "too many bones (%u/%u) @%s!\n", 
+                           bone_count, skele->bone_count,
+                           mdl_pstr( mdl, pnode->pstr_name ));
+
+               goto error_dealloc;
+            }
+
+            struct skeleton_bone *sb = &skele->bones[bone_count];
+
+            v3_copy( pnode->co, sb->co );
+            v3_copy( pnode->s, sb->end );
+            sb->parent = pnode->parent-skeleton_root;
+
+            if( is_ik )
+            {
+               struct classtype_ik_bone *ik_inf = mdl_get_entdata( mdl, pnode );
+               sb->deform = ik_inf->deform;
+               sb->ik = 1; /* TODO: place into new IK array */
+               skele->bones[ sb->parent ].ik = 1;
+               
+               if( ik_count == skele->ik_count )
+               {
+                  vg_error( "Too many ik bones, corrupt model file\n" );
+                  goto error_dealloc;
+               }
+
+               struct skeleton_ik *ik = &skele->ik[ ik_count ++ ];
+               ik->upper = bone_count;
+               ik->lower = sb->parent;
+               ik->target = ik_inf->target;
+               ik->pole = ik_inf->pole;
+            }
+            else
+            {
+               struct classtype_bone *bone_inf = mdl_get_entdata( mdl, pnode );
+               sb->deform = bone_inf->deform;
+               sb->ik = 0;
+            }
+
+            bone_count ++;
+         }
+         else
+         {
+            break;
+         }
+      }
+   }
+
+   if( !inf )
+   {
+      vg_error( "No skeleton in model\n" );
+      return 0;
+   }
+
+   if( bone_count != skele->bone_count )
+   {
+      vg_error( "Loaded %u bones out of %u\n", bone_count, skele->bone_count );
+      goto error_dealloc;
+   }
+
+   if( ik_count != skele->ik_count )
+   {
+      vg_error( "Loaded %u ik bones out of %u\n", ik_count, skele->ik_count );
+      goto error_dealloc;
+   }
+
+   /* fill in implicit root bone */
+   v3_zero( skele->bones[0].co );
+   v3_copy( (v3f){0.0f,1.0f,0.0f}, skele->bones[0].end );
+   skele->bones[0].parent = 0xffffffff;
+   
+   skele->final_mtx = malloc( sizeof(m4x3f) * skele->bone_count );
+   skele->anim_count = inf->anim_count;
+   skele->anims = malloc( sizeof(struct skeleton_anim) * inf->anim_count);
+   
+   for( int i=0; i<inf->anim_count; i++ )
+   {
+      mdl_animation *anim = 
+         mdl_animation_from_id( mdl, inf->anim_start+i );
+
+      skele->anims[i].rate = anim->rate;
+      skele->anims[i].length = anim->length;
+      strncpy( skele->anims[i].name, mdl_pstr(mdl, anim->pstr_name), 32 );
+
+      u32 total_keyframes = (skele->bone_count-1)*anim->length;
+      size_t block_size = sizeof(mdl_keyframe) * total_keyframes;
+      mdl_keyframe *dst = malloc( block_size );
+
+      skele->anims[i].anim_data = dst;
+      memcpy( dst, mdl_get_animdata( mdl, anim ), block_size );
+   }
+
+   skeleton_create_inverses( skele );
+   vg_success( "Loaded skeleton with %u bones\n", skele->bone_count );
+   return 1;
+
+error_dealloc:
+   free( skele->bones );
+   free( skele->ik );
+   return 0;
+}
+
+static void skeleton_debug( struct skeleton *skele )
+{
+   for( int i=0; i<skele->bone_count; i ++ )
+   {
+      struct skeleton_bone *sb = &skele->bones[i];
+
+      v3f p0, p1;
+      v3_copy( sb->co, p0 );
+      v3_add( p0, sb->end, p1 );
+      //vg_line( p0, p1, 0xffffffff );
+
+      m4x3_mulv( skele->final_mtx[i], p0, p0 );
+      m4x3_mulv( skele->final_mtx[i], p1, p1 );
+
+      if( sb->deform )
+      {
+         if( sb->ik )
+         {
+            vg_line( p0, p1, 0xff0000ff );
+         }
+         else
+         {
+            vg_line( p0, p1, 0xffcccccc );
+         }
+      }
+      else
+         vg_line( p0, p1, 0xff00ffff );
+   }
+}
+
+#endif /* SKELETON_H */
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+#if 0
+#ifndef SKELETON_H
+#define SKELETON_H
+
+#include "model.h"
+
 struct skeleton
 {
    struct skeleton_bone
@@ -12,6 +549,7 @@ struct skeleton
 
       /* info, not real */
       int deform, ik;
+      int defer;
 
       mdl_keyframe kf;
    }
@@ -41,6 +579,130 @@ struct skeleton
                                  useless cpu IK bones. */
 };
 
+/*
+ * Lerp between two sets of keyframes and store in dest. Rotations use Nlerp.
+ */
+static void keyframe_lerp_pose( mdl_keyframe *kfa, mdl_keyframe *kfb, float t,
+                                mdl_keyframe *kfd, int count )
+{
+   for( int i=0; i<count; i++ )
+   {
+      v3_lerp( kfa[i].co, kfb[i].co, t, kfd[i].co );
+      q_nlerp( kfa[i].q,  kfb[i].q,  t, kfd[i].q );
+      v3_lerp( kfa[i].s,  kfb[i].s,  t, kfd[i].s );
+   }
+}
+
+static void skeleton_lerp_pose( struct skeleton *skele,
+                                mdl_keyframe *kfa, mdl_keyframe *kfb, float t,
+                                mdl_keyframe *kfd )
+{
+   keyframe_lerp_pose( kfa, kfb, t, kfd, skele->bone_count-1 );
+}
+
+/*
+ * Sample animation between 2 closest frames using time value. Output is a
+ * keyframe buffer that is allocated with an appropriate size
+ */
+static void skeleton_sample_anim( struct skeleton *skele,
+                                  struct skeleton_anim *anim,
+                                  float time,
+                                  mdl_keyframe *output )
+{
+   float animtime = time*anim->rate;
+
+   u32 frame = ((u32)animtime) % anim->length,
+       next  = (frame+1) % anim->length;
+
+   float t = vg_fractf( animtime );
+
+   mdl_keyframe *base  = anim->anim_data + (skele->bone_count-1)*frame,
+                *nbase = anim->anim_data + (skele->bone_count-1)*next;
+
+   skeleton_lerp_pose( skele, base, nbase, t, output );
+}
+
+typedef enum anim_apply
+{
+   k_anim_apply_always,
+   k_anim_apply_defer_ik,
+   k_anim_apply_deffered_only
+}
+anim_apply;
+
+static int should_apply_bone( struct skeleton *skele, u32 id, anim_apply type )
+{
+   struct skeleton_bone *sb = &skele->bones[ id ],
+                        *sp = &skele->bones[ sb->parent ];
+
+   if( type == k_anim_apply_defer_ik )
+   {
+      if( sp->ik || sp->defer )
+      {
+         sb->defer = 1;
+         return 0;
+      }
+   }
+   else if( type == k_anim_apply_deffered_only )
+   {
+      if( !sp->defer )
+         return 0;
+   }
+
+   return 1;
+}
+
+/*
+ * Apply block of keyframes to skeletons final pose
+ */
+static void skeleton_apply_pose( m4x3f transform,
+                                 struct skeleton *skele, mdl_keyframe *pose,
+                                 anim_apply passtype )
+{
+   m4x3_copy( transform, skele->final_transforms[0] );
+   skele->bones[0].defer = 0;
+   skele->bones[0].ik = 0;
+
+   for( int i=1; i<skele->bone_count; i++ )
+   {
+      struct skeleton_bone *sb = &skele->bones[i],
+                           *sp = &skele->bones[ sb->parent ];
+      
+      if( !should_apply_bone( skele, i, passtype ) )
+         continue;
+
+      sb->defer = 0;
+
+      /* process pose */
+      m4x3f posemtx;
+
+      v3f temp_delta;
+      v3_sub( skele->bones[i].co, skele->bones[sb->parent].co, temp_delta );
+
+      /* pose matrix */
+      mdl_keyframe *kf = &pose[i-1];
+      q_m3x3( kf->q, posemtx );
+      v3_copy( kf->co, posemtx[3] );
+      v3_add( temp_delta, posemtx[3], posemtx[3] );
+
+      /* final matrix */
+      m4x3_mul( skele->final_transforms[ sb->parent ], posemtx, 
+                skele->final_transforms[i] );
+   }
+
+   /* bone space inverse matrix ( for verts ) TODO: move to seperate pass */
+   for( int i=1; i<skele->bone_count; i++ )
+   {
+      if( !should_apply_bone( skele, i, passtype ) )
+         continue;
+
+      m4x3f abmtx;
+      m3x3_identity( abmtx );
+      v3_negate( skele->bones[i].co, abmtx[3] );
+      m4x3_mul( skele->final_transforms[i], abmtx, skele->final_transforms[i] );
+   }
+}
+
 static void skeleton_apply_frame( m4x3f transform,
                                   struct skeleton *skele,
                                   struct skeleton_anim *anim, 
@@ -398,3 +1060,4 @@ static void skeleton_debug( struct skeleton *skele )
 }
 
 #endif /* SKELETON_H */
+#endif