My own rigid body phyiscs that works, wat

game/game.odin

@@ -46,7 +46,7 @@ Car :: struct {
 
 SOLVER_CONFIG :: physics.Solver_Config {
 	timestep = 1.0 / 120,
-	gravity  = rl.Vector3{0, 0, 0},
+	gravity  = rl.Vector3{0, -9.8, 0},
 }
 
 Game_Memory :: struct {
@@ -363,7 +363,7 @@ update :: proc() {
 			physics.Body_Config {
 				initial_pos = {0, 1, 0},
 				initial_rot = linalg.QUATERNIONF32_IDENTITY,
-				initial_ang_vel = {0, 1, 0},
+				initial_ang_vel = {0, 0, 0},
 				mass = 100,
 				inertia_tensor = physics.inertia_tensor_box(car_bounds.max - car_bounds.min),
 			},
@@ -374,12 +374,14 @@ update :: proc() {
 		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}
+			g_mem.camera.position = g_mem.camera.target - rl.Vector3{10, 0, 10}
 		}
 
 		// 1.6 is a good value
-		wheel_extent_x := f32(2)
+		wheel_extent_x := f32(2.0)
-		rest := f32(1)
+		rest := f32(0.9)
+		suspension_stiffness := f32(10000)
+		compliance := 1.0 / suspension_stiffness
 
 		physics.immediate_suspension_constraint(
 			&get_world().physics_scene,
@@ -389,6 +391,7 @@ update :: proc() {
 				rel_pos = {-wheel_extent_x, 0, 2.5},
 				rel_dir = {0, -1, 0},
 				rest = rest,
+				compliance = compliance,
 				body = g_mem.car_handle,
 			},
 		)
@@ -400,6 +403,7 @@ update :: proc() {
 				rel_pos = {wheel_extent_x, 0, 2.5},
 				rel_dir = {0, -1, 0},
 				rest = rest,
+				compliance = compliance,
 				body = g_mem.car_handle,
 			},
 		)
@@ -411,6 +415,7 @@ update :: proc() {
 				rel_pos = {-wheel_extent_x, 0, -3},
 				rel_dir = {0, -1, 0},
 				rest = rest,
+				compliance = compliance,
 				body = g_mem.car_handle,
 			},
 		)
@@ -422,6 +427,7 @@ update :: proc() {
 				rel_pos = {wheel_extent_x, 0, -3},
 				rel_dir = {0, -1, 0},
 				rest = rest,
+				compliance = compliance,
 				body = g_mem.car_handle,
 			},
 		)
@@ -484,6 +490,8 @@ draw :: proc() {
 		rl.GetScreenToWorldRay(rl.GetMousePosition(), camera),
 	)
 
+	car_model := assets.get_model(&g_mem.assetman, "assets/toyota_corolla_ae86_trueno.glb")
+
 	{
 		rl.BeginMode3D(camera)
 		defer rl.EndMode3D()
@@ -504,7 +512,6 @@ draw :: proc() {
 		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)
@@ -596,7 +603,13 @@ draw :: proc() {
 			}
 		} 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)
+			rl.DrawText(
+				fmt.ctprintf("Car Pos: %v. Mesh count: %v", car_pos, car_model.meshCount),
+				5,
+				32,
+				8,
+				rl.ORANGE,
+			)
 		}
 	}
 

game/physics/helpers.odin

@@ -1,5 +1,6 @@
 package physics
 
+import lg "core:math/linalg"
 import rl "vendor:raylib"
 
 inertia_tensor_box :: proc(size: rl.Vector3) -> (tensor: rl.Vector3) {
@@ -13,3 +14,13 @@ inertia_tensor_box :: proc(size: rl.Vector3) -> (tensor: rl.Vector3) {
 
 	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_world_to_local :: #force_inline proc(body: Body_Ptr, pos: rl.Vector3) -> rl.Vector3 {
+	// TODO: maybe store that
+	inv_q := lg.quaternion_inverse(body.q)
+	return lg.quaternion_mul_vector3(inv_q, pos - body.x)
+}

game/physics/immediate.odin

@@ -27,12 +27,12 @@ initialize_body_from_config :: proc(body: ^Body, config: Body_Config) {
 	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)
+	body.inv_inertia_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)
+	body.inv_inertia_tensor = 1.0 / (config.inertia_tensor * config.mass)
 }
 
 update_suspension_constraint_from_config :: proc(

game/physics/scene.odin

@@ -7,25 +7,29 @@ Scene :: struct {
 	suspension_constraints:                    #soa[dynamic]Suspension_Constraint,
 	first_free_body_plus_one:                  i32,
 	first_free_suspension_constraint_plus_one: i32,
+
+	// Slices. When you call get_body or get_suspension_constraint you will get a pointer to an element in this slice
+	bodies_slice:                              #soa[]Body,
+	suspension_constraints_slice:              #soa[]Suspension_Constraint,
 }
 
 Body :: struct {
 	// Is this body alive (if not it doesn't exist)
-	alive:               bool,
+	alive:              bool,
 	// Pos
-	x:                   rl.Vector3,
+	x:                  rl.Vector3,
 	// Linear vel
-	v:                   rl.Vector3,
+	v:                  rl.Vector3,
 	// Orientation
-	q:                   rl.Quaternion,
+	q:                  rl.Quaternion,
 	// Angular vel (omega)
-	w:                   rl.Vector3,
+	w:                  rl.Vector3,
 	// Mass
-	inv_mass:            f32,
+	inv_mass:           f32,
 	// Moment of inertia
-	inv_intertia_tensor: rl.Vector3,
+	inv_inertia_tensor: rl.Vector3,
 	// 
-	next_plus_one:       i32,
+	next_plus_one:      i32,
 }
 
 Suspension_Constraint :: struct {
@@ -55,6 +59,9 @@ Suspension_Constraint :: struct {
 Body_Handle :: distinct i32
 Suspension_Constraint_Handle :: distinct i32
 
+INVALID_BODY :: Body_Handle(0)
+INVALID_SUSPENSION_CONSTRAINT :: Suspension_Constraint_Handle(0)
+
 is_body_handle_valid :: proc(handle: Body_Handle) -> bool {
 	return i32(handle) > 0
 }
@@ -70,18 +77,19 @@ Body_Ptr :: #soa^#soa[]Body
 Suspension_Constraint_Ptr :: #soa^#soa[]Suspension_Constraint
 
 _invalid_body: #soa[1]Body
+_invalid_body_slice: #soa[]Body
+
 _invalid_suspension_constraint: #soa[1]Suspension_Constraint
+_invalid_suspension_constraint_slice := _invalid_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 &_invalid_body_slice[0]
-		return &slice[0]
 	}
 
-	bodies_slice := scene.bodies[:]
+	return &scene.bodies_slice[index]
-	return &bodies_slice[index]
 }
 
 add_body :: proc(scene: ^Scene, body: Body) -> Body_Handle {
@@ -101,6 +109,9 @@ add_body :: proc(scene: ^Scene, body: Body) -> Body_Handle {
 
 	append_soa(&scene.bodies, body_copy)
 	index := len(scene.bodies)
+
+	scene.bodies_slice = scene.bodies[:]
+
 	return Body_Handle(index)
 }
 
@@ -120,13 +131,11 @@ get_suspension_constraint :: proc(
 	handle: Suspension_Constraint_Handle,
 ) -> Suspension_Constraint_Ptr {
 	if !is_handle_valid(handle) {
-		slice := _invalid_suspension_constraint[:]
+		return &_invalid_suspension_constraint_slice[0]
-		return &slice[0]
 	}
 
 	index := int(handle) - 1
-	slice := scene.suspension_constraints[:]
+	return &scene.suspension_constraints_slice[index]
-	return &slice[index]
 }
 
 add_suspension_constraint :: proc(
@@ -148,6 +157,7 @@ add_suspension_constraint :: proc(
 	}
 
 	append_soa(&scene.suspension_constraints, copy)
+	scene.suspension_constraints_slice = scene.suspension_constraints[:]
 	index := len(scene.suspension_constraints)
 	return Suspension_Constraint_Handle(index)
 }

game/physics/simulation.odin

@@ -1,9 +1,12 @@
 package physics
 
 import "collision"
+import "core:math"
 import lg "core:math/linalg"
 import rl "vendor:raylib"
 
+_ :: math
+
 Solver_Config :: struct {
 	// Will automatically do fixed timestep
 	timestep: f32,
@@ -93,14 +96,29 @@ simulate_step :: proc(scene: ^Scene, config: Solver_Config) {
 			body := get_body(scene, v.body)
 
 			q := body.q
-			pos := body.x
+			pos := body_local_to_world(body, v.rel_pos)
-			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}),
 			)
+
+			if v.hit {
+				corr := v.hit_point - pos
+				distance := lg.length(corr)
+				corr = corr / distance if distance > 0 else 0
+
+				apply_constraint_correction_unilateral(
+					dt,
+					body,
+					v.compliance,
+					error = distance - v.rest,
+					error_gradient = corr,
+					pos = pos,
+					other_combined_inv_mass = 0,
+				)
+			}
 		}
 	}
 
@@ -118,3 +136,68 @@ simulate_step :: proc(scene: ^Scene, config: Solver_Config) {
 		}
 	}
 }
+
+apply_constraint_correction_unilateral :: proc(
+	dt: f32,
+	body: Body_Ptr,
+	compliance: f32,
+	error: f32,
+	error_gradient: rl.Vector3,
+	pos: rl.Vector3,
+	other_combined_inv_mass: f32 = 0,
+) {
+	if error == 0 {
+		return
+	}
+
+	w := get_body_inverse_mass(body, error_gradient, pos)
+	w += other_combined_inv_mass
+
+	if w == 0 {
+		return
+	}
+
+	alpha := compliance / dt / dt
+	lambda := -error / (w + alpha)
+
+	delta_pos := error_gradient * -lambda
+
+	apply_correction(body, delta_pos, pos)
+}
+
+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)
+	delta_omega := pos - body.x
+	delta_omega = lg.cross(delta_omega, corr)
+	delta_omega = lg.quaternion_mul_vector3(inv_q, delta_omega)
+	delta_omega *= body.inv_inertia_tensor
+	delta_omega = lg.quaternion_mul_vector3(q, delta_omega)
+
+	delta_rot := quaternion(x = delta_omega.x, y = delta_omega.y, z = delta_omega.z, w = 0)
+	delta_rot *= q
+	q.x += 0.5 * delta_rot.x
+	q.y += 0.5 * delta_rot.y
+	q.z += 0.5 * delta_rot.z
+	q.w += 0.5 * delta_rot.w
+	q = lg.normalize0(q)
+
+	body.q = q
+}
+
+get_body_inverse_mass :: proc(body: Body_Ptr, normal, pos: rl.Vector3) -> f32 {
+	q := body.q
+	inv_q := 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 += body.inv_mass
+
+	return w
+}