Created
April 28, 2023 14:59
-
-
Save KipJM/a0561a64e35cecb99b839fb570c746b8 to your computer and use it in GitHub Desktop.
A simple GJK mesh intersection checker for Unity
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
using System; | |
using System.Collections.Generic; | |
using System.Diagnostics.CodeAnalysis; | |
using System.IO; | |
using System.Linq; | |
using UnityEngine; | |
namespace Destruct.Core.Connectivity | |
{ | |
class Simplex | |
{ | |
public List<Vector3?> points = new List<Vector3?>(5) { null, null, null, null }; | |
public void AddToFront(Vector3 point) | |
{ | |
points.Insert(0, point); | |
if (points.Count >= 5) | |
{ | |
points.RemoveAt(4); | |
} | |
} | |
public int GetSize() | |
{ | |
return points.Count(s => s != null); | |
} | |
} | |
/// <summary> | |
/// Check if two world-space meshes intersect | |
/// Done using a simple Unity implementation of GJK | |
/// From https://blog.winter.dev/2020/gjk-algorithm/ | |
/// Referenced https://youtube.com/watch?v=ajv46BSqcK4 | |
/// </summary> | |
public static class IntersectionCheck | |
{ | |
static Vector3 FurthestPoint(Vector3[] vertices, Vector3 direction) | |
{ | |
Vector3 maxVertex = Vector3.zero; | |
float maxDist = Mathf.NegativeInfinity; | |
foreach (var vertex in vertices) | |
{ | |
float distance = Vector3.Dot(vertex, direction); | |
if (distance > maxDist) | |
{ | |
maxDist = distance; | |
maxVertex = vertex; | |
} | |
} | |
return maxVertex; | |
} | |
static Vector3 GetSupportPoint(Vector3[] vertsA, Vector3[] vertsB, Vector3 direction) | |
{ | |
return FurthestPoint(vertsA, direction) - FurthestPoint(vertsB, -direction); | |
} | |
static bool IsSameDirection(Vector3 direction, Vector3 ao) | |
{ | |
// ao: Vector pointing from a to origin (o) | |
return Vector3.Dot(direction, ao) > 0; | |
} | |
// Notice: This uses WORLD-SPACE meshes. | |
// It's designed to work even without GameObjects and Transforms, so you need to convert the verts to world-space | |
// your self using Transform.TransformPoint first. | |
// It only uses the vertex data | |
public static bool CheckIntersection(Mesh meshA, Mesh meshB) | |
{ | |
Vector3[] vertsA = meshA.vertices; | |
Vector3[] vertsB = meshB.vertices; | |
// Starting point | |
// TODO: Better starting point | |
Vector3 supportPoint = GetSupportPoint(vertsA, vertsB, Vector3.forward); | |
// We're trying to find a simplex that includes the origin. If so, then the difference includes the origin | |
Simplex simplex = new Simplex(); | |
simplex.AddToFront(supportPoint); | |
// If the simplex includes the origin (0,0,0) Then the objs are intersecting | |
// So we're trying to find the points that's most likely to include the origin | |
// So we're searching first pointed TOWARDS the origin | |
// x0 -> o | x0 - o -> x1 | |
Vector3 direction = Vector3.zero - supportPoint; | |
// Theoretically, this should always return after a while. Otherwise it will hang Unity :( | |
while (true) | |
{ | |
supportPoint = GetSupportPoint(vertsA, vertsB, direction); | |
// If the new dot did not pass the origin, then the simplex definitely doesn't include the origin | |
if (Vector3.Dot(supportPoint, direction) <= 0) | |
{ | |
return false; | |
} | |
simplex.AddToFront(supportPoint); | |
if (GetNextSimplex(ref simplex, ref direction)) | |
{ | |
return true; | |
} | |
} | |
} | |
static bool GetNextSimplex(ref Simplex simplex, ref Vector3 direction) | |
{ | |
switch (simplex.GetSize()) | |
{ | |
case 2: | |
return CalculateLine(ref simplex, ref direction); | |
case 3: | |
return CalculateTriangle(ref simplex, ref direction); | |
case 4: | |
return CalculateTetrahedron(ref simplex, ref direction); | |
// Failsafe. Should never be reached | |
default: | |
throw new InvalidDataException(); | |
} | |
} | |
[SuppressMessage("ReSharper", "PossibleInvalidOperationException")] | |
private static bool CalculateLine(ref Simplex simplex, ref Vector3 direction) | |
{ | |
Vector3 a = (Vector3)simplex.points[0]; | |
Vector3 b = (Vector3)simplex.points[1]; | |
Vector3 ab = b - a; | |
Vector3 ao = Vector3.zero - a; | |
if (IsSameDirection(ab, ao)) | |
{ | |
direction = Vector3.Cross(Vector3.Cross(ab, ao), ab); | |
} | |
else | |
{ | |
simplex.points = new List<Vector3?>() { a }; | |
} | |
return false; | |
} | |
[SuppressMessage("ReSharper", "PossibleInvalidOperationException")] | |
private static bool CalculateTriangle(ref Simplex simplex, ref Vector3 direction) | |
{ | |
Vector3 a = (Vector3)simplex.points[0]; | |
Vector3 b = (Vector3)simplex.points[1]; | |
Vector3 c = (Vector3)simplex.points[2]; | |
Vector3 ab = b - a; | |
Vector3 ac = c - a; | |
Vector3 ao = Vector3.zero - a; | |
Vector3 abc = Vector3.Cross(ab, ac); | |
if (IsSameDirection(Vector3.Cross(abc, ac), ao)) | |
{ | |
if (IsSameDirection(ac, ao)) | |
{ | |
simplex.points = new List<Vector3?>() { a, c }; | |
direction = Vector3.Cross(Vector3.Cross(ac, ao), ac); | |
} | |
else | |
{ | |
simplex.points = new List<Vector3?>() { a, b }; | |
return CalculateLine(ref simplex, ref direction); | |
} | |
} | |
else | |
{ | |
if (IsSameDirection(Vector3.Cross(ab, abc), ao)) | |
{ | |
simplex.points = new List<Vector3?>() { a, b }; | |
return CalculateLine(ref simplex, ref direction); | |
} | |
else | |
{ | |
if (IsSameDirection(abc, ao)) | |
{ | |
direction = abc; | |
} | |
else | |
{ | |
simplex.points = new List<Vector3?>() { a, c, b }; | |
direction = -abc; | |
} | |
} | |
} | |
return false; | |
} | |
[SuppressMessage("ReSharper", "PossibleInvalidOperationException")] | |
private static bool CalculateTetrahedron(ref Simplex simplex, ref Vector3 direction) | |
{ | |
Vector3 a = (Vector3)simplex.points[0]; | |
Vector3 b = (Vector3)simplex.points[1]; | |
Vector3 c = (Vector3)simplex.points[2]; | |
Vector3 d = (Vector3)simplex.points[3]; | |
Vector3 ab = b - a; | |
Vector3 ac = c - a; | |
Vector3 ad = d - a; | |
Vector3 ao = Vector3.zero - a; | |
Vector3 abc = Vector3.Cross(ab, ac); | |
Vector3 acd = Vector3.Cross(ac, ad); | |
Vector3 adb = Vector3.Cross(ad, ab); | |
if (IsSameDirection(abc, ao)) | |
{ | |
simplex.points = new List<Vector3?>() { a, b, c }; | |
return CalculateTriangle(ref simplex, ref direction); | |
} | |
if (IsSameDirection(acd, ao)) | |
{ | |
simplex.points = new List<Vector3?>() { a, c, d }; | |
return CalculateTriangle(ref simplex, ref direction); | |
} | |
if (IsSameDirection(adb, ao)) | |
{ | |
simplex.points = new List<Vector3?>() { a, d, b }; | |
return CalculateTriangle(ref simplex, ref direction); | |
} | |
return true; | |
} | |
} | |
} |
Sign up for free
to join this conversation on GitHub.
Already have an account?
Sign in to comment
Performs reasonably well, https://blog.winter.dev/2020/gjk-algorithm/ ported into C#/Unity.