From 1a835fec01fa167936f4ac6bb969fdbf038584c7 Mon Sep 17 00:00:00 2001 From: Tyler Veness Date: Tue, 4 Mar 2025 18:52:17 -0800 Subject: [PATCH] [wpimath] Fix singularities in Ellipse2d::Nearest() (#7851) The problem was ill-conditioned if either semiaxis had zero length. --- wpimath/src/main/native/cpp/geometry/Ellipse2d.cpp | 11 ++++++----- 1 file changed, 6 insertions(+), 5 deletions(-) diff --git a/wpimath/src/main/native/cpp/geometry/Ellipse2d.cpp b/wpimath/src/main/native/cpp/geometry/Ellipse2d.cpp index 17ef087c19..9c4fe4fc1e 100644 --- a/wpimath/src/main/native/cpp/geometry/Ellipse2d.cpp +++ b/wpimath/src/main/native/cpp/geometry/Ellipse2d.cpp @@ -34,11 +34,12 @@ Translation2d Ellipse2d::Nearest(const Translation2d& point) const { slp::pow(y - rotPoint.Y().value(), 2)); // (x − x_c)²/a² + (y − y_c)²/b² = 1 - problem.SubjectTo(slp::pow(x - m_center.X().value(), 2) / - (m_xSemiAxis.value() * m_xSemiAxis.value()) + - slp::pow(y - m_center.Y().value(), 2) / - (m_ySemiAxis.value() * m_ySemiAxis.value()) == - 1); + // b²(x − x_c)² + a²(y − y_c)² = a²b² + double a2 = m_xSemiAxis.value() * m_xSemiAxis.value(); + double b2 = m_ySemiAxis.value() * m_ySemiAxis.value(); + problem.SubjectTo(b2 * slp::pow(x - m_center.X().value(), 2) + + a2 * slp::pow(y - m_center.Y().value(), 2) == + a2 * b2); problem.Solve();