diff --git a/LibCarla/source/carla/geom/Math.cpp b/LibCarla/source/carla/geom/Math.cpp index 993497bb0..592804a8b 100644 --- a/LibCarla/source/carla/geom/Math.cpp +++ b/LibCarla/source/carla/geom/Math.cpp @@ -55,11 +55,12 @@ namespace geom { p.y = -p.y; start_pos.y = -start_pos.y; heading = -heading; + curvature = -curvature; // since this algorithm is working for positive curvatures, // and we are only calculating distances, we can invert the y // axis (along with the curvature and the heading), so if the - // curvature is negative, so the algorithm will work as expected + // curvature is negative, the algorithm will work as expected if (curvature < 0.0) { p.y = -p.y; start_pos.y = -start_pos.y; diff --git a/LibCarla/source/carla/road/Map.cpp b/LibCarla/source/carla/road/Map.cpp index 1af2bdcbb..b5ba5c71f 100644 --- a/LibCarla/source/carla/road/Map.cpp +++ b/LibCarla/source/carla/road/Map.cpp @@ -214,7 +214,7 @@ namespace road { lane_tangent = computed_width.second; } - // get a directed pooint in s and apply the computed lateral offet + // get a directed point in s and apply the computed lateral offet DirectedPoint dp = road->GetDirectedPointIn(waypoint.s); geom::Rotation rot( diff --git a/LibCarla/source/carla/road/Road.cpp b/LibCarla/source/carla/road/Road.cpp index 86691f600..1209245d6 100644 --- a/LibCarla/source/carla/road/Road.cpp +++ b/LibCarla/source/carla/road/Road.cpp @@ -177,7 +177,7 @@ namespace road { const auto lane_offset = _info.GetInfo(clamped_s); const float offset = lane_offset->GetPolynomial().Evaluate(clamped_s); - element::DirectedPoint p = geometry->GetGeometry().PosFromDist(clamped_s); + element::DirectedPoint p = geometry->GetGeometry().PosFromDist(clamped_s - geometry->GetDistance()); p.ApplyLateralOffset(offset); const auto elevation_info = GetElevationOn(s); diff --git a/LibCarla/source/carla/road/element/Geometry.h b/LibCarla/source/carla/road/element/Geometry.h index 73996268f..5127afad4 100644 --- a/LibCarla/source/carla/road/element/Geometry.h +++ b/LibCarla/source/carla/road/element/Geometry.h @@ -157,11 +157,11 @@ namespace element { assert(std::fabs(_curvature) > 1e-15); const double radius = 1.0 / _curvature; DirectedPoint p(_start_position, _heading); - p.location.x -= radius * std::cos(p.tangent + geom::Math::pi_half()); - p.location.y -= radius * std::sin(p.tangent + geom::Math::pi_half()); - p.tangent -= dist * _curvature; p.location.x += radius * std::cos(p.tangent + geom::Math::pi_half()); p.location.y += radius * std::sin(p.tangent + geom::Math::pi_half()); + p.tangent += dist * _curvature; + p.location.x -= radius * std::cos(p.tangent + geom::Math::pi_half()); + p.location.y -= radius * std::sin(p.tangent + geom::Math::pi_half()); return p; }