diff --git a/physics.jai b/physics.jai index 260781e..f5ed0e2 100644 --- a/physics.jai +++ b/physics.jai @@ -94,6 +94,7 @@ Rect :: struct { halfsize: Vector2; pos , vel , force : Vector2; // pos is in world space angle, angle_vel, torque: float; + static: bool; mass: float = 1.0; }; moment_of_inertia :: (using r: Rect) -> float { @@ -106,6 +107,7 @@ apply_force_at_point :: (r: *Rect, force: Vector2, point_world_space: Vector2) { } // everything needed to resolve the collision Manifold :: struct { + a, b: *Rect; count: int; depths: [2] float; contact_points: [2] Vector2; // in absolute coordinates @@ -119,9 +121,24 @@ perp :: (v: Vector2) -> Vector2 { return xy(-v.y, v.x); } -handle_collision :: (m: Manifold, a: Rect, b: Rect) { +handle_collision :: (m: Manifold, dt: float) { + a := m.a; + b := m.b; if m.count == 0 return; - k_scalar :: (a: Rectangle, b: Rectangle, r1: Vector2, r2: Vector2, n: Vector2) { + for 0..m.count-1 + { + total_momentum: float = length(a.vel) * a.mass + length(b.vel) * b.mass; + impulse_strength: float = 0.0; + //impulse_strength += m.depths[it] * 3.0; + impulse_strength += total_momentum / 2.0; + + if a.static || b.static impulse_strength *= 2.0; + + impulse := m.normal * impulse_strength; + apply_force_at_point(b, impulse / dt, m.contact_points[it]); + apply_force_at_point(a, -impulse / dt, m.contact_points[it]); + } + /*k_scalar :: (a: Rectangle, b: Rectangle, r1: Vector2, r2: Vector2, n: Vector2) { k_scalar_rect :: (a: Rectangle, r: Vector2, n: Vector2) { rcn := cross(r, n); @@ -134,7 +151,7 @@ handle_collision :: (m: Manifold, a: Rect, b: Rect) { } r1 := m.contact_points[0] - a.pos; r2 := m.contact_points[0] - b.pos; - nMass := 1.0 / k_scalar(a, b, r1, r2, + nMass := 1.0 / k_scalar(a, b, r1, r2, */ } MAX_POLYGON_VERTS :: 8; @@ -232,7 +249,7 @@ check_faces :: (a: Polygon, b: Polygon) -> (separation: float, face_index: int) return sep, index; } -rect_to_rect :: (a: Rect, b: Rect) -> Manifold { +rect_to_rect :: (a: *Rect, b: *Rect) -> Manifold { to_return : Manifold; poly_a := poly_from(a); @@ -361,6 +378,8 @@ rect_to_rect :: (a: Rect, b: Rect) -> Manifold { for 0..m.count-1 { push_pip(m.contact_points[it]); } + m.a = a; + m.b = b; return m; } dump_polygon :: (poly: Polygon) { @@ -379,7 +398,7 @@ do_test :: () { #if true { print("A %\n", A_rect); print("B %\n", B_rect); - print("Collision %\n", rect_to_rect(A_rect, B_rect)); + print("Collision %\n", rect_to_rect(*A_rect, *B_rect)); } @@ -488,7 +507,8 @@ main :: () { rects: [..]Rect; array_add(*rects, .{pos = #run xy(0.0, 0.0), halfsize = #run xy(0.3)}); array_add(*rects, .{pos = #run xy(2.5, 0.0), halfsize = #run xy(0.3)}); - + array_add(*rects, .{pos = #run xy(-2.5, 0.0), halfsize = #run xy(0.3)}); + array_add(*rects, .{pos = #run xy(0.0, -3.0), halfsize = #run xy(3.0, 0.2), static = true}); quit := false; last_time := get_time(); @@ -536,26 +556,43 @@ main :: () { { apply_force_at_point(*rects[0], xy(3, 0), rects[0].pos + xy(-0.1, 0.03)); } - for * rects - { - defer { it.force = .{}; it.torque = 0.0; }; - it.vel += (it.force/MASS) * TIMESTEP; - it.pos += it.vel * TIMESTEP; - - from_rect := it; + collisions: [..] Manifold; + defer array_free(collisions); + for * from_rect: rects { for * to_rect: rects { if to_rect != from_rect { manifold_out := rect_to_rect(from_rect, to_rect); if manifold_out.count > 0 { - handle_collision(manifold_out, from_rect, to_rect); - //from_rect.vel *= -1.0; + unique := true; + for collisions { + if (it.a == manifold_out.a && it.b == manifold_out.b) || (it.a == manifold_out.b && it.b == manifold_out.a) { + unique = false; + break; + } + } + if unique array_add(*collisions, manifold_out); } } } - - it.angle_vel += (it.torque / moment_of_inertia(it)) * TIMESTEP; - it.angle += it.angle_vel * TIMESTEP; + } + for collisions { + handle_collision(it, TIMESTEP); + } + for * rects { + defer it.force = .{}; + defer it.torque = 0.0; + + // gravity + it.force.y += -9.81; + + if !it.static + { + it.vel += (it.force/it.mass) * TIMESTEP; + it.pos += it.vel * TIMESTEP; + it.angle_vel += (it.torque / moment_of_inertia(it)) * TIMESTEP; + it.angle += it.angle_vel * TIMESTEP; + } } } }