SpiralGroundCurve: initial implementation.

- Partial implementation of the SpiralGroundCurve.
- Test coverage for the entire class when the curve is built with normalized parameters.

Signed-off-by: Agustin Alba Chicar <[email protected]>

src/maliput_malidrive/road_curve/CMakeLists.txt

@@ -10,6 +10,7 @@ add_library(road_curve
   piecewise_ground_curve.cc
   road_curve.cc
   road_curve_offset.cc
+  spiral_ground_curve.cc
 )
 add_library(maliput_malidrive::road_curve ALIAS road_curve)
 set_target_properties(road_curve

src/maliput_malidrive/road_curve/spiral_ground_curve.cc

@@ -0,0 +1,206 @@
+// BSD 3-Clause License
+//
+// Copyright (c) 2024, Woven Planet. All rights reserved.
+//
+// Redistribution and use in source and binary forms, with or without
+// modification, are permitted provided that the following conditions are met:
+//
+// * Redistributions of source code must retain the above copyright notice, this
+//   list of conditions and the following disclaimer.
+//
+// * Redistributions in binary form must reproduce the above copyright notice,
+//   this list of conditions and the following disclaimer in the documentation
+//   and/or other materials provided with the distribution.
+//
+// * Neither the name of the copyright holder nor the names of its
+//   contributors may be used to endorse or promote products derived from
+//   this software without specific prior written permission.
+//
+// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
+// AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
+// IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
+// DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE
+// FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL
+// DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR
+// SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
+// CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY,
+// OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE
+// OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
+#include "maliput_malidrive/road_curve/spiral_ground_curve.h"
+
+#include <algorithm>
+#include <array>
+#include <cmath>
+#include <numeric>
+
+#include <maliput/math/saturate.h>
+
+namespace malidrive {
+namespace road_curve {
+namespace {
+// Number of coefficients for the series expansion of Fresnel sin and cosine.
+static constexpr size_t kNumCoefficients{10};
+
+// Computes the factorial of @p n at compile time whenever possible.
+// @param n A non-negative integer to compute the factorial to.
+// @return n!
+constexpr long Factorial(const long n) { return n <= 1L ? 1L : n * Factorial(n - 1L); }
+
+// Computes @p base ^ @p exponent at compile time whenever possible.
+// @param base The base of the exponential function.
+// @param exponent The exponent of the exponential function.
+// @return @p base ^ @p exponent.
+constexpr double PowerOf(const double base, const long exponent) {
+  return exponent == 0L ? 1L : exponent == 1L ? base : base * PowerOf(base, exponent - 1L);
+}
+
+// @return The coefficients of the series expansion for a Fresnel cosine.
+constexpr std::array<double, kNumCoefficients> GetFresnelCosCoefficients() {
+  constexpr std::array<double, kNumCoefficients> coeff{
+      PowerOf(-1., 0) / (double{Factorial(2L * 0L)} * (4. * 0. + 1.)),
+      PowerOf(-1., 1) / (double{Factorial(2L * 1L)} * (4. * 1. + 1.)),
+      PowerOf(-1., 2) / (double{Factorial(2L * 2L)} * (4. * 2. + 1.)),
+      PowerOf(-1., 3) / (double{Factorial(2L * 3L)} * (4. * 3. + 1.)),
+      PowerOf(-1., 4) / (double{Factorial(2L * 4L)} * (4. * 4. + 1.)),
+      PowerOf(-1., 5) / (double{Factorial(2L * 5L)} * (4. * 5. + 1.)),
+      PowerOf(-1., 6) / (double{Factorial(2L * 6L)} * (4. * 6. + 1.)),
+      PowerOf(-1., 7) / (double{Factorial(2L * 7L)} * (4. * 7. + 1.)),
+      PowerOf(-1., 8) / (double{Factorial(2L * 8L)} * (4. * 8. + 1.)),
+      PowerOf(-1., 9) / (double{Factorial(2L * 9L)} * (4. * 9. + 1.)),
+  };
+  return coeff;
+}
+
+// @return The coefficients of the series expansion for a Fresnel sine.
+constexpr std::array<double, kNumCoefficients> GetFresnelSinCoefficients() {
+  constexpr std::array<double, kNumCoefficients> coeff{
+      PowerOf(-1., 0) / (double{Factorial(2L * 0L + 1L)} * (4. * 0. + 3.)),
+      PowerOf(-1., 1) / (double{Factorial(2L * 1L + 1L)} * (4. * 1. + 3.)),
+      PowerOf(-1., 2) / (double{Factorial(2L * 2L + 1L)} * (4. * 2. + 3.)),
+      PowerOf(-1., 3) / (double{Factorial(2L * 3L + 1L)} * (4. * 3. + 3.)),
+      PowerOf(-1., 4) / (double{Factorial(2L * 4L + 1L)} * (4. * 4. + 3.)),
+      PowerOf(-1., 5) / (double{Factorial(2L * 5L + 1L)} * (4. * 5. + 3.)),
+      PowerOf(-1., 6) / (double{Factorial(2L * 6L + 1L)} * (4. * 6. + 3.)),
+      PowerOf(-1., 7) / (double{Factorial(2L * 7L + 1L)} * (4. * 7. + 3.)),
+      PowerOf(-1., 8) / (double{Factorial(2L * 8L + 1L)} * (4. * 8. + 3.)),
+      PowerOf(-1., 9) / (double{Factorial(2L * 9L + 1L)} * (4. * 9. + 3.)),
+  };
+  return coeff;
+}
+
+// @return The exponents of series expansion for a Fresnel cosine.
+constexpr std::array<double, kNumCoefficients> GetFresnelCosExponents() {
+  constexpr std::array<double, kNumCoefficients> exponents{
+      4. * 0. + 1., 4. * 1. + 1., 4. * 2. + 1., 4. * 3. + 1., 4. * 4. + 1.,
+      4. * 5. + 1., 4. * 6. + 1., 4. * 7. + 1., 4. * 8. + 1., 4. * 9. + 1.,
+  };
+  return exponents;
+}
+
+// @return The exponents of series expansion for a Fresnel sine.
+constexpr std::array<double, kNumCoefficients> GetFresnelSinExponents() {
+  constexpr std::array<double, kNumCoefficients> exponents{
+      4. * 0. + 3., 4. * 1. + 3., 4. * 2. + 3., 4. * 3. + 3., 4. * 4. + 3.,
+      4. * 5. + 3., 4. * 6. + 3., 4. * 7. + 3., 4. * 8. + 3., 4. * 9. + 3.,
+  };
+  return exponents;
+}
+
+// Computes the position of a Fresnel spiral at @p t.
+// @param t The normalized spiral parameter.
+// @return The position of a Fresnel spiral at @p t.
+maliput::math::Vector2 FresnelSpiral(double t) {
+  static constexpr std::array<double, kNumCoefficients> cos_exponents = GetFresnelCosExponents();
+  static constexpr std::array<double, kNumCoefficients> sin_exponents = GetFresnelSinExponents();
+  static constexpr std::array<double, kNumCoefficients> cos_coefficients = GetFresnelCosCoefficients();
+  static constexpr std::array<double, kNumCoefficients> sin_coefficients = GetFresnelSinCoefficients();
+
+  std::array<double, kNumCoefficients> x_terms{};
+  std::array<double, kNumCoefficients> y_terms{};
+  std::fill(x_terms.begin(), x_terms.end(), t);
+  std::fill(y_terms.begin(), y_terms.end(), t);
+
+  for (size_t i = 0; i < kNumCoefficients; ++i) {
+    x_terms[i] = PowerOf(x_terms[i], cos_exponents[i]);
+    y_terms[i] = PowerOf(y_terms[i], sin_exponents[i]);
+  }
+
+  return maliput::math::Vector2{
+      std::inner_product(x_terms.begin(), x_terms.end(), cos_coefficients.begin(), 0.),
+      std::inner_product(y_terms.begin(), y_terms.end(), sin_coefficients.begin(), 0.),
+  };
+}
+
+// @return A vector equivalent of @p v rotated by @p theta.
+inline maliput::math::Vector2 Rotate2dVector(const maliput::math::Vector2& v, double theta) {
+  const double cos_theta = std::cos(theta);
+  const double sin_theta = std::sin(theta);
+  return maliput::math::Vector2(v.x() * cos_theta - v.y() * sin_theta, v.x() * sin_theta + v.y() * cos_theta);
+}
+
+}  // namespace
+
+SpiralGroundCurve::SpiralGroundCurve(double linear_tolerance, const maliput::math::Vector2& xy0, double heading0,
+                                     double curvature0, double curvature1, double arc_length, double p0, double p1)
+    : linear_tolerance_(linear_tolerance),
+      xy0_(xy0),
+      heading0_(heading0),
+      curvature0_(curvature0),
+      curvature1_(curvature1),
+      arc_length_(arc_length),
+      p0_(p0),
+      p1_(p1),
+      validate_p_(maliput::common::RangeValidator::GetAbsoluteEpsilonValidator(p0_, p1_, linear_tolerance_,
+                                                                               GroundCurve::kEpsilon)),
+      k_dot_{(curvature1 - curvature0) / arc_length},
+      norm_{1. / std::sqrt(k_dot_)},
+      spiral_p0_{curvature0 * arc_length / (curvature1 - curvature0)},
+      spiral_heading0_{std::atan2(std::sin(PowerOf(spiral_p0_, 2)), std::cos(PowerOf(spiral_p0_, 2)))},
+      spiral_xy0_{FresnelSpiral(spiral_p0_ / norm_)} {
+  MALIDRIVE_THROW_UNLESS(linear_tolerance_ > 0.);
+  MALIDRIVE_THROW_UNLESS(arc_length_ >= GroundCurve::kEpsilon);
+  MALIDRIVE_THROW_UNLESS(p0_ >= 0.);
+  MALIDRIVE_THROW_UNLESS(p1_ - p0_ >= GroundCurve::kEpsilon);
+  MALIDRIVE_THROW_UNLESS(std::fabs(curvature1_ - curvature0_) >= GroundCurve::kEpsilon);
+  MALIDRIVE_THROW_UNLESS(std::signbit(curvature1_) == std::signbit(curvature0_));
+}
+
+double SpiralGroundCurve::DoPFromP(double xodr_p) const { return validate_p_(xodr_p); }
+
+double SpiralGroundCurve::NormalizedSpiralCoordinateAt(double p) const { return (p - p0_ + spiral_p0_) / norm_; }
+
+maliput::math::Vector2 SpiralGroundCurve::DoG(double p) const {
+  p = validate_p_(p);
+  const double t = NormalizedSpiralCoordinateAt(p);
+  // TODO(#265): when k_dot_ is negative, y must be multiplied by -1.
+  const maliput::math::Vector2 spiral_pos = (FresnelSpiral(t) - spiral_xy0_) * norm_;
+  return xy0_ + Rotate2dVector(spiral_pos, heading0_ - spiral_heading0_);
+}
+
+maliput::math::Vector2 SpiralGroundCurve::DoGDot(double p) const {
+  p = validate_p_(p);
+  const double t = NormalizedSpiralCoordinateAt(p);
+  // TODO(#265): when k_dot_ is negative, y must be multiplied by -1.
+  return maliput::math::Vector2{std::cos(PowerOf(t, 2)), std::sin(PowerOf(t, 2))} * norm_;
+}
+
+double SpiralGroundCurve::DoGInverse(const maliput::math::Vector2&) const {
+  MALIPUT_THROW_MESSAGE("Unimplemented: SpiralGroundCurve::DoGInverse().");
+  return {};
+}
+
+double SpiralGroundCurve::DoHeading(double p) const {
+  p = validate_p_(p);
+  const double t = NormalizedSpiralCoordinateAt(p);
+  const maliput::math::Vector2 g_dot = GDot(t);
+  return std::atan2(g_dot.y(), g_dot.x());
+}
+
+double SpiralGroundCurve::DoHeadingDot(double p) const {
+  p = validate_p_(p);
+  const double t = NormalizedSpiralCoordinateAt(p);
+  return 2. * t * k_dot_ * norm_;
+}
+
+}  // namespace road_curve
+}  // namespace malidrive

src/maliput_malidrive/road_curve/spiral_ground_curve.h

@@ -0,0 +1,157 @@
+// BSD 3-Clause License
+//
+// Copyright (c) 2024, Woven Planet. All rights reserved.
+//
+// Redistribution and use in source and binary forms, with or without
+// modification, are permitted provided that the following conditions are met:
+//
+// * Redistributions of source code must retain the above copyright notice, this
+//   list of conditions and the following disclaimer.
+//
+// * Redistributions in binary form must reproduce the above copyright notice,
+//   this list of conditions and the following disclaimer in the documentation
+//   and/or other materials provided with the distribution.
+//
+// * Neither the name of the copyright holder nor the names of its
+//   contributors may be used to endorse or promote products derived from
+//   this software without specific prior written permission.
+//
+// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
+// AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
+// IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
+// DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE
+// FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL
+// DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR
+// SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
+// CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY,
+// OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE
+// OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
+#pragma once
+
+#include <maliput/common/range_validator.h>
+#include <maliput/math/vector.h>
+
+#include "maliput_malidrive/common/macros.h"
+#include "maliput_malidrive/road_curve/ground_curve.h"
+
+namespace malidrive {
+namespace road_curve {
+
+/// GroundCurve specification for a reference curve that describes an Euler spiral, i.e. a clothoid.
+///
+/// Queries accept @f$ p ∈ [p0, p1] @f$ with a linear tolerance.
+///
+/// Implementation details: it relies on an approximate power series expansion of the Fresnel
+/// sine and cosine integrals using 10 terms. The series is known to be sensitive to numerical error
+/// when using floating point numbers due to the high range values. Consequently, it is necessary to
+/// rely on function parameter normalization (@f$ p @f$ parameter) to reduce the range. It occurs
+/// at construction time and it is used throughout the API. Start and end curvatures are both used to define
+/// the normalization factor. The @f$ p @f$ parameter is used in the public API of this class and it is
+/// converted into the normalized parameter @f$ t @f$ by means of a linear function.
+///
+/// For mathematical details about the implementation please refer to:
+/// - https://en.wikipedia.org/wiki/Euler_spiral
+/// - https://en.wikipedia.org/wiki/Fresnel_integral
+///
+/// Another reference implementation can be found at:
+///
+/// <a href="https://github.com/pageldev/libOpenDRIVE/tree/master">libOpenDRIVE</a>
+/// <a href="https://github.com/pageldev/libOpenDRIVE/blob/master/LICENSE">Apache License 2.0</a>
+/// <a
+/// href="https://github.com/pageldev/libOpenDRIVE/blob/9a0437f8a18d445d5c43fe2a4c9401d8a4b770f0/src/Geometries/Spiral.cpp">Spiral.cpp</a>
+///
+/// This implementation does not admit different signs in the start and end curvatures.
+// TODO(#265): Evaluate start and end curvature being of different sign.
+class SpiralGroundCurve : public GroundCurve {
+ public:
+  MALIDRIVE_NO_COPY_NO_MOVE_NO_ASSIGN(SpiralGroundCurve);
+
+  SpiralGroundCurve() = delete;
+
+  /// Constructs an SpiralGroundCurve.
+  ///
+  /// @param linear_tolerance A non-negative value expected to be the same as
+  /// maliput::api::RoadGeometry::linear_tolerance().
+  /// @param xy0 A 2D vector that represents the first point of the spiral.
+  /// @param heading0 The orientation of the tangent vector at @p xy0.
+  /// @param curvature0 Quantity which indicates the reciprocal of the
+  ///        turning radius of the arc at @p p0. A positive @p curvature0 makes a
+  ///        counterclockwise turn. It must be different from @p curvature1 by at least
+  ///        GroundCurve::kEpsilon. It must be of the same sign as @p curvature1.
+  /// @param curvature1 Quantity which indicates the reciprocal of the
+  ///        turning radius of the arc at @p p1. A positive @p curvature1 makes a
+  ///        counterclockwise turn. It must be different from @p curvature1 by at least
+  ///        GroundCurve::kEpsilon. It must be of the same sign as @p curvature0.
+  /// @param arc_length The spiral's length. It must be greate-r or equal to
+  ///        GroundCurve::kEpsilon.
+  /// @param p0 The value of the @f$ p @f$ parameter at the beginning of the
+  ///        spiral, which must be non negative and smaller than @p p1 by at least
+  ///        GroundCurve::kEpsilon.
+  /// @param p1 The value of the @f$ p @f$ parameter at the end of the spiral,
+  ///        which must be greater than @p p0 by at least GroundCurve::kEpsilon.
+  /// @throws maliput::common::assertion_error When @p linear_tolerance is
+  ///         non-positive.
+  /// @throws maliput::common::assertion_error When @p curvature0 is
+  ///         different from @p curvature1 by less than  GroundCurve::kEpsilon.
+  /// @throws maliput::common::assertion_error When @p curvature0 and @p curvature1
+  ///         have a different sign.
+  /// @throws maliput::common::assertion_error When @p arc_length is smaller
+  ///         than GroundCurve::kEpsilon.
+  /// @throws maliput::common::assertion_error When @p p0 is negative.
+  /// @throws maliput::common::assertion_error When @p p1 is not sufficiently
+  ///         larger than @p p0.
+  SpiralGroundCurve(double linear_tolerance, const maliput::math::Vector2& xy0, double heading0, double curvature0,
+                    double curvature1, double arc_length, double p0, double p1);
+
+ private:
+  // @{ NVI implementations.
+  double DoPFromP(double xodr_p) const override;
+  maliput::math::Vector2 DoG(double p) const;
+  maliput::math::Vector2 DoGDot(double p) const override;
+  double DoGInverse(const maliput::math::Vector2& point) const override;
+  double DoHeading(double p) const override;
+  double DoHeadingDot(double p) const override;
+  double DoArcLength() const override { return arc_length_; }
+  double do_linear_tolerance() const override { return linear_tolerance_; }
+  double do_p0() const override { return p0_; }
+  double do_p1() const override { return p1_; }
+  bool DoIsG1Contiguous() const override { return true; }
+  // @} NVI implementations.
+
+  // Computes the normalized spiral function parameter at @p p.
+  // @param p The non-normalized parameter of the spiral.
+  // @return The normalized spiral parameter @f$ t @f$.
+  double NormalizedSpiralCoordinateAt(double p) const;
+
+  // Linear tolerance.
+  const double linear_tolerance_{};
+  // First point of the spiral in world coordinates.
+  const maliput::math::Vector2 xy0_{};
+  // Heading at `p0_`.
+  const double heading0_{};
+  // Curvature at `p0_`.
+  const double curvature0_{};
+  // Curvature at `p1_`.
+  const double curvature1_{};
+  // Arc length of the spiral.
+  const double arc_length_{};
+  // Value of the p parameter at the start of the spiral.
+  const double p0_{};
+  // Value of the p parameter at the end of the spiral.
+  const double p1_{};
+  // Validates that p is within [`p0`, `p1`] with `linear_tolerance_`.
+  const maliput::common::RangeValidator validate_p_;
+  // Curvature derivative at `p0_`.
+  const double k_dot_{};
+  // Normalization factor.
+  const double norm_{};
+  // Non-normalized spiral function parameter at `p0_`.
+  const double spiral_p0_{};
+  // Heading of the normalized spiral at `p0_`.
+  const double spiral_heading0_{};
+  // Start position of the spiral in the normalized spiral frame.
+  const maliput::math::Vector2 spiral_xy0_{};
+};
+
+}  // namespace road_curve
+}  // namespace malidrive

test/regression/road_curve/CMakeLists.txt

@@ -21,6 +21,7 @@ set(UNIT_TEST_ROAD_CURVE_SOURCES
   road_curve_test.cc
   road_curve_offset_test.cc
   scaled_domain_function_test.cc
+  spiral_ground_curve_test.cc
 )
 
 maliput_malidrive_build_tests(${UNIT_TEST_ROAD_CURVE_SOURCES})