sergeypdev
f2f23ee2e0
Turns out when you get a collision pair YOU SHOULD HANDLE CONTACT RESPONSE FOR BOTH BODIES, NOT JUST ONE OF THEM Otherwise they move and their next collision (if a->b, b->a are found and resolved separately) they might penetrate badly or worse and everything blows up This is probably not as important in sequential impulse type solvers because the actual position update is deferred, but in a position based one it's pretty important as it turns out. It's also better for performance because I don't have to run the collision query for the same two bodies two times
123 lines
2.8 KiB
Odin
123 lines
2.8 KiB
Odin
package physics
|
|
|
|
import "core:log"
|
|
import "core:math"
|
|
import lg "core:math/linalg"
|
|
import "game:debug"
|
|
import "libs:tracy"
|
|
import rl "vendor:raylib"
|
|
import "vendor:raylib/rlgl"
|
|
|
|
_ :: log
|
|
_ :: math
|
|
_ :: debug
|
|
|
|
draw_debug_shape :: proc(
|
|
shape: Collision_Shape,
|
|
pos: rl.Vector3,
|
|
rot: rl.Quaternion,
|
|
color: rl.Color,
|
|
) {
|
|
rlgl.PushMatrix()
|
|
defer rlgl.PopMatrix()
|
|
|
|
rlgl.Begin(rlgl.LINES)
|
|
defer rlgl.End()
|
|
|
|
switch s in shape {
|
|
case Shape_Sphere:
|
|
rl.DrawSphere(pos, s.radius, color)
|
|
case Shape_Box:
|
|
mat := lg.matrix4_from_trs(pos, rot, 1)
|
|
|
|
rlgl.LoadIdentity()
|
|
rlgl.MultMatrixf(cast([^]f32)&mat)
|
|
|
|
rl.DrawCubeWiresV(0, s.size, color)
|
|
}
|
|
}
|
|
|
|
draw_debug_scene :: proc(scene: ^Scene) {
|
|
tracy.Zone()
|
|
|
|
for &body, i in scene.bodies {
|
|
if body.alive {
|
|
pos := body.x
|
|
|
|
q := body.q
|
|
x := lg.quaternion_mul_vector3(q, rl.Vector3{1, 0, 0})
|
|
y := lg.quaternion_mul_vector3(q, rl.Vector3{0, 1, 0})
|
|
z := lg.quaternion_mul_vector3(q, rl.Vector3{0, 0, 1})
|
|
|
|
rl.DrawLine3D(pos, pos + x, rl.RED)
|
|
rl.DrawLine3D(pos, pos + y, rl.GREEN)
|
|
rl.DrawLine3D(pos, pos + z, rl.BLUE)
|
|
|
|
draw_debug_shape(body.shape, body.x, body.q, debug.int_to_color(i32(i + 1)))
|
|
}
|
|
}
|
|
|
|
for _, i in scene.suspension_constraints {
|
|
wheel := &scene.suspension_constraints_slice[i]
|
|
if wheel.alive {
|
|
body := get_body(scene, wheel.body)
|
|
t := wheel.hit_t > 0 ? wheel.hit_t : wheel.rest
|
|
|
|
pos := body.x
|
|
rot := body.q
|
|
pos += lg.quaternion_mul_vector3(rot, wheel.rel_pos)
|
|
dir := lg.quaternion_mul_vector3(rot, wheel.rel_dir)
|
|
|
|
rl.DrawLine3D(pos, pos + dir * t, rl.ORANGE)
|
|
|
|
rel_wheel_pos := wheel_get_rel_wheel_pos(body, wheel)
|
|
wheel_pos := body_local_to_world(body, rel_wheel_pos)
|
|
right := wheel_get_right_vec(body, wheel)
|
|
|
|
rl.DrawCylinderWiresEx(
|
|
wheel_pos - right * 0.1,
|
|
wheel_pos + right * 0.1,
|
|
wheel.radius,
|
|
wheel.radius,
|
|
16,
|
|
rl.RED,
|
|
)
|
|
|
|
rl.DrawLine3D(wheel_pos, wheel_pos + right * 10, rl.RED)
|
|
|
|
if wheel.hit {
|
|
// rl.DrawLine3D(
|
|
// pos + t * dir,
|
|
// pos + t * dir + wheel.applied_impulse.x * right * 10,
|
|
// rl.RED,
|
|
// )
|
|
}
|
|
|
|
if wheel.hit {
|
|
rl.DrawSphereWires(wheel.hit_point, 0.1, 4, 4, rl.RED)
|
|
}
|
|
}
|
|
}
|
|
|
|
for &contact, contact_idx in scene.contact_pairs[:scene.contact_pairs_len] {
|
|
color := debug.int_to_color(i32(contact_idx))
|
|
if contact.manifold.points_len >= 3 {
|
|
// Triangle or quad
|
|
v1 := contact.manifold.points_a[0]
|
|
|
|
for i in 2 ..< contact.manifold.points_len {
|
|
v2, v3 := contact.manifold.points_a[i - 1], contact.manifold.points_a[i]
|
|
|
|
rl.DrawTriangle3D(v1, v2, v3, color)
|
|
}
|
|
} else if contact.manifold.points_len == 2 {
|
|
// Line
|
|
rl.DrawLine3D(contact.manifold.points_a[0], contact.manifold.points_a[1], color)
|
|
}
|
|
|
|
for p in contact.manifold.points_a[:contact.manifold.points_len] {
|
|
rl.DrawSphereWires(p, 0.1, 4, 4, color)
|
|
}
|
|
}
|
|
}
|