def test_tangent_vector(self): "vector tangent to unitcircle" circ = Circle() pos = Vec2D(1, 0) self.assertTrue(circ.tangent_vector(pos, 1) == Vec2D(0, 1))
def test_overlapping_intersection(self): "test that overlapping counts as intersection" tan1 = Tangent(Vec2D(-1, 0), None, None, Vec2D(1, 0), None, None) tan2 = Tangent(Vec2D(-2, 0), None, None, Vec2D(0, 0), None, None) self.assertTrue(tan1.intersect(tan2)) self.assertTrue(tan2.intersect(tan1))
def test_sort_points_singleelement(self): "test sort_points_on_circle one element list" sorted_list = pp.sort_points_on_circle([Vec2D(1, 0)], Circle()) self.assertTrue(len(sorted_list) == 1) self.assertTrue(sorted_list[0] == Vec2D(1, 0))
def test_general_intersection(self): "test non intersecting segments" tan1 = Tangent(Vec2D(1, 0), None, None, Vec2D(2, 0), None, None) tan2 = Tangent(Vec2D(0, -2), None, None, Vec2D(0, -1), None, None) self.assertFalse(tan1.intersect(tan2)) self.assertFalse(tan2.intersect(tan1))
def test_parallel_intersection(self): "test that parallel segments don't intersect" tan1 = Tangent(Vec2D(1, 0), None, None, Vec2D(2, 0), None, None) tan2 = Tangent(Vec2D(1, 1), None, None, Vec2D(2, 1), None, None) self.assertFalse(tan1.intersect(tan2)) self.assertFalse(tan2.intersect(tan1))
def test_tangents_containing_circle(self): "test circle tangents when one circle contains another" circle1 = Circle(Vec2D(), 10) circle2 = Circle(Vec2D(), 5) self.assertTrue(len(circle1.tangent_circle(circle2)) == 0) self.assertTrue(len(circle2.tangent_circle(circle1)) == 0)
def test_radial_acc(self): "test radial_acc method" circle = Circle() seg = CircleSegment(Vec2D(1, 0), Vec2D(0, 1), circle, 1) self.assertTrue(seg.radial_acc(Vec2D(1, 0), 1) == Vec2D(-1, 0))
def test_simple_intersection(self): "test simple 1 point intersection" tan1 = Tangent(Vec2D(-1, 0), None, None, Vec2D(1, 0), None, None) tan2 = Tangent(Vec2D(0, -1), None, None, Vec2D(0, 1), None, None) self.assertTrue(tan1.intersect(tan2)) self.assertTrue(tan2.intersect(tan1))
def test_contains_tangent(self): "test start and end inside circle" circle = Circle() tangent = Tangent(Vec2D(0, -0.5), None, None, Vec2D(0, 0.5), None, None) self.assertFalse(circle.intersects_tangent(tangent))
def test_can_transform_speed(self): traj = [(Vec2D(), Vec2D(0., 1.), Vec2D(), 0.3)] t = convert_from_molly(traj, start_time=10., sampling_time=0.1) t = t.points[0] self.assertAlmostEqual(t.speed, 1., 5) self.assertAlmostEqual(t.theta, pi / 2, 3)
def test_can_transform_acceleration(self): # Circular trajectory, a = v ** 2 / r, v = 2, r = 1 # -> a = 4 m / s^2, omega = 2 rad / s traj = [(Vec2D(), Vec2D(2., 0.), Vec2D(0., 4), 0.3)] t = convert_from_molly(traj, start_time=10., sampling_time=0.1) t = t.points[0] self.assertAlmostEqual(t.omega, 2., 3)
def test_orientangle_simple(self): vec1 = Vec2D(1, 0) vec2 = Vec2D(math.sqrt(2.0) / 2, math.sqrt(2.0) / 2) self.assertAlmostEqual(vec1.oriented_angle(vec2), 0.25 * math.pi) self.assertAlmostEqual(vec2.oriented_angle(vec1), 1.75 * math.pi)
def test_colinear_nonoverlapping(self): "test that colinear nonoverlapping doesn't count as intersection" tan1 = Tangent(Vec2D(1, 0), None, None, Vec2D(2, 0), None, None) tan2 = Tangent(Vec2D(-2, 0), None, None, Vec2D(0, 0), None, None) self.assertFalse(tan1.intersect(tan2)) self.assertFalse(tan2.intersect(tan1))
def __init__(self, max_acc=1.6, max_v=0.6, time_resolution=0.1, static_poly_obs=[STAIRS], static_circ_obs=[], obs_min_r=0.1, playground_dim=(3.0, 2.0)): self.max_acc = max_acc self.max_v = max_v self.obs_min_r = obs_min_r (self.width, self.height) = playground_dim self.static_poly_obs = static_poly_obs self.static_circ_obs = static_circ_obs self.time_resolution = time_resolution corner1 = Vec2D(0, 0) corner2 = Vec2D(self.width, 0) corner3 = Vec2D(self.width, self.height) corner4 = Vec2D(0, self.height) border1 = Tangent(corner1, None, 0, corner2, None, 0) border2 = Tangent(corner2, None, 0, corner3, None, 0) border3 = Tangent(corner3, None, 0, corner4, None, 0) border4 = Tangent(corner4, None, 0, corner1, None, 0) self.bounds = [border1, border2, border3, border4]
def test_orientation_colinear(self): "test that orientation returns (almost) zero for colinear points" vec1 = Vec2D(0, 0) vec2 = Vec2D(1, 0) vec3 = Vec2D(2, 0) self.assertAlmostEqual(0, Vec2D.orientation(vec1, vec2, vec3))
def test_self_tangent(self): "test if tangent is part of circle" circle1 = Circle() circle2 = Circle(Vec2D(0, 2), 1) tan = Tangent(Vec2D(0, 1), circle1, None, Vec2D(0, 2), circle2, None) self.assertFalse(circle1.intersects_tangent(tan)) self.assertFalse(circle2.intersects_tangent(tan))
def test_next_pos(self): "test next_pos method" circle = Circle() seg = CircleSegment(Vec2D(1, 0), Vec2D(0, 1), circle, 1) self.assertTrue(seg.next_pos(Vec2D(1, 0), pi * 0.5) == Vec2D(0, 1))
def tangent_vector(self, point, orientation): "tangent vector to self at point with orientation" vec = (point - self.pos).normalized() if orientation < 0: return Vec2D(vec.pos_y, -vec.pos_x) else: return Vec2D(-vec.pos_y, vec.pos_x)
def test_negative_orientation(self): "test that orientation returns postive value for positively oriented \ points" vec1 = Vec2D(0, 0) vec2 = Vec2D(0, 1) vec3 = Vec2D(1, 0) self.assertTrue(Vec2D.orientation(vec1, vec2, vec3) < 0)
def test_approximate_equality(self): "vectors within Vec2D.EPSILON distance of each other should count as \ equal" vec = Vec2D(1, 1) approx = vec + Vec2D(1, 0) * (0.9 * Vec2D.EPSILON) self.assertTrue(vec == approx) self.assertFalse(vec != approx)
def test_neighs_circle_one_element(self): "single point has no neighbours" points = [Vec2D(1, 0)] circle = Circle() (pos, neg) = pp.neighbours_on_circle(points, circle, Vec2D(1, 0)) self.assertTrue(pos is None) self.assertTrue(neg is None)
def test_rotate_around_point(self): "rotate around some point" vec = Vec2D() center = Vec2D(-1, 0) rot = vec.rotate(math.pi / 4, center) self.assertAlmostEqual(1.0 / math.sqrt(2) - 1, rot.pos_x) self.assertAlmostEqual(1.0 / math.sqrt(2), rot.pos_y)
def test_can_transform_position_timestamp(self): traj = [(Vec2D(1., 2.), Vec2D(), Vec2D(), 0.3)] traj = convert_from_molly(traj, start_time=10., sampling_time=0.1) self.assertEqual(traj.start_time, 10.) self.assertEqual(traj.sampling_time, .1) t = traj.points[0] self.assertEqual(t.x, 1.) self.assertEqual(t.y, 2.)
def test_equality(self): "test equality (and implicitely non-equality) of cirlces" circle1 = Circle(Vec2D(1, 0), 1.0) circle2 = Circle(Vec2D(1, 0.9 * Vec2D.EPSILON), 1.0 + 0.9 * Vec2D.EPSILON) self.assertTrue(circle1 == circle2) self.assertTrue(circle2 == circle1) # equality is symmetric self.assertFalse(circle1 != circle2) self.assertFalse(circle2 != circle1)
def test_length(self): "test length method" circle = Circle() inv_rt2 = sqrt(2.0) / 2.0 seg1 = CircleSegment(Vec2D(1, 0), Vec2D(inv_rt2, inv_rt2), circle, 1) seg2 = CircleSegment(Vec2D(1, 0), Vec2D(inv_rt2, inv_rt2), circle, -1) self.assertAlmostEqual(seg1.length(), pi * 0.25, delta=Vec2D.EPSILON) self.assertAlmostEqual(seg2.length(), pi * 1.75, delta=Vec2D.EPSILON)
def test_translate_zero(self): "test that tangent equals itself after translation of (0, 0)" circ1 = Circle() circ2 = Circle(Vec2D(4, 0)) tan = Tangent(Vec2D(0, 1), circ1, 1, Vec2D(4, 1), circ2, -1) translated = tan + Vec2D() self.assertTrue(tan == translated) self.assertTrue(translated == tan)
def test_start_circles(self): "test generation of starting circles" pos = Vec2D() direction = Vec2D(0, 1) pos_circ = Circle(Vec2D(-1, 0), 1) neg_circ = Circle(Vec2D(1, 0), 1) (test_pos, test_neg) = pp.get_start_circles(pos, direction, 1) self.assertTrue(test_pos == pos_circ) self.assertTrue(test_neg == neg_circ)
def test_cross_product(self): "test the cross product of 2 vectors" posx1 = 1.0 posy1 = 2.0 posx2 = 3.0 posy2 = 4.0 vec1 = Vec2D(posx1, posy1) vec2 = Vec2D(posx2, posy2) expected = posx1 * posy2 - posy1 * posx2 self.assertAlmostEqual(expected, vec1.cross(vec2)) self.assertAlmostEqual(vec1.cross(vec2), -vec2.cross(vec1))
def _circle_tangents(self, other, invert_second_radius): "compute tangents as explained in \ https://en.wikipedia.org/wiki/Tangent_lines_to_circles#Analytic_geometry" dx = other.pos.pos_x - self.pos.pos_x dy = other.pos.pos_y - self.pos.pos_y if not invert_second_radius: dr = other.radius - self.radius else: dr = - other.radius - self.radius d = sqrt(dx*dx + dy*dy) x = dx/d y = dy/d r = dr/d a1 = r*x - y*sqrt(1 - r*r) b1 = r*y + x*sqrt(1 - r*r) c1 = self.radius - (a1*self.pos.pos_x + b1*self.pos.pos_y) a2 = r*x + y*sqrt(1 - r*r) b2 = r*y - x*sqrt(1 - r*r) c2 = self.radius - (a2*self.pos.pos_x + b2*self.pos.pos_y) x11 = (b1*(b1*self.pos.pos_x - a1*self.pos.pos_y) - a1*c1) y11 = (a1*(-b1*self.pos.pos_x + a1*self.pos.pos_y) - b1*c1) x12 = (b1*(b1*other.pos.pos_x - a1*other.pos.pos_y) - a1*c1) y12 = (a1*(-b1*other.pos.pos_x + a1*other.pos.pos_y) - b1*c1) x21 = (b2*(b2*self.pos.pos_x - a2*self.pos.pos_y) - a2*c2) y21 = (a2*(-b2*self.pos.pos_x + a2*self.pos.pos_y) - b2*c2) x22 = (b2*(b2*other.pos.pos_x - a2*other.pos.pos_y) - a2*c2) y22 = (a2*(-b2*other.pos.pos_x + a2*other.pos.pos_y) - b2*c2) start1 = Vec2D(x11, y11) end1 = Vec2D(x12, y12) orient11 = Vec2D.orientation(end1, start1, self.pos) orient12 = Vec2D.orientation(start1, end1, other.pos) start2 = Vec2D(x21, y21) end2 = Vec2D(x22, y22) orient21 = Vec2D.orientation(end2, start2, self.pos) orient22 = Vec2D.orientation(start2, end2, other.pos) tan1 = Tangent(start1, self, orient11, end1, other, orient12) tan2 = Tangent(start2, self, orient21, end2, other, orient22) return [tan1, tan2]
def test_neighs_circle_general(self): "point has two distinct neighbours" point1 = Vec2D(1, 0) point2 = point1.rotate(pi / 2) point3 = point1.rotate(pi) point4 = point1.rotate(-pi / 2) unsorted = [point2, point4, point3, point1] (pos, neg) = pp.neighbours_on_circle(unsorted, Circle(), Vec2D(1, 0)) self.assertTrue(pos == point2) self.assertTrue(neg == point4)
def test_all_tangents_simple1(self): "test that all_tangents returns all tangents of two circles \ when only those 2 circles are present" circ1 = Circle(Vec2D(-2, 0), 1) circ2 = Circle(Vec2D(2, 0), 1) tangents = circ1.tangent_circle(circ2) to_test = pp.all_tangents([circ1, circ2], []) verificator = all(test in tangents for test in to_test) self.assertTrue(verificator)
def sort_points_on_circle(points, circle): "sort points by their angle on the circle" if not points: return [] reference = points[0] rest = points[1: len(points)] sorted_points = sorted(rest, key=lambda p: Vec2D.orientation(reference, circle.pos, p)) return [reference] + sorted_points