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