option between water plane / water trigger
[carveJwlIkooP6JGAAIwe30JlM.git] / 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 }