0576963e1f59ff8f4bfd0c541c9a7e9f8813a2a6
5 #include "scene_rigidbody.h"
7 static int spawn_car( int argc
, const char *argv
[] ){
9 v3_copy( skaterift
.cam
.pos
, ra
);
10 v3_muladds( ra
, skaterift
.cam
.transform
[2], -10.0f
, rb
);
13 if( spherecast_world( world_current_instance(),
14 ra
, rb
, 1.0f
, &t
, rx
, 0 ) != -1 )
16 v3_lerp( ra
, rb
, t
, gzoomer
.rb
.co
);
17 gzoomer
.rb
.co
[1] += 4.0f
;
18 q_axis_angle( gzoomer
.rb
.q
, (v3f
){1.0f
,0.0f
,0.0f
}, 0.001f
);
19 v3_zero( gzoomer
.rb
.v
);
20 v3_zero( gzoomer
.rb
.w
);
22 rb_update_matrices( &gzoomer
.rb
);
25 vg_success( "Spawned car\n" );
28 vg_error( "Can't spawn here\n" );
34 static void vehicle_init(void){
35 q_identity( gzoomer
.rb
.q
);
36 v3_zero( gzoomer
.rb
.w
);
37 v3_zero( gzoomer
.rb
.v
);
38 v3_zero( gzoomer
.rb
.co
);
39 rb_setbody_sphere( &gzoomer
.rb
, 1.0f
, 8.0f
, 1.0f
);
41 VG_VAR_F32( k_car_spring
, flags
=VG_VAR_PERSISTENT
);
42 VG_VAR_F32( k_car_spring_damp
, flags
=VG_VAR_PERSISTENT
);
43 VG_VAR_F32( k_car_spring_length
, flags
=VG_VAR_PERSISTENT
);
44 VG_VAR_F32( k_car_wheel_radius
, flags
=VG_VAR_PERSISTENT
);
45 VG_VAR_F32( k_car_friction_lat
, flags
=VG_VAR_PERSISTENT
);
46 VG_VAR_F32( k_car_friction_roll
, flags
=VG_VAR_PERSISTENT
);
47 VG_VAR_F32( k_car_drive_force
, flags
=VG_VAR_PERSISTENT
);
48 VG_VAR_F32( k_car_air_resistance
,flags
=VG_VAR_PERSISTENT
);
49 VG_VAR_F32( k_car_downforce
, flags
=VG_VAR_PERSISTENT
);
51 VG_VAR_I32( gzoomer
.inside
);
53 vg_console_reg_cmd( "spawn_car", spawn_car
, NULL
);
55 v3_copy((v3f
){ -1.0f
, -0.25f
, -1.5f
}, gzoomer
.wheels_local
[0] );
56 v3_copy((v3f
){ 1.0f
, -0.25f
, -1.5f
}, gzoomer
.wheels_local
[1] );
57 v3_copy((v3f
){ -1.0f
, -0.25f
, 1.5f
}, gzoomer
.wheels_local
[2] );
58 v3_copy((v3f
){ 1.0f
, -0.25f
, 1.5f
}, gzoomer
.wheels_local
[3] );
61 static void vehicle_wheel_force( int index
){
63 m4x3_mulv( gzoomer
.rb
.to_world
, gzoomer
.wheels_local
[index
], pa
);
64 v3_muladds( pa
, gzoomer
.rb
.to_world
[1], -k_car_spring_length
, pb
);
69 if( spherecast_world( world_current_instance(), pa
, pb
,
70 k_car_wheel_radius
, &t
, n
, 0 ) == -1 )
77 v3_muls( gzoomer
.rb
.up
, -1.0f
, dir
);
80 hit
.dist
= k_car_spring_length
;
81 ray_world( pa
, dir
, &hit
);
83 float t
= hit
.dist
/ k_car_spring_length
;
88 v3_lerp( pa
, pb
, t
, pc
);
91 m3x3_copy( gzoomer
.rb
.to_world
, mtx
);
92 v3_copy( pc
, mtx
[3] );
93 vg_line_sphere( mtx
, k_car_wheel_radius
, VG__BLACK
);
94 vg_line( pa
, pc
, VG__WHITE
);
95 v3_copy( pc
, gzoomer
.wheels
[index
] );
99 float Fv
= (1.0f
-t
) * k_car_spring
*vg
.time_fixed_delta
;
102 v3_sub( pa
, gzoomer
.rb
.co
, delta
);
105 v3_cross( gzoomer
.rb
.w
, delta
, rv
);
106 v3_add( gzoomer
.rb
.v
, rv
, rv
);
108 Fv
+= v3_dot(rv
, gzoomer
.rb
.to_world
[1])
109 * -k_car_spring_damp
*vg
.time_fixed_delta
;
111 /* scale by normal incident */
112 Fv
*= v3_dot( n
, gzoomer
.rb
.to_world
[1] );
115 v3_muls( gzoomer
.rb
.to_world
[1], Fv
, F
);
116 rb_linear_impulse( &gzoomer
.rb
, delta
, F
);
119 * -------------------------------------------------------------*/
123 v3_cross( gzoomer
.steerv
, n
, tx
);
125 v3_cross( n
, gzoomer
.rb
.to_world
[2], tx
);
126 v3_cross( tx
, n
, ty
);
128 v3_copy( tx
, gzoomer
.tangent_vectors
[ index
][0] );
129 v3_copy( ty
, gzoomer
.tangent_vectors
[ index
][1] );
131 gzoomer
.normal_forces
[ index
] = Fv
;
132 gzoomer
.tangent_forces
[ index
][0] = 0.0f
;
133 gzoomer
.tangent_forces
[ index
][1] = 0.0f
;
135 /* orient inverse inertia tensors */
137 m3x3_mulv( gzoomer
.rb
.to_world
, gzoomer
.wheels_local
[index
], raW
);
139 v3f raCtx
, raCtxI
, raCty
, raCtyI
;
140 v3_cross( tx
, raW
, raCtx
);
141 v3_cross( ty
, raW
, raCty
);
142 m3x3_mulv( gzoomer
.rb
.iIw
, raCtx
, raCtxI
);
143 m3x3_mulv( gzoomer
.rb
.iIw
, raCty
, raCtyI
);
145 gzoomer
.tangent_mass
[index
][0] = gzoomer
.rb
.inv_mass
;
146 gzoomer
.tangent_mass
[index
][0] += v3_dot( raCtx
, raCtxI
);
147 gzoomer
.tangent_mass
[index
][0] = 1.0f
/gzoomer
.tangent_mass
[index
][0];
149 gzoomer
.tangent_mass
[index
][1] = gzoomer
.rb
.inv_mass
;
150 gzoomer
.tangent_mass
[index
][1] += v3_dot( raCty
, raCtyI
);
151 gzoomer
.tangent_mass
[index
][1] = 1.0f
/gzoomer
.tangent_mass
[index
][1];
153 /* apply drive force */
155 v3_muls( ty
, -gzoomer
.drive
* k_car_drive_force
156 * vg
.time_fixed_delta
, F
);
157 rb_linear_impulse( &gzoomer
.rb
, raW
, F
);
161 gzoomer
.normal_forces
[ index
] = 0.0f
;
162 gzoomer
.tangent_forces
[ index
][0] = 0.0f
;
163 gzoomer
.tangent_forces
[ index
][1] = 0.0f
;
167 static void vehicle_solve_friction(void){
168 rigidbody
*rb
= &gzoomer
.rb
;
169 for( int i
=0; i
<4; i
++ ){
171 m3x3_mulv( rb
->to_world
, gzoomer
.wheels_local
[i
], raW
);
174 v3_cross( rb
->w
, raW
, rv
);
175 v3_add( rb
->v
, rv
, rv
);
177 float fx
= k_car_friction_lat
* gzoomer
.normal_forces
[i
],
178 fy
= k_car_friction_roll
* gzoomer
.normal_forces
[i
],
179 vtx
= v3_dot( rv
, gzoomer
.tangent_vectors
[i
][0] ),
180 vty
= v3_dot( rv
, gzoomer
.tangent_vectors
[i
][1] ),
181 lambdax
= gzoomer
.tangent_mass
[i
][0] * -vtx
,
182 lambday
= gzoomer
.tangent_mass
[i
][1] * -vty
;
184 float tempx
= gzoomer
.tangent_forces
[i
][0],
185 tempy
= gzoomer
.tangent_forces
[i
][1];
186 gzoomer
.tangent_forces
[i
][0] = vg_clampf( tempx
+ lambdax
, -fx
, fx
);
187 gzoomer
.tangent_forces
[i
][1] = vg_clampf( tempy
+ lambday
, -fy
, fy
);
188 lambdax
= gzoomer
.tangent_forces
[i
][0] - tempx
;
189 lambday
= gzoomer
.tangent_forces
[i
][1] - tempy
;
191 v3f impulsex
, impulsey
;
192 v3_muls( gzoomer
.tangent_vectors
[i
][0], lambdax
, impulsex
);
193 v3_muls( gzoomer
.tangent_vectors
[i
][1], lambday
, impulsey
);
194 rb_linear_impulse( rb
, raW
, impulsex
);
195 rb_linear_impulse( rb
, raW
, impulsey
);
199 static void vehicle_update_fixed(void)
204 rigidbody
*rb
= &gzoomer
.rb
;
206 v3_muls( rb
->to_world
[2], -cosf(gzoomer
.steer
), gzoomer
.steerv
);
207 v3_muladds( gzoomer
.steerv
, rb
->to_world
[0],
208 sinf(gzoomer
.steer
), gzoomer
.steerv
);
210 /* apply air resistance */
213 v3_muls( rb
->v
, -k_car_air_resistance
, Fair
);
214 v3_muls( rb
->to_world
[1], -fabsf(v3_dot( rb
->v
, rb
->to_world
[2] )) *
215 k_car_downforce
, Fdown
);
217 v3_muladds( rb
->v
, Fair
, vg
.time_fixed_delta
, rb
->v
);
218 v3_muladds( rb
->v
, Fdown
, vg
.time_fixed_delta
, rb
->v
);
220 for( int i
=0; i
<4; i
++ )
221 vehicle_wheel_force( i
);
223 rigidbody _null
= {0};
224 _null
.inv_mass
= 0.0f
;
225 m3x3_zero( _null
.iI
);
228 int len
= rb_sphere__scene( rb
->to_world
, 1.0f
, NULL
,
229 world_current_instance()->geo_bh
,
231 for( int j
=0; j
<len
; j
++ ){
232 manifold
[j
].rba
= rb
;
233 manifold
[j
].rbb
= &_null
;
235 rb_manifold_filter_coplanar( manifold
, len
, 0.05f
);
238 rb_manifold_filter_backface( manifold
, len
);
239 rb_manifold_filter_joint_edges( manifold
, len
, 0.05f
);
240 rb_manifold_filter_pairs( manifold
, len
, 0.05f
);
242 len
= rb_manifold_apply_filtered( manifold
, len
);
244 rb_presolve_contacts( manifold
, vg
.time_fixed_delta
, len
);
245 for( int i
=0; i
<8; i
++ ){
246 rb_solve_contacts( manifold
, len
);
247 vehicle_solve_friction();
251 rb_update_matrices( rb
);
254 static void vehicle_update_post(void){
258 vg_line_sphere( gzoomer
.rb
.to_world
, 1.0f
, VG__WHITE
);
260 /* draw friction vectors */
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
);
268 vg_line( p0
, px
, VG__RED
);
269 vg_line( p0
, py
, VG__GREEN
);
273 #endif /* VEHICLE_H */