From d1feaa0602e2fb5926b4621a6eec859a6f1fb85a Mon Sep 17 00:00:00 2001 From: sergeypdev Date: Sun, 9 Mar 2025 22:54:40 +0400 Subject: [PATCH] More progress on PGS --- build_hot_reload.sh | 2 +- game/game.odin | 4 +- game/physics/collision/convex.odin | 31 +- game/physics/debug.odin | 2 +- game/physics/simulation.odin | 446 ++++++++++++++++++++++------- 5 files changed, 374 insertions(+), 111 deletions(-) diff --git a/build_hot_reload.sh b/build_hot_reload.sh index 1dabf5d..64d2beb 100755 --- a/build_hot_reload.sh +++ b/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 -o:speed +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 # 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 diff --git a/game/game.odin b/game/game.odin index 4bd077a..e3a3196 100644 --- a/game/game.odin +++ b/game/game.odin @@ -75,7 +75,7 @@ Car :: struct { SOLVER_CONFIG :: physics.Solver_Config { timestep = 1.0 / 60, gravity = rl.Vector3{0, -9.8, 0}, - substreps_minus_one = 4 - 1, + substreps_minus_one = 8 - 1, } Game_Memory :: struct { @@ -277,7 +277,7 @@ update_runtime_world :: proc(runtime_world: ^Runtime_World, dt: f32) { ) if true { - for x in 0 ..< 20 { + for x in 0 ..< 5 { for y in 0 ..< 20 { physics.immediate_body( &world.physics_scene, diff --git a/game/physics/collision/convex.odin b/game/physics/collision/convex.odin index a0106c8..1492b49 100644 --- a/game/physics/collision/convex.odin +++ b/game/physics/collision/convex.odin @@ -41,7 +41,23 @@ box_to_convex :: proc(box: Box, allocator := context.allocator) -> (convex: Conv return } +Contact_Type_Face :: struct { + face_idx_a: halfedge.Face_Index, + face_idx_b: halfedge.Face_Index, +} + +Contact_Type_Edge :: struct { + edge_idx_a: halfedge.Edge_Index, + edge_idx_b: halfedge.Edge_Index, +} + +Contact_Type :: union { + Contact_Type_Face, + Contact_Type_Edge, +} + Contact_Manifold :: struct { + type: Contact_Type, normal: Vec3, separation: f32, points_a: [4]Vec3, @@ -274,7 +290,8 @@ create_face_contact_manifold :: proc( ref_convex := is_ref_a ? a : b inc_convex := is_ref_a ? b : a - ref_face := ref_convex.faces[ref_face_query.face] + ref_face_idx := ref_face_query.face + ref_face := ref_convex.faces[ref_face_idx] // incident face inc_face: halfedge.Face @@ -492,9 +509,17 @@ create_face_contact_manifold :: proc( } if is_ref_a { + manifold.type = Contact_Type_Face { + face_idx_a = ref_face_idx, + face_idx_b = inc_face_idx, + } manifold.points_a = ref_points manifold.points_b = inc_points } else { + manifold.type = Contact_Type_Face { + face_idx_a = inc_face_idx, + face_idx_b = ref_face_idx, + } manifold.points_b = ref_points manifold.points_a = inc_points } @@ -526,6 +551,10 @@ create_edge_contact_manifold :: proc( _, ps := closest_point_between_segments(a1, a2, b1, b2) + manifold.type = Contact_Type_Edge { + edge_idx_a = edge_a, + edge_idx_b = edge_b, + } manifold.normal = separating_plane.normal manifold.separation = separation manifold.points_a[0] = ps[0] diff --git a/game/physics/debug.odin b/game/physics/debug.odin index a6b0405..aa0970c 100644 --- a/game/physics/debug.odin +++ b/game/physics/debug.odin @@ -108,7 +108,7 @@ draw_debug_scene :: proc(scene: ^Scene) { } } - if false { + if true { for &contact, contact_idx in sim_state.contact_container.contacts { points_a := contact.manifold.points_a points_b := contact.manifold.points_b diff --git a/game/physics/simulation.odin b/game/physics/simulation.odin index 37c922c..2c9361a 100644 --- a/game/physics/simulation.odin +++ b/game/physics/simulation.odin @@ -216,9 +216,15 @@ Contact :: struct { prev_q_a, prev_q_b: Quat, manifold: collision.Contact_Manifold, applied_corrections: int, + + // For PGS + total_normal_impulse: [4]f32, + total_tangent_impulse: [4]f32, + + // XPBD stuff lambda_normal: [4]f32, - lambda_tangent: [4]f32, - applied_static_friction: [4]bool, + lambda_tangent: f32, + applied_static_friction: bool, applied_normal_correction: [4]f32, } @@ -245,11 +251,17 @@ update_contacts :: proc(sim_state: ^Sim_State) { assert(body.alive) assert(body2.alive) + old_manifold := contact.manifold + old_total_normal_impulse := contact.total_normal_impulse + old_total_tangent_impulse := contact.total_tangent_impulse + contact.prev_x_a = body.x contact.prev_x_b = body2.x contact.prev_q_a = body.q contact.prev_q_b = body2.q contact.manifold = {} + contact.total_normal_impulse = 0 + contact.total_tangent_impulse = 0 contact.lambda_normal = 0 contact.lambda_tangent = 0 contact.applied_static_friction = false @@ -276,6 +288,15 @@ update_contacts :: proc(sim_state: ^Sim_State) { manifold := &contact.manifold manifold^ = raw_manifold + preserve_impulse := + old_manifold.points_len == raw_manifold.points_len && + old_manifold.type == raw_manifold.type + + if preserve_impulse { + contact.total_normal_impulse = old_total_normal_impulse + contact.total_tangent_impulse = old_total_tangent_impulse + } + // Convert manifold contact from world to local space for point_idx in 0 ..< manifold.points_len { manifold.points_a[point_idx] = body_world_to_local( @@ -346,79 +367,37 @@ xpbd_substep :: proc(sim_state: ^Sim_State, config: Solver_Config, dt: f32, inv_ manifold := &contact.manifold for point_idx in 0 ..< manifold.points_len { - p1, p2 := manifold.points_a[point_idx], manifold.points_b[point_idx] - p1, p2 = body_local_to_world(body, p1), body_local_to_world(body2, p2) + { + p1, p2 := manifold.points_a[point_idx], manifold.points_b[point_idx] + p1, p2 = body_local_to_world(body, p1), body_local_to_world(body2, p2) - p_diff_normal := lg.dot(p2 - p1, manifold.normal) - separation := min(p_diff_normal, 0) + p_diff_normal: f32 = lg.dot(p2 - p1, manifold.normal) + separation: f32 = min(p_diff_normal, 0) - lambda_norm, corr1, corr2, ok := calculate_constraint_params2( - dt, - body, - body2, - 0.00002, - separation, - -manifold.normal, - p1, - p2, - ) - if ok { - contact.applied_normal_correction[point_idx] = -separation - contact.applied_corrections += 1 - contact.lambda_normal[point_idx] = lambda_norm + lambda_norm, corr1, corr2, ok := calculate_constraint_params2( + dt, + body, + body2, + 0.00002, + -separation, + manifold.normal, + p1, + p2, + ) + if ok { + contact.applied_normal_correction[point_idx] = -separation + contact.applied_corrections += 1 + contact.lambda_normal[point_idx] = lambda_norm - apply_correction(body, corr1, p1) - apply_correction(body2, corr2, p2) - } - } - } - } - - if false { - tracy.ZoneN("simulate_step::static_friction") - - when false { - context = context - context.user_ptr = sim_state - slice.sort_by( - sim_state.contact_pairs[:sim_state.contact_pairs_len], - proc(c1, c2: Contact) -> bool { - sim_state := cast(^Sim_State)context.user_ptr - - find_min_contact_y :: proc( - scene: ^Sim_State, - c: Contact, - ) -> ( - min_contact_y: f32, - ) { - min_contact_y = max(f32) - body_a, body_b := get_body(scene, c.a), get_body(scene, c.b) - for i in 0 ..< c.manifold.points_len { - min_contact_y = min( - body_local_to_world(body_a, c.manifold.points_a[i]).y, - body_local_to_world(body_b, c.manifold.points_b[i]).y, - ) - } - return + apply_position_correction(body, corr1, p1) + apply_position_correction(body2, corr2, p2) } + } - min_y1 := find_min_contact_y(sim_state, c1) - min_y2 := find_min_contact_y(sim_state, c2) + if false && contact.lambda_normal[point_idx] != 0 { + p1, p2 := manifold.points_a[point_idx], manifold.points_b[point_idx] + p1, p2 = body_local_to_world(body, p1), body_local_to_world(body2, p2) - return min_y1 > min_y2 - }, - ) - } - - for &contact in sim_state.contact_container.contacts { - manifold := contact.manifold - body, body2 := get_body(sim_state, contact.a), get_body(sim_state, contact.b) - - for point_idx in 0 ..< manifold.points_len { - lambda_norm := contact.lambda_normal[point_idx] - if lambda_norm != 0 { - p1 := body_local_to_world(body, manifold.points_a[point_idx]) - p2 := body_local_to_world(body2, manifold.points_b[point_idx]) prev_p1 := body.prev_x + lg.quaternion_mul_vector3(body.prev_q, manifold.points_a[point_idx]) @@ -426,7 +405,7 @@ xpbd_substep :: proc(sim_state: ^Sim_State, config: Solver_Config, dt: f32, inv_ body2.prev_x + lg.quaternion_mul_vector3(body2.prev_q, manifold.points_b[point_idx]) - p_diff_tangent := (p1 - prev_p1) - (p2 - prev_p2) + p_diff_tangent: Vec3 = (p1 - prev_p1) - (p2 - prev_p2) p_diff_tangent -= lg.dot(p_diff_tangent, manifold.normal) * manifold.normal tangent_diff_len := lg.length(p_diff_tangent) @@ -439,20 +418,22 @@ xpbd_substep :: proc(sim_state: ^Sim_State, config: Solver_Config, dt: f32, inv_ dt, body, body2, - 0, + 0.00002, -tangent_diff_len, -tangent_diff_normalized, p1, p2, ) - STATIC_FRICTION :: 0 - if ok_tangent && delta_lambda_tangent < STATIC_FRICTION * lambda_norm { - contact.applied_static_friction[point_idx] = true - contact.lambda_tangent[point_idx] = delta_lambda_tangent + STATIC_FRICTION :: 1000 + if ok_tangent && + delta_lambda_tangent > + STATIC_FRICTION * contact.lambda_normal[point_idx] { + contact.applied_static_friction = true + contact.lambda_tangent = delta_lambda_tangent - apply_correction(body, corr1_tangent, p1) - apply_correction(body2, corr2_tangent, p2) + apply_position_correction(body, corr1_tangent, p1) + apply_position_correction(body2, corr2_tangent, p2) } } } @@ -460,7 +441,85 @@ xpbd_substep :: proc(sim_state: ^Sim_State, config: Solver_Config, dt: f32, inv_ } } - { + if false { + tracy.ZoneN("simulate_step::static_friction") + + for &contact in sim_state.contact_container.contacts { + manifold := contact.manifold + body1, body2 := get_body(sim_state, contact.a), get_body(sim_state, contact.b) + + prev_p1, prev_p2: Vec3 + p1, p2: Vec3 + total_lambda_normal := f32(0) + friciton_points_len := 0 + + for point_idx in 0 ..< contact.manifold.points_len { + if contact.lambda_normal == 0 { + continue + } + + total_lambda_normal += contact.lambda_normal[point_idx] + friciton_points_len += 1 + + point_p1, point_p2 := + body_local_to_world(body1, manifold.points_a[point_idx]), + body_local_to_world(body2, manifold.points_b[point_idx]) + + p1 += point_p1 + p2 += point_p2 + + prev_point_p1 := + body1.prev_x + + lg.quaternion_mul_vector3(body1.prev_q, manifold.points_a[point_idx]) + prev_point_p2 := + body2.prev_x + + lg.quaternion_mul_vector3(body2.prev_q, manifold.points_b[point_idx]) + + prev_p1 += prev_point_p1 + prev_p2 += prev_point_p2 + } + + if friciton_points_len > 0 { + p1 /= f32(friciton_points_len) + p2 /= f32(friciton_points_len) + + prev_p1 /= f32(friciton_points_len) + prev_p2 /= f32(friciton_points_len) + + p_diff_tangent := (p1 - prev_p1) - (p2 - prev_p2) + p_diff_tangent -= lg.dot(p_diff_tangent, manifold.normal) * manifold.normal + + tangent_diff_len := lg.length(p_diff_tangent) + + if tangent_diff_len > 0 { + tangent_diff_normalized := p_diff_tangent / tangent_diff_len + + delta_lambda_tangent, corr1_tangent, corr2_tangent, ok_tangent := + calculate_constraint_params2( + dt, + body1, + body2, + 0, + -tangent_diff_len, + tangent_diff_normalized, + p1, + p2, + ) + + STATIC_FRICTION :: 1.0 + if ok_tangent { + contact.applied_static_friction = true + contact.lambda_tangent = delta_lambda_tangent + + apply_position_correction(body1, corr1_tangent, p1) + apply_position_correction(body2, corr2_tangent, p2) + } + } + } + } + } + + if true { tracy.ZoneN("simulate_step::suspension_constraints") for &v in sim_state.suspension_constraints { @@ -510,7 +569,6 @@ xpbd_substep :: proc(sim_state: ^Sim_State, config: Solver_Config, dt: f32, inv_ for &contact in sim_state.contact_container.contacts { manifold := &contact.manifold - body, body2 := get_body(sim_state, contact.a), get_body(sim_state, contact.b) prev_q1, prev_q2 := body.prev_q, body2.prev_q @@ -563,43 +621,61 @@ xpbd_substep :: proc(sim_state: ^Sim_State, config: Solver_Config, dt: f32, inv_ tracy.ZoneN("simulate_step::dynamic_friction") for &contact in sim_state.contact_container.contacts { + if contact.manifold.points_len == 0 { + continue + } + manifold := &contact.manifold body1 := get_body(sim_state, contact.a) body2 := get_body(sim_state, contact.b) + friction_p1, friction_p2: Vec3 + total_lambda_normal := f32(0) + friciton_points_len := 0 + for point_idx in 0 ..< contact.manifold.points_len { - if contact.applied_static_friction[point_idx] || contact.lambda_normal == 0 { + if contact.applied_static_friction || contact.lambda_normal == 0 { continue } + + total_lambda_normal += contact.lambda_normal[point_idx] + friciton_points_len += 1 + p1, p2 := body_local_to_world(body1, manifold.points_a[point_idx]), body_local_to_world(body2, manifold.points_b[point_idx]) - r1, r2 := p1 - body1.x, p2 - body2.x - v1 := body_velocity_at_point(body1, p1) - v2 := body_velocity_at_point(body2, p2) + + friction_p1 += p1 + friction_p2 += p2 + } + + if friciton_points_len > 0 { + friction_p1 /= f32(friciton_points_len) + friction_p2 /= f32(friciton_points_len) + v1 := body_velocity_at_point(body1, friction_p1) + v2 := body_velocity_at_point(body2, friction_p2) + r1, r2 := friction_p1 - body1.x, friction_p2 - body2.x v := v1 - v2 v_normal := lg.dot(manifold.normal, v) * manifold.normal v_tangent := v - v_normal - DYNAMIC_FRICTION :: 1 + DYNAMIC_FRICTION :: 0.5 v_tangent_len := lg.length(v_tangent) if v_tangent_len > 0 { v_tangent_norm := v_tangent / v_tangent_len w1, w2 := - get_body_inverse_mass(body1, v_tangent_norm, p1), - get_body_inverse_mass(body2, v_tangent_norm, p2) + get_body_inverse_mass(body1, v_tangent_norm, friction_p1), + get_body_inverse_mass(body2, v_tangent_norm, friction_p2) w := w1 + w2 - if w > 0 { + if w != 0 { delta_v := -v_tangent_norm * min( - dt * - DYNAMIC_FRICTION * - abs(contact.lambda_normal[point_idx] / (dt * dt)), + dt * DYNAMIC_FRICTION * abs(total_lambda_normal / (dt * dt)), v_tangent_len / w, ) @@ -614,6 +690,39 @@ xpbd_substep :: proc(sim_state: ^Sim_State, config: Solver_Config, dt: f32, inv_ body2.w -= multiply_inv_intertia(body2, lg.cross(r2, p)) } } + + { + angular_vel_error := + lg.dot(body1.w, manifold.normal) - lg.dot(body2.w, manifold.normal) + + w1, w2 := + get_body_angular_inverse_mass(body1, manifold.normal), + get_body_angular_inverse_mass(body2, manifold.normal) + + w := w1 + w2 + + if w != 0 { + angular_impulse := manifold.normal * -angular_vel_error / (w1 + w2) + + apply_angular_velocity_correction :: proc( + body: Body_Ptr, + angular_impulse: Vec3, + ) { + q := body.q + inv_q := lg.quaternion_inverse(q) + + delta_omega := angular_impulse + 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) + + body.w += delta_omega + } + + apply_angular_velocity_correction(body1, angular_impulse) + apply_angular_velocity_correction(body2, -angular_impulse) + } + } } } } @@ -643,8 +752,8 @@ xpbd_substep :: proc(sim_state: ^Sim_State, config: Solver_Config, dt: f32, inv_ dt, body, 0, - error = damping, - error_gradient = -dir, + error = -damping, + error_gradient = dir, pos = wheel_world_pos, ) @@ -660,8 +769,8 @@ xpbd_substep :: proc(sim_state: ^Sim_State, config: Solver_Config, dt: f32, inv_ dt, body, 0, - total_impulse * dt * dt, - -forward, + -total_impulse * dt * dt, + forward, wheel_world_pos, ) body_solve_velocity(body, body.prev_x, body.prev_q, inv_dt) @@ -679,7 +788,7 @@ xpbd_substep :: proc(sim_state: ^Sim_State, config: Solver_Config, dt: f32, inv_ corr := right * impulse * dt v.applied_impulse.x = impulse - apply_correction(body, corr, v.hit_point) + apply_position_correction(body, corr, v.hit_point) body_solve_velocity(body, body.prev_x, body.prev_q, inv_dt) } } @@ -687,6 +796,92 @@ xpbd_substep :: proc(sim_state: ^Sim_State, config: Solver_Config, dt: f32, inv_ } } + +calculate_soft_constraint_params :: proc( + natural_freq, damping_ratio, dt: f32, +) -> ( + bias_rate: f32, + mass_coef: f32, + impulse_coef: f32, +) { + omega := 2.0 * math.PI * natural_freq // angular frequency + a1 := 2.0 * damping_ratio + omega * dt + a2 := dt * omega * a1 + a3 := 1.0 / (1.0 + a2) + bias_rate = omega / a1 + mass_coef = a2 * a3 + impulse_coef = a3 + + return +} + +pgs_solve_contacts :: proc( + sim_state: ^Sim_State, + config: Solver_Config, + dt: f32, + inv_dt: f32, + apply_bias: bool, +) { + bias_rate, mass_coef, impulse_coef := calculate_soft_constraint_params(100, 1, dt) + if !apply_bias { + bias_rate = 0 + } + + for i in 0 ..< len(sim_state.contact_container.contacts) { + contact := &sim_state.contact_container.contacts[i] + manifold := &contact.manifold + + body1, body2 := get_body(sim_state, contact.a), get_body(sim_state, contact.b) + + for point_idx in 0 ..< manifold.points_len { + p1, p2 := + body_local_to_world(body1, manifold.points_a[point_idx]), + body_local_to_world(body2, manifold.points_b[point_idx]) + + p_diff_normal := lg.dot(p2 - p1, manifold.normal) + separation := min(p_diff_normal, 0) + + if separation < 0 { + // r1, r2 := p1 - body1.x, p2 - body2.x + v1 := body_velocity_at_point(body1, p1) + v2 := body_velocity_at_point(body2, p2) + + w1 := get_body_inverse_mass(body1, manifold.normal, p1) + w2 := get_body_inverse_mass(body2, manifold.normal, p2) + + w := w1 + w2 + + if w == 0 { + continue + } + + inv_w := 1.0 / w + + delta_v := lg.dot(v2 - v1, manifold.normal) + + incremental_impulse := + -inv_w * mass_coef * (delta_v + bias_rate * separation) - + impulse_coef * contact.total_normal_impulse[point_idx] + + new_total_impulse := max( + contact.total_normal_impulse[point_idx] + incremental_impulse, + 0, + ) + applied_impulse := new_total_impulse - contact.total_normal_impulse[point_idx] + contact.total_normal_impulse[point_idx] = new_total_impulse + + applied_impulse_vec := applied_impulse * manifold.normal + + apply_velocity_correction(body1, -applied_impulse_vec, p1) + apply_velocity_correction(body2, applied_impulse_vec, p2) + } else { + contact.total_normal_impulse[point_idx] = 0 + } + } + } + +} + pgs_substep :: proc(sim_state: ^Sim_State, config: Solver_Config, dt: f32, inv_dt: f32) { for i in 0 ..< len(sim_state.bodies_slice) { body := &sim_state.bodies_slice[i] @@ -696,12 +891,28 @@ pgs_substep :: proc(sim_state: ^Sim_State, config: Solver_Config, dt: f32, inv_d } } - // TODO: apply impulses - // for i in 0 ..< len(sim_state.contact_pairs) { - // contact_pair := &sim_state.contact_pairs[i] + // Warm start + if true { + for i in 0 ..< len(sim_state.contact_container.contacts) { + contact := &sim_state.contact_container.contacts[i] + manifold := &contact.manifold - // - // } + body1, body2 := get_body(sim_state, contact.a), get_body(sim_state, contact.b) + + for point_idx in 0 ..< manifold.points_len { + p1, p2 := + body_local_to_world(body1, manifold.points_a[point_idx]), + body_local_to_world(body2, manifold.points_b[point_idx]) + total_normal_impulse := contact.total_normal_impulse[point_idx] * manifold.normal + + apply_velocity_correction(body1, -total_normal_impulse, p1) + apply_velocity_correction(body2, total_normal_impulse, p2) + } + } + } + + apply_bias := true + pgs_solve_contacts(sim_state, config, dt, inv_dt, apply_bias) for i in 0 ..< len(sim_state.bodies_slice) { body := &sim_state.bodies_slice[i] @@ -724,6 +935,9 @@ pgs_substep :: proc(sim_state: ^Sim_State, config: Solver_Config, dt: f32, inv_d body.q = q } } + + apply_bias = false + pgs_solve_contacts(sim_state, config, dt, inv_dt, apply_bias) } simulate_step :: proc(scene: ^Scene, sim_state: ^Sim_State, config: Solver_Config) { @@ -774,7 +988,7 @@ simulate_step :: proc(scene: ^Scene, sim_state: ^Sim_State, config: Solver_Confi PGS, } - solver := Solver.XPBD + solver := Solver.PGS switch solver { case .XPBD: @@ -858,7 +1072,7 @@ calculate_constraint_params2 :: proc( return } - w := get_body_inverse_mass(body1, -error_gradient, pos1) + w := get_body_inverse_mass(body1, error_gradient, pos1) w += get_body_inverse_mass(body2, error_gradient, pos2) if w == 0 { @@ -900,7 +1114,7 @@ apply_constraint_correction_unilateral :: proc( ) if ok { - apply_correction(body, correction, pos) + apply_position_correction(body, correction, pos) } return @@ -917,10 +1131,21 @@ multiply_inv_intertia :: proc(body: Body_Ptr, vec: Vec3) -> (result: Vec3) { return result } -apply_correction :: proc(body: Body_Ptr, corr: Vec3, pos: Vec3) { - // rl.DrawSphereWires(pos, 0.5, 4, 4, rl.BLUE) - // rl.DrawLine3D(pos, pos + corr, rl.BLUE) +apply_velocity_correction :: proc(body: Body_Ptr, impulse: Vec3, pos: Vec3) { + body.v += impulse * body.inv_mass + q := body.q + inv_q := lg.quaternion_normalize0(lg.quaternion_inverse(q)) + delta_omega := pos - body.x + delta_omega = lg.cross(delta_omega, impulse) + 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) + + body.w += delta_omega +} + +apply_position_correction :: proc(body: Body_Ptr, corr: Vec3, pos: Vec3) { body.x += corr * body.inv_mass q := body.q @@ -955,3 +1180,12 @@ get_body_inverse_mass :: proc(body: Body_Ptr, normal, pos: Vec3) -> f32 { return w } + +get_body_angular_inverse_mass :: proc(body: Body_Ptr, normal: Vec3) -> f32 { + q := body.q + inv_q := lg.quaternion_normalize0(lg.quaternion_inverse(q)) + + n := lg.quaternion_mul_vector3(inv_q, normal) + + return lg.dot(n, n * body.inv_inertia_tensor) +}