Thank you very much, I'll look into it later.
Unfortunately I have to get the full GJK algorithm working before moving on to that. This has proven more difficult than I thought. I am using a couple of sources (including some old code of yours, Randy); I thought I had everything down correctly, but the calculated witness points are just entirely wrong most of the time. I have been bashing my head into the monitor to see where I screwed up, and I just cannot see it. This has been driving me mad for the last couple of days, so if someone could take a quick (well, as quick as your can given the code's length) glance at my code and see where I made an error, I would be quite pleased.
Thank you for all your help, I really do appreciate it.
Resource 1: https://gist.github.com/vurtun/29727217c269a2fbf4c0ed9a1d11cb40#file-gjk-c
Resource 2: https://bitbucket.org/rgaul/lm/src/806fe4427db32f98e514c646b51f99ce1cfc5c14/src/lm/collision/query/lmDistance.cpp?at=default&fileviewer=file-view-default
My Code (Again, apologies for the length, but GJK is very wordy):
#include <array>
#include "GJKSolver.h"
struct GJKSimplexStruct {
std::array<glm::vec3, 4> verts{ glm::vec3(0.0f) };
std::array<size_t, 4> indicesA{ 0 };
std::array<size_t, 4> indicesB{ 0 };
std::array<float, 4> barycentricValues{ 1.0f };
size_t size = 0;
glm::vec3 witnessA;
glm::vec3 witnessB;
float distance = std::numeric_limits<float>::max();
};
GJKSolver::GJKSolver() {
}
GJKSimplexStruct GJKSolver::solveDistance(glm::vec3* vertsA, glm::vec3* vertsB, size_t sizeA, size_t sizeB) {
GJKSimplexStruct simplex;
float GJK_EPSILON = std::numeric_limits<float>::epsilon();
glm::vec3 searchDirection = glm::vec3(1.0f, 0.0f, 0.0f);
//get our first simplex point
size_t indexA = supportIndex(searchDirection, vertsA, sizeA);
size_t indexB = supportIndex(-searchDirection, vertsB, sizeB);
glm::vec3 newPoint = vertsA[indexA] - vertsB[indexB];
//add first vertex to simplex
simplex.indicesA[simplex.size] = indexA;
simplex.indicesB[simplex.size] = indexB;
simplex.barycentricValues[simplex.size] = 1.0f;
simplex.verts[simplex.size] = newPoint;
simplex.size++;
float sqDistance = glm::dot(newPoint, newPoint);
//16 is my arbitrary limit on the number of iterations
for (int i = 0; i < 16; i++) {
GJKSimplexStruct previousSimplex = simplex;
//find the closest region on the simplex to the origin and remove
//vertices that do not contribute
switch (simplex.size) {
//POINT CASE
case 1:
//only one point, clearly the point itself is closest
break;
//LINE CASE
case 2: {
glm::vec3 ab = simplex.verts[0] - simplex.verts[1];
glm::vec3 ba = simplex.verts[1] - simplex.verts[0];
float u = glm::dot(simplex.verts[1], ba);
float v = glm::dot(simplex.verts[0], ab);
if (v <= 0.0f) {
//Region A
simplex.barycentricValues[0] = 1.0f;
simplex.size = 1;
break;
}
if (u <= 0.0f) {
//Region B
simplex.verts[0] = simplex.verts[1];
simplex.barycentricValues[0] = 1.0f;
simplex.size = 1;
break;
}
//Region AB
simplex.barycentricValues[0] = u;
simplex.barycentricValues[1] = v;
simplex.size = 2;
break;
}
//TRIANGLE CASE
case 3: {
glm::vec3 ab = simplex.verts[0] - simplex.verts[1];
glm::vec3 ba = simplex.verts[1] - simplex.verts[0];
glm::vec3 bc = simplex.verts[1] - simplex.verts[2];
glm::vec3 cb = simplex.verts[2] - simplex.verts[1];
glm::vec3 ca = simplex.verts[2] - simplex.verts[0];
glm::vec3 ac = simplex.verts[0] - simplex.verts[2];
float u_ab = glm::dot(simplex.verts[1], ba);
float v_ab = glm::dot(simplex.verts[0], ab);
float u_bc = glm::dot(simplex.verts[2], cb);
float v_bc = glm::dot(simplex.verts[1], bc);
float u_ca = glm::dot(simplex.verts[0], ac);
float v_ca = glm::dot(simplex.verts[2], ca);
if (v_ab <= 0.0f && u_ca <= 0.0f) {
//Region A
simplex.barycentricValues[0] = 1.0f;
simplex.size = 1;
break;
}
if (u_ab <= 0.0f && v_bc <= 0.0f) {
//Region B
simplex.verts[0] = simplex.verts[1];
simplex.barycentricValues[0] = 1.0f;
simplex.size = 1;
break;
}
if (u_bc <= 0.0f && v_ca <= 0.0f) {
//Region C
simplex.verts[0] = simplex.verts[2];
simplex.barycentricValues[0] = 1.0f;
simplex.size = 1;
break;
}
//calculate fractional area
glm::vec3 n = glm::cross(ba, ca);
float u_abc = glm::dot(glm::cross(simplex.verts[1], simplex.verts[2]), n);
float v_abc = glm::dot(glm::cross(simplex.verts[2], simplex.verts[0]), n);
float w_abc = glm::dot(glm::cross(simplex.verts[0], simplex.verts[1]), n);
if (u_ab > 0.0f && v_ab > 0.0f && w_abc <= 0.0f) {
//Region AB
simplex.barycentricValues[0] = u_ab;
simplex.barycentricValues[1] = v_ab;
simplex.size = 2;
break;
}
if (u_bc > 0.0f && v_bc > 0.0f && u_abc <= 0.0f) {
//Region BC
simplex.verts[0] = simplex.verts[1];
simplex.verts[1] = simplex.verts[2];
simplex.barycentricValues[0] = u_bc;
simplex.barycentricValues[1] = v_bc;
simplex.size = 2;
break;
}
if (u_ca > 0.0f && v_ca > 0.0f && v_abc <= 0.0f) {
//Region CA
simplex.verts[1] = simplex.verts[0];
simplex.verts[0] = simplex.verts[2];
simplex.barycentricValues[0] = u_ca;
simplex.barycentricValues[1] = v_ca;
simplex.size = 2;
break;
}
//Region ABC
simplex.barycentricValues[0] = u_abc;
simplex.barycentricValues[1] = v_abc;
simplex.barycentricValues[2] = w_abc;
simplex.size = 3;
break;
}
//TETRAHEDRON CASE
case 4: {
glm::vec3 ab = simplex.verts[0] - simplex.verts[1];
glm::vec3 ba = simplex.verts[1] - simplex.verts[0];
glm::vec3 bc = simplex.verts[1] - simplex.verts[2];
glm::vec3 cb = simplex.verts[2] - simplex.verts[1];
glm::vec3 ca = simplex.verts[2] - simplex.verts[0];
glm::vec3 ac = simplex.verts[0] - simplex.verts[2];
glm::vec3 db = simplex.verts[3] - simplex.verts[1];
glm::vec3 bd = simplex.verts[1] - simplex.verts[3];
glm::vec3 dc = simplex.verts[3] - simplex.verts[2];
glm::vec3 cd = simplex.verts[2] - simplex.verts[3];
glm::vec3 da = simplex.verts[3] - simplex.verts[0];
glm::vec3 ad = simplex.verts[0] - simplex.verts[3];
float u_ab = glm::dot(simplex.verts[1], ba);
float v_ab = glm::dot(simplex.verts[0], ab);
float u_bc = glm::dot(simplex.verts[2], cb);
float v_bc = glm::dot(simplex.verts[1], bc);
float u_ca = glm::dot(simplex.verts[0], ac);
float v_ca = glm::dot(simplex.verts[2], ca);
float u_bd = glm::dot(simplex.verts[3], db);
float v_bd = glm::dot(simplex.verts[1], bd);
float u_dc = glm::dot(simplex.verts[2], cd);
float v_dc = glm::dot(simplex.verts[3], dc);
float u_ad = glm::dot(simplex.verts[3], da);
float v_ad = glm::dot(simplex.verts[0], ad);
if (v_ab <= 0.0f && u_ca <= 0.0f && v_ad <= 0.0f) {
//Region A
simplex.barycentricValues[0] = 1.0f;
simplex.size = 1;
break;
}
if (u_ab <= 0.0f && v_bc <= 0.0f && v_bd <= 0.0f) {
//Region B
simplex.verts[0] = simplex.verts[1];
simplex.barycentricValues[0] = 1.0f;
simplex.size = 1;
break;
}
if (u_bc <= 0.0f && v_ca <= 0.0f && u_dc <= 0.0f) {
//Region C
simplex.verts[0] = simplex.verts[2];
simplex.barycentricValues[0] = 1.0f;
simplex.size = 1;
break;
}
if (u_bd <= 0.0f && v_dc <= 0.0f && u_ad <= 0.0f) {
//Region D
simplex.verts[0] = simplex.verts[3];
simplex.barycentricValues[0] = 1.0f;
simplex.size = 1;
break;
}
//calculate fractional area
glm::vec3 n = glm::cross(da, ba);
float u_adb = glm::dot(glm::cross(simplex.verts[3], simplex.verts[1]), n);
float v_adb = glm::dot(glm::cross(simplex.verts[1], simplex.verts[0]), n);
float w_adb = glm::dot(glm::cross(simplex.verts[0], simplex.verts[3]), n);
n = glm::cross(ca, da);
float u_acd = glm::dot(glm::cross(simplex.verts[2], simplex.verts[3]), n);
float v_acd = glm::dot(glm::cross(simplex.verts[3], simplex.verts[0]), n);
float w_acd = glm::dot(glm::cross(simplex.verts[0], simplex.verts[2]), n);
n = glm::cross(bc, dc);
float u_cbd = glm::dot(glm::cross(simplex.verts[1], simplex.verts[3]), n);
float v_cbd = glm::dot(glm::cross(simplex.verts[3], simplex.verts[2]), n);
float w_cbd = glm::dot(glm::cross(simplex.verts[2], simplex.verts[1]), n);
n = glm::cross(ba, ca);
float u_abc = glm::dot(glm::cross(simplex.verts[1], simplex.verts[2]), n);
float v_abc = glm::dot(glm::cross(simplex.verts[2], simplex.verts[0]), n);
float w_abc = glm::dot(glm::cross(simplex.verts[0], simplex.verts[1]), n);
if (w_abc <= 0.0f && v_adb <= 0.0f && u_ab > 0.0f && v_ab > 0.0f) {
//Region AB
simplex.barycentricValues[0] = u_ab;
simplex.barycentricValues[1] = v_ab;
simplex.size = 2;
break;
}
if (u_abc <= 0.0f && w_cbd <= 0.0f && u_bc > 0.0f && v_bc > 0.0f) {
//Region BC
simplex.verts[0] = simplex.verts[1];
simplex.verts[1] = simplex.verts[2];
simplex.barycentricValues[0] = u_bc;
simplex.barycentricValues[1] = v_bc;
simplex.size = 2;
break;
}
if (v_abc <= 0.0f && w_acd <= 0.0f && u_ca > 0.0f && v_ca > 0.0f) {
//Region CA
simplex.verts[1] = simplex.verts[0];
simplex.verts[0] = simplex.verts[2];
simplex.barycentricValues[0] = u_ca;
simplex.barycentricValues[1] = v_ca;
simplex.size = 2;
break;
}
if (v_cbd <= 0.0f && u_acd <= 0.0f && u_dc > 0.0f && v_dc > 0.0f) {
//Region DC
simplex.verts[0] = simplex.verts[3];
simplex.verts[1] = simplex.verts[2];
simplex.barycentricValues[0] = u_dc;
simplex.barycentricValues[1] = v_dc;
simplex.size = 2;
break;
}
if (v_acd <= 0.0f && w_adb <= 0.0f && u_ad > 0.0f && v_ad > 0.0f) {
//Region AD
simplex.verts[1] = simplex.verts[3];
simplex.barycentricValues[0] = u_ad;
simplex.barycentricValues[1] = v_ad;
simplex.size = 2;
break;
}
if (u_cbd <= 0.0f && u_adb <= 0.0f && u_bd > 0.0f && v_bd > 0.0f) {
//Region BD
simplex.verts[0] = simplex.verts[1];
simplex.verts[1] = simplex.verts[3];
simplex.barycentricValues[0] = u_bd;
simplex.barycentricValues[1] = v_bd;
simplex.size = 2;
break;
}
//calculate fractional volume (keeping in mind some of these may
//be negative)
float denom = glm::dot(glm::cross(cb, ab), db);
float volume = denom == 0.0f ? 1.0f : 1.0f / denom;
float u_abcd = glm::dot(glm::cross(simplex.verts[2], simplex.verts[3]), simplex.verts[1]) * volume;
float v_abcd = glm::dot(glm::cross(simplex.verts[2], simplex.verts[0]), simplex.verts[3]) * volume;
float w_abcd = glm::dot(glm::cross(simplex.verts[3], simplex.verts[0]), simplex.verts[1]) * volume;
float x_abcd = glm::dot(glm::cross(simplex.verts[1], simplex.verts[0]), simplex.verts[2]) * volume;
if (x_abcd <= 0.0f && u_abc > 0.0f && v_abc > 0.0f && w_abc > 0.0f) {
//Region ABC
simplex.barycentricValues[0] = u_abc;
simplex.barycentricValues[1] = v_abc;
simplex.barycentricValues[2] = w_abc;
simplex.size = 3;
break;
}
if (u_abcd <= 0.0f && u_cbd > 0.0f && v_cbd > 0.0f && w_cbd > 0.0f) {
//Region CBD
simplex.verts[0] = simplex.verts[2];
simplex.verts[2] = simplex.verts[3];
simplex.barycentricValues[0] = u_cbd;
simplex.barycentricValues[1] = v_cbd;
simplex.barycentricValues[2] = w_cbd;
simplex.size = 3;
break;
}
if (v_abcd <= 0.0f && u_acd > 0.0f && v_acd > 0.0f && w_acd > 0.0f) {
//Region ACD
simplex.verts[1] = simplex.verts[2];
simplex.verts[2] = simplex.verts[3];
simplex.barycentricValues[0] = u_acd;
simplex.barycentricValues[1] = v_acd;
simplex.barycentricValues[2] = w_acd;
simplex.size = 3;
break;
}
if (w_abcd <= 0.0f && u_adb > 0.0f && v_adb > 0.0f && w_adb > 0.0f) {
//Region ADB
simplex.verts[2] = simplex.verts[1];
simplex.verts[1] = simplex.verts[3];
simplex.barycentricValues[0] = u_adb;
simplex.barycentricValues[1] = v_adb;
simplex.barycentricValues[2] = w_adb;
simplex.size = 3;
break;
}
//Region ABCD
simplex.barycentricValues[0] = u_abcd;
simplex.barycentricValues[1] = v_abcd;
simplex.barycentricValues[2] = w_abcd;
simplex.barycentricValues[3] = x_abcd;
simplex.size = 4;
break;
}
//error
default:
break;
}
//if we have a tetrahedron at this point, it means it encloses the origin;
//we have an intersection
if (simplex.size == 4) {
break;
}
//find the closest point on our simplex to the origin
float denom = 0.0f;
for (size_t i = 0; i < simplex.size; i++) {
denom += simplex.barycentricValues[i];
}
denom = 1.0f / denom;
glm::vec3 closestPoint;
switch (simplex.size) {
case 1:
closestPoint = simplex.verts[0];
break;
case 2:
closestPoint = simplex.verts[0] * (denom * simplex.barycentricValues[0])
+ simplex.verts[1] * (denom * simplex.barycentricValues[1]);
break;
case 3:
closestPoint = simplex.verts[0] * (denom * simplex.barycentricValues[0])
+ simplex.verts[1] * (denom * simplex.barycentricValues[1])
+ simplex.verts[2] * (denom * simplex.barycentricValues[2]);
break;
case 4:
closestPoint = simplex.verts[0] * (denom * simplex.barycentricValues[0])
+ simplex.verts[1] * (denom * simplex.barycentricValues[1])
+ simplex.verts[2] * (denom * simplex.barycentricValues[2])
+ simplex.verts[3] * (denom * simplex.barycentricValues[3]);
break;
}
//ensure we are actually getting closer to the origin
float currentSqDistance = glm::dot(closestPoint, closestPoint);
if (currentSqDistance > sqDistance) {
break;
}
sqDistance = currentSqDistance;
//get the new search direction
glm::vec3 searchDirection;
switch (simplex.size) {
case 1:
searchDirection = -simplex.verts[0];
break;
case 2: {
glm::vec3 ba = simplex.verts[0] - simplex.verts[1];
searchDirection = glm::cross(glm::cross(ba, -simplex.verts[0]), ba);
break;
}
case 3: {
searchDirection = glm::cross(simplex.verts[1] - simplex.verts[0],
simplex.verts[2] - simplex.verts[0]);
if (glm::dot(searchDirection, simplex.verts[0]) > 0) {
searchDirection = -searchDirection;
}
}
}
//is this search direction numerically fit?
//if this condition holds, it is likely the origin is on an edge
//or triangle of the simplex. However, it could just be REALLY
//close.
if (glm::dot(searchDirection, searchDirection) < GJK_EPSILON * GJK_EPSILON) {
break;
}
//calculate the next tentative simplex vertex using our search direction
size_t indexA = supportIndex(searchDirection, vertsA, sizeA);
size_t indexB = supportIndex(-searchDirection, vertsB, sizeB);
glm::vec3 newPoint = vertsA[indexA] - vertsB[indexB];
//check for duplication; if we are revisiting a vertex already in
//the simplex, then we won't be getting any closer
bool duplicateFound = false;
for (size_t j = 0; j < previousSimplex.size; j++) {
if (indexA != previousSimplex.indicesA[j]) continue;
if (indexB != previousSimplex.indicesB[j]) continue;
duplicateFound = true;
break;
}
if (duplicateFound) {
break;
}
//add the current vertex to the simplex
simplex.indicesA[simplex.size] = indexA;
simplex.indicesB[simplex.size] = indexB;
simplex.barycentricValues[simplex.size] = 1.0f;
simplex.verts[simplex.size] = newPoint;
simplex.size++;
}
//calculate witness points
float denom = 0.0f;
for (size_t i = 0; i < simplex.size; i++) {
denom += simplex.barycentricValues[i];
}
denom = 1.0f / denom;
switch (simplex.size) {
case 1:
simplex.witnessA = vertsA[simplex.indicesA[0]];
simplex.witnessB = vertsB[simplex.indicesB[0]];
break;
case 2:
simplex.witnessA = vertsA[simplex.indicesA[0]] * (denom * simplex.barycentricValues[0])
+ vertsA[simplex.indicesA[1]] * (denom * simplex.barycentricValues[1]);
simplex.witnessB = vertsB[simplex.indicesB[0]] * (denom * simplex.barycentricValues[0])
+ vertsB[simplex.indicesB[1]] * (denom * simplex.barycentricValues[1]);
break;
case 3:
simplex.witnessA = vertsA[simplex.indicesA[0]] * (denom * simplex.barycentricValues[0])
+ vertsA[simplex.indicesA[1]] * (denom * simplex.barycentricValues[1])
+ vertsA[simplex.indicesA[2]] * (denom * simplex.barycentricValues[2]);
simplex.witnessB = vertsB[simplex.indicesB[0]] * (denom * simplex.barycentricValues[0])
+ vertsB[simplex.indicesB[1]] * (denom * simplex.barycentricValues[1])
+ vertsB[simplex.indicesB[2]] * (denom * simplex.barycentricValues[2]);
break;
case 4:
simplex.witnessA = vertsA[simplex.indicesA[0]] * (denom * simplex.barycentricValues[0])
+ vertsA[simplex.indicesA[1]] * (denom * simplex.barycentricValues[1])
+ vertsA[simplex.indicesA[2]] * (denom * simplex.barycentricValues[2])
+ vertsA[simplex.indicesA[3]] * (denom * simplex.barycentricValues[3]);
simplex.witnessB = simplex.witnessA;
}
simplex.distance = glm::distance(simplex.witnessA, simplex.witnessB);
return simplex;
}
size_t GJKSolver::supportIndex(glm::vec3 searchDirection, glm::vec3* verts, size_t vertexCount) {
if (vertexCount == 0) {
return 0;
}
size_t maxIndex = 0;
float maxProjection = glm::dot(verts[0], searchDirection);
for (size_t i = 1; i < vertexCount; i++) {
float currentProjection = glm::dot(verts[i], searchDirection);
if (currentProjection > maxProjection) {
maxProjection = currentProjection;
maxIndex = i;
}
}
return maxIndex;
}