Triangle to AABB collision code not working

233 views Asked by At

I have been tweaking and reworking my code to test an AABB against a triangle and I am unsure what I am doing wrong. I am using separating axis theorem as I believe this is the best and only way used to detect collisions between an AABB and Triangle (correct me if I am wrong or if there is a better/faster method). Currently it does not detect anything when a collision happens.

The code is relatively small, calling isAABBIntersectingTriangle every frame, hopefully someone who knows about math or the algorithm can help me out. The function takes in a bounding box min and max as two 3D vectors(glm::vec3) and the 3 of the triangles vertices (tri1, tri2, tri3) as glm::vec3s. These coordinates are both in world space (AABB coords translates by the position and scale only, triangle translated by position rotation and scale)

bool SATTriangleAABBCheck(glm::vec3 axis, glm::vec3 bboxMin, glm::vec3 bboxMax, glm::vec3 tri1, glm::vec3 tri2, glm::vec3 tri3)
{
    //Dot triangle vertices
    float triVert1 = glm::dot(axis, tri1);
    float triVert2 = glm::dot(axis, tri2);
    float triVert3 = glm::dot(axis, tri3);

    float triMin = glm::min(glm::min(triVert1, triVert2), triVert3);
    float triMax = glm::max(glm::max(triVert1, triVert2), triVert3);

    //Dot cube vertices
    float v1 = glm::dot(axis, glm::vec3(bboxMin.x, bboxMin.y, bboxMin.z));
    float v2 = glm::dot(axis, glm::vec3(bboxMax.x, bboxMax.y, bboxMax.z));
    float v3 = glm::dot(axis, glm::vec3(bboxMax.x, bboxMax.y, bboxMin.z));
    float v4 = glm::dot(axis, glm::vec3(bboxMax.x, bboxMin.y, bboxMax.z));
    float v5 = glm::dot(axis, glm::vec3(bboxMax.x, bboxMin.y, bboxMin.z));
    float v6 = glm::dot(axis, glm::vec3(bboxMin.x, bboxMax.y, bboxMax.z));
    float v7 = glm::dot(axis, glm::vec3(bboxMin.x, bboxMin.y, bboxMax.z));
    float v8 = glm::dot(axis, glm::vec3(bboxMin.x, bboxMax.y, bboxMin.z));

    float aabbMin = glm::min(glm::min(glm::min(glm::min(glm::min(glm::min(glm::min(v1, v2), v3), v4), v5), v6), v7) ,v8);
    float aabbMax = glm::max(glm::max(glm::max(glm::max(glm::max(glm::max(glm::max(v1, v2), v3), v4), v5), v6), v7), v8);

    if ((triMin < aabbMax && triMin > aabbMin) || (triMax < aabbMax && triMax > aabbMin))
        return true;
    if ((aabbMin < triMax && aabbMin > triMin) || (aabbMax < triMax && aabbMax > triMin))
        return true;

    return false;
}

glm::vec3 CalcSurfaceNormal(glm::vec3 tri1, glm::vec3 tri2, glm::vec3 tri3)
{
    glm::vec3 u = tri2 - tri1;
    glm::vec3 v = tri3 - tri1;
    glm::vec3 nrmcross = glm::normalize(glm::cross(u, v));
    return nrmcross;
}

bool isAABBIntersectingTriangle(glm::vec3 bboxMin, glm::vec3 bboxMax, glm::vec3 tri1, glm::vec3 tri2,  glm::vec3 tri3)
{   
    //AABB face normals
    glm::vec3 axis1(1, 0, 0);
    glm::vec3 axis2(0, 1, 0);
    glm::vec3 axis3(0, 0, 1);

    //Triangle face normal
    glm::vec3 axis4 = CalcSurfaceNormal(tri1, tri2, tri3);

    //Edge normals
    glm::vec3 e1 = tri2 - tri1;
    glm::vec3 e2 = tri3 - tri1;
    glm::vec3 e3 = tri3 - tri2;
    glm::vec3 e4 = glm::vec3(bboxMax.x, bboxMax.y, bboxMax.z) - glm::vec3(bboxMin.x, bboxMax.y, bboxMax.z);
    glm::vec3 e5 = glm::vec3(bboxMax.x, bboxMax.y, bboxMax.z) - glm::vec3(bboxMax.x, bboxMin.y, bboxMax.z);
    glm::vec3 e6 = glm::vec3(bboxMax.x, bboxMax.y, bboxMax.z) - glm::vec3(bboxMax.x, bboxMax.y, bboxMin.z);

    //Cross products of each edge
    glm::vec3 axis5 = glm::normalize(glm::cross(e1, e4));
    glm::vec3 axis6 = glm::normalize(glm::cross(e1, e5));
    glm::vec3 axis7 = glm::normalize(glm::cross(e1, e6));
    glm::vec3 axis8 = glm::normalize(glm::cross(e2, e4));
    glm::vec3 axis9 = glm::normalize(glm::cross(e2, e5));
    glm::vec3 axis10 = glm::normalize(glm::cross(e2, e6));
    glm::vec3 axis11 = glm::normalize(glm::cross(e3, e4));
    glm::vec3 axis12 = glm::normalize(glm::cross(e3, e5));
    glm::vec3 axis13 = glm::normalize(glm::cross(e3, e6));


    //If no overlap on all axes
    if (!SATTriangleAABBCheck(axis1, bboxMin, bboxMax, tri1, tri2, tri3)) return false;
    if (!SATTriangleAABBCheck(axis2, bboxMin, bboxMax, tri1, tri2, tri3)) return false;
    if (!SATTriangleAABBCheck(axis3, bboxMin, bboxMax, tri1, tri2, tri3)) return false;
    if (!SATTriangleAABBCheck(axis4, bboxMin, bboxMax, tri1, tri2, tri3)) return false;
    if (!SATTriangleAABBCheck(axis5, bboxMin, bboxMax, tri1, tri2, tri3)) return false;
    if (!SATTriangleAABBCheck(axis6, bboxMin, bboxMax, tri1, tri2, tri3)) return false;
    if (!SATTriangleAABBCheck(axis7, bboxMin, bboxMax, tri1, tri2, tri3)) return false;
    if (!SATTriangleAABBCheck(axis8, bboxMin, bboxMax, tri1, tri2, tri3)) return false;
    if (!SATTriangleAABBCheck(axis9, bboxMin, bboxMax, tri1, tri2, tri3)) return false;
    if (!SATTriangleAABBCheck(axis10, bboxMin, bboxMax, tri1, tri2, tri3)) return false;
    if (!SATTriangleAABBCheck(axis11, bboxMin, bboxMax, tri1, tri2, tri3)) return false;
    if (!SATTriangleAABBCheck(axis12, bboxMin, bboxMax, tri1, tri2, tri3)) return false;
    if (!SATTriangleAABBCheck(axis13, bboxMin, bboxMax, tri1, tri2, tri3)) return false;

    return true;
}

Also here is the code in the main loop that calculates the world position of the triangle and then calls the function, I don't believe anything is wrong here as it's probably been looked over the most:

glm::vec3 tri1 = glm::vec3(entities[0]->model.meshes[0].vertices[entities[0]->model.meshes[0].indices[0]].position.x * entities[0]->scale, entities[0]->model.meshes[0].vertices[entities[0]->model.meshes[0].indices[0]].position.y * entities[0]->scale, entities[0]->model.meshes[0].vertices[entities[0]->model.meshes[0].indices[0]].position.z * entities[0]->scale);
    glm::vec3 tri2 = glm::vec3(entities[0]->model.meshes[0].vertices[entities[0]->model.meshes[0].indices[1]].position.x * entities[0]->scale, entities[0]->model.meshes[0].vertices[entities[0]->model.meshes[0].indices[1]].position.y * entities[0]->scale, entities[0]->model.meshes[0].vertices[entities[0]->model.meshes[0].indices[1]].position.z * entities[0]->scale);
    glm::vec3 tri3 = glm::vec3(entities[0]->model.meshes[0].vertices[entities[0]->model.meshes[0].indices[2]].position.x * entities[0]->scale, entities[0]->model.meshes[0].vertices[entities[0]->model.meshes[0].indices[2]].position.y * entities[0]->scale, entities[0]->model.meshes[0].vertices[entities[0]->model.meshes[0].indices[2]].position.z * entities[0]->scale);

    //Translate these tris by the model matrix
    glm::mat4 mat(1.0f);
    mat = glm::translate(mat, glm::vec3(entities[0]->xPos, entities[0]->yPos, entities[0]->zPos));
    mat = glm::rotate(mat, glm::radians(entities[0]->xRot), glm::vec3(1, 0, 0));
    mat = glm::rotate(mat, glm::radians(entities[0]->yRot), glm::vec3(0, 1, 0));
    mat = glm::rotate(mat, glm::radians(entities[0]->zRot), glm::vec3(0, 0, 1));
    mat = glm::scale(mat, glm::vec3(entities[0]->scale, entities[0]->scale, entities[0]->scale));
    glm::vec4 tri11 = mat * glm::vec4(tri1.x, tri1.y, tri1.z, 1.0f);
    glm::vec4 tri22 = mat * glm::vec4(tri2.x, tri2.y, tri2.z, 1.0f);
    glm::vec4 tri33 = mat * glm::vec4(tri3.x, tri3.y, tri3.z, 1.0f);


    if (isAABBIntersectingTriangle(entities[3]->bboxMin, entities[3]->bboxMax, glm::vec3(tri11.x, tri11.y, tri11.z), glm::vec3(tri22.x, tri22.y, tri22.z), glm::vec3(tri33.x, tri33.y, tri33.z)))
    {
        std::cout << "AABB Tri collision\n";
    }
1

There are 1 answers

0
JaMiT On

There is a section of code in the test that is not robust.

glm::vec3 axis5 = glm::normalize(glm::cross(e1, e4));
glm::vec3 axis6 = glm::normalize(glm::cross(e1, e5));
glm::vec3 axis7 = glm::normalize(glm::cross(e1, e6));
glm::vec3 axis8 = glm::normalize(glm::cross(e2, e4));
glm::vec3 axis9 = glm::normalize(glm::cross(e2, e5));
glm::vec3 axis10 = glm::normalize(glm::cross(e2, e6));
glm::vec3 axis11 = glm::normalize(glm::cross(e3, e4));
glm::vec3 axis12 = glm::normalize(glm::cross(e3, e5));
glm::vec3 axis13 = glm::normalize(glm::cross(e3, e6));

These calculations are made unconditionally. However, normalizing a vector works only when the vector's length is not zero. A cross product has a zero length when the factors are parallel. So if any of e1, e2, e3 are parallel to any of e4, e5, e6, then the corresponding cross product will be a zero-length vector, and the coordinates of the normalization will be NaN.

A big problem with NaN is that it "poisons" all calculations. In particular, a single NaN coordinate in a vector is enough to cause any dot product involving that vector to evaluate to NaN. This really messes with what SATTriangleAABBCheck tries to do. (A zero-length vector would similarly mess with the function, so skipping the normalization is not a solution.)

The solution depends on what should be done in the case where a cross product is zero. In this case, that zero means that the calculation failed to produce a separating axis. There are fewer than the maximal number of axes to check because distinct parallel lines never intersect. Skip the test on the zero axis (that was normalized to NaN).