vehicle.c

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