Reimplement suspension, now with correct friction using PGS
This commit is contained in:
parent
50d0401479
commit
60522ea836
@ -272,7 +272,7 @@ update_runtime_world :: proc(runtime_world: ^Runtime_World, dt: f32) {
|
||||
center_of_mass = car_convex.center_of_mass,
|
||||
inertia_tensor = auto_cast car_convex.inertia_tensor,
|
||||
},
|
||||
mass = 100,
|
||||
mass = 1000,
|
||||
},
|
||||
)
|
||||
|
||||
@ -320,9 +320,8 @@ update_runtime_world :: proc(runtime_world: ^Runtime_World, dt: f32) {
|
||||
wheel_extent_x := f32(1.7)
|
||||
wheel_y := f32(-0.5)
|
||||
rest := f32(1)
|
||||
suspension_stiffness := f32(2000)
|
||||
compliance := 1.0 / suspension_stiffness
|
||||
damping := f32(0.01)
|
||||
natural_frequency := f32(0.2)
|
||||
damping := f32(0.12)
|
||||
radius := f32(0.6)
|
||||
wheel_front_z := f32(3.05)
|
||||
wheel_back_z := f32(-2.45)
|
||||
@ -336,7 +335,7 @@ update_runtime_world :: proc(runtime_world: ^Runtime_World, dt: f32) {
|
||||
rel_dir = {0, -1, 0},
|
||||
radius = radius,
|
||||
rest = rest,
|
||||
compliance = compliance,
|
||||
natural_frequency = natural_frequency,
|
||||
damping = damping,
|
||||
body = runtime_world.car_handle,
|
||||
},
|
||||
@ -350,7 +349,7 @@ update_runtime_world :: proc(runtime_world: ^Runtime_World, dt: f32) {
|
||||
rel_dir = {0, -1, 0},
|
||||
radius = radius,
|
||||
rest = rest,
|
||||
compliance = compliance,
|
||||
natural_frequency = natural_frequency,
|
||||
damping = damping,
|
||||
body = runtime_world.car_handle,
|
||||
},
|
||||
@ -364,7 +363,7 @@ update_runtime_world :: proc(runtime_world: ^Runtime_World, dt: f32) {
|
||||
rel_dir = {0, -1, 0},
|
||||
radius = radius,
|
||||
rest = rest,
|
||||
compliance = compliance,
|
||||
natural_frequency = natural_frequency,
|
||||
damping = damping,
|
||||
body = runtime_world.car_handle,
|
||||
},
|
||||
@ -378,7 +377,7 @@ update_runtime_world :: proc(runtime_world: ^Runtime_World, dt: f32) {
|
||||
rel_dir = {0, -1, 0},
|
||||
radius = radius,
|
||||
rest = rest,
|
||||
compliance = compliance,
|
||||
natural_frequency = natural_frequency,
|
||||
damping = damping,
|
||||
body = runtime_world.car_handle,
|
||||
},
|
||||
@ -387,8 +386,8 @@ update_runtime_world :: proc(runtime_world: ^Runtime_World, dt: f32) {
|
||||
drive_wheels := []physics.Suspension_Constraint_Handle{wheel_rl, wheel_rr}
|
||||
turn_wheels := []physics.Suspension_Constraint_Handle{wheel_fl, wheel_fr}
|
||||
|
||||
DRIVE_IMPULSE :: 10
|
||||
BRAKE_IMPULSE :: 10
|
||||
DRIVE_IMPULSE :: 10000
|
||||
BRAKE_IMPULSE :: 10000
|
||||
TURN_ANGLE :: -f32(30) * math.RAD_PER_DEG
|
||||
|
||||
for wheel_handle in drive_wheels {
|
||||
|
||||
@ -120,34 +120,38 @@ Collision_Shape :: union {
|
||||
}
|
||||
|
||||
Suspension_Constraint :: struct {
|
||||
alive: bool,
|
||||
alive: bool,
|
||||
// Pos relative to the body
|
||||
rel_pos: Vec3,
|
||||
rel_pos: Vec3,
|
||||
// Dir relative to the body
|
||||
rel_dir: Vec3,
|
||||
rel_dir: Vec3,
|
||||
// Handle of the rigid body
|
||||
body: Body_Handle,
|
||||
body: Body_Handle,
|
||||
// Wheel radius
|
||||
radius: f32,
|
||||
radius: f32,
|
||||
// Rest distance
|
||||
rest: f32,
|
||||
// Inverse stiffness
|
||||
compliance: f32,
|
||||
rest: f32,
|
||||
// Frequency of the spring, affects stiffness
|
||||
natural_frequency: f32,
|
||||
// How much to damp velocity of the spring
|
||||
damping: f32,
|
||||
damping: f32,
|
||||
|
||||
// Accumulated impulse
|
||||
spring_impulse: f32,
|
||||
lateral_impulse: f32,
|
||||
|
||||
// Runtime state
|
||||
hit: bool,
|
||||
hit_point: Vec3,
|
||||
hit: bool,
|
||||
hit_point: Vec3,
|
||||
// rel_hit_point = rel_pos + rel_dir * hit_t
|
||||
hit_t: f32,
|
||||
turn_angle: f32,
|
||||
drive_impulse: f32,
|
||||
brake_impulse: f32,
|
||||
applied_impulse: Vec3,
|
||||
hit_t: f32,
|
||||
turn_angle: f32,
|
||||
drive_impulse: f32,
|
||||
brake_impulse: f32,
|
||||
applied_impulse: Vec3,
|
||||
|
||||
// Free list
|
||||
next_plus_one: i32,
|
||||
next_plus_one: i32,
|
||||
}
|
||||
|
||||
// Index plus one, so handle 0 maps to invalid body
|
||||
@ -237,13 +241,13 @@ Body_Config :: struct {
|
||||
|
||||
// TODO: rename to wheel
|
||||
Suspension_Constraint_Config :: struct {
|
||||
rel_pos: Vec3,
|
||||
rel_dir: Vec3,
|
||||
body: Body_Handle,
|
||||
rest: f32,
|
||||
compliance: f32,
|
||||
damping: f32,
|
||||
radius: f32,
|
||||
rel_pos: Vec3,
|
||||
rel_dir: Vec3,
|
||||
body: Body_Handle,
|
||||
rest: f32,
|
||||
natural_frequency: f32,
|
||||
damping: f32,
|
||||
radius: f32,
|
||||
}
|
||||
|
||||
calculate_body_params_from_config :: proc(
|
||||
@ -311,7 +315,7 @@ update_suspension_constraint_from_config :: proc(
|
||||
constraint.rel_dir = config.rel_dir
|
||||
constraint.body = config.body
|
||||
constraint.rest = config.rest
|
||||
constraint.compliance = config.compliance
|
||||
constraint.natural_frequency = config.natural_frequency
|
||||
constraint.damping = config.damping
|
||||
constraint.radius = config.radius
|
||||
}
|
||||
|
||||
@ -325,491 +325,6 @@ update_contacts :: proc(sim_state: ^Sim_State) {
|
||||
}
|
||||
}
|
||||
|
||||
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 {
|
||||
body.prev_x = body.x
|
||||
body.prev_v = body.v
|
||||
body.prev_w = body.w
|
||||
body.prev_q = body.q
|
||||
|
||||
body.v += config.gravity * dt * (body.inv_mass == 0 ? 0 : 1) // special case for gravity, TODO
|
||||
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
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
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_container.contacts) {
|
||||
contact := &sim_state.contact_container.contacts[i]
|
||||
body, body2 := get_body(sim_state, contact.a), get_body(sim_state, contact.b)
|
||||
|
||||
contact^ = Contact {
|
||||
a = contact.a,
|
||||
b = contact.b,
|
||||
prev_x_a = body.x,
|
||||
prev_x_b = body2.x,
|
||||
prev_q_a = body.q,
|
||||
prev_q_b = body2.q,
|
||||
manifold = contact.manifold,
|
||||
}
|
||||
|
||||
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)
|
||||
|
||||
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
|
||||
|
||||
apply_position_correction(body, corr1, p1)
|
||||
apply_position_correction(body2, corr2, p2)
|
||||
}
|
||||
}
|
||||
|
||||
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)
|
||||
|
||||
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: 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)
|
||||
|
||||
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.00002,
|
||||
-tangent_diff_len,
|
||||
-tangent_diff_normalized,
|
||||
p1,
|
||||
p2,
|
||||
)
|
||||
|
||||
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_position_correction(body, corr1_tangent, p1)
|
||||
apply_position_correction(body2, corr2_tangent, p2)
|
||||
}
|
||||
}
|
||||
}
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
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 {
|
||||
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 &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
|
||||
|
||||
for point_idx in 0 ..< manifold.points_len {
|
||||
if contact.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 &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 || 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])
|
||||
|
||||
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 :: 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, friction_p1),
|
||||
get_body_inverse_mass(body2, v_tangent_norm, friction_p2)
|
||||
|
||||
w := w1 + w2
|
||||
|
||||
if w != 0 {
|
||||
delta_v :=
|
||||
-v_tangent_norm *
|
||||
min(
|
||||
dt * DYNAMIC_FRICTION * abs(total_lambda_normal / (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))
|
||||
}
|
||||
}
|
||||
|
||||
{
|
||||
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)
|
||||
}
|
||||
}
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
|
||||
// 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_position_correction(body, corr, v.hit_point)
|
||||
body_solve_velocity(body, body.prev_x, body.prev_q, inv_dt)
|
||||
}
|
||||
}
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
|
||||
calculate_soft_constraint_params :: proc(
|
||||
natural_freq, damping_ratio, dt: f32,
|
||||
) -> (
|
||||
@ -951,6 +466,85 @@ pgs_solve_contacts :: proc(
|
||||
}
|
||||
}
|
||||
|
||||
pgs_solve_suspension :: proc(sim_state: ^Sim_State, config: Solver_Config, dt: f32, inv_dt: f32) {
|
||||
// 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 {
|
||||
wheel_world_pos := body_local_to_world(body, v.rel_pos)
|
||||
dir := body_local_to_world_vec(body, v.rel_dir)
|
||||
pos2 := wheel_world_pos + dir * v.rest
|
||||
v.hit_t, v.hit_point, v.hit = collision.intersect_segment_plane(
|
||||
{wheel_world_pos, pos2},
|
||||
collision.plane_from_point_normal({}, collision.Vec3{0, 1, 0}),
|
||||
)
|
||||
w := get_body_angular_inverse_mass(body, dir)
|
||||
inv_w := 1.0 / w
|
||||
if v.hit {
|
||||
{
|
||||
bias_coef, mass_coef, impulse_coef := calculate_soft_constraint_params(
|
||||
v.natural_frequency,
|
||||
v.damping,
|
||||
dt,
|
||||
)
|
||||
|
||||
vel := lg.dot(body_velocity_at_point(body, wheel_world_pos), dir)
|
||||
x := v.hit_t
|
||||
separation := v.rest - x
|
||||
|
||||
incremental_impulse :=
|
||||
-inv_w * mass_coef * (vel + separation * bias_coef) -
|
||||
impulse_coef * v.spring_impulse
|
||||
v.spring_impulse += incremental_impulse
|
||||
|
||||
apply_velocity_correction(body, incremental_impulse * dir, wheel_world_pos)
|
||||
}
|
||||
|
||||
// Drive forces
|
||||
if true {
|
||||
total_impulse := v.drive_impulse - v.brake_impulse
|
||||
forward := body_local_to_world_vec(body, Vec3{0, 0, 1})
|
||||
|
||||
apply_velocity_correction(
|
||||
body,
|
||||
total_impulse * forward * dt,
|
||||
wheel_world_pos,
|
||||
)
|
||||
}
|
||||
|
||||
// Lateral friction
|
||||
if true {
|
||||
vel_contact := body_velocity_at_point(body, wheel_world_pos)
|
||||
right := wheel_get_right_vec(body, v)
|
||||
|
||||
lateral_vel := lg.dot(right, vel_contact)
|
||||
|
||||
friction := f32(0.8)
|
||||
friction_clamp := -v.spring_impulse * friction
|
||||
|
||||
incremental_impulse := -inv_w * lateral_vel
|
||||
new_total_impulse := clamp(
|
||||
v.lateral_impulse + incremental_impulse,
|
||||
-friction_clamp,
|
||||
friction_clamp,
|
||||
)
|
||||
applied_impulse := new_total_impulse - v.lateral_impulse
|
||||
v.lateral_impulse = new_total_impulse
|
||||
|
||||
apply_velocity_correction(body, applied_impulse * right, wheel_world_pos)
|
||||
}
|
||||
} else {
|
||||
v.lateral_impulse = 0
|
||||
v.spring_impulse = 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]
|
||||
@ -985,10 +579,28 @@ pgs_substep :: proc(sim_state: ^Sim_State, config: Solver_Config, dt: f32, inv_d
|
||||
apply_velocity_correction(body2, total_tangent_impulse, p2)
|
||||
}
|
||||
}
|
||||
|
||||
for i in 0 ..< len(sim_state.suspension_constraints) {
|
||||
s := &sim_state.suspension_constraints_slice[i]
|
||||
|
||||
if s.hit {
|
||||
body := get_body(sim_state, s.body)
|
||||
p := body_local_to_world(body, s.rel_pos)
|
||||
right := wheel_get_right_vec(body, s)
|
||||
|
||||
apply_velocity_correction(
|
||||
body,
|
||||
s.spring_impulse * body_local_to_world_vec(body, s.rel_dir),
|
||||
p,
|
||||
)
|
||||
apply_velocity_correction(body, s.lateral_impulse * right, p)
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
apply_bias := true
|
||||
pgs_solve_contacts(sim_state, config, dt, inv_dt, apply_bias)
|
||||
pgs_solve_suspension(sim_state, config, dt, inv_dt)
|
||||
|
||||
for i in 0 ..< len(sim_state.bodies_slice) {
|
||||
body := &sim_state.bodies_slice[i]
|
||||
@ -1014,6 +626,7 @@ pgs_substep :: proc(sim_state: ^Sim_State, config: Solver_Config, dt: f32, inv_d
|
||||
|
||||
apply_bias = false
|
||||
pgs_solve_contacts(sim_state, config, dt, inv_dt, apply_bias)
|
||||
// pgs_solve_suspension(sim_state, config, dt, inv_dt, apply_bias)
|
||||
}
|
||||
|
||||
simulate_step :: proc(scene: ^Scene, sim_state: ^Sim_State, config: Solver_Config) {
|
||||
|
||||
455
game/physics/xpbd.odin
Normal file
455
game/physics/xpbd.odin
Normal file
@ -0,0 +1,455 @@
|
||||
package physics
|
||||
|
||||
import lg "core:math/linalg"
|
||||
import "libs:tracy"
|
||||
|
||||
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 {
|
||||
body.prev_x = body.x
|
||||
body.prev_v = body.v
|
||||
body.prev_w = body.w
|
||||
body.prev_q = body.q
|
||||
|
||||
body.v += config.gravity * dt * (body.inv_mass == 0 ? 0 : 1) // special case for gravity, TODO
|
||||
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
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
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_container.contacts) {
|
||||
contact := &sim_state.contact_container.contacts[i]
|
||||
body, body2 := get_body(sim_state, contact.a), get_body(sim_state, contact.b)
|
||||
|
||||
contact^ = Contact {
|
||||
a = contact.a,
|
||||
b = contact.b,
|
||||
prev_x_a = body.x,
|
||||
prev_x_b = body2.x,
|
||||
prev_q_a = body.q,
|
||||
prev_q_b = body2.q,
|
||||
manifold = contact.manifold,
|
||||
}
|
||||
|
||||
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)
|
||||
|
||||
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
|
||||
|
||||
apply_position_correction(body, corr1, p1)
|
||||
apply_position_correction(body2, corr2, p2)
|
||||
}
|
||||
}
|
||||
|
||||
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)
|
||||
|
||||
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: 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)
|
||||
|
||||
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.00002,
|
||||
-tangent_diff_len,
|
||||
-tangent_diff_normalized,
|
||||
p1,
|
||||
p2,
|
||||
)
|
||||
|
||||
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_position_correction(body, corr1_tangent, p1)
|
||||
apply_position_correction(body2, corr2_tangent, p2)
|
||||
}
|
||||
}
|
||||
}
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
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)
|
||||
}
|
||||
}
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
{
|
||||
// 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 &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
|
||||
|
||||
for point_idx in 0 ..< manifold.points_len {
|
||||
if contact.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 &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 || 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])
|
||||
|
||||
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 :: 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, friction_p1),
|
||||
get_body_inverse_mass(body2, v_tangent_norm, friction_p2)
|
||||
|
||||
w := w1 + w2
|
||||
|
||||
if w != 0 {
|
||||
delta_v :=
|
||||
-v_tangent_norm *
|
||||
min(
|
||||
dt * DYNAMIC_FRICTION * abs(total_lambda_normal / (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))
|
||||
}
|
||||
}
|
||||
|
||||
{
|
||||
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)
|
||||
}
|
||||
}
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
|
||||
// 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_position_correction(body, corr, v.hit_point)
|
||||
body_solve_velocity(body, body.prev_x, body.prev_q, inv_dt)
|
||||
}
|
||||
}
|
||||
}
|
||||
}
|
||||
}
|
||||
Loading…
x
Reference in New Issue
Block a user