vehicle.h

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