Team2890/allwpilib
4
0
Fork 0
mirror of https://github.com/wpilibsuite/allwpilib synced 2026-06-20 00:51:42 +00:00
Code Issues Packages Projects Releases Wiki Activity

[wpimath] Fix singularities in Ellipse2d::Nearest() (#7851)

Browse Source 
The problem was ill-conditioned if either semiaxis had zero length.
This commit is contained in:
Tyler Veness
2025-03-04 18:52:17 -08:00
committed by GitHub
parent 0ad595c33c
commit 1a835fec01
1 changed files with 6 additions and 5 deletions
Download Patch File Download Diff File Expand all files Collapse all files

11
wpimath/src/main/native/cpp/geometry/Ellipse2d.cpp
View File

@@ -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();