+++ /dev/null
-#pragma once
-#include "vg/vg_rigidbody.h"
-#include "vg/vg_rigidbody_collision.h"
-#include "vg/vg_rigidbody_constraints.h"
-#include "scene_rigidbody.h"
-
-#include "player.h"
-#include "audio.h"
-
-static int dev_ragdoll_saveload(int argc, const char *argv[]){
- if( argc != 2 ){
- vg_info( "Usage: ragdoll load/save filepath\n" );
- return 1;
- }
-
- if( !strcmp(argv[0],"save") ){
- FILE *fp = fopen( argv[1], "wb" );
- if( !fp ){
- vg_error( "Failed to open file\n" );
- return 1;
- }
- fwrite( &localplayer.ragdoll.parts,
- sizeof(localplayer.ragdoll.parts), 1, fp );
- fclose( fp );
- }
- else if( !strcmp(argv[0],"load") ){
- FILE *fp = fopen( argv[1], "rb" );
- if( !fp ){
- vg_error( "Failed to open file\n" );
- return 1;
- }
-
- fread( &localplayer.ragdoll.parts,
- sizeof(localplayer.ragdoll.parts), 1, fp );
- fclose( fp );
- }
- else {
- vg_error( "Unknown command: %s (options are: save,load)\n", argv[0] );
- return 1;
- }
-
- return 0;
-}
-
-static void player_ragdoll_init(void){
- VG_VAR_F32( k_ragdoll_active_threshold );
- VG_VAR_F32( k_ragdoll_angular_drag );
- VG_VAR_F32( k_ragdoll_correction );
- VG_VAR_F32( k_ragdoll_limit_scale );
- VG_VAR_F32( k_ragdoll_spring );
- VG_VAR_F32( k_ragdoll_dampening );
- VG_VAR_I32( k_ragdoll_div );
- VG_VAR_I32( k_ragdoll_debug_collider );
- VG_VAR_I32( k_ragdoll_debug_constraints );
- vg_console_reg_cmd( "ragdoll", dev_ragdoll_saveload, NULL );
-}
-
-static void player_init_ragdoll_bone_collider( struct skeleton_bone *bone,
- struct ragdoll_part *rp )
-{
- f32 k_density = 8.0f,
- k_inertia_scale = 2.0f;
-
- m4x3_identity( rp->collider_mtx );
-
- rp->type = bone->collider;
- if( bone->collider == k_bone_collider_box ){
- v3f delta;
- v3_sub( bone->hitbox[1], bone->hitbox[0], delta );
- v3_muls( delta, 0.5f, delta );
- v3_add( bone->hitbox[0], delta, rp->collider_mtx[3] );
-
- v3_muls( delta, -1.0f, rp->inf.box[0] );
- v3_copy( delta, rp->inf.box[1] );
-
- rp->colour = 0xffcccccc;
-
- rb_setbody_box( &rp->rb, rp->inf.box, k_density, k_inertia_scale );
- }
- else if( bone->collider == k_bone_collider_capsule ){
- v3f v0, v1, tx, ty;
- v3_sub( bone->hitbox[1], bone->hitbox[0], v0 );
-
- int major_axis = 0;
- float largest = -1.0f;
-
- for( int i=0; i<3; i ++ ){
- if( fabsf( v0[i] ) > largest ){
- largest = fabsf( v0[i] );
- major_axis = i;
- }
- }
-
- v3_zero( v1 );
- v1[ major_axis ] = 1.0f;
- v3_tangent_basis( v1, tx, ty );
-
- rp->inf.capsule.r = (fabsf(v3_dot(tx,v0)) + fabsf(v3_dot(ty,v0))) * 0.25f;
- rp->inf.capsule.h = fabsf(v0[ major_axis ]);
-
- /* orientation */
- v3_muls( tx, -1.0f, rp->collider_mtx[0] );
- v3_muls( v1, -1.0f, rp->collider_mtx[1] );
- v3_muls( ty, -1.0f, rp->collider_mtx[2] );
- v3_add( bone->hitbox[0], bone->hitbox[1], rp->collider_mtx[3] );
- v3_muls( rp->collider_mtx[3], 0.5f, rp->collider_mtx[3] );
-
- rp->colour = 0xff000000 | (0xff << (major_axis*8));
-
- rb_setbody_capsule( &rp->rb, rp->inf.capsule.r, rp->inf.capsule.h,
- k_density, k_inertia_scale );
- }
- else{
- vg_warn( "type: %u\n", bone->collider );
- vg_fatal_error( "Invalid bone collider type" );
- }
-
- m4x3_invert_affine( rp->collider_mtx, rp->inv_collider_mtx );
-
- /* Position collider into rest */
- m3x3_q( rp->collider_mtx, rp->rb.q );
- v3_add( rp->collider_mtx[3], bone->co, rp->rb.co );
- v3_zero( rp->rb.v );
- v3_zero( rp->rb.w );
- rb_update_matrices( &rp->rb );
-}
-
-/*
- * Get parent index in the ragdoll
- */
-static u32 ragdoll_bone_parent( struct player_ragdoll *rd, u32 bone_id ){
- for( u32 j=0; j<rd->part_count; j++ )
- if( rd->parts[ j ].bone_id == bone_id )
- return j;
-
- vg_fatal_error( "Referenced parent bone does not have a rigidbody" );
- return 0;
-}
-
-/*
- * Setup ragdoll colliders from skeleton
- */
-static void setup_ragdoll_from_skeleton( struct skeleton *sk,
- struct player_ragdoll *rd ){
- rd->part_count = 0;
-
- if( !sk->collider_count )
- return;
-
- rd->position_constraints_count = 0;
- rd->cone_constraints_count = 0;
-
- for( u32 i=1; i<sk->bone_count; i ++ ){
- struct skeleton_bone *bone = &sk->bones[i];
-
- /*
- * Bones with colliders
- */
- if( !(bone->collider) )
- continue;
-
- if( rd->part_count > vg_list_size(rd->parts) )
- vg_fatal_error( "Playermodel has too many colliders" );
-
- u32 part_id = rd->part_count;
- rd->part_count ++;
-
- struct ragdoll_part *rp = &rd->parts[ part_id ];
- rp->bone_id = i;
- rp->parent = 0xffffffff;
-
- player_init_ragdoll_bone_collider( bone, rp );
-
- /*
- * Bones with collider and parent
- */
- if( !bone->parent )
- continue;
-
- rp->parent = ragdoll_bone_parent( rd, bone->parent );
-
- if( bone->orig_bone->flags & k_bone_flag_cone_constraint ){
- u32 conid = rd->position_constraints_count;
- rd->position_constraints_count ++;
-
- struct rb_constr_pos *c = &rd->position_constraints[ conid ];
-
- struct skeleton_bone *bj = &sk->bones[rp->bone_id];
- struct ragdoll_part *pp = &rd->parts[rp->parent];
- struct skeleton_bone *bp = &sk->bones[pp->bone_id];
-
- rd->constraint_associations[conid][0] = rp->parent;
- rd->constraint_associations[conid][1] = part_id;
-
- /* Convention: rba -- parent, rbb -- child */
- c->rba = &pp->rb;
- c->rbb = &rp->rb;
-
- v3f delta;
- v3_sub( bj->co, bp->co, delta );
- m4x3_mulv( rp->inv_collider_mtx, (v3f){0.0f,0.0f,0.0f}, c->lcb );
- m4x3_mulv( pp->inv_collider_mtx, delta, c->lca );
-
-
- mdl_bone *inf = bone->orig_bone;
-
- struct rb_constr_swingtwist *a =
- &rd->cone_constraints[ rd->cone_constraints_count ++ ];
-
- a->rba = &pp->rb;
- a->rbb = &rp->rb;
- a->conet = cosf( inf->conet )-0.0001f;
-
- /* Store constraint in local space vectors */
- m3x3_mulv( c->rba->to_local, inf->conevx, a->conevx );
- m3x3_mulv( c->rba->to_local, inf->conevy, a->conevy );
- m3x3_mulv( c->rbb->to_local, inf->coneva, a->coneva );
- v3_copy( c->lca, a->view_offset );
-
- v3_cross( inf->coneva, inf->conevy, a->conevxb );
- m3x3_mulv( c->rbb->to_local, a->conevxb, a->conevxb );
-
- v3_normalize( a->conevxb );
- v3_normalize( a->conevx );
- v3_normalize( a->conevy );
- v3_normalize( a->coneva );
-
- a->conevx[3] = v3_length( inf->conevx );
- a->conevy[3] = v3_length( inf->conevy );
-
- rp->use_limits = 1;
- }
- }
-}
-
-/*
- * Make avatar copy the ragdoll
- */
-static void copy_ragdoll_pose_to_localplayer( struct player_ragdoll *rd ){
- for( int i=0; i<rd->part_count; i++ ){
- struct ragdoll_part *part = &rd->parts[i];
-
- m4x3f mtx;
-
- v4f q_int;
- v3f co_int;
-
- float substep = vg.time_fixed_extrapolate;
- v3_lerp( part->prev_co, part->rb.co, substep, co_int );
- q_nlerp( part->prev_q, part->rb.q, substep, q_int );
-
- q_m3x3( q_int, mtx );
- v3_copy( co_int, mtx[3] );
-
- m4x3_mul( mtx, part->inv_collider_mtx,
- localplayer.final_mtx[part->bone_id] );
- }
-
- for( u32 i=1; i<localplayer.skeleton.bone_count; i++ ){
- struct skeleton_bone *sb = &localplayer.skeleton.bones[i];
-
- if( sb->parent && !sb->collider ){
- v3f delta;
- v3_sub( localplayer.skeleton.bones[i].co,
- localplayer.skeleton.bones[sb->parent].co, delta );
-
- m4x3f posemtx;
- m3x3_identity( posemtx );
- v3_copy( delta, posemtx[3] );
-
- /* final matrix */
- m4x3_mul( localplayer.final_mtx[sb->parent], posemtx,
- localplayer.final_mtx[i] );
- }
- }
-
- skeleton_apply_inverses( &localplayer.skeleton, localplayer.final_mtx );
-}
-
-/*
- * Make the ragdoll copy the player model
- */
-static void copy_localplayer_to_ragdoll( struct player_ragdoll *rd,
- enum player_die_type type ){
- v3f centroid;
-
- v3f *bone_mtx = localplayer.final_mtx[localplayer.id_hip];
- m4x3_mulv( bone_mtx,
- localplayer.skeleton.bones[localplayer.id_hip].co, centroid );
-
- for( int i=0; i<rd->part_count; i++ ){
- struct ragdoll_part *part = &rd->parts[i];
-
- v3f pos, offset;
- u32 bone = part->bone_id;
-
- v3f *bone_mtx = localplayer.final_mtx[bone];
-
- m4x3_mulv( bone_mtx, localplayer.skeleton.bones[bone].co, pos );
- m3x3_mulv( bone_mtx, part->collider_mtx[3], offset );
- v3_add( pos, offset, part->rb.co );
-
- m3x3f r;
- m3x3_mul( bone_mtx, part->collider_mtx, r );
- m3x3_q( r, part->rb.q );
-
- v3f ra, v;
- v3_sub( part->rb.co, centroid, ra );
- v3_cross( localplayer.rb.w, ra, v );
- v3_add( localplayer.rb.v, v, part->rb.v );
-
- if( type == k_player_die_type_feet ){
- if( (bone == localplayer.id_foot_l) ||
- (bone == localplayer.id_foot_r) ){
- v3_zero( part->rb.v );
- }
- }
-
- v3_copy( localplayer.rb.w, part->rb.w );
-
- v3_copy( part->rb.co, part->prev_co );
- v4_copy( part->rb.q, part->prev_q );
-
- rb_update_matrices( &part->rb );
- }
-}
-
-/*
- * Ragdoll physics step
- */
-static void player_ragdoll_iter( struct player_ragdoll *rd ){
- world_instance *world = world_current_instance();
-
- int run_sim = 0;
- ragdoll_frame ++;
-
- if( ragdoll_frame >= k_ragdoll_div ){
- ragdoll_frame = 0;
- run_sim = 1;
- }
-
- rb_solver_reset();
-
- float contact_velocities[256];
-
- rigidbody _null = {0};
- _null.inv_mass = 0.0f;
- m3x3_zero( _null.iI );
-
- for( int i=0; i<rd->part_count; i ++ ){
- v4_copy( rd->parts[i].rb.q, rd->parts[i].prev_q );
- v3_copy( rd->parts[i].rb.co, rd->parts[i].prev_co );
-
- if( rb_global_has_space() ){
- rb_ct *buf = rb_global_buffer();
-
- int l;
-
- if( rd->parts[i].type == k_bone_collider_capsule ){
- l = rb_capsule__scene( rd->parts[i].rb.to_world,
- &rd->parts[i].inf.capsule,
- NULL, world->geo_bh, buf,
- k_material_flag_ghosts );
- }
- else if( rd->parts[i].type == k_bone_collider_box ){
- l = rb_box__scene( rd->parts[i].rb.to_world,
- rd->parts[i].inf.box,
- NULL, world->geo_bh, buf,
- k_material_flag_ghosts );
- }
- else continue;
-
- for( int j=0; j<l; j++ ){
- buf[j].rba = &rd->parts[i].rb;
- buf[j].rbb = &_null;
- }
-
- 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].type != k_bone_collider_capsule )
- continue;
-
- if( rd->parts[i].type != k_bone_collider_capsule )
- continue;
-
- rb_ct *buf = rb_global_buffer();
-
- int l = rb_capsule__capsule( rd->parts[i].rb.to_world,
- &rd->parts[i].inf.capsule,
- rd->parts[j].rb.to_world,
- &rd->parts[j].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;
- }
- }
- }
-
- if( world->water.enabled ){
- for( int j=0; j<rd->part_count; j++ ){
- struct ragdoll_part *pj = &rd->parts[j];
-
- if( run_sim ){
- rb_effect_simple_bouyency( &pj->rb, world->water.plane,
- k_ragdoll_floatyiness,
- k_ragdoll_floatydrag );
- }
- }
- }
-
- /*
- * PRESOLVE
- */
- for( u32 i=0; i<rb_contact_count; i++ ){
- rb_ct *ct = &rb_contact_buffer[i];
-
- v3f rv, ra, rb;
- v3_sub( ct->co, ct->rba->co, ra );
- v3_sub( ct->co, ct->rbb->co, rb );
- rb_rcv( ct->rba, ct->rbb, ra, rb, rv );
- float vn = v3_dot( rv, ct->n );
-
- contact_velocities[i] = vn;
- }
-
- 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 ++ ){
- struct ragdoll_part *rp = &rd->parts[i];
-
- if( rp->type == k_bone_collider_capsule ){
- vg_line_capsule( rp->rb.to_world,
- rp->inf.capsule.r, rp->inf.capsule.h, rp->colour );
- }
- else if( rp->type == k_bone_collider_box ){
- vg_line_boxf_transformed( rp->rb.to_world,
- rp->inf.box, rp->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 & Integrate
- */
- if( run_sim ){
- /* the solver is not very quickly converging so... */
- for( int i=0; i<40; i++ ){
- if( i<20 ){
- rb_solve_contacts( rb_contact_buffer, rb_contact_count );
- rb_solve_swingtwist_constraints( rd->cone_constraints,
- rd->cone_constraints_count );
- rb_postsolve_swingtwist_constraints( rd->cone_constraints,
- rd->cone_constraints_count );
- }
- rb_solve_position_constraints( rd->position_constraints,
- rd->position_constraints_count );
- }
-
- rb_correct_position_constraints( rd->position_constraints,
- rd->position_constraints_count,
- k_ragdoll_correction * 0.5f );
- rb_correct_swingtwist_constraints( rd->cone_constraints,
- rd->cone_constraints_count,
- k_ragdoll_correction * 0.25f );
-
- for( int i=0; i<rd->part_count; i++ ){
- rb_iter( &rd->parts[i].rb );
-
- v3f w;
- v3_copy( rd->parts[i].rb.w, w );
- if( v3_length2( w ) > 0.00001f ){
- v3_normalize( w );
- v3_muladds( rd->parts[i].rb.w, w, -k_ragdoll_angular_drag,
- rd->parts[i].rb.w );
- }
- }
-
- for( int i=0; i<rd->part_count; i++ )
- rb_update_matrices( &rd->parts[i].rb );
- }
-
- rb_ct *stress = NULL;
- float max_stress = 1.0f;
-
- for( u32 i=0; i<rb_contact_count; i++ ){
- rb_ct *ct = &rb_contact_buffer[i];
-
- v3f rv, ra, rb;
- v3_sub( ct->co, ct->rba->co, ra );
- v3_sub( ct->co, ct->rbb->co, rb );
- rb_rcv( ct->rba, ct->rbb, ra, rb, rv );
- float vn = v3_dot( rv, ct->n );
-
- float s = fabsf(vn - contact_velocities[i]);
- if( s > max_stress ){
- stress = ct;
- max_stress = s;
- }
- }
-
- static u32 temp_filter = 0;
-
- /*
- * motorized joints
- */
- if( run_sim &&
- (v3_length2(player_dead.v_lpf)>(k_ragdoll_active_threshold*
- k_ragdoll_active_threshold)) ){
- assert( rd->cone_constraints_count == rd->position_constraints_count );
-
- mdl_keyframe anim[32];
- skeleton_sample_anim( &localplayer.skeleton, player_dead.anim_bail,
- 0.0f, anim );
-
- for( u32 i=0; i<rd->cone_constraints_count; i ++ ){
- rb_constr_swingtwist *st = &rd->cone_constraints[i];
- rb_constr_pos *pc = &rd->position_constraints[i];
-
- v3f va, vap;
-
- m3x3_mulv( st->rbb->to_world, st->coneva, va );
-
- /* calculate va as seen in rest position, from the perspective of the
- * parent object, mapped to pose world space using the parents
- * transform. thats our target */
-
- u32 id_p = rd->constraint_associations[i][0],
- id_a = rd->constraint_associations[i][1];
-
- struct ragdoll_part *pa = &rd->parts[ id_a ],
- *pp = &rd->parts[ id_p ];
-
- mdl_keyframe *kf = &anim[ pa->bone_id-1 ];
- m3x3_mulv( pa->collider_mtx, st->coneva, vap );
- q_mulv( kf->q, vap, vap );
-
- /* This could be a transfer function */
- m3x3_mulv( pp->inv_collider_mtx, vap, vap );
- m3x3_mulv( st->rba->to_world, vap, vap );
-
- f32 d = v3_dot( vap, va ),
- a = acosf( vg_clampf( d, -1.0f, 1.0f ) );
-
- v3f axis;
- v3_cross( vap, va, axis );
-
- f32 Fs = -a * k_ragdoll_spring,
- Fd = -v3_dot( st->rbb->w, axis ) * k_ragdoll_dampening,
- F = Fs+Fd;
-
- v3f torque;
- v3_muls( axis, F, torque );
- v3_muladds( st->rbb->w, torque, k_rb_delta, st->rbb->w );
-
- /* apply a adjustment to keep velocity at joint 0 */
-#if 0
- v3f wcb, vcb;
- m3x3_mulv( st->rbb->to_world, pc->lcb, wcb );
- v3_cross( torque, wcb, vcb );
- v3_muladds( st->rbb->v, vcb, k_rb_delta, st->rbb->v );
-#endif
- }
- }
-
- if( temp_filter ){
- temp_filter --;
- return;
- }
-
- if( stress ){
- temp_filter = 20;
- audio_lock();
- audio_oneshot_3d( &audio_hits[vg_randu32(&vg.rand)%5],
- stress->co, 20.0f, 1.0f );
- audio_unlock();
- }
-}
projects / carveJwlIkooP6JGAAIwe30JlM.git / blobdiff