p71924506
/
carla
mirror of https://github.com/carla-simulator/carla.git
1
0
Fork 0
Code Issues Packages Projects Releases Wiki Activity

Fixed DistArcPoint on negative curvatures

This commit is contained in:
Marc 2018-11-14 14:48:09 +01:00
parent 60c97c4c14
commit 49f495d946
3 changed files with 16 additions and 5 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

15
LibCarla/source/carla/geom/Math.h
View File

@ -76,7 +76,7 @@ namespace geom {
/// Returns a pair containing:
/// - @b first: distance from v to p' where p' = p projected on segment (w - v)
/// - @b second: euclidean distance from p to p'
/// - @b second: Euclidean distance from p to p'
/// @param p point to calculate distance
/// @param v first point of the segment
/// @param w second point of the segment
@ -103,7 +103,7 @@ namespace geom {
/// Returns a pair containing:
/// - @b first: distance across the arc from start_pos to p' where p' = p projected on Arc
/// - @b second: euclidean distance from p to p'
/// - @b second: Euclidean distance from p to p'
static std::pair<double, double> DistArcPoint(
Vector3D p,
Vector3D start_pos,
@ -117,6 +117,17 @@ namespace geom {
start_pos.y = -start_pos.y;
heading = -heading;
// 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
if (curvature < 0.0) {
p.y = -p.y;
start_pos.y = -start_pos.y;
heading = -heading;
curvature = -curvature;
}
// transport point relative to the arc starting poistion and rotation
const Vector3D rotated_p(RotatePointOnOrigin2D(p - start_pos, -heading));

4
LibCarla/source/carla/road/element/Geometry.h
View File

@ -132,7 +132,7 @@ namespace element {
/// Returns a pair containing:
/// - @b first: distance to the nearest point in this line from the
/// begining of the shape.
/// - @b second: euclidean distance from the nearest point in this line to p.
/// - @b second: Euclidean distance from the nearest point in this line to p.
/// @param p point to calculate the distance
std::pair<double, double> DistanceTo(const geom::Location &p) const override {
return geom::Math::DistSegmentPoint(
@ -172,7 +172,7 @@ namespace element {
/// Returns a pair containing:
/// - @b first: distance to the nearest point in this arc from the
/// begining of the shape.
/// - @b second: euclidean distance from the nearest point in this arc to p.
/// - @b second: Euclidean distance from the nearest point in this arc to p.
/// @param p point to calculate the distance
std::pair<double, double> DistanceTo(const geom::Location &p) const override {
return geom::Math::DistArcPoint(

2
LibCarla/source/carla/road/element/RoadSegment.h
View File

@ -174,7 +174,7 @@ namespace element {
/// Returns a pair containing:
/// - @b first: distance to the nearest point on the center in
/// this road segment from the begining of it.
/// - @b second: euclidean distance from the nearest point in
/// - @b second: Euclidean distance from the nearest point in
/// this road segment to p.
/// @param loc point to calculate the distance
std::pair<double, double> GetNearestPoint(const geom::Location &loc) const {