- 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 )
-{
- world_instance *world = get_active_world();
-
- 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 ++ ){
- if( rb_global_has_space() ){
- rb_ct *buf = rb_global_buffer();
-
- int l;
-
- if( rd->parts[i].rb.type == k_rb_shape_capsule ){
- l = rb_capsule__scene( rd->parts[i].rb.to_world,
- &rd->parts[i].rb.inf.capsule,
- NULL, &world->rb_geo.inf.scene, buf );
- }
- else if( rd->parts[i].rb.type == k_rb_shape_box ){
- l = rb_box__scene( rd->parts[i].rb.to_world,
- rd->parts[i].rb.bbx,
- NULL, &world->rb_geo.inf.scene, buf );
- }
- else continue;
-
- for( int j=0; j<l; j++ ){
- buf[j].rba = &rd->parts[i].rb;
- buf[j].rbb = &world->rb_geo;
- }
-
- rb_contact_count += l;
- }
- }
-
- /*
- * self-collision
- */
- 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 ){
- if( !rb_global_has_space() )
- break;
-
- if( rd->parts[j].rb.type != k_rb_shape_capsule )
- continue;
-
- if( rd->parts[i].rb.type != k_rb_shape_capsule )
- continue;
-
- rb_ct *buf = rb_global_buffer();
-
- int l = rb_capsule__capsule( rd->parts[i].rb.to_world,
- &rd->parts[i].rb.inf.capsule,
- rd->parts[j].rb.to_world,
- &rd->parts[j].rb.inf.capsule,
- buf );
-
- for( int k=0; k<l; k++ ){
- buf[k].rba = &rd->parts[i].rb;
- buf[k].rbb = &rd->parts[j].rb;
- }
-
- rb_contact_count += l;
- }
- }
- }
-
- 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<16; 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 );
- }
-
-}