10 VG_STATIC
float k_car_spring
= 1.0f
,
11 k_car_spring_damp
= 0.001f
,
12 k_car_spring_length
= 0.5f
,
13 k_car_wheel_radius
= 0.2f
,
14 k_car_friction_lat
= 0.6f
,
15 k_car_friction_roll
= 0.01f
,
16 k_car_drive_force
= 1.0f
,
17 k_car_air_resistance
= 0.1f
,
18 k_car_downforce
= 0.5f
;
20 typedef struct drivable_vehicle drivable_vehicle
;
21 VG_STATIC
struct drivable_vehicle
28 float tangent_mass
[4][2],
35 v3f tangent_vectors
[4][2];
40 .obj
= { .type
= k_rb_shape_sphere
, .inf
.sphere
.radius
= 1.0f
}
43 VG_STATIC
int spawn_car( int argc
, const char *argv
[] )
46 v3_copy( main_camera
.pos
, ra
);
47 v3_muladds( ra
, main_camera
.transform
[2], -10.0f
, rb
);
50 if( spherecast_world( get_active_world(), ra
, rb
,
51 gzoomer
.obj
.inf
.sphere
.radius
, &t
, rx
) != -1 )
53 v3_lerp( ra
, rb
, t
, gzoomer
.obj
.rb
.co
);
54 gzoomer
.obj
.rb
.co
[1] += 4.0f
;
55 q_axis_angle( gzoomer
.obj
.rb
.q
, (v3f
){1.0f
,0.0f
,0.0f
}, 0.001f
);
56 v3_zero( gzoomer
.obj
.rb
.v
);
57 v3_zero( gzoomer
.obj
.rb
.w
);
59 rb_update_transform( &gzoomer
.obj
.rb
);
62 vg_success( "Spawned car\n" );
65 vg_error( "Can't spawn here\n" );
71 VG_STATIC
void vehicle_init(void)
73 q_identity( gzoomer
.obj
.rb
.q
);
74 v3_zero( gzoomer
.obj
.rb
.w
);
75 v3_zero( gzoomer
.obj
.rb
.v
);
76 v3_zero( gzoomer
.obj
.rb
.co
);
77 rb_init_object( &gzoomer
.obj
);
79 VG_VAR_F32( k_car_spring
, flags
=VG_VAR_PERSISTENT
);
80 VG_VAR_F32( k_car_spring_damp
, flags
=VG_VAR_PERSISTENT
);
81 VG_VAR_F32( k_car_spring_length
, flags
=VG_VAR_PERSISTENT
);
82 VG_VAR_F32( k_car_wheel_radius
, flags
=VG_VAR_PERSISTENT
);
83 VG_VAR_F32( k_car_friction_lat
, flags
=VG_VAR_PERSISTENT
);
84 VG_VAR_F32( k_car_friction_roll
, flags
=VG_VAR_PERSISTENT
);
85 VG_VAR_F32( k_car_drive_force
, flags
=VG_VAR_PERSISTENT
);
86 VG_VAR_F32( k_car_air_resistance
,flags
=VG_VAR_PERSISTENT
);
87 VG_VAR_F32( k_car_downforce
, flags
=VG_VAR_PERSISTENT
);
89 VG_VAR_I32( gzoomer
.inside
);
91 vg_console_reg_cmd( "spawn_car", spawn_car
, NULL
);
93 v3_copy((v3f
){ -1.0f
, -0.25f
, -1.5f
}, gzoomer
.wheels_local
[0] );
94 v3_copy((v3f
){ 1.0f
, -0.25f
, -1.5f
}, gzoomer
.wheels_local
[1] );
95 v3_copy((v3f
){ -1.0f
, -0.25f
, 1.5f
}, gzoomer
.wheels_local
[2] );
96 v3_copy((v3f
){ 1.0f
, -0.25f
, 1.5f
}, gzoomer
.wheels_local
[3] );
99 VG_STATIC
void vehicle_wheel_force( int index
)
102 m4x3_mulv( gzoomer
.obj
.rb
.to_world
, gzoomer
.wheels_local
[index
], pa
);
103 v3_muladds( pa
, gzoomer
.obj
.rb
.to_world
[1], -k_car_spring_length
, pb
);
108 if( spherecast_world( get_active_world(), pa
, pb
,
109 k_car_wheel_radius
, &t
, n
) == -1 )
116 v3_muls( gzoomer
.rb
.up
, -1.0f
, dir
);
119 hit
.dist
= k_car_spring_length
;
120 ray_world( pa
, dir
, &hit
);
122 float t
= hit
.dist
/ k_car_spring_length
;
127 v3_lerp( pa
, pb
, t
, pc
);
130 m3x3_copy( gzoomer
.obj
.rb
.to_world
, mtx
);
131 v3_copy( pc
, mtx
[3] );
132 debug_sphere( mtx
, k_car_wheel_radius
, VG__BLACK
);
133 vg_line( pa
, pc
, VG__WHITE
);
134 v3_copy( pc
, gzoomer
.wheels
[index
] );
138 float Fv
= (1.0f
-t
) * k_car_spring
*k_rb_delta
;
141 v3_sub( pa
, gzoomer
.obj
.rb
.co
, delta
);
144 v3_cross( gzoomer
.obj
.rb
.w
, delta
, rv
);
145 v3_add( gzoomer
.obj
.rb
.v
, rv
, rv
);
147 Fv
+= v3_dot( rv
, gzoomer
.obj
.rb
.to_world
[1] )
148 * -k_car_spring_damp
*k_rb_delta
;
150 /* scale by normal incident */
151 Fv
*= v3_dot( n
, gzoomer
.obj
.rb
.to_world
[1] );
154 v3_muls( gzoomer
.obj
.rb
.to_world
[1], Fv
, F
);
156 rb_linear_impulse( &gzoomer
.obj
.rb
, delta
, F
);
159 * -------------------------------------------------------------*/
163 v3_cross( gzoomer
.steerv
, n
, tx
);
165 v3_cross( n
, gzoomer
.obj
.rb
.to_world
[2], tx
);
166 v3_cross( tx
, n
, ty
);
168 v3_copy( tx
, gzoomer
.tangent_vectors
[ index
][0] );
169 v3_copy( ty
, gzoomer
.tangent_vectors
[ index
][1] );
171 gzoomer
.normal_forces
[ index
] = Fv
;
172 gzoomer
.tangent_forces
[ index
][0] = 0.0f
;
173 gzoomer
.tangent_forces
[ index
][1] = 0.0f
;
175 /* orient inverse inertia tensors */
177 m3x3_mulv( gzoomer
.obj
.rb
.to_world
, gzoomer
.wheels_local
[index
], raW
);
179 v3f raCtx
, raCtxI
, raCty
, raCtyI
;
180 v3_cross( tx
, raW
, raCtx
);
181 v3_cross( ty
, raW
, raCty
);
182 m3x3_mulv( gzoomer
.obj
.rb
.iIw
, raCtx
, raCtxI
);
183 m3x3_mulv( gzoomer
.obj
.rb
.iIw
, raCty
, raCtyI
);
185 gzoomer
.tangent_mass
[index
][0] = gzoomer
.obj
.rb
.inv_mass
;
186 gzoomer
.tangent_mass
[index
][0] += v3_dot( raCtx
, raCtxI
);
187 gzoomer
.tangent_mass
[index
][0] = 1.0f
/gzoomer
.tangent_mass
[index
][0];
189 gzoomer
.tangent_mass
[index
][1] = gzoomer
.obj
.rb
.inv_mass
;
190 gzoomer
.tangent_mass
[index
][1] += v3_dot( raCty
, raCtyI
);
191 gzoomer
.tangent_mass
[index
][1] = 1.0f
/gzoomer
.tangent_mass
[index
][1];
193 /* apply drive force */
195 v3_muls( ty
, -gzoomer
.drive
* k_car_drive_force
* k_rb_delta
, F
);
196 rb_linear_impulse( &gzoomer
.obj
.rb
, raW
, F
);
200 gzoomer
.normal_forces
[ index
] = 0.0f
;
201 gzoomer
.tangent_forces
[ index
][0] = 0.0f
;
202 gzoomer
.tangent_forces
[ index
][1] = 0.0f
;
206 VG_STATIC
void vehicle_solve_friction(void)
208 rigidbody
*rb
= &gzoomer
.obj
.rb
;
209 for( int i
=0; i
<4; i
++ ){
211 m3x3_mulv( rb
->to_world
, gzoomer
.wheels_local
[i
], raW
);
214 v3_cross( rb
->w
, raW
, rv
);
215 v3_add( rb
->v
, rv
, rv
);
217 float fx
= k_car_friction_lat
* gzoomer
.normal_forces
[i
],
218 fy
= k_car_friction_roll
* gzoomer
.normal_forces
[i
],
219 vtx
= v3_dot( rv
, gzoomer
.tangent_vectors
[i
][0] ),
220 vty
= v3_dot( rv
, gzoomer
.tangent_vectors
[i
][1] ),
221 lambdax
= gzoomer
.tangent_mass
[i
][0] * -vtx
,
222 lambday
= gzoomer
.tangent_mass
[i
][1] * -vty
;
224 float tempx
= gzoomer
.tangent_forces
[i
][0],
225 tempy
= gzoomer
.tangent_forces
[i
][1];
226 gzoomer
.tangent_forces
[i
][0] = vg_clampf( tempx
+ lambdax
, -fx
, fx
);
227 gzoomer
.tangent_forces
[i
][1] = vg_clampf( tempy
+ lambday
, -fy
, fy
);
228 lambdax
= gzoomer
.tangent_forces
[i
][0] - tempx
;
229 lambday
= gzoomer
.tangent_forces
[i
][1] - tempy
;
231 v3f impulsex
, impulsey
;
232 v3_muls( gzoomer
.tangent_vectors
[i
][0], lambdax
, impulsex
);
233 v3_muls( gzoomer
.tangent_vectors
[i
][1], lambday
, impulsey
);
234 rb_linear_impulse( rb
, raW
, impulsex
);
235 rb_linear_impulse( rb
, raW
, impulsey
);
239 VG_STATIC
void vehicle_update_fixed(void)
244 rigidbody
*rb
= &gzoomer
.obj
.rb
;
246 v3_muls( rb
->to_world
[2], -cosf(gzoomer
.steer
), gzoomer
.steerv
);
247 v3_muladds( gzoomer
.steerv
, rb
->to_world
[0],
248 sinf(gzoomer
.steer
), gzoomer
.steerv
);
250 /* apply air resistance */
253 v3_muls( rb
->v
, -k_car_air_resistance
, Fair
);
254 v3_muls( rb
->to_world
[1], -fabsf(v3_dot( rb
->v
, rb
->to_world
[2] )) *
255 k_car_downforce
, Fdown
);
257 v3_muladds( rb
->v
, Fair
, k_rb_delta
, rb
->v
);
258 v3_muladds( rb
->v
, Fdown
, k_rb_delta
, rb
->v
);
260 for( int i
=0; i
<4; i
++ )
261 vehicle_wheel_force( i
);
264 int len
= rb_sphere__scene( rb
->to_world
, &gzoomer
.obj
.inf
.sphere
,
265 NULL
, &get_active_world()->rb_geo
.inf
.scene
,
267 for( int j
=0; j
<len
; j
++ ){
268 manifold
[j
].rba
= rb
;
269 manifold
[j
].rbb
= &get_active_world()->rb_geo
.rb
;
271 rb_manifold_filter_coplanar( manifold
, len
, 0.05f
);
274 rb_manifold_filter_backface( manifold
, len
);
275 rb_manifold_filter_joint_edges( manifold
, len
, 0.05f
);
276 rb_manifold_filter_pairs( manifold
, len
, 0.05f
);
278 len
= rb_manifold_apply_filtered( manifold
, len
);
280 rb_presolve_contacts( manifold
, len
);
281 for( int i
=0; i
<8; i
++ ){
282 rb_solve_contacts( manifold
, len
);
283 vehicle_solve_friction();
287 rb_update_transform( rb
);
290 VG_STATIC
void vehicle_update_post(void)
295 rb_object_debug( &gzoomer
.obj
, VG__WHITE
);
297 /* draw friction vectors */
300 for( int i
=0; i
<4; i
++ ){
301 v3_copy( gzoomer
.wheels
[i
], p0
);
302 v3_muladds( p0
, gzoomer
.tangent_vectors
[i
][0], 0.5f
, px
);
303 v3_muladds( p0
, gzoomer
.tangent_vectors
[i
][1], 0.5f
, py
);
305 vg_line( p0
, px
, VG__RED
);
306 vg_line( p0
, py
, VG__GREEN
);
310 #endif /* VEHICLE_H */