CatmullRomSpline incorrect collision point calculation

[TauTheLepton]

Describe the bug
The CatmullRomSpline member function getCollisionPointIn2D calculates wrong values.
This member function uses its component curves member function getCollisionPointIn2D here:

scenario_simulator_v2/common/math/geometry/src/spline/catmull_rom_spline.cpp

Line 383 in 173a8c1

auto s = curves_[n - 1 - i].getCollisionPointIn2D(point0, point1, search_backward);

and here:

scenario_simulator_v2/common/math/geometry/src/spline/catmull_rom_spline.cpp

Line 391 in 173a8c1

auto s = curves_[i].getCollisionPointIn2D(point0, point1, search_backward);

and simply adds the obtained value to the sum of the preceding curves. This produces an incorrect result, because HermiteCurve operates on normalized lengths, meaning the s value is from range <0; 1> like in the function below.

scenario_simulator_v2/common/math/geometry/src/spline/hermite_curve.cpp

Lines 122 to 181 in 173a8c1

	std::optional<double> HermiteCurve::getCollisionPointIn2D(
	const geometry_msgs::msg::Point & point0, const geometry_msgs::msg::Point & point1,
	bool search_backward) const
	{
	std::vector<double> s_values;
	double fx = point0.x;
	double ex = (point1.x - point0.x);
	double fy = point0.y;
	double ey = (point1.y - point0.y);
	double a = ay_ * ex - ax_ * ey;
	double b = by_ * ex - bx_ * ey;
	double c = cy_ * ex - cx_ * ey;
	double d = dy_ * ex - dx_ * ey - ex * fy + ey * fx;
	const auto get_solutions = [search_backward, a, b, c, d, this]() -> std::vector<double> {
	try {
	/**
	* @note Obtain a solution to the cubic equation ax^3 + bx^2 + cx + d = 0 that falls within the range [0, 1].
	*/
	return solver_.solveCubicEquation(a, b, c, d, 0, 1);
	}
	/**
	* @note PolynomialSolver::solveCubicEquation throws common::SimulationError when any x value can satisfy the equation,
	* so the beginning and end point of this curve can collide with the line segment.
	* If search_backward = true, the line segment collisions at the end of the curve. So return 1.
	* If search_backward = false, the line segment collisions at the start of the curve. So return 0.
	*/
	catch (const common::SimulationError &) {
	return {search_backward ? 1.0 : 0.0};
	}
	};
	for (const auto solution : get_solutions()) {
	constexpr double epsilon = std::numeric_limits<double>::epsilon();
	double x = solver_.cubic(ax_, bx_, cx_, dx_, solution);
	double tx = (x - point0.x) / (point1.x - point0.x);
	double y = solver_.cubic(ay_, by_, cy_, dy_, solution);
	double ty = (y - point0.y) / (point1.y - point0.y);
	if (std::abs(tx - ty) > epsilon || std::isnan(tx) || std::isnan(ty)) {
	/**
	* @note If the curve and the line segment to be intersected are parallel to either of the x/y axes, one of the two parameters,
	* tx, ty, will be in the range [0, 1] while the other will be out of that range because of division by zero.
	*/
	if ((0 <= tx && tx <= 1) || (0 <= ty && ty <= 1)) {
	s_values.push_back(solution);
	}
	} else {
	if ((0 <= tx && tx <= 1) && (0 <= ty && ty <= 1)) {
	s_values.push_back(solution);
	}
	}
	}
	if (s_values.empty()) {
	return std::nullopt;
	}
	if (search_backward) {
	return *std::max_element(s_values.begin(), s_values.end());
	}
	return *std::min_element(s_values.begin(), s_values.end());
	}

And CatmullRomSpline operates on absolute lengths, so not normalized ones. This means that the normalized value is added to not normalized ones - which makes the incorrect result.
Many member functions in the HermiteCurve class offer a denormalize_s argument which - when set to true - converts normalized s to the absolute value of s, independent from the curvature length.
In my opinion, the best solution would be to call the HermiteCurve member function getCollisionPointIn2D with such argument denormalize_s set to true. However this member function does not offer such a functionality, so I suggest adding it the same way it is implemented here:

scenario_simulator_v2/common/math/geometry/src/spline/hermite_curve.cpp

Lines 194 to 197 in 173a8c1

	if (denormalize_s) {
	return s.value() * getLength();
	}
	return s.value();

Context:
I am adding unit tests for the geometry package and wanted to add tests for the getCollisionPointIn2D function, but it generates the wrong results.

To Reproduce
Steps to reproduce the behavior:

1. Edit any existing source code file or create a new one
2. Create a CatmullRomSpline from points spaced apart for more than 2 units (like (0, 0), (2, 0) and (4, 0) )
3. Call the getCollisionPointIn2D member function with two points creating a parallel line to the spline (e.g. (0.1, 1) and (0.1, -1)
4. Print the result to the console
5. Compile and execute
6. See the incorrect result

Expected behavior
The CatmullRomSpline member function getCollisionPointIn2D should return the correct result (in the example above 0.1, not 0.05).

Screenshots
If applicable, add screenshots to help explain your problem.

Desktop (please complete the following information):

• OS: [e.g. iOS]: Ubuntu
• Browser [e.g. chrome, safari]: Firefox
• Version [e.g. 22]: 22
• ROS 2 version: Humble
• DDS: CycloneDDS

[hakuturu583]

Thanks, it is a bug.

[hakuturu583]

#1139 was merged