Now rebuilds the PolyGrid data when writing a .geo file.
Added simple 3D vector math classes.master
TigerKat
5 years ago
parent
7a7dce51ed
commit
d56d1cc21c
@ -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 + " ")
|
||||
@ -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)
|
||||
|
||||
@ -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
|
||||
Loading…
Reference in New Issue