small refactor of model loading
[carveJwlIkooP6JGAAIwe30JlM.git] / skeleton.h
1 /*
2 * Copyright (C) 2021-2022 Mt.ZERO Software, Harry Godden - All Rights Reserved
3 */
4
5 #pragma once
6 #include "vg/vg_lines.h"
7 #include "model.h"
8
9 struct skeleton
10 {
11 struct skeleton_bone
12 {
13 v3f co, end;
14 u32 parent;
15
16 u32 flags;
17 int defer;
18
19 mdl_keyframe kf;
20 mdl_bone *orig_bone;
21
22 u32 collider;
23 boxf hitbox;
24 const char *name;
25 }
26 *bones;
27 u32 bone_count;
28
29 struct skeleton_anim
30 {
31 const char *name;
32 u32 length;
33
34 float rate;
35 mdl_keyframe *anim_data;
36 }
37 *anims;
38 u32 anim_count;
39
40 #if 0
41 m4x3f *final_mtx;
42 #endif
43
44 struct skeleton_ik
45 {
46 u32 lower, upper, target, pole;
47 m3x3f ia, ib;
48 }
49 *ik;
50 u32 ik_count;
51
52 u32
53 collider_count,
54 bindable_count;
55 };
56
57 static u32 skeleton_bone_id( struct skeleton *skele, const char *name )
58 {
59 for( u32 i=1; i<skele->bone_count; i++ ){
60 if( !strcmp( skele->bones[i].name, name ))
61 return i;
62 }
63
64 vg_error( "skeleton_bone_id( *, \"%s\" );\n", name );
65 vg_fatal_error( "Bone does not exist\n" );
66
67 return 0;
68 }
69
70 static void keyframe_copy_pose( mdl_keyframe *kfa, mdl_keyframe *kfb,
71 int num )
72 {
73 for( int i=0; i<num; i++ )
74 kfb[i] = kfa[i];
75 }
76
77
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 )
81 {
82 v3f v0, co;
83 v3_add( kf->co, offset, co );
84 v3_sub( co, origin, v0 );
85 q_mulv( q, v0, v0 );
86 v3_add( v0, origin, co );
87 v3_sub( co, offset, kf->co );
88
89 q_mul( q, kf->q, kf->q );
90 q_normalize( kf->q );
91 }
92
93 static void keyframe_lerp( mdl_keyframe *kfa, mdl_keyframe *kfb, f32 t,
94 mdl_keyframe *kfd ){
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 );
98 }
99
100 /*
101 * Lerp between two sets of keyframes and store in dest. Rotations use Nlerp.
102 */
103 static void keyframe_lerp_pose( mdl_keyframe *kfa, mdl_keyframe *kfb,
104 float t, mdl_keyframe *kfd, int count ){
105 if( t <= 0.0001f ){
106 keyframe_copy_pose( kfa, kfd, count );
107 return;
108 }
109 else if( t >= 0.9999f ){
110 keyframe_copy_pose( kfb, kfd, count );
111 return;
112 }
113
114 for( int i=0; i<count; i++ )
115 keyframe_lerp( kfa+i, kfb+i, t, kfd+i );
116 }
117
118 static
119 void skeleton_lerp_pose( struct skeleton *skele,
120 mdl_keyframe *kfa, mdl_keyframe *kfb, float t,
121 mdl_keyframe *kfd )
122 {
123 keyframe_lerp_pose( kfa, kfb, t, kfd, skele->bone_count-1 );
124 }
125
126 static void skeleton_copy_pose( struct skeleton *skele,
127 mdl_keyframe *kfa, mdl_keyframe *kfd )
128 {
129 keyframe_copy_pose( kfa, kfd, skele->bone_count-1 );
130 }
131
132 /*
133 * Sample animation between 2 closest frames using time value. Output is a
134 * keyframe buffer that is allocated with an appropriate size
135 */
136 static void skeleton_sample_anim( struct skeleton *skele,
137 struct skeleton_anim *anim,
138 float time,
139 mdl_keyframe *output )
140 {
141 f32 animtime = fmodf( time*anim->rate, anim->length ),
142 animframe = floorf( animtime ),
143 t = animtime - animframe;
144
145 u32 frame = (u32)animframe % anim->length,
146 next = (frame+1) % anim->length;
147
148 mdl_keyframe *base = anim->anim_data + (skele->bone_count-1)*frame,
149 *nbase = anim->anim_data + (skele->bone_count-1)*next;
150
151 skeleton_lerp_pose( skele, base, nbase, t, output );
152 }
153
154 static int skeleton_sample_anim_clamped( struct skeleton *skele,
155 struct skeleton_anim *anim,
156 float time,
157 mdl_keyframe *output )
158 {
159 float end = (float)(anim->length-1) / anim->rate;
160 skeleton_sample_anim( skele, anim, vg_minf( end, time ), output );
161
162 if( time > end )
163 return 0;
164 else
165 return 1;
166 }
167
168 typedef enum anim_apply
169 {
170 k_anim_apply_always,
171 k_anim_apply_defer_ik,
172 k_anim_apply_deffered_only,
173 k_anim_apply_absolute
174 }
175 anim_apply;
176
177 static
178 int should_apply_bone( struct skeleton *skele, u32 id, anim_apply type )
179 {
180 struct skeleton_bone *sb = &skele->bones[ id ],
181 *sp = &skele->bones[ sb->parent ];
182
183 if( type == k_anim_apply_defer_ik ){
184 if( ((sp->flags & k_bone_flag_ik) && !(sb->flags & k_bone_flag_ik))
185 || sp->defer )
186 {
187 sb->defer = 1;
188 return 0;
189 }
190 else{
191 sb->defer = 0;
192 return 1;
193 }
194 }
195 else if( type == k_anim_apply_deffered_only ){
196 if( sb->defer )
197 return 1;
198 else
199 return 0;
200 }
201
202 return 1;
203 }
204
205 /*
206 * Apply block of keyframes to skeletons final pose
207 */
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];
213
214 v3f *posemtx = final_mtx[i];
215
216 q_m3x3( kf->q, posemtx );
217 m3x3_scale( posemtx, kf->s );
218 v3_copy( kf->co, posemtx[3] );
219 }
220 return;
221 }
222
223 m4x3_identity( final_mtx[0] );
224 skele->bones[0].defer = 0;
225 skele->bones[0].flags &= ~k_bone_flag_ik;
226
227 for( u32 i=1; i<skele->bone_count; i++ ){
228 struct skeleton_bone *sb = &skele->bones[i],
229 *sp = &skele->bones[sb->parent];
230
231 if( !should_apply_bone( skele, i, passtype ) )
232 continue;
233
234 sb->defer = 0;
235
236 /* process pose */
237 m4x3f posemtx;
238
239 v3f temp_delta;
240 v3_sub( skele->bones[i].co, skele->bones[sb->parent].co, temp_delta );
241
242 /* pose matrix */
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] );
248
249 /* final matrix */
250 m4x3_mul( final_mtx[ sb->parent ], posemtx, final_mtx[i] );
251 }
252 }
253
254 /*
255 * Take the final matrices and decompose it into an absolute positioned anim
256 */
257 static void skeleton_decompose_mtx_absolute( struct skeleton *skele,
258 mdl_keyframe *anim,
259 m4x3f *final_mtx ){
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 );
264 }
265 }
266
267 /*
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..
270 */
271 static void skeleton_inverse_for_ik( struct skeleton *skele,
272 v3f ivaxis,
273 u32 id, m3x3f inverse )
274 {
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 );
280 }
281
282 /*
283 * Creates inverse rotation matrices which the IK system uses.
284 */
285 static void skeleton_create_inverses( struct skeleton *skele )
286 {
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];
290
291 m4x3f inverse;
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 );
297
298 skeleton_inverse_for_ik( skele, ivaxis, ik->lower, ik->ia );
299 skeleton_inverse_for_ik( skele, ivaxis, ik->upper, ik->ib );
300 }
301 }
302
303 /*
304 * Apply a model matrix to all bones, should be done last
305 */
306 static
307 void skeleton_apply_transform( struct skeleton *skele, m4x3f transform,
308 m4x3f *final_mtx )
309 {
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] );
313 }
314 }
315
316 /*
317 * Apply an inverse matrix to all bones which maps vertices from bind space into
318 * bone relative positions
319 */
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];
323 m4x3f inverse;
324 m3x3_identity( inverse );
325 v3_negate( sb->co, inverse[3] );
326
327 m4x3_mul( final_mtx[i], inverse, final_mtx[i] );
328 }
329 }
330
331 /*
332 * Apply all IK modifiers (2 bone ik reference from blender is supported)
333 */
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];
337
338 v3f v0, /* base -> target */
339 v1, /* base -> pole */
340 vaxis;
341
342 v3f co_base,
343 co_target,
344 co_pole;
345
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 );
349
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 );
354 v3_normalize( v0 );
355 v3_cross( vaxis, v0, v1 );
356
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 ) },
360 knee;
361
362 /* Compute angles (basic trig)*/
363 v2f delta;
364 v2_sub( end, base, delta );
365
366 float
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;
372
373 knee[0] = sinf(-rot) * l1;
374 knee[1] = cosf(-rot) * l1;
375
376 m4x3_identity( final_mtx[ik->lower] );
377 m4x3_identity( final_mtx[ik->upper] );
378
379 /* create rotation matrix */
380 v3f co_knee;
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 );
384
385 m4x3f transform;
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] );
392
393 m3x3_mul( transform, ik->ia, transform );
394 m4x3_copy( transform, final_mtx[ik->lower] );
395
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] );
402
403 m3x3_mul( transform, ik->ib, transform );
404 m4x3_copy( transform, final_mtx[ik->upper] );
405 }
406 }
407
408 /*
409 * Applies the typical operations that you want for an IK rig:
410 * Pose, IK, Pose(deferred), Inverses, Transform
411 */
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 );
419 }
420
421 /*
422 * Get an animation by name
423 */
424 static struct skeleton_anim *skeleton_get_anim( struct skeleton *skele,
425 const char *name ){
426 for( u32 i=0; i<skele->anim_count; i++ ){
427 struct skeleton_anim *anim = &skele->anims[i];
428
429 if( !strcmp( anim->name, name ) )
430 return anim;
431 }
432
433 vg_error( "skeleton_get_anim( *, \"%s\" )\n", name );
434 vg_fatal_error( "Invalid animation name\n" );
435
436 return NULL;
437 }
438
439 static void skeleton_alloc_from( struct skeleton *skele,
440 void *lin_alloc,
441 mdl_context *mdl,
442 mdl_armature *armature ){
443 skele->bone_count = armature->bone_count+1;
444 skele->anim_count = armature->anim_count;
445 skele->ik_count = 0;
446 skele->collider_count = 0;
447
448 for( u32 i=0; i<armature->bone_count; i++ ){
449 mdl_bone *bone = mdl_arritm( &mdl->bones, armature->bone_start+i );
450
451 if( bone->flags & k_bone_flag_ik )
452 skele->ik_count ++;
453
454 if( bone->collider )
455 skele->collider_count ++;
456 }
457
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;
462
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 );
467
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 );
472 }
473
474 static void skeleton_fatal_err(void){
475 vg_fatal_error( "Skeleton setup failed" );
476 }
477
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;
483 skele->bones = NULL;
484 //skele->final_mtx = NULL;
485 skele->anims = NULL;
486
487 if( !mdl->armatures.count ){
488 vg_error( "No skeleton in model\n" );
489 skeleton_fatal_err();
490 }
491
492 mdl_armature *armature = mdl_arritm( &mdl->armatures, 0 );
493 skeleton_alloc_from( skele, lin_alloc, mdl, armature );
494
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];
498
499 v3_copy( bone->co, sb->co );
500 v3_copy( bone->end, sb->end );
501
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;
507
508 if( sb->flags & k_bone_flag_ik ){
509 skele->bones[ sb->parent ].flags |= k_bone_flag_ik;
510
511 if( ik_count == skele->ik_count ){
512 vg_error( "Too many ik bones, corrupt model file\n" );
513 skeleton_fatal_err();
514 }
515
516 struct skeleton_ik *ik = &skele->ik[ ik_count ++ ];
517 ik->upper = i+1;
518 ik->lower = bone->parent;
519 ik->target = bone->ik_target;
520 ik->pole = bone->ik_pole;
521 }
522
523 box_copy( bone->hitbox, sb->hitbox );
524
525 if( bone->collider ){
526 if( collider_count == skele->collider_count ){
527 vg_error( "Too many collider bones\n" );
528 skeleton_fatal_err();
529 }
530
531 collider_count ++;
532 }
533 }
534
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]";
541
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 );
546
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 );
552
553 vg_info( "animation[ %f, %u ] '%s'\n", anim->rate,
554 anim->length,
555 skele->anims[i].name );
556 }
557
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 );
561 }
562
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];
566
567 v3f p0, p1;
568 v3_copy( sb->co, p0 );
569 v3_add( p0, sb->end, p1 );
570
571 m4x3_mulv( final_mtx[i], p0, p0 );
572 m4x3_mulv( final_mtx[i], p1, p1 );
573
574 if( sb->flags & k_bone_flag_deform ){
575 if( sb->flags & k_bone_flag_ik ){
576 vg_line( p0, p1, 0xff0000ff );
577 }
578 else{
579 vg_line( p0, p1, 0xffcccccc );
580 }
581 }
582 else
583 vg_line( p0, p1, 0xff00ffff );
584 }
585 }