577 lines
14 KiB
Odin
577 lines
14 KiB
Odin
package collision
|
|
|
|
import "core:container/bit_array"
|
|
import "core:log"
|
|
import "core:math"
|
|
import lg "core:math/linalg"
|
|
import "game:halfedge"
|
|
import "libs:tracy"
|
|
import rl "vendor:raylib"
|
|
import "vendor:raylib/rlgl"
|
|
|
|
_ :: math
|
|
_ :: rl
|
|
_ :: rlgl
|
|
_ :: log
|
|
|
|
Convex :: halfedge.Half_Edge_Mesh
|
|
|
|
BOX_CORNERS_NORM :: [8]Vec3 {
|
|
{-1, 1, 1},
|
|
{-1, -1, 1},
|
|
{-1, 1, -1},
|
|
{-1, -1, -1},
|
|
{1, 1, 1},
|
|
{1, -1, 1},
|
|
{1, 1, -1},
|
|
{1, -1, -1},
|
|
}
|
|
|
|
box_indices := [6 * 4]u16{0, 4, 6, 2, 3, 2, 6, 7, 7, 6, 4, 5, 5, 1, 3, 7, 1, 0, 2, 3, 5, 4, 0, 1}
|
|
|
|
box_to_convex :: proc(box: Box, allocator := context.allocator) -> (convex: Convex) {
|
|
vertices := make([]Vec3, 8, context.temp_allocator)
|
|
|
|
for corner, i in BOX_CORNERS_NORM {
|
|
vertices[i] = box.pos + corner * box.rad
|
|
}
|
|
|
|
convex = Convex(halfedge.mesh_from_vertex_index_list(vertices, box_indices[:], 4, allocator))
|
|
|
|
return
|
|
}
|
|
|
|
Contact_Type_Face :: struct {
|
|
face_idx_a: halfedge.Face_Index,
|
|
face_idx_b: halfedge.Face_Index,
|
|
}
|
|
|
|
Contact_Type_Edge :: struct {
|
|
edge_idx_a: halfedge.Edge_Index,
|
|
edge_idx_b: halfedge.Edge_Index,
|
|
}
|
|
|
|
Contact_Type :: union {
|
|
Contact_Type_Face,
|
|
Contact_Type_Edge,
|
|
}
|
|
|
|
Contact_Manifold :: struct {
|
|
type: Contact_Type,
|
|
normal: Vec3,
|
|
tangent: Vec3,
|
|
bitangent: Vec3,
|
|
separation: f32,
|
|
points_a: [4]Vec3,
|
|
points_b: [4]Vec3,
|
|
points_len: int,
|
|
}
|
|
|
|
convex_vs_convex_sat :: proc(a, b: Convex) -> (manifold: Contact_Manifold, collision: bool) {
|
|
tracy.Zone()
|
|
|
|
face_query_a := query_separation_face_directions(a, b)
|
|
if face_query_a.separation > 0 {
|
|
return
|
|
}
|
|
face_query_b := query_separation_face_directions(b, a)
|
|
if face_query_b.separation > 0 {
|
|
return
|
|
}
|
|
|
|
edge_separation, edge_a, edge_b, edge_separating_plane := query_separation_edges(a, b)
|
|
_, _ = edge_a, edge_b
|
|
if edge_separation > 0 {
|
|
return
|
|
}
|
|
biased_face_a_separation := face_query_a.separation + 0.3
|
|
biased_face_b_separation := face_query_b.separation + 0.2
|
|
biased_edge_separation := edge_separation
|
|
|
|
is_face_a_contact := biased_face_a_separation >= biased_edge_separation
|
|
is_face_b_contact := biased_face_b_separation >= biased_edge_separation
|
|
|
|
if is_face_a_contact || is_face_b_contact {
|
|
manifold = create_face_contact_manifold(face_query_a, a, face_query_b, b)
|
|
} else {
|
|
manifold = create_edge_contact_manifold(
|
|
a,
|
|
b,
|
|
edge_separating_plane,
|
|
edge_separation,
|
|
edge_a,
|
|
edge_b,
|
|
)
|
|
}
|
|
collision = manifold.points_len > 0
|
|
if collision {
|
|
manifold.bitangent = lg.normalize0(lg.cross(manifold.tangent, manifold.normal))
|
|
}
|
|
|
|
return
|
|
}
|
|
|
|
Face_Query :: struct {
|
|
separation: f32,
|
|
face: halfedge.Face_Index,
|
|
}
|
|
|
|
query_separation_face_directions :: proc(a, b: Convex) -> (result: Face_Query) {
|
|
tracy.Zone()
|
|
result.separation = min(f32)
|
|
for face, f in a.faces {
|
|
index := a.edges[face.edge].origin
|
|
|
|
pos := a.vertices[index].pos
|
|
normal := face.normal
|
|
|
|
support_point, _, _ := find_support_point(b, -normal)
|
|
|
|
plane := plane_from_point_normal(pos, normal)
|
|
|
|
distance := signed_distance_plane(support_point.pos, plane)
|
|
if distance > result.separation {
|
|
result.face = halfedge.Face_Index(f)
|
|
result.separation = distance
|
|
}
|
|
}
|
|
|
|
return
|
|
}
|
|
|
|
find_support_point_from_slice :: proc(points: []Vec3, normal: Vec3) -> (p: Vec3) {
|
|
max_proj := min(f32)
|
|
for vert in points {
|
|
proj := lg.dot(vert, normal)
|
|
if proj > max_proj {
|
|
max_proj = proj
|
|
p = vert
|
|
}
|
|
}
|
|
|
|
return
|
|
}
|
|
|
|
find_support_point :: proc(
|
|
convex: Convex,
|
|
normal: Vec3,
|
|
) -> (
|
|
vert: halfedge.Vertex,
|
|
idx: halfedge.Vertex_Index,
|
|
ok: bool,
|
|
) {
|
|
max_proj := min(f32)
|
|
for v, i in convex.vertices {
|
|
proj := lg.dot(v.pos, normal)
|
|
if proj > max_proj {
|
|
max_proj = proj
|
|
vert = v
|
|
idx = halfedge.Vertex_Index(i)
|
|
ok = true
|
|
}
|
|
}
|
|
|
|
return
|
|
}
|
|
|
|
find_furthest_point_from_point :: proc(points: []Vec3, ref: Vec3) -> (p: Vec3) {
|
|
max_dist_sq := min(f32)
|
|
for v in points {
|
|
dist_sq := lg.length2(v - ref)
|
|
if dist_sq > max_dist_sq {
|
|
p = v
|
|
max_dist_sq = dist_sq
|
|
}
|
|
}
|
|
return
|
|
}
|
|
|
|
// See: Physics for Game Programmers: The Separating Axis Test between Convex Polyhedra
|
|
// https://gdcvault.com/play/1017646/Physics-for-Game-Programmers-The
|
|
is_minkowski_face :: #force_inline proc(bxa, dxc, a, b, c, d: Vec3) -> bool {
|
|
cba := lg.dot(c, bxa)
|
|
dba := lg.dot(d, bxa)
|
|
adc := lg.dot(a, dxc)
|
|
bdc := lg.dot(b, dxc)
|
|
|
|
return cba * dba < 0 && adc * bdc < 0 && cba * bdc > 0
|
|
}
|
|
|
|
build_minkowski_face :: #force_inline proc(
|
|
mesh_a, mesh_b: Convex,
|
|
edge_a, edge_b: halfedge.Half_Edge,
|
|
) -> bool {
|
|
a := mesh_a.faces[edge_a.face].normal
|
|
b := mesh_a.faces[mesh_a.edges[edge_a.twin].face].normal
|
|
c := mesh_b.faces[edge_b.face].normal
|
|
d := mesh_b.faces[mesh_b.edges[edge_b.twin].face].normal
|
|
|
|
bxa := halfedge.get_edge_direction(mesh_a, edge_a)
|
|
dxc := halfedge.get_edge_direction(mesh_b, edge_b)
|
|
|
|
// Negate normals c and d because we're interested in minkowski difference instead of sum
|
|
return is_minkowski_face(bxa, dxc, a, b, -c, -d)
|
|
}
|
|
|
|
query_separation_edges :: proc(
|
|
a, b: Convex,
|
|
) -> (
|
|
separation: f32,
|
|
a_edge: halfedge.Edge_Index,
|
|
b_edge: halfedge.Edge_Index,
|
|
separating_plane: Plane,
|
|
) {
|
|
tracy.Zone()
|
|
|
|
separation = min(f32)
|
|
a_edge = -1
|
|
b_edge = -1
|
|
|
|
checked_pairs: bit_array.Bit_Array
|
|
bit_array.init(&checked_pairs, len(a.edges) * len(b.edges), 0, context.temp_allocator)
|
|
a_len := len(a.edges)
|
|
|
|
calc_pair_index :: proc(a, b, a_len: int) -> int {
|
|
return (b * a_len) + a
|
|
}
|
|
|
|
for edge_a, edge_a_idx in a.edges {
|
|
for edge_b, edge_b_idx in b.edges {
|
|
pair_idx := calc_pair_index(edge_a_idx, edge_b_idx, a_len)
|
|
if bit_array.get(&checked_pairs, pair_idx) {
|
|
continue
|
|
}
|
|
|
|
// TODO: sort edges so twins are next to each other, then can just iterate with step = 2 and skip this bitfield
|
|
bit_array.set(&checked_pairs, pair_idx)
|
|
bit_array.set(&checked_pairs, calc_pair_index(int(edge_a.twin), edge_b_idx, a_len))
|
|
bit_array.set(&checked_pairs, calc_pair_index(edge_a_idx, int(edge_b.twin), a_len))
|
|
bit_array.set(
|
|
&checked_pairs,
|
|
calc_pair_index(int(edge_a.twin), int(edge_b.twin), a_len),
|
|
)
|
|
|
|
if build_minkowski_face(a, b, edge_a, edge_b) {
|
|
edge_a1, edge_a2 := halfedge.get_edge_points(a, edge_a)
|
|
edge_b1, edge_b2 := halfedge.get_edge_points(b, edge_b)
|
|
edge_a_dir := edge_a2 - edge_a1
|
|
edge_b_dir := edge_b2 - edge_b1
|
|
axis := lg.normalize0(lg.cross(edge_a_dir, edge_b_dir))
|
|
if axis == 0 {
|
|
continue
|
|
}
|
|
if lg.dot(axis, edge_a1 - a.center) < 0 {
|
|
axis = -axis
|
|
}
|
|
|
|
plane_a := plane_from_point_normal(edge_a1, axis)
|
|
|
|
distance := signed_distance_plane(edge_b1, plane_a)
|
|
if distance > separation {
|
|
separation = distance
|
|
a_edge = halfedge.Edge_Index(edge_a_idx)
|
|
b_edge = halfedge.Edge_Index(edge_b_idx)
|
|
separating_plane = plane_a
|
|
}
|
|
}
|
|
}
|
|
}
|
|
|
|
return
|
|
}
|
|
|
|
create_face_contact_manifold :: proc(
|
|
face_query_a: Face_Query,
|
|
a: Convex,
|
|
face_query_b: Face_Query,
|
|
b: Convex,
|
|
) -> (
|
|
manifold: Contact_Manifold,
|
|
) {
|
|
tracy.Zone()
|
|
|
|
is_ref_a := face_query_a.separation > face_query_b.separation
|
|
ref_face_query := is_ref_a ? face_query_a : face_query_b
|
|
ref_convex := is_ref_a ? a : b
|
|
inc_convex := is_ref_a ? b : a
|
|
|
|
ref_face_idx := ref_face_query.face
|
|
ref_face := ref_convex.faces[ref_face_idx]
|
|
|
|
// incident face
|
|
inc_face: halfedge.Face
|
|
inc_face_idx: halfedge.Face_Index
|
|
// Find the most anti parallel face
|
|
{
|
|
min_dot := f32(1.0)
|
|
for face, face_idx in inc_convex.faces {
|
|
dot := lg.dot(ref_face.normal, face.normal)
|
|
if dot < min_dot {
|
|
min_dot = dot
|
|
inc_face = face
|
|
inc_face_idx = halfedge.Face_Index(face_idx)
|
|
}
|
|
}
|
|
}
|
|
|
|
inc_polygon: []Vec3
|
|
original_vert_count := 0
|
|
|
|
// Get incident face vertices
|
|
{
|
|
it := halfedge.iterator_face_edges(halfedge.Half_Edge_Mesh(inc_convex), inc_face_idx)
|
|
|
|
for _ in halfedge.iterate_next_edge(&it) {
|
|
original_vert_count += 1
|
|
}
|
|
|
|
inc_polygon = make([]Vec3, original_vert_count * 4, context.temp_allocator)
|
|
|
|
halfedge.iterator_reset_edges(&it)
|
|
|
|
i := 0
|
|
for edge in halfedge.iterate_next_edge(&it) {
|
|
inc_polygon[i] = inc_convex.vertices[edge.origin].pos
|
|
i += 1
|
|
}
|
|
}
|
|
|
|
assert(len(inc_polygon) > 0)
|
|
|
|
// Set up ping pong buffers
|
|
inc_polygon2 := make([]Vec3, len(inc_polygon), context.temp_allocator)
|
|
|
|
// Buffer 0 is the original incident polygon, start with index 1
|
|
clip_buf_idx := 1
|
|
clip_bufs := [2][]Vec3{inc_polygon, inc_polygon2}
|
|
|
|
get_other_clip_buf :: proc(idx: int) -> int {
|
|
return (idx + 1) % 2
|
|
}
|
|
|
|
step := 0
|
|
vert_count := original_vert_count
|
|
|
|
EPS :: 1e-6
|
|
|
|
{
|
|
it := halfedge.iterator_face_edges(
|
|
halfedge.Half_Edge_Mesh(ref_convex),
|
|
ref_face_query.face,
|
|
)
|
|
|
|
for edge in halfedge.iterate_next_edge(&it) {
|
|
src_polygon := clip_bufs[get_other_clip_buf(clip_buf_idx)]
|
|
clipped_polygon := clip_bufs[clip_buf_idx]
|
|
|
|
clipping_face, clipping_face_idx, _ := halfedge.get_adjacent_face(
|
|
halfedge.Half_Edge_Mesh(ref_convex),
|
|
edge,
|
|
)
|
|
clipping_face_vert :=
|
|
ref_convex.vertices[ref_convex.edges[clipping_face.edge].origin].pos
|
|
|
|
clipping_plane_center: Vec3
|
|
{
|
|
clipping_plane_it := halfedge.iterator_face_edges(
|
|
halfedge.Half_Edge_Mesh(ref_convex),
|
|
clipping_face_idx,
|
|
)
|
|
|
|
num := 0
|
|
for clip_edge in halfedge.iterate_next_edge(&clipping_plane_it) {
|
|
vert := ref_convex.vertices[clip_edge.origin].pos
|
|
clipping_plane_center += vert
|
|
num += 1
|
|
}
|
|
|
|
clipping_plane_center /= f32(num)
|
|
}
|
|
|
|
clipping_plane := plane_from_point_normal(clipping_face_vert, clipping_face.normal)
|
|
|
|
// Actual clipping
|
|
{
|
|
j := 0
|
|
for i in 0 ..< vert_count {
|
|
k := (i + 1) % vert_count
|
|
d1 := signed_distance_plane(src_polygon[i], clipping_plane)
|
|
d2 := signed_distance_plane(src_polygon[k], clipping_plane)
|
|
|
|
if d1 < -EPS && d2 < -EPS {
|
|
// Both points inside
|
|
clipped_polygon[j] = src_polygon[k]
|
|
j += 1
|
|
} else if d1 > -EPS && d2 < -EPS {
|
|
// First point is outside
|
|
_, clipped_polygon[j], _ = intersect_segment_plane(
|
|
{src_polygon[i], src_polygon[k]},
|
|
clipping_plane,
|
|
)
|
|
j += 1
|
|
clipped_polygon[j] = src_polygon[k]
|
|
j += 1
|
|
} else if d1 < -EPS && d2 > -EPS {
|
|
// Second point outside
|
|
_, clipped_polygon[j], _ = intersect_segment_plane(
|
|
{src_polygon[i], src_polygon[k]},
|
|
clipping_plane,
|
|
)
|
|
j += 1
|
|
}
|
|
}
|
|
vert_count = j
|
|
}
|
|
|
|
clip_buf_idx = get_other_clip_buf(clip_buf_idx)
|
|
step += 1
|
|
}
|
|
}
|
|
|
|
ref_face_vert, ref_face_vert2 := halfedge.get_edge_points(
|
|
ref_convex,
|
|
ref_convex.edges[ref_face.edge],
|
|
)
|
|
ref_plane := plane_from_point_normal(ref_face_vert, ref_face.normal)
|
|
ref_tangent := lg.normalize0(ref_face_vert2 - ref_face_vert)
|
|
|
|
// Final clipping, remove verts above ref face and project those left onto the reference plane
|
|
{
|
|
src_polygon := clip_bufs[get_other_clip_buf(clip_buf_idx)]
|
|
clipped_polygon := clip_bufs[clip_buf_idx]
|
|
|
|
j := 0
|
|
for i in 0 ..< vert_count {
|
|
d := signed_distance_plane(src_polygon[i], ref_plane)
|
|
|
|
if d <= 1e-02 {
|
|
clipped_polygon[j] = src_polygon[i] - d * ref_plane.normal
|
|
j += 1
|
|
}
|
|
}
|
|
vert_count = j
|
|
clip_buf_idx = get_other_clip_buf(clip_buf_idx)
|
|
}
|
|
|
|
// Resulting polygon before contact optimization
|
|
full_clipped_polygon := clip_bufs[get_other_clip_buf(clip_buf_idx)][:vert_count]
|
|
|
|
ref_points: [4]Vec3
|
|
|
|
// Contact optimization
|
|
if len(full_clipped_polygon) > 4 {
|
|
start_dir := lg.cross(ref_plane.normal, ref_face_vert)
|
|
|
|
first_point := find_support_point_from_slice(full_clipped_polygon, start_dir)
|
|
second_point := find_furthest_point_from_point(full_clipped_polygon, first_point)
|
|
|
|
third_point: Vec3
|
|
{
|
|
max_area := min(f32)
|
|
for v in full_clipped_polygon {
|
|
area := 0.5 * lg.dot(ref_plane.normal, lg.cross(first_point - v, second_point - v))
|
|
if area > max_area {
|
|
max_area = area
|
|
third_point = v
|
|
}
|
|
}
|
|
}
|
|
|
|
existing_edges := [][2]Vec3 {
|
|
{first_point, second_point},
|
|
{second_point, third_point},
|
|
{third_point, first_point},
|
|
}
|
|
|
|
fourth_point: Vec3
|
|
{
|
|
// We're looking for max negative area actually
|
|
min_area := max(f32)
|
|
for v in full_clipped_polygon {
|
|
for edge in existing_edges {
|
|
area := 0.5 * lg.dot(ref_plane.normal, lg.cross(edge[0] - v, edge[1] - v))
|
|
if area < min_area {
|
|
min_area = area
|
|
fourth_point = v
|
|
}
|
|
}
|
|
}
|
|
}
|
|
|
|
ref_points[0] = first_point
|
|
ref_points[1] = second_point
|
|
ref_points[2] = third_point
|
|
ref_points[3] = fourth_point
|
|
manifold.points_len = 4
|
|
} else {
|
|
copy(ref_points[:], full_clipped_polygon)
|
|
manifold.points_len = len(full_clipped_polygon)
|
|
assert(len(full_clipped_polygon) <= 4)
|
|
}
|
|
|
|
inc_face_vert := inc_convex.vertices[inc_convex.edges[inc_face.edge].origin].pos
|
|
inc_plane := plane_from_point_normal(inc_face_vert, inc_face.normal)
|
|
|
|
inc_points: [4]Vec3
|
|
for p, i in ref_points[:manifold.points_len] {
|
|
inc_points[i] = project_point_on_plane(p, inc_plane)
|
|
}
|
|
|
|
if is_ref_a {
|
|
manifold.type = Contact_Type_Face {
|
|
face_idx_a = ref_face_idx,
|
|
face_idx_b = inc_face_idx,
|
|
}
|
|
manifold.points_a = ref_points
|
|
manifold.points_b = inc_points
|
|
} else {
|
|
manifold.type = Contact_Type_Face {
|
|
face_idx_a = inc_face_idx,
|
|
face_idx_b = ref_face_idx,
|
|
}
|
|
manifold.points_b = ref_points
|
|
manifold.points_a = inc_points
|
|
}
|
|
|
|
manifold.separation = ref_face_query.separation
|
|
// Normal is always pointing from a to b
|
|
manifold.normal = is_ref_a ? ref_face.normal : -ref_face.normal
|
|
manifold.tangent = ref_tangent
|
|
|
|
return
|
|
}
|
|
|
|
project_point_on_plane :: proc(point: Vec3, plane: Plane) -> Vec3 {
|
|
dist := signed_distance_plane(point, plane)
|
|
return point + plane.normal * -dist
|
|
}
|
|
|
|
create_edge_contact_manifold :: proc(
|
|
a, b: Convex,
|
|
separating_plane: Plane,
|
|
separation: f32,
|
|
edge_a, edge_b: halfedge.Edge_Index,
|
|
) -> (
|
|
manifold: Contact_Manifold,
|
|
) {
|
|
tracy.Zone()
|
|
|
|
a1, a2 := halfedge.get_edge_points(a, a.edges[edge_a])
|
|
b1, b2 := halfedge.get_edge_points(b, b.edges[edge_b])
|
|
|
|
_, ps := closest_point_between_segments(a1, a2, b1, b2)
|
|
|
|
manifold.type = Contact_Type_Edge {
|
|
edge_idx_a = edge_a,
|
|
edge_idx_b = edge_b,
|
|
}
|
|
manifold.normal = separating_plane.normal
|
|
manifold.separation = separation
|
|
manifold.points_a[0] = ps[0]
|
|
manifold.points_b[0] = ps[1]
|
|
manifold.points_len = 1
|
|
manifold.tangent = lg.normalize0(lg.cross(a2 - a1, manifold.normal))
|
|
|
|
return
|
|
}
|