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

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

@@ -128,4 +128,11 @@ class BroadBVH(BVH):
         Returns which objects may be colliding from the BVH
         """
         if isinstance(self.root, BroadAABB): return self.root.get_collided(collider)
-        else: return []
+        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,11 +35,11 @@ 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 if elasticity else 0.8
+        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
@@ -51,7 +51,9 @@ class Collider():
         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
@@ -72,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,13 +1,11 @@
 import glm
 
-from basilisk.collisions.narrow.graham_scan import graham_scan
-from basilisk.collisions.narrow.sutherland_hodgman import sutherland_hodgman
 from .collider import Collider
 from .broad.broad_bvh import BroadBVH
 from .narrow.gjk import collide_gjk
 from .narrow.epa import get_epa_from_gjk
-from .narrow.contact_manifold import get_contact_manifold, points_to_2d, points_to_3d, project_points
-from .narrow.line_intersections import closest_two_lines, line_poly_intersect
+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
@@ -24,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
@@ -49,11 +48,14 @@ class ColliderHandler():
         """
         # reset collision data
         for collider in self.colliders: collider.collisions = {}
+        
+        
         # 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()
@@ -91,54 +93,6 @@ class ColliderHandler():
             
         return small_axis, small_overlap, small_index
     
-    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) #TODO precalc orthogonal basis for 2d conversion
-            
-            # 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)
-    
     def collide_obb_obb_decision(self, collider1: Collider, collider2: Collider) -> bool:
         """
         Determines if two obbs are colliding Uses SAT. 
@@ -164,20 +118,50 @@ 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
@@ -198,8 +182,8 @@ class ColliderHandler():
                 vec, distance, index = data
                 
                 # TODO replace with own contact algorithm
-                points1 = collider1.obb_points
-                points2 = collider2.obb_points
+                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)
@@ -209,17 +193,35 @@ class ColliderHandler():
                 vec, distance  = face[1], face[0]
                 
                 # TODO replace with own contact algorithm
-                points1 = [p[1] for p in polytope]
-                points2 = [p[2] for p in polytope]
+                points1 = [ContactPoint(p.index1, p.vertex1) for p in polytope]
+                points2 = [ContactPoint(p.index2, p.vertex2) for p in polytope]
                 
             if glm.dot(vec, node2.position - node1.position) > 0: vec *= -1
             
+            # apply impulse if a collider has a physic body
             if node1.physics_body or node2.physics_body:
-                manifold = get_contact_manifold(node1.position - vec, vec, points1, points2)
-                if len(manifold) == 0: 
-                    print('manifold failed to generate')
-                    continue
-                calculate_collisions(vec, node1, node2, manifold, node1.get_inverse_inertia(), node2.get_inverse_inertia(), node1.center_of_mass, node2.center_of_mass)
+                
+                # 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)))
             
             # resolve collision penetration
             multiplier = 0.5 if not (node1.static or node2.static) else 1

basilisk/collisions/narrow/contact_manifold.py

@@ -3,15 +3,13 @@ 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
     """
-    # determine the contact points from the collision
-    points1, points2 = separate_polytope(points1, points2, contact_plane_normal)
-    
     if len(points1) == 0 or len(points2) == 0: return []
     
     # project vertices onto the 2d plane
@@ -41,22 +39,23 @@ def get_contact_manifold(contact_plane_point:glm.vec3, contact_plane_normal:glm.
         
     # 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[glm.vec3], points2: list[glm.vec3], contact_plane_normal) -> list[glm.vec3]:
+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, contact_plane_normal), point) for point in points1]
-    proj2 = [(glm.dot(point, contact_plane_normal), point) for point in points2]
+    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 point: point[0])
-    max2 = max(proj2, key=lambda point: point[0])
+    min1 = min(proj1, key=lambda proj: proj[0])[0]
+    max2 = max(proj2, key=lambda proj: proj[0])[0]
     
-    proj1 = filter(lambda proj: proj < max2, proj1)
-    proj2 = filter(lambda proj: proj > min1, proj2)
+    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]
     

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=0) -> tuple: # TODO determine the return type of get_epa_from_gjk and if epsilon is good value
+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,7 +19,7 @@ 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: 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=0) -> tuple[face_type, polytope_type]:
@@ -30,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
@@ -57,15 +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
-    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]][0], 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
@@ -77,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]]:

basilisk/collisions/narrow/gjk.py

@@ -1,5 +1,6 @@
 import glm
 from .helper import get_support_point
+from .dataclasses import SupportPoint
 from ...nodes.node import Node
 from ...generic.math import triple_product
 
@@ -11,12 +12,12 @@ def collide_gjk(node1: Node, node2: Node, iterations: int=20) -> tuple: # TODO f
     # generate starting values
     dir_vec = node1.position - node2.position
     simplex = [get_support_point(node1, node2, dir_vec)]
-    dir_vec = -simplex[0][0] # set direction to point away from starting simplex point
+    dir_vec = -simplex[0].support_point # set direction to point away from starting simplex point
     
     for _ in range(iterations):
         # gets support point and checks if its across the origin
         test_point = get_support_point(node1, node2, dir_vec)
-        if glm.dot(test_point[0], dir_vec) < -1e-7: return False, simplex
+        if glm.dot(test_point.support_point, dir_vec) < -1e-7: return False, simplex
         
         # add point and find new direction vector
         simplex.append(test_point)
@@ -25,7 +26,7 @@ def collide_gjk(node1: Node, node2: Node, iterations: int=20) -> tuple: # TODO f
         if check: return True, simplex
     return False, simplex # timeout due to too many checks, usually float errors
         
-def handle_simplex(simplex: list) -> tuple[bool, glm.vec3, list[tuple[glm.vec3, glm.vec3, glm.vec3]]]:
+def handle_simplex(simplex: list[SupportPoint]) -> tuple[bool, glm.vec3, list[tuple[glm.vec3, glm.vec3, glm.vec3]]]:
     """
     Call proper function based on number of support points
     """
@@ -34,28 +35,28 @@ def handle_simplex(simplex: list) -> tuple[bool, glm.vec3, list[tuple[glm.vec3,
     if num == 3: return handle_simplex_triangle(simplex)
     return handle_simplex_tetrahedron(simplex) # simplex must be 4 points 
 
-def handle_simplex_line(simplex: list) -> tuple[bool, glm.vec3, list[tuple[glm.vec3, glm.vec3, glm.vec3]]]:
+def handle_simplex_line(simplex: list[SupportPoint]) -> tuple[bool, glm.vec3, list[tuple[glm.vec3, glm.vec3, glm.vec3]]]:
     """
     Returns the perpendicular vector to the simplex line
     """
-    vec_ab = simplex[1][0] - simplex[0][0]
-    return False, triple_product(vec_ab, -simplex[0][0], vec_ab), simplex
+    vec_ab = simplex[1].support_point - simplex[0].support_point
+    return False, triple_product(vec_ab, -simplex[0].support_point, vec_ab), simplex
     
-def handle_simplex_triangle(simplex: list) -> tuple[bool, glm.vec3, list[tuple[glm.vec3, glm.vec3, glm.vec3]]]:
+def handle_simplex_triangle(simplex: list[SupportPoint]) -> tuple[bool, glm.vec3, list[tuple[glm.vec3, glm.vec3, glm.vec3]]]:
     """
     Returns the normal vector of the triangoe pointed towards the origin
     """
-    dir_vec = glm.cross(simplex[1][0] - simplex[0][0], simplex[2][0] - simplex[0][0])
-    return False, -dir_vec if glm.dot(dir_vec, -simplex[0][0]) < 0 else dir_vec, simplex
+    dir_vec = glm.cross(simplex[1].support_point - simplex[0].support_point, simplex[2].support_point - simplex[0].support_point)
+    return False, -dir_vec if glm.dot(dir_vec, -simplex[0].support_point) < 0 else dir_vec, simplex
     
-def handle_simplex_tetrahedron(simplex: list, epsilon: float=0) -> tuple[bool, glm.vec3, list[tuple[glm.vec3, glm.vec3, glm.vec3]]]:
+def handle_simplex_tetrahedron(simplex: list[SupportPoint], epsilon: float=0) -> tuple[bool, glm.vec3, list[tuple[glm.vec3, glm.vec3, glm.vec3]]]:
     """
     Perform collision check and remove support point if no collision is found
     """
-    vec_da = simplex[3][0] - simplex[0][0]
-    vec_db = simplex[3][0] - simplex[1][0]
-    vec_dc = simplex[3][0] - simplex[2][0]
-    vec_do = -simplex[3][0]
+    vec_da = simplex[3].support_point - simplex[0].support_point
+    vec_db = simplex[3].support_point - simplex[1].support_point
+    vec_dc = simplex[3].support_point - simplex[2].support_point
+    vec_do = -simplex[3].support_point
     
     vectors = [(glm.cross(vec_da, vec_db), 2), (glm.cross(vec_dc, vec_da), 1), (glm.cross(vec_db, vec_dc), 0)] # TODO determine if this is the best way to do this
     for normal_vec, index in vectors:

basilisk/collisions/narrow/helper.py

@@ -1,14 +1,14 @@
 import glm
 from ...nodes.node import Node
+from .dataclasses import SupportPoint
 
-
-def get_support_point(node1: Node, node2: Node, dir_vec: glm.vec3) -> tuple[glm.vec3, glm.vec3, glm.vec3]:
+def get_support_point(node1: Node, node2: Node, dir_vec: glm.vec3) -> SupportPoint:
     """
     Outputs the best support point to be added to the polytop based on the direction vector.
     """
-    vertex1 = get_furthest_point(node1, dir_vec)
-    vertex2 = get_furthest_point(node2, -dir_vec)
-    return (vertex1 - vertex2, vertex1, vertex2)
+    vertex1, index1 = get_furthest_point(node1, dir_vec)
+    vertex2, index2 = get_furthest_point(node2, -dir_vec)
+    return SupportPoint(vertex1 - vertex2, index1, vertex1, index2, vertex2)
     
 def get_furthest_point(node: Node, dir_vec: glm.vec3) -> glm.vec3:
     """
@@ -16,11 +16,12 @@ def get_furthest_point(node: Node, dir_vec: glm.vec3) -> glm.vec3:
     """
     # determine furthest point by using untransformed mesh
     node_dir_vec = node.rotation * dir_vec # rotate the world space vector to node space
-    vertex = node.mesh.get_best_dot(node_dir_vec)
+    index = node.mesh.get_best_dot(node_dir_vec)
+    vertex = node.mesh.points[index]
     vertex = node.model_matrix * glm.vec4(vertex, 1.0)
     
     # transform point to world space
-    return glm.vec3(vertex)
+    return glm.vec3(vertex), index
 
 def is_ccw_turn(a:glm.vec2, b:glm.vec2, c:glm.vec2) -> bool:
     """

basilisk/collisions/narrow/sutherland_hodgman.py

@@ -22,3 +22,55 @@ def sutherland_hodgman(subject:list[glm.vec2], clip:list[glm.vec2]) -> list[glm.
                 output_poly += line_line_intersect([edge_end, edge_start], [prev_point, curr_point])
                 
     return output_poly
+
+# def get_intersect(one: glm.vec2, two: glm.vec2, thr: glm.vec2, fou: glm.vec2) -> glm.vec2:
+#     """
+#     Gets the intersection point between two lines
+#     """
+#     deno = (one.x - two.x) * (thr.y - fou.y) - (one.y - two.y) * (thr.x - fou.x)
+#     if deno == 0: # TODO determine if this happens
+#         print('sutherland-hodgman line intersection had zero denominator')
+#         return None
+#     x_num = (one.x * two.y - one.y * two.x) * (thr.x - fou.x) - (one.x - two.x) * (thr.x * fou.y - thr.y * fou.x)
+#     y_num = (one.x * two.y - one.y * two.x) * (thr.y - fou.y) - (one.y - two.y) * (thr.x * fou.y - thr.y * fou.x)
+#     return glm.vec2(x_num / deno, y_num / deno)
+
+# def clip(poly: list[glm.vec2], one: glm.vec2, two: glm.vec2) -> list[glm.vec2]:
+#     """
+#     Clip all edges of polygon with one of the clipping edges
+#     """
+#     num_points = len(poly)
+#     new_points = []
+    
+#     for i in range(num_points):
+#         k    = (i + 1) % num_points
+#         veci = poly[i]
+#         veck = poly[k]
+        
+#         posi = (two.x - one.x) * (veci.y - one.y) - (two.y - one.y) * (veci.x - one.x)
+#         posk = (two.x - one.x) * (veck.y - one.y) - (two.y - one.y) * (veck.x - one.x)
+        
+#         if posi < 0 and posk < 0: new_points.append(veck)
+#         elif posi >= 0 and posk < 0: 
+            
+#             new_points.append(get_intersect(one, two, veci, veck))
+#             new_points.append(veck)
+            
+#         elif posi < 0 and posk >= 0:
+            
+#             new_points.append(get_intersect(one, two, veci, veck))
+        
+#     return new_points
+
+# def sutherland_hodgman(subj_poly:list[glm.vec2], clip_poly:list[glm.vec2]) -> list[glm.vec2]:
+#     """
+#     Determines the clipped polygon vertices from ccw oriented polygons.
+#     """
+#     num_clip = len(clip_poly)
+    
+#     for i in range(num_clip):
+#         k = (i + 1) % num_clip
+        
+#         subj_poly = clip(subj_poly, clip_poly[i], clip_poly[k])
+        
+#     return subj_poly