Skip to main content
summaryrefslogtreecommitdiffstats
diff options
context:
space:
mode:
authorBiegel Reinhard2020-09-16 14:02:18 +0000
committerBiegel Reinhard2020-09-25 10:59:25 +0000
commit0440a0a31f015ae357b8ae3213ba27aa9d58094b (patch)
tree8e2d43569db46fb5405d6bfa2d5a1e39eff13fd6
parent5f3794bef4ae0314a4291c2fee2596e4143fae24 (diff)
downloadsimopenpass-0440a0a31f015ae357b8ae3213ba27aa9d58094b.tar.gz
simopenpass-0440a0a31f015ae357b8ae3213ba27aa9d58094b.tar.xz
simopenpass-0440a0a31f015ae357b8ae3213ba27aa9d58094b.zip
fix(OpenPassSlave): Correct and simplify spiral calculations
- Old implementation did not correctly calculate spirals with start and end curvature being not equal to zero - New implementation also allows spiral geometries with start and end curvatures of different signs in a single geometry
-rw-r--r--sim/src/core/slave/importer/road.cpp561
-rw-r--r--sim/src/core/slave/importer/road.h79
-rw-r--r--sim/tests/unitTests/core/slave/CMakeLists.txt1
-rw-r--r--sim/tests/unitTests/core/slave/openPassSlave_Tests.pro1
-rw-r--r--sim/tests/unitTests/core/slave/roadGeometry_Tests.cpp267
5 files changed, 351 insertions, 558 deletions
diff --git a/sim/src/core/slave/importer/road.cpp b/sim/src/core/slave/importer/road.cpp
index c58dd03..23352e5 100644
--- a/sim/src/core/slave/importer/road.cpp
+++ b/sim/src/core/slave/importer/road.cpp
@@ -19,7 +19,7 @@ extern "C"
namespace
{
-const double SQRT_PI_2 = std::sqrt(M_PI_2);
+const double SQRT_PI = std::sqrt(M_PI);
} // namespace
@@ -220,541 +220,116 @@ double RoadGeometryArc::GetDir(double sOffset) const
return GetDirArc(sOffset, curvature);
}
-Common::Vector2d RoadGeometrySpiral::HalfCoord(double sOffset, double tOffset) const
+RoadGeometrySpiral::RoadGeometrySpiral(double s, double x, double y, double hdg, double length, double curvStart, double curvEnd)
+ : RoadGeometry{s, x, y, hdg, length}, c_start{curvStart}, c_end{curvEnd}
{
- double _curvStart = curvStart;
- double _curvEnd = curvEnd;
-
- assert(_curvStart != _curvEnd);
- assert((0.0 <= _curvStart && 0.0 <= _curvEnd) || (0.0 >= _curvStart && 0.0 >= _curvEnd));
-
- if (length < sOffset)
+ if (length != 0.0)
{
- LOG_INTERN(LogLevel::Warning) << "exceeding length of geometry";
- sOffset = length;
+ c_dot = (c_end - c_start) / length;
}
-
- if (0.0 <= _curvStart && 0.0 <= _curvEnd)
+ else
{
- if (_curvStart < _curvEnd)
- {
- assert(0.0 != _curvEnd);
-
- double radiusEnd = 1.0 / _curvEnd;
-
- // 1. equation (definition of clothoid): const = radiusStart * distanceStart = radiusEnd * distanceEnd
- // 2. equation: length = distanceEnd - distanceStart
- // -> distanceEnd = radiusStart * length / (radiusStart - radiusEnd)
- // -> formed such that equation copes with _curvStart==0 (infinite radiusStart)
- double distanceEnd = length / (1 - radiusEnd * _curvStart);
- assert(length <= distanceEnd);
-
- double distanceStart = distanceEnd - length;
- double a = std::sqrt(2 * radiusEnd * distanceEnd);
-
- Common::Vector2d start;
- (void)fresnl(distanceStart / a / SQRT_PI_2, &start.y, &start.x);
- start.Scale(a * SQRT_PI_2);
-
- double distanceOffset = distanceStart + sOffset;
- Common::Vector2d offset;
- (void)fresnl(distanceOffset / a / SQRT_PI_2, &offset.y, &offset.x);
- offset.Scale(a * SQRT_PI_2);
- offset.Sub(start);
-
- double tangentAngle = distanceOffset * distanceOffset / a / a;
- if (0 > _curvEnd)
- {
- tangentAngle = -tangentAngle;
- }
-
- double normAngle = tangentAngle + M_PI_2;
- normAngle = std::fmod(normAngle, 2 * M_PI);
-
- // get perpendicular vector
- Common::Vector2d norm(1, 0);
- norm.Rotate(normAngle);
- norm.Scale(tOffset);
-
- offset.Add(norm);
- offset.Rotate(hdg);
-
- return Common::Vector2d(x + offset.x, y + offset.y);
- }
- else // _curvStart > _curvEnd ("curStart != _curvEnd" guaranteed by checks in caller)
- {
- std::swap(_curvStart, _curvEnd);
- sOffset = length - sOffset;
-
- assert(0.0 != _curvEnd);
-
- double radiusEnd = 1.0 / _curvEnd;
-
- // 1. equation (definition of clothoid): const = radiusStart * distanceStart = radiusEnd * distanceEnd
- // 2. equation: length = distanceEnd - distanceStart
- // -> distanceEnd = radiusStart * length / (radiusStart - radiusEnd)
- // -> formed such that equation copes with _curvStart==0 (infinite radiusStart)
- double distanceEnd = length / (1 - radiusEnd * _curvStart);
- assert(length <= distanceEnd);
-
- double distanceStart = distanceEnd - length;
- double a = std::sqrt(2 * radiusEnd * distanceEnd);
-
- Common::Vector2d start;
- (void)fresnl(distanceStart / a / SQRT_PI_2, &start.y, &start.x);
- start.Scale(a * SQRT_PI_2);
-
- double distanceOffset = distanceStart + sOffset;
- Common::Vector2d offset;
- (void)fresnl(distanceOffset / a / SQRT_PI_2, &offset.y, &offset.x);
- offset.Scale(a * SQRT_PI_2);
- offset.Sub(start);
-
- double tangentAngle = distanceOffset * distanceOffset / a / a;
- if (0 > _curvEnd)
- {
- tangentAngle = -tangentAngle;
- }
-
- double normAngle = tangentAngle + M_PI_2;
- normAngle = std::fmod(normAngle, 2 * M_PI);
-
- // get perpendicular vector
- Common::Vector2d norm(1, 0);
- norm.Rotate(normAngle);
- norm.Scale(tOffset);
-
- offset.Add(norm);
-
- // calculate end point
- Common::Vector2d endOffset;
- (void)fresnl(distanceEnd / a / SQRT_PI_2, &endOffset.y, &endOffset.x);
- endOffset.Scale(a * SQRT_PI_2);
- endOffset.Sub(start);
-
- // compensate for inverted curvatures
- double tangentAngleEnd = distanceEnd * distanceEnd / a / a;
- if (0 > _curvEnd)
- {
- tangentAngleEnd = -tangentAngleEnd;
- }
- tangentAngleEnd = -tangentAngleEnd + M_PI;
-
- offset.Sub(endOffset);
- offset.y = -offset.y;
- offset.Rotate(hdg - tangentAngleEnd);
-
- return Common::Vector2d(x + offset.x, y + offset.y);
- }
+ c_dot = 0.0;
}
- else // 0.0 >= _curvStart && 0.0 >= _curvEnd
- {
- _curvStart = -_curvStart;
- _curvEnd = -_curvEnd;
-
- if (_curvStart < _curvEnd)
- {
- assert(0.0 != _curvEnd);
-
- double radiusEnd = 1.0 / _curvEnd;
-
- // 1. equation (definition of clothoid): const = radiusStart * distanceStart = radiusEnd * distanceEnd
- // 2. equation: length = distanceEnd - distanceStart
- // -> distanceEnd = radiusStart * length / (radiusStart - radiusEnd)
- // -> formed such that equation copes with _curvStart==0 (infinite radiusStart)
- double distanceEnd = length / (1 - radiusEnd * _curvStart);
- assert(length <= distanceEnd);
- double distanceStart = distanceEnd - length;
- double a = std::sqrt(2 * radiusEnd * distanceEnd);
-
- Common::Vector2d start;
- (void)fresnl(distanceStart / a / SQRT_PI_2, &start.y, &start.x);
- start.Scale(a * SQRT_PI_2);
-
- double distanceOffset = distanceStart + sOffset;
- Common::Vector2d offset;
- (void)fresnl(distanceOffset / a / SQRT_PI_2, &offset.y, &offset.x);
- offset.Scale(a * SQRT_PI_2);
- offset.Sub(start);
-
- double tangentAngle = distanceOffset * distanceOffset / a / a;
- if (0 > _curvEnd)
- {
- tangentAngle = -tangentAngle;
- }
-
- double normAngle = tangentAngle + M_PI_2;
- normAngle = std::fmod(normAngle, 2 * M_PI);
-
- // get perpendicular vector
- Common::Vector2d norm(-1, 0);
- norm.Rotate(normAngle);
- norm.Scale(tOffset);
-
- offset.Add(norm);
- offset.y = -offset.y;
- offset.Rotate(hdg);
-
- return Common::Vector2d(x + offset.x, y + offset.y);
- }
- else // _curvStart > _curvEnd ("curStart != _curvEnd" guaranteed by checks in caller)
- {
- std::swap(_curvStart, _curvEnd);
- sOffset = length - sOffset;
-
- assert(0.0 != _curvEnd);
-
- double radiusEnd = 1.0 / _curvEnd;
-
- // 1. equation (definition of clothoid): const = radiusStart * distanceStart = radiusEnd * distanceEnd
- // 2. equation: length = distanceEnd - distanceStart
- // -> distanceEnd = radiusStart * length / (radiusStart - radiusEnd)
- // -> formed such that equation copes with _curvStart==0 (infinite radiusStart)
- double distanceEnd = length / (1 - radiusEnd * _curvStart);
- assert(length <= distanceEnd);
-
- double distanceStart = distanceEnd - length;
- double a = std::sqrt(2 * radiusEnd * distanceEnd);
-
- Common::Vector2d start;
- (void)fresnl(distanceStart / a / SQRT_PI_2, &start.y, &start.x);
- start.Scale(a * SQRT_PI_2);
-
- double distanceOffset = distanceStart + sOffset;
- Common::Vector2d offset;
- (void)fresnl(distanceOffset / a / SQRT_PI_2, &offset.y, &offset.x);
- offset.Scale(a * SQRT_PI_2);
- offset.Sub(start);
-
- double tangentAngle = distanceOffset * distanceOffset / a / a;
- if (0 > _curvEnd)
- {
- tangentAngle = -tangentAngle;
- }
-
- double normAngle = tangentAngle + M_PI_2;
- normAngle = std::fmod(normAngle, 2 * M_PI);
-
- // get perpendicular vector
- Common::Vector2d norm(-1, 0);
- norm.Rotate(normAngle);
- norm.Scale(tOffset);
-
- offset.Add(norm);
-
- // calculate end point
- Common::Vector2d endOffset;
- (void)fresnl(distanceEnd / a / SQRT_PI_2, &endOffset.y, &endOffset.x);
- endOffset.Scale(a * SQRT_PI_2);
- endOffset.Sub(start);
-
- // compensate for inverted curvatures
- double tangentAngleEnd = distanceEnd * distanceEnd / a / a;
- if (0 > _curvEnd)
- {
- tangentAngleEnd = -tangentAngleEnd;
- }
- tangentAngleEnd = tangentAngleEnd - M_PI;
-
- offset.Sub(endOffset);
- offset.Rotate(hdg - tangentAngleEnd);
-
- return Common::Vector2d(x + offset.x, y + offset.y);
- }
+ if (c_dot != 0.0)
+ {
+ l_start = c_start / c_dot;
}
-}
-
-Common::Vector2d RoadGeometrySpiral::FullCoord(double sOffset, double tOffset) const
-{
- if ((0.0 <= curvStart && 0.0 <= curvEnd) || (0.0 >= curvStart && 0.0 >= curvEnd))
+ else
{
- return HalfCoord(sOffset, tOffset);
+ l_start = 0.0;
}
- assert((0.0 > curvStart && 0.0 < curvEnd) || (0.0 < curvStart && 0.0 > curvEnd));
-
- // one degree of freedom: start position/end position can not be determined
- LOG_INTERN(LogLevel::Warning) << "could not calculate spiral coordinate";
+ double l_end = l_start + length;
+ double rl; // helper constant R * L
- return Common::Vector2d();
-}
-
-double RoadGeometrySpiral::HalfCurvature(double sOffset) const
-{
- double _curvStart = curvStart;
- double _curvEnd = curvEnd;
-
- assert(_curvStart != _curvEnd);
- assert((0.0 <= _curvStart && 0.0 <= _curvEnd) || (0.0 >= _curvStart && 0.0 >= _curvEnd));
-
- if (length < sOffset)
+ if (c_start != 0.0)
{
- LOG_INTERN(LogLevel::Warning) << "exceeding length of geometry";
- sOffset = length;
+ rl = l_start / c_start;
}
-
- if (0.0 <= _curvStart && 0.0 <= _curvEnd)
+ else if (c_end != 0.0)
{
- if (_curvStart < _curvEnd)
- {
- assert(0.0 != _curvEnd);
-
- double radiusEnd = 1.0 / _curvEnd;
-
- // 1. equation (definition of clothoid): const = radiusStart * distanceStart = radiusEnd * distanceEnd
- // 2. equation: length = distanceEnd - distanceStart
- // -> distanceEnd = radiusStart * length / (radiusStart - radiusEnd)
- // -> formed such that equation copes with _curvStart==0 (infinite radiusStart)
- double distanceEnd = length / (1 - radiusEnd * _curvStart);
- assert(length <= distanceEnd);
-
- double distanceStart = distanceEnd - length;
- double distanceOffset = distanceStart + sOffset;
-
- // equation const = radiusEnd * distanceEnd = radiusOffset * distanceOffset
- // -> curvatureOffset = 1 / radiusOffset = distanceOffset / (radiusEnd * distanceEnd)
- return distanceOffset / radiusEnd / distanceEnd;
- }
- else // _curvStart > _curvEnd ("curStart != _curvEnd" guaranteed by checks in caller)
- {
- std::swap(_curvStart, _curvEnd);
- sOffset = length - sOffset;
-
- assert(0.0 != _curvEnd);
-
- double radiusEnd = 1.0 / _curvEnd;
-
- // 1. equation (definition of clothoid): const = radiusStart * distanceStart = radiusEnd * distanceEnd
- // 2. equation: length = distanceEnd - distanceStart
- // -> distanceEnd = radiusStart * length / (radiusStart - radiusEnd)
- // -> formed such that equation copes with _curvStart==0 (infinite radiusStart)
- double distanceEnd = length / (1 - radiusEnd * _curvStart);
- assert(length <= distanceEnd);
-
- double distanceStart = distanceEnd - length;
- double distanceOffset = distanceStart + sOffset;
-
- // equation const = radiusEnd * distanceEnd = radiusOffset * distanceOffset
- // -> curvatureOffset = 1 / radiusOffset = distanceOffset / (radiusEnd * distanceEnd)
- return distanceOffset / radiusEnd / distanceEnd;
- }
+ rl = l_end / c_end;
}
- else // 0.0 >= _curvStart && 0.0 >= _curvEnd
- {
- _curvStart = -_curvStart;
- _curvEnd = -_curvEnd;
-
- if (_curvStart < _curvEnd)
- {
- assert(0.0 != _curvEnd);
-
- double radiusEnd = 1.0 / _curvEnd;
-
- // 1. equation (definition of clothoid): const = radiusStart * distanceStart = radiusEnd * distanceEnd
- // 2. equation: length = distanceEnd - distanceStart
- // -> distanceEnd = radiusStart * length / (radiusStart - radiusEnd)
- // -> formed such that equation copes with _curvStart==0 (infinite radiusStart)
- double distanceEnd = length / (1 - radiusEnd * _curvStart);
- assert(length <= distanceEnd);
-
- double distanceStart = distanceEnd - length;
- double distanceOffset = distanceStart + sOffset;
-
- // equation const = radiusEnd * distanceEnd = radiusOffset * distanceOffset
- // -> curvatureOffset = 1 / radiusOffset = distanceOffset / (radiusEnd * distanceEnd)
- return -distanceOffset / radiusEnd / distanceEnd;
- }
- else // _curvStart > _curvEnd ("curStart != _curvEnd" guaranteed by checks in caller)
- {
- std::swap(_curvStart, _curvEnd);
- sOffset = length - sOffset;
-
- assert(0.0 != _curvEnd);
-
- double radiusEnd = 1.0 / _curvEnd;
-
- // 1. equation (definition of clothoid): const = radiusStart * distanceStart = radiusEnd * distanceEnd
- // 2. equation: length = distanceEnd - distanceStart
- // -> distanceEnd = radiusStart * length / (radiusStart - radiusEnd)
- // -> formed such that equation copes with _curvStart==0 (infinite radiusStart)
- double distanceEnd = length / (1 - radiusEnd * _curvStart);
- assert(length <= distanceEnd);
-
- double distanceStart = distanceEnd - length;
- double distanceOffset = distanceStart + sOffset;
-
- // equation const = radiusEnd * distanceEnd = radiusOffset * distanceOffset
- // -> curvatureOffset = 1 / radiusOffset = distanceOffset / (radiusEnd * distanceEnd)
- return -distanceOffset / radiusEnd / distanceEnd;
- }
- }
-}
-
-double RoadGeometrySpiral::FullCurvature(double sOffset) const
-{
- if ((0.0 <= curvStart && 0.0 <= curvEnd) || (0.0 >= curvStart && 0.0 >= curvEnd))
+ else
{
- return HalfCurvature(sOffset);
+ t_start = 0.0;
+ a = 0.0;
+ sign = 0.0;
+ return;
}
- assert((0.0 > curvStart && 0.0 < curvEnd) || (0.0 < curvStart && 0.0 > curvEnd));
-
- // one degree of freedom: start position/end position can not be determined
- LOG_INTERN(LogLevel::Warning) << "could not calculate spiral curvature";
-
- return 0.0;
+ t_start = 0.5 * l_start * curvStart;
+ a = std::sqrt(std::abs(rl));
+ sign = std::signbit(rl) ? -1.0 : 1.0;
}
-double RoadGeometrySpiral::HalfDir(double sOffset) const
+Common::Vector2d RoadGeometrySpiral::FullCoord(double sOffset, double tOffset) const
{
- double _curvStart = curvStart;
- double _curvEnd = curvEnd;
-
- assert(_curvStart != _curvEnd);
- assert((0.0 <= _curvStart && 0.0 <= _curvEnd) || (0.0 >= _curvStart && 0.0 >= _curvEnd));
-
- if (length < sOffset)
- {
- LOG_INTERN(LogLevel::Warning) << "exceeding length of geometry";
- sOffset = length;
- }
-
- if (0.0 <= _curvStart && 0.0 <= _curvEnd)
- {
- if (_curvStart < _curvEnd)
- {
- assert(0.0 != _curvEnd);
-
- double radiusEnd = 1.0 / _curvEnd;
+ // curvature of the spiral at sOffset
+ double curvAtsOffet = c_start + c_dot * sOffset;
- // 1. equation (definition of clothoid): const = radiusStart * distanceStart = radiusEnd * distanceEnd
- // 2. equation: length = distanceEnd - distanceStart
- // -> distanceEnd = radiusStart * length / (radiusStart - radiusEnd)
- // -> formed such that equation copes with _curvStart==0 (infinite radiusStart)
- double distanceEnd = length / (1 - radiusEnd * _curvStart);
- assert(length <= distanceEnd);
+ // start and end coordinates of spiral
+ Common::Vector2d start, end;
- double distanceStart = distanceEnd - length;
- double a = std::sqrt(2 * radiusEnd * distanceEnd);
+ // calculate x and y of spiral start, assuming sOffset = 0 means start of spiral with curvature curvStart
+ (void)fresnl(l_start / a / SQRT_PI, &start.y, &start.x);
+ start.Scale(a * SQRT_PI);
+ start.y *= sign;
- double distanceOffset = distanceStart + sOffset;
- double tangentAngle = distanceOffset * distanceOffset / a / a;
+ // calculate x and y of spiral end, assuming l_start + sOffset means end of spiral with curvature curvEnd
+ (void)fresnl((l_start + sOffset) / a / SQRT_PI, &end.y, &end.x);
+ end.Scale(a * SQRT_PI);
+ end.y *= sign;
- return hdg + tangentAngle;
- }
- else // _curvStart > _curvEnd ("curStart != _curvEnd" guaranteed by checks in caller)
- {
- std::swap(_curvStart, _curvEnd);
- sOffset = length - sOffset;
-
- assert(0.0 != _curvEnd);
+ // delta x, y from start of spiral to end of spiral
+ auto diff = end - start;
- double radiusEnd = 1.0 / _curvEnd;
+ // tangent angle at end of spiral
+ double t_end = (l_start + sOffset) * curvAtsOffet / 2.0;
- // 1. equation (definition of clothoid): const = radiusStart * distanceStart = radiusEnd * distanceEnd
- // 2. equation: length = distanceEnd - distanceStart
- // -> distanceEnd = radiusStart * length / (radiusStart - radiusEnd)
- // -> formed such that equation copes with _curvStart==0 (infinite radiusStart)
- double distanceEnd = length / (1 - radiusEnd * _curvStart);
- assert(length <= distanceEnd);
+ // heading change of spiral
+ double dHdg = t_end - t_start;
- double distanceStart = distanceEnd - length;
- double a = std::sqrt(2 * radiusEnd * distanceEnd);
+ // rotate delta x, y to match starting direction and origin heading
+ diff.Rotate(-t_start+hdg);
- double distanceOffset = distanceStart + sOffset;
+ // heading at end of spiral
+ auto endHdg = hdg + dHdg;
- double tangentAngle = distanceOffset * distanceOffset / a / a;
+ // calculate t-offset to spiral
+ Common::Vector2d unit{1.0,0};
+ unit.Rotate(endHdg + M_PI_2);
+ unit.Scale(tOffset);
- // compensate for inverted curvatures
- double tangentAngleEnd = distanceEnd * distanceEnd / a / a;
-
- return hdg + tangentAngleEnd - tangentAngle;
- }
- }
- else // 0.0 >= _curvStart && 0.0 >= _curvEnd
- {
- _curvStart = -_curvStart;
- _curvEnd = -_curvEnd;
-
- if (_curvStart < _curvEnd)
- {
- assert(0.0 != _curvEnd);
-
- double radiusEnd = 1.0 / _curvEnd;
-
- // 1. equation (definition of clothoid): const = radiusStart * distanceStart = radiusEnd * distanceEnd
- // 2. equation: length = distanceEnd - distanceStart
- // -> distanceEnd = radiusStart * length / (radiusStart - radiusEnd)
- // -> formed such that equation copes with _curvStart==0 (infinite radiusStart)
- double distanceEnd = length / (1 - radiusEnd * _curvStart);
- assert(length <= distanceEnd);
-
- double distanceStart = distanceEnd - length;
- double a = std::sqrt(2 * radiusEnd * distanceEnd);
-
- double distanceOffset = distanceStart + sOffset;
-
- double tangentAngle = distanceOffset * distanceOffset / a / a;
-
- return hdg - tangentAngle;
- }
- else // _curvStart > _curvEnd ("curStart != _curvEnd" guaranteed by checks in caller)
- {
- std::swap(_curvStart, _curvEnd);
- sOffset = length - sOffset;
-
- assert(0.0 != _curvEnd);
-
- double radiusEnd = 1.0 / _curvEnd;
-
- // 1. equation (definition of clothoid): const = radiusStart * distanceStart = radiusEnd * distanceEnd
- // 2. equation: length = distanceEnd - distanceStart
- // -> distanceEnd = radiusStart * length / (radiusStart - radiusEnd)
- // -> formed such that equation copes with _curvStart==0 (infinite radiusStart)
- double distanceEnd = length / (1 - radiusEnd * _curvStart);
- assert(length <= distanceEnd);
-
- double distanceStart = distanceEnd - length;
- double a = std::sqrt(2 * radiusEnd * distanceEnd);
-
- double distanceOffset = distanceStart + sOffset;
-
- double tangentAngle = distanceOffset * distanceOffset / a / a;
-
- // compensate for inverted curvatures
- double tangentAngleEnd = distanceEnd * distanceEnd / a / a;
+ return diff + unit + Common::Vector2d(x, y);
+}
- return hdg - (tangentAngleEnd - tangentAngle);
- }
- }
+double RoadGeometrySpiral::FullCurvature(double sOffset) const
+{
+ return c_start + c_dot * sOffset;
}
double RoadGeometrySpiral::FullDir(double sOffset) const
{
- if ((0.0 <= curvStart && 0.0 <= curvEnd) || (0.0 >= curvStart && 0.0 >= curvEnd))
- {
- return HalfDir(sOffset);
- }
-
- assert((0.0 > curvStart && 0.0 < curvEnd) || (0.0 < curvStart && 0.0 > curvEnd));
-
- // one degree of freedom: start position/end position can not be determined
- LOG_INTERN(LogLevel::Warning) << "could not calculate spiral curvature";
+ // tangent_angle = L / (2 * R) = 0.5 * L * curv
+ // direction change in spiral = tangent_end - tangent_start
- return 0.0;
+ double curvEnd = FullCurvature(sOffset);
+ return hdg + 0.5 * (curvEnd * (l_start + sOffset) - c_start * l_start);
}
Common::Vector2d RoadGeometrySpiral::GetCoord(double sOffset, double tOffset) const
{
- if (0.0 == curvStart && 0.0 == curvEnd)
+ if (0.0 == c_start && 0.0 == c_end)
{
return GetCoordLine(sOffset, tOffset);
}
- if (std::abs(curvStart - curvEnd) < 1e-6 /* assumed to be equal */)
+ if (std::abs(c_start - c_end) < 1e-6 /* assumed to be equal */)
{
- return GetCoordArc(sOffset, tOffset, curvStart);
+ return GetCoordArc(sOffset, tOffset, c_start);
}
return FullCoord(sOffset, tOffset);
@@ -762,14 +337,14 @@ Common::Vector2d RoadGeometrySpiral::GetCoord(double sOffset, double tOffset) co
double RoadGeometrySpiral::GetDir(double sOffset) const
{
- if (0.0 == curvStart && 0.0 == curvEnd)
+ if (0.0 == c_start && 0.0 == c_end)
{
return GetDirLine(sOffset);
}
- if (std::abs(curvStart - curvEnd) < 1e-6 /* assumed to be equal */)
+ if (std::abs(c_start - c_end) < 1e-6 /* assumed to be equal */)
{
- return GetDirArc(sOffset, curvStart);
+ return GetDirArc(sOffset, c_start);
}
return FullDir(sOffset);
diff --git a/sim/src/core/slave/importer/road.h b/sim/src/core/slave/importer/road.h
index 4e6af45..483a3bb 100644
--- a/sim/src/core/slave/importer/road.h
+++ b/sim/src/core/slave/importer/road.h
@@ -586,97 +586,46 @@ class RoadGeometryArc : public RoadGeometry
//-----------------------------------------------------------------------------
class RoadGeometrySpiral : public RoadGeometry
{
- public:
- RoadGeometrySpiral(double s, double x, double y, double hdg, double length, double curvStart, double curvEnd)
- : RoadGeometry{s, x, y, hdg, length}, curvStart{curvStart}, curvEnd{curvEnd}
- {
- }
+public:
+ RoadGeometrySpiral(double s, double x, double y, double hdg, double length, double curvStart, double curvEnd);
virtual ~RoadGeometrySpiral() override = default;
virtual Common::Vector2d GetCoord(double sOffset, double tOffset) const override;
virtual double GetDir(double sOffset) const override;
+private:
//-----------------------------------------------------------------------------
- //! Returns the curvature at the start of the spiral.
- //!
- //! @return curvature at the start of the spiral
- //-----------------------------------------------------------------------------
- double GetCurvStart() const
- {
- return curvStart;
- }
-
- //-----------------------------------------------------------------------------
- //! Returns the curvature at the end of the spiral.
- //!
- //! @return curvature at the end of the spiral
- //-----------------------------------------------------------------------------
- double GetCurvEnd() const
- {
- return curvEnd;
- }
-
- private:
- //-----------------------------------------------------------------------------
- //! Calculates the x/y coordinates as vector. Only valid if the start and end
- //! curvature of the spiral are either both positive or negative.
+ //! Calculates the x/y coordinates as vector.
//!
//! @param[in] sOffset s offset within geometry section
//! @param[in] tOffset offset to the left
//! @return vector with the x/y coordinates
//-----------------------------------------------------------------------------
- Common::Vector2d HalfCoord(double sOffset, double tOffset) const;
-
- //-----------------------------------------------------------------------------
- //! Calculates the x/y coordinates as vector. Returns an empty vector, if start
- //! and end curvature of the spiral have different signs.
- //!
- //! @param[in] sOffset s offset within geometry section
- //! @param[in] tOffset offset to the left
- //! @return vector with the x/y coordinates, empty vector,
- //! if start and end curvature of the spiral have
- //! different signs
- //-----------------------------------------------------------------------------
Common::Vector2d FullCoord(double sOffset, double tOffset) const;
//-----------------------------------------------------------------------------
- //! Calculates the curvature. Only valid if the start and end
- //! curvature of the spiral are either both positive or negative.
+ //! Calculates the curvature.
//!
//! @param[in] sOffset s offset within geometry section
//! @return curvature
//-----------------------------------------------------------------------------
- double HalfCurvature(double sOffset) const;
-
- //-----------------------------------------------------------------------------
- //! Calculates the curvature. Returns 0.0, if start
- //! and end curvature of the spiral have different signs.
- //!
- //! @param[in] sOffset s offset within geometry section
- //! @return curvature, 0.0, if start and end curvature
- //! of the spiral have different signs
- //-----------------------------------------------------------------------------
double FullCurvature(double sOffset) const;
//-----------------------------------------------------------------------------
- //! Calculates the direction. Only valid if the start and end
- //! curvature of the spiral are either both positive or negative.
- //!
- //! @param[in] sOffset s offset within geometry section
- //! @return direction
- //-----------------------------------------------------------------------------
- double HalfDir(double sOffset) const;
-
- //-----------------------------------------------------------------------------
- //! Calculates the direction. Returns 0.0, if start
- //! and end curvature of the spiral have different signs.
+ //! Calculates the direction.
//!
//! @param[in] sOffset s offset within geometry section
//! @return direction
//-----------------------------------------------------------------------------
double FullDir(double sOffset) const;
- double curvStart;
- double curvEnd;
+
+ double c_start; //!< spiral starting curvature
+ double c_end; //!< spiral end curvature
+ double a; //!< clothoid parameter of spiral
+ double sign; //!< direction of curvature change (needes to correct Fresnel integral results)
+ double c_dot; //!< change of curvature per unit
+ double l_start; //!< offset of starting point along spiral
+ double t_start; //!< tangent angle at start point
};
//-----------------------------------------------------------------------------
diff --git a/sim/tests/unitTests/core/slave/CMakeLists.txt b/sim/tests/unitTests/core/slave/CMakeLists.txt
index 8bb1a5a..dd133fb 100644
--- a/sim/tests/unitTests/core/slave/CMakeLists.txt
+++ b/sim/tests/unitTests/core/slave/CMakeLists.txt
@@ -57,6 +57,7 @@ add_openpass_target(
${COMPONENT_SOURCE_DIR}/importer/scenarioImporterHelper.cpp
# SceneryImporter
+ roadGeometry_Tests.cpp
sceneryImporter_Tests.cpp
${COMPONENT_SOURCE_DIR}/importer/connection.cpp
${COMPONENT_SOURCE_DIR}/importer/junction.cpp
diff --git a/sim/tests/unitTests/core/slave/openPassSlave_Tests.pro b/sim/tests/unitTests/core/slave/openPassSlave_Tests.pro
index 91dc50f..8849468 100644
--- a/sim/tests/unitTests/core/slave/openPassSlave_Tests.pro
+++ b/sim/tests/unitTests/core/slave/openPassSlave_Tests.pro
@@ -101,6 +101,7 @@ SCENERYIMPORTER_TESTS = \
$$UNIT_UNDER_TEST/importer/road/roadSignal.cpp \
$$UNIT_UNDER_TEST/importer/road/roadObject.cpp \
\
+ roadGeometry_Tests.cpp \
sceneryImporter_Tests.cpp
SCENARIOIMPORTER_TESTS = \
diff --git a/sim/tests/unitTests/core/slave/roadGeometry_Tests.cpp b/sim/tests/unitTests/core/slave/roadGeometry_Tests.cpp
new file mode 100644
index 0000000..ff91a32
--- /dev/null
+++ b/sim/tests/unitTests/core/slave/roadGeometry_Tests.cpp
@@ -0,0 +1,267 @@
+/*******************************************************************************
+* Copyright (c) 2020 in-tech GmbH
+*
+* This program and the accompanying materials are made
+* available under the terms of the Eclipse Public License 2.0
+* which is available at https://www.eclipse.org/legal/epl-2.0/
+*
+* SPDX-License-Identifier: EPL-2.0
+*******************************************************************************/
+
+#include "common/opMath.h"
+
+#include "gtest/gtest.h"
+#include "gmock/gmock.h"
+
+#include "road.h"
+
+using ::testing::DoubleNear;
+
+constexpr double MAX_GEOMETRY_ERROR = 0.001;
+
+struct GeometrySpiral_Data
+{
+ double x; //!< geometry origin x
+ double y; //!< geometry origin y
+ double hdg; //!< geometry origin heading
+ double length; //!< lenth of spiral between curvature start and end
+ double curvStart; //!< curvature at s = 0
+ double curvEnd; //!< curvature at s = length
+
+ double s; //!< query s coordinate
+ double t; //!< query t coordinate
+
+ double expected_x; //!< expected x coordinate for query
+ double expected_y; //!< expected y coordinate for query
+ double expected_hdg; //!< expected heading for query
+
+ /// \brief This stream will be shown in case the test fails
+ friend std::ostream& operator<<(std::ostream& os, const GeometrySpiral_Data& obj)
+ {
+ return os
+ << "x: " << obj.x
+ << ", y: " << obj.y
+ << ", hdg: " << obj.hdg
+ << ", length: " << obj.length
+ << ", curvStart: " << obj.curvStart
+ << ", curvend: " << obj.curvEnd
+ << ", s: " << obj.s
+ << ", t: " << obj.t
+ << ", expected_x: " << obj.expected_x
+ << ", expected_y: " << obj.expected_y
+ << ", expected_hdg: " << obj.expected_hdg;
+ }
+};
+
+class RoadGeometries_Spiral : public ::testing::TestWithParam<GeometrySpiral_Data>
+{
+public:
+ virtual ~RoadGeometries_Spiral()
+ {
+ }
+};
+
+TEST_P(RoadGeometries_Spiral, GetCoordAndGetDir_ReturnCorrectValues)
+{
+ GeometrySpiral_Data data = GetParam();
+
+ const RoadGeometrySpiral rgs{0, data.x, data.y, data.hdg, data.length, data.curvStart, data.curvEnd};
+ const auto res = rgs.GetCoord(data.s, data.t);
+ const auto hdg = rgs.GetDir(data.s);
+
+ EXPECT_THAT(res.x, DoubleNear(data.expected_x, MAX_GEOMETRY_ERROR));
+ EXPECT_THAT(res.y, DoubleNear(data.expected_y, MAX_GEOMETRY_ERROR));
+ EXPECT_THAT(hdg, DoubleNear(data.expected_hdg, MAX_GEOMETRY_ERROR));
+}
+
+INSTANTIATE_TEST_SUITE_P(AtEndOfGeometryAndZeroTAndZeroOrigin, RoadGeometries_Spiral, ::testing::Values(
+ // | origin | spirial definition | query pos | expected result |
+ // | x y hdg | len c_start c_end | s t | x y hdg |
+ GeometrySpiral_Data{ 0.0, 0.0, 0.0, 100.0, 0.00, 0.00, 100.0, 0.0, 100.000, 0.000, 0.0000 },
+ GeometrySpiral_Data{ 0.0, 0.0, 0.0, 100.0, 0.00, 0.01, 100.0, 0.0, 97.529, 16.371, 0.5000 },
+ GeometrySpiral_Data{ 0.0, 0.0, 0.0, 100.0, 0.00, -0.01, 100.0, 0.0, 97.529, -16.371, -0.5000 },
+ GeometrySpiral_Data{ 0.0, 0.0, 0.0, 100.0, 0.01, 0.00, 100.0, 0.0, 93.438, 32.391, 0.5000 },
+ GeometrySpiral_Data{ 0.0, 0.0, 0.0, 100.0, -0.01, 0.00, 100.0, 0.0, 93.438, -32.391, -0.5000 }
+));
+
+INSTANTIATE_TEST_SUITE_P(AtEndOfGeometryAndZeroTAndNonzeroOrigin, RoadGeometries_Spiral, ::testing::Values(
+ // | origin | spirial definition | query pos | expected result |
+ // | x y hdg | len c_start c_end | s t | x y hdg |
+ GeometrySpiral_Data{ 1.0, 1.0, 0.0, 100.0, 0.00, 0.00, 100.0, 0.0, 101.000, 1.000, 0.0000 },
+ GeometrySpiral_Data{ -1.0, -1.0, 0.0, 100.0, 0.00, 0.00, 100.0, 0.0, 99.000, -1.000, 0.0000 },
+ GeometrySpiral_Data{ -1.0, 1.0, 0.0, 100.0, 0.00, 0.00, 100.0, 0.0, 99.000, 1.000, 0.0000 },
+ GeometrySpiral_Data{ 1.0, -1.0, 0.0, 100.0, 0.00, 0.00, 100.0, 0.0, 101.000, -1.000, 0.0000},
+
+ GeometrySpiral_Data{ 1.0, 1.0, 0.0, 100.0, 0.00, 0.01, 100.0, 0.0, 98.529, 17.371, 0.5000 },
+ GeometrySpiral_Data{ -1.0, -1.0, 0.0, 100.0, 0.00, 0.01, 100.0, 0.0, 96.529, 15.371, 0.5000 },
+ GeometrySpiral_Data{ -1.0, 1.0, 0.0, 100.0, 0.00, 0.01, 100.0, 0.0, 96.529, 17.371, 0.5000 },
+ GeometrySpiral_Data{ 1.0, -1.0, 0.0, 100.0, 0.00, 0.01, 100.0, 0.0, 98.529, 15.371, 0.5000 }
+));
+
+INSTANTIATE_TEST_SUITE_P(AtEndOfGeometryAndNonzeroTAndNonzeroOrigin, RoadGeometries_Spiral, ::testing::Values(
+ // | origin | spirial definition | query pos | expected result |
+ // | x y hdg | len c_start c_end | s t | x y hdg |
+ GeometrySpiral_Data{ 1.0, 1.0, 0.0, 100.0, 0.00, 0.01, 100.0, 1.0, 98.049, 18.249, 0.5000 },
+ GeometrySpiral_Data{ -1.0, -1.0, 0.0, 100.0, 0.00, 0.01, 100.0, 1.0, 96.049, 16.249, 0.5000 },
+ GeometrySpiral_Data{ -1.0, 1.0, 0.0, 100.0, 0.00, 0.01, 100.0, 1.0, 96.049, 18.249, 0.5000 },
+ GeometrySpiral_Data{ 1.0, -1.0, 0.0, 100.0, 0.00, 0.01, 100.0, 1.0, 98.049, 16.249, 0.5000 },
+ GeometrySpiral_Data{ 1.0, 1.0, 0.0, 100.0, 0.00, 0.01, 100.0, -1.0, 99.008, 16.494, 0.5000 },
+ GeometrySpiral_Data{ -1.0, -1.0, 0.0, 100.0, 0.00, 0.01, 100.0, -1.0, 97.008, 14.494, 0.5000 },
+ GeometrySpiral_Data{ -1.0, 1.0, 0.0, 100.0, 0.00, 0.01, 100.0, -1.0, 97.008, 16.494, 0.5000 },
+ GeometrySpiral_Data{ 1.0, -1.0, 0.0, 100.0, 0.00, 0.01, 100.0, -1.0, 99.008, 14.494, 0.5000 }
+));
+
+INSTANTIATE_TEST_SUITE_P(AtEndOfGeometryAndZeroTAndNonzeroOriginHeading, RoadGeometries_Spiral, ::testing::Values(
+ // | origin | spirial definition | query pos | expected result |
+ // | x y hdg | len c_start c_end | s t | x y hdg |
+ GeometrySpiral_Data{ 0.0, 0.0, M_PI_4, 100.0, 0.00, 0.00, 100.0, 0.0, 70.711, 70.711, 0.7854 },
+ GeometrySpiral_Data{ 0.0, 0.0, 3 * M_PI_4, 100.0, 0.00, 0.00, 100.0, 0.0, -70.711, 70.711, 2.3570 },
+ GeometrySpiral_Data{ 0.0, 0.0, -3 * M_PI_4, 100.0, 0.00, 0.00, 100.0, 0.0, -70.711, -70.711, -2.3570 },
+ GeometrySpiral_Data{ 0.0, 0.0, 5 * M_PI_4, 100.0, 0.00, 0.00, 100.0, 0.0, -70.711, -70.711, 3.9270 },
+ GeometrySpiral_Data{ 0.0, 0.0, -M_PI_4, 100.0, 0.00, 0.00, 100.0, 0.0, 70.711, -70.711, -0.7854 },
+ GeometrySpiral_Data{ 0.0, 0.0, 7 * M_PI_4, 100.0, 0.00, 0.00, 100.0, 0.0, 70.711, -70.711, 5.4978 },
+
+ GeometrySpiral_Data{ 0.0, 0.0, M_PI_4, 100.0, 0.00, 0.01, 100.0, 0.0, 57.387, 80.540, 1.2854 },
+ GeometrySpiral_Data{ 0.0, 0.0, 3 * M_PI_4, 100.0, 0.00, 0.01, 100.0, 0.0, -80.540, 57.387, 2.8562 },
+ GeometrySpiral_Data{ 0.0, 0.0, -3 * M_PI_4, 100.0, 0.00, 0.01, 100.0, 0.0, -57.387, -80.540, -1.8562 },
+ GeometrySpiral_Data{ 0.0, 0.0, 5 * M_PI_4, 100.0, 0.00, 0.01, 100.0, 0.0, -57.387, -80.540, 4.4270 },
+ GeometrySpiral_Data{ 0.0, 0.0, -M_PI_4, 100.0, 0.00, 0.01, 100.0, 0.0, 80.540, -57.387, -0.2854 },
+ GeometrySpiral_Data{ 0.0, 0.0, 7 * M_PI_4, 100.0, 0.00, 0.01, 100.0, 0.0, 80.540, -57.387, 5.9978 }
+));
+
+INSTANTIATE_TEST_SUITE_P(AtEndOfGeometryAndZeroTAndNonzeroOriginAndHeading, RoadGeometries_Spiral, ::testing::Values(
+ // | origin | spirial definition | query pos | expected result |
+ // | x y hdg | len c_start c_end | s t | x y hdg |
+ GeometrySpiral_Data{ 1.0, 1.0, M_PI_4, 100.0, 0.00, 0.01, 100.0, 0.0, 58.387, 81.540, 1.2854 },
+ GeometrySpiral_Data{ 1.0, 1.0, -M_PI_4, 100.0, 0.00, 0.01, 100.0, 0.0, 81.540, -56.387, -0.2854 },
+ GeometrySpiral_Data{ -1.0, -1.0, 3 * M_PI_4, 100.0, 0.00, -0.01, 100.0, 0.0, -58.387, 79.540, 1.8562 },
+ GeometrySpiral_Data{ -1.0, 1.0, -3 * M_PI_4, 100.0, 0.00, -0.01, 100.0, 0.0, -81.540, -56.387, -2.8562 },
+ GeometrySpiral_Data{ 1.0, -1.0, 5 * M_PI_4, 100.0, 0.00, -0.01, 100.0, 0.0, -79.540, -58.387, 3.4270 }
+));
+
+INSTANTIATE_TEST_SUITE_P(AtEndOfGeometryAndZeroTWithNonzeroStartAndEndCurvature, RoadGeometries_Spiral, ::testing::Values(
+ // | origin | spirial definition | query pos | expected result |
+ // | x y hdg | len c_start c_end | s t | x y hdg |
+ GeometrySpiral_Data{ 0.0, 0.0, 0.0, 100.0, 0.01, 0.02, 100.0, 0.0, 71.564, 55.928, 1.5 },
+ GeometrySpiral_Data{ 0.0, 0.0, M_PI_4, 100.0, 0.01, 0.02, 100.0, 0.0, 11.057, 90.151, 2.2854 },
+ GeometrySpiral_Data{ 0.0, 0.0, -M_PI_4, 100.0, 0.01, 0.02, 100.0, 0.0, 90.151, -11.057, 0.7146 },
+ GeometrySpiral_Data{ 0.0, 0.0, 3 * M_PI_4, 100.0, 0.01, 0.02, 100.0, 0.0, -90.151, 11.057, 3.8562 },
+ GeometrySpiral_Data{ 0.0, 0.0, -3 * M_PI_4, 100.0, 0.01, 0.02, 100.0, 0.0, -11.057, -90.151, -0.8562 },
+
+ GeometrySpiral_Data{ 0.0, 0.0, 0.0, 100.0, -0.01, -0.02, 100.0, 0.0, 71.564, -55.928, -1.5 },
+ GeometrySpiral_Data{ 0.0, 0.0, M_PI_4, 100.0, -0.01, -0.02, 100.0, 0.0, 90.151, 11.057, -0.7146 },
+ GeometrySpiral_Data{ 0.0, 0.0, -M_PI_4, 100.0, -0.01, -0.02, 100.0, 0.0, 11.057, -90.151, -2.2854 },
+ GeometrySpiral_Data{ 0.0, 0.0, 3 * M_PI_4, 100.0, -0.01, -0.02, 100.0, 0.0, -11.057, 90.151, 0.8562 },
+ GeometrySpiral_Data{ 0.0, 0.0, -3 * M_PI_4, 100.0, -0.01, -0.02, 100.0, 0.0, -90.151, -11.057, -3.8562 },
+
+ GeometrySpiral_Data{ 0.0, 0.0, 0.0, 100.0, 0.02, 0.01, 100.0, 0.0, 60.850, 67.429, 1.5 },
+ GeometrySpiral_Data{ 0.0, 0.0, M_PI_4, 100.0, 0.02, 0.01, 100.0, 0.0, -4.652, 90.707, 2.2854 },
+ GeometrySpiral_Data{ 0.0, 0.0, -M_PI_4, 100.0, 0.02, 0.01, 100.0, 0.0, 90.707, 4.652, 0.7146 },
+ GeometrySpiral_Data{ 0.0, 0.0, 3 * M_PI_4, 100.0, 0.02, 0.01, 100.0, 0.0, -90.707, -4.652, 3.8562 },
+ GeometrySpiral_Data{ 0.0, 0.0, -3 * M_PI_4, 100.0, 0.02, 0.01, 100.0, 0.0, 4.652, -90.707, -0.8562 },
+
+ GeometrySpiral_Data{ 0.0, 0.0, 0.0, 100.0, -0.02, -0.01, 100.0, 0.0, 60.850, -67.429, -1.5 },
+ GeometrySpiral_Data{ 0.0, 0.0, M_PI_4, 100.0, -0.02, -0.01, 100.0, 0.0, 90.707, -4.652, -0.7146 },
+ GeometrySpiral_Data{ 0.0, 0.0, -M_PI_4, 100.0, -0.02, -0.01, 100.0, 0.0, -4.652, -90.707, -2.2854 },
+ GeometrySpiral_Data{ 0.0, 0.0, 3 * M_PI_4, 100.0, -0.02, -0.01, 100.0, 0.0, 4.652, 90.707, 0.8562 },
+ GeometrySpiral_Data{ 0.0, 0.0, -3 * M_PI_4, 100.0, -0.02, -0.01, 100.0, 0.0, -90.707, 4.652, -3.8562 }
+));
+
+INSTANTIATE_TEST_SUITE_P(AtEndOfGeometryAndZeroTWithNonzeroStartAndEndCurvatureAndNonzeroOrigin, RoadGeometries_Spiral, ::testing::Values(
+ // | origin | spirial definition | query pos | expected result |
+ // | x y hdg | len c_start c_end | s t | x y hdg |
+ GeometrySpiral_Data{ 0.0, 1.0, 0.0, 100.0, 0.01, 0.02, 100.0, 0.0, 71.564, 56.928, 1.5 },
+ GeometrySpiral_Data{ 1.0, 0.0, M_PI_4, 100.0, 0.01, 0.02, 100.0, 0.0, 12.057, 90.151, 2.2854 },
+ GeometrySpiral_Data{ 1.0, 0.0, -M_PI_4, 100.0, 0.01, 0.02, 100.0, 0.0, 91.151, -11.057, 0.7146 },
+ GeometrySpiral_Data{ 1.0, 0.0, 3 * M_PI_4, 100.0, 0.01, 0.02, 100.0, 0.0, -89.151, 11.057, 3.8562 },
+ GeometrySpiral_Data{ 0.0, 1.0, -3 * M_PI_4, 100.0, 0.01, 0.02, 100.0, 0.0, -11.057, -89.151, -0.8562 },
+
+ GeometrySpiral_Data{ 0.0, 1.0, 0.0, 100.0, -0.01, -0.02, 100.0, 0.0, 71.564, -54.928, -1.5 },
+ GeometrySpiral_Data{ 1.0, 0.0, M_PI_4, 100.0, -0.01, -0.02, 100.0, 0.0, 91.151, 11.057, -0.7146 },
+ GeometrySpiral_Data{ 1.0, 0.0, -M_PI_4, 100.0, -0.01, -0.02, 100.0, 0.0, 12.057, -90.151, -2.2854 },
+ GeometrySpiral_Data{ 1.0, 0.0, 3 * M_PI_4, 100.0, -0.01, -0.02, 100.0, 0.0, -10.057, 90.151, 0.8562 },
+ GeometrySpiral_Data{ 0.0, 1.0, -3 * M_PI_4, 100.0, -0.01, -0.02, 100.0, 0.0, -90.151, -10.057, -3.8562 },
+
+ GeometrySpiral_Data{ 0.0, 1.0, 0.0, 100.0, 0.02, 0.01, 100.0, 0.0, 60.850, 68.429, 1.5 },
+ GeometrySpiral_Data{ 1.0, 0.0, M_PI_4, 100.0, 0.02, 0.01, 100.0, 0.0, -3.652, 90.707, 2.2854 },
+ GeometrySpiral_Data{ 1.0, 0.0, -M_PI_4, 100.0, 0.02, 0.01, 100.0, 0.0, 91.707, 4.652, 0.7146 },
+ GeometrySpiral_Data{ 1.0, 0.0, 3 * M_PI_4, 100.0, 0.02, 0.01, 100.0, 0.0, -89.707, -4.652, 3.8562 },
+ GeometrySpiral_Data{ 0.0, 1.0, -3 * M_PI_4, 100.0, 0.02, 0.01, 100.0, 0.0, 4.652, -89.707, -0.8562 },
+
+ GeometrySpiral_Data{ 0.0, 1.0, 0.0, 100.0, -0.02, -0.01, 100.0, 0.0, 60.850, -66.429, -1.5 },
+ GeometrySpiral_Data{ 1.0, 0.0, M_PI_4, 100.0, -0.02, -0.01, 100.0, 0.0, 91.707, -4.652, -0.7146 },
+ GeometrySpiral_Data{ 1.0, 0.0, -M_PI_4, 100.0, -0.02, -0.01, 100.0, 0.0, -3.652, -90.707, -2.2854 },
+ GeometrySpiral_Data{ 1.0, 0.0, 3 * M_PI_4, 100.0, -0.02, -0.01, 100.0, 0.0, 5.652, 90.707, 0.8562 },
+ GeometrySpiral_Data{ 0.0, 1.0, -3 * M_PI_4, 100.0, -0.02, -0.01, 100.0, 0.0, -90.707, 5.652, -3.8562 }
+));
+
+INSTANTIATE_TEST_SUITE_P(AtEndOfGeometryAndNonzeroTWithNonzeroStartAndEndCurvatureAndNonzeroOrigin, RoadGeometries_Spiral, ::testing::Values(
+ // | origin | spirial definition | query pos | expected result |
+ // | x y hdg | len c_start c_end | s t | x y hdg |
+ GeometrySpiral_Data{ 0.0, 1.0, 0.0, 100.0, 0.01, 0.02, 100.0, 2.0, 69.569, 57.069, 1.5 },
+ GeometrySpiral_Data{ 1.0, 0.0, M_PI_4, 100.0, 0.01, 0.02, 100.0, -2.0, 13.568, 91.461, 2.2854 },
+ GeometrySpiral_Data{ 1.0, 1.0, -M_PI_4, 100.0, 0.01, 0.02, 100.0, 2.0, 89.840, -8.546, 0.7146 },
+ GeometrySpiral_Data{ 1.0, 0.0, 3 * M_PI_4, 100.0, 0.01, 0.02, 100.0, -2.0, -90.461, 12.568, 3.8562 },
+ GeometrySpiral_Data{ 0.0, 1.0, -3 * M_PI_4, 100.0, 0.01, 0.02, 100.0, 2.0, -9.546, -87.840, -0.8562 },
+
+ GeometrySpiral_Data{ 0.0, 1.0, 0.0, 100.0, -0.01, -0.02, 100.0, -2.0, 69.569, -55.069, -1.5 },
+ GeometrySpiral_Data{ 1.0, 0.0, M_PI_4, 100.0, -0.01, -0.02, 100.0, 2.0, 92.461, 12.568, -0.7146 },
+ GeometrySpiral_Data{ 1.0, 1.0, -M_PI_4, 100.0, -0.01, -0.02, 100.0, -2.0, 10.546, -87.840, -2.2854 },
+ GeometrySpiral_Data{ 1.0, 0.0, 3 * M_PI_4, 100.0, -0.01, -0.02, 100.0, 2.0, -11.568, 91.461, 0.8562 },
+ GeometrySpiral_Data{ 0.0, 1.0, -3 * M_PI_4, 100.0, -0.01, -0.02, 100.0, -2.0, -88.840, -8.546, -3.8562 },
+
+ GeometrySpiral_Data{ 0.0, 1.0, 0.0, 100.0, 0.02, 0.01, 100.0, 2.0, 58.855, 68.570, 1.5 },
+ GeometrySpiral_Data{ 1.0, 0.0, M_PI_4, 100.0, 0.02, 0.01, 100.0, 2.0, -5.163, 89.396, 2.2854 },
+ GeometrySpiral_Data{ 1.0, 1.0, -M_PI_4, 100.0, 0.02, 0.01, 100.0, -2.0, 93.017, 4.141, 0.7146 },
+ GeometrySpiral_Data{ 1.0, 0.0, 3 * M_PI_4, 100.0, 0.02, 0.01, 100.0, -2.0, -91.017, -3.141, 3.8562 },
+ GeometrySpiral_Data{ 0.0, 1.0, -3 * M_PI_4, 100.0, 0.02, 0.01, 100.0, -2.0, 3.141, -91.017, -0.8562 },
+
+ GeometrySpiral_Data{ 0.0, 1.0, 0.0, 100.0, -0.02, -0.01, 100.0, -2.0, 58.855, -66.570, -1.5 },
+ GeometrySpiral_Data{ 1.0, 0.0, M_PI_4, 100.0, -0.02, -0.01, 100.0, -2.0, 90.396, -6.163, -0.7146 },
+ GeometrySpiral_Data{ 1.0, 1.0, -M_PI_4, 100.0, -0.02, -0.01, 100.0, -2.0, -5.163, -88.396, -2.2854 },
+ GeometrySpiral_Data{ 1.0, 0.0, 3 * M_PI_4, 100.0, -0.02, -0.01, 100.0, 2.0, 4.141, 92.017, 0.8562 },
+ GeometrySpiral_Data{ 0.0, 1.0, -3 * M_PI_4, 100.0, -0.02, -0.01, 100.0, 2.0, -92.017, 4.141, -3.8562 }
+));
+
+INSTANTIATE_TEST_SUITE_P(AtCenterOfGeometryAndNonzeroTWithNonzeroStartAndEndCurvatureAndNonzeroOrigin, RoadGeometries_Spiral, ::testing::Values(
+ // | origin | spirial definition | query pos | expected result |
+ // | x y hdg | len c_start c_end | s t | x y hdg |
+ GeometrySpiral_Data{ 0.0, 1.0, 0.0, 100.0, 0.01, 0.02, 50.0, 2.0, 45.942, 16.759, 0.6250 },
+ GeometrySpiral_Data{ 1.0, 0.0, M_PI_4, 100.0, 0.01, 0.02, 50.0, -2.0, 26.291, 42.991, 1.4104 },
+ GeometrySpiral_Data{ 1.0, 1.0, -M_PI_4, 100.0, 0.01, 0.02, 50.0, 2.0, 44.630, -20.343, -0.1604 },
+ GeometrySpiral_Data{ 1.0, 0.0, 3 * M_PI_4, 100.0, 0.01, 0.02, 50.0, -2.0, -41.991, 25.291, 2.9812 },
+ GeometrySpiral_Data{ 0.0, 1.0, -3 * M_PI_4, 100.0, 0.01, 0.02, 50.0, 2.0, -21.343, -42.630, -1.7312 },
+
+ GeometrySpiral_Data{ 0.0, 1.0, 0.0, 100.0, -0.01, -0.02, 50.0, 2.0, 48.283, -11.516, -0.6250 },
+ GeometrySpiral_Data{ 1.0, 0.0, M_PI_4, 100.0, -0.01, -0.02, 50.0, -2.0, 44.630, 21.343, 0.1604 },
+ GeometrySpiral_Data{ 1.0, 1.0, -M_PI_4, 100.0, -0.01, -0.02, 50.0, 2.0, 26.291, -41.991, -1.4104 },
+ GeometrySpiral_Data{ 1.0, 0.0, 3 * M_PI_4, 100.0, -0.01, -0.02, 50.0, -2.0, -20.343, 43.630, 1.7312 },
+ GeometrySpiral_Data{ 0.0, 1.0, -3 * M_PI_4, 100.0, -0.01, -0.02, 50.0, 2.0, -42.991, -24.291, -2.9812 },
+
+ GeometrySpiral_Data{ 0.0, 1.0, 0.0, 100.0, 0.02, 0.01, 50.0, 2.0, 41.880, 23.716, 0.8750 },
+ GeometrySpiral_Data{ 1.0, 0.0, M_PI_4, 100.0, 0.02, 0.01, 50.0, 2.0, 14.551, 45.677, 1.6604 },
+ GeometrySpiral_Data{ 1.0, 1.0, -M_PI_4, 100.0, 0.02, 0.01, 50.0, -2.0, 47.035, -16.535, 0.0896 },
+ GeometrySpiral_Data{ 1.0, 0.0, 3 * M_PI_4, 100.0, 0.02, 0.01, 50.0, -2.0, -45.035, 17.535, 3.2312 },
+ GeometrySpiral_Data{ 0.0, 1.0, -3 * M_PI_4, 100.0, 0.02, 0.01, 50.0, -2.0, -17.535, -45.035, -1.4812 },
+
+ GeometrySpiral_Data{ 0.0, 1.0, 0.0, 100.0, -0.02, -0.01, 50.0, -2.0, 41.880, -21.716, -0.8750 },
+ GeometrySpiral_Data{ 1.0, 0.0, M_PI_4, 100.0, -0.02, -0.01, 50.0, -2.0, 46.677, 13.551, -0.0896 },
+ GeometrySpiral_Data{ 1.0, 1.0, -M_PI_4, 100.0, -0.02, -0.01, 50.0, -2.0, 14.551, -44.677, -1.6604 },
+ GeometrySpiral_Data{ 1.0, 0.0, 3 * M_PI_4, 100.0, -0.02, -0.01, 50.0, 2.0, -16.535, 46.035, 1.4812 },
+ GeometrySpiral_Data{ 0.0, 1.0, -3 * M_PI_4, 100.0, -0.02, -0.01, 50.0, 2.0, -46.035, -16.535, -3.2312 }
+));
+
+INSTANTIATE_TEST_SUITE_P(AtEndOfGeometryAndZeroTWithAlternatingCurvatureSign, RoadGeometries_Spiral, ::testing::Values(
+ // | origin | spirial definition | query pos | expected result |
+ // | x y hdg | len c_start c_end | s t | x y hdg |
+ GeometrySpiral_Data{ -49.688, -4.148, 0.25, 100.0, -0.01, 0.01, 50.0, 0.0, 0.000, 0.000, 0.0000 },
+ GeometrySpiral_Data{ -49.688, 4.148, -0.25, 100.0, 0.01, -0.01, 50.0, 0.0, 0.000, 0.000, 0.0000 },
+ GeometrySpiral_Data{ 0.0, 0.0, 0.0, 100.0, -0.01, 0.01, 50.0, 0.0, 49.170, -8.274, -0.2500 },
+ GeometrySpiral_Data{ 0.0, 0.0, 0.0, 100.0, -0.01, 0.01, 100.0, 0.0, 98.340, -16.548, 0.0000 },
+ GeometrySpiral_Data{ 0.0, 0.0, 0.0, 100.0, 0.01, -0.01, 50.0, 0.0, 49.170, 8.274, 0.2500 },
+ GeometrySpiral_Data{ 0.0, 0.0, 0.0, 100.0, 0.01, -0.01, 100.0, 0.0, 98.340, 16.548, 0.0000 },
+ GeometrySpiral_Data{ 0.0, 0.0, 0.0, 100.0, -0.02, 0.01, 100.0, 0.0, 86.252, -47.254, -0.5000 },
+ GeometrySpiral_Data{ 0.0, 0.0, 0.0, 100.0, -0.02, 0.01, 50.0, 0.0, 45.747, -18.029, -0.6250 },
+ GeometrySpiral_Data{ 0.0, 0.0, 0.0, 100.0, 0.02, -0.01, 100.0, 0.0, 86.252, 47.254, 0.5000 },
+ GeometrySpiral_Data{ 0.0, 0.0, 0.0, 100.0, 0.02, -0.01, 50.0, 0.0, 45.747, 18.029, 0.6250 }
+));

Back to the top