def floodFill(self, parentIdx):
parent = self.samples[parentIdx[1]][parentIdx[0]]
if not parent.inGrid:
return
parent.inGrid = False
self.planeCluster.append(parent)
if parent.gridIdx[0] > 0:
childIdx = parent.gridIdx - Vector(1, 0)
child = self.samples[childIdx[1]][childIdx[0]]
if parent.distance(child) < self.config["floodThreshold"]:
self.floodFill(childIdx)
if parent.gridIdx[0] < self.config["samplesX"] - 1:
childIdx = parent.gridIdx + Vector(1, 0)
child = self.samples[childIdx[1]][childIdx[0]]
if parent.distance(child) < self.config["floodThreshold"]:
self.floodFill(childIdx)
if parent.gridIdx[1] > 0:
childIdx = parent.gridIdx - Vector(0, 1)
child = self.samples[childIdx[1]][childIdx[0]]
if parent.distance(child) < self.config["floodThreshold"]:
self.floodFill(childIdx)
if parent.gridIdx[1] < self.config["samplesY"] - 1:
childIdx = parent.gridIdx + Vector(0, 1)
child = self.samples[childIdx[1]][childIdx[0]]
if parent.distance(child) < self.config["floodThreshold"]:
self.floodFill(childIdx)
def __init__(self):
self.imagePx = Vector(0, 0)
self.bufferIdx = 0
self.gridIdx = Vector(0, 0)
self.p = Vector(0, 0, 0) #point p that describes a plane with normal n
self.n = Vector(0, 0, 0)
self.angle = 0.0
self.inGrid = True
self.clusterId = -1
def initialize(self):
self.samples.clear()
for k in range(0, self.config["samplesY"]):
V = []
for l in range(0, self.config["samplesX"]):
i = l * (self.config["IMAGE_WIDTH"]-1) / (self.config["samplesX"]-1)
j = self.config["IMAGE_HEIGHT"]-1-k * (self.config["IMAGE_HEIGHT"]-1) / (self.config["samplesY"]-1)
sample = Sample()
sample.gridIdx = Vector(l, k)
sample.imagePx = Vector(i, j)
sample.bufferIdx = int(i+j * self.config["IMAGE_WIDTH"])
V.append(sample)
self.samples.append(V)
def transformFromGroundPlane(n, p):
matrix = []#np.zeros((4,4))
for i in range(0,4):
matrix.append([0,0,0,0])
up = Vector(0,0,1)
axis = n^up
axis = axis.normalize()
angle = n.angleTo(up)
z = n*(-p)
c = math.cos(angle)
s = math.sin(angle)
t = 1.0 -c
matrix[0][0] = c + axis[0]*axis[0]*t
matrix[1][1] = c + axis[1] * axis[1] * t
matrix[2][2] = c + axis[2] * axis[2] * t
tmp1 = axis[0]*axis[1]*t
tmp2 = axis[2]*s
matrix[0][1] = tmp1 + tmp2
matrix[1][0] = tmp1 - tmp2
tmp1 = axis[0]*axis[2]*t
tmp2 = axis[1] *s
matrix[1][2] = tmp1 - tmp2
matrix[2][1] = tmp1 + tmp2
matrix[3][0] = 0.0
matrix[3][1] = 0.0
matrix[3][2] = z
matrix[0][3] = 0.0
matrix[1][3] = 0.0
matrix[2][3] = 0.0
matrix[3][3] = 1.0
return matrix
def update(self,points):
block_sum = 0
start = time.time()
if self.upVector* self.floorPlane.n > 0.5:
self.setUpVector(self.floorPlane.n)
else:
pass
for i in range(0, len(self.samples)):
for j in range(0, len(self.samples[i])):
self.samples[i][j].p = Vector(*(points[self.samples[i][j].bufferIdx].tolist()))
self.samples[i][j].inGrid = self.samples[i][j].p.any()
#normal = np.zeros((3,1))
end = time.time()
print("Part 1 took: " + str((end - start) * 1000) + "ms")
start = time.time()
for i in range(0, len(self.samples)):
for j in range(0, len(self.samples[i])):
if not self.samples[i][j].inGrid:
continue
upIdx = i + 1
if i == len(self.samples) - 1 or not self.samples[upIdx][j].inGrid:
upIdx = i
downIdx = i - 1
if i == 0 or not self.samples[downIdx][j].inGrid:
downIdx = i
rightIdx = j + 1
if j == len(self.samples[i]) - 1 or not self.samples[i][rightIdx].inGrid:
rightIdx = j
leftIdx = j - 1
if j == 0 or not self.samples[i][leftIdx].inGrid:
leftIdx = j
if upIdx == downIdx or leftIdx == rightIdx:
continue
up = self.samples[upIdx][j]
down = self.samples[downIdx][j]
right = self.samples[i][rightIdx]
left = self.samples[i][leftIdx]
start = time.time()
normal = ((up.p - down.p) ^ (right.p - left.p))
normal = -normal
normal = normal.normalize()
self.samples[i][j].n = normal
end =time.time()
block_sum += (end-start)
end = time.time()
print("Critical block took: " + str(block_sum * 1000) + "ms")
def __init__(self,config):
self.samples = []
self.prunedSamples = []
self.planes = []
self.planeAvg = []
self.floorSegment = []
self.floorPlane = Sample()
self.planeCluster = []
self.upVector = Vector(0,0,0)
self.upVector[2] = 1
self.config = config
self.floorPlane.n = self.upVector
def __init__(self, vector=Vector(0, 0)):
sprite = AnimatedSprite('player/founder.png', vector)
sprite.addAnimationState('idle', 0, 3, 2)
# sprite.addAnimationState( 'idle-fidget', 4, 7, 4 )
# sprite.addAnimationState( 'walk-left', 8, 11, 4 )
# sprite.addAnimationState( 'walk-right', 12, 15, 4 )
# sprite.addAnimationState( 'idle-chat', 16, 19, 4 )
sprite.setAnimationState('idle')
self.setSprite(sprite)
def test_default_vector_add(self):
v1 = Vector(3, 2)
v2 = Vector(2, 3)
self.assertEqual("Vector (5, 5)", str(v1+v2))
def findFloor(self):
start = time.time()
self.prune()
if len(self.prunedSamples) < 2:
return self.floorPlane
#sort by z coordinate of point
self.prunedSamples.sort(key=lambda x: x.p[2])#*self.upVector,reverse=True)
#self.prunedSamples.sort()
self.planes.clear()
self.planeAvg.clear()
self.floorSegment.clear()
self.floorPlane = self.prunedSamples[1]
end = time.time()
print("Pruning: " + str((end - start) * 1000) + "ms")
start = time.time()
for i in range(2, len(self.prunedSamples) - 1):
if not self.isIn(self.prunedSamples[i].gridIdx):
continue
self.planeCluster.clear()
self.floodFill(self.prunedSamples[i].gridIdx)
if len(self.planeCluster) == 1:
continue
avg = Sample()
avg.n[2] = 0
for j in range(0, len(self.planeCluster)):
avg += self.planeCluster[j]
avg /= float(len(self.planeCluster))
self.planeAvg.append(avg)
self.planes.append(self.planeCluster)
if self.floorPlane.distance(avg) < self.config["mergeThreshold"]:
self.floorPlane.p = (self.floorPlane.p * len(self.floorSegment) + avg.p * len(self.planeCluster)) / (len(self.floorSegment) + len(self.planeCluster))
self.floorPlane.n = (self.floorPlane.n * len(self.floorSegment) + avg.n * len(self.planeCluster)) / (len(self.floorSegment) + len(self.planeCluster))
self.floorPlane.n = self.floorPlane.n.normalize()
self.floorSegment.extend(self.planeCluster)
if len(self.floorSegment) * 20 < len(self.planeCluster):
self.floorPlane = avg
self.floorSegment.clear()
self.floorSegment.extend(self.planeCluster)
end = time.time()
print("floodfill: " + str((end - start) * 1000) + "ms")
if len(self.floorSegment) > 2:
start = time.time()
data_x = np.array([np.array([x.p[0], x.p[1],x.p[2]]) for x in self.floorSegment])
data_y = np.array([x.p[2] for x in self.floorSegment])
solution = np.linalg.lstsq(data_x,data_y, rcond=None)[0]
self.floorPlane.p[2] = self.floorPlane.p[0] * solution[0] + self.floorPlane.p[1] * solution[1] + solution[2]
self.floorPlane.n = Vector(-solution[0], -solution[1], 1).normalize()
end = time.time()
print("leastsq: " + str((end - start) * 1000) + "ms")
# start = time.time()
# data = np.array([x.p for x in self.floorSegment]).transpose()
# p, n = self.planeFit(data)
# end = time.time()
# print("SVD: " + str((end - start) * 1000) + "ms")
# self.floorPlane.p = Vector(*(p.tolist()))
# self.floorPlane.n = Vector(*(n.tolist())).normalize()
return self.floorPlane
class Sample:
up = Vector(0, 0, 0)
def __init__(self):
self.imagePx = Vector(0, 0)
self.bufferIdx = 0
self.gridIdx = Vector(0, 0)
self.p = Vector(0, 0, 0) #point p that describes a plane with normal n
self.n = Vector(0, 0, 0)
self.angle = 0.0
self.inGrid = True
self.clusterId = -1
def __gt__(self, other):
return Sample.up * self.p > Sample.up * other.p
def __lt__(self, other):
return Sample.up * self.p < Sample.up * other.p
def __eq__(self, other):
return self.bufferIdx == other.bufferIdx
def __iadd__(self, other):
self.n += other.n
self.p += other.p
return self
def __idiv__(self, other):
if type(other) == float:
self.n /= other
self.n = self.n.normalize()
self.p /= other
return self
else:
pass
def __truediv__(self, other):
if type(other) == float:
self.n /= other
self.n = self.n.normalize()
self.p /= other
return self
else:
pass
def __rdiv__(self, other):
if type(other) == float:
self.n /= other
self.n = self.n.normalize()
self.p /= other
return self
else:
pass
def distance(self, other):
d1 = math.fabs(self.n * (other.p - self.p))
d2 = math.fabs(other.n * (other.p - self.p))
#d3 = 1.0-np.dot(np.squeeze(np.asarray(self.n)), np.squeeze(np.asarray(other.n)))
return d1 + d2
def plane_distance(self, vec):
return self.n * (vec - self.p)
#vec is vec2d here
def evaluateAt(self, vec):
return (self.p * self.n - vec[0] * self.n[0] -
vec.y * self.n[2]) / self.n[3]
#vec is vec3d
def projectTo(self, vec):
return (v.x, v.y, self.evaluateAt(vec))