137 lines
3.0 KiB
Odin
137 lines
3.0 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] {
|
|
debug_draw_manifold_points(
|
|
-contact.manifold.normal,
|
|
contact.manifold.points_a[:contact.manifold.points_len],
|
|
color = debug.int_to_color(i32(contact_idx * 2 + 0)),
|
|
)
|
|
debug_draw_manifold_points(
|
|
contact.manifold.normal,
|
|
contact.manifold.points_b[:contact.manifold.points_len],
|
|
color = debug.int_to_color(i32(contact_idx * 2 + 1)),
|
|
)
|
|
}
|
|
}
|
|
|
|
debug_draw_manifold_points :: proc(normal: rl.Vector3, points: []rl.Vector3, color: rl.Color) {
|
|
if len(points) >= 3 {
|
|
// Triangle or quad
|
|
v1 := points[0]
|
|
|
|
for i in 2 ..< len(points) {
|
|
v2, v3 := points[i - 1], points[i]
|
|
|
|
rl.DrawTriangle3D(v1, v2, v3, color)
|
|
}
|
|
} else if len(points) == 2 {
|
|
// Line
|
|
rl.DrawLine3D(points[0], points[1], color)
|
|
}
|
|
|
|
for p in points {
|
|
rl.DrawLine3D(p, p + normal, color)
|
|
|
|
rl.DrawSphereWires(p, 0.1, 4, 4, color)
|
|
}
|
|
}
|