- v3f velocity )
-{
- for( int i=0; i<rd->part_count; i++ )
- {
- struct ragdoll_part *part = &rd->parts[i];
-
- v3f pos, offset;
- u32 bone = part->bone_id;
-
- m4x3_mulv( av->sk.final_mtx[bone], av->sk.bones[bone].co, pos );
- m3x3_mulv( av->sk.final_mtx[bone], part->collider_mtx[3], offset );
- v3_add( pos, offset, part->rb.co );
-
- m3x3f r;
- m3x3_mul( av->sk.final_mtx[bone], part->collider_mtx, r );
- m3x3_q( r, part->rb.q );
-
- v3_copy( velocity, part->rb.v );
- v3_zero( part->rb.w );
-
- rb_update_transform( &part->rb );
- }
-}
-
-/*
- * Draw rigidbody colliders for ragdoll
- */
-VG_STATIC void player_debug_ragdoll(void)
-{
-}
-
-/*
- * Ragdoll physics step
- */
-VG_STATIC void player_ragdoll_iter( struct player_ragdoll *rd )
-{
- int run_sim = 0;
- ragdoll_frame ++;
-
- if( ragdoll_frame >= k_ragdoll_div )
- {
- ragdoll_frame = 0;
- run_sim = 1;
- }
-
- rb_solver_reset();
- for( int i=0; i<rd->part_count; i ++ )
- rb_collide( &rd->parts[i].rb, &world.rb_geo );
-
- /*
- * COLLISION DETECTION
- */
- for( int i=0; i<rd->part_count-1; i ++ )
- {
- for( int j=i+1; j<rd->part_count; j ++ )
- {
- if( rd->parts[j].parent != i )
- rb_collide( &rd->parts[i].rb, &rd->parts[j].rb );
- }
- }
-
- for( int j=0; j<rd->part_count; j++ )
- {
- struct ragdoll_part *pj = &rd->parts[j];
-
- if( run_sim )
- {
- v4f plane = {0.0f,1.0f,0.0f,0.0f};
- rb_effect_simple_bouyency( &pj->rb, plane, k_ragdoll_floatyiness,
- k_ragdoll_floatydrag );
- }
- }
-
- /*
- * PRESOLVE
- */
- rb_presolve_contacts( rb_contact_buffer, rb_contact_count );
- rb_presolve_swingtwist_constraints( rd->cone_constraints,
- rd->cone_constraints_count );
-
- /*
- * DEBUG
- */
- if( k_ragdoll_debug_collider )
- {
- for( u32 i=0; i<rd->part_count; i ++ )
- rb_debug( &rd->parts[i].rb, rd->parts[i].colour );
- }
-
- if( k_ragdoll_debug_constraints )
- {
- rb_debug_position_constraints( rd->position_constraints,
- rd->position_constraints_count );
-
- rb_debug_swingtwist_constraints( rd->cone_constraints,
- rd->cone_constraints_count );
- }
-
- /*
- * SOLVE CONSTRAINTS
- */
- if( run_sim )
- {
- for( int i=0; i<25; i++ )
- {
- rb_solve_contacts( rb_contact_buffer, rb_contact_count );
- rb_solve_swingtwist_constraints( rd->cone_constraints,
- rd->cone_constraints_count );
- rb_solve_position_constraints( rd->position_constraints,
- rd->position_constraints_count );
- }
-
- for( int i=0; i<rd->part_count; i++ )
- rb_iter( &rd->parts[i].rb );
-
- for( int i=0; i<rd->part_count; i++ )
- rb_update_transform( &rd->parts[i].rb );
-
- rb_correct_swingtwist_constraints( rd->cone_constraints,
- rd->cone_constraints_count, 0.25f );
-
- rb_correct_position_constraints( rd->position_constraints,
- rd->position_constraints_count, 0.5f );
- }
-
-}