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