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. # Build the game.
echo "Building game$DLL_EXT" 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. # 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 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 { SOLVER_CONFIG :: physics.Solver_Config {
timestep = 1.0 / 60, timestep = 1.0 / 60,
gravity = rl.Vector3{0, -9.8, 0}, gravity = rl.Vector3{0, -9.8, 0},
substreps_minus_one = 4 - 1, substreps_minus_one = 8 - 1,
} }
Game_Memory :: struct { Game_Memory :: struct {
@ -277,7 +277,7 @@ update_runtime_world :: proc(runtime_world: ^Runtime_World, dt: f32) {
) )
if true { if true {
for x in 0 ..< 20 { for x in 0 ..< 5 {
for y in 0 ..< 20 { for y in 0 ..< 20 {
physics.immediate_body( physics.immediate_body(
&world.physics_scene, &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 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 { Contact_Manifold :: struct {
type: Contact_Type,
normal: Vec3, normal: Vec3,
separation: f32, separation: f32,
points_a: [4]Vec3, points_a: [4]Vec3,
@ -274,7 +290,8 @@ create_face_contact_manifold :: proc(
ref_convex := is_ref_a ? a : b ref_convex := is_ref_a ? a : b
inc_convex := is_ref_a ? b : a 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 // incident face
inc_face: halfedge.Face inc_face: halfedge.Face
@ -492,9 +509,17 @@ create_face_contact_manifold :: proc(
} }
if is_ref_a { 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_a = ref_points
manifold.points_b = inc_points manifold.points_b = inc_points
} else { } 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_b = ref_points
manifold.points_a = inc_points manifold.points_a = inc_points
} }
@ -526,6 +551,10 @@ create_edge_contact_manifold :: proc(
_, ps := closest_point_between_segments(a1, a2, b1, b2) _, 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.normal = separating_plane.normal
manifold.separation = separation manifold.separation = separation
manifold.points_a[0] = ps[0] 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 { for &contact, contact_idx in sim_state.contact_container.contacts {
points_a := contact.manifold.points_a points_a := contact.manifold.points_a
points_b := contact.manifold.points_b 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, prev_q_a, prev_q_b: Quat,
manifold: collision.Contact_Manifold, manifold: collision.Contact_Manifold,
applied_corrections: int, applied_corrections: int,
// For PGS
total_normal_impulse: [4]f32,
total_tangent_impulse: [4]f32,
// XPBD stuff
lambda_normal: [4]f32, lambda_normal: [4]f32,
lambda_tangent: [4]f32, lambda_tangent: f32,
applied_static_friction: [4]bool, applied_static_friction: bool,
applied_normal_correction: [4]f32, applied_normal_correction: [4]f32,
} }
@ -245,11 +251,17 @@ update_contacts :: proc(sim_state: ^Sim_State) {
assert(body.alive) assert(body.alive)
assert(body2.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_a = body.x
contact.prev_x_b = body2.x contact.prev_x_b = body2.x
contact.prev_q_a = body.q contact.prev_q_a = body.q
contact.prev_q_b = body2.q contact.prev_q_b = body2.q
contact.manifold = {} contact.manifold = {}
contact.total_normal_impulse = 0
contact.total_tangent_impulse = 0
contact.lambda_normal = 0 contact.lambda_normal = 0
contact.lambda_tangent = 0 contact.lambda_tangent = 0
contact.applied_static_friction = false contact.applied_static_friction = false
@ -276,6 +288,15 @@ update_contacts :: proc(sim_state: ^Sim_State) {
manifold := &contact.manifold manifold := &contact.manifold
manifold^ = raw_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 // Convert manifold contact from world to local space
for point_idx in 0 ..< manifold.points_len { for point_idx in 0 ..< manifold.points_len {
manifold.points_a[point_idx] = body_world_to_local( 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 manifold := &contact.manifold
for point_idx in 0 ..< manifold.points_len { 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) p_diff_normal: f32 = lg.dot(p2 - p1, manifold.normal)
separation := min(p_diff_normal, 0) separation: f32 = min(p_diff_normal, 0)
lambda_norm, corr1, corr2, ok := calculate_constraint_params2( lambda_norm, corr1, corr2, ok := calculate_constraint_params2(
dt, dt,
body, body,
body2, body2,
0.00002, 0.00002,
separation, -separation,
-manifold.normal, manifold.normal,
p1, p1,
p2, p2,
) )
if ok { if ok {
contact.applied_normal_correction[point_idx] = -separation contact.applied_normal_correction[point_idx] = -separation
contact.applied_corrections += 1 contact.applied_corrections += 1
contact.lambda_normal[point_idx] = lambda_norm contact.lambda_normal[point_idx] = lambda_norm
apply_correction(body, corr1, p1) apply_position_correction(body, corr1, p1)
apply_correction(body2, corr2, p2) apply_position_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
} }
}
min_y1 := find_min_contact_y(sim_state, c1) if false && contact.lambda_normal[point_idx] != 0 {
min_y2 := find_min_contact_y(sim_state, c2) 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 := prev_p1 :=
body.prev_x + body.prev_x +
lg.quaternion_mul_vector3(body.prev_q, manifold.points_a[point_idx]) 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 + body2.prev_x +
lg.quaternion_mul_vector3(body2.prev_q, manifold.points_b[point_idx]) 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 p_diff_tangent -= lg.dot(p_diff_tangent, manifold.normal) * manifold.normal
tangent_diff_len := lg.length(p_diff_tangent) 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, dt,
body, body,
body2, body2,
0, 0.00002,
-tangent_diff_len, -tangent_diff_len,
-tangent_diff_normalized, -tangent_diff_normalized,
p1, p1,
p2, p2,
) )
STATIC_FRICTION :: 0 STATIC_FRICTION :: 1000
if ok_tangent && delta_lambda_tangent < STATIC_FRICTION * lambda_norm { if ok_tangent &&
contact.applied_static_friction[point_idx] = true delta_lambda_tangent >
contact.lambda_tangent[point_idx] = 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_position_correction(body, corr1_tangent, p1)
apply_correction(body2, corr2_tangent, p2) 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") tracy.ZoneN("simulate_step::suspension_constraints")
for &v in sim_state.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 { for &contact in sim_state.contact_container.contacts {
manifold := &contact.manifold manifold := &contact.manifold
body, body2 := get_body(sim_state, contact.a), get_body(sim_state, contact.b) body, body2 := get_body(sim_state, contact.a), get_body(sim_state, contact.b)
prev_q1, prev_q2 := body.prev_q, body2.prev_q 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") tracy.ZoneN("simulate_step::dynamic_friction")
for &contact in sim_state.contact_container.contacts { for &contact in sim_state.contact_container.contacts {
if contact.manifold.points_len == 0 {
continue
}
manifold := &contact.manifold manifold := &contact.manifold
body1 := get_body(sim_state, contact.a) body1 := get_body(sim_state, contact.a)
body2 := get_body(sim_state, contact.b) 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 { 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 continue
} }
total_lambda_normal += contact.lambda_normal[point_idx]
friciton_points_len += 1
p1, p2 := p1, p2 :=
body_local_to_world(body1, manifold.points_a[point_idx]), body_local_to_world(body1, manifold.points_a[point_idx]),
body_local_to_world(body2, manifold.points_b[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) friction_p1 += p1
v2 := body_velocity_at_point(body2, p2) 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 := v1 - v2
v_normal := lg.dot(manifold.normal, v) * manifold.normal v_normal := lg.dot(manifold.normal, v) * manifold.normal
v_tangent := v - v_normal v_tangent := v - v_normal
DYNAMIC_FRICTION :: 1 DYNAMIC_FRICTION :: 0.5
v_tangent_len := lg.length(v_tangent) v_tangent_len := lg.length(v_tangent)
if v_tangent_len > 0 { if v_tangent_len > 0 {
v_tangent_norm := v_tangent / v_tangent_len v_tangent_norm := v_tangent / v_tangent_len
w1, w2 := w1, w2 :=
get_body_inverse_mass(body1, v_tangent_norm, p1), get_body_inverse_mass(body1, v_tangent_norm, friction_p1),
get_body_inverse_mass(body2, v_tangent_norm, p2) get_body_inverse_mass(body2, v_tangent_norm, friction_p2)
w := w1 + w2 w := w1 + w2
if w > 0 { if w != 0 {
delta_v := delta_v :=
-v_tangent_norm * -v_tangent_norm *
min( min(
dt * dt * DYNAMIC_FRICTION * abs(total_lambda_normal / (dt * dt)),
DYNAMIC_FRICTION *
abs(contact.lambda_normal[point_idx] / (dt * dt)),
v_tangent_len / w, 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)) 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, dt,
body, body,
0, 0,
error = damping, error = -damping,
error_gradient = -dir, error_gradient = dir,
pos = wheel_world_pos, pos = wheel_world_pos,
) )
@ -660,8 +769,8 @@ xpbd_substep :: proc(sim_state: ^Sim_State, config: Solver_Config, dt: f32, inv_
dt, dt,
body, body,
0, 0,
total_impulse * dt * dt, -total_impulse * dt * dt,
-forward, forward,
wheel_world_pos, wheel_world_pos,
) )
body_solve_velocity(body, body.prev_x, body.prev_q, inv_dt) 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 corr := right * impulse * dt
v.applied_impulse.x = impulse 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) 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) { pgs_substep :: proc(sim_state: ^Sim_State, config: Solver_Config, dt: f32, inv_dt: f32) {
for i in 0 ..< len(sim_state.bodies_slice) { for i in 0 ..< len(sim_state.bodies_slice) {
body := &sim_state.bodies_slice[i] 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 // Warm start
// for i in 0 ..< len(sim_state.contact_pairs) { if true {
// contact_pair := &sim_state.contact_pairs[i] 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) { for i in 0 ..< len(sim_state.bodies_slice) {
body := &sim_state.bodies_slice[i] 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 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) { 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, PGS,
} }
solver := Solver.XPBD solver := Solver.PGS
switch solver { switch solver {
case .XPBD: case .XPBD:
@ -858,7 +1072,7 @@ calculate_constraint_params2 :: proc(
return 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) w += get_body_inverse_mass(body2, error_gradient, pos2)
if w == 0 { if w == 0 {
@ -900,7 +1114,7 @@ apply_constraint_correction_unilateral :: proc(
) )
if ok { if ok {
apply_correction(body, correction, pos) apply_position_correction(body, correction, pos)
} }
return return
@ -917,10 +1131,21 @@ multiply_inv_intertia :: proc(body: Body_Ptr, vec: Vec3) -> (result: Vec3) {
return result return result
} }
apply_correction :: proc(body: Body_Ptr, corr: Vec3, pos: Vec3) { apply_velocity_correction :: proc(body: Body_Ptr, impulse: Vec3, pos: Vec3) {
// rl.DrawSphereWires(pos, 0.5, 4, 4, rl.BLUE) body.v += impulse * body.inv_mass
// rl.DrawLine3D(pos, pos + corr, rl.BLUE)
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 body.x += corr * body.inv_mass
q := body.q q := body.q
@ -955,3 +1180,12 @@ get_body_inverse_mass :: proc(body: Body_Ptr, normal, pos: Vec3) -> f32 {
return w 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)
}