vehicle.c

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