Fixed unstable suspension (feel like an idiot)

game/debug/helpers.odin

@@ -0,0 +1,24 @@
+package debug
+
+import rl "vendor:raylib"
+
+int_to_color :: proc(num: i32) -> (color: rl.Color) {
+	x := int_hash(num)
+
+	color.r = u8(x % 256)
+	color.g = u8((x / 256) % 256)
+	color.b = u8((x / 256 / 256) % 256)
+	color.a = 255
+	return
+}
+
+int_hash :: proc(num: i32) -> u32 {
+	x := cast(u32)num
+
+	x = ((x >> 16) ~ x) * 0x45d9f3b
+	x = ((x >> 16) ~ x) * 0x45d9f3b
+	x = (x >> 16) ~ x
+
+	return x
+}
+

game/game.odin

@@ -64,7 +64,7 @@ Car :: struct {
 SOLVER_CONFIG :: physics.Solver_Config {
 	timestep            = 1.0 / 120,
 	gravity             = rl.Vector3{0, -9.8, 0},
-	substreps_minus_one = 8 - 1,
+	substreps_minus_one = 4 - 1,
 }
 
 Game_Memory :: struct {
@@ -235,18 +235,31 @@ update_runtime_world :: proc(runtime_world: ^Runtime_World, dt: f32) {
 			#hash("car", "fnv32a"),
 			physics.Body_Config {
 				initial_pos = {0, 2, 0},
-				initial_rot = linalg.QUATERNIONF32_IDENTITY,
+				initial_rot = linalg.quaternion_angle_axis(
+					math.RAD_PER_DEG * 100,
+					rl.Vector3{0, 1, 0},
+				),
 				initial_ang_vel = {0, 0, 0},
+				shape = physics.Shape_Box{size = car_bounds.max - car_bounds.min},
 				mass = 100,
-				inertia_tensor = physics.inertia_tensor_box(car_bounds.max - car_bounds.min),
 			},
 		)
 
-		runtime_world.camera.up = rl.Vector3{0, 1, 0}
+		car_body := physics.get_body(&world.physics_scene, runtime_world.car_handle)
-		runtime_world.camera.fovy = 60
+
-		runtime_world.camera.projection = .PERSPECTIVE
+		camera := &runtime_world.camera
-		runtime_world.camera.target =
+
-			physics.get_body(&world.physics_scene, runtime_world.car_handle).x
+		camera.up = rl.Vector3{0, 1, 0}
+		camera.fovy = 60
+		camera.projection = .PERSPECTIVE
+		camera.position = physics.body_local_to_world(
+			car_body,
+			physics.body_world_to_local(
+				car_body,
+				physics.body_local_to_world(car_body, rl.Vector3{1, 0, -2}),
+			),
+		)
+		camera.target = physics.get_body(&world.physics_scene, runtime_world.car_handle).x
 		if runtime_world.camera.position == {} {
 			runtime_world.camera.position = runtime_world.camera.target - rl.Vector3{10, 0, 10}
 		}
@@ -257,7 +270,7 @@ update_runtime_world :: proc(runtime_world: ^Runtime_World, dt: f32) {
 		rest := f32(1)
 		suspension_stiffness := f32(2000)
 		compliance := 1.0 / suspension_stiffness
-		damping := f32(0.1)
+		damping := f32(0.01)
 		radius := f32(0.6)
 
 		wheel_fl := physics.immediate_suspension_constraint(
@@ -322,7 +335,7 @@ update_runtime_world :: proc(runtime_world: ^Runtime_World, dt: f32) {
 
 		DRIVE_IMPULSE :: 1
 		BRAKE_IMPULSE :: 2
-		TURN_ANGLE :: -f32(10) * math.RAD_PER_DEG
+		TURN_ANGLE :: -f32(30) * math.RAD_PER_DEG
 
 		for wheel_handle in drive_wheels {
 			wheel := physics.get_suspension_constraint(&world.physics_scene, wheel_handle)
@@ -469,6 +482,8 @@ draw :: proc() {
 
 		rl.DrawGrid(100, 1)
 
+		physics.draw_debug_scene(&world.physics_scene)
+
 		{
 			mesh_bvh := assets.get_bvh(&g_mem.assetman, "assets/toyota_corolla_ae86_trueno.glb")
 
@@ -505,15 +520,13 @@ draw :: proc() {
 			car_matrix := rl.QuaternionToMatrix(car_body.q)
 			car_model.transform = car_matrix
 
-			rl.DrawModel(car_model, car_body.x - runtime_world.car_com, 1, rl.WHITE)
+			rl.DrawModel(car_model, car_body.x + runtime_world.car_com, 1, rl.WHITE)
 		} else {
 			if g_mem.draw_car {
 				rl.DrawModel(car_model, 0, 1, rl.WHITE)
 			}
 		}
 
-		physics.draw_debug_scene(&world.physics_scene)
-
 		{
 			// Debug draw spline road
 			{
@@ -596,8 +609,20 @@ draw :: proc() {
 				)
 			}
 		} else {
-			car_pos := physics.get_body(&world.physics_scene, runtime_world.car_handle).x
+			car := physics.get_body(&world.physics_scene, runtime_world.car_handle)
-			rl.DrawText(fmt.ctprintf("Car Pos: %v", car_pos), 5, 32, 8, rl.ORANGE)
+			rl.DrawText(
+				fmt.ctprintf(
+					"p: %v\nv: %v\nw: %v\ng: %v",
+					car.x,
+					car.v,
+					car.w,
+					SOLVER_CONFIG.gravity,
+				),
+				5,
+				32,
+				8,
+				rl.ORANGE,
+			)
 		}
 	}
 

game/physics/bvh/bvh.odin

@@ -7,6 +7,7 @@ import "core:log"
 import "core:math"
 import lg "core:math/linalg"
 import "core:mem"
+import "game:debug"
 import rl "vendor:raylib"
 
 _ :: log
@@ -285,7 +286,7 @@ internal_traverse_bvh_ray_triangles :: proc(
 
 		rl.DrawBoundingBox(
 			{min = node.aabb.min, max = node.aabb.max},
-			debug_int_to_color(node_idx),
+			debug.int_to_color(node_idx),
 		)
 
 		if is_leaf_node(node^) {
@@ -312,9 +313,9 @@ internal_ray_tri_test :: proc(ray: Ray, mesh: Mesh, tri: u16, col: ^Collision) {
 	v1, v2, v3 := mesh.vertices[i1], mesh.vertices[i2], mesh.vertices[i3]
 
 	col.triangle_tests += 1
-	rl.DrawTriangle3D(v1, v2, v3, debug_int_to_color(i32(tri) + 1))
+	rl.DrawTriangle3D(v1, v2, v3, debug.int_to_color(i32(tri) + 1))
 
-	t, _, barycentric, ok := collision.intersect_segment_triangle(
+	t, _, barycentric, ok := collision.intersect_ray_triangle(
 		{ray.origin, ray.origin + ray.dir},
 		{v1, v2, v3},
 	)

game/physics/bvh/debug.odin

@@ -5,6 +5,7 @@ import "core:container/queue"
 import "core:fmt"
 import "core:log"
 import lg "core:math/linalg"
+import "game:debug"
 import rl "vendor:raylib"
 import "vendor:raylib/rlgl"
 
@@ -40,7 +41,7 @@ debug_draw_bvh_bounds :: proc(bvh: ^BVH, mesh: Mesh, pos: rl.Vector3, node_index
 		if should_draw {
 			rl.DrawBoundingBox(
 				rl.BoundingBox{node.aabb.min + pos, node.aabb.max + pos},
-				debug_int_to_color(node_idx + 1),
+				debug.int_to_color(node_idx + 1),
 			)
 		}
 
@@ -73,40 +74,20 @@ debug_draw_bvh_bounds :: proc(bvh: ^BVH, mesh: Mesh, pos: rl.Vector3, node_index
 				}
 
 				size := lg.length(aabb.max - aabb.min)
-				rl.DrawTriangle3D(v1, v2, v3, debug_int_to_color(i32(tri) + 1))
+				rl.DrawTriangle3D(v1, v2, v3, debug.int_to_color(i32(tri) + 1))
 				rl.DrawBoundingBox(
 					rl.BoundingBox{aabb.min, aabb.max},
-					debug_int_to_color(i32(tri) + 2),
+					debug.int_to_color(i32(tri) + 2),
 				)
 
 				if size < 1 {
-					rl.DrawCubeWiresV(centroid, 0.05, debug_int_to_color(i32(tri) + 3))
+					rl.DrawCubeWiresV(centroid, 0.05, debug.int_to_color(i32(tri) + 3))
 				}
 			}
 		}
 	}
 }
 
-debug_int_to_color :: proc(num: i32) -> (color: rl.Color) {
-	x := debug_hash(num)
-
-	color.r = u8(x % 256)
-	color.g = u8((x / 256) % 256)
-	color.b = u8((x / 256 / 256) % 256)
-	color.a = 255
-	return
-}
-
-debug_hash :: proc(num: i32) -> u32 {
-	x := cast(u32)num
-
-	x = ((x >> 16) ~ x) * 0x45d9f3b
-	x = ((x >> 16) ~ x) * 0x45d9f3b
-	x = (x >> 16) ~ x
-
-	return x
-}
-
 bvh_mesh_from_rl_mesh :: proc(mesh: rl.Mesh) -> Mesh {
 	return Mesh {
 		vertices = (cast([^]Vec3)mesh.vertices)[:mesh.vertexCount],

game/physics/collision/collision.odin

@@ -567,7 +567,7 @@ intersect_ray_polyhedron :: proc(
 	return t, true
 }
 
-intersect_segment_triangle :: proc(
+intersect_ray_triangle :: proc(
 	segment: [2]Vec3,
 	triangle: [3]Vec3,
 ) -> (
@@ -633,7 +633,7 @@ intersect_segment_plane :: proc(
 		return t, point, true
 	}
 
-	return t, segment[0], false
+	return linalg.length(ab), segment[1], false
 }
 
 // TODO: alternative with capsule endcaps

game/physics/debug.odin

@@ -3,10 +3,38 @@ package physics
 import "core:log"
 import "core:math"
 import lg "core:math/linalg"
+import "game:debug"
 import rl "vendor:raylib"
+import "vendor:raylib/rlgl"
 
 _ :: log
 _ :: math
+_ :: debug
+
+draw_debug_shape :: proc(
+	shape: Collision_Shape,
+	pos: rl.Vector3,
+	rot: rl.Quaternion,
+	color: rl.Color,
+) {
+	rlgl.PushMatrix()
+	defer rlgl.PopMatrix()
+
+	rlgl.Begin(rlgl.LINES)
+	defer rlgl.End()
+
+	switch s in shape {
+	case Shape_Sphere:
+		rl.DrawSphere(pos, s.radius, color)
+	case Shape_Box:
+		mat := lg.matrix4_from_trs(pos, rot, 1)
+
+		rlgl.LoadIdentity()
+		rlgl.MultMatrixf(cast([^]f32)&mat)
+
+		rl.DrawCubeV(0, s.size, color)
+	}
+}
 
 draw_debug_scene :: proc(scene: ^Scene) {
 	for &body in scene.bodies {
@@ -21,6 +49,8 @@ draw_debug_scene :: proc(scene: ^Scene) {
 			rl.DrawLine3D(pos, pos + x, rl.RED)
 			rl.DrawLine3D(pos, pos + y, rl.GREEN)
 			rl.DrawLine3D(pos, pos + z, rl.BLUE)
+
+			// draw_debug_shape(body.shape, body.x, body.q, debug.int_to_color(i32(i + 1)))
 		}
 	}
 
@@ -50,11 +80,15 @@ draw_debug_scene :: proc(scene: ^Scene) {
 				rl.RED,
 			)
 
-			rl.DrawLine3D(
+			rl.DrawLine3D(wheel_pos, wheel_pos + right * 10, rl.RED)
-				pos + t * dir,
+
-				pos + t * dir + wheel.applied_impulse.x * right * 10,
+			if wheel.hit {
-				rl.RED,
+				// rl.DrawLine3D(
-			)
+				// 	pos + t * dir,
+				// 	pos + t * dir + wheel.applied_impulse.x * right * 10,
+				// 	rl.RED,
+				// )
+			}
 
 			if wheel.hit {
 				rl.DrawSphereWires(wheel.hit_point, 0.1, 4, 4, rl.RED)

game/physics/helpers.odin

@@ -3,6 +3,12 @@ package physics
 import lg "core:math/linalg"
 import rl "vendor:raylib"
 
+inertia_tensor_sphere :: proc(radius: f32) -> (tensor: rl.Vector3) {
+	tensor = radius * radius * (2.0 / 3.0)
+
+	return
+}
+
 inertia_tensor_box :: proc(size: rl.Vector3) -> (tensor: rl.Vector3) {
 	CONSTANT :: f32(1.0 / 12.0)
 
@@ -15,22 +21,33 @@ inertia_tensor_box :: proc(size: rl.Vector3) -> (tensor: rl.Vector3) {
 	return
 }
 
+inertia_tensor_collision_shape :: proc(shape: Collision_Shape) -> (tensor: rl.Vector3) {
+	switch s in shape {
+	case Shape_Sphere:
+		tensor = inertia_tensor_sphere(s.radius)
+	case Shape_Box:
+		tensor = inertia_tensor_box(s.size)
+	}
+
+	return
+}
+
 body_local_to_world :: #force_inline proc(body: Body_Ptr, pos: rl.Vector3) -> rl.Vector3 {
 	return body.x + lg.quaternion_mul_vector3(body.q, pos)
 }
 
 body_local_to_world_vec :: #force_inline proc(body: Body_Ptr, vec: rl.Vector3) -> rl.Vector3 {
-	return lg.quaternion_mul_vector3(body.q, vec)
+	return lg.normalize0(lg.quaternion_mul_vector3(body.q, vec))
 }
 
 body_world_to_local :: #force_inline proc(body: Body_Ptr, pos: rl.Vector3) -> rl.Vector3 {
-	inv_q := lg.quaternion_inverse(body.q)
+	inv_q := lg.quaternion_normalize0(lg.quaternion_inverse(body.q))
 	return lg.quaternion_mul_vector3(inv_q, pos - body.x)
 }
 
 body_world_to_local_vec :: #force_inline proc(body: Body_Ptr, vec: rl.Vector3) -> rl.Vector3 {
 	inv_q := lg.quaternion_inverse(body.q)
-	return lg.quaternion_mul_vector3(inv_q, vec)
+	return lg.normalize0(lg.quaternion_mul_vector3(inv_q, vec))
 }
 
 body_angular_velocity_at_local_point :: #force_inline proc(

game/physics/immediate.odin

@@ -1,14 +1,22 @@
 package physics
 
+import lg "core:math/linalg"
 import rl "vendor:raylib"
 
+Body_Config_Inertia_Mode :: enum {
+	From_Shape,
+	Explicit,
+}
+
 // Immediate mode stuff for testing
 Body_Config :: struct {
 	initial_pos:     rl.Vector3,
 	initial_rot:     rl.Quaternion,
 	initial_vel:     rl.Vector3,
 	initial_ang_vel: rl.Vector3,
+	shape:           Collision_Shape,
 	mass:            f32,
+	inertia_mode:    Body_Config_Inertia_Mode,
 	// Unit inertia tensor
 	inertia_tensor:  rl.Vector3,
 }
@@ -24,18 +32,40 @@ Suspension_Constraint_Config :: struct {
 	radius:     f32,
 }
 
+calculate_body_params_from_config :: proc(
+	config: Body_Config,
+) -> (
+	inv_mass: f32,
+	inv_inertia_tensor: rl.Vector3,
+) {
+	inv_mass = config.mass == 0 ? 0 : 1.0 / config.mass
+
+	inertia_tensor: rl.Vector3
+	if config.inertia_mode == .Explicit {
+		inertia_tensor = config.inertia_tensor
+	} else {
+		inertia_tensor = inertia_tensor_collision_shape(config.shape)
+	}
+	inertia_tensor *= config.mass
+
+	inv_inertia_tensor = lg.all(lg.equal(inertia_tensor, rl.Vector3(0))) ? 0 : 1.0 / inertia_tensor
+
+	return
+}
+
 initialize_body_from_config :: proc(body: ^Body, config: Body_Config) {
 	body.x = config.initial_pos
 	body.q = config.initial_rot
 	body.v = config.initial_vel
 	body.w = config.initial_ang_vel
-	body.inv_mass = 1.0 / config.mass
+	body.shape = config.shape
-	body.inv_inertia_tensor = 1.0 / (config.inertia_tensor * config.mass)
+
+	body.inv_mass, body.inv_inertia_tensor = calculate_body_params_from_config(config)
 }
 
 update_body_from_config :: proc(body: Body_Ptr, config: Body_Config) {
-	body.inv_mass = 1.0 / config.mass
+	body.shape = config.shape
-	body.inv_inertia_tensor = 1.0 / (config.inertia_tensor * config.mass)
+	body.inv_mass, body.inv_inertia_tensor = calculate_body_params_from_config(config)
 }
 
 update_suspension_constraint_from_config :: proc(
@@ -66,6 +96,7 @@ immediate_body :: proc(
 			state.num_referenced_bodies += 1
 		}
 		handle = body.handle
+		update_body_from_config(get_body(scene, handle), config)
 	} else {
 		new_body: Body
 		state.num_referenced_bodies += 1

game/physics/scene.odin

@@ -28,10 +28,24 @@ Body :: struct {
 	inv_mass:           f32,
 	// Moment of inertia
 	inv_inertia_tensor: rl.Vector3,
+	shape:              Collision_Shape,
 	// 
 	next_plus_one:      i32,
 }
 
+Shape_Sphere :: struct {
+	radius: f32,
+}
+
+Shape_Box :: struct {
+	size: rl.Vector3,
+}
+
+Collision_Shape :: union {
+	Shape_Sphere,
+	Shape_Box,
+}
+
 Suspension_Constraint :: struct {
 	alive:           bool,
 	// Pos relative to the body

game/physics/simulation.odin

@@ -48,7 +48,7 @@ simulate :: proc(scene: ^Scene, state: ^Solver_State, config: Solver_Config, dt:
 	state.accumulated_time += dt
 
 	num_steps := 0
-	for state.accumulated_time >= config.timestep {
+	for state.accumulated_time > config.timestep {
 		num_steps += 1
 		state.accumulated_time -= config.timestep
 
@@ -78,15 +78,17 @@ simulate_step :: proc(scene: ^Scene, config: Solver_Config) {
 		for &body, i in scene.bodies {
 			if body.alive {
 				body_states[i].prev_x = body.x
-				body.v += dt * config.gravity
+				body.v += config.gravity * dt
-				body.x += dt * body.v
+				body.x += body.v * dt
 
 				body_states[i].prev_q = body.q
 
 				// TODO: Probably can do it using built in quaternion math but I have no idea how it works
 				// NOTE: figure out how this works https://fgiesen.wordpress.com/2012/08/24/quaternion-differentiation/
 				q := body.q
-				delta_rot := quaternion(x = body.w.x, y = body.w.y, z = body.w.z, w = 0)
+
+				w := body.w
+				delta_rot := quaternion(x = w.x, y = w.y, z = w.z, w = 0)
 				delta_rot = delta_rot * q
 				q.x += 0.5 * dt * delta_rot.x
 				q.y += 0.5 * dt * delta_rot.y
@@ -102,15 +104,13 @@ simulate_step :: proc(scene: ^Scene, config: Solver_Config) {
 			if v.alive {
 				body := get_body(scene, v.body)
 
-				q := body.q
 				pos := body_local_to_world(body, v.rel_pos)
-				dir := lg.quaternion_mul_vector3(q, v.rel_dir)
+				dir := body_local_to_world_vec(body, v.rel_dir)
 				pos2 := pos + dir * v.rest
 				v.hit_t, v.hit_point, v.hit = collision.intersect_segment_plane(
 					{pos, pos2},
 					collision.plane_from_point_normal({}, collision.Vec3{0, 1, 0}),
 				)
-
 				if v.hit {
 					corr := v.hit_point - pos
 					distance := lg.length(corr)
@@ -139,40 +139,56 @@ simulate_step :: proc(scene: ^Scene, config: Solver_Config) {
 
 				if body.alive && v.hit {
 					wheel_world_pos := body_local_to_world(body, v.rel_pos)
+					dir := body_local_to_world_vec(body, v.rel_dir)
 					body_state := body_states[i32(v.body) - 1]
 
 					// Spring damping
-					{
+					if true {
-						dir := body_local_to_world_vec(body, v.rel_dir)
+						vel_3d := body_velocity_at_local_point(body, wheel_world_pos - body.x)
-						vel_3d := body_velocity_at_local_point(body, v.rel_pos)
 
 						vel := lg.dot(vel_3d, dir)
-						damp_delta := -vel * v.damping * dt * dir
+						damping := -vel * min(v.damping * dt, 1)
+
+						apply_constraint_correction_unilateral(
+							dt,
+							body,
+							0,
+							error = damping,
+							error_gradient = dir,
+							pos = wheel_world_pos,
+						)
 
-						apply_correction(body, damp_delta, wheel_world_pos)
 						body_solve_velocity(body, body_state, inv_dt)
 					}
 
 					// Drive forces
-					{
+					if true {
 						total_impulse := v.drive_impulse - v.brake_impulse
 						forward := body_local_to_world_vec(body, rl.Vector3{0, 0, 1})
 
 						corr := total_impulse * forward * dt
 
+						apply_constraint_correction_unilateral(
+							dt,
+							body,
+							0,
+							total_impulse * dt * dt,
+							forward,
+							wheel_world_pos,
+						)
 						apply_correction(body, corr, wheel_world_pos)
 						body_solve_velocity(body, body_state, inv_dt)
 					}
 
 					// Lateral friction
-					{
+					if true {
 						local_contact_pos := v.hit_point - body.x
 						vel_contact := body_velocity_at_local_point(body, local_contact_pos)
 						right := wheel_get_right_vec(body, v)
 
 						lateral_vel := lg.dot(right, vel_contact)
 
-						friction := f32(0.7)
+						friction := f32(0.5)
 						impulse := -lateral_vel * friction
 						corr := right * impulse * dt
 						v.applied_impulse.x = impulse
@@ -239,7 +255,7 @@ apply_correction :: proc(body: Body_Ptr, corr: rl.Vector3, pos: rl.Vector3) {
 	body.x += corr * body.inv_mass
 
 	q := body.q
-	inv_q := lg.quaternion_inverse(q)
+	inv_q := lg.quaternion_normalize0(lg.quaternion_inverse(q))
 	delta_omega := pos - body.x
 	delta_omega = lg.cross(delta_omega, corr)
 	delta_omega = lg.quaternion_mul_vector3(inv_q, delta_omega)
@@ -259,14 +275,17 @@ apply_correction :: proc(body: Body_Ptr, corr: rl.Vector3, pos: rl.Vector3) {
 
 get_body_inverse_mass :: proc(body: Body_Ptr, normal, pos: rl.Vector3) -> f32 {
 	q := body.q
-	inv_q := lg.quaternion_inverse(q)
+	inv_q := lg.quaternion_normalize0(lg.quaternion_inverse(q))
 
 	rn := pos - body.x
 	rn = lg.cross(rn, normal)
 	rn = lg.quaternion_mul_vector3(inv_q, rn)
-	rn *= rn
 
-	w := lg.dot(rn, body.inv_inertia_tensor)
+	w :=
+		rn.x * rn.x * body.inv_inertia_tensor.x +
+		rn.y * rn.y * body.inv_inertia_tensor.y +
+		rn.z * rn.z * body.inv_inertia_tensor.z
+
 	w += body.inv_mass
 
 	return w