517 lines
12 KiB
Odin
517 lines
12 KiB
Odin
package physics
|
|
|
|
import "collision"
|
|
import "core:fmt"
|
|
import "core:math"
|
|
import lg "core:math/linalg"
|
|
import "game:halfedge"
|
|
import rl "vendor:raylib"
|
|
|
|
_ :: math
|
|
_ :: fmt
|
|
|
|
Solver_Config :: struct {
|
|
// Will automatically do fixed timestep
|
|
timestep: f32,
|
|
gravity: rl.Vector3,
|
|
substreps_minus_one: i32,
|
|
}
|
|
|
|
Solver_State :: struct {
|
|
accumulated_time: f32,
|
|
// Incremented when simulate is called (not simulate_step)
|
|
simulation_frame: u32,
|
|
|
|
// Number of immediate bodies referenced this frame
|
|
num_referenced_bodies: i32,
|
|
num_referenced_suspension_constraints: i32,
|
|
immedate_bodies: map[u32]Immedate_State(Body_Handle),
|
|
immediate_suspension_constraints: map[u32]Immedate_State(Suspension_Constraint_Handle),
|
|
}
|
|
|
|
destroy_solver_state :: proc(state: ^Solver_State) {
|
|
delete(state.immedate_bodies)
|
|
delete(state.immediate_suspension_constraints)
|
|
}
|
|
|
|
Immedate_State :: struct($T: typeid) {
|
|
handle: T,
|
|
// When was this referenced last time (frame number)
|
|
last_ref: u32,
|
|
}
|
|
|
|
MAX_STEPS :: 10
|
|
|
|
// Outer simulation loop for fixed timestepping
|
|
simulate :: proc(scene: ^Scene, state: ^Solver_State, config: Solver_Config, dt: f32) {
|
|
assert(config.timestep > 0)
|
|
|
|
prune_immediate(scene, state)
|
|
|
|
state.accumulated_time += dt
|
|
|
|
num_steps := 0
|
|
for state.accumulated_time >= config.timestep {
|
|
num_steps += 1
|
|
state.accumulated_time -= config.timestep
|
|
|
|
if num_steps < MAX_STEPS {
|
|
simulate_step(scene, config)
|
|
}
|
|
}
|
|
|
|
state.simulation_frame += 1
|
|
state.num_referenced_bodies = 0
|
|
state.num_referenced_suspension_constraints = 0
|
|
}
|
|
|
|
Body_Sim_State :: struct {
|
|
prev_x: rl.Vector3,
|
|
prev_q: rl.Quaternion,
|
|
}
|
|
|
|
GLOBAL_PLANE :: collision.Plane {
|
|
normal = rl.Vector3{0, 1, 0},
|
|
dist = 0,
|
|
}
|
|
|
|
Contact_Pair :: struct {
|
|
a, b: Body_Handle,
|
|
manifold: collision.Contact_Manifold,
|
|
lambdas: [4]f32,
|
|
}
|
|
|
|
simulate_step :: proc(scene: ^Scene, config: Solver_Config) {
|
|
body_states := make([]Body_Sim_State, len(scene.bodies), context.temp_allocator)
|
|
|
|
scene.contact_pairs_len = 0
|
|
|
|
substeps := config.substreps_minus_one + 1
|
|
|
|
dt := config.timestep / f32(substeps)
|
|
inv_dt := 1.0 / dt
|
|
|
|
for _ in 0 ..< substeps {
|
|
// Integrate positions and rotations
|
|
for &body, i in scene.bodies {
|
|
if body.alive {
|
|
body_states[i].prev_x = body.x
|
|
body.v += config.gravity * dt * (body.inv_mass == 0 ? 0 : 1) // special case for gravity, TODO
|
|
body.x += body.v * dt
|
|
|
|
body_states[i].prev_q = body.q
|
|
|
|
// 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
|
|
}
|
|
}
|
|
|
|
|
|
Body_Pair :: struct {
|
|
a, b: int,
|
|
}
|
|
handled_pairs := make_map(map[Body_Pair]bool, context.temp_allocator)
|
|
|
|
for _, i in scene.bodies {
|
|
body := &scene.bodies_slice[i]
|
|
if body.alive {
|
|
for _, j in scene.bodies {
|
|
body2 := &scene.bodies_slice[j]
|
|
|
|
if i != j && body2.alive && !handled_pairs[{a = min(i, j), b = max(i, j)}] {
|
|
s1, s2 := body.shape.(Shape_Box), body2.shape.(Shape_Box)
|
|
|
|
box1 := collision.box_to_convex(
|
|
collision.Box{rad = s1.size * 0.5},
|
|
context.temp_allocator,
|
|
)
|
|
box2 := collision.box_to_convex(
|
|
collision.Box{rad = s2.size * 0.5},
|
|
context.temp_allocator,
|
|
)
|
|
|
|
mat1 := lg.matrix4_translate(body.x) * lg.matrix4_from_quaternion(body.q)
|
|
mat2 := lg.matrix4_translate(body2.x) * lg.matrix4_from_quaternion(body2.q)
|
|
|
|
halfedge.transform_mesh(&box1, mat1)
|
|
halfedge.transform_mesh(&box2, mat2)
|
|
|
|
manifold, collision := collision.convex_vs_convex_sat(box1, box2)
|
|
|
|
if collision {
|
|
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
|
|
|
|
// Convert manifold contact from world to local space
|
|
for point_idx in 0 ..< manifold.points_len {
|
|
manifold.points_a[point_idx] = body_world_to_local(
|
|
body,
|
|
manifold.points_a[point_idx],
|
|
)
|
|
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 :=
|
|
manifold.points_a[point_idx], manifold.points_b[point_idx]
|
|
p1, p2 =
|
|
body_local_to_world(body, p1), body_local_to_world(body2, p2)
|
|
|
|
separation := min(lg.dot(p2 - p1, manifold.normal), 0)
|
|
|
|
handled_pairs[{a = min(i, j), b = max(i, j)}] = true
|
|
|
|
lambda_norm, corr1, corr2, ok := calculate_constraint_params2(
|
|
dt,
|
|
body,
|
|
body2,
|
|
0,
|
|
separation,
|
|
-manifold.normal,
|
|
p1,
|
|
p2,
|
|
)
|
|
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)
|
|
}
|
|
}
|
|
}
|
|
}
|
|
}
|
|
}
|
|
}
|
|
}
|
|
}
|
|
|
|
for &v in scene.suspension_constraints {
|
|
if v.alive {
|
|
body := get_body(scene, 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,
|
|
)
|
|
}
|
|
}
|
|
}
|
|
|
|
solve_velocities(scene, body_states, inv_dt)
|
|
|
|
// for pair in scene.contact_pairs[:scene.contact_pairs_len] {
|
|
// pair.a
|
|
// }
|
|
|
|
// Solve suspension velocity
|
|
for _, i in scene.suspension_constraints {
|
|
v := &scene.suspension_constraints_slice[i]
|
|
if v.alive {
|
|
body_idx := int(v.body) - 1
|
|
body := get_body(scene, v.body)
|
|
|
|
if body.alive && v.hit {
|
|
prev_x, prev_q := body_states[body_idx].prev_x, body_states[body_idx].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)
|
|
body_state := body_states[i32(v.body) - 1]
|
|
|
|
// 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_state, inv_dt)
|
|
}
|
|
|
|
// Drive forces
|
|
if true {
|
|
total_impulse := v.drive_impulse - v.brake_impulse
|
|
forward := body_local_to_world_vec(body, rl.Vector3{0, 0, 1})
|
|
|
|
_ = apply_constraint_correction_unilateral(
|
|
dt,
|
|
body,
|
|
0,
|
|
total_impulse * dt * dt,
|
|
-forward,
|
|
wheel_world_pos,
|
|
)
|
|
body_solve_velocity(body, body_state, inv_dt)
|
|
}
|
|
|
|
// Lateral friction
|
|
if true {
|
|
local_contact_pos := v.hit_point - body.x
|
|
vel_contact := body_velocity_at_local_point(body, local_contact_pos)
|
|
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_state, inv_dt)
|
|
}
|
|
}
|
|
}
|
|
}
|
|
}
|
|
}
|
|
|
|
solve_velocities :: proc(scene: ^Scene, body_states: []Body_Sim_State, inv_dt: f32) {
|
|
// Compute new linear and angular velocities
|
|
for _, i in scene.bodies_slice {
|
|
body := &scene.bodies_slice[i]
|
|
if body.alive {
|
|
body_solve_velocity(body, body_states[i], inv_dt)
|
|
}
|
|
}
|
|
}
|
|
|
|
body_solve_velocity :: #force_inline proc(body: Body_Ptr, state: Body_Sim_State, inv_dt: f32) {
|
|
body.v = (body.x - state.prev_x) * inv_dt
|
|
|
|
delta_q := body.q * lg.quaternion_inverse(state.prev_q)
|
|
body.w = rl.Vector3{delta_q.x, delta_q.y, delta_q.z} * 2.0 * inv_dt
|
|
|
|
if delta_q.w < 0 {
|
|
body.w = -body.w
|
|
}
|
|
}
|
|
|
|
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,
|
|
compliance: f32,
|
|
error: f32,
|
|
error_gradient: rl.Vector3,
|
|
pos: rl.Vector3,
|
|
other_combined_inv_mass: f32 = 0,
|
|
) -> (
|
|
lambda: f32,
|
|
) {
|
|
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)
|
|
}
|
|
|
|
return
|
|
}
|
|
|
|
apply_correction :: proc(body: Body_Ptr, corr: rl.Vector3, pos: rl.Vector3) {
|
|
body.x += corr * body.inv_mass
|
|
|
|
q := body.q
|
|
inv_q := lg.quaternion_normalize0(lg.quaternion_inverse(q))
|
|
delta_omega := pos - body.x
|
|
delta_omega = lg.cross(delta_omega, corr)
|
|
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)
|
|
|
|
delta_rot := quaternion(x = delta_omega.x, y = delta_omega.y, z = delta_omega.z, w = 0)
|
|
delta_rot *= q
|
|
q.x += 0.5 * delta_rot.x
|
|
q.y += 0.5 * delta_rot.y
|
|
q.z += 0.5 * delta_rot.z
|
|
q.w += 0.5 * delta_rot.w
|
|
q = lg.normalize0(q)
|
|
|
|
body.q = q
|
|
}
|
|
|
|
get_body_inverse_mass :: proc(body: Body_Ptr, normal, pos: rl.Vector3) -> f32 {
|
|
q := body.q
|
|
inv_q := lg.quaternion_normalize0(lg.quaternion_inverse(q))
|
|
|
|
rn := pos - body.x
|
|
rn = lg.cross(rn, normal)
|
|
rn = lg.quaternion_mul_vector3(inv_q, rn)
|
|
|
|
w := lg.dot(rn * rn, body.inv_inertia_tensor)
|
|
w += body.inv_mass
|
|
|
|
return w
|
|
}
|