def minpath( p, q, roadmap, length_attr='length' ) :
""" returns one of the min length paths between two points on a Roadmap """
# some convenient static variables
if not hasattr( minpath, 'STATICS' ) :
minpath.STATICS = True
minpath.first = RoadAddress(None,None)
minpath.second = RoadAddress(None,None)
minpath.points = [ minpath.first, minpath.second ]
# convenient non-static variables
road = p.road
edge, data = obtain_edge( roadmap, road, True )
i,j,key = edge
roadlen = data.get( length_attr, 1 )
# we will be choosing best path among options
options = []
# one possible type of path is a segment on a single road
path = minpath_on_road( p, q, road, roadmap, length_attr )
options.append( ( pathLength(path), path ) )
# two more possible types of paths are p->u->q, for u either of endpoints of road of p
minpath.first.init(road,0.)
minpath.second.init(road,roadlen)
for u, qq in zip( (i,j), minpath.points ) :
first = minpath_on_road( p, qq, road, roadmap, length_attr )
second = minpath_node_to_point( u, q, roadmap, length_attr )
path = first + second
options.append( ( pathLength(path), path ) )
# return path having the shortest length --- might be []
return min( options )[1]
def address_coord(cls, addr, roadnet, layout, length='length' ) :
if not isinstance( addr, ROAD.RoadAddress ) : raise 'not a valid RoadAddress'
edge, road_data = ROAD.obtain_edge( roadnet, addr.road, True )
u,v,key = edge
pt1 = layout[u] ; pt2 = layout[v]
dEuc = np.linalg.norm( pt2 - pt1 )
d = road_data.get( length, 1 )
if dEuc < d :
""" on arc """
center, R, t1, t2 = fitArc2Segment( pt1, pt2, d )
theta = angle_interpolate_ccw( t1, t2, addr.coord )
coord = center + R * np.array([ np.cos(theta), np.sin(theta) ])
else :
""" segment part """
idx = lnet.edge[i][j].keys().index(key)
if idx == 0 :
""" on "shorty" """
mid = ( pt1 + pt2 ) / 2
offset = ( d/dEuc / 2 ) * ( pt2 - pt1 )
pt1_inner, pt2_inner = mid-offset, mid+offset
coord = x * pt2_inner + ( 1.0 - x ) * pt1_inner
else :
""" on one of the spaced arcs """
spacing = (-1)**idx * np.ceil( float(idx) / 2 )
ht = ( dEuc / 20 ) * spacing
center, R, theta1, theta2 = fitArcThruSpacing( pt1, pt2, ht )
if ht > 0. :
dtheta = angleconvert_zerototwopi( theta2 - theta1 )
darc = R * dtheta
y = d / darc
t1 = angle_interpolate_ccw( theta1, theta2, 0.5 - y/2 )
t2 = angle_interpolate_ccw( theta1, theta2, 0.5 + y/2 )
theta = angle_interpolate_ccw( t1, t2, x )
else :
dtheta = angleconvert_zerototwopi( theta1 - theta2 )
darc = R * dtheta
y = d / darc
t1 = angle_interpolate_ccw( theta1, theta2, 0.5 - y/2, reverse=True )
t2 = angle_interpolate_ccw( theta1, theta2, 0.5 + y/2, reverse=True )
theta = angle_interpolate_ccw( t1, t2, x, reverse=True )
coord = center + R * np.array([ np.cos(theta), np.sin(theta) ])
return coord
def pathFromAStar( astar_path, roadmap, length_attr ) :
path = []
nodewalk = astar_path[::2]
edgewalk = astar_path[1::2]
for i,j, edgedata in zip( nodewalk[:-1], nodewalk[1:], edgewalk ) :
_,__,road = edgedata
edge, data = obtain_edge( roadmap, road, True )
u,v,_ = edge
roadlen = data.get( length_attr, 1 )
if i == u and j == v : # forward jump, yay!
segment = RoadSegment( road, 0., roadlen )
elif i == v and j == u : # backward jump, yay!
segment = RoadSegment( road, roadlen, 0. )
else :
raise Exception('invalid hop on roadmap')
path.append( segment )
return path
def minpath_on_road( p, q, road, roadmap, length_attr='length' ) :
"""
subroutine and useful utility:
returns the path from p to q on road, if the direction of travel is admissible;
if not, or if p and q are not co-'road'-al, returns empty path, implying non-existence
"""
edge, data = obtain_edge( roadmap, road, True )
i,j,key = edge
path = []
# regular logic, NOT error catching
try :
assert p.road == road and check_point( roadmap, p, length_attr )
assert q.road == road and check_point( roadmap, q, length_attr )
assert p.coord < q.coord or not data.get( 'oneway', False )
path.append( RoadSegment( road, p.coord, q.coord ) )
except AssertionError :
pass
return path
def minpath_node_to_point( u, q, roadmap, length_attr='length' ) :
"""
subroutine and useful utility:
returns the shortest-path distance from a node u to a point q, on digraph
"""
# some convenient static variables
scope = minpath_node_to_point
if not hasattr( scope, 'STATICS' ) :
scope.STATICS = True
scope.first = RoadAddress(None,None)
scope.second = RoadAddress(None,None)
scope.points = [ scope.first, scope.second ]
# convenient non-static variables
assert check_point( roadmap, q, length_attr )
road = q.road
edge, data = obtain_edge( roadmap, road, True )
i,j,key = edge
roadlen = data.get( length_attr, 1 )
# pre-load options with nopath; in case there are no feasible paths
options = [ (np.inf, [] ) ]
# possible paths are u->v->q, for v in endpoints of road of q
scope.first.init(road,0.)
scope.second.init(road,roadlen)
for v, pp in zip( (i,j), scope.points ) :
try :
astarPath = astar_basic.astar_path( roadmap, u, v, None, weight=length_attr )
first = pathFromAStar( astarPath, roadmap, length_attr )
except nx.NetworkXNoPath :
continue
second = minpath_on_road( pp, q, road, roadmap, length_attr )
path = first + second
options.append( (pathLength(path), path ) )
return min( options )[1]
def roadinfo( road, roadnet, length='length' ) :
edge, data = ROAD.obtain_edge( roadnet, road, True )
u, v, key = edge
roadlen = data.get( length, 1 )
#
return u, v, roadlen
def sample_onroad( road, roadnet, length='length' ) :
""" samples uniformly from the given road """
_, road_data = ROAD.obtain_edge( roadnet, road, True )
roadlen = road_data.get( length, 1 )
y = roadlen * np.random.rand()
return ROAD.RoadAddress(road,y)
