+++ /dev/null
-#ifndef VEHICLE_C
-#define VEHICLE_C
-
-#include "vehicle.h"
-#include "scene_rigidbody.h"
-
-static int spawn_car( int argc, const char *argv[] ){
- v3f ra, rb, rx;
- v3_copy( skaterift.cam.pos, ra );
- v3_muladds( ra, skaterift.cam.transform[2], -10.0f, rb );
-
- float t;
- if( spherecast_world( world_current_instance(),
- ra, rb, 1.0f, &t, rx, 0 ) != -1 )
- {
- v3_lerp( ra, rb, t, gzoomer.rb.co );
- gzoomer.rb.co[1] += 4.0f;
- q_axis_angle( gzoomer.rb.q, (v3f){1.0f,0.0f,0.0f}, 0.001f );
- v3_zero( gzoomer.rb.v );
- v3_zero( gzoomer.rb.w );
-
- rb_update_matrices( &gzoomer.rb );
- gzoomer.alive = 1;
-
- vg_success( "Spawned car\n" );
- }
- else{
- vg_error( "Can't spawn here\n" );
- }
-
- return 0;
-}
-
-static void vehicle_init(void){
- q_identity( gzoomer.rb.q );
- v3_zero( gzoomer.rb.w );
- v3_zero( gzoomer.rb.v );
- v3_zero( gzoomer.rb.co );
- rb_setbody_sphere( &gzoomer.rb, 1.0f, 8.0f, 1.0f );
-
- VG_VAR_F32( k_car_spring, flags=VG_VAR_PERSISTENT );
- VG_VAR_F32( k_car_spring_damp, flags=VG_VAR_PERSISTENT );
- VG_VAR_F32( k_car_spring_length, flags=VG_VAR_PERSISTENT );
- VG_VAR_F32( k_car_wheel_radius, flags=VG_VAR_PERSISTENT );
- VG_VAR_F32( k_car_friction_lat, flags=VG_VAR_PERSISTENT );
- VG_VAR_F32( k_car_friction_roll, flags=VG_VAR_PERSISTENT );
- VG_VAR_F32( k_car_drive_force, flags=VG_VAR_PERSISTENT );
- VG_VAR_F32( k_car_air_resistance,flags=VG_VAR_PERSISTENT );
- VG_VAR_F32( k_car_downforce, flags=VG_VAR_PERSISTENT );
-
- VG_VAR_I32( gzoomer.inside );
-
- vg_console_reg_cmd( "spawn_car", spawn_car, NULL );
-
- v3_copy((v3f){ -1.0f, -0.25f, -1.5f }, gzoomer.wheels_local[0] );
- v3_copy((v3f){ 1.0f, -0.25f, -1.5f }, gzoomer.wheels_local[1] );
- v3_copy((v3f){ -1.0f, -0.25f, 1.5f }, gzoomer.wheels_local[2] );
- v3_copy((v3f){ 1.0f, -0.25f, 1.5f }, gzoomer.wheels_local[3] );
-}
-
-static void vehicle_wheel_force( int index ){
- v3f pa, pb, n;
- m4x3_mulv( gzoomer.rb.to_world, gzoomer.wheels_local[index], pa );
- v3_muladds( pa, gzoomer.rb.to_world[1], -k_car_spring_length, pb );
-
-
-#if 1
- float t;
- if( spherecast_world( world_current_instance(), pa, pb,
- k_car_wheel_radius, &t, n, 0 ) == -1 )
- { t = 1.0f;
- }
-
-#else
-
- v3f dir;
- v3_muls( gzoomer.rb.up, -1.0f, dir );
-
- ray_hit hit;
- hit.dist = k_car_spring_length;
- ray_world( pa, dir, &hit );
-
- float t = hit.dist / k_car_spring_length;
-
-#endif
-
- v3f pc;
- v3_lerp( pa, pb, t, pc );
-
- m4x3f mtx;
- m3x3_copy( gzoomer.rb.to_world, mtx );
- v3_copy( pc, mtx[3] );
- vg_line_sphere( mtx, k_car_wheel_radius, VG__BLACK );
- vg_line( pa, pc, VG__WHITE );
- v3_copy( pc, gzoomer.wheels[index] );
-
- if( t < 1.0f ){
- /* spring force */
- float Fv = (1.0f-t) * k_car_spring*vg.time_fixed_delta;
-
- v3f delta;
- v3_sub( pa, gzoomer.rb.co, delta );
-
- v3f rv;
- v3_cross( gzoomer.rb.w, delta, rv );
- v3_add( gzoomer.rb.v, rv, rv );
-
- Fv += v3_dot(rv, gzoomer.rb.to_world[1])
- * -k_car_spring_damp*vg.time_fixed_delta;
-
- /* scale by normal incident */
- Fv *= v3_dot( n, gzoomer.rb.to_world[1] );
-
- v3f F;
- v3_muls( gzoomer.rb.to_world[1], Fv, F );
- rb_linear_impulse( &gzoomer.rb, delta, F );
-
- /* friction vectors
- * -------------------------------------------------------------*/
- v3f tx, ty;
-
- if( index <= 1 )
- v3_cross( gzoomer.steerv, n, tx );
- else
- v3_cross( n, gzoomer.rb.to_world[2], tx );
- v3_cross( tx, n, ty );
-
- v3_copy( tx, gzoomer.tangent_vectors[ index ][0] );
- v3_copy( ty, gzoomer.tangent_vectors[ index ][1] );
-
- gzoomer.normal_forces[ index ] = Fv;
- gzoomer.tangent_forces[ index ][0] = 0.0f;
- gzoomer.tangent_forces[ index ][1] = 0.0f;
-
- /* orient inverse inertia tensors */
- v3f raW;
- m3x3_mulv( gzoomer.rb.to_world, gzoomer.wheels_local[index], raW );
-
- v3f raCtx, raCtxI, raCty, raCtyI;
- v3_cross( tx, raW, raCtx );
- v3_cross( ty, raW, raCty );
- m3x3_mulv( gzoomer.rb.iIw, raCtx, raCtxI );
- m3x3_mulv( gzoomer.rb.iIw, raCty, raCtyI );
-
- gzoomer.tangent_mass[index][0] = gzoomer.rb.inv_mass;
- gzoomer.tangent_mass[index][0] += v3_dot( raCtx, raCtxI );
- gzoomer.tangent_mass[index][0] = 1.0f/gzoomer.tangent_mass[index][0];
-
- gzoomer.tangent_mass[index][1] = gzoomer.rb.inv_mass;
- gzoomer.tangent_mass[index][1] += v3_dot( raCty, raCtyI );
- gzoomer.tangent_mass[index][1] = 1.0f/gzoomer.tangent_mass[index][1];
-
- /* apply drive force */
- if( index >= 2 ){
- v3_muls( ty, -gzoomer.drive * k_car_drive_force
- * vg.time_fixed_delta, F );
- rb_linear_impulse( &gzoomer.rb, raW, F );
- }
- }
- else{
- gzoomer.normal_forces[ index ] = 0.0f;
- gzoomer.tangent_forces[ index ][0] = 0.0f;
- gzoomer.tangent_forces[ index ][1] = 0.0f;
- }
-}
-
-static void vehicle_solve_friction(void){
- rigidbody *rb = &gzoomer.rb;
- for( int i=0; i<4; i++ ){
- v3f raW;
- m3x3_mulv( rb->to_world, gzoomer.wheels_local[i], raW );
-
- v3f rv;
- v3_cross( rb->w, raW, rv );
- v3_add( rb->v, rv, rv );
-
- float fx = k_car_friction_lat * gzoomer.normal_forces[i],
- fy = k_car_friction_roll * gzoomer.normal_forces[i],
- vtx = v3_dot( rv, gzoomer.tangent_vectors[i][0] ),
- vty = v3_dot( rv, gzoomer.tangent_vectors[i][1] ),
- lambdax = gzoomer.tangent_mass[i][0] * -vtx,
- lambday = gzoomer.tangent_mass[i][1] * -vty;
-
- float tempx = gzoomer.tangent_forces[i][0],
- tempy = gzoomer.tangent_forces[i][1];
- gzoomer.tangent_forces[i][0] = vg_clampf( tempx + lambdax, -fx, fx );
- gzoomer.tangent_forces[i][1] = vg_clampf( tempy + lambday, -fy, fy );
- lambdax = gzoomer.tangent_forces[i][0] - tempx;
- lambday = gzoomer.tangent_forces[i][1] - tempy;
-
- v3f impulsex, impulsey;
- v3_muls( gzoomer.tangent_vectors[i][0], lambdax, impulsex );
- v3_muls( gzoomer.tangent_vectors[i][1], lambday, impulsey );
- rb_linear_impulse( rb, raW, impulsex );
- rb_linear_impulse( rb, raW, impulsey );
- }
-}
-
-static void vehicle_update_fixed(void)
-{
- if( !gzoomer.alive )
- return;
-
- rigidbody *rb = &gzoomer.rb;
-
- v3_muls( rb->to_world[2], -cosf(gzoomer.steer), gzoomer.steerv );
- v3_muladds( gzoomer.steerv, rb->to_world[0],
- sinf(gzoomer.steer), gzoomer.steerv );
-
- /* apply air resistance */
- v3f Fair, Fdown;
-
- v3_muls( rb->v, -k_car_air_resistance, Fair );
- v3_muls( rb->to_world[1], -fabsf(v3_dot( rb->v, rb->to_world[2] )) *
- k_car_downforce, Fdown );
-
- v3_muladds( rb->v, Fair, vg.time_fixed_delta, rb->v );
- v3_muladds( rb->v, Fdown, vg.time_fixed_delta, rb->v );
-
- for( int i=0; i<4; i++ )
- vehicle_wheel_force( i );
-
- rigidbody _null = {0};
- _null.inv_mass = 0.0f;
- m3x3_zero( _null.iI );
-
- rb_ct manifold[64];
- int len = rb_sphere__scene( rb->to_world, 1.0f, NULL,
- world_current_instance()->geo_bh,
- manifold, 0 );
- for( int j=0; j<len; j++ ){
- manifold[j].rba = rb;
- manifold[j].rbb = &_null;
- }
- rb_manifold_filter_coplanar( manifold, len, 0.05f );
-
- if( len > 1 ){
- rb_manifold_filter_backface( manifold, len );
- rb_manifold_filter_joint_edges( manifold, len, 0.05f );
- rb_manifold_filter_pairs( manifold, len, 0.05f );
- }
- len = rb_manifold_apply_filtered( manifold, len );
-
- rb_presolve_contacts( manifold, vg.time_fixed_delta, len );
- for( int i=0; i<8; i++ ){
- rb_solve_contacts( manifold, len );
- vehicle_solve_friction();
- }
-
- rb_iter( rb );
- rb_update_matrices( rb );
-}
-
-static void vehicle_update_post(void){
- if( !gzoomer.alive )
- return;
-
- vg_line_sphere( gzoomer.rb.to_world, 1.0f, VG__WHITE );
-
- /* draw friction vectors */
- v3f p0, px, py;
-
- for( int i=0; i<4; i++ ){
- v3_copy( gzoomer.wheels[i], p0 );
- v3_muladds( p0, gzoomer.tangent_vectors[i][0], 0.5f, px );
- v3_muladds( p0, gzoomer.tangent_vectors[i][1], 0.5f, py );
-
- vg_line( p0, px, VG__RED );
- vg_line( p0, py, VG__GREEN );
- }
-}
-
-#endif /* VEHICLE_H */
projects / carveJwlIkooP6JGAAIwe30JlM.git / blobdiff