def TransformSTtoXY(s, t, roadType, args):
# args = [origin, heading, length, CurvStart, CurvEnd]
if roadType == 'line':
tangent = Vector(math.cos(args[1]), math.sin(args[1]), 0)
tangent = tangent.normalize()
left_normal = tangent.rotate(90)
pt = s*tangent + t*left_normal + args[0]
elif roadType == 'arc':
R = 1/args[3]
hdg = args[1]
origin = args[0]
anti_clockwise = 1
if R<0 : anti_clockwise = -1
theta = s/R
tangent = Vector(math.cos(hdg), math.sin(hdg), 0)
tangent = tangent.normalize()
init_normal = tangent.rotate(anti_clockwise*90)
centre = origin + init_normal*R
#center = init_normal*R
pt_vec = origin - centre
pt_vec = pt_vec.normalize()
rot_pt_vec = pt_vec.rotate(180*theta/math.pi)
pt = (R-t)*pt_vec + centre
elif roadType == 'spiral':
Resolution = 0.1
num_sections = 100
distance = 0
ahead = 1
done = False
scaling = 1
dx = length/num_sections
prev_point = args[0]
for i in range(num_sections):
if ltoc:
pt_hdg = anti_clockwise*(distance*a)**2 + args[1]
else:
pt_hdg = -(length*a)**2-anti_clockwise*((length-distance)*a)**2 + args[1]
tangent = Vector(math.cos(pt_hdg), math.sin(pt_hdg), 0)
t_norm = tangent.normalize()
pt = prev_point + ahead*dx*t_norm
prev_point = pt
distance += ahead*dx*scaling
if abs(distance-s) <= RESOLUTION:
break
elif distance > s:
if ahead == 1:
scaling = scaling/2
ahead = -1
elif distance < s:
if ahead == -1:
scaling = scaling/2
ahead = 1
left_normal = t_norm.rotate(90)
pt = left_normal*t + pt
x = pt.x
y = pt.y
return x,y
def create_arc(length, curvature, lane_data, quad_number=10):
""" Gives the chord line set of points for a curved road"""
plt = [[], []]
anti_clockwise = 1
tangent = Vector(1, 0, 0)
radius = 1 / curvature
end_angle = 0
if (radius > 0):
tangent = tangent.rotate(90)
anti_clockwise = 1
else:
tangent = tangent.rotate(-90)
radius = -1 * radius
anti_clockwise = -1
center = radius * tangent
tangent = tangent.rotate(180)
start_angle = tangent.argument()
end_angle = start_angle + anti_clockwise * 360 * (length /
(2 * math.pi * radius))
angle = start_angle
radius = Vector(radius, 0, 0)
radius = radius.rotate(start_angle)
tot_lane_vertices = []
for i in range(quad_number + 1):
pt = center + radius
lane_pts = generate_lane_verts(pt, radius.rotate(90 * anti_clockwise),
lane_data)
tot_lane_vertices.append(lane_pts)
radius = radius.rotate((end_angle - start_angle) / quad_number)
plt[0].append(pt.x)
plt[1].append(pt.y)
return tot_lane_vertices
def create_arc(length,
curvature,
current_road,
start_s,
hardCode=False,
quad_number=10):
""" Gives the chord line set of points for a curved road"""
plt = [[], []]
anti_clockwise = 1
tangent = Vector(1, 0, 0)
radius = 1 / curvature
end_angle = 0
if (radius > 0):
tangent = tangent.rotate(90)
anti_clockwise = 1
else:
tangent = tangent.rotate(-90)
radius = -1 * radius
anti_clockwise = -1
center = radius * tangent
tangent = tangent.rotate(180)
start_angle = tangent.argument()
end_angle = start_angle + anti_clockwise * 360 * (length /
(2 * math.pi * radius))
angle = start_angle
radius = Vector(radius, 0, 0)
radius = radius.rotate(start_angle)
tot_lane_vertices = []
s = -0.1
for i in range(quad_number + 1):
pt = center + radius
lane_data = current_road.get_lane_data(s + start_s)
current_pt = pt + Vector(0, 0, current_road.get_elevation(s + start_s))
lane_pts = generate_lane_verts(current_pt,
radius.rotate(90 * anti_clockwise),
lane_data, hardCode)
tot_lane_vertices.append(lane_pts)
radius = radius.rotate((end_angle - start_angle) / quad_number)
plt[0].append(pt.x)
plt[1].append(pt.y)
dTheta = (end_angle - start_angle) / quad_number
dTheta = dTheta * 3.14 / 180
s = s + radius.norm() * dTheta
return tot_lane_vertices
def TransformSTtoXY(s, t, roadType, start_s, args):
# args = [origin, heading,length,CurvStart, CurvEnd]
if roadType == 'line':
tangent = Vector(math.cos(args[1]), math.sin(args[1]), 0)
tangent = tangent.normalize()
left_normal = tangent.rotate(90)
pt = (s - start_s) * tangent + t * left_normal + args[0]
elif roadType == 'arc':
R = 1 / args[3]
anti_clockwise = 1
if R < 0: anti_clockwise = -1
if s < start_s: print('yes')
theta = (s - start_s) / R
tangent = Vector(math.cos(args[1]), math.sin(args[1]), 0)
tangent = tangent.normalize()
init_normal = tangent.rotate(anti_clockwise * 90)
centre = args[0] + init_normal * abs(R)
pt_vec = args[0] - centre
pt_vec = pt_vec.normalize()
rot_pt_vec = pt_vec.rotate(180 * theta / math.pi)
tangent = tangent.rotate(180 * theta / math.pi)
pt = (abs(R) - t) * rot_pt_vec + centre
elif roadType == 'spiral':
if args[3] == 0:
ltoc = True
R = 1 / args[4]
else:
ltoc = False
R = 1 / args[3]
anti_clockwise = 1
if R < 0: anti_clockwise = -1
a = 1 / math.sqrt(2 * args[2] * abs(R)) # Scale factor
RESOLUTION = 0.1
num_sections = 1000
distance = 0
ahead = 1
done = False
scaling = 1
length = args[2]
dx = length / num_sections
prev_point = args[0]
for i in range(num_sections):
if ltoc:
pt_hdg = anti_clockwise * (distance * a)**2 + args[1]
else:
pt_hdg = anti_clockwise * ((length * a)**2 - (
(length - distance) * a)**2) + args[1]
tangent = Vector(math.cos(pt_hdg), math.sin(pt_hdg), 0)
t_norm = tangent.normalize()
pt = prev_point + ahead * dx * t_norm
prev_point = pt
distance += ahead * dx * scaling
if distance > s - start_s:
break
left_normal = t_norm.rotate(90)
pt = left_normal * t + pt
x = pt.x
y = pt.y
return pt, tangent
