From d56d1cc21cf7626538bdf6405cf52283f39b4f35 Mon Sep 17 00:00:00 2001
From: TigerKat <karynax23@protonmail.com>
Date: Sat, 13 Jul 2019 01:45:43 +0930
Subject: [PATCH] Now rebuilds the PolyGrid data when writing a .geo file.
 Added simple 3D vector math classes.

---
 geo.py      | 104 ++------------
 polygrid.py | 212 +++++++++++++++++++++++++++++
 util.py     |  53 ++++++++
 vec_math.py | 384 ++++++++++++++++++++++++++++++++++++++++++++++++++++
 4 files changed, 660 insertions(+), 93 deletions(-)
 create mode 100644 polygrid.py
 create mode 100644 util.py
 create mode 100644 vec_math.py

diff --git a/geo.py b/geo.py
index 3d32319..2b4c851 100755
--- a/geo.py
+++ b/geo.py
@@ -6,6 +6,8 @@ import sys
 import traceback
 import math
 from bones import *
+from polygrid import PolyCell, PolyGrid
+from util import Data
 
 #Ver 0 .geo pre-header:
 #Offset	Size	Description
@@ -400,97 +402,6 @@ def inferSizes(size, pack_list, other_list):
         
     return tuple(output)
 
-class PolyCell:
-    def __init__(self, model):
-        self.model = model
-        self.children = None
-        self.tri_idxs = []
-        self.tri_count = 0
-    def decode(self, data, offset):
-        (children_offset, tri_idxs_offset, self.tri_count) = struct.unpack("<iii", data[offset : offset + 12])
-        if children_offset == 0:
-            self.children = None
-        else:
-            self.children = [None] * 8
-            children_offsets = struct.unpack("<iiiiiiii", data[children_offset : children_offset + 4 * 8])
-            for i in range(8):
-                if children_offsets[i] == 0:
-                    continue
-                self.children[i] = PolyCell(self.model)
-                self.children[i].decode(data, children_offsets[i])
-        if tri_idxs_offset == 0:
-            self.tri_idxs = []
-        else:
-            self.tri_idxs = struct.unpack("<" + "H" * self.tri_count, data[tri_idxs_offset : tri_idxs_offset + self.tri_count * 2])
-    def dump(self, indent):
-        print(indent + "tri_idxs: %s" % ([self.tri_idxs], ))
-        print(indent + "children? %s" % (self.children is not None, ))
-        if self.children is not None:
-            for c in self.children:
-                if c is not None:
-                    c.dump(indent + "    ")
-
-class PolyGrid:
-    def __init__(self, model):
-        self.model = model
-        self.cell = None
-        pass
-    def parsePolyGridData(self, data):
-        self.cell = PolyCell(self.model)
-        self.cell.decode(data, 0)
-        pass
-    def dump(self):
-        if self.cell is not None:
-            self.cell.dump("        ")
-
-class Data:
-    def __init__(self, rawdata = b"", off = 0):
-        self.data = rawdata
-        self.offset = off
-    def setData(self, rawdata, off = 0):
-        self.data = rawdata
-        self.offset = off
-    def seek(self, off):
-        self.offset = off
-    def seekEnd(self, off = 0):
-        self.offset = len(self.data) + off
-    def seekRel(self, off = 0):
-        self.offset += off
-    def decode(self, fmt):
-        size = struct.calcsize(fmt)
-        val = struct.unpack(fmt, self.data[self.offset : self.offset + size])
-        self.offset += size
-        return val
-    def encode(self, fmt, *args):
-        data = struct.pack(fmt, *args)
-        self.write(data)
-    def read(self, length):
-        val = self.data[self.offset : self.offset + length]
-        self.offset += length
-        return val
-    def write(self, data):
-        #todo: handle
-        if self.offset == len(self.data):
-            self.data += data
-            self.offset = len(self.data)
-        elif self.offset > len(self.data):
-            self.data += ZERO_BYTE * (self.offset - len(self.data))
-            self.data += data
-            self.offset = len(self.data)
-        elif self.offset + len(data) >= len(self.data):
-            self.data = self.data[0 : self.offset] + data
-        else:
-            self.data = self.data[0 : self.offset] + data + self.data [self.offset + len(data) : ]
-    def truncate(self, offset = None):
-        if offset is None:
-            offset = self.offset
-        self.data[0 : offset]
-    def __len__(self):
-        return len(self.data)
-    def __str__(self):
-        return str(self.data)
-    def __repr__(self):
-        return repr(self.data)
 
 class Reductions:
     def __init__(self, model):
@@ -737,7 +648,7 @@ class Model:
             self.polygrid = None
         else:
             self.polygrid = PolyGrid(self)
-            self.polygrid.parsePolyGridData(self.grid_data)
+            self.polygrid.parsePolyGridData(self.grid_data, self.grid_header)
 
         if len(self.reductions_data) == 0:
             self.reductions = None
@@ -800,7 +711,14 @@ class Model:
         self.bone_ids += [0] * (15 - self.bone_count)
     def encode(self):
         #Regenerate dynamic data
-        #todo: build PolyGrid
+        if True:
+            self.polygrid = PolyGrid(self)
+            self.grid_data = self.polygrid.encode()
+            self.grid_header = self.polygrid.grid_header
+        else:
+            self.polygrid = None
+            self.grid_data = b""
+            self.grid_header = (0, 0.0, 0.0, 0.0, 1.0, 1.0, 0, 0)
         if self.geo.version >= 7:
             #todo: build reductions
             if self.reductions is not None:
diff --git a/polygrid.py b/polygrid.py
new file mode 100644
index 0000000..b4341ef
--- /dev/null
+++ b/polygrid.py
@@ -0,0 +1,212 @@
+
+import math
+import struct
+from vec_math import Vector3, Quaternion, Aabb, Triangle
+from util import Data
+
+#PolyGrid is an octree where the leaf nodes contain a list of all triangles that intersect that leaf node.
+#Each node on a PolyGrid is a PolyCell.
+
+
+#POLYGRID_EPSILON exapnds the cube of a PolyCell for purposes of finding a triangle collision.
+POLYGRID_EPSILON_MAX = 0.1 #Amount to expand the max corner of the AABB.
+POLYGRID_EPSILON_MIN = 0.0 #Amount to expand the min corner of the AABB.
+POLYGRID_MINIMUM_SIZE = 1
+
+
+class PolyCell:
+    def __init__(self):
+        self.children = None
+        self.tri_idxs = []
+        self.tri_count = 0
+        self.position = [0, 0, 0]
+        self.width = 0
+    def decode(self, data, offset):
+        (children_offset, tri_idxs_offset, self.tri_count) = struct.unpack("<iii", data[offset : offset + 12])
+        if children_offset == 0:
+            self.children = None
+        else:
+            self.children = [None] * 8
+            children_offsets = struct.unpack("<iiiiiiii", data[children_offset : children_offset + 4 * 8])
+            for i in range(8):
+                if children_offsets[i] == 0:
+                    continue
+                self.children[i] = PolyCell()
+                self.children[i].decode(data, children_offsets[i])
+        if tri_idxs_offset == 0:
+            self.tri_idxs = []
+        else:
+            self.tri_idxs = struct.unpack("<" + "H" * self.tri_count, data[tri_idxs_offset : tri_idxs_offset + self.tri_count * 2])
+
+    def rebuild(self, min_width, triangles, pos, width):
+        #triangles is a list of tuples (index, Triangle)
+        self.width = width
+        self.position = pos
+        halfwidth = width * 0.5
+        self.tri_idxs = []
+        self.tri_count = 0
+        #Create AABB for this cell, including epsilon margin.
+        aabb = Aabb(pos, Vector3(width, width, width) + pos)
+        aabb.min -= Vector3(POLYGRID_EPSILON_MIN, POLYGRID_EPSILON_MIN, POLYGRID_EPSILON_MIN)
+        aabb.max += Vector3(POLYGRID_EPSILON_MAX, POLYGRID_EPSILON_MAX, POLYGRID_EPSILON_MAX)
+        #Filter triangles to only those matching
+        tris = []
+        for t in triangles:
+            if t[1].testAabb(aabb):
+                tris.append(t)
+        #Return false if this node is empty.
+        if len(tris) == 0:
+            return False
+        #
+        if (width <= min_width and len(tris) < 9) or width <= 4:
+            #This cell is a leaf node. Populate triangle index list and return True.
+            for t in tris:
+                self.tri_idxs.append(t[0])
+            self.tri_count = len(self.tri_idxs)
+            return True
+        self.children = [None] * 8
+        #Populate branches
+        for i in range(8):
+            x = (0, halfwidth)[i & 0x1]
+            y = (0, halfwidth)[(i >> 1) & 0x1]
+            z = (0, halfwidth)[(i >> 2) & 0x1]
+            p = pos + Vector3(x, y, z)
+            cell = PolyCell()
+            if cell.rebuild(min_width, tris, p, halfwidth):
+                #Child cell has triangles store in its branch, store it.
+                self.children[i] = cell
+        return True
+    def encode(self, data):
+        data.seekEnd()
+        offset = data.tell()
+        o = offset + 12
+        if len(self.tri_idxs) > 0:
+            to = o
+            o += len(self.tri_idxs) * struct.calcsize("<H")
+        else:
+            to = 0
+        if self.children is not None:
+            co = o
+            cosz = struct.calcsize("<iiiiiiii")
+            o += cosz
+        else:
+            co = 0
+        data.encode("<iii", co, to, len(self.tri_idxs))
+        if len(self.tri_idxs) > 0:
+            data.encode("<" + "H" * len(self.tri_idxs), *self.tri_idxs)
+        if self.children is not None:
+            data.encode("<iiiiiiii", 0, 0, 0, 0, 0, 0, 0, 0)
+            child_offsets = [0] * 8
+            for i in range(8):
+                if self.children[i] is not None:
+                    child_offsets[i] = self.children[i].encode(data)
+            data.seek(co)
+            data.write(struct.pack("<iiiiiiii", *child_offsets))
+        return offset
+    def clear(self):
+        if self.children is not None:
+            for c in self.children:
+                if c is not None:
+                    c.clear()
+            self.children = None
+        self.tri_idxs = []
+        self.tri_count = 0
+    def collectTris(self, used_tris):
+        if self.children is not None:
+            for c in self.children:
+                if c is not None:
+                    c.collectTris(used_tris)
+        for idx in self.tri_idxs:
+            used_tris[idx] += 1
+    def countCells(self):
+        pass
+    def dump(self, indent):
+        print(indent + "position: %s  width: %s" % (repr(self.position), self.width))
+        print(indent + "tri_idxs: %s" % ([self.tri_idxs], ))
+        print(indent + "children? %s" % (self.children is not None, ))
+        if self.children is not None:
+            for c in self.children:
+                if c is not None:
+                    c.dump(indent + "    ")
+                else:
+                    print(indent + "    " + "-")
+
+
+class PolyGrid:
+    def __init__(self, model):
+        self.model = model
+        self.cell = None
+        self.position = [0, 0, 0]
+        self.width = 1.0
+        self.bits = 0
+        pass
+    def parsePolyGridData(self, data, grid_header):
+        self.cell = PolyCell()
+        self.cell.decode(data, 0)
+        self.grid_header = grid_header
+        self.position = grid_header[1:4]
+        self.width = grid_header[4]
+        self.bits = grid_header[7]
+        pass
+    def clearTree(self):
+        self.cell.clear()
+        self.cell = None
+    def rebuild(self):
+        #Extract vertices and bounding box of the Model.
+        aabb = Aabb()
+        verts = []
+        for v in self.model.verts:
+            verts.append(Vector3(*v))
+            aabb.expand(verts[-1])
+        tris = []
+        #Extract triangles from the Model.
+        for i in range(len(self.model.tris)):
+            t = self.model.tris[i]
+            tri = Triangle(verts[t[0]], verts[t[1]], verts[t[2]])
+            tris.append((i, tri))
+        #Compute the position and width of this bounding box.
+        self.position = aabb.min
+        sz = aabb.size()
+        radius = sz.mag() * 0.5
+        if radius > 2000:
+            min_width = 1024
+        elif radius > 1000:
+            min_width = 256
+        elif radius > 500:
+            min_width = 128
+        else:
+            min_width = 64
+        self.width = max(sz[0], sz[1], sz[2])
+        self.width = 2.0 ** math.ceil(math.log(self.width, 2.0))
+        self.width = max(1.0, self.width)
+        self.bits = int(math.floor(math.log(self.width, 2) + 0.5))
+
+        self.cell = PolyCell()
+        self.cell.rebuild(min_width, tris, self.position, self.width)
+
+        if not self.check():
+            print(self.model.name)
+            print(repr(tris))
+            self.dump()
+    def encode(self):
+        #Reconstruct the cell tree.
+        self.rebuild()
+        assert(self.check())
+        #Encode data
+        self.data = Data()
+        o = self.cell.encode(self.data)
+        self.grid_header = (0, self.position[0], self.position[1], self.position[2], self.width, 1.0 / self.width, 0, self.bits)
+        return self.data.data
+    def check(self):
+        """Checks that all triangles in the model are in at least of the nodes of the tree."""
+        used_tris = [0] * len(self.model.tris)
+        self.cell.collectTris(used_tris)
+        for v in used_tris:
+            if v <= 0:
+                print(repr(used_tris))
+                return False
+        return True
+    def dump(self, indent = "    "):
+        print(indent + "position: %s   width: %s" % (self.position, self.width))
+        if self.cell is not None:
+            self.cell.dump(indent + "    ")
diff --git a/util.py b/util.py
new file mode 100644
index 0000000..7bb53d1
--- /dev/null
+++ b/util.py
@@ -0,0 +1,53 @@
+import struct
+
+class Data:
+    def __init__(self, rawdata = b"", off = 0):
+        self.data = rawdata
+        self.offset = off
+    def setData(self, rawdata, off = 0):
+        self.data = rawdata
+        self.offset = off
+    def seek(self, off):
+        self.offset = off
+    def seekEnd(self, off = 0):
+        self.offset = len(self.data) + off
+    def seekRel(self, off = 0):
+        self.offset += off
+    def tell(self):
+        return self.offset
+    def decode(self, fmt):
+        size = struct.calcsize(fmt)
+        val = struct.unpack(fmt, self.data[self.offset : self.offset + size])
+        self.offset += size
+        return val
+    def encode(self, fmt, *args):
+        data = struct.pack(fmt, *args)
+        self.write(data)
+    def read(self, length):
+        val = self.data[self.offset : self.offset + length]
+        self.offset += length
+        return val
+    def write(self, data):
+        #todo: handle
+        if self.offset == len(self.data):
+            self.data += data
+            self.offset = len(self.data)
+        elif self.offset > len(self.data):
+            self.data += ZERO_BYTE * (self.offset - len(self.data))
+            self.data += data
+            self.offset = len(self.data)
+        elif self.offset + len(data) >= len(self.data):
+            self.data = self.data[0 : self.offset] + data
+        else:
+            self.data = self.data[0 : self.offset] + data + self.data [self.offset + len(data) : ]
+    def truncate(self, offset = None):
+        if offset is None:
+            offset = self.offset
+        self.data[0 : offset]
+    def __len__(self):
+        return len(self.data)
+    def __str__(self):
+        return str(self.data)
+    def __repr__(self):
+        return repr(self.data)
+
diff --git a/vec_math.py b/vec_math.py
new file mode 100644
index 0000000..35899b0
--- /dev/null
+++ b/vec_math.py
@@ -0,0 +1,384 @@
+import math
+
+
+class Vector3:
+    def __init__(self, *args):
+        if len(args) == 0:
+            self.data = [0, 0, 0]
+            return
+        if len(args) == 1:
+            if type(args[0]) in (tuple , list):
+                self.data = list(args[0])
+            if isinstance(args[0], Vector3):
+                self.data = list(args[0].data)
+                return
+        if len(args) == 3:
+            self.data = list(args)
+            return
+        #todo: raise error
+        pass
+    def __getitem__(self, index):
+        return self.data[index]
+    def __setitem__(self, index, value):
+        self.data[index] = value
+    def __len__(self):
+        return len(self.data)
+    def __str__(self):
+        return str(self.data)
+    def __repr__(self):
+        return "Vector3(%f, %f, %f)" % tuple(self.data)
+    def __add__(self, other):
+        return  Vector3(self[0] + other[0], self[1] + other[1], self[2] + other[2])
+    def __sub__(self, other):
+        return  Vector3(self[0] - other[0], self[1] - other[1], self[2] - other[2])
+    def __mul__(self, other):
+        return Vector3(self[0] * other, self[1] * other, self[2] * other)
+    def __div__(self, other):
+        return Vector3(self[0] / other, self[1] / other, self[2] / other)
+    def __iadd__(self, other):
+        self[0] += other[0]
+        self[1] += other[1]
+        self[2] += other[2]
+        return self
+    def __isub__(self, other):
+        self[0] -= other[0]
+        self[1] -= other[1]
+        self[2] -= other[2]
+        return self
+    def __imul__(self, other):
+        other = float(other)
+        self[0] *= other
+        self[1] *= other
+        self[2] *= other
+        return self
+    def __idiv__(self, other):
+        other = float(other)
+        self[0] /= other
+        self[1] /= other
+        self[2] /= other
+        return self
+    def __neg__(self):
+        return Vector3(-self[0], -self[1], -self[2])
+    def __eq__(self, other):
+        return self[0] == other[0] and self[1] == other[1] and self[2] == other[2]
+    def __ne__(self, other):
+        return self[0] != other[0] or self[1] != other[1] or self[2] != other[2]
+    def mag(self):
+        return (self.data[0] ** 2 + self.data[1] ** 2 + self.data[2] ** 2) ** 0.5
+    def mag2(self):
+        return (self.data[0] ** 2 + self.data[1] ** 2 + self.data[2] ** 2)
+    def normalize(self):
+        m = self.mag()
+        if m == 0:
+            return
+        for i in range(3):
+            self.data[i] /= m
+    def dot(self, other):
+        return self[0] * other[0] + self[1] * other[1] + self[2] * other[2]
+    def cross(self, other):
+        return Vector3(self[1] * other[2] - self[2] * other[1],
+                       self[2] * other[0] - self[0] * other[2],
+                       self[0] * other[1] - self[1] * other[0])
+
+class Quaternion:
+    #u, x, y, z
+    def __init__(self, *args):
+        if len(args) == 0:
+            self.data = [1, 0, 0, 0]
+        elif len(args) == 3:
+            (roll, pitch, yaw) = tuple(args)
+            sroll  = math.sin(roll)
+            spitch = math.sin(pitch)
+            syaw   = math.sin(yaw)
+            croll  = math.cos(roll)
+            cpitch = math.cos(pitch)
+            cyaw   = math.cos(yaw)
+            m = ( #create rotational Matrix
+                (cyaw * cpitch, cyaw * spitch * sroll - syaw * croll, cyaw * spitch * croll + syaw * sroll),
+                (syaw * cpitch, syaw * spitch * sroll + cyaw * croll, syaw * spitch * croll - cyaw * sroll),
+                (      -spitch,                       cpitch * sroll,                       cpitch * croll)
+            )
+            _u = (sqrt(max(0.0, 1 + m[0][0] + m[1][1] + m[2][2])) / 2.0)
+            _x = (sqrt(max(0.0, 1 + m[0][0] - m[1][1] - m[2][2])) / 2.0)
+            _y = (sqrt(max(0.0, 1 - m[0][0] + m[1][1] - m[2][2])) / 2.0)
+            _z = (sqrt(max(0.0, 1 - m[0][0] - m[1][1] + m[2][2])) / 2.0)
+            self.data = (_u,
+                         (m[2][1] - m[1][2]) >= 0 and abs(_x) or -abs(_x),
+                         (m[0][2] - m[2][0]) >= 0 and abs(_y) or -abs(_y),
+                         (m[1][0] - m[0][1]) >= 0 and abs(_z) or -abs(_z))
+        elif len(args) == 4:
+            self.data = list(args)
+        else:
+            #todo: raise error
+            pass
+    def __getitem__(self, index):
+        return args[index]
+    def __setitem__(self, index, value):
+        self.data[index] = value
+    def __len__(self):
+        return len(self.data)
+    def __str__(self):
+        return str(self.data)
+    def __repr__(self):
+        return "Quaternion(%f, %f, %f, %f)" % tuple(self.data)
+    def __mul__(self, other):
+        if len(other) == 3:
+            o = [0, other[0], other[1], other[2]]
+        else:
+            o = other
+        return Quaternion(self[0] * o[0] - self[1]*o[1] - self[2]*o[2] - self[3]*o[3],
+		          self[2] * o[3] - o[2]*self[3] + self[0]*o[1] + o[0]*self[1],
+		          self[3] * o[1] - o[3]*self[1] + self[0]*o[2] + o[0]*self[2],
+		          self[1] * o[2] - o[1]*self[2] + self[0]*o[3] + o[0]*self[3]);
+
+    def rotate(self, vec):
+        return self * vec * self.inv()
+    def inv(self):
+        return Quaternion(self[0], -self[1], -self[2], -self[3])
+    def mag(self):
+        return (self.data[0] ** 2 + self.data[1] ** 2 + self.data[2] ** 2 + self.data[3] ** 2) ** 0.5
+    def mag2(self):
+        return (self.data[0] ** 2 + self.data[1] ** 2 + self.data[2] ** 2 + self.data[3] ** 2)
+    def normalize(self):
+        m = self.mag()
+        if m == 0:
+            return
+        for i in range(4):
+            self.data[i] /= m
+
+
+class Aabb:
+    def __init__(self, mn = None, mx = None):
+        if mn is None:
+            self.min = Vector3(float('inf'), float('inf'), float('inf'))
+            self.max = Vector3(-float('inf'), -float('inf'), -float('inf'))
+        else:
+            self.min = mn
+            self.max = mx
+    def __str__(self):
+        return "%s" % str((self.min, self.max))
+    def __repr__(self):
+        return "Aabb(min: %s, max: %s)" % (repr(self.min), repr(self.max))
+    def isEmpty(self):
+        for i in range(3):
+            if self.min[i] > self.max[i]:
+                return True
+        return False
+    def expand(self, *args):
+        for a in args:
+            if type(a) is Aabb:
+                self.expand(a.min)
+                self.expand(a.max)
+            else:
+                for i in range(3):
+                    if a[i] < self.min[i]:
+                        self.min[i] = a[i]
+                    if a[i] > self.max[i]:
+                        self.max[i] = a[i]
+    def clear(self):
+        self.min = Vector3(float('inf'), float('inf'), float('inf'))
+        self.max = Vector3(-float('inf'), -float('inf'), -float('inf'))
+    def center(self):
+        if self.isEmpty():
+            return Vector3(0, 0, 0)
+        return (self.min + self.max) * 0.5
+    def size(self):
+        if self.isEmpty():
+            return Vector3(0, 0, 0)
+        return (self.max - self.min)
+    def test(self, other):
+        if self.isEmpty():
+            #Empty AABBs can't overlap anything.
+            return False
+        if isinstance(other, Aabb):
+            if other.isEmpty():
+                #Empty AABBs can't overlap anything.
+                return False
+            for i in range(3):
+                if self.min[i] > other.max[i]:
+                    return False
+                if other.min[i] > self.max[i]:
+                    return False
+            return True
+        else:
+            for i in range(3):
+                if other[i] < self.min[i] or other[i] > self.max[i]:
+                    return False
+            return True
+
+
+class Triangle:
+    def __init__(self, *args):
+        if len(args) == 0:
+            self.vertex = [Vector3(), Vector3(), Vector3()]
+            #print(":::a:::%s" % (repr(self), ))
+            return
+        if len(args) == 1:
+            if isinstance(args[0], Triangle):
+                v = args[0].vertex
+                self.vertex = [Vector3(v[0]), Vector3(v[1]), Vector3(v[2])]
+                #print(":::b:::%s" % (repr(self), ))
+                return
+        if len(args) != 3:
+            #todo: raise error
+            pass
+        self.vertex = list(args)
+        #print(":::c:::%s" % (repr(self), ))
+    def __str__(self):
+        return str(self.vertex)
+    def __repr__(self):
+        return "Triangle(%s)" % repr(self.vertex)
+    def translate(self, vec):
+        for i in range(3):
+            #print("%s: %s += %s" % (i, repr(self.vertex[i]), repr(vec)))
+            self.vertex[i] += vec
+    def rotate(self, quat):
+        for i in range(3):
+            self.vertex[i] = quat.rotate(self.vertex[i])
+    def scale(self, *args):
+        #print("Triangel.scale: self: %s  other: %s" % (repr(self), repr(args)))
+        if len(args) == 1:
+            if isinstance(args[0], Vector3):
+                for i in range(3):
+                    for j in range(3):
+                        self.vertex[i][j] *= args[0][j]
+            else:
+                for i in range(3):
+                    self.vertex[i] *= args[0]
+        elif len(args) == 3:
+            for i in range(3):
+                for j in range(3):
+                    self.vertex[i][j] *= args[j]
+        else:
+            #todo: raise error
+            pass
+    def testAabb(self, aabb):
+        #Empty AABBs can't collide
+        if aabb.isEmpty():
+            return False
+        #Easy test: Test if AABBs overlap, return false if they don't.
+        for i in range(3):
+            if self.vertex[0][i] < aabb.min[i] and self.vertex[1][i] < aabb.min[i] and self.vertex[2][i] < aabb.min[i]:
+                return False
+            if self.vertex[0][i] > aabb.max[i] and self.vertex[1][i] > aabb.max[i] and self.vertex[2][i] > aabb.min[i]:
+                return False
+        #Easy test: Test if points are inside the AABB, return true if they do.
+        for i in range(3):
+            if aabb.test(self.vertex[i]):
+                return True
+        #Copy triangle, to allow manipulation.
+        t = Triangle(self)
+        t.translate(-aabb.center())
+        s = aabb.size()
+        for i in range(3):
+            s[i] = 1.0 / s[i]
+        t.scale(s)
+        return t.testCubeBody(0.5)
+    def testCubeBody(self, halfwidth):
+        def testPlaneInCube():
+            vmin = Vector3()
+            vmax = Vector3()
+            for i in range(3):
+                if normal[i] > 0:
+                    vmin[i] = -halfwidth
+                    vmax[i] = halfwidth
+                else:
+                    vmin[i] = halfwidth
+                    vmax[i] = -halfwidth
+            #print("vmin: %s, %s" % (repr(vmin), normal.dot(vmin)))
+            #print("vmax: %s, %s" % (repr(vmax), normal.dot(vmax)))
+            if dist < normal.dot(vmin):
+                return False
+            if dist <= normal.dot(vmax):
+                return True
+            return False
+        def testAxis(vr, vs, ia, ib, a, b):
+            fa = abs(a)
+            fb = abs(b)
+            pr = a * vr[ia] + b * vr[ib]
+            ps = a * vs[ia] + b * vs[ib]
+            if pr < ps:
+                mn = pr
+                mx = ps
+            else:
+                mn = ps
+                mx = pr
+            rad = (fa + fb) * halfwidth
+            #print("fa: %s  fb: %s  pr: %s  ps: %s  mn: %s  mx: %s  rad: %s" % (fa, fb, pr, ps, mn, mx, rad))
+            if mn > rad or mx < -rad:
+                return False
+            return True
+
+        edge = [None, None, None]
+        for i in range(3):
+            edge[i] = self.vertex[(i + 1) % 3] - self.vertex[i]
+        #print("edge: %s" % repr(edge))
+        normal = edge[0].cross(edge[1])
+        #print("normal (raw): %s" % repr(normal))
+        normal.normalize()
+        dist = normal.dot(self.vertex[0])
+        #print("normal: %s  distance: %s" % (repr(normal), dist))
+        #print("halfwidth: %s" % (halfwidth, ))
+        #Test if the triangles plane intersects the cube.
+        if not testPlaneInCube():
+            return False
+        v = self.vertex
+        #print("vertex: %s" % (repr(v)))
+        if not testAxis(v[0], v[2], 1, 2,  edge[0][2], -edge[0][1]): return False
+        if not testAxis(v[0], v[2], 0, 2, -edge[0][2],  edge[0][0]): return False
+        if not testAxis(v[1], v[2], 0, 1,  edge[0][1], -edge[0][0]): return False
+
+        if not testAxis(v[0], v[2], 1, 2,  edge[1][2], -edge[1][1]): return False
+        if not testAxis(v[0], v[2], 0, 2, -edge[1][2],  edge[1][0]): return False
+        if not testAxis(v[0], v[1], 0, 1,  edge[1][1], -edge[1][0]): return False
+
+        if not testAxis(v[0], v[1], 1, 2,  edge[2][2], -edge[2][1]): return False
+        if not testAxis(v[0], v[1], 0, 2, -edge[2][2],  edge[2][0]): return False
+        if not testAxis(v[1], v[2], 0, 1,  edge[2][1], -edge[2][0]): return False
+        return True
+
+if __name__ == "__main__":
+    #todo: make these unit tests better
+    #Unit test Vector3
+    assert(Vector3() == [0, 0, 0])
+    assert(Vector3(0, 0, 0) == Vector3())
+    assert(Vector3(1, 1, 1) + Vector3(-1, -1, -1) == Vector3())
+    assert(Vector3(1, 1, 1) - Vector3(1, 1, 1) == Vector3())
+    assert(Vector3(2, 2, 2) * 0.5 == Vector3(1, 1, 1))
+    assert(Vector3(2, 2, 2) / 2 == Vector3(1, 1, 1))
+    assert(-Vector3(1, 1, 1) == Vector3(-1, -1, -1))
+    assert(len(Vector3()) == 3)
+    assert(Vector3(1, 1, 1).mag() == (3 ** 0.5))
+    assert(Vector3(1, 1, 1).mag2() == 3)
+    v = Vector3(0, 0, 10)
+    v.normalize()
+    assert(v == Vector3(0, 0, 1))
+    assert(Vector3(1, 2, 3).dot(Vector3(3, 2, 1)) == 10)
+    assert(Vector3(1, 0, 0).cross(Vector3(0, 1, 0)) == Vector3(0, 0, 1))
+    assert(Vector3(0, 1, 0).cross(Vector3(0, 0, 1)) == Vector3(1, 0, 0))
+    assert(Vector3(0, 1, 0).cross(Vector3(1, 0, 0)) == Vector3(0, 0, -1))
+    #todo: unit test Quaternion
+
+    #unit test Aabb
+    assert(Aabb().isEmpty())
+    assert(Aabb().min == Vector3(float("inf"), float("inf"), float("inf")))
+    assert(Aabb().max == Vector3(-float("inf"), -float("inf"), -float("inf")))
+    box = Aabb(Vector3(-1, -1, -1), Vector3(1, 1, 1))
+    assert(box.min == Vector3(-1, -1, -1))
+    assert(box.max == Vector3(1, 1, 1))
+    box = Aabb()
+    box.expand(Vector3(1, 0, -1), Vector3(-1, 1, 0), Vector3(0, -1, 1))
+    assert(box.min == Vector3(-1, -1, -1))
+    assert(box.max == Vector3(1, 1, 1))
+    assert(box.size() == Vector3(2, 2, 2))
+    assert(box.center() == Vector3())
+
+    #unit test Triangle
+    box = Aabb(Vector3(-2, -2, -2), Vector3(2, 2, 2))
+    tri = Triangle(Vector3(3, 0, 0), Vector3(0, 3, 0), Vector3(0, 0, 3))
+    assert(tri.testAabb(box))
+    box = Aabb(Vector3(-1, -1, -1), Vector3(1, 1, 1))
+    tri = Triangle(Vector3(4, 0, 0), Vector3(0, 4, 0), Vector3(0, 0, 4))
+    assert(not tri.testAabb(box))
+
+    pass