a fairly major physics update
[carveJwlIkooP6JGAAIwe30JlM.git] / skeleton.h
1 /*
2 * Copyright (C) 2021-2022 Mt.ZERO Software, Harry Godden - All Rights Reserved
3 */
4
5 #ifndef SKELETON_H
6 #define SKELETON_H
7
8 #include "model.h"
9
10 struct skeleton
11 {
12 struct skeleton_bone
13 {
14 v3f co, end;
15 u32 parent;
16
17 u32 flags;
18 int defer;
19
20 mdl_keyframe kf;
21
22 u32 orig_node;
23
24 boxf hitbox;
25 const char *name;
26 }
27 *bones;
28 u32 bone_count;
29
30 struct skeleton_anim
31 {
32 const char *name;
33 u32 length;
34
35 float rate;
36 mdl_keyframe *anim_data;
37 }
38 *anims;
39 u32 anim_count;
40
41 m4x3f *final_mtx;
42
43 struct skeleton_ik
44 {
45 u32 lower, upper, target, pole;
46 m3x3f ia, ib;
47 }
48 *ik;
49 u32 ik_count;
50
51 u32
52 collider_count,
53 bindable_count;
54 };
55
56 VG_STATIC u32 skeleton_bone_id( struct skeleton *skele, const char *name )
57 {
58 for( u32 i=1; i<skele->bone_count; i++ )
59 {
60 if( !strcmp( skele->bones[i].name, name ))
61 return i;
62 }
63
64 return 0;
65 }
66
67 VG_STATIC void keyframe_copy_pose( mdl_keyframe *kfa, mdl_keyframe *kfb, int num )
68 {
69 for( int i=0; i<num; i++ )
70 kfb[i] = kfa[i];
71 }
72
73 /*
74 * Lerp between two sets of keyframes and store in dest. Rotations use Nlerp.
75 */
76 VG_STATIC void keyframe_lerp_pose( mdl_keyframe *kfa, mdl_keyframe *kfb, float t,
77 mdl_keyframe *kfd, int count )
78 {
79 if( t <= 0.01f )
80 {
81 keyframe_copy_pose( kfa, kfd, count );
82 return;
83 }
84 else if( t >= 0.99f )
85 {
86 keyframe_copy_pose( kfb, kfd, count );
87 return;
88 }
89
90 for( int i=0; i<count; i++ )
91 {
92 v3_lerp( kfa[i].co, kfb[i].co, t, kfd[i].co );
93 q_nlerp( kfa[i].q, kfb[i].q, t, kfd[i].q );
94 v3_lerp( kfa[i].s, kfb[i].s, t, kfd[i].s );
95 }
96 }
97
98 VG_STATIC void skeleton_lerp_pose( struct skeleton *skele,
99 mdl_keyframe *kfa, mdl_keyframe *kfb, float t,
100 mdl_keyframe *kfd )
101 {
102 keyframe_lerp_pose( kfa, kfb, t, kfd, skele->bone_count-1 );
103 }
104
105 /*
106 * Sample animation between 2 closest frames using time value. Output is a
107 * keyframe buffer that is allocated with an appropriate size
108 */
109 VG_STATIC void skeleton_sample_anim( struct skeleton *skele,
110 struct skeleton_anim *anim,
111 float time,
112 mdl_keyframe *output )
113 {
114 float animtime = time*anim->rate;
115
116 u32 frame = ((u32)animtime) % anim->length,
117 next = (frame+1) % anim->length;
118
119 float t = vg_fractf( animtime );
120
121 mdl_keyframe *base = anim->anim_data + (skele->bone_count-1)*frame,
122 *nbase = anim->anim_data + (skele->bone_count-1)*next;
123
124 skeleton_lerp_pose( skele, base, nbase, t, output );
125 }
126
127 VG_STATIC int skeleton_sample_anim_clamped( struct skeleton *skele,
128 struct skeleton_anim *anim,
129 float time,
130 mdl_keyframe *output )
131 {
132 float end = (float)(anim->length-1) / anim->rate;
133 skeleton_sample_anim( skele, anim, vg_minf( end, time ), output );
134
135 if( time > end )
136 return 0;
137 else
138 return 1;
139 }
140
141 typedef enum anim_apply
142 {
143 k_anim_apply_always,
144 k_anim_apply_defer_ik,
145 k_anim_apply_deffered_only
146 }
147 anim_apply;
148
149 VG_STATIC int should_apply_bone( struct skeleton *skele, u32 id, anim_apply type )
150 {
151 struct skeleton_bone *sb = &skele->bones[ id ],
152 *sp = &skele->bones[ sb->parent ];
153
154 if( type == k_anim_apply_defer_ik )
155 {
156 if( ((sp->flags & k_bone_flag_ik) && !(sb->flags & k_bone_flag_ik))
157 || sp->defer )
158 {
159 sb->defer = 1;
160 return 0;
161 }
162 else
163 {
164 sb->defer = 0;
165 return 1;
166 }
167 }
168 else if( type == k_anim_apply_deffered_only )
169 {
170 if( sb->defer )
171 return 1;
172 else
173 return 0;
174 }
175
176 return 1;
177 }
178
179 /*
180 * Apply block of keyframes to skeletons final pose
181 */
182 VG_STATIC void skeleton_apply_pose( struct skeleton *skele, mdl_keyframe *pose,
183 anim_apply passtype )
184 {
185 m4x3_identity( skele->final_mtx[0] );
186 skele->bones[0].defer = 0;
187 skele->bones[0].flags &= ~k_bone_flag_ik;
188
189 for( int i=1; i<skele->bone_count; i++ )
190 {
191 struct skeleton_bone *sb = &skele->bones[i],
192 *sp = &skele->bones[ sb->parent ];
193
194 if( !should_apply_bone( skele, i, passtype ) )
195 continue;
196
197 sb->defer = 0;
198
199 /* process pose */
200 m4x3f posemtx;
201
202 v3f temp_delta;
203 v3_sub( skele->bones[i].co, skele->bones[sb->parent].co, temp_delta );
204
205 /* pose matrix */
206 mdl_keyframe *kf = &pose[i-1];
207 q_m3x3( kf->q, posemtx );
208 v3_copy( kf->co, posemtx[3] );
209 v3_add( temp_delta, posemtx[3], posemtx[3] );
210
211 /* final matrix */
212 m4x3_mul( skele->final_mtx[ sb->parent ], posemtx, skele->final_mtx[i] );
213 }
214 }
215
216 /*
217 * creates the reference inverse matrix for an IK bone, as it has an initial
218 * intrisic rotation based on the direction that the IK is setup..
219 */
220 VG_STATIC void skeleton_inverse_for_ik( struct skeleton *skele,
221 v3f ivaxis,
222 u32 id, m3x3f inverse )
223 {
224 v3_copy( ivaxis, inverse[0] );
225 v3_copy( skele->bones[id].end, inverse[1] );
226 v3_normalize( inverse[1] );
227 v3_cross( inverse[0], inverse[1], inverse[2] );
228 m3x3_transpose( inverse, inverse );
229 }
230
231 /*
232 * Creates inverse rotation matrices which the IK system uses.
233 */
234 VG_STATIC void skeleton_create_inverses( struct skeleton *skele )
235 {
236 /* IK: inverse 'plane-bone space' axis '(^axis,^bone,...)[base] */
237 for( int i=0; i<skele->ik_count; i++ )
238 {
239 struct skeleton_ik *ik = &skele->ik[i];
240
241 m4x3f inverse;
242 v3f iv0, iv1, ivaxis;
243 v3_sub( skele->bones[ik->target].co, skele->bones[ik->lower].co, iv0 );
244 v3_sub( skele->bones[ik->pole].co, skele->bones[ik->lower].co, iv1 );
245 v3_cross( iv0, iv1, ivaxis );
246 v3_normalize( ivaxis );
247
248 skeleton_inverse_for_ik( skele, ivaxis, ik->lower, ik->ia );
249 skeleton_inverse_for_ik( skele, ivaxis, ik->upper, ik->ib );
250 }
251 }
252
253 /*
254 * Apply a model matrix to all bones, should be done last
255 */
256 VG_STATIC void skeleton_apply_transform( struct skeleton *skele, m4x3f transform )
257 {
258 for( int i=0; i<skele->bone_count; i++ )
259 {
260 struct skeleton_bone *sb = &skele->bones[i];
261 m4x3_mul( transform, skele->final_mtx[i], skele->final_mtx[i] );
262 }
263 }
264
265 /*
266 * Apply an inverse matrix to all bones which maps vertices from bind space into
267 * bone relative positions
268 */
269 VG_STATIC void skeleton_apply_inverses( struct skeleton *skele )
270 {
271 for( int i=0; i<skele->bone_count; i++ )
272 {
273 struct skeleton_bone *sb = &skele->bones[i];
274 m4x3f inverse;
275 m3x3_identity( inverse );
276 v3_negate( sb->co, inverse[3] );
277
278 m4x3_mul( skele->final_mtx[i], inverse, skele->final_mtx[i] );
279 }
280 }
281
282 /*
283 * Apply all IK modifiers (2 bone ik reference from blender is supported)
284 */
285 VG_STATIC void skeleton_apply_ik_pass( struct skeleton *skele )
286 {
287 for( int i=0; i<skele->ik_count; i++ )
288 {
289 struct skeleton_ik *ik = &skele->ik[i];
290
291 v3f v0, /* base -> target */
292 v1, /* base -> pole */
293 vaxis;
294
295 v3f co_base,
296 co_target,
297 co_pole;
298
299 v3_copy( skele->final_mtx[ik->lower][3], co_base );
300 v3_copy( skele->final_mtx[ik->target][3], co_target );
301 v3_copy( skele->final_mtx[ik->pole][3], co_pole );
302
303 v3_sub( co_target, co_base, v0 );
304 v3_sub( co_pole, co_base, v1 );
305 v3_cross( v0, v1, vaxis );
306 v3_normalize( vaxis );
307 v3_normalize( v0 );
308 v3_cross( vaxis, v0, v1 );
309
310 /* localize problem into [x:v0,y:v1] 2d plane */
311 v2f base = { v3_dot( v0, co_base ), v3_dot( v1, co_base ) },
312 end = { v3_dot( v0, co_target ), v3_dot( v1, co_target ) },
313 knee;
314
315 /* Compute angles (basic trig)*/
316 v2f delta;
317 v2_sub( end, base, delta );
318
319 float
320 l1 = v3_length( skele->bones[ik->lower].end ),
321 l2 = v3_length( skele->bones[ik->upper].end ),
322 d = vg_clampf( v2_length(delta), fabsf(l1 - l2), l1+l2-0.00001f ),
323 c = acosf( (l1*l1 + d*d - l2*l2) / (2.0f*l1*d) ),
324 rot = atan2f( delta[1], delta[0] ) + c - VG_PIf/2.0f;
325
326 knee[0] = sinf(-rot) * l1;
327 knee[1] = cosf(-rot) * l1;
328
329 m4x3_identity( skele->final_mtx[ik->lower] );
330 m4x3_identity( skele->final_mtx[ik->upper] );
331
332 /* create rotation matrix */
333 v3f co_knee;
334 v3_muladds( co_base, v0, knee[0], co_knee );
335 v3_muladds( co_knee, v1, knee[1], co_knee );
336 vg_line( co_base, co_knee, 0xff00ff00 );
337
338 m4x3f transform;
339 v3_copy( vaxis, transform[0] );
340 v3_muls( v0, knee[0], transform[1] );
341 v3_muladds( transform[1], v1, knee[1], transform[1] );
342 v3_normalize( transform[1] );
343 v3_cross( transform[0], transform[1], transform[2] );
344 v3_copy( co_base, transform[3] );
345
346 m3x3_mul( transform, ik->ia, transform );
347 m4x3_copy( transform, skele->final_mtx[ik->lower] );
348
349 /* upper/knee bone */
350 v3_copy( vaxis, transform[0] );
351 v3_sub( co_target, co_knee, transform[1] );
352 v3_normalize( transform[1] );
353 v3_cross( transform[0], transform[1], transform[2] );
354 v3_copy( co_knee, transform[3] );
355
356 m3x3_mul( transform, ik->ib, transform );
357 m4x3_copy( transform, skele->final_mtx[ik->upper] );
358 }
359 }
360
361 /*
362 * Applies the typical operations that you want for an IK rig:
363 * Pose, IK, Pose(deferred), Inverses, Transform
364 */
365 VG_STATIC void skeleton_apply_standard( struct skeleton *skele, mdl_keyframe *pose,
366 m4x3f transform )
367 {
368 skeleton_apply_pose( skele, pose, k_anim_apply_defer_ik );
369 skeleton_apply_ik_pass( skele );
370 skeleton_apply_pose( skele, pose, k_anim_apply_deffered_only );
371 skeleton_apply_inverses( skele );
372 skeleton_apply_transform( skele, transform );
373 }
374
375 /*
376 * Get an animation by name
377 */
378 VG_STATIC struct skeleton_anim *skeleton_get_anim( struct skeleton *skele,
379 const char *name )
380 {
381 for( int i=0; i<skele->anim_count; i++ )
382 {
383 struct skeleton_anim *anim = &skele->anims[i];
384
385 if( !strcmp( anim->name, name ) )
386 return anim;
387 }
388
389 return NULL;
390 }
391
392 VG_STATIC void skeleton_alloc_from( struct skeleton *skele,
393 void *lin_alloc,
394 struct classtype_skeleton *inf )
395 {
396 skele->bone_count = inf->channels;
397 skele->ik_count = inf->ik_count;
398 skele->collider_count = inf->collider_count;
399 skele->anim_count = inf->anim_count;
400
401 u32 bone_size = sizeof(struct skeleton_bone) * skele->bone_count,
402 ik_size = sizeof(struct skeleton_ik) * skele->ik_count,
403 mtx_size = sizeof(m4x3f) * skele->bone_count,
404 anim_size = sizeof(struct skeleton_anim) * skele->anim_count;
405
406 skele->bones = vg_linear_alloc( lin_alloc, bone_size );
407 skele->ik = vg_linear_alloc( lin_alloc, ik_size );
408 skele->final_mtx = vg_linear_alloc( lin_alloc, mtx_size );
409 skele->anims = vg_linear_alloc( lin_alloc, anim_size );
410 }
411
412 VG_STATIC void skeleton_fatal_err(void)
413 {
414 vg_fatal_exit_loop( "Skeleton setup failed" );
415 }
416
417 /* Setup an animated skeleton from model. mdl's metadata should stick around */
418 VG_STATIC void skeleton_setup( struct skeleton *skele,
419 void *lin_alloc, mdl_context *mdl )
420 {
421 u32 bone_count = 1, skeleton_root = 0, ik_count = 0, collider_count = 0;
422 skele->bone_count = 0;
423 skele->bones = NULL;
424 skele->final_mtx = NULL;
425 skele->anims = NULL;
426
427 struct classtype_skeleton *inf = NULL;
428
429 for( u32 i=0; i<mdl->info.node_count; i++ )
430 {
431 mdl_node *pnode = mdl_node_from_id( mdl, i );
432
433 if( pnode->classtype == k_classtype_skeleton )
434 {
435 inf = mdl_get_entdata( mdl, pnode );
436 skeleton_alloc_from( skele, lin_alloc, inf );
437 skeleton_root = i;
438 }
439 else if( skele->bone_count )
440 {
441 int is_bone = pnode->classtype == k_classtype_bone;
442
443 if( is_bone )
444 {
445 if( bone_count == skele->bone_count )
446 {
447 vg_error( "too many bones (%u/%u) @%s!\n",
448 bone_count, skele->bone_count,
449 mdl_pstr( mdl, pnode->pstr_name ));
450
451 skeleton_fatal_err();
452 }
453
454 struct skeleton_bone *sb = &skele->bones[bone_count];
455 struct classtype_bone *bone_inf = mdl_get_entdata( mdl, pnode );
456
457 v3_copy( pnode->co, sb->co );
458 v3_copy( pnode->s, sb->end );
459 sb->parent = pnode->parent-skeleton_root;
460 sb->name = mdl_pstr( mdl, pnode->pstr_name );
461 sb->flags = bone_inf->flags;
462
463 if( sb->flags & k_bone_flag_ik )
464 {
465 skele->bones[ sb->parent ].flags |= k_bone_flag_ik;
466
467 if( ik_count == skele->ik_count )
468 {
469 vg_error( "Too many ik bones, corrupt model file\n" );
470 skeleton_fatal_err();
471 }
472
473 struct skeleton_ik *ik = &skele->ik[ ik_count ++ ];
474 ik->upper = bone_count;
475 ik->lower = sb->parent;
476 ik->target = bone_inf->ik_target;
477 ik->pole = bone_inf->ik_pole;
478 }
479
480 sb->orig_node = i;
481 box_copy( bone_inf->hitbox, sb->hitbox );
482
483 if( bone_inf->flags & k_bone_flag_collider_any )
484 {
485 if( collider_count == skele->collider_count )
486 {
487 vg_error( "Too many collider bones\n" );
488 skeleton_fatal_err();
489 }
490
491 collider_count ++;
492 }
493
494 bone_count ++;
495 }
496 else
497 {
498 break;
499 }
500 }
501 }
502
503 if( !inf )
504 {
505 vg_error( "No skeleton in model\n" );
506 skeleton_fatal_err();
507 }
508
509 if( collider_count != skele->collider_count )
510 {
511 vg_error( "Loaded %u colliders out of %u\n", collider_count,
512 skele->collider_count );
513 skeleton_fatal_err();
514 }
515
516 if( bone_count != skele->bone_count )
517 {
518 vg_error( "Loaded %u bones out of %u\n", bone_count, skele->bone_count );
519 vg_fatal_exit_loop( "Skeleton setup failed" );
520 skeleton_fatal_err();
521 }
522
523 if( ik_count != skele->ik_count )
524 {
525 vg_error( "Loaded %u ik bones out of %u\n", ik_count, skele->ik_count );
526 skeleton_fatal_err();
527 }
528
529 /* fill in implicit root bone */
530 v3_zero( skele->bones[0].co );
531 v3_copy( (v3f){0.0f,1.0f,0.0f}, skele->bones[0].end );
532 skele->bones[0].parent = 0xffffffff;
533 skele->bones[0].flags = 0;
534 skele->bones[0].name = "[root]";
535
536 /* process animation quick refs */
537 for( int i=0; i<skele->anim_count; i++ )
538 {
539 mdl_animation *anim = &mdl->anim_buffer[ inf->anim_start + i ];
540
541 skele->anims[i].rate = anim->rate;
542 skele->anims[i].length = anim->length;
543 skele->anims[i].name = mdl_pstr(mdl, anim->pstr_name);
544 skele->anims[i].anim_data = &mdl->keyframe_buffer[ anim->offset ];
545
546 vg_info( "animation[ %f, %u ] '%s'\n", anim->rate,
547 anim->length,
548 skele->anims[i].name );
549 }
550
551 skeleton_create_inverses( skele );
552 vg_success( "Loaded skeleton with %u bones\n", skele->bone_count );
553 vg_success( " %u colliders\n", skele->collider_count );
554 }
555
556 VG_STATIC void skeleton_debug( struct skeleton *skele )
557 {
558 for( int i=0; i<skele->bone_count; i ++ )
559 {
560 struct skeleton_bone *sb = &skele->bones[i];
561
562 v3f p0, p1;
563 v3_copy( sb->co, p0 );
564 v3_add( p0, sb->end, p1 );
565 //vg_line( p0, p1, 0xffffffff );
566
567 m4x3_mulv( skele->final_mtx[i], p0, p0 );
568 m4x3_mulv( skele->final_mtx[i], p1, p1 );
569
570 if( sb->flags & k_bone_flag_deform )
571 {
572 if( sb->flags & k_bone_flag_ik )
573 {
574 vg_line( p0, p1, 0xff0000ff );
575 }
576 else
577 {
578 vg_line( p0, p1, 0xffcccccc );
579 }
580 }
581 else
582 vg_line( p0, p1, 0xff00ffff );
583 }
584 }
585
586 #endif /* SKELETON_H */