diff --git a/addons/road-generator/nodes/road_segment.gd b/addons/road-generator/nodes/road_segment.gd index e4bb2d6..b89637f 100644 --- a/addons/road-generator/nodes/road_segment.gd +++ b/addons/road-generator/nodes/road_segment.gd @@ -11,6 +11,7 @@ extends Spatial # const RoadSegment = preload("res://addons/road-generator/road_segment.gd") const LOWPOLY_FACTOR = 3.0 +const RAD_NINETY_DEG = PI/2 # aka 1.5707963267949, used for offset_curve algorithm signal seg_ready(road_segment) @@ -262,10 +263,6 @@ func generate_lane_segments(_debug: bool = false) -> bool: new_ln.lane_next_tag = this_match[3] new_ln.name = ln_name - # Now decide where the two poitns should go, and their magnitudes. - var in_pos: Vector3 = start_point.global_transform.origin - var out_pos: Vector3 = end_point.global_transform.origin - var tmp = get_transition_offset( ln_type, ln_dir, lane_shift, end_is_wider, max_rev_shift) var start_shift:float = tmp[0] @@ -274,9 +271,6 @@ func generate_lane_segments(_debug: bool = false) -> bool: var in_offset = lanes_added * start_point.lane_width - start_offset + start_shift var out_offset = lanes_added * end_point.lane_width - end_offset + end_shift - in_pos += start_point.global_transform.basis.x * in_offset * _start_flip_mult - out_pos += end_point.global_transform.basis.x * out_offset * _end_flip_mult - # Set direction # TODO: When directionality is made consistent, we should no longer # need to invert the direction assignment here. @@ -286,14 +280,7 @@ func generate_lane_segments(_debug: bool = false) -> bool: # TODO(#46): Swtich to re-sampling and adding more points following the # curve along from the parent path generator, including its use of ease # in and out at the edges. - new_ln.curve.add_point( - new_ln.to_local(in_pos), - new_ln.to_local(to_global(curve.get_point_in(0))), - new_ln.to_local(to_global(curve.get_point_out(0)))) - new_ln.curve.add_point( - new_ln.to_local(out_pos), - new_ln.to_local(to_global(curve.get_point_in(1))), - new_ln.to_local(to_global(curve.get_point_out(1)))) + offset_curve(self, new_ln, in_offset, out_offset, start_point, end_point) # Visually display. new_ln.draw_in_editor = container.draw_lanes_editor @@ -321,6 +308,78 @@ func generate_lane_segments(_debug: bool = false) -> bool: return any_generated +## Offsets a destination curve from a source curve by a specified distance. +## +## Evaluates 4 points on source curve: Point 0 and 1 positions as well as +## point-0-out and point-1-in handles. Requires transforms for point 0 +## and point 1, which determine the direction of the handles. Calculates best +## fit position for destination curve given the supplied curves, transforms, +## and distance. +func offset_curve(road_seg: Spatial, road_lane: Path, in_offset: float, out_offset: float, rp0: Spatial, rp1: Spatial): + + var src:Curve3D = road_seg.curve + var dst:Curve3D = road_lane.curve + var a_gbasis := rp0.global_transform.basis + var d_gbasis := rp1.global_transform.basis + var in_pos := start_point.global_transform.origin + (a_gbasis.x * in_offset * _start_flip_mult) + var out_pos := end_point.global_transform.origin + (d_gbasis.x * out_offset * _end_flip_mult) + + # Get initial point locations + var pt_a := to_global(src.get_point_position(0)) + var pt_b := to_global(src.get_point_position(0) + src.get_point_out(0)) + var pt_c := to_global(src.get_point_position(1) + src.get_point_in(1)) + var pt_d := to_global(src.get_point_position(1)) + + # TTD: Project point(s) onto plane(s) + + # Project primary curve points to secondary curve points + var pt_e := pt_a + (a_gbasis.x * in_offset) + var pt_i := pt_b + (a_gbasis.x * in_offset) + var pt_h := pt_d + (d_gbasis.x * out_offset) + var pt_j := pt_c + (d_gbasis.x * out_offset) + + # Get vectors from points + var vec_ab := pt_b - pt_a + var vec_bc := pt_c - pt_b + var vec_cd := pt_d - pt_c + + # Calculate secondary curve handles and setup curves + var angle_q := -vec_ab.signed_angle_to(vec_bc, a_gbasis.y) * 0.5 + var angle_s := vec_cd.signed_angle_to(vec_bc, d_gbasis.y) * 0.5 + var offset_q := tan(angle_q) * in_offset + var offset_s := tan(angle_s) * out_offset + var pt_f := a_gbasis.z * (vec_ab.length() + offset_q) + var pt_g := -d_gbasis.z * (vec_cd.length() + offset_s) + + var margin := 0.1745329 # Margin to check above/below 90. 0.174 is roughly 10 degrees + + if abs(angle_q) > RAD_NINETY_DEG - margin and abs(angle_q) < RAD_NINETY_DEG + margin: + # Angle is close to 90deg. Use default values. + dst.add_point( + road_lane.to_local(in_pos), + road_lane.to_local(to_global(curve.get_point_in(0))), + road_lane.to_local(to_global(curve.get_point_out(0)))) + else: + # Use calculated values + dst.add_point( + road_lane.to_local(pt_e), + road_lane.to_local(to_global(src.get_point_in(0))), + road_lane.to_local(to_global(pt_f))) + + if abs(angle_s) > RAD_NINETY_DEG - margin and abs(angle_s) < RAD_NINETY_DEG + margin: + # Angle is close to 90deg. Use default values. + dst.add_point( + road_lane.to_local(out_pos), + road_lane.to_local(to_global(curve.get_point_in(1))), + road_lane.to_local(to_global(curve.get_point_out(1)))) + else: + # Use calculated values + dst.add_point( + road_lane.to_local(pt_h), + road_lane.to_local(to_global(pt_g)), + road_lane.to_local(to_global(src.get_point_out(1)))) + + ## Offset the curve in/out points based on lane index. ## ## Track the init (for reverse) or the stacking (fwd) number of