Funny boxes flying

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 -define:TRACY_ENABLE=true -collection:libs=./libs -collection:common=./common -collection:game=./game -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 -define:TRACY_ENABLE=true -collection:libs=./libs -collection:common=./common -collection:game=./game -build-mode:dll -out:game_tmp$DLL_EXT -strict-style -vet -debug -o:speed
 
 # 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

@@ -17,9 +17,11 @@ package game
 import "assets"
 import "core:c"
 import "core:fmt"
+import "core:hash"
 import "core:log"
 import "core:math"
 import "core:math/linalg"
+import "core:slice"
 import "game:halfedge"
 import "game:physics"
 import "game:physics/bvh"
@@ -67,7 +69,7 @@ Car :: struct {
 SOLVER_CONFIG :: physics.Solver_Config {
 	timestep            = 1.0 / 120,
 	gravity             = rl.Vector3{0, -9.8, 0},
-	substreps_minus_one = 4 - 1,
+	substreps_minus_one = 1 - 1,
 }
 
 Game_Memory :: struct {
@@ -238,6 +240,7 @@ update_runtime_world :: proc(runtime_world: ^Runtime_World, dt: f32) {
 			#hash("floor", "fnv32a"),
 			physics.Body_Config {
 				initial_pos = {0, -0.5, 0},
+				initial_rot = linalg.QUATERNIONF32_IDENTITY,
 				shape = physics.Shape_Box{size = {100, 1, 100}},
 			},
 		)
@@ -257,6 +260,22 @@ update_runtime_world :: proc(runtime_world: ^Runtime_World, dt: f32) {
 				mass = 100,
 			},
 		)
+		for x in -3 ..< 3 {
+			for y in -3 ..< 3 {
+				physics.immediate_body(
+					&world.physics_scene,
+					&runtime_world.solver_state,
+					hash.fnv32a(slice.to_bytes([]int{(x | y << 8)})),
+					physics.Body_Config {
+						initial_pos = {f32(x), 5, f32(y)},
+						initial_rot = linalg.QUATERNIONF32_IDENTITY,
+						shape = physics.Shape_Box{size = 0.5},
+						mass = 10,
+					},
+				)
+			}
+		}
+
 
 		// car_body := physics.get_body(&world.physics_scene, runtime_world.car_handle)
 
@@ -498,7 +517,7 @@ draw :: proc() {
 		rl.BeginMode3D(camera)
 		defer rl.EndMode3D()
 
-		rl.DrawGrid(100, 1)
+		// rl.DrawGrid(100, 1)
 
 		physics.draw_debug_scene(&world.physics_scene)
 

game/physics/collision/convex.odin

@@ -56,23 +56,30 @@ convex_vs_convex_sat :: proc(a, b: Convex) -> (manifold: Contact_Manifold, colli
 		return
 	}
 
-	edge_separation, edge_a, edge_b := query_separation_edges(a, b)
+	edge_separation, edge_a, edge_b, edge_separating_plane := query_separation_edges(a, b)
 	_, _ = edge_a, edge_b
 	if edge_separation > 0 {
 		return
 	}
-	biased_face_a_separation := face_query_a.separation + 0.1
+	biased_face_a_separation := face_query_a.separation
 	biased_face_b_separation := face_query_b.separation
-	biased_edge_separation := edge_separation - 0.1
+	biased_edge_separation := edge_separation
 
 	is_face_a_contact := biased_face_a_separation >= biased_edge_separation
 	is_face_b_contact := biased_face_b_separation >= biased_edge_separation
 
 	collision = true
-	if is_face_a_contact && is_face_b_contact {
+	if is_face_a_contact || is_face_b_contact {
 		manifold = create_face_contact_manifold(face_query_a, a, face_query_b, b)
 	} else {
-		manifold = create_edge_contact_manifold(a, b, edge_separation, edge_a, edge_b)
+		manifold = create_edge_contact_manifold(
+			a,
+			b,
+			edge_separating_plane,
+			edge_separation,
+			edge_a,
+			edge_b,
+		)
 	}
 
 	return
@@ -158,6 +165,7 @@ query_separation_edges :: proc(
 	separation: f32,
 	a_edge: halfedge.Edge_Index,
 	b_edge: halfedge.Edge_Index,
+	separating_plane: Plane,
 ) {
 	separation = min(f32)
 	a_edge = -1
@@ -165,7 +173,6 @@ query_separation_edges :: proc(
 
 	step := 0
 
-	separating_plane: Plane
 	separating_plane_p: Vec3
 	success_step: int
 
@@ -486,6 +493,7 @@ create_face_contact_manifold :: proc(
 
 create_edge_contact_manifold :: proc(
 	a, b: Convex,
+	separating_plane: Plane,
 	separation: f32,
 	edge_a, edge_b: halfedge.Edge_Index,
 ) -> (
@@ -496,8 +504,8 @@ create_edge_contact_manifold :: proc(
 
 	_, ps := closest_point_between_segments(a1, a2, b1, b2)
 
-	manifold.normal = lg.normalize0(ps[1] - ps[0])
+	manifold.normal = separating_plane.normal
-	manifold.separation = separation
+	manifold.separation = lg.dot(ps[1] - ps[0], manifold.normal)
 	manifold.points[0] = (ps[0] + ps[1]) * 0.5
 	manifold.points_len = 1
 

game/physics/debug.odin

@@ -98,4 +98,25 @@ draw_debug_scene :: proc(scene: ^Scene) {
 			}
 		}
 	}
+
+	for &contact, contact_idx in scene.contact_pairs[:scene.contact_pairs_len] {
+		color := debug.int_to_color(i32(contact_idx))
+		if contact.manifold.points_len >= 3 {
+			// Triangle or quad
+			v1 := contact.manifold.points[0]
+
+			for i in 2 ..< contact.manifold.points_len {
+				v2, v3 := contact.manifold.points[i - 1], contact.manifold.points[i]
+
+				rl.DrawTriangle3D(v1, v2, v3, color)
+			}
+		} else if contact.manifold.points_len == 2 {
+			// Line
+			rl.DrawLine3D(contact.manifold.points[0], contact.manifold.points[1], color)
+		}
+
+		for p in contact.manifold.points[:contact.manifold.points_len] {
+			rl.DrawSphereWires(p, 0.1, 4, 4, color)
+		}
+	}
 }

game/physics/scene.odin

@@ -2,6 +2,8 @@ package physics
 
 import rl "vendor:raylib"
 
+MAX_CONTACTS :: 1024
+
 Scene :: struct {
 	bodies:                                    #soa[dynamic]Body,
 	suspension_constraints:                    #soa[dynamic]Suspension_Constraint,
@@ -11,6 +13,10 @@ Scene :: struct {
 	// 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,
+
+	// Persistent stuff for simulation
+	contact_pairs:                             [MAX_CONTACTS]Contact_Pair,
+	contact_pairs_len:                         int,
 }
 
 Body :: struct {

game/physics/simulation.odin

@@ -40,6 +40,8 @@ Immedate_State :: struct($T: typeid) {
 	last_ref: u32,
 }
 
+MAX_STEPS :: 10
+
 // Outer simulation loop for fixed timestepping
 simulate :: proc(scene: ^Scene, state: ^Solver_State, config: Solver_Config, dt: f32) {
 	assert(config.timestep > 0)
@@ -53,8 +55,10 @@ simulate :: proc(scene: ^Scene, state: ^Solver_State, config: Solver_Config, dt:
 		num_steps += 1
 		state.accumulated_time -= config.timestep
 
+		if num_steps < MAX_STEPS {
 			simulate_step(scene, config)
 		}
+	}
 
 	state.simulation_frame += 1
 	state.num_referenced_bodies = 0
@@ -71,9 +75,16 @@ GLOBAL_PLANE :: collision.Plane {
 	dist   = 0,
 }
 
+Contact_Pair :: struct {
+	a, b:     Body_Handle,
+	manifold: collision.Contact_Manifold,
+}
+
 simulate_step :: proc(scene: ^Scene, config: Solver_Config) {
 	body_states := make([]Body_Sim_State, len(scene.bodies), context.temp_allocator)
 
+	scene.contact_pairs_len = 0
+
 	substeps := config.substreps_minus_one + 1
 
 	dt := config.timestep / f32(substeps)
@@ -84,12 +95,11 @@ 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 += config.gravity * dt
+				body.v += config.gravity * dt * (body.inv_mass == 0 ? 0 : 1) // special case for gravity, TODO
 				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
 
@@ -112,7 +122,7 @@ simulate_step :: proc(scene: ^Scene, config: Solver_Config) {
 				for _, j in scene.bodies {
 					body2 := &scene.bodies_slice[j]
 
-					if body2.alive {
+					if i != j && body2.alive {
 						s1, s2 := body.shape.(Shape_Box), body2.shape.(Shape_Box)
 
 						box1 := collision.box_to_convex(
@@ -133,29 +143,36 @@ simulate_step :: proc(scene: ^Scene, config: Solver_Config) {
 						manifold, collision := collision.convex_vs_convex_sat(box1, box2)
 
 						if collision {
+							scene.contact_pairs[scene.contact_pairs_len] = Contact_Pair {
+								a        = Body_Handle(i + 1),
+								b        = Body_Handle(j + 1),
+								manifold = manifold,
+							}
+							scene.contact_pairs_len += 1
+							factor := 1.0 / f32(manifold.points_len)
 							for p in manifold.points[:manifold.points_len] {
-								body1_inv_mass := get_body_inverse_mass(body, manifold.normal, p)
+								// body1_inv_mass := get_body_inverse_mass(body, manifold.normal, p)
 								body2_inv_mass := get_body_inverse_mass(body2, manifold.normal, p)
 
 								apply_constraint_correction_unilateral(
 									dt,
 									body,
 									0,
-									-manifold.separation,
+									-manifold.separation * factor,
-									manifold.normal,
+									-manifold.normal,
 									p,
 									body2_inv_mass,
 								)
 
-								apply_constraint_correction_unilateral(
+								// apply_constraint_correction_unilateral(
-									dt,
+								// 	dt,
-									body2,
+								// 	body2,
-									0,
+								// 	0,
-									-manifold.separation,
+								// 	-manifold.separation,
-									-manifold.normal,
+								// 	manifold.normal,
-									p,
+								// 	p,
-									body1_inv_mass,
+								// 	body1_inv_mass,
-								)
+								// )
 							}
 						}
 					}