#ifndef PLAYER_RAGDOLL_H
#define PLAYER_RAGDOLL_H
-#include "player.h"
+#define VG_GAME
+#include "vg/vg.h"
+#include "skeleton.h"
+#include "rigidbody.h"
+#include "player_model.h"
+#include "world.h"
+
+struct player_ragdoll
+{
+ struct ragdoll_part
+ {
+ u32 bone_id;
+
+ /* Collider transform relative to bone */
+ m4x3f collider_mtx,
+ inv_collider_mtx;
+
+ u32 use_limits;
+ v3f limits[2];
+
+ rigidbody rb;
+ u32 parent;
+ u32 colour;
+ }
+ parts[32];
+ u32 part_count;
+
+ rb_constr_pos position_constraints[32];
+ u32 position_constraints_count;
+
+ rb_constr_swingtwist cone_constraints[32];
+ u32 cone_constraints_count;
+
+ int shoes[2];
+};
+
+VG_STATIC void player_init_ragdoll_bone_collider( struct skeleton_bone *bone,
+ struct ragdoll_part *rp )
+{
+ m4x3_identity( rp->collider_mtx );
+
+ 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_copy( delta, rp->rb.bbx[1] );
+ v3_muls( delta, -1.0f, rp->rb.bbx[0] );
+
+ q_identity( rp->rb.q );
+ rp->rb.type = k_rb_shape_box;
+ rp->colour = 0xffcccccc;
+ }
+ 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;
+ rb_tangent_basis( v1, tx, ty );
+
+ float r = (fabsf(v3_dot(tx,v0)) + fabsf(v3_dot(ty,v0))) * 0.25f,
+ l = 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->rb.type = k_rb_shape_capsule;
+ rp->rb.inf.capsule.height = l;
+ rp->rb.inf.capsule.radius = r;
+
+ rp->colour = 0xff000000 | (0xff << (major_axis*8));
+ }
+ else
+ vg_fatal_exit_loop( "Invalid bone collider type" );
-static float k_ragdoll_floatyiness = 10.0f,
- k_ragdoll_floatydrag = 1.0f;
+ 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 );
+ rp->rb.is_world = 0;
+ rb_init( &rp->rb );
+}
+
+/*
+ * Get parent index in the ragdoll
+ */
+VG_STATIC u32 ragdoll_bone_parent( struct player_ragdoll *rd,
+ struct player_avatar *av, u32 bone_id )
+{
+ for( u32 j=0; j<rd->part_count; j++ )
+ if( rd->parts[ j ].bone_id == bone_id )
+ return j;
+
+ vg_fatal_exit_loop( "Referenced parent bone does not have a rigidbody" );
+ return 0;
+}
/*
* Setup ragdoll colliders
*/
-static void player_init_ragdoll( mdl_header *src )
+VG_STATIC void player_setup_ragdoll_from_avatar( struct player_ragdoll *rd,
+ struct player_avatar *av )
{
- struct player_model *mdl = &player.mdl;
+ rd->part_count = 0;
- if( !mdl->sk.collider_count )
- {
- mdl->ragdoll_count = 0;
+ if( !av->sk.collider_count )
return;
- }
- mdl->ragdoll = malloc(sizeof(struct ragdoll_part) * mdl->sk.collider_count);
- mdl->ragdoll_count = 0;
+ rd->position_constraints_count = 0;
+ rd->cone_constraints_count = 0;
- for( u32 i=0; i<mdl->sk.bone_count; i ++ )
- {
- struct skeleton_bone *bone = &mdl->sk.bones[i];
+ for( u32 i=0; i<av->sk.bone_count; i ++ ){
+ struct skeleton_bone *bone = &av->sk.bones[i];
+
+ /*
+ * Bones with colliders
+ */
+ if( !(bone->collider) )
+ continue;
+
+ if( rd->part_count > vg_list_size(rd->parts) )
+ vg_fatal_exit_loop( "Playermodel has too many colliders" );
+
+ struct ragdoll_part *rp = &rd->parts[ rd->part_count ++ ];
+ rp->bone_id = i;
+ rp->parent = 0xffffffff;
+
+ player_init_ragdoll_bone_collider( bone, rp );
- if( bone->collider )
- {
- struct ragdoll_part *rp = &mdl->ragdoll[ mdl->ragdoll_count ++ ];
- rp->bone_id = i;
-
+ /*
+ * Bones with collider and parent
+ */
+ if( !bone->parent )
+ continue;
+
+ rp->parent = ragdoll_bone_parent( rd, av, bone->parent );
+
+
+ if( bone->orig_bone->flags & k_bone_flag_cone_constraint ){
+ struct rb_constr_pos *c =
+ &rd->position_constraints[ rd->position_constraints_count ++ ];
+
+ struct skeleton_bone *bj = &av->sk.bones[rp->bone_id];
+ struct ragdoll_part *pp = &rd->parts[rp->parent];
+ struct skeleton_bone *bp = &av->sk.bones[pp->bone_id];
+
+ /* Convention: rba -- parent, rbb -- child */
+ c->rba = &pp->rb;
+ c->rbb = &rp->rb;
+
v3f delta;
- v3_sub( bone->hitbox[1], bone->hitbox[0], delta );
- v3_muls( delta, 0.5f, delta );
-
- v3_add( bone->hitbox[0], delta, rp->offset );
-
- v3_copy( delta, rp->rb.bbx[1] );
- v3_muls( delta, -1.0f, rp->rb.bbx[0] );
-
- q_identity( rp->rb.q );
- v3_add( bone->co, rp->offset, rp->rb.co );
- rp->rb.type = k_rb_shape_box;
- rp->rb.is_world = 0;
- rp->parent = 0xffffffff;
-
- if( bone->parent )
- {
- for( u32 j=0; j<mdl->ragdoll_count; j++ )
- {
- if( mdl->ragdoll[ j ].bone_id == bone->parent )
- {
- rp->parent = j;
- break;
- }
- }
- }
+ 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;
- /* TODO: refactor to use this style elswhere */
- struct mdl_node *pnode = mdl_node_from_id( src, bone->orig_node );
- struct classtype_bone *bone_inf = mdl_get_entdata( src, pnode );
+ /* 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 );
- rp->use_limits = bone_inf->use_limits;
- v3_copy( bone_inf->angle_limits[0], rp->limits[0] );
- v3_copy( bone_inf->angle_limits[1], rp->limits[1] );
+ a->conevx[3] = v3_length( inf->conevx );
+ a->conevy[3] = v3_length( inf->conevy );
- rb_init( &rp->rb );
+ rp->use_limits = 1;
}
}
}
/*
- * Make the player model copy the ragdoll
+ * Make avatar copy the ragdoll
*/
-static void player_model_copy_ragdoll(void)
+VG_STATIC void copy_ragdoll_pose_to_avatar( struct player_ragdoll *rd,
+ struct player_avatar *av )
{
- struct player_model *mdl = &player.mdl;
-
- for( int i=0; i<mdl->ragdoll_count; i++ )
- {
- struct ragdoll_part *part = &mdl->ragdoll[i];
+ for( int i=0; i<rd->part_count; i++ ){
+ struct ragdoll_part *part = &rd->parts[i];
m4x3f offset;
m3x3_identity(offset);
- v3_negate( part->offset, offset[3] );
- m4x3_mul( part->rb.to_world, offset, mdl->sk.final_mtx[part->bone_id] );
+ m4x3_mul( part->rb.to_world, part->inv_collider_mtx,
+ av->sk.final_mtx[part->bone_id] );
}
- skeleton_apply_inverses( &mdl->sk );
+ skeleton_apply_inverses( &av->sk );
}
/*
* Make the ragdoll copy the player model
*/
-static void player_ragdoll_copy_model( v3f v )
+VG_STATIC void copy_avatar_pose_to_ragdoll( struct player_avatar *av,
+ struct player_ragdoll *rd,
+ v3f velocity )
{
- struct player_model *mdl = &player.mdl;
-
- for( int i=0; i<mdl->ragdoll_count; i++ )
- {
- struct ragdoll_part *part = &mdl->ragdoll[i];
+ 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( mdl->sk.final_mtx[bone], mdl->sk.bones[bone].co, pos );
- m3x3_mulv( mdl->sk.final_mtx[bone], part->offset, offset );
+
+ 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 );
- m3x3_q( mdl->sk.final_mtx[bone], part->rb.q );
- v3_copy( v, part->rb.v );
+
+ 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
*/
-static void player_debug_ragdoll(void)
+VG_STATIC void player_debug_ragdoll(void)
{
- struct player_model *mdl = &player.mdl;
-
- for( u32 i=0; i<mdl->ragdoll_count; i ++ )
- rb_debug( &mdl->ragdoll[i].rb, 0xff00ff00 );
}
/*
* Ragdoll physics step
*/
-static void player_ragdoll_iter(void)
+VG_STATIC void player_ragdoll_iter( struct player_ragdoll *rd )
{
- struct player_model *mdl = &player.mdl;
+ 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 i=0; i<mdl->ragdoll_count; i ++ )
- rb_collide( &mdl->ragdoll[i].rb, &world.rb_geo );
+ for( int j=0; j<l; j++ ){
+ buf[j].rba = &rd->parts[i].rb;
+ buf[j].rbb = &world->rb_geo;
+ }
- rb_presolve_contacts( rb_contact_buffer, rb_contact_count );
+ rb_contact_count += l;
+ }
+ }
- v3f rv;
+ /*
+ * 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 0
- float shoe_vel[2] = {0.0f,0.0f};
- for( int i=0; i<2; i++ )
- if( mdl->shoes[i] )
- shoe_vel[i] = v3_length( mdl->ragdoll[i].rb.v );
-#endif
-
- for( int j=0; j<mdl->ragdoll_count; j++ )
- {
- struct ragdoll_part *pj = &mdl->ragdoll[j];
- struct skeleton_bone *bj = &mdl->sk.bones[pj->bone_id];
+ if( rd->parts[j].rb.type != k_rb_shape_capsule )
+ continue;
- if( pj->parent != 0xffffffff )
- {
- struct ragdoll_part *pp = &mdl->ragdoll[pj->parent];
- struct skeleton_bone *bp = &mdl->sk.bones[pp->bone_id];
+ if( rd->parts[i].rb.type != k_rb_shape_capsule )
+ continue;
- v3f lca, lcb;
- v3_negate( pj->offset, lca );
- v3_add( bp->co, pp->offset, lcb );
- v3_sub( bj->co, lcb, lcb );
+ rb_ct *buf = rb_global_buffer();
- rb_debug_constraint_position( &pj->rb, lca, &pp->rb, lcb );
+ 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 );
- if( pj->use_limits )
- {
- rb_debug_constraint_limits( &pj->rb, &pp->rb, lca, pj->limits );
+ 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;
}
}
-
- v4f plane = {0.0f,1.0f,0.0f,0.0f};
- rb_effect_simple_bouyency( &pj->rb, plane, k_ragdoll_floatyiness,
- k_ragdoll_floatydrag );
}
- /* CONSTRAINTS */
- for( int i=0; i<10; i++ )
- {
- rb_solve_contacts( rb_contact_buffer, rb_contact_count );
+ for( int j=0; j<rd->part_count; j++ ){
+ struct ragdoll_part *pj = &rd->parts[j];
- for( int j=0; j<mdl->ragdoll_count; j++ )
- {
- struct ragdoll_part *pj = &mdl->ragdoll[j];
- struct skeleton_bone *bj = &mdl->sk.bones[pj->bone_id];
+ 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 );
+ }
+ }
- if( (pj->parent != 0xffffffff) && pj->use_limits )
- {
- struct ragdoll_part *pp = &mdl->ragdoll[pj->parent];
- struct skeleton_bone *bp = &mdl->sk.bones[pp->bone_id];
+ /*
+ * 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 );
+ }
- v3f lca, lcb;
- v3_negate( pj->offset, lca );
- v3_add( bp->co, pp->offset, lcb );
- v3_sub( bj->co, lcb, lcb );
+ if( k_ragdoll_debug_constraints ){
+ rb_debug_position_constraints( rd->position_constraints,
+ rd->position_constraints_count );
- rb_constraint_position( &pj->rb, lca, &pp->rb, lcb );
+ rb_debug_swingtwist_constraints( rd->cone_constraints,
+ rd->cone_constraints_count );
+ }
- rb_constraint_limits( &pj->rb, lca, &pp->rb, lcb, pj->limits );
- }
+ /*
+ * 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 );
}
- /* INTEGRATION */
- for( int i=0; i<mdl->ragdoll_count; i++ )
- rb_iter( &mdl->ragdoll[i].rb );
-
- /* SHOES */
- for( int i=0; i<mdl->ragdoll_count; i++ )
- rb_update_transform( &mdl->ragdoll[i].rb );
}
#endif /* PLAYER_RAGDOLL_H */
projects / carveJwlIkooP6JGAAIwe30JlM.git / blobdiff