365 lines
8.2 KiB
Plaintext
365 lines
8.2 KiB
Plaintext
Simplex :: struct {
|
|
points: [4] Vector3;
|
|
size: s32;
|
|
}
|
|
|
|
Edge :: struct {
|
|
i1: s64;
|
|
i2: s64;
|
|
}
|
|
|
|
Collision_Point :: struct {
|
|
normal: Vector3;
|
|
penetration_depth: float;
|
|
has_collision: bool;
|
|
}
|
|
|
|
get_face_normals :: (polytope: [] Vector3, faces: [] s64) -> [..] Vector4, s64 {
|
|
normals : [..] Vector4;
|
|
normals.allocator = temp;
|
|
|
|
min_triangle : s64 = 0;
|
|
min_distance := FLOAT32_MAX;
|
|
|
|
i := 0;
|
|
while i < faces.count {
|
|
defer i += 3;
|
|
|
|
a := polytope[faces[i+0]];
|
|
b := polytope[faces[i+1]];
|
|
c := polytope[faces[i+2]];
|
|
|
|
normal := normalize(cross(b - a, c - a));
|
|
distance := dot(normal, a);
|
|
|
|
if distance < 0 {
|
|
normal *= -1.0;
|
|
distance *= -1.0;
|
|
}
|
|
|
|
data : Vector4;
|
|
data.x = normal.x;
|
|
data.y = normal.y;
|
|
data.z = normal.z;
|
|
data.w = distance;
|
|
array_add(*normals, data);
|
|
|
|
if distance < min_distance {
|
|
min_triangle = i / 3;
|
|
min_distance = distance;
|
|
}
|
|
}
|
|
|
|
return normals, min_triangle;
|
|
}
|
|
|
|
add_if_unique_edge :: (edges: *[..] Edge, faces: [] s64, a: s64, b: s64) {
|
|
i := 0;
|
|
found := false;
|
|
while i < edges.count {
|
|
defer i += 1;
|
|
|
|
edge := edges.*[i];
|
|
if edge.i1 == faces[b] && edge.i2 == faces[a] {
|
|
array_unordered_remove_by_index(edges, i);
|
|
found = true;
|
|
break;
|
|
}
|
|
}
|
|
|
|
if !found {
|
|
array_add(edges, .{faces[a], faces[b]});
|
|
}
|
|
}
|
|
|
|
nearly_equal :: (a: float, b: float) -> bool {
|
|
return abs(a - b) < 0.00001;
|
|
}
|
|
|
|
polytope : [..] Vector3;
|
|
|
|
epa :: (simplex: Simplex, collider_a: Collider, collider_b: Collider) -> Collision_Point {
|
|
polytope.count = 0;
|
|
|
|
array_reserve(*polytope, simplex.size);
|
|
for simplex.points {
|
|
array_add(*polytope, it);
|
|
}
|
|
|
|
faces : [..] s64;
|
|
faces.allocator = temp;
|
|
array_resize(*faces, 12);
|
|
|
|
faces[0] = 0;
|
|
faces[1] = 1;
|
|
faces[2] = 2;
|
|
faces[3] = 0;
|
|
faces[4] = 3;
|
|
faces[5] = 1;
|
|
faces[6] = 0;
|
|
faces[7] = 2;
|
|
faces[8] = 3;
|
|
faces[9] = 1;
|
|
faces[10] = 3;
|
|
faces[11] = 2;
|
|
|
|
normals, min_face := get_face_normals(polytope, faces);
|
|
|
|
min_normal : Vector3;
|
|
min_distance := FLOAT32_MAX;
|
|
|
|
while nearly_equal(min_distance, FLOAT32_MAX) {
|
|
min_normal = to_v3(normals[min_face]);
|
|
min_distance = normals[min_face].w;
|
|
|
|
s := support(collider_a, collider_b, min_normal);
|
|
s_distance := dot(min_normal, s);
|
|
|
|
if abs(s_distance - min_distance) > 0.01 {
|
|
min_distance = FLOAT32_MAX;
|
|
|
|
unique_edges : [..] Edge;
|
|
unique_edges.allocator = temp;
|
|
|
|
i := 0;
|
|
while i < normals.count {
|
|
defer i += 1;
|
|
if dot(to_v3(normals[i]), s) > dot(to_v3(normals[i]), polytope[faces[i*3]]) { //same_direction(to_v3(normals[i]), s) {
|
|
f := i * 3;
|
|
add_if_unique_edge(*unique_edges, faces, f, f + 1);
|
|
add_if_unique_edge(*unique_edges, faces, f + 1, f + 2);
|
|
add_if_unique_edge(*unique_edges, faces, f + 2, f);
|
|
|
|
faces[f+2] = faces[faces.count-1];
|
|
faces[f+1] = faces[faces.count-2];
|
|
faces[f+0] = faces[faces.count-3];
|
|
|
|
faces.count -= 3;
|
|
|
|
normals[i] = normals[normals.count-1];
|
|
normals.count -= 1;
|
|
i -= 1;
|
|
}
|
|
}
|
|
|
|
new_faces : [..] s64;
|
|
new_faces.allocator = temp;
|
|
for e: unique_edges {
|
|
array_add(*new_faces, e.i1);
|
|
array_add(*new_faces, e.i2);
|
|
array_add(*new_faces, polytope.count);
|
|
}
|
|
|
|
array_add(*polytope, s);
|
|
|
|
new_normals, new_min_face := get_face_normals(polytope, new_faces);
|
|
|
|
old_min_distance := FLOAT32_MAX;
|
|
|
|
for n: normals {
|
|
if n.w < old_min_distance {
|
|
old_min_distance = n.w;
|
|
min_face = it_index;
|
|
}
|
|
}
|
|
|
|
if new_normals[new_min_face].w < old_min_distance {
|
|
min_face = new_min_face + normals.count;
|
|
}
|
|
|
|
|
|
for f: new_faces {
|
|
array_add(*faces, f);
|
|
}
|
|
|
|
for n: new_normals {
|
|
array_add(*normals, n);
|
|
}
|
|
}
|
|
}
|
|
|
|
point : Collision_Point;
|
|
point.normal = min_normal;
|
|
point.penetration_depth = min_distance + 0.001;
|
|
point.has_collision = true;
|
|
|
|
return point;
|
|
}
|
|
|
|
find_furthest_point :: (collider: Collider, direction: Vector3) -> Vector3 {
|
|
max_point : Vector3;
|
|
max_distance := -FLOAT32_MAX;
|
|
|
|
if collider.type == {
|
|
case .MESH; {
|
|
for collider.mesh.vertices {
|
|
distance := dot(it, direction);
|
|
if distance > max_distance {
|
|
max_distance = distance;
|
|
max_point = it;
|
|
}
|
|
}
|
|
}
|
|
}
|
|
|
|
return max_point;
|
|
}
|
|
|
|
push_front :: (simplex: *Simplex, p: Vector3) {
|
|
simplex.points[3] = simplex.points[2];
|
|
simplex.points[2] = simplex.points[1];
|
|
simplex.points[1] = simplex.points[0];
|
|
simplex.points[0] = p;
|
|
|
|
simplex.size = min(simplex.size + 1, 4);
|
|
}
|
|
|
|
tetrahedron :: (simplex: *Simplex, direction: *Vector3) -> bool {
|
|
a := simplex.points[0];
|
|
b := simplex.points[1];
|
|
c := simplex.points[2];
|
|
d := simplex.points[3];
|
|
|
|
ab := b - a;
|
|
ac := c - a;
|
|
ad := d - a;
|
|
ao := - a;
|
|
|
|
abc := cross(ab, ac);
|
|
acd := cross(ac, ad);
|
|
adb := cross(ad, ab);
|
|
|
|
if same_direction(abc, ao) {
|
|
simplex.points[0] = a;
|
|
simplex.points[1] = b;
|
|
simplex.points[2] = c;
|
|
simplex.size = 3;
|
|
return triangle(simplex, direction);
|
|
}
|
|
|
|
if same_direction(acd, ao) {
|
|
simplex.points[0] = a;
|
|
simplex.points[1] = c;
|
|
simplex.points[2] = d;
|
|
simplex.size = 3;
|
|
return triangle(simplex, direction);
|
|
}
|
|
|
|
if same_direction(adb, ao) {
|
|
simplex.points[0] = a;
|
|
simplex.points[1] = d;
|
|
simplex.points[2] = b;
|
|
simplex.size = 3;
|
|
return triangle(simplex, direction);
|
|
}
|
|
|
|
return true;
|
|
}
|
|
|
|
triangle :: (simplex: *Simplex, direction: *Vector3) -> bool {
|
|
a := simplex.points[0];
|
|
b := simplex.points[1];
|
|
c := simplex.points[2];
|
|
|
|
ab := b - a;
|
|
ac := c - a;
|
|
ao := - a;
|
|
|
|
abc := cross(ab, ac);
|
|
|
|
if same_direction(cross(abc, ac), ao) {
|
|
if same_direction(ac, ao) {
|
|
simplex.points[0] = a;
|
|
simplex.points[1] = c;
|
|
simplex.size = 2;
|
|
direction.* = cross(cross(ac, ao), ac);
|
|
} else {
|
|
simplex.points[0] = a;
|
|
simplex.points[1] = b;
|
|
simplex.size = 2;
|
|
return line(simplex, direction);
|
|
}
|
|
} else {
|
|
if same_direction(cross(ab, abc), ao) {
|
|
simplex.points[0] = a;
|
|
simplex.points[1] = b;
|
|
simplex.size = 2;
|
|
return line(simplex, direction);
|
|
} else {
|
|
if same_direction(abc, ao) {
|
|
direction.* = abc;
|
|
} else {
|
|
simplex.points[0] = a;
|
|
simplex.points[1] = c;
|
|
simplex.points[2] = b;
|
|
simplex.size = 3;
|
|
direction.* = -abc;
|
|
}
|
|
}
|
|
}
|
|
|
|
return false;
|
|
}
|
|
|
|
same_direction :: (direction: Vector3, ao: Vector3) -> bool {
|
|
return dot(direction, ao) > 0;
|
|
}
|
|
|
|
line :: (simplex: *Simplex, direction: *Vector3) -> bool {
|
|
a := simplex.points[0];
|
|
b := simplex.points[1];
|
|
|
|
ab := b - a;
|
|
ao := - a;
|
|
|
|
if same_direction(ab, ao) {
|
|
direction.* = cross(cross(ab, ao), ab);
|
|
} else {
|
|
simplex.points[0] = a;
|
|
simplex.size = 1;
|
|
direction.* = ao;
|
|
}
|
|
|
|
return false;
|
|
}
|
|
|
|
next_simplex :: (simplex: *Simplex, direction: *Vector3) -> bool {
|
|
if simplex.size == {
|
|
case 2; return line(simplex, direction);
|
|
case 3; return triangle(simplex, direction);
|
|
case 4; return tetrahedron(simplex, direction);
|
|
}
|
|
|
|
return false;
|
|
}
|
|
|
|
support :: (a: Collider, b: Collider, direction: Vector3) -> Vector3 {
|
|
return find_furthest_point(a, direction) - find_furthest_point(b, -direction);
|
|
}
|
|
|
|
gjk :: (a: Collider, b: Collider) -> Collision_Point {
|
|
sup := support(a, b, .{1,0,0});
|
|
|
|
points : Simplex;
|
|
push_front(*points, sup);
|
|
|
|
direction := -sup;
|
|
|
|
while true {
|
|
sup = support(a, b, direction);
|
|
|
|
if dot(sup, direction) <= 0 {
|
|
return .{};
|
|
}
|
|
|
|
push_front(*points, sup);
|
|
|
|
if next_simplex(*points, *direction) {
|
|
collision_point := epa(points, a, b);
|
|
return collision_point;
|
|
}
|
|
}
|
|
|
|
return .{};
|
|
}
|