vehicle.c

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