Fix dynamic friction (mistake in paper actually), improve friction
This commit is contained in:
parent
ef51a9ee30
commit
9f6d57b5ea
@ -1,80 +1,60 @@
|
||||
# Blender 4.3.2
|
||||
# www.blender.org
|
||||
o Object_5
|
||||
v -1.841497 0.441915 2.882739
|
||||
v -1.528260 -0.439969 4.246670
|
||||
v -1.859343 0.407766 2.882677
|
||||
v -1.496358 -0.439898 4.246497
|
||||
v -1.532271 -0.014560 4.306011
|
||||
v -1.458986 0.585761 4.084804
|
||||
v -1.169056 1.845618 -0.044352
|
||||
v -1.794738 0.759021 -2.991723
|
||||
v -1.751396 -0.067717 -3.937637
|
||||
v 1.598544 -0.437865 4.228289
|
||||
v -1.355442 0.205843 4.443853
|
||||
v -1.495447 -0.513379 -3.072723
|
||||
v -1.081991 -0.669625 2.507290
|
||||
v 1.152275 -0.667619 2.498550
|
||||
v 0.035142 0.587956 4.202047
|
||||
v 0.035142 1.889431 -0.088813
|
||||
v 0.035142 1.953676 -2.216545
|
||||
v -1.179023 1.913049 -2.281286
|
||||
v -1.418515 1.165435 -4.711776
|
||||
v -1.589515 0.055308 -5.153280
|
||||
v 1.558805 0.203133 4.425279
|
||||
v 1.565731 -0.511086 -3.071773
|
||||
v 1.821680 -0.067717 -3.937637
|
||||
v 1.911781 0.441916 2.882739
|
||||
v 0.035142 -0.194318 -4.868722
|
||||
v 1.529270 0.585761 4.084804
|
||||
v 1.239340 1.845618 -0.044352
|
||||
v 1.249308 1.913049 -2.281285
|
||||
v 1.488799 1.165434 -4.711775
|
||||
v 0.035142 1.168489 -4.782736
|
||||
v 0.035142 0.107657 -5.212766
|
||||
v 1.659799 0.055307 -5.153279
|
||||
v 1.865023 0.759021 -2.991723
|
||||
v -1.458925 0.588598 4.098169
|
||||
v -1.169193 1.844997 -0.020979
|
||||
v -1.767808 0.808841 -2.967643
|
||||
v -1.755688 0.019238 -3.954955
|
||||
v 1.566465 -0.437912 4.228478
|
||||
v -1.355442 0.201413 4.442485
|
||||
v -1.527377 -0.513372 -3.070427
|
||||
v -1.081991 -0.669592 2.507292
|
||||
v 1.152275 -0.667651 2.498548
|
||||
v -1.177522 1.915790 -2.273865
|
||||
v -1.440784 1.165512 -4.710642
|
||||
v -1.567565 0.052245 -5.155589
|
||||
v 1.558806 0.207560 4.426651
|
||||
v 1.597780 -0.511147 -3.072741
|
||||
v 1.825968 0.019292 -3.954931
|
||||
v 1.929686 0.407656 2.882679
|
||||
v 1.529256 0.593033 4.083607
|
||||
v 1.239481 1.844996 -0.020995
|
||||
v 1.247809 1.915789 -2.273863
|
||||
v 1.511068 1.165512 -4.710640
|
||||
v 1.637697 0.053622 -5.155048
|
||||
v 1.838094 0.808837 -2.967645
|
||||
s 0
|
||||
f 1 2 3
|
||||
f 1 3 4
|
||||
f 1 4 5
|
||||
f 31 26 25 22
|
||||
f 25 22 21 19
|
||||
f 1 6 7
|
||||
f 1 7 2
|
||||
f 18 19 8 17
|
||||
f 2 9 3
|
||||
f 2 7 10
|
||||
f 2 10 11
|
||||
f 20 12 11 10
|
||||
f 17 12 11 10
|
||||
f 3 9 4
|
||||
f 4 9 13
|
||||
f 24 25 14 13
|
||||
f 25 26 15 14
|
||||
f 6 16 17
|
||||
f 6 17 18
|
||||
f 6 18 7
|
||||
f 7 18 10
|
||||
f 8 12 20
|
||||
f 8 20 21
|
||||
f 8 21 22
|
||||
f 8 22 19
|
||||
f 9 19 13
|
||||
f 10 18 23
|
||||
f 10 23 20
|
||||
f 8 19 9 2
|
||||
f 13 19 24
|
||||
f 15 16 5 14
|
||||
f 27 28 15 26
|
||||
f 17 16 15 28
|
||||
f 29 18 17 28
|
||||
f 29 28 27 30
|
||||
f 18 29 30
|
||||
f 18 30 23
|
||||
f 19 22 24
|
||||
f 20 23 30
|
||||
f 20 30 21
|
||||
f 21 31 22
|
||||
f 21 30 31
|
||||
f 22 25 24
|
||||
f 26 31 27
|
||||
f 9 16 20 4
|
||||
f 6 13 14
|
||||
f 23 25 18 24
|
||||
f 7 15 10
|
||||
f 8 12 17
|
||||
f 8 19 16
|
||||
f 8 16 9 2
|
||||
f 14 23 24 15
|
||||
f 22 13 5 21
|
||||
f 17 10 15 24
|
||||
f 16 19 20
|
||||
f 17 24 18
|
||||
f 18 25 19
|
||||
f 7 10 2 1
|
||||
f 19 21 20
|
||||
f 22 25 23
|
||||
f 12 8 2 11
|
||||
f 27 31 30
|
||||
f 14 5 4 13
|
||||
f 5 16 6 1
|
||||
f 5 13 6 1
|
||||
f 5 4 20 21
|
||||
f 15 7 6 14
|
||||
f 14 13 22 23
|
||||
|
||||
@ -69,7 +69,7 @@ Car :: struct {
|
||||
SOLVER_CONFIG :: physics.Solver_Config {
|
||||
timestep = 1.0 / 120,
|
||||
gravity = rl.Vector3{0, -9.8, 0},
|
||||
substreps_minus_one = 1 - 1,
|
||||
substreps_minus_one = 2 - 1,
|
||||
}
|
||||
|
||||
Game_Memory :: struct {
|
||||
@ -277,9 +277,9 @@ update_runtime_world :: proc(runtime_world: ^Runtime_World, dt: f32) {
|
||||
&runtime_world.solver_state,
|
||||
hash.fnv32a(slice.to_bytes([]int{(x | y << 8)})),
|
||||
physics.Body_Config {
|
||||
initial_pos = {f32(x) * 2, 5, f32(y) * 2},
|
||||
initial_pos = {f32(x) * 3 + 20, 5 + f32(y) * 2, 0},
|
||||
initial_rot = linalg.QUATERNIONF32_IDENTITY,
|
||||
shape = physics.Shape_Box{size = 1.5},
|
||||
shape = physics.Shape_Box{size = 1},
|
||||
mass = 10,
|
||||
},
|
||||
)
|
||||
|
||||
@ -4,6 +4,7 @@ import "collision"
|
||||
import "core:fmt"
|
||||
import "core:math"
|
||||
import lg "core:math/linalg"
|
||||
import "core:slice"
|
||||
import "libs:tracy"
|
||||
import rl "vendor:raylib"
|
||||
|
||||
@ -76,10 +77,13 @@ GLOBAL_PLANE :: collision.Plane {
|
||||
}
|
||||
|
||||
Contact_Pair :: struct {
|
||||
a, b: Body_Handle,
|
||||
manifold: collision.Contact_Manifold,
|
||||
lambda_normal: [4]f32,
|
||||
lambda_tangent: [4]f32,
|
||||
a, b: Body_Handle,
|
||||
prev_x_a, prev_x_b: rl.Vector3,
|
||||
prev_q_a, prev_q_b: rl.Quaternion,
|
||||
manifold: collision.Contact_Manifold,
|
||||
applied_corrections: int,
|
||||
lambda_normal: [4]f32,
|
||||
lambda_tangent: [4]f32,
|
||||
}
|
||||
|
||||
simulate_step :: proc(scene: ^Scene, config: Solver_Config) {
|
||||
@ -150,6 +154,10 @@ simulate_step :: proc(scene: ^Scene, config: Solver_Config) {
|
||||
contact_pair^ = Contact_Pair {
|
||||
a = Body_Handle(i + 1),
|
||||
b = Body_Handle(j + 1),
|
||||
prev_x_a = body.x,
|
||||
prev_x_b = body2.x,
|
||||
prev_q_a = body.q,
|
||||
prev_q_b = body2.q,
|
||||
manifold = raw_manifold,
|
||||
}
|
||||
scene.contact_pairs_len += 1
|
||||
@ -173,7 +181,8 @@ simulate_step :: proc(scene: ^Scene, config: Solver_Config) {
|
||||
body_local_to_world(body, p1),
|
||||
body_local_to_world(body2, p2)
|
||||
|
||||
separation := min(lg.dot(p2 - p1, manifold.normal), 0)
|
||||
p_diff_normal := lg.dot(p2 - p1, manifold.normal)
|
||||
separation := min(p_diff_normal, 0)
|
||||
|
||||
handled_pairs[{a = min(i, j), b = max(i, j)}] = true
|
||||
|
||||
@ -187,67 +196,13 @@ simulate_step :: proc(scene: ^Scene, config: Solver_Config) {
|
||||
p1,
|
||||
p2,
|
||||
)
|
||||
contact_pair.lambda_normal[point_idx] = lambda_norm
|
||||
|
||||
if ok {
|
||||
contact_pair.applied_corrections += 1
|
||||
contact_pair.lambda_normal[point_idx] = lambda_norm
|
||||
|
||||
apply_correction(body, corr1, p1)
|
||||
apply_correction(body2, corr2, p2)
|
||||
}
|
||||
|
||||
// Static friction
|
||||
if ok {
|
||||
p1, p2 =
|
||||
body_local_to_world(
|
||||
body,
|
||||
manifold.points_a[point_idx],
|
||||
),
|
||||
body_local_to_world(
|
||||
body2,
|
||||
manifold.points_b[point_idx],
|
||||
)
|
||||
prev_p1 :=
|
||||
body_states[i].prev_x +
|
||||
lg.quaternion_mul_vector3(
|
||||
body_states[i].prev_q,
|
||||
manifold.points_a[point_idx],
|
||||
)
|
||||
prev_p2 :=
|
||||
body_states[j].prev_x +
|
||||
lg.quaternion_mul_vector3(
|
||||
body_states[j].prev_q,
|
||||
manifold.points_b[point_idx],
|
||||
)
|
||||
|
||||
delta_p := (p2 - prev_p2) - (p1 - prev_p1)
|
||||
delta_p_tangent_norm :=
|
||||
delta_p -
|
||||
lg.dot(manifold.normal, delta_p) * manifold.normal
|
||||
delta_p_tangent_len := lg.length(delta_p_tangent_norm)
|
||||
|
||||
if delta_p_tangent_len > 0 {
|
||||
delta_p_tangent_norm /= delta_p_tangent_len
|
||||
|
||||
lambda_tangent, corr1_tangent, corr2_tangent, ok_tangent :=
|
||||
calculate_constraint_params2(
|
||||
dt,
|
||||
body,
|
||||
body2,
|
||||
0,
|
||||
-delta_p_tangent_len,
|
||||
delta_p_tangent_norm,
|
||||
p1,
|
||||
p2,
|
||||
)
|
||||
contact_pair.lambda_tangent[point_idx] = lambda_tangent
|
||||
|
||||
STATIC_FRICTION :: 0.7
|
||||
if ok_tangent &&
|
||||
lambda_tangent < lambda_norm * STATIC_FRICTION {
|
||||
apply_correction(body, corr1_tangent, p1)
|
||||
apply_correction(body2, corr2_tangent, p2)
|
||||
}
|
||||
}
|
||||
}
|
||||
}
|
||||
}
|
||||
}
|
||||
@ -256,6 +211,103 @@ simulate_step :: proc(scene: ^Scene, config: Solver_Config) {
|
||||
}
|
||||
}
|
||||
|
||||
{
|
||||
tracy.ZoneN("simulate_step::static_friction")
|
||||
|
||||
if true {
|
||||
|
||||
context = context
|
||||
context.user_ptr = scene
|
||||
slice.sort_by(
|
||||
scene.contact_pairs[:scene.contact_pairs_len],
|
||||
proc(c1, c2: Contact_Pair) -> bool {
|
||||
scene := cast(^Scene)context.user_ptr
|
||||
|
||||
find_min_contact_y :: proc(
|
||||
scene: ^Scene,
|
||||
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(scene, c1)
|
||||
min_y2 := find_min_contact_y(scene, c2)
|
||||
|
||||
return min_y1 > min_y2
|
||||
},
|
||||
)
|
||||
}
|
||||
|
||||
for &contact_pair in scene.contact_pairs[:scene.contact_pairs_len] {
|
||||
manifold := contact_pair.manifold
|
||||
body, body2 := get_body(scene, contact_pair.a), get_body(scene, contact_pair.b)
|
||||
i, j := int(contact_pair.a) - 1, int(contact_pair.b) - 1
|
||||
|
||||
lambda_tangent: f32
|
||||
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_states[i].prev_x +
|
||||
lg.quaternion_mul_vector3(
|
||||
body_states[i].prev_q,
|
||||
manifold.points_a[point_idx],
|
||||
)
|
||||
prev_p2 :=
|
||||
body_states[j].prev_x +
|
||||
lg.quaternion_mul_vector3(
|
||||
body_states[j].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 / f32(contact_pair.applied_corrections),
|
||||
-tangent_diff_normalized,
|
||||
p1,
|
||||
p2,
|
||||
)
|
||||
contact_pair.lambda_tangent[point_idx] = lambda_tangent
|
||||
|
||||
new_lambda_tangent := lambda_tangent + delta_lambda_tangent
|
||||
|
||||
STATIC_FRICTION :: 0.7
|
||||
if ok_tangent && delta_lambda_tangent < STATIC_FRICTION * lambda_norm {
|
||||
lambda_tangent = new_lambda_tangent
|
||||
apply_correction(body, corr1_tangent, p1)
|
||||
apply_correction(body2, corr2_tangent, p2)
|
||||
}
|
||||
}
|
||||
}
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
{
|
||||
tracy.ZoneN("simulate_step::suspension_constraints")
|
||||
|
||||
@ -291,8 +343,8 @@ simulate_step :: proc(scene: ^Scene, config: Solver_Config) {
|
||||
|
||||
solve_velocities(scene, body_states, inv_dt)
|
||||
|
||||
{
|
||||
tracy.ZoneN("simulate_step::collision_velocities")
|
||||
if true {
|
||||
tracy.ZoneN("simulate_step::dynamic_friction")
|
||||
|
||||
for &pair in scene.contact_pairs[:scene.contact_pairs_len] {
|
||||
manifold := &pair.manifold
|
||||
@ -311,27 +363,30 @@ simulate_step :: proc(scene: ^Scene, config: Solver_Config) {
|
||||
v_normal := lg.dot(manifold.normal, v) * manifold.normal
|
||||
v_tangent := v - v_normal
|
||||
|
||||
DYNAMIC_FRICTION :: 0.2
|
||||
DYNAMIC_FRICTION :: 0.4
|
||||
v_tangent_len := lg.length(v_tangent)
|
||||
if v_tangent_len > 0 {
|
||||
delta_v :=
|
||||
-(v_tangent / v_tangent_len) *
|
||||
min(
|
||||
dt *
|
||||
DYNAMIC_FRICTION *
|
||||
abs(pair.lambda_normal[point_idx] / (dt * dt)),
|
||||
v_tangent_len,
|
||||
)
|
||||
|
||||
delta_v_norm := lg.normalize0(delta_v)
|
||||
v_tangent_norm := v_tangent / v_tangent_len
|
||||
|
||||
w1, w2 :=
|
||||
get_body_inverse_mass(body1, delta_v_norm, p1),
|
||||
get_body_inverse_mass(body2, delta_v_norm, p2)
|
||||
get_body_inverse_mass(body1, v_tangent_norm, p1),
|
||||
get_body_inverse_mass(body2, v_tangent_norm, p2)
|
||||
|
||||
w := w1 + w2
|
||||
if w != 0 {
|
||||
p := delta_v / w
|
||||
|
||||
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
|
||||
@ -489,7 +544,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 {
|
||||
|
||||
BIN
src_assets/car_convex.blend
(Stored with Git LFS)
BIN
src_assets/car_convex.blend
(Stored with Git LFS)
Binary file not shown.
BIN
src_assets/car_convex.blend1
(Stored with Git LFS)
BIN
src_assets/car_convex.blend1
(Stored with Git LFS)
Binary file not shown.
Loading…
x
Reference in New Issue
Block a user