Fix suspension when driving on other rigid bodies, super solid now

The key was to limit the total spring impulse similar to how contact impulse is limited, so that spring always pushes and never pulls

game/game.odin

@@ -557,7 +557,7 @@ update_world :: proc(world: ^World, dt: f32, config: World_Update_Config) {
 						u32(box_name),
 						physics.Body_Config {
 							name = box_name,
-							initial_pos = {-5, 0.5 + f32(y) * 1.1, f32(x) * 3 + 10},
+							initial_pos = {-5 + f32(y) * 1.01, 1, f32(x) * 1.01 + -11.5},
 							initial_rot = linalg.QUATERNIONF32_IDENTITY,
 							shapes = {
 								{
@@ -598,8 +598,8 @@ update_world :: proc(world: ^World, dt: f32, config: World_Update_Config) {
 		wheel_extent_x_back := f32(2) / 2
 		wheel_y := f32(0.5)
 		rest := f32(0.7)
-		natural_frequency := f32(0.4)
+		natural_frequency := f32(1.2)
-		damping := f32(0.06)
+		damping := f32(0.4)
 		radius := f32(0.737649) / 2
 		wheel_front_z := f32(1.6)
 		wheel_back_z := f32(-1.63)
@@ -818,9 +818,9 @@ update_runtime_world :: proc(runtime_world: ^Runtime_World, dt: f32) {
 	cur_world := runtime_world_current_world(runtime_world)
 
 	runtime_world.dt = dt
-	should_single_step := rl.IsKeyPressed(.PERIOD)
+	should_single_step := g_mem.physics_pause // rl.IsKeyPressed(.PERIOD)
 	runtime_world.rewind_simulation = rl.IsKeyPressed(.COMMA)
-	runtime_world.commit_simulation = !g_mem.physics_pause || should_single_step
+	runtime_world.commit_simulation = !g_mem.physics_pause || rl.IsKeyPressed(.PERIOD)
 	runtime_world.single_step_simulation = should_single_step
 
 	if !runtime_world.rewind_simulation {

game/physics/scene.odin

@@ -288,16 +288,21 @@ Suspension_Constraint :: struct {
 	lateral_impulse:        f32,
 	longitudinal_impulse:   f32,
 	brake_friction_impulse: f32,
-
+	inv_mass:               f32,
 	// Inverse inertia along wheel rotation axis
 	inv_inertia:            f32,
 
+	// Wheel pos along suspension dir
+	x:                      f32,
 	// Rotation
 	q:                      f32,
+	// Linear velocity along suspension dir
+	v:                      f32,
 	// Angular velocity
 	w:                      f32,
 	hit:                    bool,
 	hit_normal:             Vec3,
+	hit_body:               Body_Handle,
 	hit_point:              Vec3,
 	// rel_hit_point = rel_pos + rel_dir * hit_t
 	hit_t:                  f32,

game/physics/simulation.odin

@@ -247,6 +247,7 @@ raycast_bodies :: proc(
 ) -> (
 	t: f32,
 	normal: Vec3,
+	body_handle: Body_Handle,
 	hit: bool,
 ) {
 	temp := runtime.default_temp_allocator_temp_begin()
@@ -263,8 +264,9 @@ raycast_bodies :: proc(
 	for leaf_node in bvh.iterator_intersect_leaf_next(&it) {
 		for j in 0 ..< leaf_node.prim_len {
 			body_idx := tlas.bvh_tree.primitives[leaf_node.child_or_prim_start + j]
+			tmp_body_handle := index_to_body_handle(int(body_idx))
 
-			body := get_body(sim_state, index_to_body_handle(int(body_idx)))
+			body := get_body(sim_state, tmp_body_handle)
 
 			it := shapes_iterator(sim_state, body.shape)
 			for shape in shapes_iterator_next(&it) {
@@ -285,12 +287,17 @@ raycast_bodies :: proc(
 				if ok && (!hit || hit_t < t) {
 					t = hit_t
 					normal = tmp_normal
+					body_handle = tmp_body_handle
 					hit = true
 				}
 			}
 		}
 	}
 
+	if hit {
+		normal = lg.normalize0(normal)
+	}
+
 	return
 }
 
@@ -304,12 +311,13 @@ raycast :: proc(
 ) -> (
 	t: f32,
 	normal: Vec3,
+	hit_body: Body_Handle,
 	hit: bool,
 ) {
 	t = distance
 
 	t1, normal1, hit1 := raycasts_level(sim_state, sim_cache, static_tlas, origin, dir, t)
-	t2, normal2, hit2 := raycast_bodies(sim_state, dyn_tlas, origin, dir, t)
+	t2, normal2, body, hit2 := raycast_bodies(sim_state, dyn_tlas, origin, dir, t)
 
 	hit = hit1 || hit2
 
@@ -320,6 +328,7 @@ raycast :: proc(
 		} else {
 			t = t2
 			normal = normal2
+			hit_body = body
 		}
 	} else if hit1 {
 		t = t1
@@ -327,11 +336,9 @@ raycast :: proc(
 	} else if hit2 {
 		t = t2
 		normal = normal2
+		hit_body = body
 	}
 
-	// TODO: raycast_level and raycast_bodies should return a normalized vec
-	normal = lg.normalize0(normal)
-
 	return
 }
 
@@ -1267,18 +1274,31 @@ pgs_solve_engines :: proc(sim_state: ^Sim_State, config: Solver_Config, dt: f32,
 
 calculate_ground_vel :: proc(
 	body: Body_Ptr,
+	hit_body: Body_Ptr,
 	p: Vec3,
 	right, forward: Vec3,
 ) -> (
 	body_vel_at_contact_patch: Vec3,
 	ground_vel: Vec2,
 ) {
-	body_vel_at_contact_patch = body_velocity_at_point(body, p)
+	body_vel_at_contact_patch =
+		body_velocity_at_point(body, p) - body_velocity_at_point(hit_body, p)
 	ground_vel.x = lg.dot(body_vel_at_contact_patch, right)
 	ground_vel.y = lg.dot(body_vel_at_contact_patch, forward)
 	return
 }
 
+combine_inv_mass :: proc(w1, w2: f32) -> f32 {
+	if w1 > 0 && w2 > 0 {
+		return (w1 * w2) / (w1 + w2)
+	}
+	if w1 > 0 {
+		return w1
+	}
+
+	return w2
+}
+
 pgs_solve_suspension :: proc(
 	sim_state: ^Sim_State,
 	sim_cache: ^Sim_Cache,
@@ -1287,6 +1307,7 @@ pgs_solve_suspension :: proc(
 	config: Solver_Config,
 	dt: f32,
 	inv_dt: f32,
+	apply_bias: bool,
 ) {
 	// Solve suspension velocity
 	for _, i in sim_state.suspension_constraints {
@@ -1297,7 +1318,8 @@ pgs_solve_suspension :: proc(
 			if body.alive {
 				wheel_world_pos := body_local_to_world(body, v.rel_pos + body.shape_offset)
 				dir := body_local_to_world_vec(body, v.rel_dir)
-				v.hit_t, v.hit_normal, v.hit = raycast(
+				prev_hit_body := v.hit_body
+				v.hit_t, v.hit_normal, v.hit_body, v.hit = raycast(
 					sim_state,
 					sim_cache,
 					static_tlas,
@@ -1306,13 +1328,12 @@ pgs_solve_suspension :: proc(
 					dir,
 					v.rest,
 				)
-				// log.debugf("hit_t: %v, hit: %v", v.hit_t, v.hit)
+				hit_body := get_body(sim_state, v.hit_body)
 				v.hit_point = wheel_world_pos + dir * v.hit_t
 
-				forward := wheel_get_forward_vec(body, v)
+				w_normal1 := get_body_inverse_mass(body, -dir, wheel_world_pos)
-				right := wheel_get_right_vec(body, v)
+				w_normal2 := get_body_inverse_mass(hit_body, dir, v.hit_point)
-
+				w_normal := combine_inv_mass(w_normal1, w_normal2)
-				w_normal := get_body_angular_inverse_mass(body, -v.hit_normal)
 				inv_w_normal := 1.0 / w_normal
 
 				// Drive force
@@ -1347,7 +1368,17 @@ pgs_solve_suspension :: proc(
 					}
 				}
 
+				if !v.hit || v.hit_body != prev_hit_body {
+					v.lateral_impulse = 0
+					v.spring_impulse = 0
+					v.longitudinal_impulse = 0
+				}
+
 				if v.hit {
+					forward := wheel_get_forward_vec(body, v)
+					right := lg.normalize0(lg.cross(forward, v.hit_normal))
+					forward = lg.normalize0(lg.cross(v.hit_normal, right))
+
 
 					// Spring force
 					{
@@ -1356,21 +1387,30 @@ pgs_solve_suspension :: proc(
 							f64(v.damping),
 							f64(dt),
 						)
+						if !apply_bias {
+							mass_coef = 1
+							bias_coef = 0
+							impulse_coef = 0
+						}
 
-						vel := lg.dot(body_velocity_at_point(body, v.hit_point), -v.hit_normal)
+						vel := lg.dot(
+							body_velocity_at_point(body, v.hit_point) -
+							body_velocity_at_point(hit_body, v.hit_point),
+							dir,
+						)
 						x := v.hit_t
 						separation := v.rest - x
 
+						// spring_dot_normal := abs(lg.dot(dir, v.hit_normal))
 						incremental_impulse :=
 							-inv_w_normal * mass_coef * (vel + separation * bias_coef) -
 							impulse_coef * v.spring_impulse
-						v.spring_impulse += incremental_impulse
+						new_spring_impulse := min(0, v.spring_impulse + incremental_impulse)
+						applied_impulse := new_spring_impulse - v.spring_impulse
+						v.spring_impulse = new_spring_impulse
 
-						apply_velocity_correction(
+						apply_velocity_correction(body, applied_impulse * dir, wheel_world_pos)
-							body,
+						apply_velocity_correction(hit_body, applied_impulse * -dir, v.hit_point)
-							incremental_impulse * v.hit_normal,
-							v.hit_point,
-						)
 					}
 
 					// Positive means spinning forward
@@ -1378,6 +1418,7 @@ pgs_solve_suspension :: proc(
 
 					body_vel_at_contact_patch, ground_vel := calculate_ground_vel(
 						body,
+						hit_body,
 						v.hit_point,
 						right,
 						forward,
@@ -1432,6 +1473,7 @@ pgs_solve_suspension :: proc(
 					if true {
 						body_vel_at_contact_patch, ground_vel = calculate_ground_vel(
 							body,
+							hit_body,
 							v.hit_point,
 							right,
 							forward,
@@ -1442,9 +1484,11 @@ pgs_solve_suspension :: proc(
 
 						friction_clamp := abs(v.spring_impulse) * long_friction
 
-						w_body := get_body_inverse_mass(body, forward, v.hit_point)
+						w_body1 := get_body_inverse_mass(body, forward, v.hit_point)
+						w_body2 := get_body_inverse_mass(hit_body, forward, v.hit_point)
+						w_body := w_body1 + v.inv_inertia + w_body2
 
-						w_long := w_body + v.inv_inertia
+						w_long := w_body
 						inv_w_long := 1.0 / w_long
 
 						incremental_impulse := -inv_w_long * relative_vel
@@ -1457,12 +1501,18 @@ pgs_solve_suspension :: proc(
 						v.longitudinal_impulse = new_total_impulse
 
 						apply_velocity_correction(body, applied_impulse * forward, v.hit_point)
+						apply_velocity_correction(
+							hit_body,
+							-applied_impulse * forward,
+							v.hit_point,
+						)
 						v.w += v.inv_inertia * applied_impulse
 					}
 					// Lateral friction
 					if true {
 						body_vel_at_contact_patch, ground_vel = calculate_ground_vel(
 							body,
+							hit_body,
 							v.hit_point,
 							right,
 							forward,
@@ -1470,7 +1520,14 @@ pgs_solve_suspension :: proc(
 						lateral_vel := ground_vel.x
 						friction_clamp := abs(v.spring_impulse) * lat_friction
 
-						incremental_impulse := -inv_w_normal * lateral_vel
+						w_body1 := get_body_inverse_mass(body, right, v.hit_point)
+						w_body2 := get_body_inverse_mass(hit_body, right, v.hit_point)
+						w_body := w_body1 + w_body2
+
+						w_lat := w_body
+						inv_w_lat := 1.0 / w_lat
+
+						incremental_impulse := -inv_w_lat * lateral_vel
 						new_total_impulse := clamp(
 							v.lateral_impulse + incremental_impulse,
 							-friction_clamp,
@@ -1480,11 +1537,8 @@ pgs_solve_suspension :: proc(
 						v.lateral_impulse = new_total_impulse
 
 						apply_velocity_correction(body, applied_impulse * right, v.hit_point)
+						apply_velocity_correction(hit_body, -applied_impulse * right, v.hit_point)
 					}
-				} else {
-					v.lateral_impulse = 0
-					v.spring_impulse = 0
-					v.longitudinal_impulse = 0
 				}
 			}
 		}
@@ -1589,18 +1643,25 @@ pgs_substep :: proc(
 
 			if s.hit {
 				body := get_body(sim_state, s.body)
+				hit_body := get_body(sim_state, s.hit_body)
 				p := body_local_to_world(body, s.rel_pos + body.shape_offset)
+				dir := body_local_to_world_vec(body, s.rel_dir)
 				hit_p := body_local_to_world(
 					body,
 					s.rel_pos + body.shape_offset + s.rel_dir * s.hit_t,
 				)
 				forward := wheel_get_forward_vec(body, s)
-				right := wheel_get_right_vec(body, s)
+				right := lg.normalize0(lg.cross(forward, s.hit_normal))
+				forward = lg.normalize0(lg.cross(s.hit_normal, right))
 
-				apply_velocity_correction(body, s.spring_impulse * -s.hit_normal, hit_p)
+				apply_velocity_correction(body, s.spring_impulse * dir, p)
-				apply_velocity_correction(body, s.lateral_impulse * right, p)
+				apply_velocity_correction(hit_body, s.spring_impulse * -dir, hit_p)
+
+				apply_velocity_correction(body, s.lateral_impulse * right, hit_p)
+				apply_velocity_correction(hit_body, -s.lateral_impulse * right, hit_p)
 
 				apply_velocity_correction(body, s.longitudinal_impulse * forward, hit_p)
+				apply_velocity_correction(hit_body, -s.longitudinal_impulse * forward, hit_p)
 				s.w += s.inv_inertia * s.longitudinal_impulse
 			}
 
@@ -1611,7 +1672,16 @@ pgs_substep :: proc(
 	apply_bias := true
 	pgs_solve_contacts(sim_state, config, dt, inv_dt, apply_bias)
 	pgs_solve_engines(sim_state, config, dt, inv_dt)
-	pgs_solve_suspension(sim_state, sim_cache, static_tlas, dyn_tlas, config, dt, inv_dt)
+	pgs_solve_suspension(
+		sim_state,
+		sim_cache,
+		static_tlas,
+		dyn_tlas,
+		config,
+		dt,
+		inv_dt,
+		apply_bias,
+	)
 
 	for i in 0 ..< len(sim_state.bodies_slice) {
 		body := &sim_state.bodies_slice[i]
@@ -1644,12 +1714,22 @@ pgs_substep :: proc(
 	for i in 0 ..< len(sim_state.suspension_constraints_slice) {
 		s := &sim_state.suspension_constraints_slice[i]
 
+		s.x += s.v * dt
 		s.q = math.mod_f32(s.q + 0.5 * s.w * dt, math.PI * 2)
 	}
 
 	apply_bias = false
 	pgs_solve_contacts(sim_state, config, dt, inv_dt, apply_bias)
-	// pgs_solve_suspension(sim_state, config, dt, inv_dt, apply_bias)
+	// pgs_solve_suspension(
+	// 	sim_state,
+	// 	sim_cache,
+	// 	static_tlas,
+	// 	dyn_tlas,
+	// 	config,
+	// 	dt,
+	// 	inv_dt,
+	// 	apply_bias,
+	// )
 }
 
 simulate_step :: proc(