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_spiral_road(length,
curvStart,
curvEnd,
current_road,
start_s,
hardCode=False,
quad_number=10):
# if(curvStart==0):
# return spiral_line_to_curve(length,curvEnd,lane_data)
# else:
# return spiral_curve_to_line(length,curvStart,lane_data)
origin = Vector(0, 0, 0)
hdg = 0
if curvStart == 0:
ltoc = True
R = 1 / curvEnd
else:
ltoc = False
R = 1 / curvStart
anti_clockwise = 1
if R < 0: anti_clockwise = -1
a = 1 / math.sqrt(2 * length * abs(R)) # Scale factor
RESOLUTION = 0.1
num_sections = 100
distance = 0
ahead = 1
done = False
scaling = 1
dx = length / num_sections
prev_point = origin
total_pts = []
for i in range(num_sections):
if ltoc:
pt_hdg = anti_clockwise * (distance * a)**2 + hdg
else:
pt_hdg = anti_clockwise * ((length * a)**2 -
((length - distance) * a)**2) + hdg
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
lane_data = current_road.get_lane_data(distance + start_s)
current_pt = pt + Vector(
0, 0, current_road.get_elevation(distance + start_s))
total_pts.append(
generate_lane_verts(current_pt, tangent, lane_data, hardCode))
return total_pts
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
