package physics import "bvh" import "collision" import "core:container/bit_array" import "core:fmt" import "core:math" import lg "core:math/linalg" import "core:math/rand" import "core:slice" import "libs:tracy" _ :: rand _ :: math _ :: fmt _ :: slice Solver_Config :: struct { // Will automatically do fixed timestep timestep: f32, gravity: Vec3, substreps_minus_one: i32, } Solver_State :: struct { accumulated_time: f32, // Incremented when simulate is called (not simulate_step) simulation_frame: u32, // Number of immediate bodies referenced this frame num_referenced_bodies: i32, num_referenced_suspension_constraints: i32, immedate_bodies: map[u32]Immedate_State(Body_Handle), immediate_suspension_constraints: map[u32]Immedate_State(Suspension_Constraint_Handle), } destroy_solver_state :: proc(state: ^Solver_State) { delete(state.immedate_bodies) delete(state.immediate_suspension_constraints) } Immedate_State :: struct($T: typeid) { handle: T, // When was this referenced last time (frame number) last_ref: u32, } MAX_STEPS :: 10 // TODO: move into scene.odin // Copy current state to next sim_state_copy :: proc(dst: ^Sim_State, src: ^Sim_State) { tracy.Zone() convex_container_reconcile(&src.convex_container) dst.num_bodies = src.num_bodies dst.first_free_body_plus_one = src.first_free_body_plus_one dst.first_free_suspension_constraint_plus_one = src.first_free_suspension_constraint_plus_one resize(&dst.bodies, len(src.bodies)) resize(&dst.suspension_constraints, len(src.suspension_constraints)) dst.bodies_slice = dst.bodies[:] dst.suspension_constraints_slice = dst.suspension_constraints[:] for i in 0 ..< len(dst.bodies) { dst.bodies[i] = src.bodies[i] } for i in 0 ..< len(dst.suspension_constraints) { dst.suspension_constraints[i] = src.suspension_constraints[i] } contact_container_copy(&dst.contact_container, src.contact_container) convex_container_copy(&dst.convex_container, src.convex_container) } Step_Mode :: enum { Accumulated_Time, Single, } // Top Level Acceleration Structure TLAS :: struct { bvh_tree: bvh.BVH, body_aabbs: []bvh.AABB, } // TODO: free intermediate temp allocs // Creates TLAS using temp allocator build_tlas :: proc(sim_state: ^Sim_State, config: Solver_Config) -> TLAS { tracy.Zone() body_aabbs := make([]bvh.AABB, sim_state.num_bodies, context.temp_allocator) body_indices := make([]u16, sim_state.num_bodies, context.temp_allocator) { aabb_index := 0 for i in 0 ..< len(sim_state.bodies_slice) { body := &sim_state.bodies_slice[i] if body.alive { aabb := &body_aabbs[aabb_index] body_indices[aabb_index] = u16(i) aabb_index += 1 phys_aabb := body_get_aabb(body) EXPAND_K :: 2 expand := lg.abs(EXPAND_K * config.timestep * body.v) phys_aabb.extent += expand * 0.5 aabb.min = phys_aabb.center - phys_aabb.extent aabb.max = phys_aabb.center + phys_aabb.extent } } } sim_state_bvh := bvh.build_bvh_from_aabbs(body_aabbs, body_indices, context.temp_allocator) return TLAS{bvh_tree = sim_state_bvh, body_aabbs = body_aabbs} } // TODO: free intermediate temp allocs find_new_contacts :: proc(sim_state: ^Sim_State, tlas: ^TLAS) { tracy.Zone() for i in 0 ..< len(sim_state.bodies_slice) { assert(i <= int(max(u16))) body_idx := u16(i) body := &sim_state.bodies_slice[i] if body.alive { body_aabb := tlas.body_aabbs[i] it := bvh.iterator_intersect_leaf(&tlas.bvh_tree, body_aabb) for leaf_node in bvh.iterator_intersect_leaf_next(&it) { for j in 0 ..< leaf_node.prim_len { other_body_idx := tlas.bvh_tree.primitives[leaf_node.child_or_prim_start + j] prim_aabb := tlas.body_aabbs[other_body_idx] pair := make_contact_pair(i32(body_idx), i32(other_body_idx)) if body_idx != other_body_idx && bvh.test_aabb_vs_aabb(body_aabb, prim_aabb) && !(pair in sim_state.contact_container.lookup) { new_contact_idx := len(sim_state.contact_container.contacts) resize_soa(&sim_state.contact_container.contacts, new_contact_idx + 1) contact := &sim_state.contact_container.contacts[new_contact_idx] contact^ = Contact { a = Body_Handle(i + 1), b = Body_Handle(other_body_idx + 1), } sim_state.contact_container.lookup[pair] = i32(new_contact_idx) } } } } } } // Outer simulation loop for fixed timestepping simulate :: proc( scene: ^Scene, state: ^Solver_State, config: Solver_Config, dt: f32, commit := true, // commit = false is a special mode for debugging physics stepping to allow rerunning the same step each frame step_mode := Step_Mode.Accumulated_Time, ) { tracy.Zone() assert(config.timestep > 0) prune_immediate(scene, state) sim_state_copy(get_next_sim_state(scene), get_sim_state(scene)) sim_state := get_next_sim_state(scene) num_steps := 0 switch step_mode { case .Accumulated_Time: state.accumulated_time += dt for state.accumulated_time >= config.timestep { num_steps += 1 state.accumulated_time -= config.timestep if num_steps < MAX_STEPS { simulate_step(scene, sim_state, config) } } case .Single: simulate_step(scene, get_next_sim_state(scene), config) num_steps += 1 } if num_steps > 0 && commit { flip_sim_state(scene) } state.simulation_frame += 1 state.num_referenced_bodies = 0 state.num_referenced_suspension_constraints = 0 } GLOBAL_PLANE :: collision.Plane { normal = Vec3{0, 1, 0}, dist = 0, } Contact :: struct { a, b: Body_Handle, prev_x_a, prev_x_b: Vec3, prev_q_a, prev_q_b: Quat, manifold: collision.Contact_Manifold, applied_corrections: int, // For PGS total_normal_impulse: [4]f32, // xy - tangent and bitangent (linear friction), z - twist friction around normal (rotational impulse only) total_friction_impulse: [4]Vec2, // XPBD stuff lambda_normal: [4]f32, lambda_tangent: f32, applied_static_friction: bool, applied_normal_correction: [4]f32, } update_contacts :: proc(sim_state: ^Sim_State) { tracy.Zone() graph_color_bitmask: [4]bit_array.Bit_Array for i in 0 ..< len(graph_color_bitmask) { bit_array.init( &graph_color_bitmask[i], len(sim_state.bodies_slice), 0, context.temp_allocator, ) } for contact_idx in 0 ..< len(sim_state.contact_container.contacts) { contact := &sim_state.contact_container.contacts[contact_idx] i, j := i32(contact.a) - 1, i32(contact.b) - 1 body, body2 := &sim_state.bodies_slice[i], &sim_state.bodies_slice[j] assert(body.alive) assert(body2.alive) old_manifold := contact.manifold old_total_normal_impulse := contact.total_normal_impulse old_total_friction_impulse := contact.total_friction_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_friction_impulse = 0 contact.lambda_normal = 0 contact.lambda_tangent = 0 contact.applied_static_friction = false contact.applied_normal_correction = 0 aabb1, aabb2 := body_get_aabb(body), body_get_aabb(body2) // TODO: extract common math functions into a sane place if !collision.test_aabb_vs_aabb( {min = aabb1.center - aabb1.extent, max = aabb1.center + aabb1.extent}, {min = aabb2.center - aabb2.extent, max = aabb2.center + aabb2.extent}, ) { continue } m1, m2 := 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) if collision { assert(raw_manifold.points_len > 0) manifold := &contact.manifold manifold^ = raw_manifold // 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( body, manifold.points_a[point_idx], ) manifold.points_b[point_idx] = body_world_to_local( body2, manifold.points_b[point_idx], ) } 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 for point_idx in 0 ..< manifold.points_len { contact.total_friction_impulse[point_idx].x = lg.dot( old_total_friction_impulse[point_idx].x * old_manifold.tangent, manifold.tangent, ) contact.total_friction_impulse[point_idx].y = lg.dot( old_total_friction_impulse[point_idx].y * old_manifold.bitangent, manifold.bitangent, ) } } } } } 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(30, 0.8, dt) if !apply_bias { mass_coef = 1 bias_rate = 0 impulse_coef = 0 } random_order := make([]i32, len(sim_state.contact_container.contacts), context.temp_allocator) { for i in 0 ..< len(random_order) { random_order[i] = i32(i) } // for i in 0 ..< len(random_order) - 1 { // j := rand.int_max(len(random_order)) // slice.swap(random_order, i, j) // } } for i in 0 ..< len(sim_state.contact_container.contacts) { contact := &sim_state.contact_container.contacts[random_order[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 := v2 - v1 { delta_v_norm := lg.dot(delta_v, manifold.normal) bias := f32(0.0) MAX_BAUMGARTE_VELOCITY :: 4.0 if separation > 0 { bias = separation * inv_dt } else if apply_bias { bias = lg.max(bias_rate * separation, -MAX_BAUMGARTE_VELOCITY) } incremental_impulse := -inv_w * mass_coef * (delta_v_norm + bias) - 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) } { DYNAMIC_FRICTION :: 0.6 STATIC_FRICTION :: 0.8 STATIC_FRICTION_VELOCITY_THRESHOLD :: 0.01 delta_v_tang := Vec2 { lg.dot(delta_v, manifold.tangent), lg.dot(delta_v, manifold.bitangent), } use_static_friction := lg.dot(delta_v_tang, delta_v_tang) < STATIC_FRICTION_VELOCITY_THRESHOLD * STATIC_FRICTION_VELOCITY_THRESHOLD friction: f32 = use_static_friction ? STATIC_FRICTION : DYNAMIC_FRICTION friction_clamp := contact.total_normal_impulse[point_idx] * friction incremental_impulse := -inv_w * delta_v_tang new_total_impulse: Vec2 = lg.clamp( contact.total_friction_impulse[point_idx] + incremental_impulse, Vec2(-friction_clamp), Vec2(friction_clamp), ) applied_impulse := new_total_impulse - contact.total_friction_impulse[point_idx] contact.total_friction_impulse[point_idx] = new_total_impulse applied_impulse_vec := applied_impulse.x * manifold.tangent + applied_impulse.y * manifold.bitangent apply_velocity_correction(body1, -applied_impulse_vec, p1) apply_velocity_correction(body2, applied_impulse_vec, p2) } } else { contact.total_normal_impulse[point_idx] = 0 contact.total_friction_impulse[point_idx] = 0 } } } } pgs_solve_suspension :: proc(sim_state: ^Sim_State, config: Solver_Config, dt: f32, inv_dt: f32) { // Solve suspension velocity for _, i in sim_state.suspension_constraints { v := &sim_state.suspension_constraints_slice[i] if v.alive { body := get_body(sim_state, v.body) if body.alive { wheel_world_pos := body_local_to_world(body, v.rel_pos) dir := body_local_to_world_vec(body, v.rel_dir) pos2 := wheel_world_pos + dir * v.rest v.hit_t, v.hit_point, v.hit = collision.intersect_segment_plane( {wheel_world_pos, pos2}, collision.plane_from_point_normal({}, collision.Vec3{0, 1, 0}), ) forward := body_local_to_world_vec(body, Vec3{0, 0, 1}) contact_patch_linear_vel := body_velocity_at_point(body, v.hit_point) + (v.radius * v.w * forward) w := get_body_angular_inverse_mass(body, dir) inv_w := 1.0 / w if v.hit { { bias_coef, mass_coef, impulse_coef := calculate_soft_constraint_params( v.natural_frequency, v.damping, dt, ) vel := lg.dot(body_velocity_at_point(body, wheel_world_pos), dir) x := v.hit_t separation := v.rest - x incremental_impulse := -inv_w * mass_coef * (vel + separation * bias_coef) - impulse_coef * v.spring_impulse v.spring_impulse += incremental_impulse apply_velocity_correction(body, incremental_impulse * dir, wheel_world_pos) } right := wheel_get_right_vec(body, v) // Longitudinal friction if true { vel_long := lg.dot(contact_patch_linear_vel, forward) friction := f32(1) friction_clamp := abs(v.spring_impulse) * friction incremental_impulse := -v.inv_inertia * vel_long new_total_impulse := clamp( v.longitudinal_impulse + incremental_impulse, -friction_clamp, friction_clamp, ) applied_impulse := new_total_impulse - v.longitudinal_impulse v.longitudinal_impulse = new_total_impulse v.w += v.inv_inertia * applied_impulse } // Drive forces if true { total_impulse := v.drive_impulse - v.brake_impulse apply_velocity_correction( body, total_impulse * forward * dt, wheel_world_pos, ) } // Lateral friction if true { vel_contact := body_velocity_at_point(body, wheel_world_pos) lateral_vel := lg.dot(right, vel_contact) friction := f32(1) friction_clamp := -v.spring_impulse * friction incremental_impulse := -inv_w * lateral_vel new_total_impulse := clamp( v.lateral_impulse + incremental_impulse, -friction_clamp, friction_clamp, ) applied_impulse := new_total_impulse - v.lateral_impulse v.lateral_impulse = new_total_impulse apply_velocity_correction(body, applied_impulse * right, wheel_world_pos) } } else { v.lateral_impulse = 0 v.spring_impulse = 0 v.longitudinal_impulse = 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] if body.alive { body.v += config.gravity * dt * (body.inv_mass == 0 ? 0 : 1) // special case for gravity, TODO } } // 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) total_tangent_impulse := contact.total_friction_impulse[point_idx].x * manifold.tangent + contact.total_friction_impulse[point_idx].y * manifold.bitangent apply_velocity_correction(body1, -total_tangent_impulse, p1) apply_velocity_correction(body2, total_tangent_impulse, p2) } } for i in 0 ..< len(sim_state.suspension_constraints) { s := &sim_state.suspension_constraints_slice[i] if s.hit { body := get_body(sim_state, s.body) p := body_local_to_world(body, s.rel_pos) right := wheel_get_right_vec(body, s) apply_velocity_correction( body, s.spring_impulse * body_local_to_world_vec(body, s.rel_dir), p, ) apply_velocity_correction(body, s.lateral_impulse * right, p) s.w += s.inv_inertia * s.longitudinal_impulse } } } apply_bias := true pgs_solve_contacts(sim_state, config, dt, inv_dt, apply_bias) pgs_solve_suspension(sim_state, config, dt, inv_dt) for i in 0 ..< len(sim_state.bodies_slice) { body := &sim_state.bodies_slice[i] if body.alive { body.x += body.v * dt // NOTE: figure out how this works https://fgiesen.wordpress.com/2012/08/24/quaternion-differentiation/ q := body.q w := body.w delta_rot := quaternion(x = w.x, y = w.y, z = w.z, w = 0) delta_rot = delta_rot * q q.x += 0.5 * dt * delta_rot.x q.y += 0.5 * dt * delta_rot.y q.z += 0.5 * dt * delta_rot.z q.w += 0.5 * dt * delta_rot.w q = lg.normalize0(q) body.q = q } } for i in 0 ..< len(sim_state.suspension_constraints_slice) { s := &sim_state.suspension_constraints_slice[i] s.q = math.mod_f32(s.q + 0.5 * s.w * dt, math.PI * 2) } apply_bias = false pgs_solve_contacts(sim_state, config, dt, inv_dt, apply_bias) // pgs_solve_suspension(sim_state, config, dt, inv_dt, apply_bias) } simulate_step :: proc(scene: ^Scene, sim_state: ^Sim_State, config: Solver_Config) { tracy.Zone() substeps := config.substreps_minus_one + 1 dt := config.timestep / f32(substeps) inv_dt := 1.0 / dt tlas := build_tlas(sim_state, config) { tracy.ZoneN("simulate_step::remove_invalid_contacts") i := 0 for i < len(sim_state.contact_container.contacts) { contact := sim_state.contact_container.contacts[i] aabb_a := tlas.body_aabbs[int(contact.a) - 1] aabb_b := tlas.body_aabbs[int(contact.b) - 1] if !bvh.test_aabb_vs_aabb(aabb_a, aabb_b) { removed_pair := make_contact_pair(i32(contact.a) - 1, i32(contact.b) - 1) delete_key(&sim_state.contact_container.lookup, removed_pair) unordered_remove_soa(&sim_state.contact_container.contacts, i) if i < len(sim_state.contact_container.contacts) { moved_contact := &sim_state.contact_container.contacts[i] moved_pair := make_contact_pair( i32(moved_contact.a) - 1, i32(moved_contact.b) - 1, ) sim_state.contact_container.lookup[moved_pair] = i32(i) } } else { i += 1 } } } find_new_contacts(sim_state, &tlas) update_contacts(sim_state) Solver :: enum { XPBD, PGS, } solver := Solver.PGS switch solver { case .XPBD: for _ in 0 ..< substeps { xpbd_substep(sim_state, config, dt, inv_dt) } case .PGS: for _ in 0 ..< substeps { pgs_substep(sim_state, config, dt, inv_dt) } } } body_solve_velocity :: #force_inline proc( body: Body_Ptr, prev_x: Vec3, prev_q: Quat, inv_dt: f32, ) { body.v = (body.x - prev_x) * inv_dt delta_q := body.q * lg.quaternion_inverse(prev_q) body.w = Vec3{delta_q.x, delta_q.y, delta_q.z} * 2.0 * inv_dt if delta_q.w < 0 { body.w = -body.w } } calculate_constraint_params1 :: proc( dt: f32, body: Body_Ptr, compliance: f32, error: f32, error_gradient: Vec3, pos: Vec3, other_combined_inv_mass: f32, ) -> ( lambda: f32, w: f32, correction: Vec3, ok: bool, ) { if error == 0 { return } w = get_body_inverse_mass(body, error_gradient, pos) w += other_combined_inv_mass if w == 0 { return } ok = true alpha := compliance / dt / dt lambda = -error / (w + alpha) correction = -error_gradient * -lambda return } calculate_constraint_params2 :: proc( dt: f32, body1: Body_Ptr, body2: Body_Ptr, compliance: f32, error: f32, error_gradient: Vec3, pos1: Vec3, pos2: Vec3, ) -> ( lambda: f32, correction1: Vec3, correction2: Vec3, ok: bool, ) { if error == 0 { return } w := get_body_inverse_mass(body1, error_gradient, pos1) w += get_body_inverse_mass(body2, error_gradient, pos2) if w == 0 { return } ok = true alpha := compliance / dt / dt lambda = -error / (w + alpha) correction1 = -error_gradient * -lambda correction2 = error_gradient * -lambda return } apply_constraint_correction_unilateral :: proc( dt: f32, body: Body_Ptr, compliance: f32, error: f32, error_gradient: Vec3, pos: Vec3, other_combined_inv_mass: f32 = 0, ) -> ( lambda: f32, ) { w: f32 correction: Vec3 ok: bool lambda, w, correction, ok = calculate_constraint_params1( dt, body, compliance, error, error_gradient, pos, other_combined_inv_mass, ) if ok { apply_position_correction(body, correction, pos) } return } multiply_inv_intertia :: proc(body: Body_Ptr, vec: Vec3) -> (result: Vec3) { q := body.q inv_q := lg.quaternion_normalize0(lg.quaternion_inverse(q)) result = lg.quaternion_mul_vector3(inv_q, vec) result *= body.inv_inertia_tensor result = lg.quaternion_mul_vector3(q, result) return result } apply_velocity_correction :: #force_inline proc "contextless" ( body: Body_Ptr, impulse: Vec3, pos: Vec3, ) { apply_velocity_correction_linear(body, impulse) angular_impulse := lg.cross(pos - body.x, impulse) apply_velocity_correction_angular(body, angular_impulse) } apply_velocity_correction_linear :: #force_inline proc "contextless" ( body: Body_Ptr, impulse: Vec3, ) { body.v += impulse * body.inv_mass } apply_velocity_correction_angular :: #force_inline proc "contextless" ( body: Body_Ptr, angular_impulse: Vec3, ) { q := body.q inv_q := lg.quaternion_normalize0(lg.quaternion_inverse(q)) delta_omega := lg.quaternion_mul_vector3(inv_q, angular_impulse) 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 inv_q := lg.quaternion_normalize0(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 } // Total inverse mass (linear + angular) get_body_inverse_mass :: proc(body: Body_Ptr, normal, pos: Vec3) -> f32 { linear, angular := get_body_inverse_mass_separate(body, normal, pos) return linear + angular } get_body_inverse_mass_separate :: proc( body: Body_Ptr, normal, pos: Vec3, ) -> ( linear: f32, angular: f32, ) { q := body.q inv_q := lg.quaternion_normalize0(lg.quaternion_inverse(q)) rn := pos - body.x rn = lg.cross(rn, normal) rn = lg.quaternion_mul_vector3(inv_q, rn) linear = body.inv_mass angular = lg.dot(rn, rn * body.inv_inertia_tensor) return } 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) }