def collides(self, other):
"""Checks if this shape collides with another
Args:
other: the shape to test collision against
Returns:
True if the two shapes collide, False if they do not
"""
# Start with a bounding circle test, using the effective
# lengths as radii for the circles. If we don't at least
# pass this, then the shapes don't collide, and we skip
# doing the more computationally expensive collision test
if distance(self.pos, other.pos) > (self.effective_length +
other.effective_length):
return False
# We passed the bounding circle test, so we now need to do the
# more accurate test for collision, using the separating axis
# theorem
# First grab the transformed (world-space) vertices of each
# shape. Then, grab the axes, which are normalized vectors
# perpendicular to each side
verts1 = self._get_transformed_verts()
verts2 = other._get_transformed_verts()
axes1 = generate_axes(verts1)
axes2 = generate_axes(verts2)
# Loop over each set of axes, and project both shapes onto
# each axis. If they don't overlap on the axis, the shapes do
# not collide
for axis in axes1:
proj1 = project(verts1, axis)
proj2 = project(verts2, axis)
if not proj1.overlaps(proj2):
return False
for axis in axes2:
proj1 = project(verts1, axis)
proj2 = project(verts2, axis)
if not proj1.overlaps(proj2):
return False
# If we got this far, the shapes overlap on each axis,
# and the shapes collide
return True
def test_project(self):
v1 = np.array([[1, 2, 3], [4, 5, 6]])
v2 = np.array([[1, 0, 0], [0, 1, 0]])
correct = array([[1., 0., 0.], [0., 5., 0.]])
result = vector.project(v1, v2)
self.assertTrue(np.all(np.abs(result - correct) < self.delta))
# Test default "projection_type"
result = vector.project(v1, v2, projection_type='1D')
self.assertTrue(np.all(np.abs(result - correct) < self.delta))
# Test with lists
v1 = list(v1[0])
v2 = list(v2[0])
correct = correct[0]
result = vector.project(v1, v2)
self.assertTrue(np.all(np.abs(result - correct) < self.delta))
def test_project(self):
v1 = np.array([[1,2,3],
[4,5,6]])
v2 = np.array([[1,0,0],
[0,1,0]])
result = vector.project(v1,v2)
correct = array([[ 1., 0., 0.],
[ 0., 5., 0.]])
self.assertTrue(np.all(np.abs(result-correct)<self.delta))
def test_project(self):
v1 = np.array([[1,2,3],
[4,5,6]])
v2 = np.array([[1,0,0],
[0,1,0]])
result = vector.project(v1,v2)
correct = array([[ 1., 0., 0.],
[ 0., 5., 0.]])
self.assertTrue(np.all(np.abs(result-correct)<self.delta))
def test_project(self):
v1 = np.array([[1,2,3],
[4,5,6]])
v2 = np.array([[1,0,0],
[0,1,0]])
correct = array([[ 1., 0., 0.],
[ 0., 5., 0.]])
result = vector.project(v1,v2)
self.assertTrue(np.all(np.abs(result-correct)<self.delta))
# Test default "projection_type"
result = vector.project(v1,v2, projection_type='1D')
self.assertTrue(np.all(np.abs(result-correct)<self.delta))
# Test with lists
v1 = list(v1[0])
v2 = list(v2[0])
correct = correct[0]
result = vector.project(v1, v2)
self.assertTrue(np.all(np.abs(result-correct)<self.delta))
def FitPointsCompass(debug, points, current, norm):
# ensure current and norm are float
current = lmap(float, current)
norm = lmap(float, norm)
zpoints = [[], [], [], [], [], []]
for i in range(6):
zpoints[i] = lmap(lambda x : x[i], points)
# determine if we have 0D, 1D, 2D, or 3D set of points
point_fit, point_dev, point_max_dev = PointFit(points)
if point_max_dev < 9:
debug('0d fit, insufficient data %.1f %.1f < 9' % (point_dev, point_max_dev))
return False
line, plane = LinearFit(points)
line_fit, line_dev, line_max_dev = line
plane_fit, plane_dev, plane_max_dev = plane
# initial guess average min and max for bias, and average range for radius
minc = [1000, 1000, 1000]
maxc = [-1000, -1000, -1000]
for p in points:
minc = lmap(min, p[:3], minc)
maxc = lmap(max, p[:3], maxc)
guess = lmap(lambda a, b : (a+b)/2, minc, maxc)
diff = lmap(lambda a, b : b-a, minc, maxc)
guess.append((diff[0]+diff[1]+diff[2])/3)
#debug('initial guess', guess)
# initial is the closest to guess on the uv plane containing current
initial = vector.add(current[:3], vector.project(vector.sub(guess[:3], current[:3]), norm))
initial.append(current[3])
#debug('initial 1d fit', initial)
# attempt 'normal' fit along normal vector
'''
def f_sphere1(beta, x):
bias = lmap(lambda x, n: x + beta[0]*n, initial[:3], norm)
b = numpy.matrix(lmap(lambda a, b : a - b, x[:3], bias))
m = list(numpy.array(b.transpose()))
r0 = lmap(lambda y : beta[1] - vector.norm(y), m)
return r0
sphere1d_fit = FitLeastSq([0, initial[3]], f_sphere1, zpoints)
if not sphere1d_fit or sphere1d_fit[1] < 0:
print('FitLeastSq sphere1d failed!!!! ', len(points))
return False
sphere1d_fit = lmap(lambda x, n: x + sphere1d_fit[0]*n, initial[:3], norm) + [sphere1d_fit[1]]
debug('sphere1 fit', sphere1d_fit, ComputeDeviation(points, sphere1d_fit))
'''
def f_new_sphere1(beta, x):
bias = lmap(lambda x, n: x + beta[0]*n, initial[:3], norm)
b = numpy.matrix(lmap(lambda a, b : a - b, x[:3], bias))
m = list(numpy.array(b.transpose()))
r0 = lmap(lambda y : beta[1] - vector.norm(y), m)
g = list(numpy.array(numpy.matrix(x[3:]).transpose()))
fac = 1 # weight deviation
def dip(y, z):
n = min(max(vector.dot(y, z)/vector.norm(y), -1), 1)
return n
r1 = lmap(lambda y, z : fac*beta[1]*(beta[2]-dip(y, z)), m, g)
return r0 + r1
new_sphere1d_fit = FitLeastSq([0, initial[3], 0], f_new_sphere1, zpoints, debug, 2)
if not new_sphere1d_fit or new_sphere1d_fit[1] < 0 or abs(new_sphere1d_fit[2]) > 1:
debug('FitLeastSq new_sphere1 failed!!!! ', len(points), new_sphere1d_fit)
new_sphere1d_fit = current
else:
new_sphere1d_fit = lmap(lambda x, a: x + new_sphere1d_fit[0]*a, initial[:3], norm) + [new_sphere1d_fit[1], math.degrees(math.asin(new_sphere1d_fit[2]))]
new_sphere1d_fit = [new_sphere1d_fit, ComputeDeviation(points, new_sphere1d_fit), 1]
#print('new sphere1 fit', new_sphere1d_fit)
if line_max_dev < 2:
debug('line fit found, insufficient data %.1f %.1f' % (line_dev, line_max_dev))
return False
# 2d sphere fit across normal vector
u = vector.cross(norm, [norm[1]-norm[2], norm[2]-norm[0], norm[0]-norm[1]])
v = vector.cross(norm, u)
u = vector.normalize(u)
v = vector.normalize(v)
# initial is the closest to guess on the uv plane containing current
initial = vector.add(guess[:3], vector.project(vector.sub(current[:3], guess[:3]), norm))
initial.append(current[3])
#debug('initial 2d fit', initial)
'''
def f_sphere2(beta, x):
bias = lmap(lambda x, a, b: x + beta[0]*a + beta[1]*b, initial[:3], u, v)
b = numpy.matrix(lmap(lambda a, b : a - b, x[:3], bias))
m = list(numpy.array(b.transpose()))
r0 = lmap(lambda y : beta[2] - vector.norm(y), m)
return r0
sphere2d_fit = FitLeastSq([0, 0, initial[3]], f_sphere2, zpoints)
if not sphere2d_fit or sphere2d_fit[2] < 0:
print('FitLeastSq sphere2d failed!!!! ', len(points))
new_sphere2d_fit = initial
else:
sphere2d_fit = lmap(lambda x, a, b: x + sphere2d_fit[0]*a + sphere2d_fit[1]*b, initial[:3], u, v) + [sphere2d_fit[2]]
debug('sphere2 fit', sphere2d_fit, ComputeDeviation(points, sphere2d_fit))
'''
def f_new_sphere2(beta, x):
bias = lmap(lambda x, a, b: x + beta[0]*a + beta[1]*b, initial[:3], u, v)
b = numpy.matrix(lmap(lambda a, b : a - b, x[:3], bias))
m = list(numpy.array(b.transpose()))
r0 = lmap(lambda y : beta[2] - vector.norm(y), m)
#r0 = lmap(lambda y : 1 - vector.norm(y)/beta[2], m)
g = list(numpy.array(numpy.matrix(x[3:]).transpose()))
fac = 1 # weight deviation
def dip(y, z):
n = min(max(vector.dot(y, z)/vector.norm(y), -1), 1)
return n
r1 = lmap(lambda y, z : fac*beta[2]*(beta[3]-dip(y, z)), m, g)
return r0 + r1
new_sphere2d_fit = FitLeastSq([0, 0, initial[3], 0], f_new_sphere2, zpoints, debug, 2)
if not new_sphere2d_fit or new_sphere2d_fit[2] < 0 or abs(new_sphere2d_fit[3]) >= 1:
debug('FitLeastSq sphere2 failed!!!! ', len(points), new_sphere2d_fit)
return False
new_sphere2d_fit = lmap(lambda x, a, b: x + new_sphere2d_fit[0]*a + new_sphere2d_fit[1]*b, initial[:3], u, v) + [new_sphere2d_fit[2], math.degrees(math.asin(new_sphere2d_fit[3]))]
new_sphere2d_fit = [new_sphere2d_fit, ComputeDeviation(points, new_sphere2d_fit), 2]
if plane_max_dev < 1.2:
ang = math.degrees(math.asin(vector.norm(vector.cross(plane_fit[1], norm))))
debug('plane fit found, 2D fit only', ang, plane_fit, plane_dev, plane_max_dev)
if ang > 30:
debug('angle of plane not aligned to normal: no 2d fit')
new_sphere2d_fit = False
return [new_sphere1d_fit, new_sphere2d_fit, False]
# ok to use best guess for 3d fit
initial = guess
'''
def f_sphere3(beta, x):
bias = beta[:3]
b = numpy.matrix(lmap(lambda a, b : a - b, x[:3], bias))
m = list(numpy.array(b.transpose()))
r0 = lmap(lambda y : beta[3] - vector.norm(y), m)
return r0
sphere3d_fit = FitLeastSq(initial[:4], f_sphere3, zpoints)
if not sphere3d_fit or sphere3d_fit[3] < 0:
print('FitLeastSq sphere failed!!!! ', len(points))
return False
debug('sphere3 fit', sphere3d_fit, ComputeDeviation(points, sphere3d_fit))
'''
def f_new_sphere3(beta, x):
b = numpy.matrix(lmap(lambda a, b : a - b, x[:3], beta[:3]))
m = list(numpy.array(b.transpose()))
r0 = lmap(lambda y : beta[3] - vector.norm(y), m)
#r0 = lmap(lambda y : 1 - vector.norm(y)/beta[3], m)
g = list(numpy.array(numpy.matrix(x[3:]).transpose()))
fac = 1 # weight deviation
def dip(y, z):
n = min(max(vector.dot(y, z)/vector.norm(y), -1), 1)
return n
r1 = lmap(lambda y, z : fac*beta[3]*(beta[4]-dip(y, z)), m, g)
return r0 + r1
new_sphere3d_fit = FitLeastSq(initial[:4] + [0], f_new_sphere3, zpoints, debug, 2)
if not new_sphere3d_fit or new_sphere3d_fit[3] < 0 or abs(new_sphere3d_fit[4]) >= 1:
debug('FitLeastSq sphere3 failed!!!! ', len(points))
return False
new_sphere3d_fit[4] = math.degrees(math.asin(new_sphere3d_fit[4]))
new_sphere3d_fit = [new_sphere3d_fit, ComputeDeviation(points, new_sphere3d_fit), 3]
#debug('new sphere3 fit', new_sphere3d_fit)
return [new_sphere1d_fit, new_sphere2d_fit, new_sphere3d_fit]
def FitPoints(points, current, norm):
if len(points) < 5:
return False
# ensure current and norm are float
current = map(float, current)
norm = map(float, norm)
zpoints = [[], [], [], [], [], []]
for i in range(6):
zpoints[i] = map(lambda x: x[i], points)
# determine if we have 0D, 1D, 2D, or 3D set of points
point_fit, point_dev, point_max_dev = PointFit(points)
if point_max_dev < 9:
if debug:
print '0d fit, insufficient data', point_dev, point_max_dev, '< 9'
return False
line, plane = LinearFit(points)
line_fit, line_dev, line_max_dev = line
plane_fit, plane_dev, plane_max_dev = plane
# initial guess average min and max for bias, and average range for radius
minc = [1000, 1000, 1000]
maxc = [-1000, -1000, -1000]
for p in points:
minc = map(min, p[:3], minc)
maxc = map(max, p[:3], maxc)
guess = map(lambda a, b: (a + b) / 2, minc, maxc)
diff = map(lambda a, b: b - a, minc, maxc)
guess.append((diff[0] + diff[1] + diff[2]) / 3)
if debug:
print 'initial guess', guess
# initial is the closest to guess on the uv plane containing current
initial = vector.add(
current[:3], vector.project(vector.sub(guess[:3], current[:3]), norm))
initial.append(current[3])
if debug:
print 'initial 1d fit', initial
# attempt 'normal' fit along normal vector
'''
def f_sphere1(beta, x):
bias = map(lambda x, n: x + beta[0]*n, initial[:3], norm)
b = numpy.matrix(map(lambda a, b : a - b, x[:3], bias))
m = list(numpy.array(b.transpose()))
r0 = map(lambda y : beta[1] - vector.norm(y), m)
return r0
sphere1d_fit = FitLeastSq([0, initial[3]], f_sphere1, zpoints)
if not sphere1d_fit or sphere1d_fit[1] < 0:
print 'FitLeastSq sphere1d failed!!!! ', len(points)
return False
sphere1d_fit = map(lambda x, n: x + sphere1d_fit[0]*n, initial[:3], norm) + [sphere1d_fit[1]]
if debug:
print 'sphere1 fit', sphere1d_fit, ComputeDeviation(points, sphere1d_fit)
'''
def f_new_sphere1(beta, x):
bias = map(lambda x, n: x + beta[0] * n, initial[:3], norm)
b = numpy.matrix(map(lambda a, b: a - b, x[:3], bias))
m = list(numpy.array(b.transpose()))
r0 = map(lambda y: beta[1] - vector.norm(y), m)
g = list(numpy.array(numpy.matrix(x[3:]).transpose()))
fac = .03 # weight deviation as 1 degree ~ .03 mag
r1 = map(
lambda y, z: fac * beta[1] * (beta[2] - math.degrees(
math.asin(vector.dot(y, z) / vector.norm(y)))), m, g)
return r0 + r1
new_sphere1d_fit = FitLeastSq([0, initial[3], 0], f_new_sphere1, zpoints,
2)
if not new_sphere1d_fit or new_sphere1d_fit[1] < 0:
if debug:
print 'FitLeastSq new_sphere1 failed!!!! ', len(points)
new_sphere1d_fit = current
else:
new_sphere1d_fit = map(lambda x, a: x + new_sphere1d_fit[0] * a,
initial[:3], norm) + new_sphere1d_fit[1:]
new_sphere1d_fit = [
new_sphere1d_fit,
ComputeDeviation(points, new_sphere1d_fit), 1
]
#print 'new sphere1 fit', new_sphere1d_fit
if line_max_dev < 3:
if debug:
print 'line fit found, insufficient data', line_dev, line_max_dev
return [new_sphere1d_fit, False, False]
# 2d sphere fit across normal vector
u = vector.cross(norm,
[norm[1] - norm[2], norm[2] - norm[0], norm[0] - norm[1]])
v = vector.cross(norm, u)
u = vector.normalize(u)
v = vector.normalize(v)
# initial is the closest to guess on the uv plane containing current
initial = vector.add(
guess[:3], vector.project(vector.sub(current[:3], guess[:3]), norm))
initial.append(current[3])
if debug:
print 'initial 2d fit', initial
'''
def f_sphere2(beta, x):
bias = map(lambda x, a, b: x + beta[0]*a + beta[1]*b, initial[:3], u, v)
b = numpy.matrix(map(lambda a, b : a - b, x[:3], bias))
m = list(numpy.array(b.transpose()))
r0 = map(lambda y : beta[2] - vector.norm(y), m)
return r0
sphere2d_fit = FitLeastSq([0, 0, initial[3]], f_sphere2, zpoints)
if not sphere2d_fit or sphere2d_fit[2] < 0:
print 'FitLeastSq sphere2d failed!!!! ', len(points)
new_sphere2d_fit = initial
else:
sphere2d_fit = map(lambda x, a, b: x + sphere2d_fit[0]*a + sphere2d_fit[1]*b, initial[:3], u, v) + [sphere2d_fit[2]]
if debug:
print 'sphere2 fit', sphere2d_fit, ComputeDeviation(points, sphere2d_fit)
'''
def f_new_sphere2(beta, x):
bias = map(lambda x, a, b: x + beta[0] * a + beta[1] * b, initial[:3],
u, v)
b = numpy.matrix(map(lambda a, b: a - b, x[:3], bias))
m = list(numpy.array(b.transpose()))
r0 = map(lambda y: beta[2] - vector.norm(y), m)
g = list(numpy.array(numpy.matrix(x[3:]).transpose()))
fac = .03 # weight deviation as 1 degree ~ .03 mag
r1 = map(
lambda y, z: fac * beta[2] * (beta[3] - math.degrees(
math.asin(vector.dot(y, z) / vector.norm(y)))), m, g)
return r0 + r1
new_sphere2d_fit = FitLeastSq([0, 0, initial[3], 0], f_new_sphere2,
zpoints, 2)
if not new_sphere2d_fit or new_sphere2d_fit[2] < 0:
if debug:
print 'FitLeastSq sphere failed!!!! ', len(points)
return False
new_sphere2d_fit = map(
lambda x, a, b: x + new_sphere2d_fit[0] * a + new_sphere2d_fit[1] * b,
initial[:3], u, v) + new_sphere2d_fit[2:]
new_sphere2d_fit = [
new_sphere2d_fit,
ComputeDeviation(points, new_sphere2d_fit), 2
]
#print 'new sphere2 fit', new_sphere2d_fit
if plane_max_dev < 1.2:
if debug:
print 'plane fit found, 2D fit only', plane_fit, plane_dev, plane_max_dev
return [new_sphere1d_fit, new_sphere2d_fit, False]
# ok to use best guess for 3d fit
initial = guess
'''
def f_sphere3(beta, x):
bias = beta[:3]
b = numpy.matrix(map(lambda a, b : a - b, x[:3], bias))
m = list(numpy.array(b.transpose()))
r0 = map(lambda y : beta[3] - vector.norm(y), m)
return r0
sphere3d_fit = FitLeastSq(initial[:4], f_sphere3, zpoints)
if not sphere3d_fit or sphere3d_fit[3] < 0:
print 'FitLeastSq sphere failed!!!! ', len(points)
return False
if debug:
print 'sphere3 fit', sphere3d_fit, ComputeDeviation(points, sphere3d_fit)
'''
def f_new_sphere3(beta, x):
b = numpy.matrix(map(lambda a, b: a - b, x[:3], beta[:3]))
m = list(numpy.array(b.transpose()))
r0 = map(lambda y: beta[3] - vector.norm(y), m)
g = list(numpy.array(numpy.matrix(x[3:]).transpose()))
fac = .03 # weight deviation as 1 degree ~ .03 mag
r1 = map(
lambda y, z: fac * beta[3] * (beta[4] - math.degrees(
math.asin(vector.dot(y, z) / vector.norm(y)))), m, g)
return r0 + r1
new_sphere3d_fit = FitLeastSq(initial[:4] + [0], f_new_sphere3, zpoints, 2)
if not new_sphere3d_fit or new_sphere3d_fit[3] < 0:
if debug:
print 'FitLeastSq sphere failed!!!! ', len(points)
return False
new_sphere3d_fit = [
new_sphere3d_fit,
ComputeDeviation(points, new_sphere3d_fit), 3
]
#print 'new sphere3 fit', new_sphere3d_fit
return [new_sphere1d_fit, new_sphere2d_fit, new_sphere3d_fit]