interpolation problems

[carveJwlIkooP6JGAAIwe30JlM.git] / player_ragdoll.h

diff --git a/player_ragdoll.h b/player_ragdoll.h
index d6607ae89331a9ef7ec2883e37099f7665043f73..40ee45158ded5b131eff9649bba16d4691c922cc 100644 (file)
--- a/player_ragdoll.h
+++ b/player_ragdoll.h
@@ -17,6 +17,9 @@ struct player_ragdoll{
       m4x3f collider_mtx,
             inv_collider_mtx;
 
+      v4f prev_q;
+      v3f prev_co;
+
       u32 use_limits;
       v3f limits[2];
 
@@ -232,10 +235,20 @@ VG_STATIC void copy_ragdoll_pose_to_avatar( struct player_ragdoll *rd,
 {
    for( int i=0; i<rd->part_count; i++ ){
       struct ragdoll_part *part = &rd->parts[i];
-      m4x3f offset;
-      m3x3_identity(offset);
-      m4x3_mul( part->obj.rb.to_world, part->inv_collider_mtx, 
-                  av->sk.final_mtx[part->bone_id] );
+
+      m4x3f mtx;
+
+      v4f q_int;
+      v3f co_int;
+
+      float substep = vg.time_fixed_extrapolate;
+      v3_lerp( part->prev_co, part->obj.rb.co, substep, co_int );
+      q_nlerp( part->prev_q, part->obj.rb.q, substep, q_int );
+
+      q_m3x3( q_int, mtx );
+      v3_copy( co_int, mtx[3] );
+
+      m4x3_mul( mtx, part->inv_collider_mtx, av->sk.final_mtx[part->bone_id] );
    }
 
    for( u32 i=1; i<av->sk.bone_count; i++ ){
@@ -280,6 +293,9 @@ VG_STATIC void copy_avatar_pose_to_ragdoll( struct player_avatar *av,
 
       v3_copy( velocity, part->obj.rb.v );
       v3_zero( part->obj.rb.w );
+
+      v3_copy( part->obj.rb.co, part->prev_co );
+      v4_copy( part->obj.rb.q, part->prev_q );
       
       rb_update_transform( &part->obj.rb );
    }
@@ -312,6 +328,9 @@ VG_STATIC void player_ragdoll_iter( struct player_ragdoll *rd )
    float contact_velocities[256];
 
    for( int i=0; i<rd->part_count; i ++ ){
+      v4_copy( rd->parts[i].obj.rb.q, rd->parts[i].prev_q );
+      v3_copy( rd->parts[i].obj.rb.co, rd->parts[i].prev_co );
+
       if( rb_global_has_space() ){
          rb_ct *buf = rb_global_buffer();