Static friction and refactor constraint params
This commit is contained in:
parent
aca890bcb7
commit
1a74039ad8
@ -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 {
|
||||
@ -303,7 +303,7 @@ update_runtime_world :: proc(runtime_world: ^Runtime_World, dt: f32) {
|
||||
|
||||
// 1.6 is a good value
|
||||
wheel_extent_x := f32(2)
|
||||
wheel_y := f32(-0.5)
|
||||
wheel_y := f32(-1)
|
||||
rest := f32(1)
|
||||
suspension_stiffness := f32(2000)
|
||||
compliance := 1.0 / suspension_stiffness
|
||||
|
||||
@ -131,6 +131,6 @@ debug_draw_manifold_points :: proc(normal: rl.Vector3, points: []rl.Vector3, col
|
||||
for p in points {
|
||||
rl.DrawLine3D(p, p + normal, color)
|
||||
|
||||
rl.DrawSphereWires(p, len(points) == 1 ? 0.5 : 0.1, 4, 4, color)
|
||||
rl.DrawSphereWires(p, 0.1, 4, 4, color)
|
||||
}
|
||||
}
|
||||
|
||||
@ -78,6 +78,7 @@ GLOBAL_PLANE :: collision.Plane {
|
||||
Contact_Pair :: struct {
|
||||
a, b: Body_Handle,
|
||||
manifold: collision.Contact_Manifold,
|
||||
lambdas: [4]f32,
|
||||
}
|
||||
|
||||
simulate_step :: proc(scene: ^Scene, config: Solver_Config) {
|
||||
@ -149,59 +150,99 @@ simulate_step :: proc(scene: ^Scene, config: Solver_Config) {
|
||||
manifold, collision := collision.convex_vs_convex_sat(box1, box2)
|
||||
|
||||
if collision {
|
||||
scene.contact_pairs[scene.contact_pairs_len] = Contact_Pair {
|
||||
contact_pair := &scene.contact_pairs[scene.contact_pairs_len]
|
||||
contact_pair^ = Contact_Pair {
|
||||
a = Body_Handle(i + 1),
|
||||
b = Body_Handle(j + 1),
|
||||
manifold = manifold,
|
||||
}
|
||||
scene.contact_pairs_len += 1
|
||||
|
||||
points_a_local, points_b_local: [4]rl.Vector3
|
||||
// Convert manifold contact from world to local space
|
||||
for point_idx in 0 ..< manifold.points_len {
|
||||
points_a_local[point_idx] = body_world_to_local(
|
||||
manifold.points_a[point_idx] = body_world_to_local(
|
||||
body,
|
||||
manifold.points_a[point_idx],
|
||||
)
|
||||
points_b_local[point_idx] = body_world_to_local(
|
||||
manifold.points_b[point_idx] = body_world_to_local(
|
||||
body2,
|
||||
manifold.points_b[point_idx],
|
||||
)
|
||||
}
|
||||
for point_idx in 0 ..< manifold.points_len {
|
||||
p1, p2 := points_a_local[point_idx], points_b_local[point_idx]
|
||||
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)
|
||||
body1_inv_mass := get_body_inverse_mass(body, manifold.normal, p1)
|
||||
body2_inv_mass := get_body_inverse_mass(body2, manifold.normal, p2)
|
||||
|
||||
diff := p2 - p1
|
||||
length := lg.length(diff)
|
||||
if length != 0 {
|
||||
diff /= length
|
||||
}
|
||||
separation := min(lg.dot(p2 - p1, manifold.normal), 0)
|
||||
|
||||
handled_pairs[{a = min(i, j), b = max(i, j)}] = true
|
||||
|
||||
apply_constraint_correction_unilateral(
|
||||
lambda_norm, corr1, corr2, ok := calculate_constraint_params2(
|
||||
dt,
|
||||
body,
|
||||
0,
|
||||
-separation,
|
||||
-manifold.normal,
|
||||
p1,
|
||||
body2_inv_mass,
|
||||
)
|
||||
|
||||
apply_constraint_correction_unilateral(
|
||||
dt,
|
||||
body2,
|
||||
0,
|
||||
-separation,
|
||||
manifold.normal,
|
||||
separation,
|
||||
-manifold.normal,
|
||||
p1,
|
||||
p2,
|
||||
body1_inv_mass,
|
||||
)
|
||||
contact_pair.lambdas[point_idx] = lambda_norm
|
||||
|
||||
if ok {
|
||||
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,
|
||||
)
|
||||
|
||||
STATIC_FRICTION :: 2
|
||||
if ok_tangent &&
|
||||
lambda_tangent < lambda_norm * STATIC_FRICTION {
|
||||
apply_correction(body, corr1_tangent, p1)
|
||||
apply_correction(body2, corr2_tangent, p2)
|
||||
}
|
||||
}
|
||||
}
|
||||
}
|
||||
}
|
||||
}
|
||||
@ -221,11 +262,11 @@ simulate_step :: proc(scene: ^Scene, config: Solver_Config) {
|
||||
collision.plane_from_point_normal({}, collision.Vec3{0, 1, 0}),
|
||||
)
|
||||
if v.hit {
|
||||
corr := v.hit_point - pos
|
||||
corr := pos - v.hit_point
|
||||
distance := lg.length(corr)
|
||||
corr = corr / distance if distance > 0 else 0
|
||||
|
||||
apply_constraint_correction_unilateral(
|
||||
_ = apply_constraint_correction_unilateral(
|
||||
dt,
|
||||
body,
|
||||
v.compliance,
|
||||
@ -266,12 +307,12 @@ simulate_step :: proc(scene: ^Scene, config: Solver_Config) {
|
||||
vel := lg.dot(vel_3d, dir)
|
||||
damping := -vel * min(v.damping * dt, 1)
|
||||
|
||||
apply_constraint_correction_unilateral(
|
||||
_ = apply_constraint_correction_unilateral(
|
||||
dt,
|
||||
body,
|
||||
0,
|
||||
error = damping,
|
||||
error_gradient = dir,
|
||||
error_gradient = -dir,
|
||||
pos = wheel_world_pos,
|
||||
)
|
||||
|
||||
@ -283,12 +324,12 @@ simulate_step :: proc(scene: ^Scene, config: Solver_Config) {
|
||||
total_impulse := v.drive_impulse - v.brake_impulse
|
||||
forward := body_local_to_world_vec(body, rl.Vector3{0, 0, 1})
|
||||
|
||||
apply_constraint_correction_unilateral(
|
||||
_ = apply_constraint_correction_unilateral(
|
||||
dt,
|
||||
body,
|
||||
0,
|
||||
total_impulse * dt * dt,
|
||||
forward,
|
||||
-forward,
|
||||
wheel_world_pos,
|
||||
)
|
||||
body_solve_velocity(body, body_state, inv_dt)
|
||||
@ -337,6 +378,76 @@ body_solve_velocity :: #force_inline proc(body: Body_Ptr, state: Body_Sim_State,
|
||||
}
|
||||
}
|
||||
|
||||
calculate_constraint_params1 :: proc(
|
||||
dt: f32,
|
||||
body: Body_Ptr,
|
||||
compliance: f32,
|
||||
error: f32,
|
||||
error_gradient: rl.Vector3,
|
||||
pos: rl.Vector3,
|
||||
other_combined_inv_mass: f32,
|
||||
) -> (
|
||||
lambda: f32,
|
||||
w: f32,
|
||||
correction: rl.Vector3,
|
||||
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: rl.Vector3,
|
||||
pos1: rl.Vector3,
|
||||
pos2: rl.Vector3,
|
||||
) -> (
|
||||
lambda: f32,
|
||||
correction1: rl.Vector3,
|
||||
correction2: rl.Vector3,
|
||||
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,
|
||||
@ -345,26 +456,29 @@ apply_constraint_correction_unilateral :: proc(
|
||||
error_gradient: rl.Vector3,
|
||||
pos: rl.Vector3,
|
||||
other_combined_inv_mass: f32 = 0,
|
||||
) -> (
|
||||
lambda: f32,
|
||||
) {
|
||||
if error == 0 {
|
||||
return
|
||||
w: f32
|
||||
correction: rl.Vector3
|
||||
ok: bool
|
||||
lambda, w, correction, ok = calculate_constraint_params1(
|
||||
dt,
|
||||
body,
|
||||
compliance,
|
||||
error,
|
||||
error_gradient,
|
||||
pos,
|
||||
other_combined_inv_mass,
|
||||
)
|
||||
|
||||
if ok {
|
||||
apply_correction(body, correction, pos)
|
||||
}
|
||||
|
||||
w := get_body_inverse_mass(body, error_gradient, pos)
|
||||
w += other_combined_inv_mass
|
||||
|
||||
if w == 0 {
|
||||
return
|
||||
}
|
||||
|
||||
alpha := compliance / dt / dt
|
||||
lambda := -error / (w + alpha)
|
||||
|
||||
delta_pos := error_gradient * -lambda
|
||||
|
||||
apply_correction(body, delta_pos, pos)
|
||||
}
|
||||
|
||||
apply_correction :: proc(body: Body_Ptr, corr: rl.Vector3, pos: rl.Vector3) {
|
||||
body.x += corr * body.inv_mass
|
||||
|
||||
@ -395,11 +509,7 @@ get_body_inverse_mass :: proc(body: Body_Ptr, normal, pos: rl.Vector3) -> f32 {
|
||||
rn = lg.cross(rn, normal)
|
||||
rn = lg.quaternion_mul_vector3(inv_q, rn)
|
||||
|
||||
w :=
|
||||
rn.x * rn.x * body.inv_inertia_tensor.x +
|
||||
rn.y * rn.y * body.inv_inertia_tensor.y +
|
||||
rn.z * rn.z * body.inv_inertia_tensor.z
|
||||
|
||||
w := lg.dot(rn * rn, body.inv_inertia_tensor)
|
||||
w += body.inv_mass
|
||||
|
||||
return w
|
||||
|
||||
Loading…
x
Reference in New Issue
Block a user