Skip to content

Instantly share code, notes, and snippets.

@KipJM
Created April 28, 2023 14:59
Show Gist options
  • Save KipJM/a0561a64e35cecb99b839fb570c746b8 to your computer and use it in GitHub Desktop.
Save KipJM/a0561a64e35cecb99b839fb570c746b8 to your computer and use it in GitHub Desktop.
A simple GJK mesh intersection checker for Unity
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;
}
}
}
@KipJM
Copy link
Author

KipJM commented Apr 28, 2023

Performs reasonably well, https://blog.winter.dev/2020/gjk-algorithm/ ported into C#/Unity.

Sign up for free to join this conversation on GitHub. Already have an account? Sign in to comment