def test_scalar_projection(self):
"""Unit test for Vector.scalar_projection_on."""
tests = (((3, -8), (1, 2), (-13 * sqrt(1 / 5))), ((-1, 5), (2, 4),
(9 * sqrt(1 / 5))))
for test in tests:
result = Point(*test[0]).scalar_projection_on(Point(*test[1]))
self.assertAlmostEqual(result, test[2])
def test_projection(self):
"""Unit test for Vector.projection_on."""
tests = (((3, -8), (1, 2), (-13 / 5, -26 / 5)), ((-1, 5), (2, 4),
(9 / 5, 18 / 5)))
for test in tests:
result = Point(*test[0]).projection_on(Point(*test[1]))
self.assertAlmostEqual(result, Point(*test[2]))
def test_midpoint(self):
"""Unit test for midpoint."""
tests = (((-1, 2), (1, -2), (0, 0)), ((1, 1), (1, 1), (1, 1)),
((-4, 2), (1, 1), (-3 / 2, 3 / 2)))
for test in tests:
result = midpoint(Point(*test[0]), Point(*test[1]))
self.assertAlmostEqual(result, Point(*test[2]))
def __init__(self, parent: NodePath):
super().__init__()
self.parent = parent
self.mouse_np = p3d.camera.attach_new_node(PandaNode('mouse'))
self.mouse_np.set_y(p3d.lens.get_near())
picker_node = CollisionNode('mouse_ray')
picker_np = p3d.camera.attach_new_node(picker_node)
self._picker_ray = CollisionRay()
picker_node.add_solid(self._picker_ray)
self._collision_handler = CollisionHandlerQueue()
self._traverser = CollisionTraverser('mouse_traverser')
self._traverser.add_collider(picker_np, self._collision_handler)
self.actor = Actor(
resource_filename('tsim', 'data/models/cursor'),
{'spin': resource_filename('tsim', 'data/models/cursor-spin')})
self.actor.loop('spin')
self.actor.reparent_to(parent)
self.actor.set_pos(0.0, 0.0, 0.0)
self.actor.set_shader_off()
self._position = Point(0.0, 0.0)
self.last_position = self._position
self.moved = False
self.pointed_at = None
self._tool: Optional[Tool] = None
self._register_events()
def test_dijkstra(self):
"""Test the dijkstra algorithm implementation.
Generate a size x size grid of ways and find use dijkstra on the first
oriented way. Check that the first oriented way points to None and the
others all reach the first node with a valid path.
"""
size = 3
position = list(product(range(0, 100 * size, 100), repeat=2))
nodes = [Node(Point(*p)) for p in position]
ways = []
for i, node in enumerate(nodes):
if i % size != size - 1:
ways.append(Way(node, nodes[i + 1], (1, 1)))
if i < len(nodes) - size:
ways.append(Way(node, nodes[i + size], (1, 1)))
oriented_ways = [w.oriented() for w in ways]
oriented_ways.extend(
w.oriented(Endpoint.END) for w in ways if w.lane_count[1] > 0)
single_source = dijkstra(oriented_ways[0])
for way, (_, depth) in single_source.items():
depth += 1
way_ = way
while way_ is not oriented_ways[0]:
way_, depth_ = single_source[way_]
self.assertEqual(depth_, depth - 1)
depth = depth_
self.assertEqual(depth, 1)
way_, depth_ = single_source[way_]
self.assertEqual(depth_, 0)
self.assertIsNone(way_)
def evaluate_position(self, position: float,
curve_override: Optional[bezier.Curve] = None) \
-> Point:
"""Evaluate bezier curve at t=position/length."""
curve = self.curve if curve_override is None else curve_override
return Point(*chain.from_iterable(
curve.evaluate(position / self.length)))
def world_position(self, position: float) -> Tuple[Point, Vector]:
"""Get world position and direction in the given curve position."""
param = position / self.length
params = ((param, (position + 0.1) / self.length)
if position < 0.1 else
((position - 0.1) / self.length, param))
evaluated = self.curve.evaluate_multi(numpy.array(params))
points = (Point(*chain.from_iterable(evaluated[:, 0])),
Point(*chain.from_iterable(evaluated[:, 1])))
point = (points[0 if position < 0.1 else 1] + self.node.position)
vector = (points[1] - points[0]).normalized()
return point, vector
def curve_points(self, lane_connection: LaneConnection = None,
relative: bool = True) -> Tuple[Point]:
"""Get control points of curve for the given lane connection."""
nodes = self.curves[lane_connection].curve.nodes
points = (Point(*nodes[:, i]) for i in range(3))
if relative:
points = map((-self.node_ref().position).add, points)
return tuple(points)
def test_line_intersection(self):
"""Unit test for line_intersection."""
tests = (((0, 0), (3, 2), (1, 5), (-3, -4), (-11 / 2, -11 / 3)),
((-2, 0), (1, 0), (5, 5), (0, 1),
(5, 0)), ((-1, -1), (0, 3), (0, 0), (2, 1), (-1, -1 / 2)),
((-1, -3), (0, 5), (0, 1), (2, 0), (-1, 1)))
for test in tests:
result = line_intersection(Point(*test[0]), Vector(*test[1]),
Point(*test[2]), Vector(*test[3]))
self.assertAlmostEqual(result, Point(*test[4]))
tests = (((0, 0), (1, 2), (1, 0), (1, 2)),
((-1, 0), (0, 2), (0, 0), (0, -1)), ((-1, 0), (1, 0), (0, 5),
(5, 0)))
for test in tests:
with self.assertRaises(ZeroDivisionError):
line_intersection(Point(*test[0]), Vector(*test[1]),
Point(*test[2]), Vector(*test[3]))
def distance_and_point(self, point: Point, squared: bool = False) \
-> Tuple[float, Point]:
"""Calculate smallest distance from the way to a point.
Returns the smallest distance and the closest point in a tuple.
"""
result, closest = min((point.distance_to_segment(p, v, squared=True)
for p, v in zip(self.points(), self.vectors())),
key=itemgetter(0))
return (result if squared else sqrt(result), closest)
def position(self, value: Union[Point, Iterable]):
if not isinstance(value, Point):
value = Point(*islice(value, 2))
self.actor.set_x(value.x)
self.actor.set_y(value.y)
self._position = value
def distance(self, point: Point, squared: bool = False) -> float:
"""Calculate smallest distance from the way to a point."""
result, _ = min((point.distance_to_segment(p, v, squared=True)
for p, v in zip(self.points(), self.vectors())),
key=itemgetter(0))
return result if squared else sqrt(result)
def osm_reader(name: str):
"""Run the osm reader."""
log_config(name)
INDEX.reset(name)
tree = ElementTree.parse(name)
root = tree.getroot()
try:
bounds = {k: float(v) for k, v in root.find('bounds').attrib.items()
if k in ('maxlat', 'maxlon', 'minlat', 'minlon')}
except AttributeError:
bounds = calculate_bounds(root)
center = ((bounds['maxlat'] - bounds['minlat']) / 2 + bounds['minlat'],
(bounds['maxlon'] - bounds['minlon']) / 2 + bounds['minlon'])
nodes = {int(n.get('id')):
Node(Point(*coord_meters(*center, float(n.get('lat')),
float(n.get('lon')))))
for n in root.iterfind('node')}
nodes_inv = {node: osm_id for osm_id, node in nodes.items()}
for xid, node in nodes.items():
node.xid = xid
Highway = namedtuple('Highway', ('nodes', 'level', 'one_way', 'lane_count',
'xid', 'max_speed'))
highways = [
Highway(nodes=[int(n.get('ref')) for n in w.iterfind('nd')],
level=(1 if any((t.get('k') == 'bridge' and
t.get('v') == 'viaduct')
for t in w.iterfind('tag'))
else 0),
one_way=any((t.get('k') == 'oneway' and
t.get('v') in ('yes', 'true', '1'))
for t in w.iterfind('tag')),
lane_count=next((int(t.get('v')) for t in w.iterfind('tag')
if t.get('k') == 'lanes'),
None),
xid=int(w.get('id')),
max_speed=next((kph_to_mps(float(t.get('v').split()[0]))
for t in w.iterfind('tag')
if t.get('k') == 'maxspeed'),
None))
for w in root.iterfind('way')
if any(t.get('k') == 'highway' and t.get('v') != 'footway'
for t in w.iterfind('tag'))]
for highway in highways:
for start, end in zip(highway.nodes, islice(highway.nodes, 1, None)):
if highway.level != 0:
nodes[start].level = nodes[end].level = highway.level
if highway.one_way:
lane_count = ((highway.lane_count, 0)
if highway.lane_count is not None else (2, 0))
else:
lane_count = ((highway.lane_count // 2,
highway.lane_count - highway.lane_count // 2)
if highway.lane_count is not None else (1, 1))
way = (Way(nodes[start], nodes[end], lane_count)
if highway.max_speed is None
else Way(nodes[start], nodes[end], lane_count,
max_speed=highway.max_speed))
way.xid = highway.xid
dissolve_nodes(nodes_inv)
filter_waypoints()
find_paths()
compute_cached()
INDEX.save()
def evaluate(self, param: float) -> Point:
"""Evaluate bezier curve at t=param."""
return Point(*chain.from_iterable(self.curve.evaluate(param)))