CheckCollisionWithHeightmap

bool BoxCollider::CheckCollisionWithHeightmap(HeightmapCollider* _pHeightmapCollider, CollisionPoint& _oCollisionResult)
{
    // Find where box center is located on heightmap texture
    glm::mat4 matModel = _pHeightmapCollider->GetHeightmapModelMatrix();
    glm::mat4 matModelInverse = glm::inverse(matModel);
    glm::vec3 vLocalHeightmapPosition = matModelInverse * glm::vec4(m_vPosition, 1.f); //[-0.5, 0.5]
    vLocalHeightmapPosition += glm::vec3(0.5f, 0.f, 0.5f); // [0, 1]

    uint32_t nEdgeCellsCount = _pHeightmapCollider->GetEdgeCellsCount();
    int iCellIDX = (int)std::floor(vLocalHeightmapPosition.x / (1.f / nEdgeCellsCount));
    int iCellIDY = (int)std::floor(vLocalHeightmapPosition.z / (1.f / nEdgeCellsCount));

    // Skip heightmap boundaries
    if (iCellIDX < 0 || iCellIDX > (int)nEdgeCellsCount || iCellIDY < 0 || iCellIDY > (int)nEdgeCellsCount)
        return false;

    // Load heightmap image
    int iWidth, iHeight, iBytesPerPixel;
    stbi_uc* pData = stbi_load(_pHeightmapCollider->GetImagePath().c_str(),
        &iWidth, &iHeight, &iBytesPerPixel, 4);

    // Coordinates in texture of closest (min) triangle vertex coordinates
    int iHeightmapCoordsX0 = (iWidth / nEdgeCellsCount) * iCellIDX;
    int iHeightmapCoordsY0 = (iHeight / nEdgeCellsCount) * iCellIDY;
    int iHeightmapCoordsX1 = (iWidth / nEdgeCellsCount) * (iCellIDX + 1);
    int iHeightmapCoordsY1 = (iHeight / nEdgeCellsCount) * (iCellIDY + 1);

    // Coordinates of closed X/Y in local space
    float fLocalCellX0 = (float)iCellIDX / (float)nEdgeCellsCount;
    float fLocalCellX1 = (float)(iCellIDX + 1) / (float)nEdgeCellsCount;
    float fLocalCellY0 = (float)iCellIDY / (float)nEdgeCellsCount;
    float fLocalCellY1 = (float)(iCellIDY + 1) / (float)nEdgeCellsCount;

    // Get height between where player is located
    // NOT RIGHT VALUES => HOW SAMPLED IS MADE USING STBI ?
    float fH00 = (float)*(pData + (iBytesPerPixel * (iHeightmapCoordsY0 * iWidth + iHeightmapCoordsX0))) / 256.f;
    float fT0 = pData[0];
    float fT1 = pData[1];
    float fT2 = pData[2];
    float fT3 = pData[3];
    float fT4 = pData[4];
    float fT5 = pData[5];
    float fT6 = pData[6];
    float fT7 = pData[7];


    float fH01 = (float)*(pData + (iBytesPerPixel * (iHeightmapCoordsY0 * iWidth + iHeightmapCoordsX1))) / 256.f;
    float fH10 = (float)*(pData + (iBytesPerPixel * (iHeightmapCoordsY1 * iWidth + iHeightmapCoordsX0))) / 256.f;
    float fH11 = (float)*(pData + (iBytesPerPixel * (iHeightmapCoordsY1 * iWidth + iHeightmapCoordsX1))) / 256.f;

    // Get triangles in local space
    CLTriangle3D oTriangleDown({
        glm::vec3(fLocalCellX1, fH11, fLocalCellY1),
        glm::vec3(fLocalCellX0, fH01, fLocalCellY1),
        glm::vec3(fLocalCellX1, fH10, fLocalCellY0)
    });
    CLTriangle3D oTriangleUp({
        glm::vec3(fLocalCellX0, fH00, fLocalCellY0),
        glm::vec3(fLocalCellX1, fH10, fLocalCellY0),
        glm::vec3(fLocalCellX0, fH01, fLocalCellY1)
    });

    // Find weights w1 (presence of UV in vect UV0 UV1) and w2 (vect UV0 UV2)
    float fW1Num = oTriangleUp[0].x * (oTriangleUp[2].z - oTriangleUp[0].z) + (vLocalHeightmapPosition.x - oTriangleUp[0].z) * (oTriangleUp[2].x - oTriangleUp[0].x) - vLocalHeightmapPosition.z * (oTriangleUp[2].z - oTriangleUp[0].z);
    float fW1Den = (oTriangleUp[1].z - oTriangleUp[0].z) * (oTriangleUp[2].x - oTriangleUp[0].x) - (oTriangleUp[1].x - oTriangleUp[0].x) * (oTriangleUp[2].z - oTriangleUp[0].z);
    float fW1 = fW1Num / fW1Den;
    float fW2 = (vLocalHeightmapPosition.x - oTriangleUp[0].z - fW1 * (oTriangleUp[1].z - oTriangleUp[0].z)) / (oTriangleUp[2].z - oTriangleUp[0].z);
    // Interpolate heights using weights to get actual height at position UV
    float fHeightFinal = oTriangleUp[0].y + fW1 * (oTriangleUp[1].y - fH00) + fW2 * (oTriangleUp[2].y - oTriangleUp[0].y);

	// Test collision with bounding box
    bool bCollisionFound = false;

	float fPenetration = 0.0f;

    glm::vec3 vProjectedPositionTriangleWorldSpace = glm::vec4(vLocalHeightmapPosition.x - 0.5f, oTriangleUp[0].y, vLocalHeightmapPosition.z - 0.5f, 1.f) * matModel;
    // Test whether local position is up of down a triangle
    // TODONathan : Mismatch : check how fHs is computed
    if (fHeightFinal > vLocalHeightmapPosition.y)
        bCollisionFound = true;

	stbi_image_free(pData);
    return bCollisionFound;
}