basilisk-engine 0.0.9__py3-none-any.whl → 0.1.1__py3-none-any.whl

basilisk/__init__.py

@@ -1,10 +1,12 @@
+import pygame as pg
 from .engine import Engine
 from .scene import Scene
 from .nodes.node import Node
 from .mesh.mesh import Mesh
 from .render.image import Image
 from .render.material import Material
+from .render.shader import Shader
 from .render.shader_handler import ShaderHandler
 from .draw import draw
-from .render.camera import FreeCamera, StaticCamera
+from .render.camera import FreeCamera, StaticCamera, FollowCamera, OrbitCamera
 from .render.sky import Sky

basilisk/collisions/broad/broad_aabb.py

@@ -1,6 +1,6 @@
 import glm
 from ...generic.abstract_bvh import AbstractAABB as AABB
-from ...generic.collisions import collide_aabb_aabb
+from ...generic.collisions import collide_aabb_aabb, collide_aabb_line
 from ...generic.meshes import get_aabb_surface_area
 from ..collider import Collider
 
@@ -79,6 +79,13 @@ class BroadAABB(AABB):
         if isinstance(self.b, BroadAABB): possible.extend(self.b.get_collided(collider))
         elif collide_aabb_aabb(self.b.top_right, self.b.bottom_left, collider.top_right, collider.bottom_left): possible.append(self.b)
         return possible
+    
+    def get_line_collided(self, position: glm.vec3, forward: glm.vec3) -> list[Collider]:
+        """
+        Returns the colliders that may intersect with the given line
+        """
+        if not collide_aabb_line(self.top_right, self.bottom_left, position, forward): return []
+        return (self.a.get_line_collided(position, forward) if isinstance(self.a, BroadAABB) else [self.a]) + (self.b.get_line_collided(position, forward) if isinstance(self.b, BroadAABB) else [self.b])
         
     def get_all_aabbs(self, layer: int) -> list[tuple[glm.vec3, glm.vec3, int]]: # TODO test function
         """

basilisk/collisions/broad/broad_bvh.py

@@ -58,7 +58,35 @@ class BroadBVH(BVH):
         if isinstance(self.root, BroadAABB): return self.root.get_all_aabbs(0)
         return [(self.root.top_right, self.root.bottom_left, 0)]
         
-    def remove(self, collider: Collider) -> None: ...
+    def remove(self, collider: Collider) -> None:
+        """
+        Removes a collider from the BVH, refitting the tree and adjusting relations
+        """
+        parent: BroadAABB | None = collider.parent
+        
+        # if collider is the root, remove the root
+        if not parent:
+            self.root = None
+            return
+        
+        # if collider has no grandparent, remove parent and set sibling as root
+        grand   = parent.parent
+        sibling = parent.b if collider == parent.a else parent.a
+        if not grand:
+            self.root = sibling
+            sibling.parent = None
+            return
+    
+        # if grandparent exists
+        if parent == grand.a: grand.a = sibling
+        else:                 grand.b = sibling
+        sibling.parent = grand
+        
+        # move up and refit tree
+        aabb = grand
+        while aabb:
+            aabb.update_points()
+            aabb = aabb.parent
     
     def rotate(self, aabb: BroadAABB) -> None:
         """
@@ -99,4 +127,12 @@ class BroadBVH(BVH):
         """
         Returns which objects may be colliding from the BVH
         """
-        return self.root.get_collided(collider)
+        if isinstance(self.root, BroadAABB): return self.root.get_collided(collider)
+        else: return [] # if there is only one collider in the scene then there is nothing to collide with
+        
+    def get_line_collided(self, position: glm.vec3, forward: glm.vec3) -> list[Collider]:
+        """
+        Returns the colliders that may intersect with the given line
+        """
+        if isinstance(self.root, BroadAABB): return self.root.get_line_collided(position, forward)
+        return [self.root]

basilisk/collisions/collider.py

@@ -35,22 +35,25 @@ class Collider():
     mesh: Mesh
     """Reference to the colliding mesh"""
 
-    def __init__(self, collider_handler, node, box_mesh: bool=False, static_friction: glm.vec3=0.7, kinetic_friction: glm.vec3=0.3, elasticity: glm.vec3=0.1, collision_group: str=None):
-        self.collider_handler = collider_handler
+    def __init__(self, node, box_mesh: bool=False, static_friction: glm.vec3=0.7, kinetic_friction: glm.vec3=0.3, elasticity: glm.vec3=0.2, collision_group: str=None):
+        self.collider_handler = None
         self.node = node
-        self.mesh = self.collider_handler.cube if box_mesh else self.node.mesh
-        self.static_friction = static_friction
-        self.kinetic_friction = kinetic_friction
-        self.elasticity = elasticity
+        self.static_friction = static_friction if elasticity else 0.8 # added checks to prevent floats being set to None. Also done for kinetic and elasticity
+        self.box_mesh = box_mesh
+        self.kinetic_friction = kinetic_friction if elasticity else 0.4
+        self.elasticity = elasticity if elasticity else 0.1
         self.collision_group = collision_group
         self.collision_velocity = 0
         self.collisions = {}
         self.parent = None
         
         # lazy update variables TODO change to distinguish between static and nonstatic objects
-        self.needs_obb = True
-        self.needs_half_dimensions = True
-        
+        self.needs_obb = True # pos, scale, rot
+        self.needs_half_dimensions = True # scale, rot
+        self.needs_bvh = True # pos, scale, rot
+    
+    @property
+    def collider_handler(self): return self._collider_handler
     @property
     def has_collided(self): return bool(self.collisions)
     @property
@@ -71,4 +74,11 @@ class Collider():
         if self.needs_obb: 
             self._obb_points = transform_points(self.mesh.aabb_points, self.node.model_matrix)
             self.needs_obb = False
-        return self._obb_points
+        return self._obb_points
+    
+    @collider_handler.setter
+    def collider_handler(self, value):
+        self._collider_handler = value
+        if not value: return
+        self.mesh = value.cube if self.box_mesh else self.node.mesh
+        value.add(self)

basilisk/collisions/collider_handler.py

@@ -1,11 +1,14 @@
 import glm
-from .narrow.gjk import *
+
 from .collider import Collider
 from .broad.broad_bvh import BroadBVH
-from ..mesh.cube import Cube
-from ..generic.collisions import get_sat_axes
 from .narrow.gjk import collide_gjk
 from .narrow.epa import get_epa_from_gjk
+from .narrow.contact_manifold import get_contact_manifold, separate_polytope
+from .narrow.dataclasses import SupportPoint, ContactPoint, ContactManifold
+from ..nodes.node import Node
+from ..generic.collisions import get_sat_axes
+from ..physics.impulse import calculate_collisions
 
 class ColliderHandler():
     scene: ...
@@ -19,13 +22,14 @@ class ColliderHandler():
         self.scene = scene
         self.cube = self.scene.engine.cube
         self.colliders = []
+        self.polytope_data = {}
+        self.contact_manifolds: dict[tuple[Collider, Collider] : ContactManifold] = {}
         self.bvh = BroadBVH(self)
         
-    def add(self, node, box_mesh: bool=False, static_friction: glm.vec3=0.7, kinetic_friction: glm.vec3=0.3, elasticity: glm.vec3=0.1, collision_group: str=None) -> Collider:
+    def add(self, collider: Collider) -> Collider:
         """
         Creates a collider and adds it to the collider list
         """
-        collider = Collider(self, node, box_mesh, static_friction, kinetic_friction, elasticity, collision_group)
         self.colliders.append(collider)
         self.bvh.add(collider)
         return collider
@@ -44,9 +48,18 @@ class ColliderHandler():
         """
         # reset collision data
         for collider in self.colliders: collider.collisions = {}
-        # TODO update BVH
+        
+        
+        # update BVH
+        for collider in self.colliders:
+            if collider.needs_bvh:
+                self.bvh.remove(collider)
+                self.bvh.add(collider)
+                collider.needs_bvh = False
+        
+        # resolve collisions
         broad_collisions = self.resolve_broad_collisions()
-        self.resolve_narrow_collisions(broad_collisions)
+        self.resolve_narrow_collisions(broad_collisions) 
         
     def collide_obb_obb(self, collider1: Collider, collider2: Collider) -> tuple[glm.vec3, float] | None:
         """
@@ -59,7 +72,8 @@ class ColliderHandler():
         # test axes
         small_axis    = None
         small_overlap = 1e10
-        for axis in axes: # TODO add optimization for points on cardinal axis of cuboid
+        small_index = 0
+        for i, axis in enumerate(axes): # TODO add optimization for points on cardinal axis of cuboid
             # "project" points
             proj1 = [glm.dot(p, axis) for p in points1]
             proj2 = [glm.dot(p, axis) for p in points2]
@@ -69,17 +83,15 @@ class ColliderHandler():
             
             # if lines are not intersecting
             if   max1 > max2 and min1 < min2: overlap = min(max1 - min2, max2 - min1)
-            elif max2 > max1 and min2 < min1: overlap = min(max1 - min2, max2 - min1)
+            elif max2 > max1 and min2 < min1: overlap = min(max2 - min1, max1 - min2)
             else:                             overlap = min(max1, max2) - max(min1, min2) # TODO check if works with containment
             
             if abs(overlap) > abs(small_overlap): continue
             small_overlap = overlap
             small_axis    = axis
-        
-        print(axes.index(small_axis), glm.length(small_axis))
-        print('overlap:', small_overlap)
+            small_index   = i
             
-        return small_axis, small_overlap
+        return small_axis, small_overlap, small_index
     
     def collide_obb_obb_decision(self, collider1: Collider, collider2: Collider) -> bool:
         """
@@ -106,26 +118,54 @@ class ColliderHandler():
         """
         collisions = set()
         for collider1 in self.colliders:
-            
+            if collider1.node.static: continue
             # traverse bvh to find aabb aabb collisions
             colliding = self.bvh.get_collided(collider1)
             for collider2 in colliding:
-                if collider1 == collider2: continue
-                if (collider1, collider2) in collisions or (collider2, collider1) in collisions: continue # TODO find a secure way for ordering colliders
+                if collider1 is collider2: continue
+                if ((collider1, collider2) if id(collider1) < id(collider2) else (collider2, collider1)) in collisions: continue
                 
                 # run broad collision for specified mesh types
                 if max(len(collider1.mesh.points), len(collider2.mesh.points)) > 250 and not self.collide_obb_obb_decision(collider1, collider2): continue # contains at least one "large" mesh TODO write heuristic algorithm for determining large meshes
-                
-                collisions.add((collider1, collider2)) # TODO find a secure way for ordering colliders
+                collisions.add((collider1, collider2) if id(collider1) < id(collider2) else (collider2, collider1))
                 
         return collisions
     
+    def merge_contact_points(self, vec: glm.vec3, collider1: Collider, collider2: Collider, points1: list[ContactPoint], points2: list[ContactPoint]) -> None:
+        """
+        
+        """
+        def merge_points(node: Node, existing: dict[int, glm.vec3], incoming: list[ContactPoint]) -> dict[int, glm.vec3]:
+            incoming_indices = set()
+            
+            # add incoming points
+            for point in incoming:
+                incoming_indices.add(point.index)
+                if point.index not in existing or glm.length2(point.vertex - existing[point.index]) > 1e-5: existing[point.index] = glm.vec3(point.vertex)
+                # if glm.length2(point.vertex - existing[point.index]) != 0: print(point.vertex - existing[point.index])
+                    
+            # remove changed stored points
+            remove_indices = []
+            for index, vertex in existing.items():
+                if index in incoming_indices: continue
+                if glm.length2(node.get_vertex(index) - vertex) > 1e-5: remove_indices.append(index) # check to see if point has moved
+            
+            # remove unused and moved points
+            for index in remove_indices: del existing[index]
+            return existing
+        
+        # check if collision is logged, if not create a new one
+        collider_tuple = (collider1, collider2)
+        if collider_tuple not in self.contact_manifolds or glm.length2(self.contact_manifolds[collider_tuple].normal - vec) > 1e-7: self.contact_manifolds[collider_tuple] = ContactManifold(vec, dict(), dict())
+        
+        # add contact point from current collision and check overlap
+        self.contact_manifolds[collider_tuple].contact_points1 = merge_points(collider1.node, self.contact_manifolds[collider_tuple].contact_points1, points1)
+        self.contact_manifolds[collider_tuple].contact_points2 = merge_points(collider2.node, self.contact_manifolds[collider_tuple].contact_points2, points2)
+    
     def resolve_narrow_collisions(self, broad_collisions: list[tuple[Collider, Collider]]) -> None:
         """
         Determines if two colliders are colliding, if so resolves their penetration and applies impulse
         """
-        collided = []
-        
         for collision in broad_collisions: # assumes that broad collisions are unique
             collider1 = collision[0]
             collider2 = collision[1]
@@ -139,7 +179,11 @@ class ColliderHandler():
                 data = self.collide_obb_obb(collider1, collider2)
                 if not data: continue
                 
-                vec, distance = data
+                vec, distance, index = data
+                
+                # TODO replace with own contact algorithm
+                points1 = [ContactPoint(index, vertex) for index, vertex in enumerate(collider1.obb_points)]
+                points2 = [ContactPoint(index, vertex) for index, vertex in enumerate(collider2.obb_points)]
                 
             else: # use gjk to determine collisions between non-cuboid meshes
                 has_collided, simplex = collide_gjk(node1, node2)
@@ -148,17 +192,38 @@ class ColliderHandler():
                 face, polytope = get_epa_from_gjk(node1, node2, simplex)
                 vec, distance  = face[1], face[0]
                 
-            if glm.dot(vec, node2.position - node1.position) > 0:
-                vec *= -1
+                # TODO replace with own contact algorithm
+                points1 = [ContactPoint(p.index1, p.vertex1) for p in polytope]
+                points2 = [ContactPoint(p.index2, p.vertex2) for p in polytope]
                 
-            print('\033[92m', vec, distance, '\033[0m')
-            
-            # resolve collision penetration
-            node2.position -= vec * distance
+            if glm.dot(vec, node2.position - node1.position) > 0: vec *= -1
             
-            collided.append((node1, node2, vec * distance))
-            
-            # TODO add penetration resolution
-            # TODO add impulse
+            # apply impulse if a collider has a physic body
+            if node1.physics_body or node2.physics_body:
+                
+                # determine the contact points from the collision
+                points1, points2 = separate_polytope(points1, points2, vec)
+                self.merge_contact_points(vec, collider1, collider2, points1, points2)
+                
+                # for manifold in self.contact_manifolds.values(): print(list(manifold.contact_points1.values()) + list(manifold.contact_points2.values()))
+                
+                collider_tuple = (collider1, collider2)
+                # print(self.contact_manifolds[collider_tuple])
+                manifold = get_contact_manifold(
+                    node1.position - vec, 
+                    vec, 
+                    self.contact_manifolds[collider_tuple].contact_points1.values(), 
+                    self.contact_manifolds[collider_tuple].contact_points2.values()
+                )
+                
+                collision_normal = node1.velocity - node2.velocity
+                collision_normal = vec if glm.length2(collision_normal) < 1e-12 else glm.normalize(collision_normal)
+                calculate_collisions(collision_normal, node1, node2, manifold, node1.get_inverse_inertia(), node2.get_inverse_inertia(), node1.center_of_mass, node2.center_of_mass)
+
+                # for i, point in enumerate(manifold):
+                #     self.scene.add(Node(position = point, scale = (0.1, 0.1, 0.1)))
             
-        return collided
+            # resolve collision penetration
+            multiplier = 0.5 if not (node1.static or node2.static) else 1
+            if not node1.static: node1.position += multiplier * vec * distance
+            if not node2.static: node2.position -= multiplier * vec * distance

basilisk/collisions/narrow/contact_manifold.py

@@ -0,0 +1,91 @@
+import glm
+from random import randint
+from .line_intersections import line_line_intersect, line_poly_intersect
+from .graham_scan import graham_scan
+from .sutherland_hodgman import sutherland_hodgman
+from .dataclasses import ContactPoint
+
+# sutherland hodgman clipping algorithm
+def get_contact_manifold(contact_plane_point:glm.vec3, contact_plane_normal:glm.vec3, points1:list[glm.vec3], points2:list[glm.vec3]) -> list[glm.vec3]:
+    """
+    computes the contact manifold for a collision between two nearby polyhedra
+    """
+    if len(points1) == 0 or len(points2) == 0: return []
+    
+    # project vertices onto the 2d plane
+    points1 = project_points(contact_plane_point, contact_plane_normal, points1)
+    points2 = project_points(contact_plane_point, contact_plane_normal, points2)
+    
+    # check if collsion was on a vertex
+    if len(points1) == 1: return points1
+    if len(points2) == 1: return points2
+    
+    # convert points to 2d for intersection algorithms
+    points1, u1, v1 = points_to_2d(contact_plane_point, contact_plane_normal, points1)
+    points2, u2, v2 = points_to_2d(contact_plane_point, contact_plane_normal, points2, u1, v1) #TODO precalc orthogonal basis for 2d conversion
+    
+    # convert arbitrary points to polygon
+    if len(points1) > 2: points1 = graham_scan(points1)
+    if len(points2) > 2: points2 = graham_scan(points2)
+    
+    # run clipping algorithms
+    manifold = []
+    is_line1, is_line2 = len(points1) == 2, len(points2) == 2
+    if is_line1 and is_line2: manifold = line_line_intersect(points1, points2)
+    else: 
+        if is_line1:   manifold = line_poly_intersect(points1, points2)
+        elif is_line2: manifold = line_poly_intersect(points2, points1)
+        else:          manifold = sutherland_hodgman(points1, points2)
+        
+    # fall back if manifold fails to develope
+    if len(manifold) == 0: return []
+    
+    # convert inertsection algorithm output to 3d
+    return points_to_3d(u1, v1, contact_plane_point, manifold)
+
+def separate_polytope(points1: list[ContactPoint], points2: list[ContactPoint], contact_plane_normal, epsilon: float=1e-5) -> tuple[list[ContactPoint], list[ContactPoint]]:
+    """
+    Determines the potential contact manifold points of each shape based on their position along the penetrating axis
+    """
+    proj1 = [(glm.dot(point.vertex, contact_plane_normal), point) for point in points1]
+    proj2 = [(glm.dot(point.vertex, contact_plane_normal), point) for point in points2]
+    
+    # min1 and max2 should be past the collising points of node2 and node1 respectively
+    min1 = min(proj1, key=lambda proj: proj[0])[0]
+    max2 = max(proj2, key=lambda proj: proj[0])[0]
+    
+    proj1 = filter(lambda proj: proj[0] <= max2 + epsilon, proj1)
+    proj2 = filter(lambda proj: proj[0] + epsilon >= min1, proj2)
+    
+    return [point[1] for point in proj1], [point[1] for point in proj2]
+    
+def distance_to_plane(contact_plane_point:glm.vec3, contact_plane_normal:glm.vec3, point:glm.vec3) -> float:
+    """gets the smallest distance a point is from a plane"""
+    return glm.dot(point - contact_plane_point, contact_plane_normal) #TODO check this formula
+
+def project_points(contact_plane_point:glm.vec3, contact_plane_normal:glm.vec3, points:list[glm.vec3]) -> list[glm.vec3]:
+    """gets the projected positions of the given points onto the given plane"""
+    return [point - glm.dot(point - contact_plane_point, contact_plane_normal) * contact_plane_normal for point in points]
+
+def points_to_2d(contact_plane_point:glm.vec3, contact_plane_normal:glm.vec3, points:list[glm.vec3], u = None, v = None) -> tuple[list[glm.vec2], glm.vec3, glm.vec3]:
+    """converts a list of points on a plane to their 2d representation"""
+    # generate a new basis
+    k = get_noncolinear_vector(contact_plane_normal)
+    u = u if u else glm.normalize(glm.cross(contact_plane_normal, k))
+    v = v if v else glm.cross(contact_plane_normal, u)
+    
+    # convert points to new basis
+    return [glm.vec2(glm.dot(vec := point - contact_plane_point, u), glm.dot(vec, v)) for point in points], u, v
+    
+def points_to_3d(u:glm.vec3, v:glm.vec3, contact_plane_point:glm.vec3, points:list[glm.vec2]) -> list[glm.vec3]:
+    """converts a list of points on a plane to their 3d representation"""
+    return [contact_plane_point + point.x * u + point.y * v for point in points]
+    
+# vector math
+def get_noncolinear_vector(vector:glm.vec3) -> glm.vec3:
+    """generates a non colinear vector based on the given vector"""
+    test_vector = (1, 1, 1)
+    while glm.cross(test_vector, vector) == (0, 0, 0):
+        val = randint(0, 7) # 000 to 111
+        test_vector = (val & 1, val & 2, val & 4) # one random for three digits
+    return test_vector

basilisk/collisions/narrow/dataclasses.py

@@ -0,0 +1,27 @@
+import glm
+from dataclasses import dataclass
+
+# frozen because data does not need to be mutable
+# used in creating polytopes for GJK/EPA
+@dataclass(frozen=True)
+class SupportPoint():
+    support_point: glm.vec3
+    
+    index1: int # index of the vertex in the mesh
+    vertex1: glm.vec3 # world space location of the vertex at collision
+    
+    index2: int
+    vertex2: glm.vec3
+    
+# used for generating contact points for the contact manifold
+@dataclass(frozen=True)
+class ContactPoint():
+    index: int
+    vertex: glm.vec3
+    
+# contact manifold object used in the contact handler list
+@dataclass
+class ContactManifold():
+    normal: glm.vec3
+    contact_points1: dict[int : glm.vec3] # contact point index : collision position
+    contact_points2: dict[int : glm.vec3]

basilisk/collisions/narrow/deprecated.py

@@ -0,0 +1,47 @@
+    # def sat_manifold(self, points1: list[glm.vec3], points2: list[glm.vec3], axis: glm.vec3, plane_point: glm.vec3, digit: int) -> list[glm.vec3]:
+    #     """
+    #     Returns the contact manifold from an SAT OBB OBB collision
+    #     """
+    #     def get_test_points(contact_plane_normal:glm.vec3, points:list[glm.vec3], count: int):
+    #         test_points = [(glm.dot(contact_plane_normal, p), p) for p in points]
+    #         test_points.sort(key=lambda p: p[0])
+    #         return [p[1] for p in test_points[:count]]
+        
+    #     def get_test_points_unknown(contact_plane_normal:glm.vec3, points:list[glm.vec3]):
+    #         test_points = [(glm.dot(contact_plane_normal, p), p) for p in points]
+    #         test_points.sort(key=lambda p: p[0])
+    #         if test_points[2][0] - test_points[0][0] > 1e-3: return [p[1] for p in test_points[:2]]
+    #         else:                                            return [p[1] for p in test_points[:4]]        
+        
+    #     if digit < 6: # there must be at least one face in the collision
+    #         reference, incident = (get_test_points(-axis, points1, 4), get_test_points_unknown(axis, points2)) if digit < 3 else (get_test_points(axis, points2, 4), get_test_points_unknown(-axis, points1))
+            
+    #         # project vertices onto the 2d plane
+    #         reference = project_points(plane_point, axis, reference)
+    #         incident  = project_points(plane_point, axis, incident)
+            
+    #         # convert points to 2d for intersection algorithms
+    #         reference, u1, v1 = points_to_2d(plane_point, axis, reference)
+    #         incident,  u2, v2 = points_to_2d(plane_point, axis, incident, u1, v1)
+            
+    #         # convert arbitrary points to polygon
+    #         reference = graham_scan(reference)
+    #         if len(incident) == 4:  incident =  graham_scan(incident)
+            
+    #         # run clipping algorithms
+    #         manifold = []
+    #         if len(incident) == 2: manifold = line_poly_intersect(incident, reference)
+    #         else:                  manifold = sutherland_hodgman(reference, incident)
+                
+    #         # # fall back if manifold fails to develope
+    #         assert len(manifold), 'sat did not generate points'
+            
+    #         # # convert inertsection algorithm output to 3d
+    #         return points_to_3d(u1, v1, plane_point, manifold)
+        
+    #     else: # there is an edge edge collision
+            
+    #         points1 = get_test_points(-axis, points1, 2)
+    #         points2 = get_test_points(axis, points2, 2)
+            
+    #         return closest_two_lines(*points1, *points2)

basilisk/collisions/narrow/epa.py

@@ -1,13 +1,14 @@
 import glm
 from .helper import get_support_point
+from .dataclasses import SupportPoint
 from...nodes.node import Node
 
 
 # TODO change these to structs when converting to C++
 face_type     = list[tuple[float, glm.vec3, glm.vec3, int, int, int]] # distance, normal, center, index 1, index 2, index 3
-polytope_type = list[tuple[glm.vec3, glm.vec3, glm.vec3]] # polytope vertex, node1 vertex, node2 vertex
+polytope_type = list[SupportPoint] # polytope vertex, node1 vertex, node2 vertex
 
-def get_epa_from_gjk(node1: Node, node2: Node, polytope: polytope_type, epsilon: float=1e-7) -> tuple: # TODO determine the return type of get_epa_from_gjk
+def get_epa_from_gjk(node1: Node, node2: Node, polytope: polytope_type, epsilon: float=0) -> tuple[face_type, polytope_type]: # TODO determine the return type of get_epa_from_gjk and if epsilon is good value
     """
     Determines the peneration vector from a collision using EPA. The returned face normal is normalized but the rest are not guarunteed to be. 
     """
@@ -18,14 +19,10 @@ def get_epa_from_gjk(node1: Node, node2: Node, polytope: polytope_type, epsilon:
     # develope the polytope until the nearest real face has been found, within epsilon
     while True:
         new_point = get_support_point(node1, node2, faces[0][1])
-        if new_point in polytope or glm.length(new_point[0]) - faces[0][0] < epsilon: 
-            faces[0] = list(faces[0])
-            faces[0][0] = glm.sqrt(faces[0][0]) # square root distance squared to get real distance
-            faces[0][1] = glm.normalize(faces[0][1])
-            return faces[0], polytope
+        if new_point in polytope or glm.length(new_point.support_point) - faces[0][0] < epsilon: return faces[0], polytope
         faces, polytope = insert_point(polytope, faces, new_point)
 
-def insert_point(polytope: polytope_type, faces: face_type, point: glm.vec3, epsilon: float=1e-7) -> tuple[face_type, polytope_type]:
+def insert_point(polytope: polytope_type, faces: face_type, point: glm.vec3, epsilon: float=0) -> tuple[face_type, polytope_type]:
     """
     Inserts a point into the polytope sorting by distance from the origin
     """ 
@@ -34,8 +31,8 @@ def insert_point(polytope: polytope_type, faces: face_type, point: glm.vec3, eps
     support_index = len(polytope) - 1
     visible_faces = [
         face for face in faces
-        if glm.dot(face[1], polytope[support_index][0]) >= epsilon and # if the normal of a face is pointing towards the added point
-           glm.dot(face[1], polytope[support_index][0] - face[2]) >= epsilon # TODO check if this ever occurs
+        if glm.dot(face[1], polytope[support_index].support_point) >= epsilon and # if the normal of a face is pointing towards the added point
+           glm.dot(face[1], polytope[support_index].support_point - face[2]) >= epsilon # TODO check if this ever occurs
     ]
     
     # generate new edges
@@ -43,6 +40,9 @@ def insert_point(polytope: polytope_type, faces: face_type, point: glm.vec3, eps
     for face in visible_faces:
         for p1, p2 in get_face_edges(face):
             if (p2, p1) in edges: edges.remove((p2, p1)) # edges can only be shared by two faces, running opposite to each other. 
+            elif (p1, p2) in edges: # TODO remove this
+                edges.remove((p1, p2))
+                # print('not reversed')
             else: edges.append((p1, p2))
     
     # remove visible faces
@@ -58,9 +58,15 @@ def insert_face(polytope: polytope_type, faces: face_type, indices: tuple[int, i
     """
     Inserts a face into the face priority queue based on the indices given in the polytope
     """
-    center   = (polytope[indices[0]][0] + polytope[indices[1]][0] + polytope[indices[2]][0]) / 3
-    distance = glm.length2(center) # TODO face distance is length squared to reduce calculations
-    normal   = glm.cross(polytope[indices[1]][0] - polytope[indices[0]][0], polytope[indices[2]][0] - polytope[indices[0]][0]) # closest face normal will be normalized once returned to avoid square roots and division
+    center   = (polytope[indices[0]].support_point + polytope[indices[1]].support_point + polytope[indices[2]].support_point) / 3
+    normal   = glm.cross(polytope[indices[1]].support_point - polytope[indices[0]].support_point, polytope[indices[2]].support_point - polytope[indices[0]].support_point) # closest face normal will be normalized once returned to avoid square roots and division
+    if glm.dot(center, normal) < 0: 
+        normal *= -1
+        indices = (indices[2], indices[1], indices[0])
+
+    # TODO solve cases where face may contain origin
+    normal = glm.normalize(normal)
+    distance = abs(glm.dot(polytope[indices[0]].support_point, normal))
     new_face = (distance, normal, center, *indices)
     
     # insert faces into priority queue based on distance from origin
@@ -72,11 +78,11 @@ def insert_face(polytope: polytope_type, faces: face_type, indices: tuple[int, i
     
     return faces
 
-def orient_face(polytope: polytope_type, indices: tuple[int, int, int]) -> tuple[int, int, int]: # TODO finish this
+def orient_face(polytope: polytope_type, indices: tuple[int, int, int]) -> tuple[int, int, int]:
     """
     Orients the face indices to have a counter clockwise normal
     """
-    if glm.dot(glm.cross(polytope[indices[1]][0], polytope[indices[2]][0]), polytope[indices[0]][0]) < 0: return (indices[2], indices[1], indices[0])
+    if glm.dot(glm.cross(polytope[indices[1]].support_point, polytope[indices[2]].support_point), polytope[indices[0]].support_point) < 0: return (indices[2], indices[1], indices[0])
     return indices
     
 def get_face_edges(face: tuple[float, glm.vec3, glm.vec3, int, int, int]) -> list[tuple[int, int]]: