Start of the physics engine with immediate mode api (waaat)

build_debug.sh

@@ -1,3 +1,3 @@
 #!/usr/bin/env bash
 
-odin build main_release -out:game_debug.bin -strict-style -vet -debug
+odin build main_release -collection:common=./common -collection:game=./game  -out:game_debug.bin -strict-style -vet -debug

build_hot_reload.sh

@@ -35,7 +35,7 @@ esac
 
 # Build the game.
 echo "Building game$DLL_EXT"
-odin build game -extra-linker-flags:"$EXTRA_LINKER_FLAGS" -define:RAYLIB_SHARED=true -collection:common=./common -build-mode:dll -out:game_tmp$DLL_EXT -strict-style -vet -debug
+odin build game -extra-linker-flags:"$EXTRA_LINKER_FLAGS" -define:RAYLIB_SHARED=true -collection:common=./common -collection:game=./game -build-mode:dll -out:game_tmp$DLL_EXT -strict-style -vet -debug
 
 # Need to use a temp file on Linux because it first writes an empty `game.so`, which the game will load before it is actually fully written.
 mv game_tmp$DLL_EXT game$DLL_EXT

game/game.odin

@@ -20,6 +20,7 @@ import "core:fmt"
 import "core:log"
 import "core:math"
 import "core:math/linalg"
+import "game:physics"
 import rl "vendor:raylib"
 import "vendor:raylib/rlgl"
 
@@ -30,7 +31,22 @@ Track :: struct {
 }
 
 World :: struct {
-	track: Track,
+	track:         Track,
+	physics_scene: physics.Scene,
+}
+
+destroy_world :: proc(world: ^World) {
+	delete(world.track.points)
+	physics.destroy_physics_scene(&world.physics_scene)
+}
+
+Car :: struct {
+	pos: rl.Vector3,
+}
+
+SOLVER_CONFIG :: physics.Solver_Config {
+	timestep = 1.0 / 120,
+	gravity  = rl.Vector3{0, 0, 0},
 }
 
 Game_Memory :: struct {
@@ -38,6 +54,9 @@ Game_Memory :: struct {
 	player_pos:       rl.Vector3,
 	camera_yaw_pitch: rl.Vector2,
 	camera_speed:     f32,
+	camera:           rl.Camera3D,
+	solver_state:     physics.Solver_State,
+	car_handle:       physics.Body_Handle,
 	es:               Editor_State,
 	editor:           bool,
 }
@@ -92,13 +111,17 @@ camera_forward_vec :: proc() -> rl.Vector3 {
 }
 
 game_camera_3d :: proc() -> rl.Camera3D {
-	return {
+	if g_mem.editor {
-		position = g_mem.player_pos,
+		return {
-		up = {0, 1, 0},
+			position = g_mem.player_pos,
-		fovy = 60,
+			up = {0, 1, 0},
-		target = g_mem.player_pos + camera_forward_vec(),
+			fovy = 60,
-		projection = .PERSPECTIVE,
+			target = g_mem.player_pos + camera_forward_vec(),
+			projection = .PERSPECTIVE,
+		}
 	}
+
+	return g_mem.camera
 }
 
 ui_camera :: proc() -> rl.Camera2D {
@@ -314,25 +337,107 @@ update_editor :: proc() {
 			}
 		}
 	}
-
 }
 
 update :: proc() {
 	if rl.IsKeyPressed(.TAB) {
 		g_mem.editor = !g_mem.editor
 
-		if g_mem.editor {
+		// if g_mem.editor {
-			rl.EnableCursor()
+		// 	rl.EnableCursor()
-		} else {
+		// } else {
-			rl.DisableCursor()
+		// 	rl.DisableCursor()
-		}
+		// }
 	}
 
-	if g_mem.editor {
+	dt := rl.GetFrameTime()
+
+	if !g_mem.editor {
+		car_model := assets.get_model(&g_mem.assetman, "assets/toyota_corolla_ae86_trueno.glb")
+		car_bounds := rl.GetModelBoundingBox(car_model)
+
+		g_mem.car_handle = physics.immediate_body(
+			&get_world().physics_scene,
+			&g_mem.solver_state,
+			#hash("car", "fnv32a"),
+			physics.Body_Config {
+				initial_pos = {0, 1, 0},
+				initial_rot = linalg.QUATERNIONF32_IDENTITY,
+				initial_ang_vel = {0, 1, 0},
+				mass = 100,
+				inertia_tensor = physics.inertia_tensor_box(car_bounds.max - car_bounds.min),
+			},
+		)
+
+		g_mem.camera.up = rl.Vector3{0, 1, 0}
+		g_mem.camera.fovy = 60
+		g_mem.camera.projection = .PERSPECTIVE
+		g_mem.camera.target = physics.get_body(&get_world().physics_scene, g_mem.car_handle).x
+		if g_mem.camera.position == {} {
+			g_mem.camera.position = g_mem.camera.target - rl.Vector3{0, 0, 10}
+		}
+
+		// 1.6 is a good value
+		wheel_extent_x := f32(2)
+		rest := f32(1)
+
+		physics.immediate_suspension_constraint(
+			&get_world().physics_scene,
+			&g_mem.solver_state,
+			#hash("FL", "fnv32a"),
+			{
+				rel_pos = {-wheel_extent_x, 0, 2.5},
+				rel_dir = {0, -1, 0},
+				rest = rest,
+				body = g_mem.car_handle,
+			},
+		)
+		physics.immediate_suspension_constraint(
+			&get_world().physics_scene,
+			&g_mem.solver_state,
+			#hash("FR", "fnv32a"),
+			{
+				rel_pos = {wheel_extent_x, 0, 2.5},
+				rel_dir = {0, -1, 0},
+				rest = rest,
+				body = g_mem.car_handle,
+			},
+		)
+		physics.immediate_suspension_constraint(
+			&get_world().physics_scene,
+			&g_mem.solver_state,
+			#hash("RL", "fnv32a"),
+			{
+				rel_pos = {-wheel_extent_x, 0, -3},
+				rel_dir = {0, -1, 0},
+				rest = rest,
+				body = g_mem.car_handle,
+			},
+		)
+		physics.immediate_suspension_constraint(
+			&get_world().physics_scene,
+			&g_mem.solver_state,
+			#hash("RR", "fnv32a"),
+			{
+				rel_pos = {wheel_extent_x, 0, -3},
+				rel_dir = {0, -1, 0},
+				rest = rest,
+				body = g_mem.car_handle,
+			},
+		)
+
+	} else {
 		update_free_look_camera()
 
 		update_editor()
 	}
+
+	physics.simulate(
+		&g_mem.es.world.physics_scene,
+		&g_mem.solver_state,
+		SOLVER_CONFIG,
+		g_mem.editor ? 0 : dt,
+	)
 }
 
 catmull_rom_coefs :: proc(
@@ -384,22 +489,32 @@ draw :: proc() {
 		defer rl.EndMode3D()
 
 		if collision.hit {
-			tangent, bitangent := get_point_frame(interpolated_points, segment_idx)
+			// tangent, bitangent := get_point_frame(interpolated_points, segment_idx)
 
-			rot_matrix: linalg.Matrix3f32
+			// rot_matrix: linalg.Matrix3f32
-			rot_matrix[0] = bitangent
+			// rot_matrix[0] = bitangent
-			rot_matrix[1] = interpolated_points[segment_idx].normal
+			// rot_matrix[1] = interpolated_points[segment_idx].normal
-			rot_matrix[2] = -tangent
+			// rot_matrix[2] = -tangent
 
-			angle, axis := linalg.angle_axis_from_quaternion(
+			// angle, axis := linalg.angle_axis_from_quaternion(
-				linalg.quaternion_from_matrix3(rot_matrix),
+			// 	linalg.quaternion_from_matrix3(rot_matrix),
-			)
+			// )
-			car_model := assets.get_model(&g_mem.assetman, "assets/toyota_corolla_ae86_trueno.glb")
-			car_model.transform = rl.MatrixRotate(axis, angle)
-
-			rl.DrawModel(car_model, collision.point, 1, rl.WHITE)
 		}
 
+		rl.DrawGrid(100, 1)
+
+		if !g_mem.editor {
+			car_model := assets.get_model(&g_mem.assetman, "assets/toyota_corolla_ae86_trueno.glb")
+
+			car_body := physics.get_body(&get_world().physics_scene, g_mem.car_handle)
+			car_matrix := rl.QuaternionToMatrix(car_body.q)
+			car_model.transform = car_matrix
+
+			rl.DrawModel(car_model, car_body.x, 1, rl.WHITE)
+		}
+
+		physics.draw_debug_scene(&get_world().physics_scene)
+
 		// road: rl.Mesh
 		// defer rl.UnloadMesh(road)
 		// road_vertices: [dynamic]f32
@@ -479,6 +594,9 @@ draw :: proc() {
 					rl.ORANGE,
 				)
 			}
+		} else {
+			car_pos := physics.get_body(&get_world().physics_scene, g_mem.car_handle).x
+			rl.DrawText(fmt.ctprintf("Car Pos: %v", car_pos), 5, 32, 8, rl.ORANGE)
 		}
 	}
 
@@ -638,7 +756,7 @@ game_init_window :: proc() {
 	rl.InitWindow(1280, 720, "Odin + Raylib + Hot Reload template!")
 	rl.SetExitKey(.KEY_NULL)
 	rl.SetWindowPosition(200, 200)
-	rl.SetTargetFPS(500)
+	rl.SetTargetFPS(60)
 }
 
 @(export)
@@ -653,8 +771,10 @@ game_init :: proc() {
 @(export)
 game_shutdown :: proc() {
 	assets.shutdown(&g_mem.assetman)
-	delete(get_world().track.points)
+	destroy_world(get_world())
 	delete(g_mem.es.point_selection)
+	physics.destroy_solver_state(&g_mem.solver_state)
+
 	free(g_mem)
 }
 

game/physics/debug.odin

@@ -0,0 +1,42 @@
+package physics
+
+import "core:log"
+import lg "core:math/linalg"
+import rl "vendor:raylib"
+
+_ :: log
+
+draw_debug_scene :: proc(scene: ^Scene) {
+	for &body in scene.bodies {
+		if body.alive {
+			pos := body.x
+
+			q := body.q
+			x := lg.quaternion_mul_vector3(q, rl.Vector3{1, 0, 0})
+			y := lg.quaternion_mul_vector3(q, rl.Vector3{0, 1, 0})
+			z := lg.quaternion_mul_vector3(q, rl.Vector3{0, 0, 1})
+
+			rl.DrawLine3D(pos, pos + x, rl.RED)
+			rl.DrawLine3D(pos, pos + y, rl.GREEN)
+			rl.DrawLine3D(pos, pos + z, rl.BLUE)
+		}
+	}
+
+	for &constraint in scene.suspension_constraints {
+		if constraint.alive {
+			body := get_body(scene, constraint.body)
+			t := constraint.hit_t > 0 ? constraint.hit_t : constraint.rest
+
+			pos := body.x
+			rot := body.q
+			pos += lg.quaternion_mul_vector3(rot, constraint.rel_pos)
+			dir := lg.quaternion_mul_vector3(rot, constraint.rel_dir)
+
+			rl.DrawLine3D(pos, pos + dir * t, rl.ORANGE)
+
+			if constraint.hit {
+				rl.DrawSphereWires(constraint.hit_point, 0.1, 4, 4, rl.RED)
+			}
+		}
+	}
+}

game/physics/helpers.odin

@@ -0,0 +1,15 @@
+package physics
+
+import rl "vendor:raylib"
+
+inertia_tensor_box :: proc(size: rl.Vector3) -> (tensor: rl.Vector3) {
+	CONSTANT :: f32(1.0 / 12.0)
+
+	tensor.x = size.z * size.z + size.y * size.y
+	tensor.y = size.x * size.x + size.z * size.z
+	tensor.z = size.x * size.x + size.y * size.y
+
+	tensor *= CONSTANT
+
+	return
+}

game/physics/immediate.odin

@@ -0,0 +1,168 @@
+package physics
+
+import rl "vendor:raylib"
+
+// 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,
+	mass:            f32,
+	// Unit inertia tensor
+	inertia_tensor:  rl.Vector3,
+}
+
+Suspension_Constraint_Config :: struct {
+	rel_pos:    rl.Vector3,
+	rel_dir:    rl.Vector3,
+	body:       Body_Handle,
+	rest:       f32,
+	compliance: f32,
+}
+
+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.inv_intertia_tensor = 1.0 / (config.inertia_tensor * config.mass)
+}
+
+update_body_from_config :: proc(body: Body_Ptr, config: Body_Config) {
+	body.inv_mass = 1.0 / config.mass
+	body.inv_intertia_tensor = 1.0 / (config.inertia_tensor * config.mass)
+}
+
+update_suspension_constraint_from_config :: proc(
+	constraint: Suspension_Constraint_Ptr,
+	config: Suspension_Constraint_Config,
+) {
+	constraint.rel_pos = config.rel_pos
+	constraint.rel_dir = config.rel_dir
+	constraint.body = config.body
+	constraint.rest = config.rest
+	constraint.compliance = config.compliance
+}
+
+immediate_body :: proc(
+	scene: ^Scene,
+	state: ^Solver_State,
+	id: u32,
+	config: Body_Config,
+) -> (
+	handle: Body_Handle,
+) {
+	if id in state.immedate_bodies {
+		body := &state.immedate_bodies[id]
+		if body.last_ref != state.simulation_frame {
+			body.last_ref = state.simulation_frame
+			state.num_referenced_bodies += 1
+		}
+		handle = body.handle
+	} else {
+		new_body: Body
+		state.num_referenced_bodies += 1
+		initialize_body_from_config(&new_body, config)
+		handle = add_body(scene, new_body)
+		state.immedate_bodies[id] = {
+			handle   = handle,
+			last_ref = state.simulation_frame,
+		}
+	}
+
+	return
+}
+
+immediate_suspension_constraint :: proc(
+	scene: ^Scene,
+	state: ^Solver_State,
+	id: u32,
+	config: Suspension_Constraint_Config,
+) -> (
+	handle: Suspension_Constraint_Handle,
+) {
+	if id in state.immediate_suspension_constraints {
+		constraint := &state.immediate_suspension_constraints[id]
+		if constraint.last_ref != state.simulation_frame {
+			constraint.last_ref = state.simulation_frame
+			state.num_referenced_suspension_constraints += 1
+		}
+		handle = constraint.handle
+	} else {
+		state.num_referenced_suspension_constraints += 1
+		handle = add_suspension_constraint(scene, {})
+		state.immediate_suspension_constraints[id] = {
+			handle   = handle,
+			last_ref = state.simulation_frame,
+		}
+	}
+
+	update_suspension_constraint_from_config(get_suspension_constraint(scene, handle), config)
+
+	return
+}
+
+prune_immediate :: proc(scene: ^Scene, state: ^Solver_State) {
+	prune_immediate_bodies(scene, state)
+	prune_immediate_suspension_constraints(scene, state)
+}
+
+// TODO: Generic version
+prune_immediate_bodies :: proc(scene: ^Scene, state: ^Solver_State) {
+	if int(state.num_referenced_bodies) == len(state.immedate_bodies) {
+		return
+	}
+
+	num_unreferenced_bodies := len(state.immedate_bodies) - int(state.num_referenced_bodies)
+	assert(num_unreferenced_bodies >= 0)
+
+	bodies_to_remove := make([]u32, num_unreferenced_bodies, context.temp_allocator)
+
+	i := 0
+	for k, &v in state.immedate_bodies {
+		if v.last_ref != state.simulation_frame {
+			bodies_to_remove[i] = k
+			i += 1
+		}
+	}
+
+	assert(i == len(bodies_to_remove))
+
+	for k in bodies_to_remove {
+		handle := state.immedate_bodies[k].handle
+		delete_key(&state.immedate_bodies, k)
+		remove_body(scene, handle)
+	}
+}
+
+prune_immediate_suspension_constraints :: proc(scene: ^Scene, state: ^Solver_State) {
+	if int(state.num_referenced_suspension_constraints) ==
+	   len(state.immediate_suspension_constraints) {
+		return
+	}
+
+	num_unreferenced_constraints :=
+		len(state.immediate_suspension_constraints) -
+		int(state.num_referenced_suspension_constraints)
+	assert(num_unreferenced_constraints >= 0)
+
+	constraints_to_remove := make([]u32, num_unreferenced_constraints, context.temp_allocator)
+
+	i := 0
+	for k, &v in state.immediate_suspension_constraints {
+		if v.last_ref != state.simulation_frame {
+			constraints_to_remove[i] = k
+			i += 1
+		}
+	}
+
+	assert(i == len(constraints_to_remove))
+
+	for k in constraints_to_remove {
+		handle := state.immediate_suspension_constraints[k].handle
+		delete_key(&state.immediate_suspension_constraints, k)
+		remove_suspension_constraint(scene, handle)
+	}
+}

game/physics/physics.odin

@@ -1 +0,0 @@
-package physics

game/physics/scene.odin

@@ -0,0 +1,172 @@
+package physics
+
+import rl "vendor:raylib"
+
+Scene :: struct {
+	bodies:                                    #soa[dynamic]Body,
+	suspension_constraints:                    #soa[dynamic]Suspension_Constraint,
+	first_free_body_plus_one:                  i32,
+	first_free_suspension_constraint_plus_one: i32,
+}
+
+Body :: struct {
+	// Is this body alive (if not it doesn't exist)
+	alive:               bool,
+	// Pos
+	x:                   rl.Vector3,
+	// Linear vel
+	v:                   rl.Vector3,
+	// Orientation
+	q:                   rl.Quaternion,
+	// Angular vel (omega)
+	w:                   rl.Vector3,
+	// Mass
+	inv_mass:            f32,
+	// Moment of inertia
+	inv_intertia_tensor: rl.Vector3,
+	// 
+	next_plus_one:       i32,
+}
+
+Suspension_Constraint :: struct {
+	alive:         bool,
+	// Pos relative to the body
+	rel_pos:       rl.Vector3,
+	// Dir relative to the body
+	rel_dir:       rl.Vector3,
+	// Handle of the rigid body
+	body:          Body_Handle,
+	// Rest distance
+	rest:          f32,
+	// Inverse stiffness
+	compliance:    f32,
+
+	// Runtime state
+	hit:           bool,
+	hit_point:     rl.Vector3,
+	// rel_hit_point = rel_pos + rel_dir * hit_t
+	hit_t:         f32,
+
+	// Free list
+	next_plus_one: i32,
+}
+
+// Index plus one, so handle 0 maps to invalid body
+Body_Handle :: distinct i32
+Suspension_Constraint_Handle :: distinct i32
+
+is_body_handle_valid :: proc(handle: Body_Handle) -> bool {
+	return i32(handle) > 0
+}
+is_suspension_constraint_handle_valid :: proc(handle: Suspension_Constraint_Handle) -> bool {
+	return i32(handle) > 0
+}
+is_handle_valid :: proc {
+	is_body_handle_valid,
+	is_suspension_constraint_handle_valid,
+}
+
+Body_Ptr :: #soa^#soa[]Body
+Suspension_Constraint_Ptr :: #soa^#soa[]Suspension_Constraint
+
+_invalid_body: #soa[1]Body
+_invalid_suspension_constraint: #soa[1]Suspension_Constraint
+
+/// Returns pointer to soa slice. NEVER STORE IT
+get_body :: proc(scene: ^Scene, handle: Body_Handle) -> Body_Ptr {
+	index := int(handle) - 1
+	if index < 0 {
+		slice := _invalid_body[:]
+		return &slice[0]
+	}
+
+	bodies_slice := scene.bodies[:]
+	return &bodies_slice[index]
+}
+
+add_body :: proc(scene: ^Scene, body: Body) -> Body_Handle {
+	body_copy := body
+
+	body_copy.alive = true
+	body_copy.next_plus_one = 0
+
+	if scene.first_free_body_plus_one > 1 {
+		index := scene.first_free_body_plus_one
+		new_body := get_body(scene, Body_Handle(index))
+		next_plus_one := new_body.next_plus_one
+		new_body^ = body_copy
+		scene.first_free_body_plus_one = next_plus_one
+		return Body_Handle(index)
+	}
+
+	append_soa(&scene.bodies, body_copy)
+	index := len(scene.bodies)
+	return Body_Handle(index)
+}
+
+remove_body :: proc(scene: ^Scene, handle: Body_Handle) {
+	if int(handle) > 1 {
+		body := get_body(scene, handle)
+
+		body.alive = false
+		body.next_plus_one = scene.first_free_body_plus_one
+		scene.first_free_body_plus_one = i32(handle)
+	}
+}
+
+/// Returns pointer to soa slice. NEVER STORE IT
+get_suspension_constraint :: proc(
+	scene: ^Scene,
+	handle: Suspension_Constraint_Handle,
+) -> Suspension_Constraint_Ptr {
+	if !is_handle_valid(handle) {
+		slice := _invalid_suspension_constraint[:]
+		return &slice[0]
+	}
+
+	index := int(handle) - 1
+	slice := scene.suspension_constraints[:]
+	return &slice[index]
+}
+
+add_suspension_constraint :: proc(
+	scene: ^Scene,
+	constraint: Suspension_Constraint,
+) -> Suspension_Constraint_Handle {
+	copy := constraint
+
+	copy.alive = true
+	copy.next_plus_one = 0
+
+	if scene.first_free_suspension_constraint_plus_one > 0 {
+		index := scene.first_free_suspension_constraint_plus_one
+		new_constraint := get_suspension_constraint(scene, Suspension_Constraint_Handle(index))
+		next_plus_one := new_constraint.next_plus_one
+		new_constraint^ = copy
+		scene.first_free_suspension_constraint_plus_one = next_plus_one
+		return Suspension_Constraint_Handle(index)
+	}
+
+	append_soa(&scene.suspension_constraints, copy)
+	index := len(scene.suspension_constraints)
+	return Suspension_Constraint_Handle(index)
+}
+
+remove_suspension_constraint :: proc(scene: ^Scene, handle: Suspension_Constraint_Handle) {
+	if is_handle_valid(handle) {
+		constraint := get_suspension_constraint(scene, handle)
+
+		constraint.alive = false
+		constraint.next_plus_one = scene.first_free_suspension_constraint_plus_one
+		scene.first_free_suspension_constraint_plus_one = i32(handle)
+	}
+}
+
+_get_first_free_body :: proc(scene: ^Scene) -> i32 {
+	return scene.first_free_body_plus_one - 1
+}
+
+destroy_physics_scene :: proc(scene: ^Scene) {
+	delete_soa(scene.bodies)
+	delete_soa(scene.suspension_constraints)
+}

game/physics/simulation.odin

@@ -0,0 +1,120 @@
+package physics
+
+import "collision"
+import lg "core:math/linalg"
+import rl "vendor:raylib"
+
+Solver_Config :: struct {
+	// Will automatically do fixed timestep
+	timestep: f32,
+	gravity:  rl.Vector3,
+}
+
+Solver_State :: struct {
+	accumulated_time:                      f32,
+	// Incremented when simulate is called (not simulate_step)
+	simulation_frame:                      u32,
+
+	// Number of immediate bodies referenced this frame
+	num_referenced_bodies:                 i32,
+	num_referenced_suspension_constraints: i32,
+	immedate_bodies:                       map[u32]Immedate_State(Body_Handle),
+	immediate_suspension_constraints:      map[u32]Immedate_State(Suspension_Constraint_Handle),
+}
+
+Immedate_State :: struct($T: typeid) {
+	handle:   T,
+	// When was this referenced last time (frame number)
+	last_ref: u32,
+}
+
+destroy_solver_state :: proc(state: ^Solver_State) {
+	delete(state.immedate_bodies)
+}
+
+// Outer simulation loop for fixed timestepping
+simulate :: proc(scene: ^Scene, state: ^Solver_State, config: Solver_Config, dt: f32) {
+	assert(config.timestep > 0)
+
+	prune_immediate(scene, state)
+
+	state.accumulated_time += dt
+
+	num_steps := 0
+	for state.accumulated_time >= config.timestep {
+		num_steps += 1
+		state.accumulated_time -= config.timestep
+
+		simulate_step(scene, config)
+	}
+
+	state.simulation_frame += 1
+	state.num_referenced_bodies = 0
+	state.num_referenced_suspension_constraints = 0
+}
+
+Body_Sim_State :: struct {
+	prev_x: rl.Vector3,
+	prev_q: rl.Quaternion,
+}
+
+simulate_step :: proc(scene: ^Scene, config: Solver_Config) {
+	body_states := make_soa(#soa[]Body_Sim_State, len(scene.bodies), context.temp_allocator)
+
+	dt := config.timestep
+	inv_dt := 1.0 / dt
+
+	// Integrate positions and rotations
+	for &body, i in scene.bodies {
+		if body.alive {
+			body_states[i].prev_x = body.x
+			body.v += dt * config.gravity
+			body.x += dt * body.v
+
+			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)
+			delta_rot = delta_rot * q
+			q.x += 0.5 * dt * delta_rot.x
+			q.y += 0.5 * dt * delta_rot.y
+			q.z += 0.5 * dt * delta_rot.z
+			q.w += 0.5 * dt * delta_rot.w
+			q = lg.normalize0(q)
+
+			body.q = q
+		}
+	}
+
+	for &v in scene.suspension_constraints {
+		if v.alive {
+			body := get_body(scene, v.body)
+
+			q := body.q
+			pos := body.x
+			pos += lg.quaternion_mul_vector3(q, v.rel_pos)
+			dir := lg.quaternion_mul_vector3(q, 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}),
+			)
+		}
+	}
+
+	// Compute new linear and angular velocities
+	for &body, i in scene.bodies {
+		if body.alive {
+			body.v = (body.x - body_states[i].prev_x) * inv_dt
+
+			delta_q := body.q * lg.quaternion_inverse(body_states[i].prev_q)
+			body.w = rl.Vector3{delta_q.x, delta_q.y, delta_q.z} * 2.0 * inv_dt
+
+			if delta_q.w < 0 {
+				body.w = -body.w
+			}
+		}
+	}
+}