An implementation of the GJK algorithm for finding intersections between convex shapes. As described by Casey Muntori (http://mollyrocket.com/849) and refined by Phill Djonov (http://vec3.ca/gjk/implementation/)

gjk.cpp

typedef Vec3 (*GjkSupportFunc)(Vec3 searchDirection, void* shape1, void* shape2);

struct Simplex {
  Vec3 b, c, d; // a is always the latest point added, as the others get pushed down the stack.
  int pointCount;
  Vec3 searchDirection;
};

local_function Vec3 crossSelf(Vec3 a, Vec3 b) {
  return a ^ b ^ a;
}

local_function bool sameDirection(Vec3 ao, Vec3 ab) {
  return ab * ao > 0.0f;
}

local_function void doTriangleSimplex(Simplex* simplex, Vec3 a) {
  Vec3& b = simplex->b;
  Vec3& c = simplex->c;

  auto ao = -a;
  auto ab = b-a;
  auto ac = c-a;

  auto abc = ab^ac;

  // Normal of edge ab, lying on plane of triangle.
  auto abn = ab^abc;

  if (sameDirection(abn, ao)) {
    // Origin lies outside triangle near ab.
    simplex->c = b;
    simplex->b = a;
    simplex->searchDirection = crossSelf(ab, ao);
    return;
  }

  // Same test for ac edge.
  auto acn = abc^ac;

  if (sameDirection(acn, ao)) {
    simplex->b = a;
    simplex->searchDirection = crossSelf(ac, ao);
    return;
  }

  // If we get here the origin is within the triangle. Check if it's above or
  // below.
  if (sameDirection(abc, ao)) {
    simplex->d = c;
    simplex->c = b;
    simplex->b = a;
    simplex->searchDirection = abc;
  } else {
    simplex->d = b;
    simplex->b = a;
    simplex->searchDirection = -abc;
  }

  simplex->pointCount = 3;
}

local_function bool doTetrahedronSimplex(Simplex* simplex, Vec3 a) {
  Vec3& b = simplex->b;
  Vec3& c = simplex->c;
  Vec3& d = simplex->d;
  Vec3& v = simplex->searchDirection;
  int& n = simplex->pointCount;

  auto ao = -a;

  auto ab = b-a;
  auto ac = c-a;

  auto abc = ab^ac;

  if (sameDirection(abc, ao)) {
    // In front of triangle ABC
    goto check_face;
  }

  auto ad = d-a;
  auto acd = ac^ad;

  if (sameDirection(acd, ao)) {
    // In front of triangle ACD
    b = c;
    c = d;

    ab = ac;
    ac = ad;

    abc = acd;

    goto check_face;
  }

  auto adb = ad^ab;

  if (sameDirection(adb, ao)) {
    // In front of triangle ADB
    c = b;
    b = d;

    ac = ab;
    ab = ad;

    abc = adb;

    goto check_face;
  }

  // Behind all three faces means origin is inside
  return true;

  check_face:

  auto abn = ab^abc;

  if (sameDirection(abn, ao)) {
    c = b;
    b = a;
    v = crossSelf(ab, ao);
    n = 2;
    return false;
  }

  auto acn = abc^ac;

  if (sameDirection(acn, ao)) {
    b = a;
    v = crossSelf(ac, ao);
    n = 2;
    return false;
  }

  d = c;
  c = b;
  b = a;

  v = abc;

  n = 3;

  return false;
}

local_function bool doSimplex(Simplex* simplex, Vec3 a) {
  if (simplex->pointCount == 2) {
    doTriangleSimplex(simplex, a);
    return false;
  }
  else if (simplex->pointCount == 3) {
    return doTetrahedronSimplex(simplex, a);
  }

  SHA_ASSERT(false);
  return true;
}

bool gjkSolve(GjkSupportFunc support, void* shape1, void* shape2) {
  Simplex simplex = {};

  simplex.searchDirection = unitX();
  simplex.c = support(simplex.searchDirection, shape1, shape2);

  if (!sameDirection(simplex.c, simplex.searchDirection)) {
    return false;
  }

  // Next search back towards the origin.
  simplex.searchDirection = -simplex.searchDirection;
  simplex.b = support(simplex.searchDirection, shape1, shape2);

  if (!sameDirection(simplex.b, simplex.searchDirection)) {
    return false;
  }

  simplex.searchDirection = crossSelf(simplex.c - simplex.b, -simplex.b);
  simplex.pointCount = 2;

  for (int interation = 0; interation < 20; interation++) {
    if (isZeroVector(simplex.searchDirection)) {
      // We went straight through the origin, so the cross product failed
      // to give us a new vector.
      return true;
    }

    auto point = support(simplex.searchDirection, shape1, shape2);

    if (!sameDirection(point, simplex.searchDirection)) {
      // The new point wasn't on the far side of the origin. No intersection.
      return false;
    }

    if (doSimplex(&simplex, point)) {
      return true;
    }
  }

  return true;
}

supports.cpp

Vec3 unitSphereSupport(Vec3 searchDirection, void* shape1, void* shape2) {
  return searchDirection;
}

Vec3 offsetUnitSphereSupport(Vec3 searchDirection, void* shape1, void* shape2) {
  auto o1 = (Vec3*)shape1;
  auto o2 = (Vec3*)shape2;

  auto v = Normalize(searchDirection);

  auto p1 = *o1 + v;
  auto p2 = *o2 - v;

  return p1 - p2;
}