Work on swept collision

src/math/box3d.c

@@ -15,4 +15,9 @@ int box3DHasOverlap(struct Box3D* a, struct Box3D* b) {
     return a->min.x <= b->max.x && a->max.x >= b->min.x &&
         a->min.y <= b->max.y && a->max.y >= b->min.y &&
         a->min.z <= b->max.z && a->max.z >= b->min.z;
+}
+
+void box3DUnion(struct Box3D* a, struct Box3D* b, struct Box3D* out) {
+    vector3Max(&a->max, &b->max, &out->max);
+    vector3Min(&a->min, &b->min, &out->min);
 }

src/math/box3d.h

@@ -11,5 +11,6 @@ struct Box3D {
 int box3DContainsPoint(struct Box3D* box, struct Vector3* point);
 
 int box3DHasOverlap(struct Box3D* a, struct Box3D* b);
+void box3DUnion(struct Box3D* a, struct Box3D* b, struct Box3D* out);
 
 #endif

src/math/vector3.c

@@ -2,6 +2,8 @@
 #include "vector3.h"
 #include "mathf.h"
 
+#include <ultra64.h>
+
 struct Vector3 gRight = {1.0f, 0.0f, 0.0f};
 struct Vector3 gUp = {0.0f, 1.0f, 0.0f};
 struct Vector3 gForward = {0.0f, 0.0f, 1.0f};
@@ -134,6 +136,18 @@ void vector3TripleProduct(struct Vector3* a, struct Vector3* b, struct Vector3*
     vector3AddScaled(output, a, -vector3Dot(b, c), output);
 }
 
+void vector3Max(struct Vector3* a, struct Vector3* b, struct Vector3* out) {
+    out->x = MAX(a->x, b->x);
+    out->y = MAX(a->y, b->y);
+    out->z = MAX(a->z, b->z);
+}
+
+void vector3Min(struct Vector3* a, struct Vector3* b, struct Vector3* out) {
+    out->x = MIN(a->x, b->x);
+    out->y = MIN(a->y, b->y);
+    out->z = MIN(a->z, b->z);
+}
+
 int vector3IsZero(struct Vector3* vector) {
     return vector->x == 0.0f && vector->y == 0.0f && vector->z == 0.0f;
 }

src/math/vector3.h

@@ -37,6 +37,9 @@ void vector3ProjectPlane(struct Vector3* in, struct Vector3* normal, struct Vect
 int vector3MoveTowards(struct Vector3* from, struct Vector3* towards, float maxDistance, struct Vector3* out);
 void vector3TripleProduct(struct Vector3* a, struct Vector3* b, struct Vector3* c, struct Vector3* output);
 
+void vector3Max(struct Vector3* a, struct Vector3* b, struct Vector3* out);
+void vector3Min(struct Vector3* a, struct Vector3* b, struct Vector3* out);
+
 int vector3IsZero(struct Vector3* vector);
 
 void vector3ToVector3u8(struct Vector3* input, struct Vector3u8* output);

src/physics/collision_object.c

@@ -84,12 +84,12 @@ void collisionObjectCollideWithQuad(struct CollisionObject* object, struct Colli
     contactInsert(contact, &result);
 }
 
-void collisionObjectCollideWithQuadSwept(struct CollisionObject* object, struct Vector3* objectPrevPos, struct CollisionObject* quadObject, struct ContactSolver* contactSolver) {
+void collisionObjectCollideWithQuadSwept(struct CollisionObject* object, struct Vector3* objectPrevPos, struct Box3D* sweptBB, struct CollisionObject* quadObject, struct ContactSolver* contactSolver) {
     if ((object->collisionLayers & quadObject->collisionLayers) == 0) {
         return;
     }
 
-    if (!box3DHasOverlap(&object->boundingBox, &quadObject->boundingBox)) {
+    if (!box3DHasOverlap(sweptBB, &quadObject->boundingBox)) {
         return;
     }
 
@@ -123,14 +123,16 @@ void collisionObjectCollideWithQuadSwept(struct CollisionObject* object, struct
     }
 
     struct EpaResult result;
-    epaSolveSwept(
+    if (!epaSolveSwept(
         &simplex, 
         quad, minkowsiSumAgainstQuad, 
         &sweptObject, minkowsiSumAgainstSweptObject,
         objectPrevPos,
         &object->body->transform.position,
         &result
-    );
+    )) {
+        return;
+    }
 
     if (collisionSceneIsTouchingPortal(&result.contactA, &result.normal)) {
         object->body->flags |= RigidBodyIsTouchingPortal;
@@ -146,6 +148,8 @@ void collisionObjectCollideWithQuadSwept(struct CollisionObject* object, struct
     contact->friction = MAX(object->collider->friction, quadObject->collider->friction);
     contact->restitution = MIN(object->collider->bounce, quadObject->collider->bounce);
 
+    object->body->velocity = gZeroVec;
+
     transformPointInverseNoScale(&object->body->transform, &result.contactB, &result.contactB);
     contactInsert(contact, &result);
 }

src/physics/collision_object.h

@@ -37,7 +37,7 @@ int collisionObjectShouldGenerateConctacts(struct CollisionObject* object);
 void collisionObjectInit(struct CollisionObject* object, struct ColliderTypeData *collider, struct RigidBody* body, float mass, int collisionLayers);
 
 void collisionObjectCollideWithQuad(struct CollisionObject* object, struct CollisionObject* quad, struct ContactSolver* contactSolver);
-void collisionObjectCollideWithQuadSwept(struct CollisionObject* object, struct Vector3* objectPrevPos, struct CollisionObject* quadObject, struct ContactSolver* contactSolver);
+void collisionObjectCollideWithQuadSwept(struct CollisionObject* object, struct Vector3* objectPrevPos, struct Box3D* sweptBB, struct CollisionObject* quadObject, struct ContactSolver* contactSolver);
 void collisionObjectCollideTwoObjects(struct CollisionObject* a, struct CollisionObject* b, struct ContactSolver* contactSolver);
 
 void collisionObjectUpdateBB(struct CollisionObject* object);

src/physics/collision_scene.c

@@ -110,12 +110,12 @@ void collisionObjectCollideWithScene(struct CollisionObject* object, struct Coll
     }
 }
 
-void collisionObjectCollideWithSceneSwept(struct CollisionObject* object, struct Vector3* objectPrevPos, struct CollisionScene* scene, struct ContactSolver* contactSolver) {    
+void collisionObjectCollideWithSceneSwept(struct CollisionObject* object, struct Vector3* objectPrevPos, struct Box3D* sweptBB, struct CollisionScene* scene, struct ContactSolver* contactSolver) {    
     short colliderIndices[MAX_COLLIDERS];
-    int quadCount = collisionObjectRoomColliders(&scene->world->rooms[object->body->currentRoom], &object->boundingBox, colliderIndices);
+    int quadCount = collisionObjectRoomColliders(&scene->world->rooms[object->body->currentRoom], sweptBB, colliderIndices);
 
     for (int i = 0; i < quadCount; ++i) {
-        collisionObjectCollideWithQuadSwept(object, objectPrevPos, &scene->quads[colliderIndices[i]], contactSolver);
+        collisionObjectCollideWithQuadSwept(object, objectPrevPos, sweptBB, &scene->quads[colliderIndices[i]], contactSolver);
     }
 }
 
@@ -566,9 +566,14 @@ void collisionSceneUpdateDynamics() {
             collisionObjectCollideWithScene(object, &gCollisionScene, &gContactSolver);
         } else {
             struct Vector3 prevPos = object->body->transform.position;
+            struct Box3D sweptBB = object->boundingBox;
+
             rigidBodyUpdate(object->body);
             collisionObjectUpdateBB(object);
-            collisionObjectCollideWithSceneSwept(object, &prevPos, &gCollisionScene, &gContactSolver);
+            box3DUnion(&sweptBB, &object->boundingBox, &sweptBB);
+
+            collisionObjectCollideWithSceneSwept(object, &prevPos, &sweptBB, &gCollisionScene, &gContactSolver);
+            collisionObjectUpdateBB(object);
         }
     }
 

src/physics/collision_scene.h

@@ -26,7 +26,7 @@ extern struct CollisionScene gCollisionScene;
 
 void collisionSceneInit(struct CollisionScene* scene, struct CollisionObject* quads, int quadCount, struct World* world);
 void collisionObjectCollideWithScene(struct CollisionObject* object, struct CollisionScene* scene, struct ContactSolver* contactSolver);
-void collisionObjectCollideWithSceneSwept(struct CollisionObject* object, struct Vector3* objectPrevPos, struct CollisionScene* scene, struct ContactSolver* contactSolver);
+void collisionObjectCollideWithSceneSwept(struct CollisionObject* object, struct Vector3* objectPrevPos, struct Box3D* sweptBB, struct CollisionScene* scene, struct ContactSolver* contactSolver);
 
 int collisionSceneIsTouchingSinglePortal(struct Vector3* contactPoint, struct Vector3* contactNormal, struct Transform* portalTransform, int portalIndex);
 int collisionSceneIsTouchingPortal(struct Vector3* contactPoint, struct Vector3* contactNormal);

src/physics/epa.c

@@ -316,15 +316,16 @@ void expandingSimplexInit(struct ExpandingSimplex* expandingSimplex, struct Simp
     }
 }
 
-void expandingSimplexExpand(struct ExpandingSimplex* expandingSimplex, int newPointIndex, struct SimplexTriangle* faceToRemove) {
+void expandingSimplexExpand(struct ExpandingSimplex* expandingSimplex, int newPointIndex, int faceToRemoveIndex) {
     if (newPointIndex == -1) {
         return;
     }
 
+    struct SimplexTriangle* faceToRemove = &expandingSimplex->triangles[faceToRemoveIndex];
     union SimplexTriangleIndexData existing = faceToRemove->indexData;
 
     unsigned char triangleIndices[3];
-    triangleIndices[0] = expandingSimplex->triangleHeap[0];
+    triangleIndices[0] = faceToRemoveIndex;
     triangleIndices[1] = expandingSimplex->triangleCount;
     triangleIndices[2] = expandingSimplex->triangleCount + 1;
 
@@ -422,7 +423,7 @@ void epaSolve(struct Simplex* startingSimplex, void* objectA, MinkowsiSum object
         }
 
         ++simplex->pointCount;
-        expandingSimplexExpand(simplex, nextIndex, closestFace);
+        expandingSimplexExpand(simplex, nextIndex, simplex->triangleHeap[0]);
     }
 
     if (closestFace) {
@@ -439,7 +440,9 @@ void epaSolve(struct Simplex* startingSimplex, void* objectA, MinkowsiSum object
 void epaSweptFindFace(struct ExpandingSimplex* simplex, struct Vector3* direction, int* startTriangleIndex, int* startFaceEdge) {
     int currentFace = NEXT_FACE(*startFaceEdge);
 
-    while (currentFace != *startFaceEdge) {
+    int i = 0;
+
+    while (currentFace != *startFaceEdge && i < MAX_ITERATIONS) {
         int nextFace = NEXT_FACE(currentFace);
 
         struct SimplexTriangle* triangle = &simplex->triangles[*startTriangleIndex];
@@ -451,7 +454,7 @@ void epaSweptFindFace(struct ExpandingSimplex* simplex, struct Vector3* directio
             &normal
         );
 
-        if (vector3Dot(&normal, direction) < 0.00001f) {
+        if (vector3Dot(&normal, direction) < 0) {
             *startTriangleIndex = triangle->indexData.adjacentFaces[currentFace];
             nextFace = triangle->indexData.oppositePoints[currentFace];
             *startFaceEdge = NEXT_FACE(triangle->indexData.oppositePoints[currentFace]);
@@ -459,10 +462,11 @@ void epaSweptFindFace(struct ExpandingSimplex* simplex, struct Vector3* directio
         }
 
         currentFace = nextFace;
+        ++i;
     }
 }
 
-void epaSolveSwept(struct Simplex* startingSimplex, void* objectA, MinkowsiSum objectASum, void* objectB, MinkowsiSum objectBSum, struct Vector3* bStart, struct Vector3* bEnd, struct EpaResult* result) {
+int epaSolveSwept(struct Simplex* startingSimplex, void* objectA, MinkowsiSum objectASum, void* objectB, MinkowsiSum objectBSum, struct Vector3* bStart, struct Vector3* bEnd, struct EpaResult* result) {
     struct ExpandingSimplex* simplex = stackMalloc(sizeof(struct ExpandingSimplex));
     expandingSimplexInit(simplex, startingSimplex, SimplexFlagsSkipDistance);
     struct SimplexTriangle* closestFace = NULL;
@@ -470,7 +474,7 @@ void epaSolveSwept(struct Simplex* startingSimplex, void* objectA, MinkowsiSum o
     int currentTriangle = 0;
     int currentEdge = 0;
     struct Vector3 raycastDir;
-    vector3Sub(bEnd, bStart, &raycastDir);
+    vector3Sub(bStart, bEnd, &raycastDir);
 
     for (int i = 0; i < MAX_ITERATIONS; ++i) {
         struct Vector3 reverseNormal;
@@ -492,13 +496,14 @@ void epaSolveSwept(struct Simplex* startingSimplex, void* objectA, MinkowsiSum o
         simplex->ids[nextIndex] = COMBINE_CONTACT_IDS(aId, bId);
 
         projection = vector3Dot(&simplex->points[nextIndex], &closestFace->normal);
+        closestFace->distanceToOrigin = vector3Dot(&simplex->points[closestFace->indexData.indices[0]], &closestFace->normal);
 
         if ((projection - closestFace->distanceToOrigin) < 0.001f) {
             break;
         }
 
         ++simplex->pointCount;
-        expandingSimplexExpand(simplex, nextIndex, closestFace);
+        expandingSimplexExpand(simplex, nextIndex, currentTriangle);
     }
 
     if (closestFace) {
@@ -511,10 +516,9 @@ void epaSolveSwept(struct Simplex* startingSimplex, void* objectA, MinkowsiSum o
         
         float distance;
         if (!planeRayIntersection(&facePlane, &gZeroVec, &raycastDir, &distance)) {
-            goto exit;
+            goto error;
         }
 
-        result->normal = closestFace->normal;
         result->penetration = 0;
 
         struct Vector3 planePos;
@@ -525,8 +529,11 @@ void epaSolveSwept(struct Simplex* startingSimplex, void* objectA, MinkowsiSum o
         epaCalculateContact(simplex, closestFace, &planePos, result);
     }
 
-exit:
     stackMallocFree(simplex);
+    return 1;
+error:
+    stackMallocFree(simplex);
+    return 0;
 }
 
 void epaSwapResult(struct EpaResult* result) {

src/physics/epa.h

@@ -13,7 +13,7 @@ struct EpaResult {
 };
 
 void epaSolve(struct Simplex* startingSimplex, void* objectA, MinkowsiSum objectASum, void* objectB, MinkowsiSum objectBSum, struct EpaResult* result);
-void epaSolveSwept(struct Simplex* startingSimplex, void* objectA, MinkowsiSum objectASum, void* objectB, MinkowsiSum objectBSum, struct Vector3* bStart, struct Vector3* bEnd, struct EpaResult* result);
+int epaSolveSwept(struct Simplex* startingSimplex, void* objectA, MinkowsiSum objectASum, void* objectB, MinkowsiSum objectBSum, struct Vector3* bStart, struct Vector3* bEnd, struct EpaResult* result);
 void epaSwapResult(struct EpaResult* result);
 
 #endif

src/player/player.c

@@ -120,8 +120,14 @@ void playerApplyPortalGrab(struct Player* player, int portalIndex) {
 }
 
 void playerUpdateGrabbedObject(struct Player* player) {
-    if (controllerGetButtonDown(0, B_BUTTON)) {
+    if (controllerGetButtonDown(0, B_BUTTON) || controllerGetButtonDown(0, U_JPAD)) {
         if (player->grabbing) {
+            if (controllerGetButtonDown(0, U_JPAD)) {
+                struct Vector3 forward;
+                quatMultVector(&player->lookTransform.rotation, &gForward, &forward);
+                vector3AddScaled(&player->grabbing->body->velocity, &forward, -50.0f, &player->grabbing->body->velocity);
+            }
+
             player->grabbing = NULL;
         } else {
             struct Ray ray;

src/scene/scene.c

@@ -89,6 +89,8 @@ void sceneInit(struct Scene* scene) {
     for (int i = 0; i < scene->elevatorCount; ++i) {
         elevatorInit(&scene->elevators[i], &gCurrentLevel->elevators[i]);
     }
+
+    // scene->player.grabbing = &scene->decor[0]->collisionObject;
 }
 
 void sceneRenderWithProperties(void* data, struct RenderProps* properties, struct RenderState* renderState) {

tools/debug_simplex.py

@@ -0,0 +1,363 @@
+from glob import glob
+from math import cos, sin, sqrt
+from re import L
+import re
+import sys
+import traceback
+import gdb
+import contextlib
+import ctypes
+from OpenGL import GL as gl
+import glfw
+
+window_width = 800
+window_height = 600
+
+@contextlib.contextmanager
+def create_main_window():
+    global window_height
+    global window_width
+
+    if not glfw.init():
+        sys.exit(1)
+    try:
+        glfw.window_hint(glfw.CONTEXT_VERSION_MAJOR, 3)
+        glfw.window_hint(glfw.CONTEXT_VERSION_MINOR, 3)
+        glfw.window_hint(glfw.OPENGL_FORWARD_COMPAT, True)
+        glfw.window_hint(glfw.OPENGL_PROFILE, glfw.OPENGL_CORE_PROFILE)
+
+        title = 'Surface Generator Debug'
+        window = glfw.create_window(window_width, window_height, title, None, None)
+        if not window:
+            sys.exit(2)
+        glfw.make_context_current(window)
+
+        glfw.set_input_mode(window, glfw.STICKY_KEYS, True)
+        gl.glClearColor(0, 0, 0, 0)
+
+        yield window
+
+    finally:
+        glfw.terminate()
+
+def dot_product(a, b):
+    return a.x * b.x + a.y * b.y
+    
+class Vertex:
+    def __init__(self, x, y, z):
+        self.x = x
+        self.y = y
+        self.z = z
+
+    def __repr__(self):
+        return "Vertex(" + str(self.x) + ", " + str(self.y) + ", " + str(self.z) + ")"
+
+
+    def __str__(self):
+        return "(" + str(self.x) + ", " + str(self.y) + ", " + str(self.z) + ")"
+
+    def midpoint(self, to):
+        return Vertex((self.x + to.x) / 2, (self.y + to.y) / 2, (self.z + to.z) / 2)
+
+    def add(self, other):
+        return Vertex(self.x + other.x, self.y + other.y, self.z + other.z)
+
+    def sub(self, other):
+        return Vertex(self.x - other.x, self.y - other.y, self.z - other.z)
+
+    def scale(self, scalar):
+        return Vertex(self.x * scalar, self.y * scalar, self.z * scalar)
+
+class Triangle:
+    def __init__(self, a, b, c):
+        self.a = a
+        self.b = b
+        self.c = c
+
+    def __repr__(self):
+        return "Edge(" + str(self.aIndex) + ", " + str(self.bIndex) + ", " + str(self.nextEdge) + ", " + str(self.prevEdge) + ", " + str(self.nextEdgeReverse) + ", " + str(self.prevEdgeReverse) + ")"
+
+class Simplex:
+    def __init__(self, vertices, triangles, closestFace):
+        self.vertices = vertices
+        self.triangles = triangles
+        self.closestFace = closestFace
+
+    def build_vertex_buffer(self, raycastDir):
+        vertex_array_id = gl.glGenVertexArrays(1)
+        gl.glBindVertexArray(vertex_array_id)
+
+        attr_id = 0
+
+        vertex_buffer = gl.glGenBuffers(1)
+        gl.glBindBuffer(gl.GL_ARRAY_BUFFER, vertex_buffer)
+
+        vertex_pos = []
+        vertex_color = []
+        indices = []
+
+        def append_vertex(pos, color):
+            vertex_pos.append(pos.x)
+            vertex_pos.append(pos.y)
+            vertex_pos.append(pos.z)
+
+            vertex_color.append(color[0])
+            vertex_color.append(color[1])
+            vertex_color.append(color[2])
+
+
+        for vertex in self.vertices:
+            append_vertex(vertex, [0, 1, 1])
+
+        for triangle in self.triangles:
+            indices.append(triangle.a)
+            indices.append(triangle.b)
+
+            indices.append(triangle.b)
+            indices.append(triangle.c)
+
+            indices.append(triangle.c)
+            indices.append(triangle.a)
+
+        indices.append(len(self.vertices))
+        indices.append(len(self.vertices) + 1)
+
+        append_vertex(Vertex(0, 0, 0), [0, 1, 0])
+        append_vertex(raycastDir, [0, 1, 0])
+
+        if self.closestFace:
+            append_vertex(self.vertices[self.closestFace.a], [1, 0, 0])
+            append_vertex(self.vertices[self.closestFace.b], [1, 0, 0])
+            append_vertex(self.vertices[self.closestFace.c], [1, 0, 0])
+
+            indices.append(len(self.vertices) + 2)
+            indices.append(len(self.vertices) + 3)
+
+            indices.append(len(self.vertices) + 3)
+            indices.append(len(self.vertices) + 4)
+
+            indices.append(len(self.vertices) + 4)
+            indices.append(len(self.vertices) + 2)
+            
+
+        array_type = (gl.GLfloat * len(vertex_pos))
+
+        gl.glBufferData(gl.GL_ARRAY_BUFFER, len(vertex_pos) * ctypes.sizeof(ctypes.c_float), array_type(*vertex_pos), gl.GL_STATIC_DRAW)
+
+        gl.glVertexAttribPointer(
+            attr_id,
+            3,
+            gl.GL_FLOAT,
+            False,
+            0,
+            None
+        )
+        gl.glEnableVertexAttribArray(0)
+
+        attr_id = 1
+
+        color_buffer = gl.glGenBuffers(1)
+        gl.glBindBuffer(gl.GL_ARRAY_BUFFER, color_buffer)
+        array_type = (gl.GLfloat * len(vertex_color))
+        gl.glBufferData(gl.GL_ARRAY_BUFFER,
+            len(vertex_color) * ctypes.sizeof(ctypes.c_float),
+            array_type(*vertex_color),
+            gl.GL_STATIC_DRAW)
+        gl.glVertexAttribPointer(
+            attr_id,
+            3,
+            gl.GL_FLOAT,
+            False,
+            0,
+            None
+        )
+        gl.glEnableVertexAttribArray(attr_id)
+
+        index_buffer = gl.glGenBuffers(1)
+        array_type = (gl.GLshort * len(indices))
+        gl.glBindBuffer(gl.GL_ELEMENT_ARRAY_BUFFER, index_buffer)
+        gl.glBufferData(
+            gl.GL_ELEMENT_ARRAY_BUFFER,
+            len(indices) * ctypes.sizeof(ctypes.c_short),
+            array_type(*indices),
+            gl.GL_STATIC_DRAW
+        )
+
+        return len(indices)
+        
+def build_shaders():
+    shaders = {
+        gl.GL_VERTEX_SHADER: '''\
+            #version 330 core
+            layout(location = 0) in vec3 vertexPosition_modelspace;
+            layout(location = 1) in vec3 vertexColor;
+
+            varying vec3 pixelColor;
+
+            uniform mat4 transformMatrix;
+
+            void main(){
+              gl_Position = transformMatrix * vec4(vertexPosition_modelspace, 1);
+              pixelColor = vertexColor;
+            }
+            ''',
+        gl.GL_FRAGMENT_SHADER: '''\
+            #version 330 core
+
+            varying vec3 pixelColor;
+
+            out vec3 color;
+            void main(){
+              color = pixelColor;
+            }
+            '''
+        }
+    program_id = gl.glCreateProgram()
+
+    shader_ids = []
+    for shader_type, shader_src in shaders.items():
+        shader_id = gl.glCreateShader(shader_type)
+        gl.glShaderSource(shader_id, shader_src)
+
+        gl.glCompileShader(shader_id)
+
+        # check if compilation was successful
+        result = gl.glGetShaderiv(shader_id, gl.GL_COMPILE_STATUS)
+        info_log_len = gl.glGetShaderiv(shader_id, gl.GL_INFO_LOG_LENGTH)
+        if info_log_len:
+            logmsg = gl.glGetShaderInfoLog(shader_id)
+            print(logmsg)
+            return None
+
+        gl.glAttachShader(program_id, shader_id)
+        shader_ids.append(shader_id)
+
+    gl.glLinkProgram(program_id)
+
+    # check if linking was successful
+    result = gl.glGetProgramiv(program_id, gl.GL_LINK_STATUS)
+    info_log_len = gl.glGetProgramiv(program_id, gl.GL_INFO_LOG_LENGTH)
+    if info_log_len:
+        logmsg = gl.glGetProgramInfoLog(program_id)
+        print(logmsg)
+        return None
+
+    gl.glUseProgram(program_id)
+
+    return program_id
+
+def buildUniforms(program_id, scale, yaw, pitch):
+    global window_height
+    global window_width
+
+    gl.glUseProgram(program_id)
+    transform_matrix = gl.glGetUniformLocation(program_id, "transformMatrix")
+
+    cosH = cos(yaw)
+    sinH = sin(yaw)
+
+    cosP = cos(pitch)
+    sinP = sin(pitch)
+
+    gl.glUniformMatrix4fv(transform_matrix, 1, gl.GL_FALSE, [
+        scale * cosH, 0, -scale * sinH, 0,
+        scale * sinH * sinP, scale * cosP, scale * cosH * sinP, 0,
+        scale * sinH * cosP, scale * -sinP, scale * cosH * cosP, 0,
+        0, 0, 0, 1
+    ])
+
+def extract_simplex_data(simplex, closestFace):
+    vertex_count = simplex["pointCount"]
+    triangle_count = simplex["triangleCount"]
+
+    input_vertices = simplex["points"]
+    output_vertices = []
+
+    for vertex_index in range(0, vertex_count):
+        input_vertex = input_vertices[vertex_index]
+        output_vertices.append(Vertex(float(input_vertex["x"]), float(input_vertex["y"]), float(input_vertex["z"])))
+
+    input_triangles = simplex["triangles"]
+    output_triangles = []
+
+    for edge_index in range(0, triangle_count):
+        input_triangle = input_triangles[edge_index]
+        output_triangles.append(Triangle(
+            int(input_triangle["indexData"]["indices"][0]),
+            int(input_triangle["indexData"]["indices"][1]),
+            int(input_triangle["indexData"]["indices"][2])
+        ))
+
+    return Simplex(output_vertices, output_triangles, Triangle(
+        int(closestFace["indexData"]["indices"][0]),
+        int(closestFace["indexData"]["indices"][1]),
+        int(closestFace["indexData"]["indices"][2])
+    ))
+
+def main():
+    print("main")
+    print(gl.glGetString(gl.GL_VENDOR), gl.glGetString(gl.GL_RENDERER))
+    frame = gdb.selected_frame()
+    print("Getting simplex")
+    simplex = extract_simplex_data(frame.read_var("simplex"), frame.read_var("closestFace"))
+    raycastDir = frame.read_var("raycastDir")
+
+
+    with create_main_window() as window:
+        print("Building vertex buffer")
+        index_count = simplex.build_vertex_buffer(Vertex(float(raycastDir["x"]), float(raycastDir["y"]), float(raycastDir["z"])))
+
+        print("Building shaders")
+        program_id = build_shaders()
+        if program_id is None:
+            return
+
+        max_extent = 0
+
+        for vertex in simplex.vertices:
+            max_extent = max(max_extent, abs(vertex.x))
+            max_extent = max(max_extent, abs(vertex.y))
+
+        max_extent = max_extent + 10
+
+        scale = float(1) / float(max_extent)
+        yaw = 0
+        pitch = 0
+
+        buildUniforms(program_id, scale, yaw, pitch)
+
+        while (
+            glfw.get_key(window, glfw.KEY_ESCAPE) != glfw.PRESS and
+            not glfw.window_should_close(window)
+        ):
+            if glfw.get_key(window, glfw.KEY_MINUS):
+                scale = scale * 0.98
+
+            if glfw.get_key(window, glfw.KEY_EQUAL):
+                scale = scale / 0.98
+
+            if glfw.get_key(window, glfw.KEY_LEFT):
+                yaw = yaw + 0.01
+
+            if glfw.get_key(window, glfw.KEY_RIGHT):
+                yaw = yaw - 0.01
+
+            if glfw.get_key(window, glfw.KEY_UP):
+                pitch = pitch - 0.01
+
+            if glfw.get_key(window, glfw.KEY_DOWN):
+                pitch = pitch + 0.01
+
+            buildUniforms(program_id, scale, yaw, pitch)
+
+            gl.glClear(gl.GL_COLOR_BUFFER_BIT)
+            gl.glDrawElements(gl.GL_LINES, index_count, gl.GL_UNSIGNED_SHORT, None)
+
+            glfw.swap_buffers(window)
+            glfw.poll_events()
+
+try:
+    main()
+except:
+    print("An error happened")
+    print(traceback.format_exc())