535 lines
13 KiB
Odin
535 lines
13 KiB
Odin
package physics
|
|
|
|
import "base:runtime"
|
|
import "bvh"
|
|
import "common:name"
|
|
import "core:fmt"
|
|
import "core:log"
|
|
import "core:math"
|
|
import lg "core:math/linalg"
|
|
import "core:mem"
|
|
import "core:slice"
|
|
import "core:strings"
|
|
import "game:assets"
|
|
import "game:debug"
|
|
import he "game:halfedge"
|
|
import "game:ui"
|
|
import rl "libs:raylib"
|
|
import "libs:raylib/rlgl"
|
|
import "libs:tracy"
|
|
|
|
_ :: name
|
|
_ :: mem
|
|
_ :: fmt
|
|
_ :: log
|
|
_ :: math
|
|
_ :: debug
|
|
_ :: he
|
|
|
|
draw_debug_shape :: proc(
|
|
sim_state: ^Sim_State,
|
|
shape: Collision_Shape,
|
|
pos: Vec3,
|
|
rot: Quat,
|
|
color: rl.Color,
|
|
) {
|
|
mat := lg.matrix4_from_trs(pos, rot, 1)
|
|
shape_mat := lg.matrix4_translate_f32(shape.rel_x) * lg.matrix4_from_quaternion(shape.rel_q)
|
|
mat *= auto_cast shape_mat
|
|
|
|
rlgl.PushMatrix()
|
|
defer rlgl.PopMatrix()
|
|
|
|
rlgl.LoadIdentity()
|
|
rlgl.MultMatrixf(cast([^]f32)&mat)
|
|
|
|
switch s in shape.inner_shape {
|
|
case Shape_Box:
|
|
rl.DrawCubeV(0, s.size, color)
|
|
case Internal_Shape_Convex:
|
|
mesh := convex_container_get_mesh(&sim_state.convex_container, s.mesh)
|
|
he.debug_draw_mesh_wires(mesh, color)
|
|
}
|
|
}
|
|
|
|
Debug_State :: struct {
|
|
selected_contacts: map[int]struct {},
|
|
}
|
|
|
|
init_debug_state :: proc(debug_state: ^Debug_State) {
|
|
debug_state.selected_contacts = make(map[int]struct {}, context.temp_allocator)
|
|
}
|
|
|
|
draw_debug_scene :: proc(scene: ^Scene, debug_state: ^Debug_State) {
|
|
tracy.Zone()
|
|
|
|
runtime.DEFAULT_TEMP_ALLOCATOR_TEMP_GUARD()
|
|
|
|
sim_state := get_sim_state(scene)
|
|
sim_cache: Sim_Cache
|
|
sim_cache.level_geom_asset_bvh = make_map(
|
|
map[Level_Geom_Handle]assets.Loaded_BVH,
|
|
context.temp_allocator,
|
|
)
|
|
|
|
// Static_TLAS
|
|
if true && sim_state.static_tlas.built {
|
|
bvh.debug_draw_bvh_bounds(
|
|
&sim_state.static_tlas.bvh_tree,
|
|
sim_state.static_tlas.level_geom_aabbs,
|
|
0,
|
|
)
|
|
|
|
it := bvh.iterator(&sim_state.static_tlas.bvh_tree)
|
|
|
|
if false {
|
|
for node in bvh.iterator_next(&it) {
|
|
if bvh.is_leaf_node(node) {
|
|
prim_start := node.child_or_prim_start
|
|
|
|
for level_geom_idx in prim_start ..< prim_start + node.prim_len {
|
|
level_geom_handle := index_to_level_geom(int(level_geom_idx))
|
|
level_geom := get_level_geom(sim_state, level_geom_handle)
|
|
blas, vertices, indices := get_level_geom_data(
|
|
sim_state,
|
|
&sim_cache,
|
|
level_geom_handle,
|
|
)
|
|
|
|
bvh.debug_draw_bvh_bounds_mesh(
|
|
&blas,
|
|
bvh.Mesh{vertices = vertices, indices = indices},
|
|
level_geom.x,
|
|
0,
|
|
)
|
|
}
|
|
}
|
|
}
|
|
}
|
|
}
|
|
|
|
// Dynamic TLAS
|
|
if false && sim_state.dynamic_tlas.built {
|
|
bvh.debug_draw_bvh_bounds(
|
|
&sim_state.dynamic_tlas.bvh_tree,
|
|
sim_state.dynamic_tlas.body_aabbs,
|
|
0,
|
|
)
|
|
}
|
|
|
|
for _, i in sim_state.bodies {
|
|
body := &sim_state.bodies_slice[i]
|
|
if body.alive {
|
|
pos := body.x
|
|
|
|
q := body.q
|
|
x := lg.quaternion_mul_vector3(q, Vec3{1, 0, 0})
|
|
y := lg.quaternion_mul_vector3(q, Vec3{0, 1, 0})
|
|
z := lg.quaternion_mul_vector3(q, Vec3{0, 0, 1})
|
|
|
|
rl.DrawLine3D(pos, pos + x, rl.RED)
|
|
rl.DrawLine3D(pos, pos + y, rl.GREEN)
|
|
rl.DrawLine3D(pos, pos + z, rl.BLUE)
|
|
|
|
it := shapes_iterator(sim_state, body.shape)
|
|
for shape in shapes_iterator_next(&it) {
|
|
draw_debug_shape(
|
|
sim_state,
|
|
shape^,
|
|
body_get_shape_pos(body),
|
|
body.q,
|
|
debug.int_to_color(i32(i + 2)),
|
|
)
|
|
|
|
shape_aabb := body_transform_shape_aabb(body, shape_get_aabb(shape^))
|
|
rl.DrawBoundingBox(
|
|
rl.BoundingBox {
|
|
min = shape_aabb.center - shape_aabb.extent,
|
|
max = shape_aabb.center + shape_aabb.extent,
|
|
},
|
|
debug.int_to_color(i32(i + 2)),
|
|
)
|
|
}
|
|
}
|
|
}
|
|
|
|
// if true {
|
|
// for &level_geom, geom_idx in sim_state.level_geoms {
|
|
// if level_geom.alive {
|
|
// vertices, indices := get_level_geom_data(sim_state, level_geom.geometry)
|
|
|
|
// for i in 0 ..< len(indices) / 3 {
|
|
// i1, i2, i3 := indices[i * 3 + 0], indices[i * 3 + 1], indices[i * 3 + 2]
|
|
// v1, v2, v3 := vertices[i1], vertices[i2], vertices[i3]
|
|
|
|
// rl.DrawTriangle3D(v1, v2, v3, debug.int_to_color(i32(geom_idx + 1)))
|
|
// }
|
|
// }
|
|
// }
|
|
// }
|
|
|
|
for _, i in sim_state.suspension_constraints {
|
|
wheel := &sim_state.suspension_constraints_slice[i]
|
|
if wheel.alive {
|
|
body := get_body(sim_state, wheel.body)
|
|
|
|
pos := body.x
|
|
rot := body.q
|
|
pos += lg.quaternion_mul_vector3(rot, wheel.rel_pos + body.shape_offset)
|
|
|
|
wheel_world_pos := body_local_to_world(body, wheel.rel_pos + body.shape_offset)
|
|
dir := body_local_to_world_vec(body, wheel.rel_dir)
|
|
rl.DrawLine3D(wheel_world_pos, wheel_world_pos + dir, rl.BLUE)
|
|
|
|
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)
|
|
|
|
rotation_dir := body_local_to_world_vec(
|
|
body,
|
|
lg.matrix3_rotate(wheel.q, Vec3{-1, 0, 0}) * Vec3{0, 1, 0},
|
|
)
|
|
|
|
rl.DrawLine3D(
|
|
wheel_pos - right * 0.1,
|
|
wheel_pos - right * 0.1 + rotation_dir * wheel.radius,
|
|
rl.ORANGE,
|
|
)
|
|
rl.DrawLine3D(
|
|
wheel_pos + right * 0.1,
|
|
wheel_pos + right * 0.1 + rotation_dir * wheel.radius,
|
|
rl.ORANGE,
|
|
)
|
|
|
|
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)
|
|
}
|
|
}
|
|
}
|
|
|
|
if true {
|
|
for &contact, contact_idx in sim_state.contact_container.contacts {
|
|
if contact_idx in debug_state.selected_contacts ||
|
|
len(debug_state.selected_contacts) == 0 {
|
|
points_a := contact.manifold.points_a
|
|
points_b := contact.manifold.points_b
|
|
points_a_slice, points_b_slice :=
|
|
points_a[:contact.manifold.points_len], points_b[:contact.manifold.points_len]
|
|
debug_transform_points_local_to_world(
|
|
get_body(sim_state, contact.a),
|
|
points_a_slice,
|
|
)
|
|
b_handle :=
|
|
Body_Handle(contact.b) if contact.type == .Body_vs_Body else INVALID_BODY
|
|
debug_transform_points_local_to_world(
|
|
get_body(sim_state, b_handle),
|
|
points_b_slice,
|
|
)
|
|
debug_draw_manifold_points(
|
|
contact,
|
|
-1,
|
|
points_a_slice,
|
|
color = debug.int_to_color(i32(contact_idx * 2 + 0)),
|
|
)
|
|
debug_draw_manifold_points(
|
|
contact,
|
|
1,
|
|
points_b_slice,
|
|
color = debug.int_to_color(i32(contact_idx * 2 + 1)),
|
|
)
|
|
}
|
|
}
|
|
}
|
|
}
|
|
|
|
draw_debug_ui :: proc(
|
|
debug_state: ^Debug_State,
|
|
ctx: ^ui.Context,
|
|
scene: ^Scene,
|
|
config: Solver_Config,
|
|
) {
|
|
tracy.Zone()
|
|
|
|
sim_state := get_sim_state(scene)
|
|
|
|
if .ACTIVE in ui.treenode(ctx, "Bodies") {
|
|
for _, i in sim_state.bodies_slice {
|
|
body := &sim_state.bodies_slice[i]
|
|
if body.alive {
|
|
if .ACTIVE in
|
|
ui.treenode(ctx, fmt.tprintf("%v (%v)", name.to_string(body.name), i)) {
|
|
ui.keyval(ctx, "Pos", body.x)
|
|
ui.keyval(ctx, "Shape Offset", body.shape_offset)
|
|
aabb := body_get_aabb(body)
|
|
ui.keyval(ctx, "AABB", aabb)
|
|
ui.keyval(ctx, "Inv Inertia", body.inv_inertia_tensor)
|
|
}
|
|
}
|
|
}
|
|
}
|
|
|
|
if .ACTIVE in ui.treenode(ctx, "Contacts") {
|
|
@(static) search_buf: [1024]u8
|
|
@(static) search_len: int
|
|
ui.textbox(ctx, search_buf[:], &search_len)
|
|
search_str := string(search_buf[0:search_len])
|
|
Contact_And_Index :: struct {
|
|
contact: Contact,
|
|
idx: int,
|
|
}
|
|
sorted_contacts := make(
|
|
[]Contact_And_Index,
|
|
len(sim_state.contact_container.contacts),
|
|
context.temp_allocator,
|
|
)
|
|
for &contact, i in sim_state.contact_container.contacts {
|
|
sorted_contacts[i] = Contact_And_Index {
|
|
contact = contact,
|
|
idx = i,
|
|
}
|
|
}
|
|
slice.reverse_sort_by_key(
|
|
sorted_contacts,
|
|
#force_inline proc(contact_and_index: Contact_And_Index) -> f32 {
|
|
return lg.max(contact_and_index.contact.total_normal_impulse)
|
|
},
|
|
)
|
|
for &contact_and_index, i in sorted_contacts {
|
|
contact := contact_and_index.contact
|
|
title: string
|
|
|
|
ui.push_id(ctx, mem.any_to_bytes(i))
|
|
defer ui.pop_id(ctx)
|
|
|
|
if contact.type == .Body_vs_Body {
|
|
title = fmt.tprintf(
|
|
"%d %v v %v",
|
|
contact_and_index.idx,
|
|
name.to_string(get_body(sim_state, contact.a).name),
|
|
name.to_string(get_body(sim_state, Body_Handle(contact.b)).name),
|
|
)
|
|
} else {
|
|
title = fmt.tprintf(
|
|
"%d %v v lvl",
|
|
contact_and_index.idx,
|
|
name.to_string(get_body(sim_state, contact.a).name),
|
|
)
|
|
}
|
|
|
|
if strings.contains(title, search_str) {
|
|
if ui.inspect_value(ctx, title, contact) {
|
|
debug_state.selected_contacts[contact_and_index.idx] = {}
|
|
}
|
|
}
|
|
}
|
|
}
|
|
|
|
if .ACTIVE in ui.treenode(ctx, "Static TLAS") {
|
|
if sim_state.static_tlas.built {
|
|
bvh.debug_ui_bvh_node_recursive(
|
|
ctx,
|
|
sim_state.static_tlas.bvh_tree,
|
|
sim_state.static_tlas.level_geom_aabbs,
|
|
0,
|
|
)
|
|
}
|
|
}
|
|
|
|
if .ACTIVE in ui.treenode(ctx, "Dynamic TLAS") {
|
|
if sim_state.dynamic_tlas.built {
|
|
bvh.debug_ui_bvh_node_recursive(
|
|
ctx,
|
|
sim_state.dynamic_tlas.bvh_tree,
|
|
sim_state.dynamic_tlas.body_aabbs,
|
|
0,
|
|
)
|
|
}
|
|
}
|
|
|
|
// active_wheels := []int{0, 1}
|
|
|
|
// w, h: i32 = 500, 500
|
|
|
|
// window_x: i32 = 0
|
|
|
|
// for i in 0 ..< len(sim_state.suspension_constraints_slice) {
|
|
// s := &sim_state.suspension_constraints_slice[i]
|
|
|
|
// if s.alive {
|
|
// for idx in active_wheels {
|
|
// if i == idx {
|
|
// if ui.window(
|
|
// ctx,
|
|
// fmt.tprintf("Wheel %v", i),
|
|
// ui.Rect{x = window_x, y = 0, w = w, h = h},
|
|
// ui.Options{},
|
|
// ) {
|
|
// NUM_SAMPLES :: 100
|
|
|
|
// dt := f32(config.timestep) / f32(config.substreps_minus_one + 1)
|
|
// inv_dt := 1.0 / dt
|
|
|
|
// {
|
|
// ui.layout_row(ctx, {-1}, 300)
|
|
// {
|
|
// ui.begin_line(ctx, ui.Color{255, 0, 0, 255})
|
|
// defer ui.end_line(ctx)
|
|
|
|
// for j in 0 ..< NUM_SAMPLES {
|
|
// alpha := f32(j) / f32(NUM_SAMPLES - 1)
|
|
// x := alpha * 200.0 - 100.0
|
|
|
|
// long_friction := abs(
|
|
// pacejka_94_longitudinal(
|
|
// s.pacejka_long,
|
|
// x,
|
|
// max(abs(s.spring_impulse), 0.001) * inv_dt * 0.001,
|
|
// ),
|
|
// )
|
|
|
|
// ui.push_line_point(
|
|
// ctx,
|
|
// ui.Vec2f{alpha, long_friction * -0.5 + 1},
|
|
// )
|
|
// }
|
|
|
|
// long_friction := abs(
|
|
// pacejka_94_longitudinal(
|
|
// s.pacejka_long,
|
|
// s.slip_ratio,
|
|
// max(abs(s.spring_impulse), 0.001) * inv_dt * 0.001,
|
|
// ),
|
|
// )
|
|
|
|
// rect := ui.get_line(ctx).rect
|
|
|
|
// cur_point :=
|
|
// Vec2 {
|
|
// (s.slip_ratio + 100.0) / 200.0,
|
|
// long_friction * -0.5 + 1,
|
|
// } *
|
|
// Vec2{f32(rect.w), f32(rect.h)} +
|
|
// Vec2{f32(rect.x), f32(rect.y)}
|
|
// ui.draw_rect(
|
|
// ctx,
|
|
// ui.rect_from_point_extent(
|
|
// ui.Vec2{i32(cur_point.x), i32(cur_point.y)},
|
|
// 2,
|
|
// ),
|
|
// ui.Color{255, 255, 0, 255},
|
|
// )
|
|
// }
|
|
// }
|
|
|
|
// {
|
|
// ui.layout_row(ctx, {-1}, 300)
|
|
// ui.begin_line(ctx, ui.Color{0, 255, 0, 255})
|
|
// defer ui.end_line(ctx)
|
|
|
|
// for j in 0 ..< NUM_SAMPLES {
|
|
// alpha := f32(j) / f32(NUM_SAMPLES - 1)
|
|
// x := alpha * 180.0 - 90.0
|
|
|
|
// lat_friction := abs(
|
|
// pacejka_94_lateral(
|
|
// s.pacejka_lat,
|
|
// x,
|
|
// max(abs(s.spring_impulse), 0.001) * inv_dt * 0.001,
|
|
// 0.0,
|
|
// ),
|
|
// )
|
|
|
|
// ui.push_line_point(ctx, ui.Vec2f{alpha, lat_friction * -0.5 + 1})
|
|
// }
|
|
|
|
// lat_friction := abs(
|
|
// pacejka_94_lateral(
|
|
// s.pacejka_lat,
|
|
// s.slip_angle,
|
|
// max(abs(s.spring_impulse), 0.001) * inv_dt * 0.001,
|
|
// 0.0,
|
|
// ),
|
|
// )
|
|
|
|
// rect := ui.get_line(ctx).rect
|
|
|
|
// cur_point :=
|
|
// Vec2{(s.slip_angle + 100.0) / 200.0, lat_friction * -0.5 + 1} *
|
|
// Vec2{f32(rect.w), f32(rect.h)} +
|
|
// Vec2{f32(rect.x), f32(rect.y)}
|
|
// ui.draw_rect(
|
|
// ctx,
|
|
// ui.rect_from_point_extent(
|
|
// ui.Vec2{i32(cur_point.x), i32(cur_point.y)},
|
|
// 2,
|
|
// ),
|
|
// ui.Color{255, 255, 0, 255},
|
|
// )
|
|
// }
|
|
|
|
// window_x += w
|
|
// }
|
|
// }
|
|
// }
|
|
// }
|
|
// }
|
|
}
|
|
|
|
debug_transform_points_local_to_world :: proc(body: Body_Ptr, points: []Vec3) {
|
|
for i in 0 ..< len(points) {
|
|
points[i] = body_local_to_world(body, points[i])
|
|
}
|
|
}
|
|
|
|
debug_draw_manifold_points :: proc(
|
|
contact: Contact,
|
|
impulse_sign: f32,
|
|
points: []Vec3,
|
|
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 point_idx in 0 ..< len(points) {
|
|
p := points[point_idx]
|
|
total_impulse :=
|
|
contact.total_normal_impulse[point_idx] * contact.manifold.normal +
|
|
contact.total_friction_impulse[point_idx].x * contact.manifold.tangent +
|
|
contact.total_friction_impulse[point_idx].y * contact.manifold.bitangent
|
|
rl.DrawLine3D(p, p + total_impulse * impulse_sign, color)
|
|
|
|
rl.DrawSphereWires(p, 0.1, 4, 4, color)
|
|
}
|
|
}
|