FrustumTest

// g++ -std=c++11 FrustumTest.cpp -o FrustumTest -lGL -lglut
/*{
    "cmd": ["g++", "-std=c++11", "$file", "-o", "${file_path}/$file_base_name", "-I/usr/local/include", "-lGL", "-lglut"],
    "selector": "source.c++",
    "variants": [
        {
            "name": "Run",
            "cmd": ["bash", "-c", "'${file_path}/${file_base_name}'"]
        }
    ]
}*/

#include <GL/gl.h>
#include <GL/glut.h>
#include <glm/glm.hpp>
#include <glm/ext.hpp>
#include <vector>
#include <cmath>

typedef glm::vec4 Plane;
typedef std::vector<glm::vec4> Frustum;

Frustum ExtractFrustum(const glm::mat4& source);
std::vector<glm::vec3> GetWorldCorners(const Frustum& frustum);
std::vector<glm::vec3> GetWorldCorners(const glm::mat4& projectionView);
glm::vec3 PlaneIntersection(const Plane& p1, const Plane& p2, const Plane& p3);
void RenderFrustum(const std::vector<glm::vec3>& frustum);

void Reshape(int w, int h);
void Render();
void RenderOrigin();
void RenderFrustumRed();
void RenderFrustumGreen();
void RenderFrustumBlue();
void RenderFrustumYellow();

int main(int argc, char** argv) {
    glutInit(&argc, argv);
    glutInitDisplayMode(GLUT_RGBA | GLUT_DOUBLE | GLUT_DEPTH);
    glutInitWindowPosition(0,0);
    glutInitWindowSize(800 , 600);
    glutCreateWindow("Frustum Demo");
    Reshape(800, 600);

    glutDisplayFunc(Render);
    glutReshapeFunc(Reshape);

    glMatrixMode(GL_MODELVIEW);
    glLoadIdentity();

    glutPostRedisplay();
    glutMainLoop();
}

void Render() {
    glClearColor(0.5f, 0.6f, 0.7f, 1.0f);
    glClear(GL_COLOR_BUFFER_BIT);

    glMatrixMode(GL_MODELVIEW);
    glLoadIdentity();
    glm::mat4 glmLookAt = glm::lookAt(glm::vec3(-10.0f, 10.0f, 8.0f),
        glm::vec3(0.0f, 4.0f, 0.0f),  glm::vec3(0.0f, 1.0f, 0.0f));
    glMultMatrixf(glm::value_ptr(glmLookAt));

    RenderOrigin();
    RenderFrustumRed();
    RenderFrustumGreen();
    RenderFrustumBlue();
    RenderFrustumYellow();

    glutSwapBuffers();
    glutPostRedisplay();
}

void RenderFrustumRed() {
    glm::mat4 cameraWorldPos = glm::translate(0.0f, 7.0f, 0.0f);
    glm::mat4 cameraProjection = glm::perspective(60.0f, 0.9235993f, 1.0f, 5.0f);
    glm::mat4 cameraView = cameraWorldPos._inverse();

    glm::mat4 viewProjMatrix = cameraProjection;
    Frustum worldFrustum = ExtractFrustum(viewProjMatrix);
    glColor3f(1.0f, 0.0f, 0.0f);
    RenderFrustum(GetWorldCorners(worldFrustum));
}

void RenderFrustumGreen() {
    glm::mat4 cameraWorldPos = glm::translate(0.0f, 7.0f, 0.0f);
    glm::mat4 cameraProjection = glm::perspective(60.0f, 0.9235993f, 1.0f, 5.0f);
    glm::mat4 cameraView = cameraWorldPos._inverse();

    glm::mat4 viewProjMatrix = cameraProjection * cameraView;
    Frustum worldFrustum = ExtractFrustum(viewProjMatrix);
    glColor3f(0.0f, 1.0f, 0.0f);
    RenderFrustum(GetWorldCorners(worldFrustum));
}

void RenderFrustumBlue() {
    glm::mat4 cameraWorldPos = glm::translate(0.0f, 7.0f, 0.0f);
    glm::mat4 cameraProjection = glm::perspective(60.0f, 0.9235993f, 1.0f, 5.0f);

    glm::mat4 viewProjMatrix = cameraProjection;
    Frustum worldFrustum = ExtractFrustum(viewProjMatrix);
    std::vector<glm::vec3> corners = GetWorldCorners(worldFrustum);

    for (int i = 0, size = corners.size(); i < size; ++i) {
        glm::vec4 vertex = cameraWorldPos * glm::vec4(corners[i], 1.0f);
        corners[i] = glm::vec3(vertex.x, vertex.y, vertex.z);
    }

    glColor3f(0.0f, 0.0f, 1.0f);
    RenderFrustum(corners);
}

void RenderFrustumYellow() {
    glm::mat4 cameraWorldPos = glm::translate(0.0f, 7.0f, 0.0f);
    glm::mat4 cameraProjection = glm::perspective(60.0f, 0.9235993f, 1.0f, 5.0f);
    glm::mat4 cameraView = cameraWorldPos._inverse();

    glm::mat4 viewProjMatrix = cameraProjection * cameraView;
    glColor3f(1.0f, 1.0f, 0.0f);
    RenderFrustum(GetWorldCorners(viewProjMatrix));
}

void RenderOrigin() {
    glBegin(GL_LINES);
    glColor3f(1.0f, 0.0f, 0.0f);
    glVertex3f(0.0f, 0.0f, 0.0f);
    glVertex3f(1.0f, 0.0f, 0.0f);
    glColor3f(0.0f, 1.0f, 0.0f);
    glVertex3f(0.0f, 0.0f, 0.0f);
    glVertex3f(0.0f, 1.0f, 0.0f);
    glColor3f(0.0f, 0.0f, 1.0f);
    glVertex3f(0.0f, 0.0f, 0.0f);
    glVertex3f(0.0f, 0.0f, 1.0f);
    glEnd();
}

void Reshape(int w, int h) {
    glMatrixMode(GL_PROJECTION);
    glLoadIdentity();

    float fov = 60.0f;
    float aspectRatio = ((float)w) / ((float)h);
    float znear = 0.1f;
    float zfar = 1000.0f;

    float ymax = znear * tanf(fov * 3.14f / 360.0f);
    float xmax = ymax * aspectRatio;

    glFrustum(-xmax, xmax, -ymax, ymax, znear, zfar);
    glMatrixMode(GL_MODELVIEW);
}

void RenderFrustum(const std::vector<glm::vec3>& corners) {
    glBegin(GL_LINES);
    for (int i = 0, size = corners.size(); i < size; ++i) {
        for (int j = 0; j < size; ++j) {
            if (i == j) continue;
            glVertex3f(corners[i].x, corners[i].y, corners[i].z);
            glVertex3f(corners[j].x, corners[j].y, corners[j].z);
        }
    }
    glEnd();
}

std::vector<glm::vec3> GetWorldCorners(const glm::mat4& projectionView) {
    glm::mat4 inverseProjectionMatrix = glm::inverse(projectionView);

    std::vector<glm::vec3> corners; corners.reserve(8);
    for (unsigned x = 0; x < 2; x++)
        for (unsigned y = 0; y < 2; y++)
            for (unsigned z = 0; z < 2; z++) { // I might mess up the order of the frustrum corners here. But the position should be correct
                glm::vec4 projClipSpacePosition(x*2.0f-1.0f, y*2.0f-1.0f, z*2.0f-1.0f, 1.0f);
                glm::vec4 projWorldSpacePosition = inverseProjectionMatrix * projClipSpacePosition;
                corners.push_back(glm::vec3(projWorldSpacePosition.x / projWorldSpacePosition.w,
                                            projWorldSpacePosition.y / projWorldSpacePosition.w,
                                            projWorldSpacePosition.z / projWorldSpacePosition.w));
            }
    return corners;
}

std::vector<glm::vec3> GetWorldCorners(const Frustum& frustum) {
    std::vector<glm::vec3> corners; corners.reserve(8);
    corners.push_back(PlaneIntersection(frustum[4], frustum[0], frustum[2]));
    corners.push_back(PlaneIntersection(frustum[4], frustum[0], frustum[3]));
    corners.push_back(PlaneIntersection(frustum[4], frustum[1], frustum[3]));
    corners.push_back(PlaneIntersection(frustum[4], frustum[1], frustum[2]));
    corners.push_back(PlaneIntersection(frustum[5], frustum[1], frustum[2]));
    corners.push_back(PlaneIntersection(frustum[5], frustum[1], frustum[3]));
    corners.push_back(PlaneIntersection(frustum[5], frustum[0], frustum[3]));
    corners.push_back(PlaneIntersection(frustum[5], frustum[0], frustum[2]));
    return corners;
}

glm::vec3 PlaneIntersection(const Plane& p1, const Plane& p2, const Plane& p3) {
    // Calculate the determinant of the matrix (i.e | n1 n2 n3 |).
    float det = p1.x * (p2.y * p3.z -
                p2.z * p3.y) - p2.x *(p1.y * p3.z -
                p1.z * p3.y) + p3.x * (p1.y * p2.z - p1.z * p2.y);
    // If the determinant is zero, then the planes do not all intersect.
    if (fabs(det) <= 0.0001f)
        return glm::vec3();
    // Create 3 points, one on each plane.
    float p1x = -p1.x * p1.w;
    float p1y = -p1.y * p1.w;
    float p1z = -p1.z * p1.w;
    float p2x = -p2.x * p2.w;
    float p2y = -p2.y * p2.w;
    float p2z = -p2.z * p2.w;
    float p3x = -p3.x * p3.w;
    float p3y = -p3.y * p3.w;
    float p3z = -p3.z * p3.w;
    // Calculate the cross products of the normals.
    float c1x = (p2.y * p3.z) - (p2.z * p3.y);
    float c1y = (p2.z * p3.x) - (p2.x * p3.z);
    float c1z = (p2.x * p3.y) - (p2.y * p3.x);
    float c2x = (p3.y * p1.z) - (p3.z * p1.y);
    float c2y = (p3.z * p1.x) - (p3.x * p1.z);
    float c2z = (p3.x * p1.y) - (p3.y * p1.x);
    float c3x = (p1.y * p2.z) - (p1.z * p2.y);
    float c3y = (p1.z * p2.x) - (p1.x * p2.z);
    float c3z = (p1.x * p2.y) - (p1.y * p2.x);
    // Calculate the point of intersection using the formula:
    // x = (| n1 n2 n3 |)^-1 * [(x1 * n1)(n2 x n3) + (x2 * n2)(n3 x n1) + (x3 * n3)(n1 x n2)]
    float s1 = p1x * p1.x + p1y * p1.y + p1z * p1.z;
    float s2 = p2x * p2.x + p2y * p2.y + p2z * p2.z;
    float s3 = p3x * p3.x + p3y * p3.y + p3z * p3.z;
    float detI = 1.0f / det;

    glm::vec3 point;
    point.x = (s1 * c1x + s2 * c2x + s3 * c3x) * detI;
    point.y = (s1 * c1y + s2 * c2y + s3 * c3y) * detI;
    point.z = (s1 * c1z + s2 * c2z + s3 * c3z) * detI;
    return point;
}

Frustum ExtractFrustum(const glm::mat4& source) {
    const float* matrix = glm::value_ptr(source);
    Frustum result;
    result.resize(6);

    // Left clipping plane
    result[0].x = matrix[3] + matrix[0];
    result[0].y = matrix[7] + matrix[4];
    result[0].z = matrix[11] + matrix[8];
    result[0].w = matrix[15] + matrix[12];

    // Right clipping plane
    result[1].x = matrix[3] - matrix[0];
    result[1].y = matrix[7] - matrix[4];
    result[1].z = matrix[11] - matrix[8];
    result[1].w = matrix[15] - matrix[12];

    // Top clipping plane
    result[2].x = matrix[3] - matrix[1];
    result[2].y = matrix[7] - matrix[5];
    result[2].z = matrix[11] - matrix[9];
    result[2].w = matrix[15] - matrix[13];

    // Bottom clipping plane
    result[3].x = matrix[3] + matrix[1];
    result[3].y = matrix[7] + matrix[5];
    result[3].z = matrix[11] + matrix[9];
    result[3].w = matrix[15] + matrix[13];

    // Near clipping plane
    result[4].x = matrix[3] + matrix[2];
    result[4].y = matrix[7] + matrix[6];
    result[4].z = matrix[11] + matrix[10];
    result[4].w = matrix[15] + matrix[14];

    // Far clipping plane
    result[5].x = matrix[3] - matrix[2];
    result[5].y = matrix[7] - matrix[6];
    result[5].z = matrix[11] - matrix[10];
    result[5].w = matrix[15] - matrix[14];

    for (int i = 0; i < 6; ++i) {
        float mag = sqrtf(result[i].x * result[i].x + result[i].y * result[i].y + result[i].z * result[i].z);
        result[i].x /= mag;
        result[i].y /= mag;
        result[i].z /= mag;
        result[i].w /= mag;
    }

    return result;
}