--- /dev/null
+#ifndef VEHICLE_H
+#define VEHICLE_H
+
+#define VG_GAME
+#include "vg/vg.h"
+#include "rigidbody.h"
+#include "world.h"
+#include "player_physics.h"
+
+VG_STATIC float k_car_spring = 1.0f,
+ k_car_spring_damp = 0.001f,
+ k_car_spring_length = 0.5f,
+ k_car_wheel_radius = 0.2f,
+ k_car_friction_lat = 0.6f,
+ k_car_friction_roll = 0.01f,
+ k_car_drive_force = 1.0f,
+ k_car_air_resistance = 0.1f;
+
+VG_STATIC struct gvehicle
+{
+ int alive, inside;
+ rigidbody rb;
+
+ v3f wheels[4];
+
+ float tangent_mass[4][2],
+ normal_forces[4],
+ tangent_forces[4][2];
+
+ float steer, drive;
+ v3f steerv;
+
+ v3f tangent_vectors[4][2];
+ v3f wheels_local[4];
+}
+gzoomer =
+{
+ .rb = { .type = k_rb_shape_sphere, .inf.sphere.radius = 1.0f }
+};
+
+VG_STATIC int spawn_car( int argc, const char *argv[] )
+{
+ v3f ra, rb, rx;
+ v3_copy( main_camera.pos, ra );
+ v3_muladds( ra, main_camera.transform[2], -10.0f, rb );
+
+ float t;
+ if( spherecast_world( ra, rb, gzoomer.rb.inf.sphere.radius, &t, rx ) != -1 )
+ {
+ v3_lerp( ra, rb, t, gzoomer.rb.co );
+ gzoomer.rb.co[1] += 4.0f;
+ q_axis_angle( gzoomer.rb.q, (v3f){1.0f,0.0f,0.0f}, 0.001f );
+ v3_zero( gzoomer.rb.v );
+ v3_zero( gzoomer.rb.w );
+
+ rb_update_transform( &gzoomer.rb );
+ gzoomer.alive = 1;
+
+ vg_success( "Spawned car\n" );
+ }
+ else
+ {
+ vg_error( "Can't spawn here\n" );
+ }
+
+ return 0;
+}
+
+VG_STATIC void vehicle_init(void)
+{
+ q_identity( gzoomer.rb.q );
+ rb_init( &gzoomer.rb );
+
+ VG_VAR_F32_PERSISTENT( k_car_spring );
+ VG_VAR_F32_PERSISTENT( k_car_spring_damp );
+ VG_VAR_F32_PERSISTENT( k_car_spring_length );
+ VG_VAR_F32_PERSISTENT( k_car_wheel_radius );
+ VG_VAR_F32_PERSISTENT( k_car_friction_lat );
+ VG_VAR_F32_PERSISTENT( k_car_friction_roll );
+ VG_VAR_F32_PERSISTENT( k_car_drive_force );
+ VG_VAR_F32_PERSISTENT( k_car_air_resistance );
+
+ vg_function_push( (struct vg_cmd){
+ .name = "spawn_car",
+ .function = spawn_car
+ });
+
+ v3_copy((v3f){ -1.0f, -0.25f, -1.0f }, gzoomer.wheels_local[0] );
+ v3_copy((v3f){ 1.0f, -0.25f, -1.0f }, gzoomer.wheels_local[1] );
+ v3_copy((v3f){ -1.0f, -0.25f, 1.0f }, gzoomer.wheels_local[2] );
+ v3_copy((v3f){ 1.0f, -0.25f, 1.0f }, gzoomer.wheels_local[3] );
+}
+
+VG_STATIC void vehicle_wheel_force( int index )
+{
+ v3f pa, pb, n;
+ m4x3_mulv( gzoomer.rb.to_world, gzoomer.wheels_local[index], pa );
+ v3_muladds( pa, gzoomer.rb.up, -k_car_spring_length, pb );
+
+
+#if 1
+ float t;
+ if( spherecast_world( pa, pb, k_car_wheel_radius, &t, n ) == -1 )
+ t = 1.0f;
+
+#else
+
+ v3f dir;
+ v3_muls( gzoomer.rb.up, -1.0f, dir );
+
+ ray_hit hit;
+ hit.dist = k_car_spring_length;
+ ray_world( pa, dir, &hit );
+
+ float t = hit.dist / k_car_spring_length;
+
+#endif
+
+ v3f pc;
+ v3_lerp( pa, pb, t, pc );
+
+ m4x3f mtx;
+ m3x3_copy( gzoomer.rb.to_world, mtx );
+ v3_copy( pc, mtx[3] );
+ debug_sphere( mtx, k_car_wheel_radius, VG__BLACK );
+ vg_line( pa, pc, VG__WHITE );
+ v3_copy( pc, gzoomer.wheels[index] );
+
+ if( t < 1.0f )
+ {
+ /* spring force */
+ float Fv = (1.0f-t) * k_car_spring*k_rb_delta;
+
+ v3f delta;
+ v3_sub( pa, gzoomer.rb.co, delta );
+
+ v3f rv;
+ v3_cross( gzoomer.rb.w, delta, rv );
+ v3_add( gzoomer.rb.v, rv, rv );
+
+ Fv += v3_dot( rv, gzoomer.rb.up ) * -k_car_spring_damp*k_rb_delta;
+
+ /* scale by normal incident */
+ Fv *= v3_dot( n, gzoomer.rb.up );
+
+ v3f F;
+ v3_muls( gzoomer.rb.up, Fv, F );
+
+ rb_linear_impulse( &gzoomer.rb, delta, F );
+
+ /* friction vectors
+ * -------------------------------------------------------------*/
+ v3f tx, ty;
+
+ if( index <= 1 )
+ v3_cross( gzoomer.steerv, n, tx );
+ else
+ v3_cross( gzoomer.rb.forward, n, tx );
+ v3_cross( tx, n, ty );
+
+ v3_copy( tx, gzoomer.tangent_vectors[ index ][0] );
+ v3_copy( ty, gzoomer.tangent_vectors[ index ][1] );
+
+ gzoomer.normal_forces[ index ] = Fv;
+ gzoomer.tangent_forces[ index ][0] = 0.0f;
+ gzoomer.tangent_forces[ index ][1] = 0.0f;
+
+ /* orient inverse inertia tensors */
+ v3f raW;
+ m3x3_mulv( gzoomer.rb.to_world, gzoomer.wheels_local[index], raW );
+
+ v3f raCtx, raCtxI, raCty, raCtyI;
+ v3_cross( tx, raW, raCtx );
+ v3_cross( ty, raW, raCty );
+ m3x3_mulv( gzoomer.rb.iIw, raCtx, raCtxI );
+ m3x3_mulv( gzoomer.rb.iIw, raCty, raCtyI );
+
+ gzoomer.tangent_mass[index][0] = gzoomer.rb.inv_mass;
+ gzoomer.tangent_mass[index][0] += v3_dot( raCtx, raCtxI );
+ gzoomer.tangent_mass[index][0] = 1.0f/gzoomer.tangent_mass[index][0];
+
+ gzoomer.tangent_mass[index][1] = gzoomer.rb.inv_mass;
+ gzoomer.tangent_mass[index][1] += v3_dot( raCty, raCtyI );
+ gzoomer.tangent_mass[index][1] = 1.0f/gzoomer.tangent_mass[index][1];
+
+ /* apply drive force */
+ if( index >= 2 )
+ {
+ v3_muls( ty, gzoomer.drive * k_car_drive_force * k_rb_delta, F );
+ rb_linear_impulse( &gzoomer.rb, raW, F );
+ }
+ }
+ else
+ {
+ gzoomer.normal_forces[ index ] = 0.0f;
+ gzoomer.tangent_forces[ index ][0] = 0.0f;
+ gzoomer.tangent_forces[ index ][1] = 0.0f;
+ }
+}
+
+VG_STATIC void vehicle_solve_friction(void)
+{
+ for( int i=0; i<4; i++ )
+ {
+ v3f raW;
+ m3x3_mulv( gzoomer.rb.to_world, gzoomer.wheels_local[i], raW );
+
+ v3f rv;
+ v3_cross( gzoomer.rb.w, raW, rv );
+ v3_add( gzoomer.rb.v, rv, rv );
+
+ float fx = k_car_friction_lat * gzoomer.normal_forces[i],
+ fy = k_car_friction_roll * gzoomer.normal_forces[i],
+ vtx = v3_dot( rv, gzoomer.tangent_vectors[i][0] ),
+ vty = v3_dot( rv, gzoomer.tangent_vectors[i][1] ),
+ lambdax = gzoomer.tangent_mass[i][0] * -vtx,
+ lambday = gzoomer.tangent_mass[i][1] * -vty;
+
+ float tempx = gzoomer.tangent_forces[i][0],
+ tempy = gzoomer.tangent_forces[i][1];
+ gzoomer.tangent_forces[i][0] = vg_clampf( tempx + lambdax, -fx, fx );
+ gzoomer.tangent_forces[i][1] = vg_clampf( tempy + lambday, -fy, fy );
+ lambdax = gzoomer.tangent_forces[i][0] - tempx;
+ lambday = gzoomer.tangent_forces[i][1] - tempy;
+
+ v3f impulsex, impulsey;
+ v3_muls( gzoomer.tangent_vectors[i][0], lambdax, impulsex );
+ v3_muls( gzoomer.tangent_vectors[i][1], lambday, impulsey );
+ rb_linear_impulse( &gzoomer.rb, raW, impulsex );
+ rb_linear_impulse( &gzoomer.rb, raW, impulsey );
+ }
+}
+
+VG_STATIC void vehicle_update_fixed(void)
+{
+ if( !gzoomer.alive )
+ return;
+
+ gzoomer.steer = vg_lerpf( gzoomer.steer,
+ player.input_walkh->axis.value * 0.2f,
+ k_rb_delta * 8.0f );
+
+ gzoomer.drive = player.input_walkv->axis.value * k_car_drive_force;
+ v3_muls( gzoomer.rb.forward, cosf(gzoomer.steer), gzoomer.steerv );
+ v3_muladds( gzoomer.steerv, gzoomer.rb.right,
+ sinf(gzoomer.steer), gzoomer.steerv );
+
+ /* apply air resistance */
+ v3_muladds( gzoomer.rb.v, gzoomer.rb.v,
+ -k_car_air_resistance * k_rb_delta, gzoomer.rb.v );
+
+ for( int i=0; i<4; i++ )
+ vehicle_wheel_force( i );
+
+ rb_ct manifold[64];
+
+ int len = rb_sphere_scene( &gzoomer.rb, &world.rb_geo, manifold );
+ rb_manifold_filter_coplanar( manifold, len, 0.05f );
+
+ if( len > 1 )
+ {
+ rb_manifold_filter_backface( manifold, len );
+ rb_manifold_filter_joint_edges( manifold, len, 0.05f );
+ rb_manifold_filter_pairs( manifold, len, 0.05f );
+ }
+ len = rb_manifold_apply_filtered( manifold, len );
+
+ rb_presolve_contacts( manifold, len );
+ for( int i=0; i<8; i++ )
+ {
+ rb_solve_contacts( manifold, len );
+ vehicle_solve_friction();
+ }
+
+ rb_iter( &gzoomer.rb );
+ rb_update_transform( &gzoomer.rb );
+}
+
+VG_STATIC void vehicle_update_post(void)
+{
+ if( !gzoomer.alive )
+ return;
+
+ rb_debug( &gzoomer.rb, VG__WHITE );
+ vg_line( player.phys.rb.co, gzoomer.rb.co, VG__WHITE );
+
+ /* draw friction vectors */
+ v3f p0, px, py;
+
+ for( int i=0; i<4; i++ )
+ {
+ v3_copy( gzoomer.wheels[i], p0 );
+ v3_muladds( p0, gzoomer.tangent_vectors[i][0], 0.5f, px );
+ v3_muladds( p0, gzoomer.tangent_vectors[i][1], 0.5f, py );
+
+ vg_line( p0, px, VG__RED );
+ vg_line( p0, py, VG__GREEN );
+ }
+}
+
+#endif /* VEHICLE_H */
projects / carveJwlIkooP6JGAAIwe30JlM.git / commitdiff