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]]) 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]