diff --git a/game/container/spanpool/spanpool.odin b/game/container/spanpool/spanpool.odin index ccede88..527210f 100644 --- a/game/container/spanpool/spanpool.odin +++ b/game/container/spanpool/spanpool.odin @@ -1,5 +1,6 @@ package spanpool +import "base:builtin" import "core:fmt" import "core:slice" @@ -133,3 +134,13 @@ reconcile :: proc(s: ^$T/Span_Pool($E)) { } } } + +copy :: proc(dst: ^Span_Pool($E), src: Span_Pool(E)) { + resize(&dst.elems, len(src.elems)) + resize(&dst.generations, len(src.generations)) + resize(&dst.free_spans, len(src.free_spans)) + + builtin.copy(dst.elems[:], src.elems[:]) + builtin.copy(dst.generations[:], src.generations[:]) + builtin.copy(dst.free_spans[:], src.free_spans[:]) +} diff --git a/game/game.odin b/game/game.odin index ec5b9d6..d326946 100644 --- a/game/game.odin +++ b/game/game.odin @@ -72,7 +72,7 @@ Car :: struct { } SOLVER_CONFIG :: physics.Solver_Config { - timestep = 1.0 / 120, + timestep = 1.0 / 60, gravity = rl.Vector3{0, -9.8, 0}, substreps_minus_one = 2 - 1, } diff --git a/game/physics/debug.odin b/game/physics/debug.odin index 71ec383..ac16f22 100644 --- a/game/physics/debug.odin +++ b/game/physics/debug.odin @@ -14,6 +14,7 @@ _ :: math _ :: debug draw_debug_shape :: proc( + sim_state: ^Sim_State, shape: Collision_Shape, pos: rl.Vector3, rot: rl.Quaternion, @@ -30,8 +31,9 @@ draw_debug_shape :: proc( switch s in shape { case Shape_Box: rl.DrawCubeV(0, s.size, color) - case Shape_Convex: - halfedge.debug_draw_mesh_wires(s.mesh, color) + case Internal_Shape_Convex: + mesh := convex_container_get_mesh(&sim_state.convex_container, s.mesh) + halfedge.debug_draw_mesh_wires(mesh, color) } } @@ -55,6 +57,7 @@ draw_debug_scene :: proc(scene: ^Scene) { rl.DrawLine3D(pos, pos + z, rl.BLUE) draw_debug_shape( + sim_state, body.shape, body_get_shape_pos(body), body.q, diff --git a/game/physics/helpers.odin b/game/physics/helpers.odin index f78fc2b..d5fc139 100644 --- a/game/physics/helpers.odin +++ b/game/physics/helpers.odin @@ -23,7 +23,7 @@ inertia_tensor_box :: proc(size: rl.Vector3) -> (tensor: Matrix3) { return } -inertia_tensor_collision_shape :: proc(shape: Collision_Shape) -> (tensor: Matrix3) { +inertia_tensor_collision_shape :: proc(shape: Input_Shape) -> (tensor: Matrix3) { switch s in shape { case Shape_Box: tensor = inertia_tensor_box(s.size) @@ -78,7 +78,7 @@ wheel_get_right_vec :: #force_inline proc( body_get_shape_offset_local :: proc(body: Body_Ptr) -> (offset: rl.Vector3) { #partial switch s in body.shape { - case Shape_Convex: + case Internal_Shape_Convex: offset = -s.center_of_mass } return @@ -92,6 +92,7 @@ body_get_shape_pos :: proc(body: Body_Ptr) -> rl.Vector3 { } body_get_convex_shape_world :: proc( + sim_state: ^Sim_State, body: Body_Ptr, allocator := context.temp_allocator, ) -> ( @@ -100,8 +101,10 @@ body_get_convex_shape_world :: proc( switch s in body.shape { case Shape_Box: mesh = collision.box_to_convex(collision.Box{rad = s.size * 0.5}, allocator) - case Shape_Convex: - mesh = halfedge.copy_mesh(s.mesh, context.temp_allocator) + case Internal_Shape_Convex: + mesh = convex_container_get_mesh(&sim_state.convex_container, s.mesh) + // TODO: make sure this works as intendent + mesh = halfedge.copy_mesh(mesh, context.temp_allocator) } transform := diff --git a/game/physics/immediate.odin b/game/physics/immediate.odin index c4a65d1..6c17958 100644 --- a/game/physics/immediate.odin +++ b/game/physics/immediate.odin @@ -1,8 +1,6 @@ package physics import "core:log" -import lg "core:math/linalg" -import rl "vendor:raylib" _ :: log @@ -11,79 +9,6 @@ Body_Config_Inertia_Mode :: enum { 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: Matrix3, -} - -// TODO: rename to wheel -Suspension_Constraint_Config :: struct { - rel_pos: rl.Vector3, - rel_dir: rl.Vector3, - body: Body_Handle, - rest: f32, - compliance: f32, - damping: f32, - radius: f32, -} - -calculate_body_params_from_config :: proc( - config: Body_Config, -) -> ( - inv_mass: f32, - inv_inertia_tensor: Matrix3, -) { - inv_mass = config.mass == 0 ? 0 : 1.0 / config.mass - - inertia_tensor: Matrix3 - if config.inertia_mode == .Explicit { - inertia_tensor = config.inertia_tensor - } else { - inertia_tensor = inertia_tensor_collision_shape(config.shape) - } - inertia_tensor = inertia_tensor * Matrix3(config.mass) - - inv_inertia_tensor = lg.determinant(inertia_tensor) == 0 ? 0 : lg.inverse(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.shape = config.shape - - 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.shape = config.shape - body.inv_mass, body.inv_inertia_tensor = calculate_body_params_from_config(config) -} - -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 - constraint.damping = config.damping - constraint.radius = config.radius -} - immediate_body :: proc( scene: ^Scene, state: ^Solver_State, @@ -101,10 +26,8 @@ immediate_body :: proc( handle = body.handle update_body_from_config(get_body(get_sim_state(scene), handle), config) } else { - new_body: Body state.num_referenced_bodies += 1 - initialize_body_from_config(&new_body, config) - handle = add_body(get_sim_state(scene), new_body) + handle = add_body(get_sim_state(scene), config) state.immedate_bodies[id] = { handle = handle, last_ref = state.simulation_frame, diff --git a/game/physics/scene.odin b/game/physics/scene.odin index 93d3ce2..5b2590e 100644 --- a/game/physics/scene.odin +++ b/game/physics/scene.odin @@ -1,7 +1,7 @@ package physics import "collision" -import "game:halfedge" +import lg "core:math/linalg" import rl "vendor:raylib" MAX_CONTACTS :: 1024 @@ -21,6 +21,7 @@ Sim_State :: struct { // Persistent stuff for simulation contact_pairs: [MAX_CONTACTS]Contact_Pair, contact_pairs_len: int, + convex_container: Convex_Container, } Scene :: struct { @@ -56,7 +57,12 @@ Shape_Box :: struct { size: rl.Vector3, } -// TODO: Assuming mesh is generated and reinserted every frame for now, make it persistent yada yada +Internal_Shape_Convex :: struct { + mesh: Convex_Handle, + center_of_mass: rl.Vector3, + inertia_tensor: Matrix3, +} + Shape_Convex :: struct { mesh: collision.Convex, center_of_mass: rl.Vector3, @@ -65,7 +71,7 @@ Shape_Convex :: struct { Collision_Shape :: union { Shape_Box, - Shape_Convex, + Internal_Shape_Convex, } Suspension_Constraint :: struct { @@ -151,59 +157,127 @@ get_body :: proc(sim_state: ^Sim_State, handle: Body_Handle) -> Body_Ptr { return &sim_state.bodies_slice[index] } -copy_shape :: proc( - src: Collision_Shape, - allocator := context.allocator, -) -> ( - dst: Collision_Shape, -) { - switch s in src { - case Shape_Box: - dst = s - case Shape_Convex: - new_convex := s - new_convex.mesh = halfedge.copy_mesh(s.mesh, allocator) - dst = new_convex - } - return -} - -destroy_shape :: proc(shape: ^Collision_Shape, allocator := context.allocator) { +remove_shape :: proc(sim_state: ^Sim_State, shape: ^Collision_Shape) { switch &s in shape { case Shape_Box: - case Shape_Convex: - delete(s.mesh.faces, allocator) - delete(s.mesh.edges, allocator) - delete(s.mesh.vertices, allocator) - s.mesh = {} + case Internal_Shape_Convex: + convex_container_remove(&sim_state.convex_container, s.mesh) } } -copy_body :: proc(src: Body, allocator := context.allocator) -> (dst: Body) { - dst = src - dst.shape = copy_shape(src.shape) - dst.next_plus_one = 0 +Input_Shape :: union { + Shape_Box, + Shape_Convex, +} + +Body_Config :: struct { + initial_pos: rl.Vector3, + initial_rot: rl.Quaternion, + initial_vel: rl.Vector3, + initial_ang_vel: rl.Vector3, + shape: Input_Shape, + mass: f32, + inertia_mode: Body_Config_Inertia_Mode, + // Unit inertia tensor + inertia_tensor: Matrix3, +} + +// TODO: rename to wheel +Suspension_Constraint_Config :: struct { + rel_pos: rl.Vector3, + rel_dir: rl.Vector3, + body: Body_Handle, + rest: f32, + compliance: f32, + damping: f32, + radius: f32, +} + +calculate_body_params_from_config :: proc( + config: Body_Config, +) -> ( + inv_mass: f32, + inv_inertia_tensor: Matrix3, +) { + inv_mass = config.mass == 0 ? 0 : 1.0 / config.mass + + inertia_tensor: Matrix3 + if config.inertia_mode == .Explicit { + inertia_tensor = config.inertia_tensor + } else { + inertia_tensor = inertia_tensor_collision_shape(config.shape) + } + inertia_tensor = inertia_tensor * Matrix3(config.mass) + + inv_inertia_tensor = lg.determinant(inertia_tensor) == 0 ? 0 : lg.inverse(inertia_tensor) return } -add_body :: proc(sim_state: ^Sim_State, body: Body) -> Body_Handle { - body_copy := copy_body(body) +add_shape :: proc(sim_state: ^Sim_State, shape: Input_Shape) -> (result: Collision_Shape) { + switch s in shape { + case Shape_Box: + result = s + case Shape_Convex: + convex: Internal_Shape_Convex + convex.mesh = convex_container_add(&sim_state.convex_container, s.mesh) + convex.center_of_mass = s.center_of_mass + convex.inertia_tensor = s.inertia_tensor + result = convex + } - body_copy.alive = true - body_copy.next_plus_one = 0 + return +} + +initialize_body_from_config :: proc(sim_state: ^Sim_State, 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.shape = add_shape(sim_state, config.shape) + + body.inv_mass, body.inv_inertia_tensor = calculate_body_params_from_config(config) +} + +update_body_from_config :: proc(body: Body_Ptr, config: Body_Config) { + // TODO: Figure out how to update shape + // body.shape = config.shape + body.inv_mass, body.inv_inertia_tensor = calculate_body_params_from_config(config) +} + +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 + constraint.damping = config.damping + constraint.radius = config.radius +} + + +add_body :: proc(sim_state: ^Sim_State, config: Body_Config) -> Body_Handle { + body: Body + + initialize_body_from_config(sim_state, &body, config) + + body.alive = true if sim_state.first_free_body_plus_one > 0 { index := sim_state.first_free_body_plus_one new_body := get_body(sim_state, Body_Handle(index)) next_plus_one := new_body.next_plus_one - new_body^ = body_copy + new_body^ = body sim_state.first_free_body_plus_one = next_plus_one return Body_Handle(index) } - append_soa(&sim_state.bodies, body_copy) + append_soa(&sim_state.bodies, body) index := len(sim_state.bodies) sim_state.bodies_slice = sim_state.bodies[:] @@ -217,7 +291,7 @@ remove_body :: proc(sim_state: ^Sim_State, handle: Body_Handle) { body.alive = false - destroy_shape(&body.shape) + remove_shape(sim_state, &body.shape) body.next_plus_one = sim_state.first_free_body_plus_one sim_state.first_free_body_plus_one = i32(handle) @@ -278,6 +352,7 @@ _get_first_free_body :: proc(sim_state: ^Sim_State) -> i32 { destry_sim_state :: proc(sim_state: ^Sim_State) { delete_soa(sim_state.bodies) delete_soa(sim_state.suspension_constraints) + convex_container_destroy(&sim_state.convex_container) } destroy_physics_scene :: proc(scene: ^Scene) { diff --git a/game/physics/shapes.odin b/game/physics/shapes.odin index af38a8a..beb7c82 100644 --- a/game/physics/shapes.odin +++ b/game/physics/shapes.odin @@ -1,4 +1,66 @@ package physics + +import he "game:halfedge" +import "game:container/spanpool" + +Convex_Container :: struct { + vertices: spanpool.Span_Pool(he.Vertex), + faces: spanpool.Span_Pool(he.Face), + edges: spanpool.Span_Pool(he.Half_Edge), +} + +Convex_Handle :: struct { + vertices: spanpool.Handle, + faces: spanpool.Handle, + edges: spanpool.Handle, +} + +convex_container_add :: proc(container: ^Convex_Container, mesh: he.Half_Edge_Mesh) -> (handle: Convex_Handle) { + handle.vertices = spanpool.allocate_elems(&container.vertices, ..mesh.vertices) + handle.faces = spanpool.allocate_elems(&container.faces, ..mesh.faces) + handle.edges = spanpool.allocate_elems(&container.edges, ..mesh.edges) + + return +} + +convex_container_get_mesh :: proc(container: ^Convex_Container, handle: Convex_Handle) -> (mesh: he.Half_Edge_Mesh) { + mesh.vertices = spanpool.resolve_slice(&container.vertices, handle.vertices) + mesh.faces = spanpool.resolve_slice(&container.faces, handle.faces) + mesh.edges = spanpool.resolve_slice(&container.edges, handle.edges) + + // TODO: save and return mesh center + avg_scale := 1.0 / f32(len(mesh.vertices)) + for v in mesh.vertices { + mesh.center += v.pos * avg_scale + } + + return +} + +convex_container_remove :: proc(container: ^Convex_Container, handle: Convex_Handle) { + spanpool.free(&container.vertices, handle.vertices) + spanpool.free(&container.faces, handle.faces) + spanpool.free(&container.edges, handle.edges) +} + +convex_container_reconcile :: proc(container: ^Convex_Container) { + spanpool.reconcile(&container.vertices) + spanpool.reconcile(&container.faces) + spanpool.reconcile(&container.edges) +} + +convex_container_copy :: proc(dst: ^Convex_Container, src: Convex_Container) { + spanpool.copy(&dst.vertices, src.vertices) + spanpool.copy(&dst.faces, src.faces) + spanpool.copy(&dst.edges, src.edges) +} + +convex_container_destroy :: proc(container: ^Convex_Container) { + spanpool.destroy_spanpool(&container.vertices) + spanpool.destroy_spanpool(&container.faces) + spanpool.destroy_spanpool(&container.edges) +} + // // import "core:container/intrusive/list" // import "game:container/spanpool" diff --git a/game/physics/simulation.odin b/game/physics/simulation.odin index 652fb6a..c410977 100644 --- a/game/physics/simulation.odin +++ b/game/physics/simulation.odin @@ -64,6 +64,8 @@ prepare_next_sim_state :: proc(scene: ^Scene) { for i in 0 ..< len(next_state.suspension_constraints) { next_state.suspension_constraints[i] = current_state.suspension_constraints[i] } + + convex_container_copy(&next_state.convex_container, current_state.convex_container) } Step_Mode :: enum { @@ -195,8 +197,8 @@ simulate_step :: proc(sim_state: ^Sim_State, config: Solver_Config) { body2.alive && !handled_pairs[{a = min(i, j), b = max(i, j)}] { m1, m2 := - body_get_convex_shape_world(body), - body_get_convex_shape_world(body2) + body_get_convex_shape_world(sim_state, body), + body_get_convex_shape_world(sim_state, body2) // Raw manifold has contact points in world space raw_manifold, collision := collision.convex_vs_convex_sat(m1, m2)