Start implementing a different solver
This commit is contained in:
parent
ccd987aeae
commit
cb24365933
@ -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 = 8 - 1,
|
||||
substreps_minus_one = 4 - 1,
|
||||
}
|
||||
|
||||
Game_Memory :: struct {
|
||||
@ -277,8 +277,8 @@ update_runtime_world :: proc(runtime_world: ^Runtime_World, dt: f32) {
|
||||
)
|
||||
|
||||
if true {
|
||||
for x in 0 ..< 40 {
|
||||
for y in 0 ..< 5 {
|
||||
for x in 0 ..< 20 {
|
||||
for y in 0 ..< 20 {
|
||||
physics.immediate_body(
|
||||
&world.physics_scene,
|
||||
&runtime_world.solver_state,
|
||||
@ -287,7 +287,7 @@ update_runtime_world :: proc(runtime_world: ^Runtime_World, dt: f32) {
|
||||
initial_pos = {0, 0.5 + f32(y) * 1.1, f32(x) * 3 + 10},
|
||||
initial_rot = linalg.QUATERNIONF32_IDENTITY,
|
||||
shape = physics.Shape_Box{size = 1},
|
||||
mass = 1,
|
||||
mass = 10,
|
||||
},
|
||||
)
|
||||
}
|
||||
|
||||
@ -10,6 +10,7 @@ import "core:math/rand"
|
||||
import "core:slice"
|
||||
import "libs:tracy"
|
||||
|
||||
_ :: rand
|
||||
_ :: math
|
||||
_ :: fmt
|
||||
_ :: slice
|
||||
@ -79,24 +80,16 @@ Step_Mode :: enum {
|
||||
|
||||
Potential_Pair :: [2]u16
|
||||
|
||||
// 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,
|
||||
) {
|
||||
// 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()
|
||||
assert(config.timestep > 0)
|
||||
|
||||
prune_immediate(scene, state)
|
||||
|
||||
sim_state_copy(get_next_sim_state(scene), get_sim_state(scene))
|
||||
sim_state_copy(&scene.scratch_sim_state, get_sim_state(scene))
|
||||
|
||||
sim_state := get_next_sim_state(scene)
|
||||
|
||||
body_aabbs := make([]bvh.AABB, sim_state.num_bodies, context.temp_allocator)
|
||||
body_indices := make([]u16, sim_state.num_bodies, context.temp_allocator)
|
||||
@ -125,35 +118,35 @@ simulate :: proc(
|
||||
|
||||
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_potential_pairs :: proc(sim_state: ^Sim_State, tlas: ^TLAS) -> []Potential_Pair {
|
||||
tracy.Zone()
|
||||
potential_pairs_map := make(map[Potential_Pair]bool, context.temp_allocator)
|
||||
|
||||
{
|
||||
tracy.ZoneN("physics.simulate::find_potential_pairs")
|
||||
for i in 0 ..< len(sim_state.bodies_slice) {
|
||||
assert(i <= int(max(u16)))
|
||||
body_idx := u16(i)
|
||||
body := &sim_state.bodies_slice[i]
|
||||
|
||||
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)
|
||||
|
||||
if body.alive {
|
||||
body_aabb := body_aabbs[i]
|
||||
it := bvh.iterator_intersect_leaf(&sim_state_bvh, body_aabb)
|
||||
for leaf_node in bvh.iterator_intersect_leaf_next(&it) {
|
||||
for i in 0 ..< leaf_node.prim_len {
|
||||
other_body_idx := tlas.bvh_tree.primitives[leaf_node.child_or_prim_start + i]
|
||||
prim_aabb := tlas.body_aabbs[other_body_idx]
|
||||
|
||||
for leaf_node in bvh.iterator_intersect_leaf_next(&it) {
|
||||
for i in 0 ..< leaf_node.prim_len {
|
||||
other_body_idx :=
|
||||
sim_state_bvh.primitives[leaf_node.child_or_prim_start + i]
|
||||
prim_aabb := body_aabbs[other_body_idx]
|
||||
|
||||
if body_idx != other_body_idx &&
|
||||
bvh.test_aabb_vs_aabb(body_aabb, prim_aabb) {
|
||||
pair := Potential_Pair {
|
||||
min(body_idx, other_body_idx),
|
||||
max(body_idx, other_body_idx),
|
||||
}
|
||||
|
||||
potential_pairs_map[pair] = true
|
||||
if body_idx != other_body_idx && bvh.test_aabb_vs_aabb(body_aabb, prim_aabb) {
|
||||
pair := Potential_Pair {
|
||||
min(body_idx, other_body_idx),
|
||||
max(body_idx, other_body_idx),
|
||||
}
|
||||
|
||||
potential_pairs_map[pair] = true
|
||||
}
|
||||
}
|
||||
}
|
||||
@ -170,11 +163,26 @@ simulate :: proc(
|
||||
}
|
||||
}
|
||||
|
||||
resize_soa(&sim_state.contact_pairs, 0)
|
||||
predict_positions(&scene.scratch_sim_state, config, config.timestep)
|
||||
find_collisions(&scene.scratch_sim_state, &sim_state.contact_pairs, potential_pairs)
|
||||
return potential_pairs
|
||||
}
|
||||
|
||||
// bvh.debug_draw_bvh_bounds(&sim_state_bvh, body_aabbs, 0)
|
||||
// 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 {
|
||||
@ -186,11 +194,11 @@ simulate :: proc(
|
||||
state.accumulated_time -= config.timestep
|
||||
|
||||
if num_steps < MAX_STEPS {
|
||||
simulate_step(sim_state, config)
|
||||
simulate_step(scene, sim_state, config)
|
||||
}
|
||||
}
|
||||
case .Single:
|
||||
simulate_step(get_next_sim_state(scene), config)
|
||||
simulate_step(scene, get_next_sim_state(scene), config)
|
||||
num_steps += 1
|
||||
}
|
||||
|
||||
@ -203,13 +211,6 @@ simulate :: proc(
|
||||
state.num_referenced_suspension_constraints = 0
|
||||
}
|
||||
|
||||
Body_Sim_State :: struct {
|
||||
prev_x: Vec3,
|
||||
prev_v: Vec3,
|
||||
prev_w: Vec3,
|
||||
prev_q: Quat,
|
||||
}
|
||||
|
||||
GLOBAL_PLANE :: collision.Plane {
|
||||
normal = Vec3{0, 1, 0},
|
||||
dist = 0,
|
||||
@ -299,7 +300,7 @@ find_collisions :: proc(
|
||||
}
|
||||
}
|
||||
|
||||
predict_positions :: proc(sim_state: ^Sim_State, config: Solver_Config, dt: f32) {
|
||||
xpbd_predict_positions :: proc(sim_state: ^Sim_State, config: Solver_Config, dt: f32) {
|
||||
// Integrate positions and rotations
|
||||
for &body in sim_state.bodies {
|
||||
if body.alive {
|
||||
@ -328,7 +329,413 @@ predict_positions :: proc(sim_state: ^Sim_State, config: Solver_Config, dt: f32)
|
||||
}
|
||||
}
|
||||
|
||||
simulate_step :: proc(sim_state: ^Sim_State, config: Solver_Config) {
|
||||
xpbd_substep :: proc(sim_state: ^Sim_State, config: Solver_Config, dt: f32, inv_dt: f32) {
|
||||
xpbd_predict_positions(sim_state, config, dt)
|
||||
|
||||
Body_Pair :: struct {
|
||||
a, b: int,
|
||||
}
|
||||
|
||||
{
|
||||
tracy.ZoneN("simulate_step::solve_collisions")
|
||||
for i in 0 ..< len(sim_state.contact_pairs) {
|
||||
contact_pair := &sim_state.contact_pairs[i]
|
||||
body, body2 := get_body(sim_state, contact_pair.a), get_body(sim_state, contact_pair.b)
|
||||
|
||||
contact_pair^ = Contact_Pair {
|
||||
a = contact_pair.a,
|
||||
b = contact_pair.b,
|
||||
prev_x_a = body.x,
|
||||
prev_x_b = body2.x,
|
||||
prev_q_a = body.q,
|
||||
prev_q_b = body2.q,
|
||||
manifold = contact_pair.manifold,
|
||||
}
|
||||
|
||||
manifold := &contact_pair.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)
|
||||
|
||||
p_diff_normal := lg.dot(p2 - p1, manifold.normal)
|
||||
separation := 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_pair.applied_normal_correction[point_idx] = -separation
|
||||
contact_pair.applied_corrections += 1
|
||||
contact_pair.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_Pair) -> bool {
|
||||
sim_state := cast(^Sim_State)context.user_ptr
|
||||
|
||||
find_min_contact_y :: proc(
|
||||
scene: ^Sim_State,
|
||||
c: Contact_Pair,
|
||||
) -> (
|
||||
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)
|
||||
min_y2 := find_min_contact_y(sim_state, c2)
|
||||
|
||||
return min_y1 > min_y2
|
||||
},
|
||||
)
|
||||
}
|
||||
|
||||
for &contact_pair in sim_state.contact_pairs {
|
||||
manifold := contact_pair.manifold
|
||||
body, body2 := get_body(sim_state, contact_pair.a), get_body(sim_state, contact_pair.b)
|
||||
|
||||
for point_idx in 0 ..< manifold.points_len {
|
||||
lambda_norm := contact_pair.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])
|
||||
prev_p2 :=
|
||||
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 -= 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,
|
||||
body,
|
||||
body2,
|
||||
0,
|
||||
-tangent_diff_len,
|
||||
-tangent_diff_normalized,
|
||||
p1,
|
||||
p2,
|
||||
)
|
||||
|
||||
STATIC_FRICTION :: 0
|
||||
if ok_tangent && delta_lambda_tangent < STATIC_FRICTION * lambda_norm {
|
||||
contact_pair.applied_static_friction[point_idx] = true
|
||||
contact_pair.lambda_tangent[point_idx] = delta_lambda_tangent
|
||||
|
||||
apply_correction(body, corr1_tangent, p1)
|
||||
apply_correction(body2, corr2_tangent, p2)
|
||||
}
|
||||
}
|
||||
}
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
{
|
||||
tracy.ZoneN("simulate_step::suspension_constraints")
|
||||
|
||||
for &v in sim_state.suspension_constraints {
|
||||
if v.alive {
|
||||
body := get_body(sim_state, v.body)
|
||||
|
||||
pos := body_local_to_world(body, v.rel_pos)
|
||||
dir := body_local_to_world_vec(body, v.rel_dir)
|
||||
pos2 := pos + dir * v.rest
|
||||
v.hit_t, v.hit_point, v.hit = collision.intersect_segment_plane(
|
||||
{pos, pos2},
|
||||
collision.plane_from_point_normal({}, collision.Vec3{0, 1, 0}),
|
||||
)
|
||||
if v.hit {
|
||||
corr := pos - v.hit_point
|
||||
distance := lg.length(corr)
|
||||
corr = corr / distance if distance > 0 else 0
|
||||
|
||||
_ = apply_constraint_correction_unilateral(
|
||||
dt,
|
||||
body,
|
||||
v.compliance,
|
||||
error = distance - v.rest,
|
||||
error_gradient = corr,
|
||||
pos = pos,
|
||||
other_combined_inv_mass = 0,
|
||||
)
|
||||
}
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
{
|
||||
// Compute new linear and angular velocities
|
||||
for _, i in sim_state.bodies_slice {
|
||||
body := &sim_state.bodies_slice[i]
|
||||
if body.alive {
|
||||
body_solve_velocity(body, body.prev_x, body.prev_q, inv_dt)
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
// Restituion
|
||||
if true {
|
||||
tracy.ZoneN("simulate_step::restitution")
|
||||
|
||||
for &pair in sim_state.contact_pairs {
|
||||
manifold := &pair.manifold
|
||||
|
||||
|
||||
body, body2 := get_body(sim_state, pair.a), get_body(sim_state, pair.b)
|
||||
prev_q1, prev_q2 := body.prev_q, body2.prev_q
|
||||
|
||||
for point_idx in 0 ..< manifold.points_len {
|
||||
if pair.lambda_normal[point_idx] == 0 {
|
||||
continue
|
||||
}
|
||||
prev_r1 := lg.quaternion_mul_vector3(prev_q1, manifold.points_a[point_idx])
|
||||
prev_r2 := lg.quaternion_mul_vector3(prev_q2, manifold.points_b[point_idx])
|
||||
|
||||
r1 := lg.quaternion_mul_vector3(body.q, manifold.points_a[point_idx])
|
||||
r2 := lg.quaternion_mul_vector3(body2.q, manifold.points_b[point_idx])
|
||||
|
||||
prev_v :=
|
||||
(body.prev_v + lg.cross(body.prev_w, prev_r1)) -
|
||||
(body2.prev_v + lg.cross(body2.prev_w, prev_r2))
|
||||
v := (body.v + lg.cross(body.w, r1)) - (body2.v + lg.cross(body2.w, r2))
|
||||
|
||||
prev_v_normal := lg.dot(prev_v, manifold.normal)
|
||||
v_normal := lg.dot(v, manifold.normal)
|
||||
|
||||
RESTITUTION :: 0
|
||||
|
||||
restitution := f32(RESTITUTION)
|
||||
|
||||
if abs(v_normal) <= 2 * abs(lg.dot(manifold.normal, -config.gravity) * dt) {
|
||||
restitution = 0
|
||||
}
|
||||
|
||||
delta_v := manifold.normal * (-v_normal + min(-RESTITUTION * prev_v_normal, 0))
|
||||
|
||||
w1 := get_body_inverse_mass(body, manifold.normal, r1 + body.x)
|
||||
w2 := get_body_inverse_mass(body2, manifold.normal, r2 + body2.x)
|
||||
w := w1 + w2
|
||||
|
||||
if w != 0 {
|
||||
p := delta_v / w
|
||||
|
||||
body.v += p * body.inv_mass
|
||||
body2.v -= p * body2.inv_mass
|
||||
|
||||
body.w += multiply_inv_intertia(body, lg.cross(r1, p))
|
||||
body2.w -= multiply_inv_intertia(body2, lg.cross(r2, p))
|
||||
}
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
if true {
|
||||
tracy.ZoneN("simulate_step::dynamic_friction")
|
||||
|
||||
for &pair in sim_state.contact_pairs {
|
||||
manifold := &pair.manifold
|
||||
body1 := get_body(sim_state, pair.a)
|
||||
body2 := get_body(sim_state, pair.b)
|
||||
|
||||
for point_idx in 0 ..< pair.manifold.points_len {
|
||||
if pair.applied_static_friction[point_idx] || pair.lambda_normal == 0 {
|
||||
continue
|
||||
}
|
||||
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)
|
||||
|
||||
v := v1 - v2
|
||||
v_normal := lg.dot(manifold.normal, v) * manifold.normal
|
||||
v_tangent := v - v_normal
|
||||
|
||||
DYNAMIC_FRICTION :: 1
|
||||
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)
|
||||
|
||||
w := w1 + w2
|
||||
|
||||
if w > 0 {
|
||||
delta_v :=
|
||||
-v_tangent_norm *
|
||||
min(
|
||||
dt *
|
||||
DYNAMIC_FRICTION *
|
||||
abs(pair.lambda_normal[point_idx] / (dt * dt)),
|
||||
v_tangent_len / w,
|
||||
)
|
||||
|
||||
// delta_v_norm := lg.normalize0(delta_v)
|
||||
|
||||
p := delta_v
|
||||
|
||||
body1.v += p * body1.inv_mass
|
||||
body2.v -= p * body2.inv_mass
|
||||
|
||||
body1.w += multiply_inv_intertia(body1, lg.cross(r1, p))
|
||||
body2.w -= multiply_inv_intertia(body2, lg.cross(r2, p))
|
||||
}
|
||||
}
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
|
||||
// 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 && v.hit {
|
||||
prev_x, prev_q := body.prev_x, body.prev_q
|
||||
|
||||
wheel_world_pos := body_local_to_world(body, v.rel_pos)
|
||||
prev_wheel_world_pos := prev_x + lg.quaternion_mul_vector3(prev_q, v.rel_pos)
|
||||
|
||||
vel_3d := (wheel_world_pos - prev_wheel_world_pos) * inv_dt
|
||||
dir := body_local_to_world_vec(body, v.rel_dir)
|
||||
|
||||
// Spring damping
|
||||
if true {
|
||||
vel := lg.dot(vel_3d, dir)
|
||||
damping := -vel * min(v.damping * dt, 1)
|
||||
|
||||
_ = apply_constraint_correction_unilateral(
|
||||
dt,
|
||||
body,
|
||||
0,
|
||||
error = damping,
|
||||
error_gradient = -dir,
|
||||
pos = wheel_world_pos,
|
||||
)
|
||||
|
||||
body_solve_velocity(body, body.prev_x, body.prev_q, inv_dt)
|
||||
}
|
||||
|
||||
// Drive forces
|
||||
if true {
|
||||
total_impulse := v.drive_impulse - v.brake_impulse
|
||||
forward := body_local_to_world_vec(body, Vec3{0, 0, 1})
|
||||
|
||||
_ = apply_constraint_correction_unilateral(
|
||||
dt,
|
||||
body,
|
||||
0,
|
||||
total_impulse * dt * dt,
|
||||
-forward,
|
||||
wheel_world_pos,
|
||||
)
|
||||
body_solve_velocity(body, body.prev_x, body.prev_q, inv_dt)
|
||||
}
|
||||
|
||||
// Lateral friction
|
||||
if true {
|
||||
vel_contact := body_velocity_at_point(body, v.hit_point)
|
||||
right := wheel_get_right_vec(body, v)
|
||||
|
||||
lateral_vel := lg.dot(right, vel_contact)
|
||||
|
||||
friction := f32(0.5)
|
||||
impulse := -lateral_vel * friction
|
||||
corr := right * impulse * dt
|
||||
v.applied_impulse.x = impulse
|
||||
|
||||
apply_correction(body, corr, v.hit_point)
|
||||
body_solve_velocity(body, body.prev_x, body.prev_q, inv_dt)
|
||||
}
|
||||
}
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
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
|
||||
}
|
||||
}
|
||||
|
||||
// TODO: apply impulses
|
||||
// for i in 0 ..< len(sim_state.contact_pairs) {
|
||||
// contact_pair := &sim_state.contact_pairs[i]
|
||||
|
||||
//
|
||||
// }
|
||||
|
||||
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
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
simulate_step :: proc(scene: ^Scene, sim_state: ^Sim_State, config: Solver_Config) {
|
||||
tracy.Zone()
|
||||
|
||||
substeps := config.substreps_minus_one + 1
|
||||
@ -336,391 +743,35 @@ simulate_step :: proc(sim_state: ^Sim_State, config: Solver_Config) {
|
||||
dt := config.timestep / f32(substeps)
|
||||
inv_dt := 1.0 / dt
|
||||
|
||||
for _ in 0 ..< substeps {
|
||||
predict_positions(sim_state, config, dt)
|
||||
resize_soa(&sim_state.contact_pairs, 0)
|
||||
|
||||
Body_Pair :: struct {
|
||||
a, b: int,
|
||||
tlas := build_tlas(sim_state, config)
|
||||
potential_pairs := find_potential_pairs(sim_state, &tlas)
|
||||
|
||||
Solver :: enum {
|
||||
XPBD,
|
||||
PGS,
|
||||
}
|
||||
|
||||
solver := Solver.PGS
|
||||
|
||||
switch solver {
|
||||
case .XPBD:
|
||||
sim_state_copy(&scene.scratch_sim_state, get_sim_state(scene))
|
||||
xpbd_predict_positions(&scene.scratch_sim_state, config, config.timestep)
|
||||
find_collisions(&scene.scratch_sim_state, &sim_state.contact_pairs, potential_pairs)
|
||||
|
||||
for _ in 0 ..< substeps {
|
||||
xpbd_substep(sim_state, config, dt, inv_dt)
|
||||
}
|
||||
case .PGS:
|
||||
find_collisions(sim_state, &sim_state.contact_pairs, potential_pairs)
|
||||
|
||||
// Shuffle contacts
|
||||
{
|
||||
num_contacts := len(sim_state.contact_pairs)
|
||||
for i in 0 ..< num_contacts {
|
||||
j := rand.int_max(num_contacts)
|
||||
|
||||
if i != j {
|
||||
tmp := sim_state.contact_pairs[i]
|
||||
sim_state.contact_pairs[i] = sim_state.contact_pairs[j]
|
||||
sim_state.contact_pairs[j] = tmp
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
{
|
||||
tracy.ZoneN("simulate_step::solve_collisions")
|
||||
for i in 0 ..< len(sim_state.contact_pairs) {
|
||||
contact_pair := &sim_state.contact_pairs[i]
|
||||
body, body2 :=
|
||||
get_body(sim_state, contact_pair.a), get_body(sim_state, contact_pair.b)
|
||||
|
||||
contact_pair^ = Contact_Pair {
|
||||
a = contact_pair.a,
|
||||
b = contact_pair.b,
|
||||
prev_x_a = body.x,
|
||||
prev_x_b = body2.x,
|
||||
prev_q_a = body.q,
|
||||
prev_q_b = body2.q,
|
||||
manifold = contact_pair.manifold,
|
||||
}
|
||||
|
||||
manifold := &contact_pair.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)
|
||||
|
||||
p_diff_normal := lg.dot(p2 - p1, manifold.normal)
|
||||
separation := min(p_diff_normal, 0)
|
||||
|
||||
lambda_norm, corr1, corr2, ok := calculate_constraint_params2(
|
||||
dt,
|
||||
body,
|
||||
body2,
|
||||
0,
|
||||
separation,
|
||||
-manifold.normal,
|
||||
p1,
|
||||
p2,
|
||||
)
|
||||
if ok {
|
||||
contact_pair.applied_normal_correction[point_idx] = -separation
|
||||
contact_pair.applied_corrections += 1
|
||||
contact_pair.lambda_normal[point_idx] = lambda_norm
|
||||
|
||||
apply_correction(body, corr1, p1)
|
||||
apply_correction(body2, corr2, p2)
|
||||
}
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
{
|
||||
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_Pair) -> bool {
|
||||
sim_state := cast(^Sim_State)context.user_ptr
|
||||
|
||||
find_min_contact_y :: proc(
|
||||
scene: ^Sim_State,
|
||||
c: Contact_Pair,
|
||||
) -> (
|
||||
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)
|
||||
min_y2 := find_min_contact_y(sim_state, c2)
|
||||
|
||||
return min_y1 > min_y2
|
||||
},
|
||||
)
|
||||
}
|
||||
|
||||
for &contact_pair in sim_state.contact_pairs {
|
||||
manifold := contact_pair.manifold
|
||||
body, body2 :=
|
||||
get_body(sim_state, contact_pair.a), get_body(sim_state, contact_pair.b)
|
||||
|
||||
for point_idx in 0 ..< manifold.points_len {
|
||||
lambda_norm := contact_pair.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])
|
||||
prev_p2 :=
|
||||
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 -= 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,
|
||||
body,
|
||||
body2,
|
||||
0,
|
||||
-tangent_diff_len /
|
||||
max(f32(contact_pair.applied_corrections) * 0.5, 1),
|
||||
-tangent_diff_normalized,
|
||||
p1,
|
||||
p2,
|
||||
)
|
||||
|
||||
STATIC_FRICTION :: 0.6
|
||||
if ok_tangent && delta_lambda_tangent < STATIC_FRICTION * lambda_norm {
|
||||
contact_pair.applied_static_friction[point_idx] = true
|
||||
contact_pair.lambda_tangent[point_idx] = delta_lambda_tangent
|
||||
|
||||
apply_correction(body, corr1_tangent, p1)
|
||||
apply_correction(body2, corr2_tangent, p2)
|
||||
}
|
||||
}
|
||||
}
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
{
|
||||
tracy.ZoneN("simulate_step::suspension_constraints")
|
||||
|
||||
for &v in sim_state.suspension_constraints {
|
||||
if v.alive {
|
||||
body := get_body(sim_state, v.body)
|
||||
|
||||
pos := body_local_to_world(body, v.rel_pos)
|
||||
dir := body_local_to_world_vec(body, v.rel_dir)
|
||||
pos2 := pos + dir * v.rest
|
||||
v.hit_t, v.hit_point, v.hit = collision.intersect_segment_plane(
|
||||
{pos, pos2},
|
||||
collision.plane_from_point_normal({}, collision.Vec3{0, 1, 0}),
|
||||
)
|
||||
if v.hit {
|
||||
corr := pos - v.hit_point
|
||||
distance := lg.length(corr)
|
||||
corr = corr / distance if distance > 0 else 0
|
||||
|
||||
_ = apply_constraint_correction_unilateral(
|
||||
dt,
|
||||
body,
|
||||
v.compliance,
|
||||
error = distance - v.rest,
|
||||
error_gradient = corr,
|
||||
pos = pos,
|
||||
other_combined_inv_mass = 0,
|
||||
)
|
||||
}
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
{
|
||||
// Compute new linear and angular velocities
|
||||
for _, i in sim_state.bodies_slice {
|
||||
body := &sim_state.bodies_slice[i]
|
||||
if body.alive {
|
||||
body_solve_velocity(body, body.prev_x, body.prev_q, inv_dt)
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
// Restituion
|
||||
if true {
|
||||
tracy.ZoneN("simulate_step::restitution")
|
||||
|
||||
for &pair in sim_state.contact_pairs {
|
||||
manifold := &pair.manifold
|
||||
|
||||
body, body2 := get_body(sim_state, pair.a), get_body(sim_state, pair.b)
|
||||
prev_q1, prev_q2 := body.prev_q, body2.prev_q
|
||||
|
||||
for point_idx in 0 ..< manifold.points_len {
|
||||
if pair.lambda_normal[point_idx] == 0 {
|
||||
continue
|
||||
}
|
||||
prev_r1 := lg.quaternion_mul_vector3(prev_q1, manifold.points_a[point_idx])
|
||||
prev_r2 := lg.quaternion_mul_vector3(prev_q2, manifold.points_b[point_idx])
|
||||
|
||||
r1 := lg.quaternion_mul_vector3(body.q, manifold.points_a[point_idx])
|
||||
r2 := lg.quaternion_mul_vector3(body2.q, manifold.points_b[point_idx])
|
||||
|
||||
prev_v :=
|
||||
(body.prev_v + lg.cross(body.prev_w, prev_r1)) -
|
||||
(body2.prev_v + lg.cross(body2.prev_w, prev_r2))
|
||||
v := (body.v + lg.cross(body.w, r1)) - (body2.v + lg.cross(body2.w, r2))
|
||||
|
||||
prev_v_normal := lg.dot(prev_v, manifold.normal)
|
||||
v_normal := lg.dot(v, manifold.normal)
|
||||
|
||||
RESTITUTION :: 0
|
||||
|
||||
restitution := f32(RESTITUTION)
|
||||
|
||||
if abs(v_normal) <= 2 * abs(lg.dot(manifold.normal, -config.gravity) * dt) {
|
||||
restitution = 0
|
||||
}
|
||||
|
||||
delta_v := manifold.normal * (-v_normal + min(-RESTITUTION * prev_v_normal, 0))
|
||||
|
||||
w1 := get_body_inverse_mass(body, manifold.normal, r1 + body.x)
|
||||
w2 := get_body_inverse_mass(body2, manifold.normal, r2 + body2.x)
|
||||
w := w1 + w2
|
||||
|
||||
if w != 0 {
|
||||
p := delta_v / w
|
||||
|
||||
body.v += p * body.inv_mass
|
||||
body2.v -= p * body2.inv_mass
|
||||
|
||||
body.w += multiply_inv_intertia(body, lg.cross(r1, p))
|
||||
body2.w -= multiply_inv_intertia(body2, lg.cross(r2, p))
|
||||
}
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
if true {
|
||||
tracy.ZoneN("simulate_step::dynamic_friction")
|
||||
|
||||
for &pair in sim_state.contact_pairs {
|
||||
manifold := &pair.manifold
|
||||
body1 := get_body(sim_state, pair.a)
|
||||
body2 := get_body(sim_state, pair.b)
|
||||
|
||||
for point_idx in 0 ..< pair.manifold.points_len {
|
||||
if pair.applied_static_friction[point_idx] {
|
||||
continue
|
||||
}
|
||||
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)
|
||||
|
||||
v := v1 - v2
|
||||
v_normal := lg.dot(manifold.normal, v) * manifold.normal
|
||||
v_tangent := v - v_normal
|
||||
|
||||
DYNAMIC_FRICTION :: 0.3
|
||||
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)
|
||||
|
||||
w := w1 + w2
|
||||
|
||||
if w > 0 {
|
||||
delta_v :=
|
||||
-v_tangent_norm *
|
||||
min(
|
||||
dt *
|
||||
DYNAMIC_FRICTION *
|
||||
abs(pair.lambda_normal[point_idx] / (dt * dt)),
|
||||
v_tangent_len / w,
|
||||
)
|
||||
|
||||
// delta_v_norm := lg.normalize0(delta_v)
|
||||
|
||||
p := delta_v
|
||||
|
||||
body1.v += p * body1.inv_mass
|
||||
body2.v -= p * body2.inv_mass
|
||||
|
||||
body1.w += multiply_inv_intertia(body1, lg.cross(r1, p))
|
||||
body2.w -= multiply_inv_intertia(body2, lg.cross(r2, p))
|
||||
}
|
||||
}
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
|
||||
// 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 && v.hit {
|
||||
prev_x, prev_q := body.prev_x, body.prev_q
|
||||
|
||||
wheel_world_pos := body_local_to_world(body, v.rel_pos)
|
||||
prev_wheel_world_pos := prev_x + lg.quaternion_mul_vector3(prev_q, v.rel_pos)
|
||||
|
||||
vel_3d := (wheel_world_pos - prev_wheel_world_pos) * inv_dt
|
||||
dir := body_local_to_world_vec(body, v.rel_dir)
|
||||
|
||||
// Spring damping
|
||||
if true {
|
||||
vel := lg.dot(vel_3d, dir)
|
||||
damping := -vel * min(v.damping * dt, 1)
|
||||
|
||||
_ = apply_constraint_correction_unilateral(
|
||||
dt,
|
||||
body,
|
||||
0,
|
||||
error = damping,
|
||||
error_gradient = -dir,
|
||||
pos = wheel_world_pos,
|
||||
)
|
||||
|
||||
body_solve_velocity(body, body.prev_x, body.prev_q, inv_dt)
|
||||
}
|
||||
|
||||
// Drive forces
|
||||
if true {
|
||||
total_impulse := v.drive_impulse - v.brake_impulse
|
||||
forward := body_local_to_world_vec(body, Vec3{0, 0, 1})
|
||||
|
||||
_ = apply_constraint_correction_unilateral(
|
||||
dt,
|
||||
body,
|
||||
0,
|
||||
total_impulse * dt * dt,
|
||||
-forward,
|
||||
wheel_world_pos,
|
||||
)
|
||||
body_solve_velocity(body, body.prev_x, body.prev_q, inv_dt)
|
||||
}
|
||||
|
||||
// Lateral friction
|
||||
if true {
|
||||
vel_contact := body_velocity_at_point(body, v.hit_point)
|
||||
right := wheel_get_right_vec(body, v)
|
||||
|
||||
lateral_vel := lg.dot(right, vel_contact)
|
||||
|
||||
friction := f32(0.5)
|
||||
impulse := -lateral_vel * friction
|
||||
corr := right * impulse * dt
|
||||
v.applied_impulse.x = impulse
|
||||
|
||||
apply_correction(body, corr, v.hit_point)
|
||||
body_solve_velocity(body, body.prev_x, body.prev_q, inv_dt)
|
||||
}
|
||||
}
|
||||
}
|
||||
for _ in 0 ..< substeps {
|
||||
pgs_substep(sim_state, config, dt, inv_dt)
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
solve_velocities :: proc(scene: ^Sim_State, body_states: []Body_Sim_State, inv_dt: f32) {
|
||||
}
|
||||
|
||||
body_solve_velocity :: #force_inline proc(
|
||||
|
||||
Loading…
x
Reference in New Issue
Block a user