#include "character.h"
#include "bvh.h"
+/*
+ * Constants
+ */
+
+static float
+ k_walkspeed = 2.0f,
+ k_board_radius = 0.25f,
+ k_board_length = 0.65f,
+ k_board_allowance = 0.04f,
+ k_friction_lat = 8.68f,
+ k_friction_resistance = 0.02f,
+ k_max_push_speed = 16.0f,
+ k_push_accel = 5.0f,
+ k_push_cycle_rate = 8.0f,
+ k_steer_ground = 2.5f,
+ k_steer_air = 3.6f,
+ k_steer_air_lerp = 0.3f,
+ k_downforce = 5.0f;
+
static int freecam = 0;
-static float k_walkspeed = 2.0f;
static int walk_grid_iterations = 1;
static struct gplayer
{
/* Physics */
- v3f co, v, a, v_last, m, bob, vl;
+ v3f co, v, w, a, v_last, m, bob, vl;
+
+ /* Utility */
+ v3f up, right, forward;
+
v4f rot;
float vswitch, slip, slip_last,
reverse;
+ int rf, rb;
+ v3f ctf, ctb;
+
float iY; /* Yaw inertia */
int in_air, is_dead, on_board;
.on_board = 1
};
-static float *player_cam_pos(void)
-{
- return player.camera_pos;
-}
+/*
+ * Player API
+ */
static void player_transform_update(void)
{
v3_copy( player.co, player.to_world[3] );
m4x3_invert_affine( player.to_world, player.to_local );
-}
-
-static int reset_player( int argc, char const *argv[] )
-{
- struct respawn_point *rp = NULL, *r;
-
- if( argc > 1 )
- {
- for( int i=0; i<world.spawn_count; i++ )
- {
- r = &world.spawns[i];
- if( !strcmp( r->name, argv[0] ) )
- {
- rp = r;
- break;
- }
- }
-
- if( !rp )
- vg_warn( "No spawn named '%s'\n", argv[0] );
- }
-
- if( !rp )
- {
- float min_dist = INFINITY;
- for( int i=0; i<world.spawn_count; i++ )
- {
- r = &world.spawns[i];
- float d = v3_dist2( r->co, player.co );
-
- if( d < min_dist )
- {
- min_dist = d;
- rp = r;
- }
- }
- }
-
- if( !rp )
- {
- vg_error( "No spawn found\n" );
- return 0;
- }
-
- v4_copy( r->q, player.rot );
- v3_copy( r->co, player.co );
-
- player.vswitch = 1.0f;
- player.slip_last = 0.0f;
- player.is_dead = 0;
- player.in_air = 1;
- m3x3_identity( player.vr );
- player.mdl.shoes[0] = 1;
- player.mdl.shoes[1] = 1;
-
- player_transform_update();
- m3x3_mulv( player.to_world, (v3f){ 0.0f, 0.0f, -0.2f }, player.v );
- return 1;
+ m3x3_mulv( player.to_world, (v3f){1.0f,0.0f, 0.0f}, player.right );
+ m3x3_mulv( player.to_world, (v3f){0.0f,1.0f, 0.0f}, player.up );
+ m3x3_mulv( player.to_world, (v3f){0.0f,0.0f,-1.0f}, player.forward );
}
+/*
+ * Free camera movement
+ */
+
static void player_mouseview(void)
{
if( gui_want_mouse() )
v3_add( move_vel, player.camera_pos, player.camera_pos );
}
+/*
+ * Player Physics Implementation
+ */
+
static void apply_gravity( v3f vel, float const timestep )
{
v3f gravity = { 0.0f, -9.6f, 0.0f };
* TODO: The angle bias should become greater when launching from a steeper
* angle and skewed towords more 'downwards' angles when launching from
* shallower trajectories
+ *
+ * it should also be tweaked by the controller left stick being pushed
+ * up or down
*/
static void player_start_air(void)
{
+ if( player.in_air )
+ return;
+
player.in_air = 1;
float pstep = ktimestep*10.0f;
float k_bias = 0.97f;
- v3f axis, vup;
- m3x3_mulv( player.to_world, (v3f){0.0f,1.0f,0.0f}, vup );
- v3_cross( vup, player.v, axis );
+ v3f axis;
+ v3_cross( player.up, player.v, axis );
v3_normalize( axis );
player.land_log_count = 0;
}
}
}
-
- //v3_rotate( player.v, best_velocity_mod, axis, player.v );
-
- return;
- v3_muls( player.v, best_velocity_mod, player.v );
}
-static int sample_if_resistant( v3f pos )
-{
- v3f ground;
- v3_copy( pos, ground );
- ground[1] += 4.0f;
-
- ray_hit hit;
- hit.dist = INFINITY;
-
- if( ray_world( ground, (v3f){0.0f,-1.0f,0.0f}, &hit ))
- {
- v3f angle;
- v3_copy( player.v, angle );
- v3_normalize( angle );
- float resistance = v3_dot( hit.normal, angle );
-
- if( resistance < 0.25f )
- {
- v3_copy( hit.pos, pos );
- return 1;
- }
- }
-
- return 0;
-}
-
-static float stable_force( float current, float diff )
+static void draw_cross(v3f pos,u32 colour, float scale)
{
- float new = current + diff;
-
- if( new * current < 0.0f )
- return 0.0f;
-
- return new;
+ v3f p0, p1;
+ v3_add( (v3f){ scale,0.0f,0.0f}, pos, p0 );
+ v3_add( (v3f){-scale,0.0f,0.0f}, pos, p1 );
+ vg_line( p0, p1, colour );
+ v3_add( (v3f){0.0f, scale,0.0f}, pos, p0 );
+ v3_add( (v3f){0.0f,-scale,0.0f}, pos, p1 );
+ vg_line( p0, p1, colour );
+ v3_add( (v3f){0.0f,0.0f, scale}, pos, p0 );
+ v3_add( (v3f){0.0f,0.0f,-scale}, pos, p1 );
+ vg_line( p0, p1, colour );
}
-static void player_physics_ground(void)
+static void player_physics_control(void)
{
- /*
- * Getting surface collision points,
- * the contact manifold is a triangle for simplicity.
+ /*
+ * Computing localized friction forces for controlling the character
+ * Friction across X is significantly more than Z
*/
- v3f contact_front, contact_back, contact_norm, vup, vside,
- axis;
-
- float klength = 0.65f;
- 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 );
- m3x3_mulv( player.to_world, (v3f){ 0.0f, 1.0f, 0.0f}, vup );
- m3x3_mulv( player.to_world, (v3f){ 1.0f, 0.0f, 0.0f}, vside );
-
- v3f cn0, cn1, cn2;
-
- int contact_count =
- sample_if_resistant( contact_front ) +
- sample_if_resistant( contact_back ) +
- sample_if_resistant( contact_norm );
-
- if( contact_count < 3 )
- {
- player_start_air();
- return;
- }
-
- v3f norm;
- v3f v0, v1;
- v3_sub( contact_norm, contact_front, v0 );
- v3_sub( contact_back, contact_front, v1 );
- v3_cross( v1, v0, norm );
- v3_normalize( norm );
-
- vg_line( contact_norm, contact_front, 0xff00ff00 );
- vg_line( contact_back, contact_front, 0xff0000ff );
-
- /* Surface alignment */
- float angle = v3_dot( vup, norm );
- v3_cross( vup, norm, axis );
-
- if( angle < 0.999f )
- {
- v4f correction;
- q_axis_angle( correction, axis, acosf(angle) );
- q_mul( correction, player.rot, player.rot );
- }
-
- float resistance = v3_dot( norm, player.v );
- if( resistance >= 0.0f )
- {
- player_start_air();
- return;
- }
- else
- {
- v3_muladds( player.v, norm, -resistance, player.v );
- }
-
- /* This is where velocity integration used to be */
-
- float slip = 0.0f;
-
- player.co[1] = (contact_front[1]+contact_back[1])*0.5f;
v3f vel;
m3x3_mulv( player.to_local, player.v, vel );
-
- /* Calculate local forces */
+ float slip = 0.0f;
if( fabsf(vel[2]) > 0.01f )
slip = fabsf(-vel[0] / vel[2]) * vg_signf(vel[0]);
for( int i=0; i<5; i++ )
{
vel[2] = stable_force( vel[2], vg_signf( vel[2] ) * fwd_resistance );
-
- /* This used to be -7.0, then -10.0 */
- vel[0] = stable_force( vel[0], vg_signf( vel[0] ) * -8.5f *substep );
+ vel[0] = stable_force( vel[0], vg_signf( vel[0] ) * -8.78f *substep );
}
static double start_push = 0.0;
if( !vg_get_button("break") && vg_get_button( "push" ) )
{
- float const k_maxpush = 16.0f,
- k_pushaccel = 5.0f;
-
- float cycle_time = vg_time-start_push,
- amt = k_pushaccel * (sinf( cycle_time * 8.0f )*0.5f+0.5f)*ktimestep,
+ float cycle_time = (vg_time-start_push)*k_push_cycle_rate,
+ amt = k_push_accel * (sinf(cycle_time)*0.5f+0.5f)*ktimestep,
current = v3_length( vel ),
- new_vel = vg_minf( current + amt, k_maxpush );
- new_vel -= vg_minf(current, k_maxpush);
+ new_vel = vg_minf( current + amt, k_max_push_speed );
+ new_vel -= vg_minf(current, k_max_push_speed);
vel[2] -= new_vel * player.reverse;
}
m3x3_mulv( player.to_world, vel, player.v );
-
- if( vg_get_button( "yawl" ) )
- player.iY += 3.6f * ktimestep;
- if( vg_get_button( "yawr" ) )
- player.iY -= 3.6f * ktimestep;
float steer = vg_get_axis( "horizontal" );
- player.iY -= vg_signf(steer)*powf(steer,2.0f) * 2.5f * ktimestep;
+ player.iY -= vg_signf(steer)*powf(steer,2.0f) * k_steer_ground * ktimestep;
- /* Too much lean and it starts to look like a snowboard here */
v2_lerp( player.board_xy, (v2f){ slip*0.25f, 0.0f },
ktimestep*5.0f, player.board_xy);
}
-static void draw_cross(v3f pos,u32 colour, float scale)
-{
- v3f p0, p1;
- v3_add( (v3f){ scale,0.0f,0.0f}, pos, p0 );
- v3_add( (v3f){-scale,0.0f,0.0f}, pos, p1 );
- vg_line( p0, p1, colour );
- v3_add( (v3f){0.0f, scale,0.0f}, pos, p0 );
- v3_add( (v3f){0.0f,-scale,0.0f}, pos, p1 );
- vg_line( p0, p1, colour );
- v3_add( (v3f){0.0f,0.0f, scale}, pos, p0 );
- v3_add( (v3f){0.0f,0.0f,-scale}, pos, p1 );
- vg_line( p0, p1, colour );
-}
-
-static void player_physics_air(void)
+static void player_physics_control_air(void)
{
m3x3_mulv( player.vr, player.v, player.v );
- draw_cross( player.land_target, 0xff0000ff, 1 );
+ draw_cross( player.land_target, 0xff0000ff, 0.25f );
- v3f ground_pos;
- v3_copy( player.co, ground_pos );
- ground_pos[1] += 4.0f;
-
ray_hit hit;
- hit.dist = INFINITY;
- if( ray_world( ground_pos, (v3f){0.0f,-1.0f,0.0f}, &hit ))
- {
- if( hit.pos[1] > player.co[1] )
- {
- player.in_air = 0;
-
- if( !ray_hit_is_ramp( &hit ) )
- {
- player.is_dead = 1;
- character_ragdoll_copypose( &player.mdl, player.v );
- }
- return;
- }
- }
-
- /* Prediction
+ /*
+ * Prediction
*/
float pstep = ktimestep*10.0f;
q_mul( correction, player.rot, player.rot );
}
- draw_cross( contact.pos, 0xffff0000, 1 );
+ draw_cross( contact.pos, 0xffff0000, 0.25f );
break;
}
time_to_impact += pstep;
}
- player.iY -= vg_get_axis( "horizontal" ) * 3.6f * ktimestep;
+ player.iY -= vg_get_axis( "horizontal" ) * k_steer_air * ktimestep;
{
-
float iX = vg_get_axis( "vertical" ) *
- player.reverse * 3.6f * limiter * ktimestep;
+ player.reverse * k_steer_air * limiter * ktimestep;
+
static float siX = 0.0f;
- siX = vg_lerpf( siX, iX, 0.3f );
+ siX = vg_lerpf( siX, iX, k_steer_air_lerp );
v4f rotate;
v3f vside;
v2_lerp( player.board_xy, target, ktimestep*3.0f, player.board_xy );
}
+static void player_physics(void)
+{
+ m4x3f mboard;
+ v3_copy( player.to_world[0], mboard[0] );
+ v3_copy( player.to_world[2], mboard[1] );
+ v3_copy( player.to_world[1], mboard[2] );
+ m4x3_mulv( player.to_world, (v3f){ 0.0f, 0.3f, 0.0f }, mboard[3] );
+
+ debug_capsule( mboard, k_board_length*2.0f, k_board_radius, 0xff0000ff );
+
+ boxf region = {{ -k_board_radius, -k_board_length, -k_board_radius },
+ { k_board_radius, k_board_length, k_board_radius }};
+ m4x3_transform_aabb( mboard, region );
+
+ u32 geo[256];
+ v3f tri[3];
+ int len = bh_select( &world.geo.bhtris, region, geo, 256 );
+
+ v3f poles[2];
+ m4x3_mulv(mboard, (v3f){0.0f,-k_board_length+k_board_radius,0.0f}, poles[0]);
+ m4x3_mulv(mboard, (v3f){0.0f, k_board_length-k_board_radius,0.0f}, poles[1]);
+
+ struct contact manifold[12];
+ int manifold_count = 0;
+
+ v3f surface_avg = {0.0f, 0.0f, 0.0f};
+
+ for( int i=0; i<len; i++ )
+ {
+ u32 *ptri = &world.geo.indices[ geo[i]*3 ];
+
+ for( int j=0; j<3; j++ )
+ v3_copy( world.geo.verts[ptri[j]].co, tri[j] );
+
+ vg_line(tri[0],tri[1],0xff00ff00 );
+ vg_line(tri[1],tri[2],0xff00ff00 );
+ vg_line(tri[2],tri[0],0xff00ff00 );
+
+ v3f co, norm;
+ float p;
+
+ for( int j=0; j<2; j++ )
+ {
+ if( sphere_vs_triangle( poles[j], k_board_radius, tri,co,norm,&p) )
+ {
+ if(manifold_count >= vg_list_size(manifold))
+ {
+ vg_error("Manifold overflow!\n");
+ break;
+ }
+
+ v3f p1;
+ v3_muladds( poles[j], norm, p, p1 );
+ vg_line( poles[j], p1, 0xffffffff );
+
+ struct contact *ct = &manifold[manifold_count ++];
+ v3_sub( co, player.co, ct->delta );
+ v3_copy( co, ct->co );
+ v3_copy( norm, ct->n );
+ ct->bias = -0.2f*k_rb_rate*vg_minf(0.0f,-p+k_board_allowance);
+ ct->norm_impulse = 0.0f;
+
+ v3_add( norm, surface_avg, surface_avg );
+ }
+ }
+ }
+
+ if( !manifold_count )
+ {
+ player_start_air();
+ }
+ else
+ {
+ v3_normalize( surface_avg );
+
+ if( v3_dot( player.v, surface_avg ) > 0.5f )
+ {
+ player_start_air();
+ }
+ else
+ player.in_air = 0;
+ }
+
+ for( int j=0; j<10; j++ )
+ {
+ for( int i=0; i<manifold_count; i++ )
+ {
+ struct contact *ct = &manifold[i];
+
+ v3f dv;
+ v3_cross( player.w, ct->delta, dv );
+ v3_add( player.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, player.v, player.v );
+
+ v3_cross( ct->delta, impulse, impulse );
+
+ /*
+ * W Impulses are limited to the Y and X axises, we don't really want
+ * roll angular velocities being included.
+ *
+ * Can also tweak the resistance of each axis here by scaling the wx,wy
+ * components.
+ */
+
+ float wy = v3_dot( player.up, impulse ),
+ wx = v3_dot( player.right, impulse );
+
+ v3_muladds( player.w, player.up, wy, player.w );
+ v3_muladds( player.w, player.right, wx, player.w );
+ }
+ }
+
+ if( !player.in_air )
+ {
+ v3f axis;
+ float angle = v3_dot( player.up, surface_avg );
+ v3_cross( player.up, surface_avg, axis );
+
+ float cz = v3_dot( player.forward, axis );
+ v3_muls( player.forward, cz, axis );
+
+ if( angle < 0.999f )
+ {
+ v4f correction;
+ q_axis_angle( correction, axis, acosf(angle)*0.3f );
+ q_mul( correction, player.rot, player.rot );
+ }
+
+ v3_muladds( player.v, player.up, -k_downforce*ktimestep, player.v );
+ player_physics_control();
+ }
+ else
+ {
+ player_physics_control_air();
+ }
+}
+
static void player_do_motion(void)
{
float horizontal = vg_get_axis("horizontal"),
vertical = vg_get_axis("vertical");
- if( player.in_air )
- player_physics_air();
-
- if( !player.in_air )
- player_physics_ground();
+ player_physics();
/* Integrate velocity */
v3f prevco;
apply_gravity( player.v, ktimestep );
v3_muladds( player.co, player.v, ktimestep, player.co );
- /* Integrate inertia */
- v4f rotate; v3f vup = {0.0f,1.0f,0.0f};
- m3x3_mulv( player.to_world, vup, vup );
+ /* Real angular velocity integration */
+ v3_lerp( player.w, (v3f){0.0f,0.0f,0.0f}, 0.125f, player.w );
+ if( v3_length2( player.w ) > 0.0f )
+ {
+ v4f rotation;
+ v3f axis;
+ v3_copy( player.w, axis );
+
+ float mag = v3_length( axis );
+ v3_divs( axis, mag, axis );
+ q_axis_angle( rotation, axis, mag*k_rb_delta );
+ q_mul( rotation, player.rot, player.rot );
+ }
- static float siY = 0.0f;
+ /* Faux angular velocity */
+ v4f rotate;
+ static float siY = 0.0f;
float lerpq = player.in_air? 0.04f: 0.3f;
siY = vg_lerpf( siY, player.iY, lerpq );
- q_axis_angle( rotate, vup, siY );
+ q_axis_angle( rotate, player.up, siY );
q_mul( rotate, player.rot, player.rot );
+ player.iY = 0.0f;
- player.iY = 0.0f; /* temp */
-
- /* GATE COLLISION */
-
+ /*
+ * Gate intersection, by tracing a line over the gate planes
+ */
for( int i=0; i<world.gate_count; i++ )
{
teleport_gate *gate = &world.gates[i];
}
}
- /* Camera and character */
player_transform_update();
-
- v3_lerp( player.vl, player.v, 0.05f, player.vl );
-
- player.angles[0] = atan2f( player.vl[0], -player.vl[2] );
- player.angles[1] = atan2f( -player.vl[1], sqrtf(player.vl[0]*player.vl[0]+
- player.vl[2]*player.vl[2]) ) * 0.7f;
}
-static int player_walkgrid_tri_walkable( u32 tri[3] )
-{
- return tri[0] < world.sm_surface.vertex_count;
-}
+/*
+ * Walkgrid implementation,
+ * loosely based of cmuratoris youtube video 'Killing the Walkmonster'
+ */
#define WALKGRID_SIZE 16
struct walkgrid
float h;
};
+static int player_walkgrid_tri_walkable( u32 tri[3] )
+{
+ return tri[0] < world.sm_surface.vertex_count;
+}
+
/*
* Get a sample at this pole location, will return 1 if the sample is valid,
* and pos will be updated to be the intersection location.
player_transform_update();
}
+/*
+ * Animation
+ */
+
static void player_animate(void)
{
/* Camera position */
v3_add( player.m, player.a, player.m );
v3_lerp( player.m, (v3f){0.0f,0.0f,0.0f}, 0.1f, player.m );
- v3f target;
-
+
player.m[0] = vg_clampf( player.m[0], -2.0f, 2.0f );
- player.m[1] = vg_clampf( player.m[1], -0.2f, 5.0f );
+ player.m[1] = vg_clampf( player.m[1], -2.0f, 2.0f );
player.m[2] = vg_clampf( player.m[2], -2.0f, 2.0f );
- v3_copy( player.m, target );
- v3_lerp( player.bob, target, 0.2f, player.bob );
+ v3_lerp( player.bob, player.m, 0.2f, player.bob );
/* Head */
- float lslip = fabsf(player.slip); //vg_minf( 0.4f, slip );
+ float lslip = fabsf(player.slip);
float grabt = vg_get_axis( "grabr" )*0.5f+0.5f;
player.grab = vg_lerpf( player.grab, grabt, 0.04f );
float kheight = 2.0f,
kleg = 0.6f;
- v3f head;
- head[0] = 0.0f;
- head[1] = (0.3f+cosf(lslip)*0.5f*(1.0f-player.grab*0.7f)) * kheight;
- head[2] = 0.0f;
-
v3f offset;
m3x3_mulv( player.to_local, player.bob, offset );
- offset[0] *= 0.3333f;
- offset[1] *= -0.25f;
- offset[2] *= 0.7f;
- v3_muladds( head, offset, 0.7f, head );
- head[1] = vg_clampf( head[1], 0.3f, kheight );
+ static float speed_wobble = 0.0f, speed_wobble_2 = 0.0f;
+
+ float kickspeed = vg_clampf(v3_length(player.v)*(1.0f/40.0f), 0.0f, 1.0f);
+ float kicks = (vg_randf()-0.5f)*2.0f*kickspeed;
+ float sign = vg_signf( kicks );
+ speed_wobble = vg_lerpf( speed_wobble, kicks*kicks*sign, 0.1f );
+ speed_wobble_2 = vg_lerpf( speed_wobble_2, speed_wobble, 0.04f );
+
+ offset[0] *= 0.26f;
+ offset[0] += speed_wobble_2*3.0f;
+
+ offset[1] *= -0.3f;
+ offset[2] *= 0.01f;
+
+ offset[0] = vg_clampf( offset[0], -0.8f, 0.8f );
+ offset[1] = vg_clampf( offset[1], -0.5f, 0.0f );
+
/*
* Animation blending
float speed = v3_length( player.v );
fstand = vg_lerpf(fstand, 1.0f-vg_clampf(speed*0.03f,0.0f,1.0f),0.1f);
- fslide = vg_lerpf(fslide, vg_clampf(lslip+fabsf(offset[0])*0.2f,
- 0.0f,1.0f), 0.04f);
+ fslide = vg_lerpf(fslide, vg_clampf(lslip,0.0f,1.0f), 0.04f);
fdirz = vg_lerpf(fdirz, player.reverse > 0.0f? 1.0f: 0.0f, 0.04f );
- fdirx = vg_lerpf(fdirx, player.slip < 0.0f? 1.0f: 0.0f, 0.04f );
+ fdirx = vg_lerpf(fdirx, player.slip < 0.0f? 1.0f: 0.0f, 0.01f );
ffly = vg_lerpf(ffly, player.in_air? 1.0f: 0.0f, 0.04f );
character_pose_reset( &player.mdl );
/* TODO */
- fstand = 1.0f;
+ float fstand1 = 1.0f-(1.0f-fstand)*0.3f;
float amt_air = ffly*ffly,
amt_ground = 1.0f-amt_air,
amt_std = (1.0f-fslide) * amt_ground,
- amt_stand = amt_std * fstand,
- amt_aero = amt_std * (1.0f-fstand),
+ amt_stand = amt_std * fstand1,
+ amt_aero = amt_std * (1.0f-fstand1),
amt_slide = amt_ground * fslide;
character_final_pose( &player.mdl, offset, &pose_stand, amt_stand*fdirz );
character_final_pose( &player.mdl, (v3f){0.0f,0.0f,0.0f},
&pose_fly, amt_air );
-#if 0
- static float fupper = 0.0f;
- fupper = vg_lerpf( fupper, -vg_get_axis("horizontal")*0.2f, 0.1f );
- character_yaw_upper( &player.mdl, fupper );
-#endif
-
- /* Camera position */
- v3_lerp( player.smooth_localcam, player.mdl.cam_pos, 0.08f,
- player.smooth_localcam );
- v3_muladds( player.smooth_localcam, offset, 0.7f, player.camera_pos );
- player.camera_pos[1] = vg_clampf( player.camera_pos[1], 0.3f, kheight );
-
- m4x3_mulv( player.to_world, player.camera_pos, player.camera_pos );
-
- player.air_blend = vg_lerpf( player.air_blend, player.in_air, 0.04f );
- v3_muladds( player.camera_pos, player.v, -0.05f*player.air_blend,
- player.camera_pos );
/*
* Additive effects
v3f localv;
m3x3_mulv( player.to_local, player.v, localv );
-#if 0
- v3_muladds( arm_l->end, localv, -0.01f, arm_l->end );
- v3_muladds( arm_r->end, localv, -0.01f, arm_r->end );
-#endif
-
/* New board transformation */
v4f board_rotation; v3f board_location;
}
}
- v3_lerp( player.handl, player.handl_target, 0.1f, player.handl );
- v3_lerp( player.handr, player.handr_target, 0.1f, player.handr );
+ v3_lerp( player.handl, player.handl_target, 1.0f, player.handl );
+ v3_lerp( player.handr, player.handr_target, 1.0f, player.handr );
v3_copy( player.handl, player.mdl.ik_arm_l.end );
v3_copy( player.handr, player.mdl.ik_arm_r.end );
player.mdl.rhead = rhead;
}
-static int giftwrapXZ( v3f *points, int *output, int len )
+static void player_camera_update(void)
{
- int l, p, q, count;
-
- if( len < 3 )
- return 0;
-
- l = 0;
- for( int i=1; i<len; i++ )
- if( points[i][0] < points[l][0] )
- l = i;
-
- p = l;
- count = 0;
- do
- {
- if( count >= len )
- {
- vg_error ("MANIFOLD ERR (%d)\n", count );
- return 0;
- }
- output[ count ++ ] = p;
-
- q = (p+1)%len;
-
- for( int i=0; i<len; i++ )
- {
- float orient =
- (points[i][2]-points[p][2])*(points[q][0]-points[i][0]) -
- (points[i][0]-points[p][0])*(points[q][2]-points[i][2]);
-
- if( orient > 0.0001f )
- {
- q = i;
- }
- }
- p = q;
- }
- while( p != l );
-
- return count;
+ /* Update camera matrices */
+ m4x3_identity( player.camera );
+ m4x3_rotate_y( player.camera, -player.angles[0] );
+ m4x3_rotate_x( player.camera, -player.angles[1] );
+ v3_copy( player.camera_pos, player.camera[3] );
+ m4x3_invert_affine( player.camera, player.camera_inverse );
}
-
-static void player_do_collision( rigidbody *rb )
-{
- /*
- * If point is inside box
- * find normal (theres 8 simple pyramid regions for this, x>y/dim .. etc)
- * find distance (same sorta thing)
- *
- * apply normal impulse to rotation
- * correct position based on new penetration amount if needed
- * apply normal impulse to velocity
- */
-
- v3f pfront, pback;
- m4x3_mulv( player.to_world, (v3f){ 0.0f,0.0f,-1.0f }, pfront );
- m4x3_mulv( player.to_world, (v3f){ 0.0f,0.0f, 1.0f }, pback );
-
- float const kheight = 2.0f;
-
- v3f verts[8];
-
- v3f a, b;
- v3_copy( rb->bbx[0], a );
- v3_copy( rb->bbx[1], b );
-
- m4x3f compound;
- m4x3_mul( player.to_local, rb->to_world, compound );
-
- m4x3_mulv( compound, (v3f){ a[0], a[1], a[2] }, verts[0] );
- m4x3_mulv( compound, (v3f){ a[0], b[1], a[2] }, verts[1] );
- m4x3_mulv( compound, (v3f){ b[0], b[1], a[2] }, verts[2] );
- m4x3_mulv( compound, (v3f){ b[0], a[1], a[2] }, verts[3] );
- m4x3_mulv( compound, (v3f){ a[0], a[1], b[2] }, verts[4] );
- m4x3_mulv( compound, (v3f){ a[0], b[1], b[2] }, verts[5] );
- m4x3_mulv( compound, (v3f){ b[0], b[1], b[2] }, verts[6] );
- m4x3_mulv( compound, (v3f){ b[0], a[1], b[2] }, verts[7] );
-
- int const indices[12][2] = {
- {0,1},{1,2},{2,3},{3,0},{4,5},{5,6},{6,7},{7,4},
- {0,4},{1,5},{2,6},{3,7}
- };
-
- v3f hull[12*2 + 8];
- int hull_indices[12*2 + 8];
- int hull_len = 0;
-
- for( int i=0; i<8; i++ )
- {
- int ia = indices[i][0];
- float ya = verts[ia][1];
-
- if( ya > 0.2f && ya < kheight )
- {
- int add_point = 1;
- for( int j=0; j<hull_len; j++ )
- {
- v2f delta = { verts[ia][0]-hull[j][0], verts[ia][2]-hull[j][2] };
- if( v2_length2( delta ) < 0.0004f )
- {
- add_point = 0;
- break;
- }
- }
- if( add_point )
- v3_copy( verts[ia], hull[hull_len] );
-
- hull[hull_len ++][1] = 0.2f;
- }
- }
-
- for( int i=0; i<vg_list_size(indices); i++ )
- {
- int ia = indices[i][0],
- ib = indices[i][1];
-
- v3f p0, p1;
+static void player_animate_death_cam(void)
+{
+ v3f delta;
+ v3f head_pos;
+ v3_copy( player.mdl.ragdoll[k_chpart_head].co, head_pos );
- float ya = verts[ia][1],
- yb = verts[ib][1],
- d = 1.0f/(yb-ya),
- qa;
+ v3_sub( head_pos, player.camera_pos, delta );
+ v3_normalize( delta );
- float planes[] = { 0.2f, kheight };
-
- for( int k=0; k<vg_list_size(planes); k++ )
- {
- float clip = planes[k];
+ v3f follow_pos;
+ v3_muladds( head_pos, delta, -2.5f, follow_pos );
+ v3_lerp( player.camera_pos, follow_pos, 0.1f, player.camera_pos );
- if( (ya-clip) * (yb-clip) < 0.0f )
- {
- v3_muls( verts[ia], (yb-clip)*d, p0 );
- v3_muladds( p0, verts[ib], -(ya-clip)*d, p0 );
-
- int add_point = 1;
- for( int j=0; j<hull_len; j++ )
- {
- v2f delta = { p0[0]-hull[j][0], p0[2]-hull[j][2] };
- if( v2_length2( delta ) < 0.0004f )
- {
- add_point = 0;
- break;
- }
- }
+ /*
+ * Make sure the camera stays above the ground
+ */
+ v3f min_height = {0.0f,1.0f,0.0f};
- if( add_point )
- v3_copy( p0, hull[hull_len ++] );
+ v3f sample;
+ v3_add( player.camera_pos, min_height, sample );
+ ray_hit hit;
+ hit.dist = min_height[1]*2.0f;
- m4x3_mulv( player.to_world, p0, p0 );
- vg_line_pt3( p0, 0.1f, 0xffffff00 );
- }
- }
- }
+ if( ray_world( sample, (v3f){0.0f,-1.0f,0.0f}, &hit ))
+ v3_add( hit.pos, min_height, player.camera_pos );
- if( hull_len < 3 )
- return;
+ player.camera_pos[1] =
+ vg_maxf( wrender.height + 2.0f, player.camera_pos[1] );
- int len = giftwrapXZ( hull, hull_indices, hull_len );
- for( int i=0; i<len; i++ )
- {
- v3f p0, p1, p2, p3;
- v3_copy( hull[hull_indices[i]], p0 );
- v3_copy( hull[hull_indices[(i+1)%len]], p1 );
- p0[1] = 0.2f;
- p1[1] = 0.2f;
- v3_add( p0, (v3f){0,kheight-0.2f,0}, p2 );
- v3_add( p1, (v3f){0,kheight-0.2f,0}, p3 );
-
- m4x3_mulv( player.to_world, p0, p0 );
- m4x3_mulv( player.to_world, p1, p1 );
- m4x3_mulv( player.to_world, p2, p2 );
- m4x3_mulv( player.to_world, p3, p3 );
-
- vg_line2( p0, p1, 0xff00ffff, 0xff000000 );
- vg_line( p2, p3, 0xff00ffff );
- vg_line( p0, p2, 0xff00ffa0 );
- }
+ player.angles[0] = atan2f( delta[0], -delta[2] );
+ player.angles[1] = -asinf( delta[1] );
+}
- v2f endpoints[] = {{ 0.0f, -1.0f },{ 0.0f, 1.0f }};
+static void player_animate_camera(void)
+{
+ v3f offs = { -0.29f, 0.08f, 0.0f };
+ m3x3_mulv( player.to_world, offs, offs );
+ m4x3_mulv( player.to_world, player.mdl.ik_body.end, player.camera_pos );
+ v3_add( offs, player.camera_pos, player.camera_pos );
- for( int j=0; j<vg_list_size(endpoints); j++ )
- {
- v2f point;
- v2_copy( endpoints[j], point );
-
- int collide = 1;
- float min_dist = 99999.9f;
- v2f normal = {0.0f,0.0f};
- for( int i=0; i<len; i++ )
- {
- v2f p0, p1;
- p0[0] = hull[hull_indices[i]][0];
- p0[1] = hull[hull_indices[i]][2];
- p1[0] = hull[hull_indices[(i+1)%len]][0];
- p1[1] = hull[hull_indices[(i+1)%len]][2];
-
- v2f t,n, rel;
- v2_sub( p1, p0, t );
- n[0] = -t[1];
- n[1] = t[0];
- v2_normalize(n);
-
- v2_sub( point, p0, rel );
- float d = -v2_dot( n, rel ) + 0.5f;
-
- if( d < 0.0f )
- {
- collide = 0;
- break;
- }
-
- if( d < min_dist )
- {
- min_dist = d;
- v2_copy( n, normal );
- }
- }
-
- if( collide )
- {
- v3f p0, p1;
- p0[0] = 0.0f;
- p0[1] = 0.2f;
- p0[2] = -1.0f;
-
- p1[0] = p0[0] + normal[0]*min_dist;
- p1[1] = p0[1];
- p1[2] = p0[2] + normal[1]*min_dist;
-
- m4x3_mulv( player.to_world, p0, p0 );
- m4x3_mulv( player.to_world, p1, p1 );
-
- vg_line( p0, p1, 0xffffffff );
-
- v3f vel;
- m3x3_mulv( player.to_local, player.v, vel );
- vel[1] = vel[2];
+ /* Look angles */
+ v3_lerp( player.vl, player.v, 0.05f, player.vl );
- float vn = vg_maxf( -v2_dot( vel, normal ), 0.0f );
- vn += -0.2f * (1.0f/k_rb_delta) * vg_minf( 0.0f, -min_dist+0.04f );
-
- v2f impulse;
- if( vn > 14.0f )
- {
- player.is_dead = 1;
- character_ragdoll_copypose( &player.mdl, player.v );
- return;
- }
+ float yaw = atan2f( player.vl[0], -player.vl[2] ),
+ pitch = atan2f( -player.vl[1],
+ sqrtf(
+ player.vl[0]*player.vl[0] + player.vl[2]*player.vl[2]
+ )) * 0.7f;
- if( vn > 0.0f )
- {
- v2_muls( normal, min_dist, impulse );
- float rotation = v2_cross( point, impulse )*0.08f;
- v4f rot;
- v3f up = {0.0f,1.0f,0.0f};
- m3x3_mulv( player.to_world, up, up );
- q_axis_angle( rot, up, -rotation );
- q_mul( rot, player.rot, player.rot );
- }
+ player.angles[0] = yaw;
+ player.angles[1] = pitch + 0.30f;
- v2_muls( normal, vn*0.03f, impulse );
- v3f impulse_world = { impulse[0], 0.0f, impulse[1] };
+ /* Camera shake */
+ static v2f shake_damp = {0.0f,0.0f};
+ v2f shake = { vg_randf()-0.5f, vg_randf()-0.5f };
+ v2_muls( shake, v3_length(player.v)*0.3f *(1.0f+fabsf(player.slip)), shake);
+ v2_lerp( shake_damp, shake, 0.01f, shake_damp );
+ shake_damp[0] *= 0.2f;
- m3x3_mulv( player.to_world, impulse_world, impulse_world );
- v3_add( impulse_world, player.v, player.v );
- }
- }
+ v2_muladds( player.angles, shake_damp, 0.1f, player.angles );
}
+/*
+ * Audio
+ */
static void player_audio(void)
{
float speed = vg_minf(v3_length( player.v )*0.1f,1.0f),
}
}
+/*
+ * Public Endpoints
+ */
+static float *player_cam_pos(void)
+{
+ return player.camera_pos;
+}
+
+static int reset_player( int argc, char const *argv[] )
+{
+ struct respawn_point *rp = NULL, *r;
+
+ if( argc == 1 )
+ {
+ for( int i=0; i<world.spawn_count; i++ )
+ {
+ r = &world.spawns[i];
+ if( !strcmp( r->name, argv[0] ) )
+ {
+ rp = r;
+ break;
+ }
+ }
+
+ if( !rp )
+ vg_warn( "No spawn named '%s'\n", argv[0] );
+ }
+
+ if( !rp )
+ {
+ float min_dist = INFINITY;
+
+ vg_info( "%f %f %f\n", player.co[0], player.co[1], player.co[2] );
+ for( int i=0; i<world.spawn_count; i++ )
+ {
+ r = &world.spawns[i];
+ float d = v3_dist2( r->co, player.co );
+
+ vg_info( "Dist %s : %f\n", r->name, d );
+ if( d < min_dist )
+ {
+ min_dist = d;
+ rp = r;
+ }
+ }
+ }
+
+ if( !rp )
+ {
+ vg_error( "No spawn found\n" );
+ if( !world.spawn_count )
+ return 0;
+
+ rp = &world.spawns[0];
+ }
+
+ v4_copy( rp->q, player.rot );
+ v3_copy( rp->co, player.co );
+
+ player.vswitch = 1.0f;
+ player.slip_last = 0.0f;
+ player.is_dead = 0;
+ player.in_air = 1;
+ m3x3_identity( player.vr );
+
+ player.mdl.shoes[0] = 1;
+ player.mdl.shoes[1] = 1;
+
+ player_transform_update();
+ m3x3_mulv( player.to_world, (v3f){ 0.0f, 0.0f, -1.2f }, player.v );
+ return 1;
+}
+
static void player_update(void)
{
for( int i=0; i<player.land_log_count; i++ )
- draw_cross( player.land_target_log[i], player.land_target_colours[i], 1);
+ draw_cross( player.land_target_log[i],
+ player.land_target_colours[i], 0.25f);
if( vg_get_axis("grabl")>0.0f)
reset_player(0,NULL);
player.on_board ^= 0x1;
}
- if( freecam )
+ if( player.is_dead )
{
- player_freecam();
+ character_ragdoll_iter( &player.mdl );
+ character_debug_ragdoll( &player.mdl );
+
+ if( !freecam )
+ player_animate_death_cam();
}
else
{
- if( player.is_dead )
+ if( player.on_board )
{
- /*
- * Follow camera
- */
- character_ragdoll_iter( &player.mdl );
- character_debug_ragdoll( &player.mdl );
-
- v3f delta;
- v3f head_pos;
- v3_copy( player.mdl.ragdoll[k_chpart_head].co, head_pos );
-
- v3_sub( head_pos, player.camera_pos, delta );
- v3_normalize( delta );
-
- v3f follow_pos;
- v3_muladds( head_pos, delta, -2.5f, follow_pos );
- v3_lerp( player.camera_pos, follow_pos, 0.1f, player.camera_pos );
-
- /*
- * Make sure the camera stays above the ground
- */
- v3f min_height = {0.0f,1.0f,0.0f};
+ player_do_motion();
+ player_animate();
- v3f sample;
- v3_add( player.camera_pos, min_height, sample );
- ray_hit hit;
- hit.dist = min_height[1]*2.0f;
-
- if( ray_world( sample, (v3f){0.0f,-1.0f,0.0f}, &hit ))
- v3_add( hit.pos, min_height, player.camera_pos );
-
- player.camera_pos[1] =
- vg_maxf( wrender.height + 2.0f, player.camera_pos[1] );
-
- player.angles[0] = atan2f( delta[0], -delta[2] );
- player.angles[1] = -asinf( delta[1] );
+ if( !freecam )
+ player_animate_camera();
}
else
{
- if( player.on_board )
- {
- bh_debug_node(&world.bhcubes, 0,
- player.camera_pos, 0xff80ff00 );
-
- u32 colliders[16];
- boxf wbox = {{ -2.0f, -2.0f, -2.0f },
- { 2.0f, 2.0f, 2.0f }};
- m4x3_transform_aabb( player.to_world, wbox );
- int len = bh_select( &world.bhcubes, wbox, colliders, 32 );
-
- for( int i=0; i<len; i++ )
- player_do_collision( &world.temp_rbs[colliders[i]] );
-
- player_do_motion();
- player_animate();
-
- v3f offs = { -0.29f, 0.08f, 0.0f };
- m3x3_mulv( player.to_world, offs, offs );
- m4x3_mulv( player.to_world, player.mdl.ik_body.end, player.camera_pos );
- //m4x3_mulv( player.mdl.matrices[k_chpart_head], offs, player.camera_pos );
- // v3_copy( player.mdl.matrices[k_chpart_head][3], player.camera_pos );
- v3_add( offs, player.camera_pos, player.camera_pos );
- }
- else
- {
- player_walkgrid();
- }
+ player_walkgrid();
}
}
- player_audio();
+ if( freecam )
+ player_freecam();
- /* Update camera matrices */
- m4x3_identity( player.camera );
- m4x3_rotate_y( player.camera, -player.angles[0] );
- m4x3_rotate_x( player.camera, -0.30f -player.angles[1] );
- v3_copy( player.camera_pos, player.camera[3] );
- m4x3_invert_affine( player.camera, player.camera_inverse );
+ player_camera_update();
+ player_audio();
}
static void draw_player(void)
#define RIGIDBODY_H
#define RB_DEPR
-#define k_rb_delta (1.0f/60.0f)
+#define k_rb_rate 60.0f
+#define k_rb_delta (1.0f/k_rb_rate)
typedef struct rigidbody rigidbody;
struct rigidbody
{
v3f co, v, I;
v4f q;
+
+ enum rb_shape
+ {
+ k_rb_shape_box,
+ k_rb_shape_capsule
+ }
+ type;
+ v3f top, bottom;
+ float radius;
+
boxf bbx, bbx_world;
float inv_mass;
/* 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 */
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 );
+ vg_line_boxf_transformed( rb->to_world, rb->bbx, colour );
}
/*
m4x3_mulv( ra->to_world, (v3f){ b[0], b[1], b[2] }, verts[6] );
m4x3_mulv( ra->to_world, (v3f){ b[0], a[1], b[2] }, verts[7] );
+ vg_line_boxf_transformed( rb_static->to_world, rb_static->bbx, 0xff0000ff );
+
int count = 0;
for( int i=0; i<8; i++ )
}
}
+/*
+ * Capsule phyics
+ */
+
+static float closest_segment_segment( v3f p1, v3f q1, v3f p2, v3f q2,
+ float *s, float *t, v3f c1, v3f c2)
+{
+ v3f d1,d2,r;
+ v3_sub( q1, p1, d1 );
+ v3_sub( q2, p2, d2 );
+ v3_sub( p1, p2, r );
+
+ float a = v3_length2( d1 ),
+ e = v3_length2( d2 ),
+ f = v3_dot( d2, r );
+
+ const float kEpsilon = 0.0001f;
+
+ if( a <= kEpsilon && e <= kEpsilon )
+ {
+ *s = 0.0f;
+ *t = 0.0f;
+ v3_copy( p1, c1 );
+ v3_copy( p2, c2 );
+
+ v3f v0;
+ v3_sub( c1, c2, v0 );
+
+ return v3_length2( v0 );
+ }
+
+ if( a<= kEpsilon )
+ {
+ *s = 0.0f;
+ *t = vg_clampf( f / e, 0.0f, 1.0f );
+ }
+ else
+ {
+ float c = v3_dot( d1, r );
+ if( e <= kEpsilon )
+ {
+ *t = 0.0f;
+ *s = vg_clampf( -c / a, 0.0f, 1.0f );
+ }
+ else
+ {
+ float b = v3_dot(d1,d2),
+ d = a*e-b*b;
+
+ if( d != 0.0f )
+ {
+ *s = vg_clampf((b*f - c*e)/d, 0.0f, 1.0f);
+ }
+ else
+ {
+ *s = 0.0f;
+ }
+
+ *t = (b*(*s)+f) / e;
+
+ if( *t < 0.0f )
+ {
+ *t = 0.0f;
+ *s = vg_clampf( -c / a, 0.0f, 1.0f );
+ }
+ else if( *t > 1.0f )
+ {
+ *t = 1.0f;
+ *s = vg_clampf((b-c)/a,0.0f,1.0f);
+ }
+ }
+ }
+
+ v3_muladds( p1, d1, *s, c1 );
+ v3_muladds( p2, d2, *t, c2 );
+
+ v3f v0;
+ v3_sub( c1, c2, v0 );
+ return v3_length2( v0 );
+}
+
+static void closest_point_segment( v3f a, v3f b, v3f point, v3f dest )
+{
+ v3f v0, v1;
+ v3_sub( b, a, v0 );
+ v3_sub( point, a, v1 );
+
+ float t = v3_dot( v1, v0 ) / v3_length2(v0);
+ v3_muladds( a, v0, vg_clampf(t,0.0f,1.0f), dest );
+}
+
+/* Real-Time Collision Detection */
+static void closest_on_triangle( v3f p, v3f tri[3], v3f dest )
+{
+ v3f ab, ac, ap;
+ float d1, d2;
+
+ /* Region outside A */
+ v3_sub( tri[1], tri[0], ab );
+ v3_sub( tri[2], tri[0], ac );
+ v3_sub( p, tri[0], ap );
+
+ d1 = v3_dot(ab,ap);
+ d2 = v3_dot(ac,ap);
+ if( d1 <= 0.0f && d2 <= 0.0f )
+ {
+ v3_copy( tri[0], dest );
+ v3_copy( (v3f){INFINITY,INFINITY,INFINITY}, dest );
+ return;
+ }
+
+ /* Region outside B */
+ v3f bp;
+ float d3, d4;
+
+ v3_sub( p, tri[1], bp );
+ d3 = v3_dot( ab, bp );
+ d4 = v3_dot( ac, bp );
+
+ if( d3 >= 0.0f && d4 <= d3 )
+ {
+ v3_copy( tri[1], dest );
+ v3_copy( (v3f){INFINITY,INFINITY,INFINITY}, dest );
+ return;
+ }
+
+ /* Edge region of AB */
+ float vc = d1*d4 - d3*d2;
+ if( vc <= 0.0f && d1 >= 0.0f && d3 <= 0.0f )
+ {
+ float v = d1 / (d1-d3);
+ v3_muladds( tri[0], ab, v, dest );
+ v3_copy( (v3f){INFINITY,INFINITY,INFINITY}, dest );
+ return;
+ }
+
+ /* Region outside C */
+ v3f cp;
+ float d5, d6;
+ v3_sub( p, tri[2], cp );
+ d5 = v3_dot(ab, cp);
+ d6 = v3_dot(ac, cp);
+
+ if( d6 >= 0.0f && d5 <= d6 )
+ {
+ v3_copy( tri[2], dest );
+ v3_copy( (v3f){INFINITY,INFINITY,INFINITY}, dest );
+ return;
+ }
+
+ /* Region of AC */
+ float vb = d5*d2 - d1*d6;
+ if( vb <= 0.0f && d2 >= 0.0f && d6 <= 0.0f )
+ {
+ float w = d2 / (d2-d6);
+ v3_muladds( tri[0], ac, w, dest );
+ v3_copy( (v3f){INFINITY,INFINITY,INFINITY}, dest );
+ return;
+ }
+
+ /* Region of BC */
+ float va = d3*d6 - d5*d4;
+ if( va <= 0.0f && (d4-d3) >= 0.0f && (d5-d6) >= 0.0f )
+ {
+ float w = (d4-d3) / ((d4-d3) + (d5-d6));
+ v3f bc;
+ v3_sub( tri[2], tri[1], bc );
+ v3_muladds( tri[1], bc, w, dest );
+ v3_copy( (v3f){INFINITY,INFINITY,INFINITY}, dest );
+ return;
+ }
+
+ /* P inside region, Q via barycentric coordinates uvw */
+ float d = 1.0f/(va+vb+vc),
+ v = vb*d,
+ w = vc*d;
+
+ v3_muladds( tri[0], ab, v, dest );
+ v3_muladds( dest, ac, w, dest );
+}
+
+static int sphere_vs_triangle( v3f c, float r, v3f tri[3],
+ v3f co, v3f norm, float *p )
+{
+ v3f delta;
+ closest_on_triangle( c, tri, co );
+
+ v3_sub( c, co, delta );
+
+
+ float d = v3_length2( delta );
+ if( d < r*r )
+ {
+ v3f ab, ac, tn;
+ v3_sub( tri[1], tri[0], ab );
+ v3_sub( tri[2], tri[0], ac );
+ v3_cross( ac, ab, tn );
+
+ if( v3_dot( delta, tn ) > 0.0f )
+ v3_muls( delta, -1.0f, delta );
+
+ vg_line_pt3( co, 0.05f, 0xff00ff00 );
+
+ d = sqrtf(d);
+ v3_muls( delta, 1.0f/d, norm );
+
+ *p = r-d;
+ return 1;
+ }
+
+ return 0;
+}
+
+static void debug_capsule( m4x3f m, float height, float radius, u32 colour )
+{
+ v3f last = { 0.0f, 0.0f, radius };
+ m4x3f lower, upper;
+ m3x3_copy( m, lower );
+ m3x3_copy( m, upper );
+ m4x3_mulv( m, (v3f){0.0f,-height*0.5f+radius,0.0f}, lower[3] );
+ m4x3_mulv( m, (v3f){0.0f, height*0.5f-radius,0.0f}, upper[3] );
+
+ for( int i=0; i<16; i++ )
+ {
+ float t = ((float)(i+1) * (1.0f/16.0f)) * VG_PIf * 2.0f,
+ s = sinf(t),
+ c = cosf(t);
+
+ v3f p = { s*radius, 0.0f, c*radius };
+
+ v3f p0, p1;
+ m4x3_mulv( lower, p, p0 );
+ m4x3_mulv( lower, last, p1 );
+ vg_line( p0, p1, colour );
+
+ m4x3_mulv( upper, p, p0 );
+ m4x3_mulv( upper, last, p1 );
+ vg_line( p0, p1, colour );
+
+ v3_copy( p, last );
+ }
+
+ for( int i=0; i<4; i++ )
+ {
+ float t = ((float)(i) * (1.0f/4.0f)) * VG_PIf * 2.0f,
+ s = sinf(t),
+ c = cosf(t);
+
+ v3f p = { s*radius, 0.0f, c*radius };
+
+ v3f p0, p1;
+ m4x3_mulv( lower, p, p0 );
+ m4x3_mulv( upper, p, p1 );
+ vg_line( p0, p1, colour );
+
+ m4x3_mulv( lower, (v3f){0.0f,-radius,0.0f}, p0 );
+ m4x3_mulv( upper, (v3f){0.0f, radius,0.0f}, p1 );
+ vg_line( p0, p1, colour );
+ }
+}
+
/*
* BVH implementation, this is ONLY for static rigidbodies, its to slow for
* realtime use.
projects / carveJwlIkooP6JGAAIwe30JlM.git / commitdiff