sergeypdev/gutter_runner
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

More progress on PGS

Browse Source
This commit is contained in:
sergeypdev 2025-03-09 22:54:40 +04:00
parent a2ad9e490a
commit d1feaa0602
5 changed files with 374 additions and 111 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

2
build_hot_reload.sh
View File

@ -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

4
game/game.odin
View File

@ -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,

31
game/physics/collision/convex.odin
View File

@ -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]

2
game/physics/debug.odin
View File

@ -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

446
game/physics/simulation.odin
View File

@ -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)
}