vehicle.h

   1 #ifndef VEHICLE_H
   2 #define VEHICLE_H
   3
   4 #define VG_GAME
   5 #include "vg/vg.h"
   6 #include "rigidbody.h"
   7 #include "world.h"
   8 #include "player_physics.h"
   9
  10 VG_STATIC float k_car_spring = 1.0f,
  11                 k_car_spring_damp = 0.001f,
  12                 k_car_spring_length = 0.5f,
  13                 k_car_wheel_radius = 0.2f,
  14                 k_car_friction_lat = 0.6f,
  15                 k_car_friction_roll = 0.01f,
  16                 k_car_drive_force = 1.0f,
  17                 k_car_air_resistance = 0.1f;
  18
  19 VG_STATIC struct gvehicle
  20 {
  21    int alive, inside;
  22    rigidbody rb;
  23
  24    v3f wheels[4];
  25
  26    float tangent_mass[4][2],
  27          normal_forces[4],
  28          tangent_forces[4][2];
  29
  30    float steer, drive;
  31    v3f steerv;
  32
  33    v3f   tangent_vectors[4][2];
  34    v3f   wheels_local[4];
  35 }
  36 gzoomer =
  37 {
  38    .rb = { .type = k_rb_shape_sphere, .inf.sphere.radius = 1.0f }
  39 };
  40
  41 VG_STATIC int spawn_car( int argc, const char *argv[] )
  42 {
  43    v3f ra, rb, rx;
  44    v3_copy( main_camera.pos, ra );
  45    v3_muladds( ra, main_camera.transform[2], -10.0f, rb );
  46
  47    float t;
  48    if( spherecast_world( ra, rb, gzoomer.rb.inf.sphere.radius, &t, rx ) != -1 )
  49    {
  50       v3_lerp( ra, rb, t, gzoomer.rb.co );
  51       gzoomer.rb.co[1] += 4.0f;
  52       q_axis_angle( gzoomer.rb.q, (v3f){1.0f,0.0f,0.0f}, 0.001f );
  53       v3_zero( gzoomer.rb.v );
  54       v3_zero( gzoomer.rb.w );
  55
  56       rb_update_transform( &gzoomer.rb );
  57       gzoomer.alive = 1;
  58
  59       vg_success( "Spawned car\n" );
  60    }
  61    else
  62    {
  63       vg_error( "Can't spawn here\n" );
  64    }
  65
  66    return 0;
  67 }
  68
  69 VG_STATIC void vehicle_init(void)
  70 {
  71    q_identity( gzoomer.rb.q );
  72    rb_init( &gzoomer.rb );
  73
  74    VG_VAR_F32_PERSISTENT( k_car_spring );
  75    VG_VAR_F32_PERSISTENT( k_car_spring_damp );
  76    VG_VAR_F32_PERSISTENT( k_car_spring_length );
  77    VG_VAR_F32_PERSISTENT( k_car_wheel_radius );
  78    VG_VAR_F32_PERSISTENT( k_car_friction_lat );
  79    VG_VAR_F32_PERSISTENT( k_car_friction_roll );
  80    VG_VAR_F32_PERSISTENT( k_car_drive_force );
  81    VG_VAR_F32_PERSISTENT( k_car_air_resistance );
  82
  83         vg_function_push( (struct vg_cmd){
  84                 .name = "spawn_car",
  85                 .function = spawn_car
  86         });
  87
  88    v3_copy((v3f){ -1.0f, -0.25f, -1.0f }, gzoomer.wheels_local[0] );
  89    v3_copy((v3f){  1.0f, -0.25f, -1.0f }, gzoomer.wheels_local[1] );
  90    v3_copy((v3f){ -1.0f, -0.25f,  1.0f }, gzoomer.wheels_local[2] );
  91    v3_copy((v3f){  1.0f, -0.25f,  1.0f }, gzoomer.wheels_local[3] );
  92 }
  93
  94 VG_STATIC void vehicle_wheel_force( int index )
  95 {
  96    v3f pa, pb, n;
  97    m4x3_mulv( gzoomer.rb.to_world, gzoomer.wheels_local[index], pa );
  98    v3_muladds( pa, gzoomer.rb.up, -k_car_spring_length, pb );
  99
 100
 101 #if 1
 102    float t;
 103    if( spherecast_world( pa, pb, k_car_wheel_radius, &t, n ) == -1 )
 104       t = 1.0f;
 105
 106 #else
 107
 108    v3f dir;
 109    v3_muls( gzoomer.rb.up, -1.0f, dir );
 110
 111    ray_hit hit;
 112    hit.dist = k_car_spring_length;
 113    ray_world( pa, dir, &hit );
 114
 115    float t = hit.dist / k_car_spring_length;
 116
 117 #endif
 118
 119    v3f pc;
 120    v3_lerp( pa, pb, t, pc );
 121
 122    m4x3f mtx;
 123    m3x3_copy( gzoomer.rb.to_world, mtx );
 124    v3_copy( pc, mtx[3] );
 125    debug_sphere( mtx, k_car_wheel_radius, VG__BLACK );
 126    vg_line( pa, pc, VG__WHITE );
 127    v3_copy( pc, gzoomer.wheels[index] );
 128
 129    if( t < 1.0f )
 130    {
 131       /* spring force */
 132       float Fv = (1.0f-t) * k_car_spring*k_rb_delta;
 133
 134       v3f delta;
 135       v3_sub( pa, gzoomer.rb.co, delta );
 136
 137       v3f rv;
 138       v3_cross( gzoomer.rb.w, delta, rv );
 139       v3_add( gzoomer.rb.v, rv, rv );
 140
 141       Fv += v3_dot( rv, gzoomer.rb.up ) * -k_car_spring_damp*k_rb_delta;
 142
 143       /* scale by normal incident */
 144       Fv *= v3_dot( n, gzoomer.rb.up );
 145
 146       v3f F;
 147       v3_muls( gzoomer.rb.up, Fv, F );
 148
 149       rb_linear_impulse( &gzoomer.rb, delta, F );
 150
 151       /* friction vectors
 152        * -------------------------------------------------------------*/
 153       v3f tx, ty;
 154
 155       if( index <= 1 )
 156          v3_cross( gzoomer.steerv, n, tx );
 157       else
 158          v3_cross( gzoomer.rb.forward, n, tx );
 159       v3_cross( tx, n, ty );
 160
 161       v3_copy( tx, gzoomer.tangent_vectors[ index ][0] );
 162       v3_copy( ty, gzoomer.tangent_vectors[ index ][1] );
 163
 164       gzoomer.normal_forces[ index ] = Fv;
 165       gzoomer.tangent_forces[ index ][0] = 0.0f;
 166       gzoomer.tangent_forces[ index ][1] = 0.0f;
 167
 168       /* orient inverse inertia tensors */
 169       v3f raW;
 170       m3x3_mulv( gzoomer.rb.to_world, gzoomer.wheels_local[index], raW );
 171
 172       v3f raCtx, raCtxI, raCty, raCtyI;
 173       v3_cross( tx, raW, raCtx );
 174       v3_cross( ty, raW, raCty );
 175       m3x3_mulv( gzoomer.rb.iIw, raCtx, raCtxI );
 176       m3x3_mulv( gzoomer.rb.iIw, raCty, raCtyI );
 177
 178       gzoomer.tangent_mass[index][0] = gzoomer.rb.inv_mass;
 179       gzoomer.tangent_mass[index][0] += v3_dot( raCtx, raCtxI );
 180       gzoomer.tangent_mass[index][0] = 1.0f/gzoomer.tangent_mass[index][0];
 181
 182       gzoomer.tangent_mass[index][1] = gzoomer.rb.inv_mass;
 183       gzoomer.tangent_mass[index][1] += v3_dot( raCty, raCtyI );
 184       gzoomer.tangent_mass[index][1] = 1.0f/gzoomer.tangent_mass[index][1];
 185
 186       /* apply drive force */
 187       if( index >= 2 )
 188       {
 189          v3_muls( ty, gzoomer.drive * k_car_drive_force * k_rb_delta, F );
 190          rb_linear_impulse( &gzoomer.rb, raW, F );
 191       }
 192    }
 193    else
 194    {
 195       gzoomer.normal_forces[ index ] = 0.0f;
 196       gzoomer.tangent_forces[ index ][0] = 0.0f;
 197       gzoomer.tangent_forces[ index ][1] = 0.0f;
 198    }
 199 }
 200
 201 VG_STATIC void vehicle_solve_friction(void)
 202 {
 203    for( int i=0; i<4; i++ )
 204    {
 205       v3f raW;
 206       m3x3_mulv( gzoomer.rb.to_world, gzoomer.wheels_local[i], raW );
 207
 208       v3f rv;
 209       v3_cross( gzoomer.rb.w, raW, rv );
 210       v3_add( gzoomer.rb.v, rv, rv );
 211
 212       float     fx = k_car_friction_lat * gzoomer.normal_forces[i],
 213                 fy = k_car_friction_roll * gzoomer.normal_forces[i],
 214                vtx = v3_dot( rv, gzoomer.tangent_vectors[i][0] ),
 215                vty = v3_dot( rv, gzoomer.tangent_vectors[i][1] ),
 216            lambdax = gzoomer.tangent_mass[i][0] * -vtx,
 217            lambday = gzoomer.tangent_mass[i][1] * -vty;
 218
 219       float tempx = gzoomer.tangent_forces[i][0],
 220             tempy = gzoomer.tangent_forces[i][1];
 221       gzoomer.tangent_forces[i][0] = vg_clampf( tempx + lambdax, -fx, fx );
 222       gzoomer.tangent_forces[i][1] = vg_clampf( tempy + lambday, -fy, fy );
 223       lambdax = gzoomer.tangent_forces[i][0] - tempx;
 224       lambday = gzoomer.tangent_forces[i][1] - tempy;
 225
 226       v3f impulsex, impulsey;
 227       v3_muls( gzoomer.tangent_vectors[i][0], lambdax, impulsex );
 228       v3_muls( gzoomer.tangent_vectors[i][1], lambday, impulsey );
 229       rb_linear_impulse( &gzoomer.rb, raW, impulsex );
 230       rb_linear_impulse( &gzoomer.rb, raW, impulsey );
 231    }
 232 }
 233
 234 VG_STATIC void vehicle_update_fixed(void)
 235 {
 236    if( !gzoomer.alive )
 237       return;
 238
 239    gzoomer.steer = vg_lerpf( gzoomer.steer,
 240                              player.input_walkh->axis.value * 0.2f,
 241                              k_rb_delta * 8.0f );
 242
 243    gzoomer.drive = player.input_walkv->axis.value * k_car_drive_force;
 244    v3_muls( gzoomer.rb.forward, cosf(gzoomer.steer), gzoomer.steerv );
 245    v3_muladds( gzoomer.steerv, gzoomer.rb.right,
 246                sinf(gzoomer.steer), gzoomer.steerv );
 247
 248    /* apply air resistance */
 249    v3_muladds( gzoomer.rb.v, gzoomer.rb.v,
 250                -k_car_air_resistance * k_rb_delta, gzoomer.rb.v );
 251
 252    for( int i=0; i<4; i++ )
 253       vehicle_wheel_force( i );
 254
 255    rb_ct manifold[64];
 256
 257    int len = rb_sphere_scene( &gzoomer.rb, &world.rb_geo, manifold );
 258    rb_manifold_filter_coplanar( manifold, len, 0.05f );
 259
 260    if( len > 1 )
 261    {
 262       rb_manifold_filter_backface( manifold, len );
 263       rb_manifold_filter_joint_edges( manifold, len, 0.05f );
 264       rb_manifold_filter_pairs( manifold, len, 0.05f );
 265    }
 266    len = rb_manifold_apply_filtered( manifold, len );
 267
 268    rb_presolve_contacts( manifold, len );
 269    for( int i=0; i<8; i++ )
 270    {
 271       rb_solve_contacts( manifold, len );
 272       vehicle_solve_friction();
 273    }
 274
 275    rb_iter( &gzoomer.rb );
 276    rb_update_transform( &gzoomer.rb );
 277 }
 278
 279 VG_STATIC void vehicle_update_post(void)
 280 {
 281    if( !gzoomer.alive )
 282       return;
 283
 284    rb_debug( &gzoomer.rb, VG__WHITE );
 285    vg_line( player.phys.rb.co, gzoomer.rb.co, VG__WHITE );
 286
 287    /* draw friction vectors */
 288    v3f p0, px, py;
 289
 290    for( int i=0; i<4; i++ )
 291    {
 292       v3_copy( gzoomer.wheels[i], p0 );
 293       v3_muladds( p0, gzoomer.tangent_vectors[i][0], 0.5f, px );
 294       v3_muladds( p0, gzoomer.tangent_vectors[i][1], 0.5f, py );
 295
 296       vg_line( p0, px, VG__RED );
 297       vg_line( p0, py, VG__GREEN );
 298    }
 299 }
 300
 301 #endif /* VEHICLE_H */