From: hgn Date: Sun, 12 Jun 2022 22:41:28 +0000 (+0100) Subject: ragdoll X-Git-Url: https://harrygodden.com/git/?a=commitdiff_plain;h=d2fa06a21b490b38352195005c3134683679f9c0;p=carveJwlIkooP6JGAAIwe30JlM.git ragdoll --- diff --git a/character.h b/character.h index 70c1727..f5a9d67 100644 --- a/character.h +++ b/character.h @@ -3,7 +3,9 @@ #include "vg/vg.h" #include "model.h" +#include "scene.h" #include "ik.h" +#include "rigidbody.h" SHADER_DEFINE( shader_player, @@ -87,7 +89,6 @@ static const char *character_part_strings[] = FOREACH_PART( MAKE_STRING ) }; - struct character { glmesh mesh; @@ -97,6 +98,7 @@ struct character /* Auxillary information */ v3f offsets[ PART_COUNT ]; + rigidbody ragdoll[ PART_COUNT ]; /* * Controls @@ -564,4 +566,284 @@ static void character_shader_register(void) SHADER_INIT(shader_player); } + +/* + * Ragdoll Stuff + */ + +static void character_rd_box( struct character *ch, enum character_part id, + v3f dims ) +{ + v3_muls( dims, -0.5f, ch->ragdoll[id].bbx[0] ); + v3_muls( dims, 0.5f, ch->ragdoll[id].bbx[1] ); +} + +struct rd_joint +{ + enum character_part ia, ib; + v3f lca, lcb; + + struct rd_joint_axis + { + v3f va, vb; + float spring, ang; + } + min, maj; +}; + +static const float k_human_major = 0.9f, + k_human_minor = 0.9f, + k_human_major_max = 1.4f, + k_human_minor_max = 0.4f; + +#define HUMAN_VERTICAL_DEFAULT \ +.min = { \ + .va = {1.0f,0.0f,0.0f}, .vb = {1.0f,0.0f,0.0f}, \ + .spring = k_human_minor, .ang = k_human_minor_max \ +}, \ +.maj = { \ + .va = {0.0f,1.0f,0.0f}, .vb = {0.0f,1.0f,0.0f}, \ + .spring = k_human_major, .ang = k_human_major_max \ +} + +#define HUMAN_ARM_LEFT \ +.min = { \ + .va = {1.0f,0.0f,0.0f}, .vb = {1.0f,0.0f,0.0f}, \ + .spring = k_human_minor, .ang = k_human_minor_max \ +}, \ +.maj = { \ + .va = {0.0f,0.0f,1.0f}, .vb = {0.0f,0.0f,1.0f}, \ + .spring = k_human_major, .ang = k_human_major_max \ +} + +#define HUMAN_ARM_RIGHT \ +.min = { \ + .va = {1.0f,0.0f,0.0f}, .vb = {1.0f,0.0f,0.0f}, \ + .spring = k_human_minor, .ang = k_human_minor_max \ +}, \ +.maj = { \ + .va = {0.0f,0.0f,-1.0f}, .vb = {0.0f,0.0f,-1.0f}, \ + .spring = k_human_major, .ang = k_human_major_max \ +} + +static struct rd_joint rd_joints[] = +{ + { .ia = k_chpart_body0, .ib = k_chpart_body1, HUMAN_VERTICAL_DEFAULT }, + { .ia = k_chpart_body1, .ib = k_chpart_neck, HUMAN_VERTICAL_DEFAULT }, + { .ia = k_chpart_neck, .ib = k_chpart_head, HUMAN_VERTICAL_DEFAULT }, + { .ia = k_chpart_body0, .ib = k_chpart_leg_l0, HUMAN_VERTICAL_DEFAULT }, + { .ia = k_chpart_leg_l0, .ib = k_chpart_leg_l1, HUMAN_VERTICAL_DEFAULT }, + { .ia = k_chpart_body0, .ib = k_chpart_leg_r0, HUMAN_VERTICAL_DEFAULT }, + { .ia = k_chpart_leg_r0, .ib = k_chpart_leg_r1, HUMAN_VERTICAL_DEFAULT }, + + { .ia = k_chpart_body1, .ib = k_chpart_arm_l0, HUMAN_ARM_LEFT }, + { .ia = k_chpart_arm_l0, .ib = k_chpart_arm_l1, HUMAN_ARM_LEFT }, + { .ia = k_chpart_arm_l1, .ib = k_chpart_hand_l, HUMAN_ARM_LEFT }, + + { .ia = k_chpart_body1, .ib = k_chpart_arm_r0, HUMAN_ARM_RIGHT }, + { .ia = k_chpart_arm_r0, .ib = k_chpart_arm_r1, HUMAN_ARM_RIGHT }, + { .ia = k_chpart_arm_r1, .ib = k_chpart_hand_r, HUMAN_ARM_RIGHT }, +}; + +static void character_init_ragdoll_joints( struct character *ch ) +{ + for( int i=0; iparts[joint->ib].pivot; + v3_sub( hinge, ch->ragdoll[joint->ia].co, joint->lca ); + v3_sub( hinge, ch->ragdoll[joint->ib].co, joint->lcb ); + } +} + +static void character_init_ragdoll( struct character *ch ) +{ + v3f *offs = ch->offsets; + rigidbody *rbs = ch->ragdoll; + + /* CHest */ + float chest_width = fabsf(offs[k_chpart_arm_r0][2])*2.0f, + chest_depth = chest_width * 0.571f, + chest_height = offs[k_chpart_neck][1]; + v3f chest_dims = { chest_depth, chest_height, chest_width }; + character_rd_box( ch, k_chpart_body1, chest_dims ); + + v3_copy( ch->parts[k_chpart_body1].pivot, rbs[k_chpart_body1].co ); + rbs[k_chpart_body1].co[1] += chest_height*0.5f; + + /* Torso */ + v3f torso_dims = { chest_depth, + offs[k_chpart_body1][1]-offs[k_chpart_leg_l0][1], + chest_width*0.85f }; + v3_copy( ch->parts[k_chpart_body0].pivot, rbs[k_chpart_body0].co ); + character_rd_box( ch, k_chpart_body0, torso_dims ); + + /* Neck */ + v3f neck_dims = { chest_depth*0.5f, + offs[k_chpart_head][1], + chest_depth*0.5f }; + v3_copy( ch->parts[k_chpart_neck].pivot, rbs[k_chpart_neck].co ); + rbs[k_chpart_neck].co[1] += neck_dims[1]*0.5f; + character_rd_box( ch, k_chpart_neck, neck_dims ); + + /* Head */ + v3f head_dims = { chest_width*0.5f, chest_width*0.5f, chest_width*0.5f }; + v3_copy( ch->parts[k_chpart_head].pivot, rbs[k_chpart_head].co ); + rbs[k_chpart_head].co[1] += head_dims[1]*0.5f; + character_rd_box( ch, k_chpart_head, head_dims ); + + /* ARms */ + v3f ua_dims = { 0.0f, 0.0f, fabsf(offs[k_chpart_arm_l1][2]) }; + ua_dims[1] = 0.38f*ua_dims[2]; + ua_dims[0] = 0.38f*ua_dims[2]; + v3f la_dims = { ua_dims[0], ua_dims[1], fabsf(offs[k_chpart_hand_l][2]) }; + v3f hand_dims = { ua_dims[1], ua_dims[1]*0.5f, ua_dims[1] }; + + character_rd_box( ch, k_chpart_arm_l0, ua_dims ); + character_rd_box( ch, k_chpart_arm_r0, ua_dims ); + character_rd_box( ch, k_chpart_arm_l1, la_dims ); + character_rd_box( ch, k_chpart_arm_r1, la_dims ); + character_rd_box( ch, k_chpart_hand_l, hand_dims ); + character_rd_box( ch, k_chpart_hand_r, hand_dims ); + + v3_copy( ch->parts[k_chpart_arm_l0].pivot, rbs[k_chpart_arm_l0].co ); + rbs[k_chpart_arm_l0].co[2] += ua_dims[2] * 0.5f; + v3_copy( ch->parts[k_chpart_arm_l1].pivot, rbs[k_chpart_arm_l1].co ); + rbs[k_chpart_arm_l1].co[2] += la_dims[2] * 0.5f; + v3_copy( ch->parts[k_chpart_hand_l].pivot, rbs[k_chpart_hand_l].co ); + rbs[k_chpart_hand_l].co[2] += hand_dims[2] * 0.5f; + + v3_copy( ch->parts[k_chpart_arm_r0].pivot, rbs[k_chpart_arm_r0].co ); + rbs[k_chpart_arm_r0].co[2] -= ua_dims[2] * 0.5f; + v3_copy( ch->parts[k_chpart_arm_r1].pivot, rbs[k_chpart_arm_r1].co ); + rbs[k_chpart_arm_r1].co[2] -= la_dims[2] * 0.5f; + v3_copy( ch->parts[k_chpart_hand_r].pivot, rbs[k_chpart_hand_r].co ); + rbs[k_chpart_hand_r].co[2] -= hand_dims[2] * 0.5f; + + /* LEgs */ + v3f ul_dims = { 0.0f, fabsf(offs[k_chpart_leg_l1][1]), 0.0f }; + ul_dims[0] = 0.38f*ul_dims[1]; + ul_dims[2] = 0.38f*ul_dims[1]; + v3f ll_dims = { ul_dims[0], fabsf(offs[k_chpart_foot_l][1]), ul_dims[2] }; + v3f foot_dims = { 2.0f*ul_dims[0], ul_dims[0], ul_dims[0] }; + + character_rd_box( ch, k_chpart_leg_l0, ul_dims ); + character_rd_box( ch, k_chpart_leg_r0, ul_dims ); + character_rd_box( ch, k_chpart_leg_l1, ll_dims ); + character_rd_box( ch, k_chpart_leg_r1, ll_dims ); + character_rd_box( ch, k_chpart_foot_l, foot_dims ); + character_rd_box( ch, k_chpart_foot_r, foot_dims ); + + v3_copy( ch->parts[k_chpart_leg_l0].pivot, rbs[k_chpart_leg_l0].co ); + rbs[k_chpart_leg_l0].co[1] -= ul_dims[1] * 0.5f; + v3_copy( ch->parts[k_chpart_leg_l1].pivot, rbs[k_chpart_leg_l1].co ); + rbs[k_chpart_leg_l1].co[1] -= ll_dims[1] * 0.5f; + v3_copy( ch->parts[k_chpart_foot_l].pivot, rbs[k_chpart_foot_l].co ); + rbs[k_chpart_foot_l].co[1] -= foot_dims[1] * 0.5f; + rbs[k_chpart_foot_l].co[0] -= foot_dims[0] * 0.5f; + + v3_copy( ch->parts[k_chpart_leg_r0].pivot, rbs[k_chpart_leg_r0].co ); + rbs[k_chpart_leg_r0].co[1] -= ul_dims[1] * 0.5f; + v3_copy( ch->parts[k_chpart_leg_r1].pivot, rbs[k_chpart_leg_r1].co ); + rbs[k_chpart_leg_r1].co[1] -= ll_dims[1] * 0.5f; + v3_copy( ch->parts[k_chpart_foot_r].pivot, rbs[k_chpart_foot_r].co ); + rbs[k_chpart_foot_r].co[1] -= foot_dims[1] * 0.5f; + rbs[k_chpart_foot_r].co[0] -= foot_dims[0] * 0.5f; + + character_init_ragdoll_joints( ch ); + + for( int i=0; iragdoll[i] ); +} + +static void character_ragdoll_go( struct character *ch, v3f pos ) +{ + character_init_ragdoll( ch ); + for( int i=0; iragdoll[i].co, ch->ragdoll[i].co ); +} + +static void character_ragdoll_copypose( struct character *ch, v3f v ) +{ + character_init_ragdoll(ch); + + for( int i=0; iragdoll[i]; + + v3_sub( rb->co, ch->parts[i].pivot, offset ); + + m4x3_mulv( ch->matrices[i], offset, rb->co ); + m3x3_q( ch->matrices[i], rb->q ); + v3_copy( v, rb->v ); + v3_zero( rb->I ); + rb->manifold_count = 0; /* ? */ + + rb_update_transform( rb ); + } +} + +static void character_debug_ragdoll( struct character *ch ) +{ + rb_debug( &ch->ragdoll[k_chpart_body0], 0xffffffff ); + rb_debug( &ch->ragdoll[k_chpart_body1], 0xffffffff ); + rb_debug( &ch->ragdoll[k_chpart_neck], 0xff00ff00 ); + rb_debug( &ch->ragdoll[k_chpart_head], 0xff00ff00 ); + + rb_debug( &ch->ragdoll[k_chpart_arm_l0], 0xffffa500 ); + rb_debug( &ch->ragdoll[k_chpart_arm_l1], 0xffffa500 ); + rb_debug( &ch->ragdoll[k_chpart_hand_l], 0xffffa500 ); + + rb_debug( &ch->ragdoll[k_chpart_arm_r0], 0xff00a5ff ); + rb_debug( &ch->ragdoll[k_chpart_arm_r1], 0xff00a5ff ); + rb_debug( &ch->ragdoll[k_chpart_hand_r], 0xff00a5ff ); + + rb_debug( &ch->ragdoll[k_chpart_leg_l0], 0xffffa500 ); + rb_debug( &ch->ragdoll[k_chpart_leg_l1], 0xffffa500 ); + rb_debug( &ch->ragdoll[k_chpart_foot_l], 0xffffa500 ); + rb_debug( &ch->ragdoll[k_chpart_leg_r0], 0xff00a5ff ); + rb_debug( &ch->ragdoll[k_chpart_leg_r1], 0xff00a5ff ); + rb_debug( &ch->ragdoll[k_chpart_foot_r], 0xff00a5ff ); +} + +static void character_ragdoll_iter( struct character *ch, scene *sc ) +{ + for( int i=0; iragdoll[i], sc ); + } + + for( int i=0; i<20; i++ ) + { + float const k_springfactor = 1.0f/20.0f; + + for( int j=0; jragdoll[j] ); + + for( int j=0; jragdoll[joint->ia], + *rbb = &ch->ragdoll[joint->ib]; + + rb_constraint_position( rba, joint->lca, rbb, joint->lcb ); + rb_constraint_angle( rba, joint->maj.va, rbb, joint->maj.vb, + joint->maj.ang, + joint->maj.spring * k_springfactor ); + + rb_constraint_angle( rba, joint->min.va, rbb, joint->min.vb, + joint->min.ang, + joint->min.spring * k_springfactor ); + } + } + + for( int i=0; iragdoll[i] ); + + for( int i=0; iragdoll[i] ); +} + #endif diff --git a/main.c b/main.c index 61f0a58..804a6cd 100644 --- a/main.c +++ b/main.c @@ -67,6 +67,8 @@ static int replay_buffer_frame = 0; #include "ik.h" #include "character.h" #include "terrain.h" +#include "ragdoll.h" +#include "rigidbody.h" int main( int argc, char *argv[] ) { @@ -96,7 +98,7 @@ static struct gplayer float pitch; v3f land_target; - v3f land_target_log[12]; + v3f land_target_log[22]; int land_log_count; m3x3f vr; @@ -127,8 +129,17 @@ static struct grender } render; +rigidbody mr_box = { + .bbx = {{ -1.0f, -0.25f, -0.25f }, { 1.0f, 0.25f, 0.25f }} +}; + +rigidbody mrs_box = { + .bbx = {{ -0.5f, -0.25f, -0.25f }, { 0.5f, 0.25f, 0.25f }} +}; + static void player_transform_update(void) { + q_normalize( player.rot ); q_m3x3( player.rot, player.to_world ); v3_copy( player.co, player.to_world[3] ); @@ -171,7 +182,11 @@ void vg_start(void) replay_buffer = malloc( sizeof(m4x3f) * REPLAY_LENGTH * (PART_COUNT) ); vg_tex2d_init( texture_list, vg_list_size( texture_list ) ); - + + rb_init( &mr_box ); + rb_init( &mrs_box ); + mrs_box.co[2] += 2.0f; + vg_convar_push( (struct vg_convar){ .name = "frame", .data = &replay_buffer_frame, @@ -253,6 +268,7 @@ void vg_start(void) v3_normalize( lightDir ); character_load( &player.mdl, "ch_default" ); + character_init_ragdoll( &player.mdl ); /* Setup scene */ scene_init( &world.geo ); @@ -261,6 +277,9 @@ void vg_start(void) scene_add_model( &world.geo, mworld, submodel_get( mworld, "mp_dev" ), (v3f){0.0f,0.0f,0.0f}, 0.0f, 1.0f ); + scene_add_model( &world.geo, mworld, submodel_get( mworld, "terrain" ), + (v3f){0.0f,0.0f,0.0f}, 0.0f, 1.0f ); + free( mworld ); scene_upload( &world.geo ); bvh_create( &world.geo ); @@ -375,9 +394,9 @@ static void player_start_air(void) m3x3_identity( player.vr ); - for( int m=0;m<=5; m++ ) + for( int m=-3;m<=12; m++ ) { - float vmod = ((float)m / 5.0f)*0.09f; + float vmod = ((float)m / 15.0f)*0.09f; v3f pco, pco1, pv; v3_copy( player.co, pco ); @@ -476,11 +495,10 @@ static void player_physics_ground(void) * Getting surface collision points, * the contact manifold is a triangle for simplicity. */ - v3f contact_front, contact_back, fwd, fwd1, contact_norm, vup, vside, + v3f contact_front, contact_back, contact_norm, vup, vside, axis; float klength = 0.65f; - m3x3_mulv( player.to_world, (v3f){ 0.0f, 0.0f,-1.0f}, fwd ); m4x3_mulv( player.to_world, (v3f){ 0.15f,0.0f,-klength}, contact_norm ); m4x3_mulv( player.to_world, (v3f){-0.15f,0.0f,-klength}, contact_front ); m4x3_mulv( player.to_world, (v3f){ 0.00f,0.0f, klength}, contact_back ); @@ -522,7 +540,6 @@ static void player_physics_ground(void) } float resistance = v3_dot( norm, player.v ); - if( resistance >= 0.0f ) { player_start_air(); @@ -668,18 +685,18 @@ static void player_update(void) { static int clock = 0; - clock ++; - if( clock >= clock_divider ) - clock = 0; - else - return; - /* temp */ if( freecam ) { player_freecam(); return; } + + clock ++; + if( clock >= clock_divider ) + clock = 0; + else + return; if( vg_get_axis("grabl")>0.0f) reset_player(0,NULL); @@ -738,7 +755,41 @@ static void player_update(void) void vg_update(void) { player_update(); - bvh_debug( &world.geo, player.co ); + + //rb_torque( &mr_box, (v3f){0.0f,0.0f,1.0f}, 0.01f ); + + if( glfwGetKey( vg_window, GLFW_KEY_F ) ) + character_ragdoll_go( &player.mdl, player.view ); + + if( glfwGetKey( vg_window, GLFW_KEY_G ) ) + character_ragdoll_copypose( &player.mdl, player.v ); + + static int clock = 0; + + clock ++; + if( clock >= clock_divider ) + { + character_debug_ragdoll( &player.mdl ); + character_ragdoll_iter( &player.mdl, &world.geo ); + + + rb_build_manifold( &mr_box, &world.geo ); + rb_build_manifold( &mrs_box, &world.geo ); + rb_constraint_manifold( &mr_box ); + rb_constraint_manifold( &mrs_box ); + + rb_iter( &mr_box ); + rb_iter( &mrs_box ); + + rb_debug( &mr_box, 0xffffffff ); + rb_debug( &mrs_box, 0xff00ff00 ); + + rb_update_transform( &mr_box ); + rb_update_transform( &mrs_box ); + + clock = 0; + } + } static void player_animate(void) @@ -1071,13 +1122,12 @@ void vg_render(void) glBindFramebuffer( GL_FRAMEBUFFER, 0 ); glClearColor( 0.0f, 0.0f, 0.0f, 0.0f ); -#if 1 +#if 0 glClear( GL_COLOR_BUFFER_BIT ); #else glClear( GL_COLOR_BUFFER_BIT|GL_DEPTH_BUFFER_BIT ); #endif - if( !freecam ) draw_player(); /* Draw back in the background */ diff --git a/ragdoll.h b/ragdoll.h new file mode 100644 index 0000000..fa51b6c --- /dev/null +++ b/ragdoll.h @@ -0,0 +1,9 @@ +#ifndef RAGDOLL_H +#define RAGDOLL_H + +#include "vg/vg.h" +#include "scene.h" + + + +#endif diff --git a/rigidbody.h b/rigidbody.h new file mode 100644 index 0000000..cf258b8 --- /dev/null +++ b/rigidbody.h @@ -0,0 +1,348 @@ +/* SHite fphysics */ + +#ifndef RIGIDBODY_H +#define RIGIDBODY_H + +#include "vg/vg.h" +#include "scene.h" + +#define k_rb_delta (1.0f/60.0f) + +typedef struct rigidbody rigidbody; +struct rigidbody +{ + v3f co, v, I; + v4f q; + boxf bbx; + + struct contact + { + v3f co, n, delta; + v3f t[2]; + float bias, norm_impulse, tangent_impulse[2]; + } + manifold[4]; + int manifold_count; + + m4x3f to_world, to_local; +}; + +static void rb_update_transform( rigidbody *rb ) +{ + q_normalize( rb->q ); + q_m3x3( rb->q, rb->to_world ); + v3_copy( rb->co, rb->to_world[3] ); + + m4x3_invert_affine( rb->to_world, rb->to_local ); +} + +static void rb_init( rigidbody *rb ) +{ + q_identity( rb->q ); + v3_zero( rb->v ); + v3_zero( rb->I ); + + rb_update_transform( rb ); +} + +static void rb_iter( rigidbody *rb ) +{ + v3f gravity = { 0.0f, -9.6f, 0.0f }; + v3_muladds( rb->v, gravity, k_rb_delta, rb->v ); + + /* intergrate velocity */ + v3_muladds( rb->co, rb->v, k_rb_delta, rb->co ); + + v3_lerp( rb->I, (v3f){0.0f,0.0f,0.0f}, 0.0025f, rb->I ); + + /* inegrate inertia */ + if( v3_length2( rb->I ) > 0.0f ) + { + v4f rotation; + v3f axis; + v3_copy( rb->I, axis ); + + float mag = v3_length( axis ); + v3_divs( axis, mag, axis ); + q_axis_angle( rotation, axis, mag*k_rb_delta ); + q_mul( rotation, rb->q, rb->q ); + } +} + +static void rb_torque( rigidbody *rb, v3f axis, float mag ) +{ + v3_muladds( rb->I, axis, mag*k_rb_delta, rb->I ); +} + +static void rb_tangent_basis( v3f n, v3f tx, v3f ty ) +{ + /* Compute tangent basis (box2d) */ + if( fabsf( n[0] ) >= 0.57735027f ) + { + tx[0] = n[1]; + tx[1] = -n[0]; + tx[2] = 0.0f; + } + else + { + tx[0] = 0.0f; + tx[1] = n[2]; + tx[2] = -n[1]; + } + + v3_normalize( tx ); + v3_cross( n, tx, ty ); +} + +static void rb_build_manifold( rigidbody *rb, scene *sc ) +{ + v3f *box = rb->bbx; + v3f pts[8]; + float *p000 = pts[0], *p001 = pts[1], *p010 = pts[2], *p011 = pts[3], + *p100 = pts[4], *p101 = pts[5], *p110 = pts[6], *p111 = pts[7]; + + p000[0]=box[0][0];p000[1]=box[0][1];p000[2]=box[0][2]; + p001[0]=box[0][0];p001[1]=box[0][1];p001[2]=box[1][2]; + p010[0]=box[0][0];p010[1]=box[1][1];p010[2]=box[0][2]; + p011[0]=box[0][0];p011[1]=box[1][1];p011[2]=box[1][2]; + + p100[0]=box[1][0];p100[1]=box[0][1];p100[2]=box[0][2]; + p101[0]=box[1][0];p101[1]=box[0][1];p101[2]=box[1][2]; + p110[0]=box[1][0];p110[1]=box[1][1];p110[2]=box[0][2]; + p111[0]=box[1][0];p111[1]=box[1][1];p111[2]=box[1][2]; + + m4x3_mulv( rb->to_world, p000, p000 ); + m4x3_mulv( rb->to_world, p001, p001 ); + m4x3_mulv( rb->to_world, p010, p010 ); + m4x3_mulv( rb->to_world, p011, p011 ); + m4x3_mulv( rb->to_world, p100, p100 ); + m4x3_mulv( rb->to_world, p101, p101 ); + m4x3_mulv( rb->to_world, p110, p110 ); + m4x3_mulv( rb->to_world, p111, p111 ); + + rb->manifold_count = 0; + + for( int i=0; i<8; i++ ) + { + float *point = pts[i]; + struct contact *ct = &rb->manifold[rb->manifold_count]; + + v3f surface; + + v3_copy( point, surface ); + bvh_scene_sample( sc, surface, ct->n ); + + float p = vg_minf( surface[1] - point[1], 1.0f ); + + if( p > 0.0f ) + { + v3_add( point, surface, ct->co ); + v3_muls( ct->co, 0.5f, ct->co ); + + //vg_line_pt3( ct->co, 0.0125f, 0xff0000ff ); + + v3_sub( ct->co, rb->co, ct->delta ); + ct->bias = -0.2f * (1.0f/k_rb_delta) * vg_minf( 0.0f, -p+0.04f ); + rb_tangent_basis( ct->n, ct->t[0], ct->t[1] ); + + ct->norm_impulse = 0.0f; + ct->tangent_impulse[0] = 0.0f; + ct->tangent_impulse[1] = 0.0f; + + rb->manifold_count ++; + if( rb->manifold_count == 4 ) + break; + } + } +} + +static void rb_constraint_manifold( rigidbody *rb ) +{ + float k_friction = 0.07f; + + /* Friction Impulse */ + for( int i=0; imanifold_count; i++ ) + { + struct contact *ct = &rb->manifold[i]; + + v3f dv; + v3_cross( rb->I, ct->delta, dv ); + v3_add( rb->v, dv, dv ); + + for( int j=0; j<2; j++ ) + { + float vt = vg_clampf( -v3_dot( dv, ct->t[j] ), + -k_friction, k_friction ); + + vt = -v3_dot( dv, ct->t[j] ); + + float temp = ct->tangent_impulse[j]; + ct->tangent_impulse[j] = vg_clampf( temp+vt, -k_friction, k_friction ); + vt = ct->tangent_impulse[j] - temp; + + v3f impulse; + + v3_muls( ct->t[j], vt, impulse ); + v3_add( impulse, rb->v, rb->v ); + v3_cross( ct->delta, impulse, impulse ); + v3_add( impulse, rb->I, rb->I ); + } + } + + /* Normal Impulse */ + for( int i=0; imanifold_count; i++ ) + { + struct contact *ct = &rb->manifold[i]; + + v3f dv; + v3_cross( rb->I, ct->delta, dv ); + v3_add( rb->v, dv, dv ); + + float vn = -v3_dot( dv, ct->n ); + vn += ct->bias; + + float temp = ct->norm_impulse; + ct->norm_impulse = vg_maxf( temp + vn, 0.0f ); + vn = ct->norm_impulse - temp; + + v3f impulse; + + v3_muls( ct->n, vn, impulse ); + v3_add( impulse, rb->v, rb->v ); + v3_cross( ct->delta, impulse, impulse ); + v3_add( impulse, rb->I, rb->I ); + } +} + +static void rb_constraint_angle( rigidbody *rba, v3f va, + rigidbody *rbb, v3f vb, + float max, float spring ) +{ + v3f wva, wvb; + m3x3_mulv( rba->to_world, va, wva ); + m3x3_mulv( rbb->to_world, vb, wvb ); + + float dt = v3_dot(wva,wvb)*0.999f, + ang = fabsf(dt); + + v3f axis; + v3_cross( wva, wvb, axis ); + v3_muladds( rba->I, axis, ang*spring*0.5f, rba->I ); + v3_muladds( rbb->I, axis, -ang*spring*0.5f, rbb->I ); + + return; + + /* TODO: convert max into the dot product value so we dont have to always + * evaluate acosf, only if its greater than the angle specified */ + ang = acosf( dt ); + if( ang > max ) + { + float correction = max-ang; + + v4f rotation; + q_axis_angle( rotation, axis, -correction*0.125f ); + q_mul( rotation, rba->q, rba->q ); + + q_axis_angle( rotation, axis, correction*0.125f ); + q_mul( rotation, rbb->q, rbb->q ); + } +} + +static void rb_constraint_position( rigidbody *ra, v3f lca, + rigidbody *rb, v3f lcb ) +{ + /* C = (COa + Ra*LCa) - (COb + Rb*LCb) = 0 */ + + v3f wca, wcb; + m3x3_mulv( ra->to_world, lca, wca ); + m3x3_mulv( rb->to_world, lcb, wcb ); + + v3f delta; + v3_add( wcb, rb->co, delta ); + v3_sub( delta, wca, delta ); + v3_sub( delta, ra->co, delta ); + + v3_muladds( ra->co, delta, 0.5f, ra->co ); + v3_muladds( rb->co, delta, -0.5f, rb->co ); + + v3f rcv; + v3_sub( ra->v, rb->v, rcv ); + + v3f rcv_Ra, rcv_Rb; + v3_cross( ra->I, wca, rcv_Ra ); + v3_cross( rb->I, wcb, rcv_Rb ); + v3_add( rcv_Ra, rcv, rcv ); + v3_sub( rcv, rcv_Rb, rcv ); + + v3f impulse; + v3_muls( rcv, 0.5f, impulse ); + v3_add( impulse, rb->v, rb->v ); + v3_cross( wcb, impulse, impulse ); + v3_add( impulse, rb->I, rb->I ); + + v3_muls( rcv, -0.5f, impulse ); + v3_add( impulse, ra->v, ra->v ); + v3_cross( wca, impulse, impulse ); + v3_add( impulse, ra->I, ra->I ); + + +#if 0 + v3f impulse; + v3_muls( delta, 0.5f*spring, impulse ); + + v3_add( impulse, ra->v, ra->v ); + v3_cross( wca, impulse, impulse ); + v3_add( impulse, ra->I, ra->I ); + + v3_muls( delta, -0.5f*spring, impulse ); + + v3_add( impulse, rb->v, rb->v ); + v3_cross( wcb, impulse, impulse ); + v3_add( impulse, rb->I, rb->I ); +#endif +} + +static void rb_debug( rigidbody *rb, u32 colour ) +{ + v3f *box = rb->bbx; + v3f p000, p001, p010, p011, p100, p101, p110, p111; + + p000[0]=box[0][0];p000[1]=box[0][1];p000[2]=box[0][2]; + p001[0]=box[0][0];p001[1]=box[0][1];p001[2]=box[1][2]; + p010[0]=box[0][0];p010[1]=box[1][1];p010[2]=box[0][2]; + p011[0]=box[0][0];p011[1]=box[1][1];p011[2]=box[1][2]; + + p100[0]=box[1][0];p100[1]=box[0][1];p100[2]=box[0][2]; + p101[0]=box[1][0];p101[1]=box[0][1];p101[2]=box[1][2]; + p110[0]=box[1][0];p110[1]=box[1][1];p110[2]=box[0][2]; + p111[0]=box[1][0];p111[1]=box[1][1];p111[2]=box[1][2]; + + m4x3_mulv( rb->to_world, p000, p000 ); + m4x3_mulv( rb->to_world, p001, p001 ); + m4x3_mulv( rb->to_world, p010, p010 ); + m4x3_mulv( rb->to_world, p011, p011 ); + m4x3_mulv( rb->to_world, p100, p100 ); + m4x3_mulv( rb->to_world, p101, p101 ); + m4x3_mulv( rb->to_world, p110, p110 ); + m4x3_mulv( rb->to_world, p111, p111 ); + + vg_line( p000, p001, colour ); + vg_line( p001, p011, colour ); + vg_line( p011, p010, colour ); + vg_line( p010, p000, colour ); + + vg_line( p100, p101, colour ); + vg_line( p101, p111, colour ); + vg_line( p111, p110, colour ); + vg_line( p110, p100, colour ); + + vg_line( p100, p000, colour ); + vg_line( p101, p001, colour ); + vg_line( p110, p010, colour ); + vg_line( p111, p011, colour ); + + vg_line( p000, p110, colour ); + vg_line( p100, p010, colour ); +} + +#endif /* RIGIDBODY_H */ diff --git a/scene.h b/scene.h index 10015f5..23307ad 100644 --- a/scene.h +++ b/scene.h @@ -971,6 +971,23 @@ static void bvh_subdiv( scene *s, u32 inode ) float split = node->bbx[0][axis] + extent[axis]*0.5f; + /* To beat: 121,687 / 136,579 + * 136,375 + */ + + float avg = 0.0; + for( u32 t=0; tcount; t++ ) + { + u32 *ti = &s->indices[(node->start+t)*3]; + float a = s->verts[ti[0]].co[axis], + b = s->verts[ti[1]].co[axis], + c = s->verts[ti[2]].co[axis]; + avg += (a+b+c)/3.0; + } + avg /= (float)node->count; + + split = avg; + i32 i = node->start, j = i + node->count-1; @@ -1090,55 +1107,6 @@ static void bvh_debug( scene *s, v3f pos ) bvh_debug_node( s, 0, pos, 0x4000ffa8 ); } -static int bvh_scene_sample_node( scene *s, u32 inode, v3f pos, v3f norm ) -{ - bvh_node *node = &s->bvh.nodes[ inode ]; - - if( (pos[0] >= node->bbx[0][0] && pos[0] <= node->bbx[1][0]) && - (pos[2] >= node->bbx[0][2] && pos[2] <= node->bbx[1][2]) ) - { - if( !node->count ) - { - if( bvh_scene_sample_node( s, node->il, pos, norm )) return 1; - if( bvh_scene_sample_node( s, node->ir, pos, norm )) return 1; - } - else - { - for( u32 i=0; icount; i++ ) - { - u32 idx = (node->start+i)*3; - model_vert *pa = &s->verts[ s->indices[ idx+0 ] ], - *pb = &s->verts[ s->indices[ idx+1 ] ], - *pc = &s->verts[ s->indices[ idx+2 ] ]; - - float height; - if( triangle_raycast2d( pa->co, pb->co, pc->co, pos, &height )) - { - pos[1] = height; - - if( norm ) - { - v3f v0, v1; - v3_sub( pa->co, pb->co, v0 ); - v3_sub( pc->co, pb->co, v1 ); - v3_cross( v1, v0, norm ); - v3_normalize( norm ); - } - - return 1; - } - } - } - } - - return 0; -} - -static int bvh_scene_sample( scene *s, v3f pos, v3f norm) -{ - return bvh_scene_sample_node( s, 0, pos, norm ); -} - typedef struct ray_hit ray_hit; struct ray_hit { @@ -1248,7 +1216,7 @@ static int bvh_raycast( scene *s, v3f co, v3f dir, ray_hit *hit ) if( count ) { - vg_line( co, pb, 0xff00ffff ); + //vg_line( co, pb, 0xff00ffff ); v3f v0, v1; @@ -1263,9 +1231,81 @@ static int bvh_raycast( scene *s, v3f co, v3f dir, ray_hit *hit ) v3_muladds( co, dir, hit->dist, hit->pos ); } else - vg_line( co, pb, 0xff0000ff ); + { + //vg_line( co, pb, 0xff0000ff ); + } return count; } +static int bvh_scene_sample_node_h( scene *s, u32 inode, v3f pos, v3f norm ) +{ + bvh_node *node = &s->bvh.nodes[ inode ]; + + if( (pos[0] >= node->bbx[0][0] && pos[0] <= node->bbx[1][0]) && + (pos[2] >= node->bbx[0][2] && pos[2] <= node->bbx[1][2]) ) + { + if( !node->count ) + { + if( bvh_scene_sample_node_h( s, node->il, pos, norm )) return 1; + if( bvh_scene_sample_node_h( s, node->ir, pos, norm )) return 1; + } + else + { + for( u32 i=0; icount; i++ ) + { + u32 idx = (node->start+i)*3; + model_vert *pa = &s->verts[ s->indices[ idx+0 ] ], + *pb = &s->verts[ s->indices[ idx+1 ] ], + *pc = &s->verts[ s->indices[ idx+2 ] ]; + + float height; + if( triangle_raycast2d( pa->co, pb->co, pc->co, pos, &height )) + { + pos[1] = height; + + if( norm ) + { + v3f v0, v1; + v3_sub( pa->co, pb->co, v0 ); + v3_sub( pc->co, pb->co, v1 ); + v3_cross( v1, v0, norm ); + v3_normalize( norm ); + } + + return 1; + } + } + } + } + + return 0; +} + +static int bvh_scene_sample_h( scene *s, v3f pos, v3f norm) +{ + return bvh_scene_sample_node_h( s, 0, pos, norm ); +} + +static int bvh_scene_sample( scene *s, v3f pos, v3f norm ) +{ + ray_hit hit; + hit.dist = INFINITY; + + v3f ray_pos; + v3_add( pos, (v3f){0.0f,16000.0f,0.0f}, ray_pos ); + + if( bvh_raycast( s, ray_pos, (v3f){0.0f,-1.0f,0.0f}, &hit )) + { + pos[1] = hit.pos[1]; + + if( norm ) + v3_copy( hit.normal, norm ); + + return 1; + } + + return 0; +} + #endif diff --git a/textures/grid_terrain.png b/textures/grid_terrain.png new file mode 100644 index 0000000..f88b759 Binary files /dev/null and b/textures/grid_terrain.png differ