2 * Copyright (C) 2021-2022 Mt.ZERO Software, Harry Godden - All Rights Reserved
6 #include "vg/vg_lines.h"
35 mdl_keyframe
*anim_data
;
46 u32 lower
, upper
, target
, pole
;
57 static u32
skeleton_bone_id( struct skeleton
*skele
, const char *name
)
59 for( u32 i
=1; i
<skele
->bone_count
; i
++ ){
60 if( !strcmp( skele
->bones
[i
].name
, name
))
64 vg_error( "skeleton_bone_id( *, \"%s\" );\n", name
);
65 vg_fatal_error( "Bone does not exist\n" );
70 static void keyframe_copy_pose( mdl_keyframe
*kfa
, mdl_keyframe
*kfb
,
73 for( int i
=0; i
<num
; i
++ )
78 /* apply a rotation from the perspective of root */
79 static void keyframe_rotate_around( mdl_keyframe
*kf
,
80 v3f origin
, v3f offset
, v4f q
)
83 v3_add( kf
->co
, offset
, co
);
84 v3_sub( co
, origin
, v0
);
86 v3_add( v0
, origin
, co
);
87 v3_sub( co
, offset
, kf
->co
);
89 q_mul( q
, kf
->q
, kf
->q
);
93 static void keyframe_lerp( mdl_keyframe
*kfa
, mdl_keyframe
*kfb
, f32 t
,
95 v3_lerp( kfa
->co
, kfb
->co
, t
, kfd
->co
);
96 q_nlerp( kfa
->q
, kfb
->q
, t
, kfd
->q
);
97 v3_lerp( kfa
->s
, kfb
->s
, t
, kfd
->s
);
101 * Lerp between two sets of keyframes and store in dest. Rotations use Nlerp.
103 static void keyframe_lerp_pose( mdl_keyframe
*kfa
, mdl_keyframe
*kfb
,
104 float t
, mdl_keyframe
*kfd
, int count
){
106 keyframe_copy_pose( kfa
, kfd
, count
);
109 else if( t
>= 0.9999f
){
110 keyframe_copy_pose( kfb
, kfd
, count
);
114 for( int i
=0; i
<count
; i
++ )
115 keyframe_lerp( kfa
+i
, kfb
+i
, t
, kfd
+i
);
119 void skeleton_lerp_pose( struct skeleton
*skele
,
120 mdl_keyframe
*kfa
, mdl_keyframe
*kfb
, float t
,
123 keyframe_lerp_pose( kfa
, kfb
, t
, kfd
, skele
->bone_count
-1 );
126 static void skeleton_copy_pose( struct skeleton
*skele
,
127 mdl_keyframe
*kfa
, mdl_keyframe
*kfd
)
129 keyframe_copy_pose( kfa
, kfd
, skele
->bone_count
-1 );
133 * Sample animation between 2 closest frames using time value. Output is a
134 * keyframe buffer that is allocated with an appropriate size
136 static void skeleton_sample_anim( struct skeleton
*skele
,
137 struct skeleton_anim
*anim
,
139 mdl_keyframe
*output
)
141 f32 animtime
= fmodf( time
*anim
->rate
, anim
->length
),
142 animframe
= floorf( animtime
),
143 t
= animtime
- animframe
;
145 u32 frame
= (u32
)animframe
% anim
->length
,
146 next
= (frame
+1) % anim
->length
;
148 mdl_keyframe
*base
= anim
->anim_data
+ (skele
->bone_count
-1)*frame
,
149 *nbase
= anim
->anim_data
+ (skele
->bone_count
-1)*next
;
151 skeleton_lerp_pose( skele
, base
, nbase
, t
, output
);
154 static int skeleton_sample_anim_clamped( struct skeleton
*skele
,
155 struct skeleton_anim
*anim
,
157 mdl_keyframe
*output
)
159 float end
= (float)(anim
->length
-1) / anim
->rate
;
160 skeleton_sample_anim( skele
, anim
, vg_minf( end
, time
), output
);
168 typedef enum anim_apply
171 k_anim_apply_defer_ik
,
172 k_anim_apply_deffered_only
,
173 k_anim_apply_absolute
178 int should_apply_bone( struct skeleton
*skele
, u32 id
, anim_apply type
)
180 struct skeleton_bone
*sb
= &skele
->bones
[ id
],
181 *sp
= &skele
->bones
[ sb
->parent
];
183 if( type
== k_anim_apply_defer_ik
){
184 if( ((sp
->flags
& k_bone_flag_ik
) && !(sb
->flags
& k_bone_flag_ik
))
195 else if( type
== k_anim_apply_deffered_only
){
206 * Apply block of keyframes to skeletons final pose
208 static void skeleton_apply_pose( struct skeleton
*skele
, mdl_keyframe
*pose
,
209 anim_apply passtype
, m4x3f
*final_mtx
){
210 if( passtype
== k_anim_apply_absolute
){
211 for( u32 i
=1; i
<skele
->bone_count
; i
++ ){
212 mdl_keyframe
*kf
= &pose
[i
-1];
214 v3f
*posemtx
= final_mtx
[i
];
216 q_m3x3( kf
->q
, posemtx
);
217 m3x3_scale( posemtx
, kf
->s
);
218 v3_copy( kf
->co
, posemtx
[3] );
223 m4x3_identity( final_mtx
[0] );
224 skele
->bones
[0].defer
= 0;
225 skele
->bones
[0].flags
&= ~k_bone_flag_ik
;
227 for( u32 i
=1; i
<skele
->bone_count
; i
++ ){
228 struct skeleton_bone
*sb
= &skele
->bones
[i
],
229 *sp
= &skele
->bones
[sb
->parent
];
231 if( !should_apply_bone( skele
, i
, passtype
) )
240 v3_sub( skele
->bones
[i
].co
, skele
->bones
[sb
->parent
].co
, temp_delta
);
243 mdl_keyframe
*kf
= &pose
[i
-1];
244 q_m3x3( kf
->q
, posemtx
);
245 m3x3_scale( posemtx
, kf
->s
);
246 v3_copy( kf
->co
, posemtx
[3] );
247 v3_add( temp_delta
, posemtx
[3], posemtx
[3] );
250 m4x3_mul( final_mtx
[ sb
->parent
], posemtx
, final_mtx
[i
] );
255 * Take the final matrices and decompose it into an absolute positioned anim
257 static void skeleton_decompose_mtx_absolute( struct skeleton
*skele
,
260 for( u32 i
=1; i
<skele
->bone_count
; i
++ ){
261 struct skeleton_bone
*sb
= &skele
->bones
[i
];
262 mdl_keyframe
*kf
= &anim
[i
-1];
263 m4x3_decompose( final_mtx
[i
], kf
->co
, kf
->q
, kf
->s
);
268 * creates the reference inverse matrix for an IK bone, as it has an initial
269 * intrisic rotation based on the direction that the IK is setup..
271 static void skeleton_inverse_for_ik( struct skeleton
*skele
,
273 u32 id
, m3x3f inverse
)
275 v3_copy( ivaxis
, inverse
[0] );
276 v3_copy( skele
->bones
[id
].end
, inverse
[1] );
277 v3_normalize( inverse
[1] );
278 v3_cross( inverse
[0], inverse
[1], inverse
[2] );
279 m3x3_transpose( inverse
, inverse
);
283 * Creates inverse rotation matrices which the IK system uses.
285 static void skeleton_create_inverses( struct skeleton
*skele
)
287 /* IK: inverse 'plane-bone space' axis '(^axis,^bone,...)[base] */
288 for( u32 i
=0; i
<skele
->ik_count
; i
++ ){
289 struct skeleton_ik
*ik
= &skele
->ik
[i
];
292 v3f iv0
, iv1
, ivaxis
;
293 v3_sub( skele
->bones
[ik
->target
].co
, skele
->bones
[ik
->lower
].co
, iv0
);
294 v3_sub( skele
->bones
[ik
->pole
].co
, skele
->bones
[ik
->lower
].co
, iv1
);
295 v3_cross( iv0
, iv1
, ivaxis
);
296 v3_normalize( ivaxis
);
298 skeleton_inverse_for_ik( skele
, ivaxis
, ik
->lower
, ik
->ia
);
299 skeleton_inverse_for_ik( skele
, ivaxis
, ik
->upper
, ik
->ib
);
304 * Apply a model matrix to all bones, should be done last
307 void skeleton_apply_transform( struct skeleton
*skele
, m4x3f transform
,
310 for( u32 i
=0; i
<skele
->bone_count
; i
++ ){
311 struct skeleton_bone
*sb
= &skele
->bones
[i
];
312 m4x3_mul( transform
, final_mtx
[i
], final_mtx
[i
] );
317 * Apply an inverse matrix to all bones which maps vertices from bind space into
318 * bone relative positions
320 static void skeleton_apply_inverses( struct skeleton
*skele
, m4x3f
*final_mtx
){
321 for( u32 i
=0; i
<skele
->bone_count
; i
++ ){
322 struct skeleton_bone
*sb
= &skele
->bones
[i
];
324 m3x3_identity( inverse
);
325 v3_negate( sb
->co
, inverse
[3] );
327 m4x3_mul( final_mtx
[i
], inverse
, final_mtx
[i
] );
332 * Apply all IK modifiers (2 bone ik reference from blender is supported)
334 static void skeleton_apply_ik_pass( struct skeleton
*skele
, m4x3f
*final_mtx
){
335 for( u32 i
=0; i
<skele
->ik_count
; i
++ ){
336 struct skeleton_ik
*ik
= &skele
->ik
[i
];
338 v3f v0
, /* base -> target */
339 v1
, /* base -> pole */
346 v3_copy( final_mtx
[ik
->lower
][3], co_base
);
347 v3_copy( final_mtx
[ik
->target
][3], co_target
);
348 v3_copy( final_mtx
[ik
->pole
][3], co_pole
);
350 v3_sub( co_target
, co_base
, v0
);
351 v3_sub( co_pole
, co_base
, v1
);
352 v3_cross( v0
, v1
, vaxis
);
353 v3_normalize( vaxis
);
355 v3_cross( vaxis
, v0
, v1
);
357 /* localize problem into [x:v0,y:v1] 2d plane */
358 v2f base
= { v3_dot( v0
, co_base
), v3_dot( v1
, co_base
) },
359 end
= { v3_dot( v0
, co_target
), v3_dot( v1
, co_target
) },
362 /* Compute angles (basic trig)*/
364 v2_sub( end
, base
, delta
);
367 l1
= v3_length( skele
->bones
[ik
->lower
].end
),
368 l2
= v3_length( skele
->bones
[ik
->upper
].end
),
369 d
= vg_clampf( v2_length(delta
), fabsf(l1
- l2
), l1
+l2
-0.00001f
),
370 c
= acosf( (l1
*l1
+ d
*d
- l2
*l2
) / (2.0f
*l1
*d
) ),
371 rot
= atan2f( delta
[1], delta
[0] ) + c
- VG_PIf
/2.0f
;
373 knee
[0] = sinf(-rot
) * l1
;
374 knee
[1] = cosf(-rot
) * l1
;
376 m4x3_identity( final_mtx
[ik
->lower
] );
377 m4x3_identity( final_mtx
[ik
->upper
] );
379 /* create rotation matrix */
381 v3_muladds( co_base
, v0
, knee
[0], co_knee
);
382 v3_muladds( co_knee
, v1
, knee
[1], co_knee
);
383 vg_line( co_base
, co_knee
, 0xff00ff00 );
386 v3_copy( vaxis
, transform
[0] );
387 v3_muls( v0
, knee
[0], transform
[1] );
388 v3_muladds( transform
[1], v1
, knee
[1], transform
[1] );
389 v3_normalize( transform
[1] );
390 v3_cross( transform
[0], transform
[1], transform
[2] );
391 v3_copy( co_base
, transform
[3] );
393 m3x3_mul( transform
, ik
->ia
, transform
);
394 m4x3_copy( transform
, final_mtx
[ik
->lower
] );
396 /* upper/knee bone */
397 v3_copy( vaxis
, transform
[0] );
398 v3_sub( co_target
, co_knee
, transform
[1] );
399 v3_normalize( transform
[1] );
400 v3_cross( transform
[0], transform
[1], transform
[2] );
401 v3_copy( co_knee
, transform
[3] );
403 m3x3_mul( transform
, ik
->ib
, transform
);
404 m4x3_copy( transform
, final_mtx
[ik
->upper
] );
409 * Applies the typical operations that you want for an IK rig:
410 * Pose, IK, Pose(deferred), Inverses, Transform
412 static void skeleton_apply_standard( struct skeleton
*skele
, mdl_keyframe
*pose
,
413 m4x3f transform
, m4x3f
*final_mtx
){
414 skeleton_apply_pose( skele
, pose
, k_anim_apply_defer_ik
, final_mtx
);
415 skeleton_apply_ik_pass( skele
, final_mtx
);
416 skeleton_apply_pose( skele
, pose
, k_anim_apply_deffered_only
, final_mtx
);
417 skeleton_apply_inverses( skele
, final_mtx
);
418 skeleton_apply_transform( skele
, transform
, final_mtx
);
422 * Get an animation by name
424 static struct skeleton_anim
*skeleton_get_anim( struct skeleton
*skele
,
426 for( u32 i
=0; i
<skele
->anim_count
; i
++ ){
427 struct skeleton_anim
*anim
= &skele
->anims
[i
];
429 if( !strcmp( anim
->name
, name
) )
433 vg_error( "skeleton_get_anim( *, \"%s\" )\n", name
);
434 vg_fatal_error( "Invalid animation name\n" );
439 static void skeleton_alloc_from( struct skeleton
*skele
,
442 mdl_armature
*armature
){
443 skele
->bone_count
= armature
->bone_count
+1;
444 skele
->anim_count
= armature
->anim_count
;
446 skele
->collider_count
= 0;
448 for( u32 i
=0; i
<armature
->bone_count
; i
++ ){
449 mdl_bone
*bone
= mdl_arritm( &mdl
->bones
, armature
->bone_start
+i
);
451 if( bone
->flags
& k_bone_flag_ik
)
455 skele
->collider_count
++;
458 u32 bone_size
= sizeof(struct skeleton_bone
) * skele
->bone_count
,
459 ik_size
= sizeof(struct skeleton_ik
) * skele
->ik_count
,
460 mtx_size
= sizeof(m4x3f
) * skele
->bone_count
,
461 anim_size
= sizeof(struct skeleton_anim
) * skele
->anim_count
;
463 skele
->bones
= vg_linear_alloc( lin_alloc
, bone_size
);
464 skele
->ik
= vg_linear_alloc( lin_alloc
, ik_size
);
465 //skele->final_mtx = vg_linear_alloc( lin_alloc, mtx_size );
466 skele
->anims
= vg_linear_alloc( lin_alloc
, anim_size
);
468 memset( skele
->bones
, 0, bone_size
);
469 memset( skele
->ik
, 0, ik_size
);
470 //memset( skele->final_mtx, 0, mtx_size );
471 memset( skele
->anims
, 0, anim_size
);
474 static void skeleton_fatal_err(void){
475 vg_fatal_error( "Skeleton setup failed" );
478 /* Setup an animated skeleton from model. mdl's metadata should stick around */
479 static void skeleton_setup( struct skeleton
*skele
,
480 void *lin_alloc
, mdl_context
*mdl
){
481 u32 ik_count
= 0, collider_count
= 0;
482 skele
->bone_count
= 0;
484 //skele->final_mtx = NULL;
487 if( !mdl
->armatures
.count
){
488 vg_error( "No skeleton in model\n" );
489 skeleton_fatal_err();
492 mdl_armature
*armature
= mdl_arritm( &mdl
->armatures
, 0 );
493 skeleton_alloc_from( skele
, lin_alloc
, mdl
, armature
);
495 for( u32 i
=0; i
<armature
->bone_count
; i
++ ){
496 mdl_bone
*bone
= mdl_arritm( &mdl
->bones
, armature
->bone_start
+i
);
497 struct skeleton_bone
*sb
= &skele
->bones
[i
+1];
499 v3_copy( bone
->co
, sb
->co
);
500 v3_copy( bone
->end
, sb
->end
);
502 sb
->parent
= bone
->parent
;
503 sb
->name
= mdl_pstr( mdl
, bone
->pstr_name
);
504 sb
->flags
= bone
->flags
;
505 sb
->collider
= bone
->collider
;
506 sb
->orig_bone
= bone
;
508 if( sb
->flags
& k_bone_flag_ik
){
509 skele
->bones
[ sb
->parent
].flags
|= k_bone_flag_ik
;
511 if( ik_count
== skele
->ik_count
){
512 vg_error( "Too many ik bones, corrupt model file\n" );
513 skeleton_fatal_err();
516 struct skeleton_ik
*ik
= &skele
->ik
[ ik_count
++ ];
518 ik
->lower
= bone
->parent
;
519 ik
->target
= bone
->ik_target
;
520 ik
->pole
= bone
->ik_pole
;
523 box_copy( bone
->hitbox
, sb
->hitbox
);
525 if( bone
->collider
){
526 if( collider_count
== skele
->collider_count
){
527 vg_error( "Too many collider bones\n" );
528 skeleton_fatal_err();
535 /* fill in implicit root bone */
536 v3_zero( skele
->bones
[0].co
);
537 v3_copy( (v3f
){0.0f
,1.0f
,0.0f
}, skele
->bones
[0].end
);
538 skele
->bones
[0].parent
= 0xffffffff;
539 skele
->bones
[0].flags
= 0;
540 skele
->bones
[0].name
= "[root]";
542 /* process animation quick refs */
543 for( u32 i
=0; i
<skele
->anim_count
; i
++ ){
544 mdl_animation
*anim
=
545 mdl_arritm( &mdl
->animations
, armature
->anim_start
+i
);
547 skele
->anims
[i
].rate
= anim
->rate
;
548 skele
->anims
[i
].length
= anim
->length
;
549 skele
->anims
[i
].name
= mdl_pstr(mdl
, anim
->pstr_name
);
550 skele
->anims
[i
].anim_data
=
551 mdl_arritm( &mdl
->keyframes
, anim
->offset
);
553 vg_info( "animation[ %f, %u ] '%s'\n", anim
->rate
,
555 skele
->anims
[i
].name
);
558 skeleton_create_inverses( skele
);
559 vg_success( "Loaded skeleton with %u bones\n", skele
->bone_count
);
560 vg_success( " %u colliders\n", skele
->collider_count
);
563 static void skeleton_debug( struct skeleton
*skele
, m4x3f
*final_mtx
){
564 for( u32 i
=1; i
<skele
->bone_count
; i
++ ){
565 struct skeleton_bone
*sb
= &skele
->bones
[i
];
568 v3_copy( sb
->co
, p0
);
569 v3_add( p0
, sb
->end
, p1
);
571 m4x3_mulv( final_mtx
[i
], p0
, p0
);
572 m4x3_mulv( final_mtx
[i
], p1
, p1
);
574 if( sb
->flags
& k_bone_flag_deform
){
575 if( sb
->flags
& k_bone_flag_ik
){
576 vg_line( p0
, p1
, 0xff0000ff );
579 vg_line( p0
, p1
, 0xffcccccc );
583 vg_line( p0
, p1
, 0xff00ffff );