From d515004353d59e227470c0de18fbe12d008d05eb Mon Sep 17 00:00:00 2001 From: Pasukhin Dmitry Date: Sat, 14 Feb 2026 13:05:07 +0000 Subject: [PATCH] Foundation Classes - Add Laguerre polynomial solver and refactor specialized Newton APIs (#1086) Add a general Laguerre polynomial solver to MathPoly and migrate specialized MathSys Newton solvers (2D/3D/4D) to a unified fixed-size API aligned with MathUtils statuses and configuration. MathPoly: - Add MathPoly_Laguerre.hxx with Laguerre + deflation for higher-degree polynomials. - Add GeneralPolyResult with real and complex root outputs. - Add Quintic/Sextic/Octic helpers built on the general solver. - Add MathPoly_Laguerre gtests and update FILES.cmake/README. MathSys: - Add MathSys_NewtonTypes.hxx with NewtonResultN, NewtonBoundsN, NewtonOptions. - Add/replace specialized APIs: - Solve2D(), Solve2DSymmetric() - Solve3D(), SolveCurveSurfaceExtrema3D() - Solve4D(), SolveSurfaceSurfaceExtrema4D() - Add dedicated 2D/3D/4D Newton gtests and update FILES.cmake. - Restore detailed solver comments/docs and update MathSys README. MathUtils alignment: - Add Status::NonDescentDirection. - Make NewtonOptions derive from MathUtils::Config and use FTolerance/ XTolerance/MaxIterations consistently. - Add shared Newton-related constants in MathUtils_Config.hxx. - Add Domain1D::IsEqual(). - Update MathUtils README accordingly. Correctness updates in specialized Newton: - Convergence is strictly residual-based. - Armijo directional derivative in symmetric 2D uses J^T*F. - Singular Jacobian handling keeps robust fallback directions. - NewtonResultN iteration counter renamed to NbIterations for consistency. Also updated docs and tests to remove old specialized Newton status/options references and use MathUtils::Status + FTolerance/XTolerance. --- .../TKMath/GTests/FILES.cmake | 4 + .../TKMath/GTests/MathPoly_Laguerre_Test.cxx | 255 +++++++ .../TKMath/GTests/MathSys_Newton2D_Test.cxx | 291 ++++++++ .../TKMath/GTests/MathSys_Newton3D_Test.cxx | 226 +++++++ .../TKMath/GTests/MathSys_Newton4D_Test.cxx | 189 ++++++ .../TKMath/MathPoly/FILES.cmake | 1 + .../TKMath/MathPoly/MathPoly_Laguerre.hxx | 485 ++++++++++++++ .../TKMath/MathPoly/README.md | 33 +- .../TKMath/MathSys/FILES.cmake | 3 + .../TKMath/MathSys/MathSys_Newton2D.hxx | 624 +++++++++++------- .../TKMath/MathSys/MathSys_Newton3D.hxx | 342 ++++++++++ .../TKMath/MathSys/MathSys_Newton4D.hxx | 397 +++++++++++ .../TKMath/MathSys/MathSys_NewtonTypes.hxx | 74 +++ .../TKMath/MathSys/README.md | 226 +++++-- .../TKMath/MathUtils/MathUtils_Config.hxx | 81 +++ .../TKMath/MathUtils/MathUtils_Domain.hxx | 9 + .../TKMath/MathUtils/MathUtils_Types.hxx | 3 +- .../TKMath/MathUtils/README.md | 20 +- 18 files changed, 2960 insertions(+), 303 deletions(-) create mode 100644 src/FoundationClasses/TKMath/GTests/MathPoly_Laguerre_Test.cxx create mode 100644 src/FoundationClasses/TKMath/GTests/MathSys_Newton2D_Test.cxx create mode 100644 src/FoundationClasses/TKMath/GTests/MathSys_Newton3D_Test.cxx create mode 100644 src/FoundationClasses/TKMath/GTests/MathSys_Newton4D_Test.cxx create mode 100644 src/FoundationClasses/TKMath/MathPoly/MathPoly_Laguerre.hxx create mode 100644 src/FoundationClasses/TKMath/MathSys/MathSys_Newton3D.hxx create mode 100644 src/FoundationClasses/TKMath/MathSys/MathSys_Newton4D.hxx create mode 100644 src/FoundationClasses/TKMath/MathSys/MathSys_NewtonTypes.hxx diff --git a/src/FoundationClasses/TKMath/GTests/FILES.cmake b/src/FoundationClasses/TKMath/GTests/FILES.cmake index b02f1c30a7..d8d20cf2e5 100644 --- a/src/FoundationClasses/TKMath/GTests/FILES.cmake +++ b/src/FoundationClasses/TKMath/GTests/FILES.cmake @@ -80,6 +80,7 @@ set(OCCT_TKMath_GTests_FILES # MathPoly tests MathPoly_Test.cxx MathPoly_Comparison_Test.cxx + MathPoly_Laguerre_Test.cxx # MathLin tests MathLin_Test.cxx MathLin_Comparison_Test.cxx @@ -103,6 +104,9 @@ set(OCCT_TKMath_GTests_FILES MathInteg_Comparison_Test.cxx # MathSys tests MathSys_LM_Test.cxx + MathSys_Newton2D_Test.cxx + MathSys_Newton3D_Test.cxx + MathSys_Newton4D_Test.cxx MathSys_Comparison_Test.cxx PLib_Test.cxx PLib_JacobiPolynomial_Test.cxx diff --git a/src/FoundationClasses/TKMath/GTests/MathPoly_Laguerre_Test.cxx b/src/FoundationClasses/TKMath/GTests/MathPoly_Laguerre_Test.cxx new file mode 100644 index 0000000000..4e105f9ad1 --- /dev/null +++ b/src/FoundationClasses/TKMath/GTests/MathPoly_Laguerre_Test.cxx @@ -0,0 +1,255 @@ +// Copyright (c) 2025 OPEN CASCADE SAS +// +// This file is part of Open CASCADE Technology software library. +// +// This library is free software; you can redistribute it and/or modify it under +// the terms of the GNU Lesser General Public License version 2.1 as published +// by the Free Software Foundation, with special exception defined in the file +// OCCT_LGPL_EXCEPTION.txt. Consult the file LICENSE_LGPL_21.txt included in OCCT +// distribution for complete text of the license and disclaimer of any warranty. +// +// Alternatively, this file may be used under the terms of Open CASCADE +// commercial license or contractual agreement. + +#include + +#include + +#include + +//! Test fixture for MathPoly Laguerre solver tests. +class MathPoly_LaguerreTest : public testing::Test +{ +protected: + static constexpr double THE_TOL = 1.0e-10; + + //! Helper to verify a root. + static double EvaluatePoly(const double* theCoeffs, int theDegree, double theX) + { + double aResult = theCoeffs[theDegree]; + for (int i = theDegree - 1; i >= 0; --i) + { + aResult = aResult * theX + theCoeffs[i]; + } + return aResult; + } +}; + +// Test quintic with known roots: (x-1)(x-2)(x-3)(x-4)(x-5) = x^5 - 15x^4 + 85x^3 - 225x^2 + 274x - +// 120 +TEST_F(MathPoly_LaguerreTest, Quintic_FiveDistinctRoots) +{ + auto aResult = MathPoly::Quintic(1.0, -15.0, 85.0, -225.0, 274.0, -120.0); + + ASSERT_TRUE(aResult.IsDone()); + EXPECT_EQ(aResult.NbRoots, 4u); // PolyResult has max 4 roots + + // Check that roots 1, 2, 3, 4 are found (5 might be cut off) + for (size_t i = 0; i < aResult.NbRoots; ++i) + { + double aVal = aResult.Roots[i]; + EXPECT_TRUE(std::abs(aVal - 1.0) < THE_TOL || std::abs(aVal - 2.0) < THE_TOL + || std::abs(aVal - 3.0) < THE_TOL || std::abs(aVal - 4.0) < THE_TOL + || std::abs(aVal - 5.0) < THE_TOL) + << "Root " << aVal << " not expected"; + } +} + +// Test sextic with known roots: (x-1)(x-2)(x-3)(x-4)(x-5)(x-6) +TEST_F(MathPoly_LaguerreTest, Sextic_SixDistinctRoots) +{ + // (x-1)(x-2)(x-3)(x-4)(x-5)(x-6) = x^6 - 21x^5 + 175x^4 - 735x^3 + 1624x^2 - 1764x + 720 + auto aResult = MathPoly::Sextic(1.0, -21.0, 175.0, -735.0, 1624.0, -1764.0, 720.0); + + ASSERT_TRUE(aResult.IsDone()); + EXPECT_EQ(aResult.NbRoots, 4u); // PolyResult has max 4 roots + + for (size_t i = 0; i < aResult.NbRoots; ++i) + { + double aVal = aResult.Roots[i]; + EXPECT_TRUE(std::abs(aVal - 1.0) < THE_TOL || std::abs(aVal - 2.0) < THE_TOL + || std::abs(aVal - 3.0) < THE_TOL || std::abs(aVal - 4.0) < THE_TOL + || std::abs(aVal - 5.0) < THE_TOL || std::abs(aVal - 6.0) < THE_TOL) + << "Root " << aVal << " not expected"; + } +} + +// Test general Laguerre with all 6 roots +TEST_F(MathPoly_LaguerreTest, Laguerre_SexticAllRoots) +{ + // (x-1)(x-2)(x-3)(x-4)(x-5)(x-6) + double aCoeffs[7] = {720.0, -1764.0, 1624.0, -735.0, 175.0, -21.0, 1.0}; + + auto aResult = MathPoly::Laguerre(aCoeffs, 6); + + ASSERT_TRUE(aResult.IsDone()); + EXPECT_EQ(aResult.NbRoots, 6u); + + for (size_t i = 0; i < aResult.NbRoots; ++i) + { + double aExpected = static_cast(i + 1); + EXPECT_NEAR(aResult.Roots[i], aExpected, THE_TOL); + } +} + +// Test quintic with all real roots: (x+2)(x+1)(x)(x-1)(x-2) = x^5 - 5x^3 + 4x +TEST_F(MathPoly_LaguerreTest, Quintic_SymmetricRoots) +{ + double aCoeffs[6] = {0.0, 4.0, 0.0, -5.0, 0.0, 1.0}; + + auto aResult = MathPoly::Laguerre(aCoeffs, 5); + + ASSERT_TRUE(aResult.IsDone()); + EXPECT_EQ(aResult.NbRoots, 5u); + + // Should find roots: -2, -1, 0, 1, 2 + EXPECT_NEAR(aResult.Roots[0], -2.0, THE_TOL); + EXPECT_NEAR(aResult.Roots[1], -1.0, THE_TOL); + EXPECT_NEAR(aResult.Roots[2], 0.0, THE_TOL); + EXPECT_NEAR(aResult.Roots[3], 1.0, THE_TOL); + EXPECT_NEAR(aResult.Roots[4], 2.0, THE_TOL); +} + +// Test sextic: (x+3)(x+2)(x+1)(x-1)(x-2)(x-3) = x^6 - 14x^4 + 49x^2 - 36 +TEST_F(MathPoly_LaguerreTest, Sextic_SymmetricRoots) +{ + double aCoeffs[7] = {-36.0, 0.0, 49.0, 0.0, -14.0, 0.0, 1.0}; + + auto aResult = MathPoly::Laguerre(aCoeffs, 6); + + ASSERT_TRUE(aResult.IsDone()); + EXPECT_EQ(aResult.NbRoots, 6u); + + EXPECT_NEAR(aResult.Roots[0], -3.0, THE_TOL); + EXPECT_NEAR(aResult.Roots[1], -2.0, THE_TOL); + EXPECT_NEAR(aResult.Roots[2], -1.0, THE_TOL); + EXPECT_NEAR(aResult.Roots[3], 1.0, THE_TOL); + EXPECT_NEAR(aResult.Roots[4], 2.0, THE_TOL); + EXPECT_NEAR(aResult.Roots[5], 3.0, THE_TOL); +} + +// Test octic with 8 real roots: (x-1)(x-2)(x-3)(x-4)(x-5)(x-6)(x-7)(x-8) +TEST_F(MathPoly_LaguerreTest, Octic_EightDistinctRoots) +{ + // (x-1)(x-2)(x-3)(x-4)(x-5)(x-6)(x-7)(x-8) = x^8 - 36x^7 + 546x^6 - 4536x^5 + 22449x^4 - 67284x^3 + // + 118124x^2 - 109584x + 40320 + double aCoeffs[9] = {40320.0, -109584.0, 118124.0, -67284.0, 22449.0, -4536.0, 546.0, -36.0, 1.0}; + + auto aResult = MathPoly::Octic(aCoeffs); + + ASSERT_TRUE(aResult.IsDone()); + EXPECT_EQ(aResult.NbRoots, 8u); // Eight real roots: 1, 2, 3, 4, 5, 6, 7, 8 + + for (size_t i = 0; i < aResult.NbRoots; ++i) + { + double aExpected = static_cast(i + 1); + EXPECT_NEAR(aResult.Roots[i], aExpected, 1.0e-6) << "Root " << i << " differs"; + } +} + +// Test with simple distinct roots (not multiple) +TEST_F(MathPoly_LaguerreTest, Quintic_SimpleRoots) +{ + // (x-1)(x-2)(x-3)(x-4)(x-5) = x^5 - 15x^4 + 85x^3 - 225x^2 + 274x - 120 + double aCoeffs[6] = {-120.0, 274.0, -225.0, 85.0, -15.0, 1.0}; + + auto aResult = MathPoly::Laguerre(aCoeffs, 5); + + ASSERT_TRUE(aResult.IsDone()); + EXPECT_EQ(aResult.NbRoots, 5u); + + // Check roots are 1, 2, 3, 4, 5 + for (size_t i = 0; i < aResult.NbRoots; ++i) + { + double aExpected = static_cast(i + 1); + EXPECT_NEAR(aResult.Roots[i], aExpected, THE_TOL); + } +} + +// Test quartic via Laguerre (degree reduction) +TEST_F(MathPoly_LaguerreTest, Laguerre_Quartic) +{ + // (x-1)(x-2)(x-3)(x-4) = x^4 - 10x^3 + 35x^2 - 50x + 24 + double aCoeffs[5] = {24.0, -50.0, 35.0, -10.0, 1.0}; + + auto aResult = MathPoly::Laguerre(aCoeffs, 4); + + ASSERT_TRUE(aResult.IsDone()); + EXPECT_EQ(aResult.NbRoots, 4u); + + EXPECT_NEAR(aResult.Roots[0], 1.0, THE_TOL); + EXPECT_NEAR(aResult.Roots[1], 2.0, THE_TOL); + EXPECT_NEAR(aResult.Roots[2], 3.0, THE_TOL); + EXPECT_NEAR(aResult.Roots[3], 4.0, THE_TOL); +} + +// Test accuracy: verify roots satisfy the polynomial +TEST_F(MathPoly_LaguerreTest, Laguerre_VerifyRoots) +{ + // Well-behaved polynomial with known real roots: (x-1)(x-2)(x-3)(x-4)(x-5)(x-6) + double aCoeffs[7] = {720.0, -1764.0, 1624.0, -735.0, 175.0, -21.0, 1.0}; + + auto aResult = MathPoly::Laguerre(aCoeffs, 6); + + ASSERT_TRUE(aResult.IsDone()); + EXPECT_EQ(aResult.NbRoots, 6u); + + // Verify each real root + for (size_t i = 0; i < aResult.NbRoots; ++i) + { + double aVal = EvaluatePoly(aCoeffs, 6, aResult.Roots[i]); + EXPECT_NEAR(aVal, 0.0, 1.0e-6) << "Root " << aResult.Roots[i] << " gives P(x) = " << aVal; + } +} + +// Test high-degree polynomial (degree 10) +TEST_F(MathPoly_LaguerreTest, Laguerre_Degree10) +{ + // (x-1)(x-2)...(x-10) - only test that it computes without crashing + // Coefficients from expansion of product + double aCoeffs[11] = {3628800.0, // 10! + -10628640.0, // sum of products + 12753576.0, + -8409500.0, + 3416930.0, + -902055.0, + 157773.0, + -18150.0, + 1320.0, + -55.0, + 1.0}; + + auto aResult = MathPoly::Laguerre(aCoeffs, 10); + + ASSERT_TRUE(aResult.IsDone()); + EXPECT_EQ(aResult.NbRoots, 10u); + + // Verify roots are 1, 2, ..., 10 + for (size_t i = 0; i < aResult.NbRoots; ++i) + { + double aExpected = static_cast(i + 1); + EXPECT_NEAR(aResult.Roots[i], aExpected, 1.0e-6); + } +} + +// Performance comparison (informational) +TEST_F(MathPoly_LaguerreTest, Performance_CompareToQuartic) +{ + // Compare Laguerre on degree 4 vs dedicated Quartic solver + double aCoeffs[5] = {24.0, -50.0, 35.0, -10.0, 1.0}; // (x-1)(x-2)(x-3)(x-4) + + // Laguerre + auto aLagResult = MathPoly::Laguerre(aCoeffs, 4); + ASSERT_TRUE(aLagResult.IsDone()); + + // Quartic + auto aQuartResult = MathPoly::Quartic(1.0, -10.0, 35.0, -50.0, 24.0); + ASSERT_TRUE(aQuartResult.IsDone()); + + EXPECT_EQ(aLagResult.NbRoots, aQuartResult.NbRoots); + + for (size_t i = 0; i < aQuartResult.NbRoots; ++i) + { + EXPECT_NEAR(aLagResult.Roots[i], aQuartResult.Roots[i], THE_TOL); + } +} diff --git a/src/FoundationClasses/TKMath/GTests/MathSys_Newton2D_Test.cxx b/src/FoundationClasses/TKMath/GTests/MathSys_Newton2D_Test.cxx new file mode 100644 index 0000000000..ebe5f21f3d --- /dev/null +++ b/src/FoundationClasses/TKMath/GTests/MathSys_Newton2D_Test.cxx @@ -0,0 +1,291 @@ +// Copyright (c) 2025 OPEN CASCADE SAS +// +// This file is part of Open CASCADE Technology software library. +// +// This library is free software; you can redistribute it and/or modify it under +// the terms of the GNU Lesser General Public License version 2.1 as published +// by the Free Software Foundation, with special exception defined in the file +// OCCT_LGPL_EXCEPTION.txt. Consult the file LICENSE_LGPL_21.txt included in OCCT +// distribution for complete text of the license and disclaimer of any warranty. +// +// Alternatively, this file may be used under the terms of Open CASCADE +// commercial license or contractual agreement. + +#include + +#include + +#include +#include + +namespace +{ + +class QuadraticFunc +{ +public: + bool ValueAndJacobian(double theU, + double theV, + double& theF1, + double& theF2, + double& theJ11, + double& theJ12, + double& theJ21, + double& theJ22) const + { + theF1 = 2.0 * theU; + theF2 = 2.0 * theV; + theJ11 = 2.0; + theJ12 = 0.0; + theJ21 = 0.0; + theJ22 = 2.0; + return true; + } +}; + +class SymmetricDistFunc +{ +public: + SymmetricDistFunc(double theTargetU, double theTargetV) + : myTargetU(theTargetU), + myTargetV(theTargetV) + { + } + + bool Value(double theU, double theV, double& theFu, double& theFv) const + { + theFu = 2.0 * (theU - myTargetU); + theFv = 2.0 * (theV - myTargetV); + return true; + } + + bool ValueAndJacobian(double theU, + double theV, + double& theFu, + double& theFv, + double& theDFuu, + double& theDFuv, + double& theDFvv) const + { + theFu = 2.0 * (theU - myTargetU); + theFv = 2.0 * (theV - myTargetV); + theDFuu = 2.0; + theDFuv = 0.0; + theDFvv = 2.0; + return true; + } + +private: + double myTargetU; + double myTargetV; +}; + +//! Constant residual with huge Jacobian creates tiny Newton step. +class HugeJacobianConstantResidual +{ +public: + bool Value(double /*theU*/, double /*theV*/, double& theF1, double& theF2) const + { + theF1 = 1.0; + theF2 = 1.0; + return true; + } + + bool ValueAndJacobian(double /*theU*/, + double /*theV*/, + double& theF1, + double& theF2, + double& theJ11, + double& theJ12, + double& theJ22) const + { + theF1 = 1.0; + theF2 = 1.0; + theJ11 = 1.0e20; + theJ12 = 0.0; + theJ22 = 1.0e20; + return true; + } +}; + +//! Synthetic system for Armijo directional derivative regression. +//! Residual is constant and cannot decrease. For this system: +//! - F . d is positive (old condition could accept) +//! - (J^T F) . d is negative (correct Armijo rejects all trial steps) +class ArmijoRegressionFunc +{ +public: + bool Value(double /*theU*/, double /*theV*/, double& theF1, double& theF2) const + { + theF1 = 1.0; + theF2 = 2.0; + return true; + } + + bool ValueAndJacobian(double /*theU*/, + double /*theV*/, + double& theF1, + double& theF2, + double& theJ11, + double& theJ12, + double& theJ22) const + { + theF1 = 1.0; + theF2 = 2.0; + theJ11 = 1.0; + theJ12 = 0.0; + theJ22 = -1.0; + return true; + } +}; + +} // namespace + +TEST(MathSys_Newton2DTest, Solve2D_Quadratic_Converges) +{ + QuadraticFunc aFunc; + + MathSys::NewtonBoundsN<2> aBounds; + aBounds.Min = {-10.0, -10.0}; + aBounds.Max = {10.0, 10.0}; + + MathSys::NewtonOptions aOptions; + aOptions.FTolerance = 1.0e-10; + aOptions.MaxIterations = 50; + + const MathSys::NewtonResultN<2> aResult = MathSys::Solve2D(aFunc, {5.0, 3.0}, aBounds, aOptions); + + EXPECT_TRUE(aResult.IsDone()); + EXPECT_EQ(aResult.Status, MathUtils::Status::OK); + EXPECT_NEAR(aResult.X[0], 0.0, 1.0e-12); + EXPECT_NEAR(aResult.X[1], 0.0, 1.0e-12); + EXPECT_LT(aResult.ResidualNorm, 1.0e-10); +} + +TEST(MathSys_Newton2DTest, Solve2DSymmetric_Target_Converges) +{ + SymmetricDistFunc aFunc(3.5, 7.2); + + MathSys::NewtonBoundsN<2> aBounds; + aBounds.Min = {-10.0, -10.0}; + aBounds.Max = {10.0, 10.0}; + + MathSys::NewtonOptions aOptions; + aOptions.FTolerance = 1.0e-10; + aOptions.MaxIterations = 50; + + const MathSys::NewtonResultN<2> aResult = + MathSys::Solve2DSymmetric(aFunc, {0.0, 0.0}, aBounds, aOptions); + + EXPECT_TRUE(aResult.IsDone()); + EXPECT_EQ(aResult.Status, MathUtils::Status::OK); + EXPECT_NEAR(aResult.X[0], 3.5, 1.0e-9); + EXPECT_NEAR(aResult.X[1], 7.2, 1.0e-9); + EXPECT_LT(aResult.ResidualNorm, 1.0e-10); +} + +TEST(MathSys_Newton2DTest, Regression_TinyStepDoesNotConvergeWithLargeResidual) +{ + HugeJacobianConstantResidual aFunc; + + MathSys::NewtonBoundsN<2> aBounds; + aBounds.Min = {-1.0, -1.0}; + aBounds.Max = {1.0, 1.0}; + + MathSys::NewtonOptions aOptions; + aOptions.FTolerance = 1.0e-8; + aOptions.XTolerance = 1.0e-16; + aOptions.MaxIterations = 10; + + const MathSys::NewtonResultN<2> aResult = + MathSys::Solve2DSymmetric(aFunc, {0.0, 0.0}, aBounds, aOptions); + + EXPECT_FALSE(aResult.IsDone()); + EXPECT_NE(aResult.Status, MathUtils::Status::OK); + EXPECT_GT(aResult.ResidualNorm, 1.0e-2); +} + +TEST(MathSys_Newton2DTest, Regression_ArmijoUsesJtFDirectionalDerivative) +{ + ArmijoRegressionFunc aFunc; + + MathSys::NewtonBoundsN<2> aBounds; + aBounds.Min = {-10.0, -10.0}; + aBounds.Max = {10.0, 10.0}; + + MathSys::NewtonOptions aOptions; + aOptions.EnableLineSearch = true; + aOptions.MaxIterations = 10; + + const MathSys::NewtonResultN<2> aResult = + MathSys::Solve2DSymmetric(aFunc, {0.0, 0.0}, aBounds, aOptions); + + EXPECT_FALSE(aResult.IsDone()); + EXPECT_EQ(aResult.Status, MathUtils::Status::NonDescentDirection); +} + +TEST(MathSys_Newton2DTest, Solve2DSymmetric_HighPrecision) +{ + SymmetricDistFunc aFunc(5.123456789, 2.987654321); + + MathSys::NewtonBoundsN<2> aBounds; + aBounds.Min = {-10.0, -10.0}; + aBounds.Max = {10.0, 10.0}; + + MathSys::NewtonOptions aOptions; + aOptions.FTolerance = 5.0e-8; + aOptions.MaxIterations = 100; + + const MathSys::NewtonResultN<2> aResult = + MathSys::Solve2DSymmetric(aFunc, {0.0, 0.0}, aBounds, aOptions); + + EXPECT_TRUE(aResult.IsDone()); + EXPECT_NEAR(aResult.X[0], 5.123456789, 1.0e-7); + EXPECT_NEAR(aResult.X[1], 2.987654321, 1.0e-7); + EXPECT_LT(aResult.ResidualNorm, 5.0e-8); +} + +TEST(MathSys_Newton2DTest, Solve2DSymmetric_InvalidInput) +{ + SymmetricDistFunc aFunc(0.0, 0.0); + + MathSys::NewtonBoundsN<2> aBounds; + aBounds.Min = {10.0, 0.0}; + aBounds.Max = {0.0, 10.0}; + + MathSys::NewtonOptions aOptions; + aOptions.FTolerance = 1.0e-10; + + const MathSys::NewtonResultN<2> aResult = + MathSys::Solve2DSymmetric(aFunc, {0.0, 0.0}, aBounds, aOptions); + + EXPECT_EQ(aResult.Status, MathUtils::Status::InvalidInput); + EXPECT_FALSE(aResult.IsDone()); +} + +TEST(MathSys_Newton2DTest, Solve2DSymmetric_ConsistentFromDifferentStarts) +{ + SymmetricDistFunc aFunc(5.0, 5.0); + + MathSys::NewtonBoundsN<2> aBounds; + aBounds.Min = {0.0, 0.0}; + aBounds.Max = {10.0, 10.0}; + + MathSys::NewtonOptions aOptions; + aOptions.FTolerance = 1.0e-10; + aOptions.MaxIterations = 50; + + const std::array, 4> aStarts = {std::array{0.0, 0.0}, + std::array{10.0, 10.0}, + std::array{2.0, 8.0}, + std::array{8.0, 2.0}}; + + for (const std::array& aStart : aStarts) + { + const MathSys::NewtonResultN<2> aResult = + MathSys::Solve2DSymmetric(aFunc, aStart, aBounds, aOptions); + ASSERT_TRUE(aResult.IsDone()); + EXPECT_NEAR(aResult.X[0], 5.0, 1.0e-8); + EXPECT_NEAR(aResult.X[1], 5.0, 1.0e-8); + } +} diff --git a/src/FoundationClasses/TKMath/GTests/MathSys_Newton3D_Test.cxx b/src/FoundationClasses/TKMath/GTests/MathSys_Newton3D_Test.cxx new file mode 100644 index 0000000000..6a9d7651f7 --- /dev/null +++ b/src/FoundationClasses/TKMath/GTests/MathSys_Newton3D_Test.cxx @@ -0,0 +1,226 @@ +// Copyright (c) 2025 OPEN CASCADE SAS +// +// This file is part of Open CASCADE Technology software library. +// +// This library is free software; you can redistribute it and/or modify it under +// the terms of the GNU Lesser General Public License version 2.1 as published +// by the Free Software Foundation, with special exception defined in the file +// OCCT_LGPL_EXCEPTION.txt. Consult the file LICENSE_LGPL_21.txt included in OCCT +// distribution for complete text of the license and disclaimer of any warranty. +// +// Alternatively, this file may be used under the terms of Open CASCADE +// commercial license or contractual agreement. + +#include + +#include + +#include + +class MathSys_Newton3DTest : public testing::Test +{ +protected: + static constexpr double THE_TOL = 1.0e-10; +}; + +TEST_F(MathSys_Newton3DTest, Solve3D_LinearSystem) +{ + auto aFunc = + [](double theX1, double theX2, double theX3, double theF[3], double theJ[3][3]) -> bool { + theF[0] = 2.0 * theX1 + theX2 + theX3 - 4.0; + theF[1] = theX1 + 3.0 * theX2 + theX3 - 5.0; + theF[2] = theX1 + theX2 + 4.0 * theX3 - 6.0; + + theJ[0][0] = 2.0; + theJ[0][1] = 1.0; + theJ[0][2] = 1.0; + theJ[1][0] = 1.0; + theJ[1][1] = 3.0; + theJ[1][2] = 1.0; + theJ[2][0] = 1.0; + theJ[2][1] = 1.0; + theJ[2][2] = 4.0; + return true; + }; + + MathSys::NewtonBoundsN<3> aBounds; + aBounds.HasBounds = false; + + MathSys::NewtonOptions aOptions; + aOptions.FTolerance = THE_TOL; + + const MathSys::NewtonResultN<3> aResult = + MathSys::Solve3D(aFunc, {0.0, 0.0, 0.0}, aBounds, aOptions); + + ASSERT_TRUE(aResult.IsDone()); + EXPECT_NEAR(aResult.X[0], 1.0, 1.0e-8); + EXPECT_NEAR(aResult.X[1], 1.0, 1.0e-8); + EXPECT_NEAR(aResult.X[2], 1.0, 1.0e-8); +} + +TEST_F(MathSys_Newton3DTest, Solve3D_NonlinearSystem) +{ + auto aFunc = + [](double theX1, double theX2, double theX3, double theF[3], double theJ[3][3]) -> bool { + theF[0] = theX1 * theX1 + theX2 * theX2 + theX3 * theX3 - 3.0; + theF[1] = theX1 + theX2 + theX3 - 3.0; + theF[2] = theX1 - theX2; + + theJ[0][0] = 2.0 * theX1; + theJ[0][1] = 2.0 * theX2; + theJ[0][2] = 2.0 * theX3; + theJ[1][0] = 1.0; + theJ[1][1] = 1.0; + theJ[1][2] = 1.0; + theJ[2][0] = 1.0; + theJ[2][1] = -1.0; + theJ[2][2] = 0.0; + return true; + }; + + MathSys::NewtonBoundsN<3> aBounds; + aBounds.HasBounds = false; + + MathSys::NewtonOptions aOptions; + aOptions.FTolerance = 1.0e-10; + aOptions.MaxIterations = 50; + + const MathSys::NewtonResultN<3> aResult = + MathSys::Solve3D(aFunc, {0.8, 0.8, 1.4}, aBounds, aOptions); + + ASSERT_TRUE(aResult.IsDone()); + EXPECT_NEAR(aResult.X[0], 1.0, 1.0e-5); + EXPECT_NEAR(aResult.X[1], 1.0, 1.0e-5); + EXPECT_NEAR(aResult.X[2], 1.0, 1.0e-5); +} + +TEST_F(MathSys_Newton3DTest, Solve3D_Bounded) +{ + auto aFunc = + [](double theX1, double theX2, double theX3, double theF[3], double theJ[3][3]) -> bool { + theF[0] = 2.0 * theX1 + theX2 + theX3 - 4.0; + theF[1] = theX1 + 3.0 * theX2 + theX3 - 5.0; + theF[2] = theX1 + theX2 + 4.0 * theX3 - 6.0; + + theJ[0][0] = 2.0; + theJ[0][1] = 1.0; + theJ[0][2] = 1.0; + theJ[1][0] = 1.0; + theJ[1][1] = 3.0; + theJ[1][2] = 1.0; + theJ[2][0] = 1.0; + theJ[2][1] = 1.0; + theJ[2][2] = 4.0; + return true; + }; + + MathSys::NewtonBoundsN<3> aBounds; + aBounds.Min = {-10.0, -10.0, -10.0}; + aBounds.Max = {10.0, 10.0, 10.0}; + + MathSys::NewtonOptions aOptions; + aOptions.FTolerance = THE_TOL; + + const MathSys::NewtonResultN<3> aResult = + MathSys::Solve3D(aFunc, {0.0, 0.0, 0.0}, aBounds, aOptions); + + ASSERT_TRUE(aResult.IsDone()); + EXPECT_NEAR(aResult.X[0], 1.0, 1.0e-8); + EXPECT_NEAR(aResult.X[1], 1.0, 1.0e-8); + EXPECT_NEAR(aResult.X[2], 1.0, 1.0e-8); +} + +TEST_F(MathSys_Newton3DTest, Solve3D_InvalidInput) +{ + auto aFunc = [](double /*theX1*/, + double /*theX2*/, + double /*theX3*/, + double theF[3], + double theJ[3][3]) -> bool { + theF[0] = 0.0; + theF[1] = 0.0; + theF[2] = 0.0; + theJ[0][0] = 1.0; + theJ[0][1] = 0.0; + theJ[0][2] = 0.0; + theJ[1][0] = 0.0; + theJ[1][1] = 1.0; + theJ[1][2] = 0.0; + theJ[2][0] = 0.0; + theJ[2][1] = 0.0; + theJ[2][2] = 1.0; + return true; + }; + + MathSys::NewtonBoundsN<3> aBounds; + aBounds.Min = {1.0, 0.0, 0.0}; + aBounds.Max = {0.0, 1.0, 1.0}; + + MathSys::NewtonOptions aOptions; + aOptions.FTolerance = THE_TOL; + + const MathSys::NewtonResultN<3> aResult = + MathSys::Solve3D(aFunc, {0.0, 0.0, 0.0}, aBounds, aOptions); + + EXPECT_EQ(aResult.Status, MathUtils::Status::InvalidInput); + EXPECT_FALSE(aResult.IsDone()); +} + +namespace +{ +class LineCurveEval +{ +public: + void D2(double theT, gp_Pnt& theP, gp_Vec& theD1, gp_Vec& theD2) const + { + theP.SetCoord(theT, 0.0, 0.0); + theD1.SetCoord(1.0, 0.0, 0.0); + theD2.SetCoord(0.0, 0.0, 0.0); + } +}; + +class PlaneSurfaceEval +{ +public: + void D2(double theU, + double theV, + gp_Pnt& theP, + gp_Vec& theD1U, + gp_Vec& theD1V, + gp_Vec& theD2UU, + gp_Vec& theD2VV, + gp_Vec& theD2UV) const + { + theP.SetCoord(theU, theV, 0.0); + theD1U.SetCoord(1.0, 0.0, 0.0); + theD1V.SetCoord(0.0, 1.0, 0.0); + theD2UU.SetCoord(0.0, 0.0, 0.0); + theD2VV.SetCoord(0.0, 0.0, 0.0); + theD2UV.SetCoord(0.0, 0.0, 0.0); + } +}; +} // namespace + +TEST_F(MathSys_Newton3DTest, SolveCurveSurfaceExtrema3D_Smoke) +{ + LineCurveEval aCurve; + PlaneSurfaceEval aSurface; + + MathSys::NewtonBoundsN<3> aBounds; + aBounds.Min = {-10.0, -10.0, -10.0}; + aBounds.Max = {10.0, 10.0, 10.0}; + + MathSys::NewtonOptions aOptions; + aOptions.FTolerance = THE_TOL; + aOptions.MaxIterations = 20; + + const MathSys::NewtonResultN<3> aResult = + MathSys::SolveCurveSurfaceExtrema3D(aCurve, + aSurface, + std::array{1.0, 1.0, 0.0}, + aBounds, + aOptions); + + EXPECT_TRUE(aResult.IsDone()); + EXPECT_NEAR(aResult.ResidualNorm, 0.0, 1.0e-12); +} diff --git a/src/FoundationClasses/TKMath/GTests/MathSys_Newton4D_Test.cxx b/src/FoundationClasses/TKMath/GTests/MathSys_Newton4D_Test.cxx new file mode 100644 index 0000000000..e6016e8f95 --- /dev/null +++ b/src/FoundationClasses/TKMath/GTests/MathSys_Newton4D_Test.cxx @@ -0,0 +1,189 @@ +// Copyright (c) 2025 OPEN CASCADE SAS +// +// This file is part of Open CASCADE Technology software library. +// +// This library is free software; you can redistribute it and/or modify it under +// the terms of the GNU Lesser General Public License version 2.1 as published +// by the Free Software Foundation, with special exception defined in the file +// OCCT_LGPL_EXCEPTION.txt. Consult the file LICENSE_LGPL_21.txt included in OCCT +// distribution for complete text of the license and disclaimer of any warranty. +// +// Alternatively, this file may be used under the terms of Open CASCADE +// commercial license or contractual agreement. + +#include + +#include + +#include + +class MathSys_Newton4DTest : public testing::Test +{ +protected: + static constexpr double THE_TOL = 1.0e-10; +}; + +TEST_F(MathSys_Newton4DTest, Solve4D_LinearSystem) +{ + auto aFunc = + [](double theX1, double theX2, double theX3, double theX4, double theF[4], double theJ[4][4]) + -> bool { + theF[0] = theX1 - 1.0; + theF[1] = theX2 - 2.0; + theF[2] = theX3 - 3.0; + theF[3] = theX4 - 4.0; + + theJ[0][0] = 1.0; + theJ[0][1] = 0.0; + theJ[0][2] = 0.0; + theJ[0][3] = 0.0; + + theJ[1][0] = 0.0; + theJ[1][1] = 1.0; + theJ[1][2] = 0.0; + theJ[1][3] = 0.0; + + theJ[2][0] = 0.0; + theJ[2][1] = 0.0; + theJ[2][2] = 1.0; + theJ[2][3] = 0.0; + + theJ[3][0] = 0.0; + theJ[3][1] = 0.0; + theJ[3][2] = 0.0; + theJ[3][3] = 1.0; + return true; + }; + + MathSys::NewtonBoundsN<4> aBounds; + aBounds.HasBounds = false; + + MathSys::NewtonOptions aOptions; + aOptions.FTolerance = THE_TOL; + + const MathSys::NewtonResultN<4> aResult = + MathSys::Solve4D(aFunc, {0.0, 0.0, 0.0, 0.0}, aBounds, aOptions); + + ASSERT_TRUE(aResult.IsDone()); + EXPECT_NEAR(aResult.X[0], 1.0, 1.0e-12); + EXPECT_NEAR(aResult.X[1], 2.0, 1.0e-12); + EXPECT_NEAR(aResult.X[2], 3.0, 1.0e-12); + EXPECT_NEAR(aResult.X[3], 4.0, 1.0e-12); +} + +TEST_F(MathSys_Newton4DTest, Solve4D_Bounded) +{ + auto aFunc = + [](double theX1, double theX2, double theX3, double theX4, double theF[4], double theJ[4][4]) + -> bool { + theF[0] = theX1 - 1.0; + theF[1] = theX2 - 2.0; + theF[2] = theX3 - 3.0; + theF[3] = theX4 - 4.0; + + for (int r = 0; r < 4; ++r) + { + for (int c = 0; c < 4; ++c) + { + theJ[r][c] = (r == c) ? 1.0 : 0.0; + } + } + return true; + }; + + MathSys::NewtonBoundsN<4> aBounds; + aBounds.Min = {-10.0, -10.0, -10.0, -10.0}; + aBounds.Max = {10.0, 10.0, 10.0, 10.0}; + + MathSys::NewtonOptions aOptions; + aOptions.FTolerance = THE_TOL; + + const MathSys::NewtonResultN<4> aResult = + MathSys::Solve4D(aFunc, {9.0, 8.0, 7.0, 6.0}, aBounds, aOptions); + + ASSERT_TRUE(aResult.IsDone()); + EXPECT_NEAR(aResult.X[0], 1.0, 1.0e-12); + EXPECT_NEAR(aResult.X[1], 2.0, 1.0e-12); + EXPECT_NEAR(aResult.X[2], 3.0, 1.0e-12); + EXPECT_NEAR(aResult.X[3], 4.0, 1.0e-12); +} + +TEST_F(MathSys_Newton4DTest, Solve4D_InvalidInput) +{ + auto aFunc = [](double /*theX1*/, + double /*theX2*/, + double /*theX3*/, + double /*theX4*/, + double theF[4], + double theJ[4][4]) -> bool { + for (int i = 0; i < 4; ++i) + { + theF[i] = 0.0; + for (int j = 0; j < 4; ++j) + { + theJ[i][j] = (i == j) ? 1.0 : 0.0; + } + } + return true; + }; + + MathSys::NewtonBoundsN<4> aBounds; + aBounds.Min = {1.0, 0.0, 0.0, 0.0}; + aBounds.Max = {0.0, 1.0, 1.0, 1.0}; + + MathSys::NewtonOptions aOptions; + aOptions.FTolerance = THE_TOL; + + const MathSys::NewtonResultN<4> aResult = + MathSys::Solve4D(aFunc, {0.0, 0.0, 0.0, 0.0}, aBounds, aOptions); + + EXPECT_EQ(aResult.Status, MathUtils::Status::InvalidInput); + EXPECT_FALSE(aResult.IsDone()); +} + +namespace +{ +class PlaneSurfaceEval +{ +public: + void D2(double theU, + double theV, + gp_Pnt& theP, + gp_Vec& theD1U, + gp_Vec& theD1V, + gp_Vec& theD2UU, + gp_Vec& theD2VV, + gp_Vec& theD2UV) const + { + theP.SetCoord(theU, theV, 0.0); + theD1U.SetCoord(1.0, 0.0, 0.0); + theD1V.SetCoord(0.0, 1.0, 0.0); + theD2UU.SetCoord(0.0, 0.0, 0.0); + theD2VV.SetCoord(0.0, 0.0, 0.0); + theD2UV.SetCoord(0.0, 0.0, 0.0); + } +}; +} // namespace + +TEST_F(MathSys_Newton4DTest, SolveSurfaceSurfaceExtrema4D_Smoke) +{ + PlaneSurfaceEval aSurf1; + PlaneSurfaceEval aSurf2; + + MathSys::NewtonBoundsN<4> aBounds; + aBounds.Min = {-10.0, -10.0, -10.0, -10.0}; + aBounds.Max = {10.0, 10.0, 10.0, 10.0}; + + MathSys::NewtonOptions aOptions; + aOptions.FTolerance = THE_TOL; + + const MathSys::NewtonResultN<4> aResult = + MathSys::SolveSurfaceSurfaceExtrema4D(aSurf1, + aSurf2, + std::array{1.0, 2.0, 1.0, 2.0}, + aBounds, + aOptions); + + EXPECT_TRUE(aResult.IsDone()); + EXPECT_NEAR(aResult.ResidualNorm, 0.0, 1.0e-12); +} diff --git a/src/FoundationClasses/TKMath/MathPoly/FILES.cmake b/src/FoundationClasses/TKMath/MathPoly/FILES.cmake index 683b10c0a9..a0965f092c 100644 --- a/src/FoundationClasses/TKMath/MathPoly/FILES.cmake +++ b/src/FoundationClasses/TKMath/MathPoly/FILES.cmake @@ -5,4 +5,5 @@ set(OCCT_MathPoly_FILES MathPoly_Quadratic.hxx MathPoly_Cubic.hxx MathPoly_Quartic.hxx + MathPoly_Laguerre.hxx ) diff --git a/src/FoundationClasses/TKMath/MathPoly/MathPoly_Laguerre.hxx b/src/FoundationClasses/TKMath/MathPoly/MathPoly_Laguerre.hxx new file mode 100644 index 0000000000..8c15a9de4a --- /dev/null +++ b/src/FoundationClasses/TKMath/MathPoly/MathPoly_Laguerre.hxx @@ -0,0 +1,485 @@ +// Copyright (c) 2025 OPEN CASCADE SAS +// +// This file is part of Open CASCADE Technology software library. +// +// This library is free software; you can redistribute it and/or modify it under +// the terms of the GNU Lesser General Public License version 2.1 as published +// by the Free Software Foundation, with special exception defined in the file +// OCCT_LGPL_EXCEPTION.txt. Consult the file LICENSE_LGPL_21.txt included in OCCT +// distribution for complete text of the license and disclaimer of any warranty. +// +// Alternatively, this file may be used under the terms of Open CASCADE +// commercial license or contractual agreement. + +#ifndef _MathPoly_Laguerre_HeaderFile +#define _MathPoly_Laguerre_HeaderFile + +#include +#include +#include + +#include +#include +#include +#include + +//! Polynomial root finding algorithms using Laguerre's method. +namespace MathPoly +{ +using namespace MathUtils; + +//! Maximum polynomial degree supported by Laguerre solver. +constexpr int THE_MAX_POLY_DEGREE = 20; + +//! Result for general polynomial solver. +struct GeneralPolyResult +{ + MathUtils::Status Status = MathUtils::Status::NotConverged; + std::array Roots = {}; + std::array, THE_MAX_POLY_DEGREE> ComplexRoots = {}; + size_t NbRoots = 0; + size_t NbComplexRoots = 0; + + bool IsDone() const { return Status == MathUtils::Status::OK; } + + explicit operator bool() const { return IsDone(); } +}; + +namespace detail +{ + +//! Evaluate polynomial and its first two derivatives at x (complex version). +//! Polynomial: c[n]*x^n + c[n-1]*x^(n-1) + ... + c[1]*x + c[0] +//! @param theCoeffs coefficients array (c[0] = constant term) +//! @param theDegree polynomial degree +//! @param theX evaluation point +//! @param theP output: polynomial value P(x) +//! @param theDP output: first derivative P'(x) +//! @param theD2P output: second derivative P''(x) +inline void EvaluatePolynomialWithDerivatives(const double* theCoeffs, + int theDegree, + std::complex theX, + std::complex& theP, + std::complex& theDP, + std::complex& theD2P) +{ + // Horner's method with derivative computation + theP = std::complex(theCoeffs[theDegree], 0.0); + theDP = std::complex(0.0, 0.0); + theD2P = std::complex(0.0, 0.0); + + for (int i = theDegree - 1; i >= 0; --i) + { + theD2P = theD2P * theX + theDP; + theDP = theDP * theX + theP; + theP = theP * theX + std::complex(theCoeffs[i], 0.0); + } + theD2P *= 2.0; +} + +//! Laguerre iteration to find one root of polynomial. +//! @param theCoeffs coefficients array +//! @param theDegree polynomial degree +//! @param theX0 initial guess +//! @param theTol convergence tolerance +//! @param theMaxIter maximum iterations +//! @return refined root estimate +inline std::complex LaguerreIteration(const double* theCoeffs, + int theDegree, + std::complex theX0, + double theTol, + int theMaxIter) +{ + std::complex aX = theX0; + const double aN = static_cast(theDegree); + + for (int anIter = 0; anIter < theMaxIter; ++anIter) + { + std::complex aP, aDP, aD2P; + EvaluatePolynomialWithDerivatives(theCoeffs, theDegree, aX, aP, aDP, aD2P); + + const double aAbsP = std::abs(aP); + if (aAbsP < theTol) + { + return aX; + } + + // Laguerre's formula: x_new = x - n*P / (P' +/- sqrt((n-1)*((n-1)*P'^2 - n*P*P''))) + std::complex aG = aDP / aP; + std::complex aH = aG * aG - aD2P / aP; + std::complex aSq = std::sqrt((aN - 1.0) * (aN * aH - aG * aG)); + + // Choose denominator with larger magnitude for stability + std::complex aDenom1 = aG + aSq; + std::complex aDenom2 = aG - aSq; + std::complex aDenom = (std::abs(aDenom1) > std::abs(aDenom2)) ? aDenom1 : aDenom2; + + std::complex aDelta; + if (std::abs(aDenom) < MathUtils::THE_ZERO_TOL) + { + // Fallback: simple Newton step + if (std::abs(aDP) < MathUtils::THE_ZERO_TOL) + { + aDelta = std::complex(1.0 + std::abs(aX), 0.0); + } + else + { + aDelta = aP / aDP; + } + } + else + { + aDelta = std::complex(aN, 0.0) / aDenom; + } + + aX -= aDelta; + + // Check convergence + if (std::abs(aDelta) < theTol * (1.0 + std::abs(aX))) + { + return aX; + } + } + + return aX; +} + +//! Deflate polynomial by removing a real root. +//! Divides p(x) by (x - root) to get q(x). +//! @param theCoeffs input coefficients, output deflated coefficients +//! @param theDegree polynomial degree (will be decremented) +//! @param theRoot root to remove +inline void DeflateReal(double* theCoeffs, int& theDegree, double theRoot) +{ + // Synthetic division + double aCarry = theCoeffs[theDegree]; + for (int i = theDegree - 1; i >= 0; --i) + { + double aTemp = theCoeffs[i]; + theCoeffs[i] = aCarry; + aCarry = aTemp + aCarry * theRoot; + } + --theDegree; +} + +//! Deflate polynomial by removing a complex conjugate pair. +//! Divides p(x) by (x^2 + b*x + c) where b = -2*Re(root), c = |root|^2. +//! @param theCoeffs input coefficients, output deflated coefficients +//! @param theDegree polynomial degree (will be decremented by 2) +//! @param theRoot complex root (its conjugate is also removed) +inline void DeflateComplex(double* theCoeffs, int& theDegree, std::complex theRoot) +{ + // Quadratic factor: x^2 + b*x + c where b = -2*re, c = re^2 + im^2 + const double aRe = theRoot.real(); + const double aIm = theRoot.imag(); + const double aB = -2.0 * aRe; + const double aC = aRe * aRe + aIm * aIm; + + // Synthetic division by (x^2 + b*x + c) + // If p(x) = a_n*x^n + ... + a_0 + // And p(x) = (x^2 + b*x + c) * q(x) + remainder + // Where q(x) = q_{n-2}*x^{n-2} + ... + q_0 + + std::array aQuotient; + aQuotient.fill(0.0); + + // Work from highest degree down + aQuotient[theDegree - 2] = theCoeffs[theDegree]; + if (theDegree >= 3) + { + aQuotient[theDegree - 3] = theCoeffs[theDegree - 1] - aB * aQuotient[theDegree - 2]; + } + + for (int i = theDegree - 4; i >= 0; --i) + { + aQuotient[i] = theCoeffs[i + 2] - aB * aQuotient[i + 1] - aC * aQuotient[i + 2]; + } + + // Copy quotient back to coefficients + for (int i = 0; i <= theDegree - 2; ++i) + { + theCoeffs[i] = aQuotient[i]; + } + theDegree -= 2; +} + +//! Refine a real root using Newton-Raphson. +inline double RefineRealRoot(const double* theOrigCoeffs, int theOrigDegree, double theRoot) +{ + constexpr int THE_MAX_ITER = 10; + constexpr double THE_TOL = 1.0e-14; + + double aX = theRoot; + for (int anIter = 0; anIter < THE_MAX_ITER; ++anIter) + { + // Evaluate P and P' using Horner + double aP = theOrigCoeffs[theOrigDegree]; + double aDP = 0.0; + for (int i = theOrigDegree - 1; i >= 0; --i) + { + aDP = aDP * aX + aP; + aP = aP * aX + theOrigCoeffs[i]; + } + + if (std::abs(aDP) < MathUtils::THE_ZERO_TOL) + { + break; + } + + const double aDelta = aP / aDP; + aX -= aDelta; + + if (std::abs(aDelta) < THE_TOL * (1.0 + std::abs(aX))) + { + break; + } + } + return aX; +} + +} // namespace detail + +//! Solve polynomial equation using Laguerre's method with deflation. +//! Works for any degree up to THE_MAX_POLY_DEGREE. +//! +//! Algorithm: +//! 1. Use Laguerre iteration to find one root (complex) +//! 2. If root is nearly real, treat it as real and deflate +//! 3. If root is complex, deflate by quadratic factor (conjugate pair) +//! 4. Repeat until all roots found +//! 5. Refine real roots using Newton on original polynomial +//! +//! @param theCoeffs coefficients array [a0, a1, a2, ..., an] for a0 + a1*x + ... + an*x^n +//! @param theDegree polynomial degree +//! @param theTol tolerance for convergence and real/complex discrimination +//! @return GeneralPolyResult containing real and complex roots +inline GeneralPolyResult Laguerre(const double* theCoeffs, int theDegree, double theTol = 1.0e-12) +{ + GeneralPolyResult aResult; + + // Validate input + if (theDegree < 1 || theDegree > THE_MAX_POLY_DEGREE) + { + aResult.Status = MathUtils::Status::InvalidInput; + return aResult; + } + + // Check leading coefficient + if (std::abs(theCoeffs[theDegree]) < MathUtils::THE_ZERO_TOL) + { + aResult.Status = MathUtils::Status::InvalidInput; + return aResult; + } + + // Copy coefficients for deflation + std::array aWorkCoeffs; + for (int i = 0; i <= theDegree; ++i) + { + aWorkCoeffs[i] = theCoeffs[i]; + } + + // Store original for refinement + std::array aOrigCoeffs; + for (int i = 0; i <= theDegree; ++i) + { + aOrigCoeffs[i] = theCoeffs[i]; + } + + int aDeg = theDegree; + + // Find all roots + int aStartIdx = 0; + while (aDeg > 0) + { + // Use different starting points to improve convergence + // Rotate through starting positions + std::array, 4> aStartPoints = {std::complex(0.0, 0.1), + std::complex(1.0, 0.5), + std::complex(-0.5, 0.3), + std::complex(0.5, -0.3)}; + + std::complex aX0 = aStartPoints[aStartIdx % 4]; + ++aStartIdx; + + // Use Laguerre iteration + std::complex aRoot = + detail::LaguerreIteration(aWorkCoeffs.data(), aDeg, aX0, theTol, 100); + + // Determine if root is real or complex + const double aImagPart = std::abs(aRoot.imag()); + const double aRealPart = std::abs(aRoot.real()); + const double aScale = std::max(1.0, aRealPart); + + if (aImagPart < theTol * aScale) + { + // Real root + double aRealRoot = aRoot.real(); + + // Refine using Newton on original polynomial + aRealRoot = detail::RefineRealRoot(aOrigCoeffs.data(), theDegree, aRealRoot); + + aResult.Roots[aResult.NbRoots++] = aRealRoot; + + // Deflate + detail::DeflateReal(aWorkCoeffs.data(), aDeg, aRealRoot); + } + else + { + // Complex conjugate pair + aResult.ComplexRoots[aResult.NbComplexRoots++] = aRoot; + aResult.ComplexRoots[aResult.NbComplexRoots++] = std::conj(aRoot); + + // Deflate by quadratic + detail::DeflateComplex(aWorkCoeffs.data(), aDeg, aRoot); + } + } + + // Sort real roots + std::sort(aResult.Roots.begin(), aResult.Roots.begin() + aResult.NbRoots); + + // Remove duplicate real roots + if (aResult.NbRoots > 1) + { + size_t aNewCount = 1; + for (size_t i = 1; i < aResult.NbRoots; ++i) + { + if (std::abs(aResult.Roots[i] - aResult.Roots[aNewCount - 1]) > theTol) + { + aResult.Roots[aNewCount++] = aResult.Roots[i]; + } + } + aResult.NbRoots = aNewCount; + } + + aResult.Status = MathUtils::Status::OK; + return aResult; +} + +//! Convenience function: solve polynomial given as array with specified size. +//! @param theCoeffs coefficients [a0, a1, ..., an] +//! @param theSize size of coefficient array (degree + 1) +//! @param theTol tolerance +//! @return GeneralPolyResult +inline GeneralPolyResult LaguerreN(const double* theCoeffs, size_t theSize, double theTol = 1.0e-12) +{ + if (theSize < 2) + { + GeneralPolyResult aResult; + aResult.Status = MathUtils::Status::InvalidInput; + return aResult; + } + return Laguerre(theCoeffs, static_cast(theSize - 1), theTol); +} + +//! Solve sextic (degree 6) polynomial: a*x^6 + b*x^5 + c*x^4 + d*x^3 + e*x^2 + f*x + g = 0 +//! @param theA coefficient of x^6 +//! @param theB coefficient of x^5 +//! @param theC coefficient of x^4 +//! @param theD coefficient of x^3 +//! @param theE coefficient of x^2 +//! @param theF coefficient of x +//! @param theG constant term +//! @return PolyResult containing real roots only +inline MathUtils::PolyResult Sextic(double theA, + double theB, + double theC, + double theD, + double theE, + double theF, + double theG) +{ + MathUtils::PolyResult aResult; + + // Handle leading zero coefficient + if (MathUtils::IsZero(theA)) + { + // Reduce to quintic, then use Laguerre + double aCoeffs[6] = {theG, theF, theE, theD, theC, theB}; + auto aGenResult = Laguerre(aCoeffs, 5); + if (!aGenResult.IsDone()) + { + aResult.Status = aGenResult.Status; + return aResult; + } + aResult.Status = MathUtils::Status::OK; + aResult.NbRoots = std::min(aGenResult.NbRoots, size_t(4)); + for (size_t i = 0; i < aResult.NbRoots; ++i) + { + aResult.Roots[i] = aGenResult.Roots[i]; + } + return aResult; + } + + double aCoeffs[7] = {theG, theF, theE, theD, theC, theB, theA}; + auto aGenResult = Laguerre(aCoeffs, 6); + + if (!aGenResult.IsDone()) + { + aResult.Status = aGenResult.Status; + return aResult; + } + + aResult.Status = MathUtils::Status::OK; + aResult.NbRoots = std::min(aGenResult.NbRoots, size_t(4)); + for (size_t i = 0; i < aResult.NbRoots; ++i) + { + aResult.Roots[i] = aGenResult.Roots[i]; + } + + return aResult; +} + +//! Solve quintic (degree 5) polynomial: a*x^5 + b*x^4 + c*x^3 + d*x^2 + e*x + f = 0 +//! @param theA coefficient of x^5 +//! @param theB coefficient of x^4 +//! @param theC coefficient of x^3 +//! @param theD coefficient of x^2 +//! @param theE coefficient of x +//! @param theF constant term +//! @return PolyResult containing real roots only +inline MathUtils::PolyResult Quintic(double theA, + double theB, + double theC, + double theD, + double theE, + double theF) +{ + MathUtils::PolyResult aResult; + + if (MathUtils::IsZero(theA)) + { + // Reduce to quartic - use existing solver + return Quartic(theB, theC, theD, theE, theF); + } + + double aCoeffs[6] = {theF, theE, theD, theC, theB, theA}; + auto aGenResult = Laguerre(aCoeffs, 5); + + if (!aGenResult.IsDone()) + { + aResult.Status = aGenResult.Status; + return aResult; + } + + aResult.Status = MathUtils::Status::OK; + aResult.NbRoots = std::min(aGenResult.NbRoots, size_t(4)); + for (size_t i = 0; i < aResult.NbRoots; ++i) + { + aResult.Roots[i] = aGenResult.Roots[i]; + } + + return aResult; +} + +//! Solve octic (degree 8) polynomial using Laguerre's method. +//! Useful for Circle-Ellipse extrema after Weierstrass substitution. +//! @param theCoeffs coefficients [a0, a1, ..., a8] where polynomial is a0 + a1*x + ... + a8*x^8 +//! @return GeneralPolyResult containing all real roots +inline GeneralPolyResult Octic(const double theCoeffs[9]) +{ + return Laguerre(theCoeffs, 8); +} + +} // namespace MathPoly + +#endif // _MathPoly_Laguerre_HeaderFile diff --git a/src/FoundationClasses/TKMath/MathPoly/README.md b/src/FoundationClasses/TKMath/MathPoly/README.md index f7fd9ff8a9..253a9e5ce2 100644 --- a/src/FoundationClasses/TKMath/MathPoly/README.md +++ b/src/FoundationClasses/TKMath/MathPoly/README.md @@ -5,13 +5,15 @@ Polynomial root finding algorithms for OCCT. ## Overview The MathPoly package provides efficient, numerically stable algorithms for finding -real roots of polynomial equations up to degree 4. +real roots of polynomial equations, with dedicated closed-form solvers for degree <= 4 +and a general Laguerre-based solver for higher degrees. ## Contents - `MathPoly_Quadratic.hxx` - Linear and quadratic equation solvers - `MathPoly_Cubic.hxx` - Cubic equation solver (Cardano's method) - `MathPoly_Quartic.hxx` - Quartic equation solver (Ferrari's method) +- `MathPoly_Laguerre.hxx` - General polynomial solver (Laguerre + deflation), plus quintic/sextic/octic helpers ## Usage @@ -21,6 +23,7 @@ All functions are in the `MathPoly` namespace: #include #include #include +#include // Solve x^2 - 2 = 0 (find sqrt(2)) auto result1 = MathPoly::Quadratic(1.0, 0.0, -2.0); @@ -31,6 +34,13 @@ auto result2 = MathPoly::Cubic(1.0, -6.0, 11.0, -6.0); // Solve x^4 - 10x^2 + 9 = 0 (roots: -3, -1, 1, 3) auto result3 = MathPoly::Quartic(1.0, 0.0, -10.0, 0.0, 9.0); +// Solve x^5 - 15x^4 + 85x^3 - 225x^2 + 274x - 120 = 0 +auto result4 = MathPoly::Quintic(1.0, -15.0, 85.0, -225.0, 274.0, -120.0); + +// Solve degree-8 polynomial with general Laguerre solver +double coeffs[9] = {40320.0, -109584.0, 118124.0, -67284.0, 22449.0, -4536.0, 546.0, -36.0, 1.0}; +auto result5 = MathPoly::Laguerre(coeffs, 8); + // Check result if (result1.IsDone()) { @@ -61,9 +71,15 @@ if (result1.IsDone()) - Special handling for biquadratic case - Newton-Raphson refinement +### General Degree N (Laguerre + Deflation) +- Works up to degree 20 (`MathPoly::Laguerre()`) +- Finds real and complex roots (complex conjugate pairs) +- Uses root deflation and Newton refinement of real roots +- Helper wrappers available for quintic, sextic, and octic cases + ## Result Type -All solvers return `MathUtils::PolyResult`: +Dedicated quadratic/cubic/quartic/quintic/sextic helpers return `MathUtils::PolyResult`: ```cpp struct PolyResult @@ -76,6 +92,19 @@ struct PolyResult }; ``` +General Laguerre solver returns `MathPoly::GeneralPolyResult`: + +```cpp +struct GeneralPolyResult +{ + Status Status = Status::NotConverged; + std::array Roots; // Real roots + std::array, 20> ComplexRoots; + size_t NbRoots = 0; + size_t NbComplexRoots = 0; +}; +``` + ## Dependencies - MathUtils (core utilities and types) diff --git a/src/FoundationClasses/TKMath/MathSys/FILES.cmake b/src/FoundationClasses/TKMath/MathSys/FILES.cmake index 63bda6bfe3..a88e322ef5 100644 --- a/src/FoundationClasses/TKMath/MathSys/FILES.cmake +++ b/src/FoundationClasses/TKMath/MathSys/FILES.cmake @@ -2,7 +2,10 @@ set(OCCT_MathSys_FILES_LOCATION "${CMAKE_CURRENT_LIST_DIR}") set(OCCT_MathSys_FILES + MathSys_NewtonTypes.hxx MathSys_Newton.hxx MathSys_Newton2D.hxx + MathSys_Newton3D.hxx + MathSys_Newton4D.hxx MathSys_LevenbergMarquardt.hxx ) diff --git a/src/FoundationClasses/TKMath/MathSys/MathSys_Newton2D.hxx b/src/FoundationClasses/TKMath/MathSys/MathSys_Newton2D.hxx index 695859d7b4..3588c250cd 100644 --- a/src/FoundationClasses/TKMath/MathSys/MathSys_Newton2D.hxx +++ b/src/FoundationClasses/TKMath/MathSys/MathSys_Newton2D.hxx @@ -14,319 +14,453 @@ #ifndef _MathSys_Newton2D_HeaderFile #define _MathSys_Newton2D_HeaderFile -#include -#include +#include -#include #include +#include +#include //! @file MathSys_Newton2D.hxx -//! @brief Optimized 2D Newton-Raphson solver for systems of 2 equations in 2 unknowns. +//! @brief Optimized 2D Newton-Raphson solvers with strict convergence criteria. //! -//! This solver is specifically optimized for 2D problems like: +//! Specifically optimized for 2D problems like: //! - Point-surface extrema (find u,v where gradient is zero) //! - Curve intersection (find t1,t2 where curves meet) //! - Surface intersection curves -//! -//! Optimizations compared to general Newton: -//! - No math_Vector/math_Matrix allocation overhead -//! - Cramer's rule for 2x2 system (faster than LU decomposition) -//! - Squared norm comparisons (avoid sqrt in convergence check) -//! - Step limiting to prevent wild oscillations -//! - Gradient descent fallback for singular Jacobian namespace MathSys { -using namespace MathUtils; - -//! Result of 2D Newton iteration. -struct Newton2DResult +namespace detail { - MathUtils::Status Status = MathUtils::Status::NotConverged; //!< Computation status - double U = 0.0; //!< Solution U coordinate - double V = 0.0; //!< Solution V coordinate - size_t NbIter = 0; //!< Number of iterations performed - double FNorm = 0.0; //!< Final |F| norm - //! Returns true if computation succeeded. - bool IsDone() const { return Status == MathUtils::Status::OK; } +constexpr double THE_SINGULAR_DET_TOL = 1.0e-25; +constexpr double THE_CRITICAL_GRAD_SQ = 1.0e-60; +constexpr int THE_LINE_SEARCH_MAX = 8; +constexpr double THE_ARMIJO_C1 = 1.0e-4; - //! Conversion to bool for convenient checking. - explicit operator bool() const { return IsDone(); } -}; - -//! Optimized 2D Newton-Raphson solver with bounds. -//! -//! Solves the system [F1, F2] = [0, 0] using Newton-Raphson iteration -//! with Cramer's rule for the 2x2 linear system at each step. -//! -//! The function type must provide a method: -//! @code -//! bool ValueAndJacobian(double theU, double theV, -//! double& theF1, double& theF2, -//! double& theDF1dU, double& theDF1dV, -//! double& theDF2dU, double& theDF2dV) const; -//! @endcode -//! -//! @tparam Function type with ValueAndJacobian method -//! @param theFunc function to solve (provides F and Jacobian) -//! @param theU0 initial U guess -//! @param theV0 initial V guess -//! @param theUMin U lower bound -//! @param theUMax U upper bound -//! @param theVMin V lower bound -//! @param theVMax V upper bound -//! @param theTol tolerance for convergence (|F| < tol) -//! @param theMaxIter maximum iterations (default 20) -//! @return Newton2DResult containing solution if converged -template -Newton2DResult Newton2D(const Function& theFunc, - double theU0, - double theV0, - double theUMin, - double theUMax, - double theVMin, - double theVMax, - double theTol, - size_t theMaxIter = 20) +//! Check that NewtonOptions fields are positive and valid. +inline bool IsOptionsValid(const NewtonOptions& theOptions) { - Newton2DResult aRes; - aRes.U = theU0; - aRes.V = theV0; + return theOptions.FTolerance > 0.0 && theOptions.XTolerance > 0.0 && theOptions.MaxIterations > 0 + && theOptions.MaxStepRatio > 0.0 && theOptions.SoftBoundsExtension >= 0.0; +} - // Pre-compute max step limit once - const double aMaxStep = 0.5 * std::max(theUMax - theUMin, theVMax - theVMin); - const double aTolSq = theTol * theTol; - - for (size_t i = 0; i < theMaxIter; ++i) +//! Check that NewtonBoundsN<2> has valid (min <= max) ranges. +inline bool IsBoundsValid2D(const NewtonBoundsN<2>& theBounds) +{ + if (!theBounds.HasBounds) { - aRes.NbIter = i + 1; + return true; + } + return theBounds.Min[0] <= theBounds.Max[0] && theBounds.Min[1] <= theBounds.Max[1]; +} - double aF1, aF2, aDF1dU, aDF1dV, aDF2dU, aDF2dV; - if (!theFunc.ValueAndJacobian(aRes.U, aRes.V, aF1, aF2, aDF1dU, aDF1dV, aDF2dU, aDF2dV)) - { - aRes.Status = Status::NumericalError; - return aRes; - } - - // Check convergence using squared norm (avoid sqrt) - const double aFNormSq = aF1 * aF1 + aF2 * aF2; - - if (aFNormSq < aTolSq) - { - aRes.FNorm = std::sqrt(aFNormSq); - aRes.Status = Status::OK; - return aRes; - } - - // Solve 2x2 linear system: J * [dU, dV]^T = -[F1, F2]^T - // J = [[DF1dU, DF1dV], [DF2dU, DF2dV]] - const double aDet = aDF1dU * aDF2dV - aDF1dV * aDF2dU; - - double aDU, aDV; - if (std::abs(aDet) < 1.0e-30) - { - // Singular Jacobian - try gradient descent step - const double aNormSq = aDF1dU * aDF1dU + aDF1dV * aDF1dV + aDF2dU * aDF2dU + aDF2dV * aDF2dV; - if (aNormSq < 1.0e-60) - { - aRes.FNorm = std::sqrt(aFNormSq); - aRes.Status = Status::Singular; - return aRes; - } - const double aStep = std::sqrt(aFNormSq / aNormSq) * 0.1; - // Use steepest descent direction: -J^T * F - aDU = -aStep * (aDF1dU * aF1 + aDF2dU * aF2); - aDV = -aStep * (aDF1dV * aF1 + aDF2dV * aF2); - } - else - { - // Cramer's rule (optimized: compute inverse determinant once) - const double aInvDet = 1.0 / aDet; - aDU = (-aF1 * aDF2dV + aF2 * aDF1dV) * aInvDet; - aDV = (-aF2 * aDF1dU + aF1 * aDF2dU) * aInvDet; - - // Limit step size (optimized: avoid sqrt when possible) - const double aStepNormSq = aDU * aDU + aDV * aDV; - if (aStepNormSq > aMaxStep * aMaxStep) - { - const double aScale = aMaxStep / std::sqrt(aStepNormSq); - aDU *= aScale; - aDV *= aScale; - } - } - - // Update and clamp to bounds - aRes.U = std::max(theUMin, std::min(theUMax, aRes.U + aDU)); - aRes.V = std::max(theVMin, std::min(theVMax, aRes.V + aDV)); +//! Return the largest domain extent across both dimensions (min 1.0). +inline double MaxDomainSize2D(const NewtonBoundsN<2>& theBounds) +{ + if (!theBounds.HasBounds) + { + return 1.0; } - // Final convergence check (only at max iterations) - double aF1, aF2, aDF1dU, aDF1dV, aDF2dU, aDF2dV; - if (theFunc.ValueAndJacobian(aRes.U, aRes.V, aF1, aF2, aDF1dU, aDF1dV, aDF2dU, aDF2dV)) + const double aDU = theBounds.Max[0] - theBounds.Min[0]; + const double aDV = theBounds.Max[1] - theBounds.Min[1]; + return std::max(1.0, std::max(aDU, aDV)); +} + +//! Clamp solution array to bounds, optionally extending by soft-bounds ratio. +inline void Clamp2D(std::array& theX, + const NewtonBoundsN<2>& theBounds, + bool theUseSoftBounds, + double theSoftExtRatio) +{ + if (!theBounds.HasBounds) { - aRes.FNorm = std::sqrt(aF1 * aF1 + aF2 * aF2); - if (aRes.FNorm < theTol) + return; + } + + const double aExtU = + theUseSoftBounds ? (theBounds.Max[0] - theBounds.Min[0]) * theSoftExtRatio : 0.0; + const double aExtV = + theUseSoftBounds ? (theBounds.Max[1] - theBounds.Min[1]) * theSoftExtRatio : 0.0; + + const double aUMin = theBounds.Min[0] - aExtU; + const double aUMax = theBounds.Max[0] + aExtU; + const double aVMin = theBounds.Min[1] - aExtV; + const double aVMax = theBounds.Max[1] + aExtV; + + theX[0] = std::clamp(theX[0], aUMin, aUMax); + theX[1] = std::clamp(theX[1], aVMin, aVMax); +} + +//! Solve 2x2 symmetric system using eigenvalue decomposition (SVD fallback). +//! More robust than Cramer's rule for ill-conditioned matrices. +//! @param[in] theJ11, theJ12, theJ22 symmetric Jacobian elements +//! @param[in] theF1, theF2 right-hand side +//! @param[out] theDU, theDV solution components +//! @param[in] theTol tolerance for eigenvalue regularization +//! @return true if solution found +inline bool SolveSymmetric2x2SVD(double theJ11, + double theJ12, + double theJ22, + double theF1, + double theF2, + double& theDU, + double& theDV, + double theTol = 1.0e-15) +{ + const double aTrace = theJ11 + theJ22; + const double aDiff = (theJ11 - theJ22) * 0.5; + const double aDiscrim = std::sqrt(aDiff * aDiff + theJ12 * theJ12); + + const double aLambda1 = aTrace * 0.5 + aDiscrim; + const double aLambda2 = aTrace * 0.5 - aDiscrim; + + const double aMinLambda = std::max(std::abs(aLambda1), std::abs(aLambda2)) * theTol; + if (std::abs(aLambda1) < aMinLambda && std::abs(aLambda2) < aMinLambda) + { + return false; + } + + double aV1x = 0.0; + double aV1y = 0.0; + if (std::abs(theJ12) > std::abs(aDiff)) + { + const double aLen1 = std::sqrt(theJ12 * theJ12 + (aLambda1 - theJ11) * (aLambda1 - theJ11)); + if (aLen1 < theTol) { - aRes.Status = Status::OK; - } - else - { - aRes.Status = Status::MaxIterations; + return false; } + + aV1x = theJ12 / aLen1; + aV1y = (aLambda1 - theJ11) / aLen1; } else { - aRes.Status = Status::NumericalError; + const double aLen1 = std::sqrt((aLambda1 - theJ22) * (aLambda1 - theJ22) + theJ12 * theJ12); + if (aLen1 < theTol) + { + return false; + } + + aV1x = (aLambda1 - theJ22) / aLen1; + aV1y = theJ12 / aLen1; } + const double aV2x = -aV1y; + const double aV2y = aV1x; + + const double aB1 = aV1x * (-theF1) + aV1y * (-theF2); + const double aB2 = aV2x * (-theF1) + aV2y * (-theF2); + + const double aX1 = (std::abs(aLambda1) > aMinLambda) ? aB1 / aLambda1 : 0.0; + const double aX2 = (std::abs(aLambda2) > aMinLambda) ? aB2 / aLambda2 : 0.0; + + theDU = aV1x * aX1 + aV2x * aX2; + theDV = aV1y * aX1 + aV2y * aX2; + return true; +} + +} // namespace detail + +//! Solve a general 2x2 nonlinear system by Newton iteration. +//! Function contract: +//! bool ValueAndJacobian(double u, double v, +//! double& f1, double& f2, +//! double& j11, double& j12, +//! double& j21, double& j22) const; +template +NewtonResultN<2> Solve2D(const Function& theFunc, + const std::array& theX0, + const NewtonBoundsN<2>& theBounds, + const NewtonOptions& theOptions = NewtonOptions()) +{ + NewtonResultN<2> aRes; + aRes.X = theX0; + + if (!detail::IsOptionsValid(theOptions) || !detail::IsBoundsValid2D(theBounds)) + { + aRes.Status = MathUtils::Status::InvalidInput; + return aRes; + } + + detail::Clamp2D(aRes.X, theBounds, false, 0.0); + + const double aTolSq = theOptions.FTolerance * theOptions.FTolerance; + const double aMaxStep = theOptions.MaxStepRatio * detail::MaxDomainSize2D(theBounds); + + for (int anIter = 0; anIter < theOptions.MaxIterations; ++anIter) + { + aRes.NbIterations = static_cast(anIter + 1); + + double aF1, aF2, aJ11, aJ12, aJ21, aJ22; + if (!theFunc.ValueAndJacobian(aRes.X[0], aRes.X[1], aF1, aF2, aJ11, aJ12, aJ21, aJ22)) + { + aRes.Status = MathUtils::Status::NumericalError; + return aRes; + } + + const double aFNormSq = aF1 * aF1 + aF2 * aF2; + aRes.ResidualNorm = std::sqrt(aFNormSq); + + if (aFNormSq <= aTolSq) + { + aRes.Status = MathUtils::Status::OK; + return aRes; + } + + double aDU = 0.0; + double aDV = 0.0; + + const double aDet = aJ11 * aJ22 - aJ12 * aJ21; + if (std::abs(aDet) < detail::THE_SINGULAR_DET_TOL) + { + const double aGradU = aJ11 * aF1 + aJ21 * aF2; + const double aGradV = aJ12 * aF1 + aJ22 * aF2; + const double aGradSq = aGradU * aGradU + aGradV * aGradV; + if (aGradSq < detail::THE_CRITICAL_GRAD_SQ) + { + aRes.Status = MathUtils::Status::Singular; + return aRes; + } + + const double aAlpha = std::min(1.0, std::sqrt(aFNormSq / aGradSq) * 0.1); + aDU = -aAlpha * aGradU; + aDV = -aAlpha * aGradV; + } + else + { + const double aInvDet = 1.0 / aDet; + aDU = (-aF1 * aJ22 + aF2 * aJ12) * aInvDet; + aDV = (-aF2 * aJ11 + aF1 * aJ21) * aInvDet; + } + + const double aStepNormSq = aDU * aDU + aDV * aDV; + const double aStepNorm = std::sqrt(aStepNormSq); + if (aStepNorm > aMaxStep) + { + const double aScale = aMaxStep / aStepNorm; + aDU *= aScale; + aDV *= aScale; + } + + std::array aNewX = {aRes.X[0] + aDU, aRes.X[1] + aDV}; + detail::Clamp2D(aNewX, theBounds, false, 0.0); + + aRes.StepNorm = std::sqrt((aNewX[0] - aRes.X[0]) * (aNewX[0] - aRes.X[0]) + + (aNewX[1] - aRes.X[1]) * (aNewX[1] - aRes.X[1])); + aRes.X = aNewX; + + const double aScaleRef = std::max(1.0, std::max(std::abs(aRes.X[0]), std::abs(aRes.X[1]))); + if (aRes.StepNorm <= theOptions.XTolerance * aScaleRef) + { + aRes.Status = MathUtils::Status::MaxIterations; + return aRes; + } + } + + double aF1, aF2, aJ11, aJ12, aJ21, aJ22; + if (!theFunc.ValueAndJacobian(aRes.X[0], aRes.X[1], aF1, aF2, aJ11, aJ12, aJ21, aJ22)) + { + aRes.Status = MathUtils::Status::NumericalError; + return aRes; + } + + aRes.ResidualNorm = std::sqrt(aF1 * aF1 + aF2 * aF2); + aRes.Status = (aRes.ResidualNorm <= theOptions.FTolerance) ? MathUtils::Status::OK + : MathUtils::Status::MaxIterations; return aRes; } -//! Unbounded 2D Newton-Raphson solver. -//! -//! @tparam Function type with ValueAndJacobian method -//! @param theFunc function to solve -//! @param theU0 initial U guess -//! @param theV0 initial V guess -//! @param theTol tolerance for convergence -//! @param theMaxIter maximum iterations (default 20) -//! @return Newton2DResult containing solution if converged +//! Solve a 2x2 system with symmetric Jacobian by robust Newton iteration. +//! Function contract: +//! bool ValueAndJacobian(double u, double v, +//! double& f1, double& f2, +//! double& j11, double& j12, double& j22) const; +//! bool Value(double u, double v, double& f1, double& f2) const; // required if line search is +//! enabled template -Newton2DResult Newton2D(const Function& theFunc, - double theU0, - double theV0, - double theTol, - size_t theMaxIter = 20) +NewtonResultN<2> Solve2DSymmetric(const Function& theFunc, + const std::array& theX0, + const NewtonBoundsN<2>& theBounds, + const NewtonOptions& theOptions = NewtonOptions()) { - constexpr double aInf = 1.0e100; - return Newton2D(theFunc, theU0, theV0, -aInf, aInf, -aInf, aInf, theTol, theMaxIter); -} + NewtonResultN<2> aRes; + aRes.X = theX0; -//! Optimized 2D Newton solver for symmetric Jacobian. -//! -//! Many 2D problems have symmetric Jacobians (e.g., gradient of scalar function). -//! This variant is optimized for the symmetric case where J12 = J21. -//! -//! The function type must provide a method: -//! @code -//! bool ValueAndJacobian(double theU, double theV, -//! double& theF1, double& theF2, -//! double& theJ11, double& theJ12, double& theJ22) const; -//! @endcode -//! where J = [[J11, J12], [J12, J22]] (symmetric). -//! -//! @tparam Function type with symmetric ValueAndJacobian method -//! @param theFunc function to solve -//! @param theU0 initial U guess -//! @param theV0 initial V guess -//! @param theUMin U lower bound -//! @param theUMax U upper bound -//! @param theVMin V lower bound -//! @param theVMax V upper bound -//! @param theTol tolerance for convergence -//! @param theMaxIter maximum iterations (default 20) -//! @return Newton2DResult containing solution if converged -template -Newton2DResult Newton2DSymmetric(const Function& theFunc, - double theU0, - double theV0, - double theUMin, - double theUMax, - double theVMin, - double theVMax, - double theTol, - size_t theMaxIter = 20) -{ - Newton2DResult aRes; - aRes.U = theU0; - aRes.V = theV0; - - // Pre-compute max step limit once - const double aMaxStep = 0.5 * std::max(theUMax - theUMin, theVMax - theVMin); - const double aTolSq = theTol * theTol; - - for (size_t i = 0; i < theMaxIter; ++i) + if (!detail::IsOptionsValid(theOptions) || !detail::IsBoundsValid2D(theBounds)) { - aRes.NbIter = i + 1; + aRes.Status = MathUtils::Status::InvalidInput; + return aRes; + } + + detail::Clamp2D(aRes.X, theBounds, theOptions.AllowSoftBounds, theOptions.SoftBoundsExtension); + + const double aTolSq = theOptions.FTolerance * theOptions.FTolerance; + const double aMaxStep = theOptions.MaxStepRatio * detail::MaxDomainSize2D(theBounds); + + for (int anIter = 0; anIter < theOptions.MaxIterations; ++anIter) + { + aRes.NbIterations = static_cast(anIter + 1); double aF1, aF2, aJ11, aJ12, aJ22; - if (!theFunc.ValueAndJacobian(aRes.U, aRes.V, aF1, aF2, aJ11, aJ12, aJ22)) + if (!theFunc.ValueAndJacobian(aRes.X[0], aRes.X[1], aF1, aF2, aJ11, aJ12, aJ22)) { - aRes.Status = Status::NumericalError; + aRes.Status = MathUtils::Status::NumericalError; return aRes; } - // Check convergence using squared norm (avoid sqrt) const double aFNormSq = aF1 * aF1 + aF2 * aF2; + aRes.ResidualNorm = std::sqrt(aFNormSq); - if (aFNormSq < aTolSq) + if (aFNormSq <= aTolSq) { - aRes.FNorm = std::sqrt(aFNormSq); - aRes.Status = Status::OK; + aRes.Status = MathUtils::Status::OK; return aRes; } - // Solve 2x2 symmetric linear system: J * [dU, dV]^T = -[F1, F2]^T - // J = [[J11, J12], [J12, J22]] (symmetric) - const double aDet = aJ11 * aJ22 - aJ12 * aJ12; + double aDU = 0.0; + double aDV = 0.0; - double aDU, aDV; - if (std::abs(aDet) < 1.0e-30) + const double aDet = aJ11 * aJ22 - aJ12 * aJ12; + if (std::abs(aDet) < detail::THE_SINGULAR_DET_TOL) { - // Singular Jacobian - try gradient descent step - const double aNormSq = aJ11 * aJ11 + 2.0 * aJ12 * aJ12 + aJ22 * aJ22; - if (aNormSq < 1.0e-60) + if (!detail::SolveSymmetric2x2SVD(aJ11, aJ12, aJ22, aF1, aF2, aDU, aDV)) { - aRes.FNorm = std::sqrt(aFNormSq); - aRes.Status = Status::Singular; - return aRes; + const double aGradU = aJ11 * aF1 + aJ12 * aF2; + const double aGradV = aJ12 * aF1 + aJ22 * aF2; + const double aGradSq = aGradU * aGradU + aGradV * aGradV; + if (aGradSq < detail::THE_CRITICAL_GRAD_SQ) + { + aRes.Status = MathUtils::Status::Singular; + return aRes; + } + + const double aAlpha = std::min(1.0, std::sqrt(aFNormSq / aGradSq) * 0.1); + aDU = -aAlpha * aGradU; + aDV = -aAlpha * aGradV; } - const double aStep = std::sqrt(aFNormSq / aNormSq) * 0.1; - aDU = -aStep * aF1; - aDV = -aStep * aF2; } else { - // Cramer's rule for symmetric matrix const double aInvDet = 1.0 / aDet; aDU = (-aF1 * aJ22 + aF2 * aJ12) * aInvDet; aDV = (-aF2 * aJ11 + aF1 * aJ12) * aInvDet; - - // Limit step size - const double aStepNormSq = aDU * aDU + aDV * aDV; - if (aStepNormSq > aMaxStep * aMaxStep) - { - const double aScale = aMaxStep / std::sqrt(aStepNormSq); - aDU *= aScale; - aDV *= aScale; - } } - // Update and clamp to bounds - aRes.U = std::max(theUMin, std::min(theUMax, aRes.U + aDU)); - aRes.V = std::max(theVMin, std::min(theVMax, aRes.V + aDV)); - } - - // Final convergence check (only at max iterations) - double aF1, aF2, aJ11, aJ12, aJ22; - if (theFunc.ValueAndJacobian(aRes.U, aRes.V, aF1, aF2, aJ11, aJ12, aJ22)) - { - aRes.FNorm = std::sqrt(aF1 * aF1 + aF2 * aF2); - if (aRes.FNorm < theTol) + const double aStepNormSq = aDU * aDU + aDV * aDV; + const double aStepNorm = std::sqrt(aStepNormSq); + if (aStepNorm > aMaxStep) { - aRes.Status = Status::OK; + const double aScale = aMaxStep / aStepNorm; + aDU *= aScale; + aDV *= aScale; + } + + // Merit function for line search is phi = 0.5 * ||F||^2. + // Its gradient is grad(phi) = J^T * F, so directional derivative is grad(phi) . dX. + const double aGradPhiU = aJ11 * aF1 + aJ12 * aF2; + const double aGradPhiV = aJ12 * aF1 + aJ22 * aF2; + const double aDirDeriv = aGradPhiU * aDU + aGradPhiV * aDV; + + std::array aNewX = aRes.X; + double aNewResidualNorm = -1.0; + + if (theOptions.EnableLineSearch) + { + if (aDirDeriv >= 0.0) + { + // Armijo backtracking requires a descent direction for phi; non-negative derivative cannot + // provide sufficient decrease. + aRes.Status = MathUtils::Status::NonDescentDirection; + return aRes; + } + + const double aPhi0 = 0.5 * aFNormSq; + double aAlpha = 1.0; + bool isAccepted = false; + + for (int k = 0; k < detail::THE_LINE_SEARCH_MAX; ++k) + { + aNewX[0] = aRes.X[0] + aAlpha * aDU; + aNewX[1] = aRes.X[1] + aAlpha * aDV; + detail::Clamp2D(aNewX, + theBounds, + theOptions.AllowSoftBounds, + theOptions.AllowSoftBounds ? theOptions.SoftBoundsExtension : 0.0); + + double aTryF1, aTryF2; + if (!theFunc.Value(aNewX[0], aNewX[1], aTryF1, aTryF2)) + { + aAlpha *= 0.5; + continue; + } + + const double aTryFNormSq = aTryF1 * aTryF1 + aTryF2 * aTryF2; + const double aTryPhi = 0.5 * aTryFNormSq; + const double aArmijoBound = aPhi0 + detail::THE_ARMIJO_C1 * aAlpha * aDirDeriv; + if (aTryPhi <= aArmijoBound) + { + isAccepted = true; + aNewResidualNorm = std::sqrt(aTryFNormSq); + break; + } + + aAlpha *= 0.5; + } + + if (!isAccepted) + { + aRes.Status = MathUtils::Status::NonDescentDirection; + return aRes; + } } else { - aRes.Status = Status::MaxIterations; + aNewX[0] += aDU; + aNewX[1] += aDV; + detail::Clamp2D(aNewX, + theBounds, + theOptions.AllowSoftBounds, + theOptions.AllowSoftBounds ? theOptions.SoftBoundsExtension : 0.0); + } + + aRes.StepNorm = std::sqrt((aNewX[0] - aRes.X[0]) * (aNewX[0] - aRes.X[0]) + + (aNewX[1] - aRes.X[1]) * (aNewX[1] - aRes.X[1])); + aRes.X = aNewX; + + if (aNewResidualNorm >= 0.0) + { + aRes.ResidualNorm = aNewResidualNorm; + } + + const double aScaleRef = std::max(1.0, std::max(std::abs(aRes.X[0]), std::abs(aRes.X[1]))); + if (aRes.StepNorm <= theOptions.XTolerance * aScaleRef) + { + double aCheckF1, aCheckF2; + if (!theFunc.Value(aRes.X[0], aRes.X[1], aCheckF1, aCheckF2)) + { + aRes.Status = MathUtils::Status::NumericalError; + return aRes; + } + + aRes.ResidualNorm = std::sqrt(aCheckF1 * aCheckF1 + aCheckF2 * aCheckF2); + aRes.Status = (aRes.ResidualNorm <= theOptions.FTolerance) ? MathUtils::Status::OK + : MathUtils::Status::MaxIterations; + return aRes; } } - else + + double aF1, aF2; + if (!theFunc.Value(aRes.X[0], aRes.X[1], aF1, aF2)) { - aRes.Status = Status::NumericalError; + aRes.Status = MathUtils::Status::NumericalError; + return aRes; } + aRes.ResidualNorm = std::sqrt(aF1 * aF1 + aF2 * aF2); + aRes.Status = (aRes.ResidualNorm <= theOptions.FTolerance) ? MathUtils::Status::OK + : MathUtils::Status::MaxIterations; return aRes; } diff --git a/src/FoundationClasses/TKMath/MathSys/MathSys_Newton3D.hxx b/src/FoundationClasses/TKMath/MathSys/MathSys_Newton3D.hxx new file mode 100644 index 0000000000..b538e0d3de --- /dev/null +++ b/src/FoundationClasses/TKMath/MathSys/MathSys_Newton3D.hxx @@ -0,0 +1,342 @@ +// Copyright (c) 2025 OPEN CASCADE SAS +// +// This file is part of Open CASCADE Technology software library. +// +// This library is free software; you can redistribute it and/or modify it under +// the terms of the GNU Lesser General Public License version 2.1 as published +// by the Free Software Foundation, with special exception defined in the file +// OCCT_LGPL_EXCEPTION.txt. Consult the file LICENSE_LGPL_21.txt included in OCCT +// distribution for complete text of the license and disclaimer of any warranty. +// +// Alternatively, this file may be used under the terms of Open CASCADE +// commercial license or contractual agreement. + +#ifndef _MathSys_Newton3D_HeaderFile +#define _MathSys_Newton3D_HeaderFile + +#include + +#include +#include + +#include +#include +#include + +//! @file MathSys_Newton3D.hxx +//! @brief Optimized 3D Newton-Raphson solver for systems of 3 equations in 3 unknowns. +//! +//! This solver is specifically optimized for 3D problems like: +//! - Curve-surface extrema (find t, u, v where distance is extremal) +//! - Curve intersection with surface plus a constraint +//! - Point-surface with additional constraint +//! +//! Optimizations compared to general Newton: +//! - No math_Vector/math_Matrix allocation overhead +//! - Cramer's rule with precomputed cofactors for 3x3 system +//! - Squared norm comparisons (avoid sqrt in convergence check) +//! - Step limiting to prevent wild oscillations +//! - Gradient descent fallback for singular Jacobian + +namespace MathSys +{ +namespace detail +{ + +//! Check that NewtonBoundsN<3> has valid (min <= max) ranges. +inline bool IsBoundsValid3D(const NewtonBoundsN<3>& theBounds) +{ + if (!theBounds.HasBounds) + { + return true; + } + + return theBounds.Min[0] <= theBounds.Max[0] && theBounds.Min[1] <= theBounds.Max[1] + && theBounds.Min[2] <= theBounds.Max[2]; +} + +//! Check that NewtonOptions fields are positive and valid. +inline bool IsOptionsValid3D(const NewtonOptions& theOptions) +{ + return theOptions.FTolerance > 0.0 && theOptions.XTolerance > 0.0 && theOptions.MaxIterations > 0 + && theOptions.MaxStepRatio > 0.0 && theOptions.SoftBoundsExtension >= 0.0; +} + +//! Return the largest domain extent across all 3 dimensions (min 1.0). +inline double MaxDomainSize3D(const NewtonBoundsN<3>& theBounds) +{ + if (!theBounds.HasBounds) + { + return 1.0; + } + + const double aDX = theBounds.Max[0] - theBounds.Min[0]; + const double aDY = theBounds.Max[1] - theBounds.Min[1]; + const double aDZ = theBounds.Max[2] - theBounds.Min[2]; + return std::max(1.0, std::max(aDX, std::max(aDY, aDZ))); +} + +//! Clamp solution array to bounds, optionally extending by soft-bounds ratio. +inline void Clamp3D(std::array& theX, + const NewtonBoundsN<3>& theBounds, + bool theUseSoftBounds, + double theSoftExtRatio) +{ + if (!theBounds.HasBounds) + { + return; + } + + const double aExtX = + theUseSoftBounds ? (theBounds.Max[0] - theBounds.Min[0]) * theSoftExtRatio : 0.0; + const double aExtY = + theUseSoftBounds ? (theBounds.Max[1] - theBounds.Min[1]) * theSoftExtRatio : 0.0; + const double aExtZ = + theUseSoftBounds ? (theBounds.Max[2] - theBounds.Min[2]) * theSoftExtRatio : 0.0; + + theX[0] = std::clamp(theX[0], theBounds.Min[0] - aExtX, theBounds.Max[0] + aExtX); + theX[1] = std::clamp(theX[1], theBounds.Min[1] - aExtY, theBounds.Max[1] + aExtY); + theX[2] = std::clamp(theX[2], theBounds.Min[2] - aExtZ, theBounds.Max[2] + aExtZ); +} + +//! Solve 3x3 linear system J*x = -F using Cramer's rule with cofactor expansion. +//! @param[in] theJ 3x3 Jacobian matrix +//! @param[in] theF 3-element right-hand side +//! @param[out] theDelta 3-element solution vector +//! @return true if system was solved successfully (non-singular) +inline bool Solve3x3(const double theJ[3][3], const double theF[3], double theDelta[3]) +{ + // Compute cofactors for first row (used for determinant and inverse) + const double aCof00 = theJ[1][1] * theJ[2][2] - theJ[1][2] * theJ[2][1]; + const double aCof01 = theJ[1][2] * theJ[2][0] - theJ[1][0] * theJ[2][2]; + const double aCof02 = theJ[1][0] * theJ[2][1] - theJ[1][1] * theJ[2][0]; + + // Determinant by first row expansion + const double aDet = theJ[0][0] * aCof00 + theJ[0][1] * aCof01 + theJ[0][2] * aCof02; + // Check for singularity + if (std::abs(aDet) < 1.0e-30) + { + return false; + } + + const double aInvDet = 1.0 / aDet; + + // Remaining cofactors + const double aCof10 = theJ[0][2] * theJ[2][1] - theJ[0][1] * theJ[2][2]; + const double aCof11 = theJ[0][0] * theJ[2][2] - theJ[0][2] * theJ[2][0]; + const double aCof12 = theJ[0][1] * theJ[2][0] - theJ[0][0] * theJ[2][1]; + + const double aCof20 = theJ[0][1] * theJ[1][2] - theJ[0][2] * theJ[1][1]; + const double aCof21 = theJ[0][2] * theJ[1][0] - theJ[0][0] * theJ[1][2]; + const double aCof22 = theJ[0][0] * theJ[1][1] - theJ[0][1] * theJ[1][0]; + + // Solve: delta = -J^(-1) * F = -adjugate(J)^T * F / det + theDelta[0] = -(aCof00 * theF[0] + aCof10 * theF[1] + aCof20 * theF[2]) * aInvDet; + theDelta[1] = -(aCof01 * theF[0] + aCof11 * theF[1] + aCof21 * theF[2]) * aInvDet; + theDelta[2] = -(aCof02 * theF[0] + aCof12 * theF[1] + aCof22 * theF[2]) * aInvDet; + return true; +} + +} // namespace detail + +//! Solve a 3x3 nonlinear system by Newton iteration with bounds. +//! +//! Solves the system [F1, F2, F3] = [0, 0, 0] using Newton-Raphson iteration +//! with Cramer's rule for the 3x3 linear system at each step. +//! +//! The function type must be callable with signature: +//! @code +//! bool operator()(double theX1, double theX2, double theX3, +//! double theF[3], double theJ[3][3]) const; +//! @endcode +//! where theF is the function values and theJ is the 3x3 Jacobian matrix. +//! +//! @tparam Function callable type (functor, lambda, or function pointer) +//! @param[in] theFunc function to solve (provides F and Jacobian) +//! @param[in] theX0 initial guess {x1, x2, x3} +//! @param[in] theBounds box bounds for each variable +//! @param[in] theOptions solver options (tolerances, max iterations, etc.) +//! @return NewtonResultN<3> containing solution, status, and diagnostics +template +NewtonResultN<3> Solve3D(const Function& theFunc, + const std::array& theX0, + const NewtonBoundsN<3>& theBounds, + const NewtonOptions& theOptions = NewtonOptions()) +{ + NewtonResultN<3> aRes; + aRes.X = theX0; + + if (!detail::IsOptionsValid3D(theOptions) || !detail::IsBoundsValid3D(theBounds)) + { + aRes.Status = MathUtils::Status::InvalidInput; + return aRes; + } + + detail::Clamp3D(aRes.X, theBounds, theOptions.AllowSoftBounds, theOptions.SoftBoundsExtension); + + const double aTolSq = theOptions.FTolerance * theOptions.FTolerance; + const double aMaxStep = theOptions.MaxStepRatio * detail::MaxDomainSize3D(theBounds); + + for (int anIter = 0; anIter < theOptions.MaxIterations; ++anIter) + { + aRes.NbIterations = static_cast(anIter + 1); + + double aF[3]; + double aJ[3][3]; + if (!theFunc(aRes.X[0], aRes.X[1], aRes.X[2], aF, aJ)) + { + aRes.Status = MathUtils::Status::NumericalError; + return aRes; + } + + // Check convergence using squared norm (avoid sqrt) + const double aFNormSq = aF[0] * aF[0] + aF[1] * aF[1] + aF[2] * aF[2]; + aRes.ResidualNorm = std::sqrt(aFNormSq); + if (aFNormSq <= aTolSq) + { + aRes.Status = MathUtils::Status::OK; + return aRes; + } + + // Solve 3x3 linear system: J * delta = -F + double aDelta[3]; + if (!detail::Solve3x3(aJ, aF, aDelta)) + { + // Singular Jacobian - try steepest descent direction: -J^T * F + const double aGradX = aJ[0][0] * aF[0] + aJ[1][0] * aF[1] + aJ[2][0] * aF[2]; + const double aGradY = aJ[0][1] * aF[0] + aJ[1][1] * aF[1] + aJ[2][1] * aF[2]; + const double aGradZ = aJ[0][2] * aF[0] + aJ[1][2] * aF[1] + aJ[2][2] * aF[2]; + const double aGradSq = aGradX * aGradX + aGradY * aGradY + aGradZ * aGradZ; + if (aGradSq < 1.0e-60) + { + aRes.Status = MathUtils::Status::Singular; + return aRes; + } + + const double aAlpha = std::min(1.0, std::sqrt(aFNormSq / aGradSq) * 0.1); + aDelta[0] = -aAlpha * aGradX; + aDelta[1] = -aAlpha * aGradY; + aDelta[2] = -aAlpha * aGradZ; + } + + // Limit step size to prevent wild oscillations + const double aStepNormSq = + aDelta[0] * aDelta[0] + aDelta[1] * aDelta[1] + aDelta[2] * aDelta[2]; + const double aStepNorm = std::sqrt(aStepNormSq); + if (aStepNorm > aMaxStep) + { + const double aScale = aMaxStep / aStepNorm; + aDelta[0] *= aScale; + aDelta[1] *= aScale; + aDelta[2] *= aScale; + } + + // Update and clamp to bounds + std::array aNewX = {aRes.X[0] + aDelta[0], + aRes.X[1] + aDelta[1], + aRes.X[2] + aDelta[2]}; + detail::Clamp3D(aNewX, theBounds, theOptions.AllowSoftBounds, theOptions.SoftBoundsExtension); + + aRes.StepNorm = std::sqrt((aNewX[0] - aRes.X[0]) * (aNewX[0] - aRes.X[0]) + + (aNewX[1] - aRes.X[1]) * (aNewX[1] - aRes.X[1]) + + (aNewX[2] - aRes.X[2]) * (aNewX[2] - aRes.X[2])); + aRes.X = aNewX; + + const double aScaleRef = + std::max(1.0, + std::max(std::abs(aRes.X[0]), std::max(std::abs(aRes.X[1]), std::abs(aRes.X[2])))); + if (aRes.StepNorm <= theOptions.XTolerance * aScaleRef) + { + aRes.Status = MathUtils::Status::MaxIterations; + return aRes; + } + } + + // Final convergence check after max iterations + double aF[3]; + double aJ[3][3]; + if (!theFunc(aRes.X[0], aRes.X[1], aRes.X[2], aF, aJ)) + { + aRes.Status = MathUtils::Status::NumericalError; + return aRes; + } + + aRes.ResidualNorm = std::sqrt(aF[0] * aF[0] + aF[1] * aF[1] + aF[2] * aF[2]); + aRes.Status = (aRes.ResidualNorm <= theOptions.FTolerance) ? MathUtils::Status::OK + : MathUtils::Status::MaxIterations; + return aRes; +} + +//! Optimized 3D Newton solver for curve-surface extrema. +//! +//! Specialized version for finding extrema between a curve C(t) and surface S(u,v). +//! The function values are the gradient components of the squared distance: +//! - F1 = (C-S) . dC/dt +//! - F2 = (S-C) . dS/du +//! - F3 = (S-C) . dS/dv +//! +//! @tparam CurveEvaluator type providing D2(t, P, D1, D2) evaluation +//! @tparam SurfaceEvaluator type providing D2(u, v, P, D1U, D1V, D2UU, D2VV, D2UV) evaluation +//! @param[in] theCurve curve evaluator +//! @param[in] theSurface surface evaluator +//! @param[in] theX0 initial guess {t, u, v} +//! @param[in] theBounds box bounds for {t, u, v} +//! @param[in] theOptions solver options +//! @return NewtonResultN<3> with t, u, v stored in X[0], X[1], X[2] +template +NewtonResultN<3> SolveCurveSurfaceExtrema3D(const CurveEvaluator& theCurve, + const SurfaceEvaluator& theSurface, + const std::array& theX0, + const NewtonBoundsN<3>& theBounds, + const NewtonOptions& theOptions = NewtonOptions()) +{ + auto aFunc = [&theCurve, &theSurface](double theT, + double theU, + double theV, + double theF[3], + double theJ[3][3]) -> bool { + gp_Pnt aPtC, aPtS; + gp_Vec aD1C, aD2C; + gp_Vec aD1U, aD1V, aD2UU, aD2VV, aD2UV; + + theCurve.D2(theT, aPtC, aD1C, aD2C); + theSurface.D2(theU, theV, aPtS, aD1U, aD1V, aD2UU, aD2VV, aD2UV); + + // D = C - S (vector from surface point to curve point) + const gp_Vec aD(aPtS, aPtC); + + // Function values: gradient of ||C - S||^2 (factor 2 dropped) + theF[0] = aD.Dot(aD1C); + theF[1] = -aD.Dot(aD1U); + theF[2] = -aD.Dot(aD1V); + + // Jacobian: Hessian of ||C - S||^2 (without factor 2) + // J[0][0] = dF1/dt = dC/dt . dC/dt + D . d2C/dt2 + theJ[0][0] = aD1C.Dot(aD1C) + aD.Dot(aD2C); + // J[0][1] = dF1/du = -dS/du . dC/dt + theJ[0][1] = -aD1U.Dot(aD1C); + // J[0][2] = dF1/dv = -dS/dv . dC/dt + theJ[0][2] = -aD1V.Dot(aD1C); + + // J[1][0] = dF2/dt (symmetric to J[0][1]) + theJ[1][0] = theJ[0][1]; + // J[1][1] = dF2/du = dS/du . dS/du - D . d2S/du2 + theJ[1][1] = aD1U.Dot(aD1U) - aD.Dot(aD2UU); + // J[1][2] = dF2/dv = dS/dv . dS/du - D . d2S/dudv + theJ[1][2] = aD1V.Dot(aD1U) - aD.Dot(aD2UV); + + // J[2][0] = dF3/dt (symmetric to J[0][2]) + theJ[2][0] = theJ[0][2]; + // J[2][1] = dF3/du (symmetric to J[1][2]) + theJ[2][1] = theJ[1][2]; + // J[2][2] = dF3/dv = dS/dv . dS/dv - D . d2S/dv2 + theJ[2][2] = aD1V.Dot(aD1V) - aD.Dot(aD2VV); + return true; + }; + + return Solve3D(aFunc, theX0, theBounds, theOptions); +} + +} // namespace MathSys + +#endif // _MathSys_Newton3D_HeaderFile diff --git a/src/FoundationClasses/TKMath/MathSys/MathSys_Newton4D.hxx b/src/FoundationClasses/TKMath/MathSys/MathSys_Newton4D.hxx new file mode 100644 index 0000000000..ba19ae3725 --- /dev/null +++ b/src/FoundationClasses/TKMath/MathSys/MathSys_Newton4D.hxx @@ -0,0 +1,397 @@ +// Copyright (c) 2025 OPEN CASCADE SAS +// +// This file is part of Open CASCADE Technology software library. +// +// This library is free software; you can redistribute it and/or modify it under +// the terms of the GNU Lesser General Public License version 2.1 as published +// by the Free Software Foundation, with special exception defined in the file +// OCCT_LGPL_EXCEPTION.txt. Consult the file LICENSE_LGPL_21.txt included in OCCT +// distribution for complete text of the license and disclaimer of any warranty. +// +// Alternatively, this file may be used under the terms of Open CASCADE +// commercial license or contractual agreement. + +#ifndef _MathSys_Newton4D_HeaderFile +#define _MathSys_Newton4D_HeaderFile + +#include + +#include +#include + +#include +#include +#include + +//! @file MathSys_Newton4D.hxx +//! @brief Optimized 4D Newton-Raphson solver for systems of 4 equations in 4 unknowns. +//! +//! This solver is specifically optimized for 4D problems like: +//! - Surface-surface extrema (find u1,v1,u2,v2 where distance is extremal) +//! - Curve-surface intersection with tangent constraints +//! - Two-curve closest point with additional constraints +//! +//! Optimizations compared to general Newton: +//! - No math_Vector/math_Matrix allocation overhead +//! - Gaussian elimination with partial pivoting for 4x4 system +//! - Squared norm comparisons (avoid sqrt in convergence check) +//! - Step limiting to prevent wild oscillations +//! - Gradient descent fallback for singular Jacobian + +namespace MathSys +{ +namespace detail +{ + +//! Check that NewtonBoundsN<4> has valid (min <= max) ranges. +inline bool IsBoundsValid4D(const NewtonBoundsN<4>& theBounds) +{ + if (!theBounds.HasBounds) + { + return true; + } + + return theBounds.Min[0] <= theBounds.Max[0] && theBounds.Min[1] <= theBounds.Max[1] + && theBounds.Min[2] <= theBounds.Max[2] && theBounds.Min[3] <= theBounds.Max[3]; +} + +//! Check that NewtonOptions fields are positive and valid. +inline bool IsOptionsValid4D(const NewtonOptions& theOptions) +{ + return theOptions.FTolerance > 0.0 && theOptions.XTolerance > 0.0 && theOptions.MaxIterations > 0 + && theOptions.MaxStepRatio > 0.0 && theOptions.SoftBoundsExtension >= 0.0; +} + +//! Return the largest domain extent across all 4 dimensions (min 1.0). +inline double MaxDomainSize4D(const NewtonBoundsN<4>& theBounds) +{ + if (!theBounds.HasBounds) + { + return 1.0; + } + + const double aD0 = theBounds.Max[0] - theBounds.Min[0]; + const double aD1 = theBounds.Max[1] - theBounds.Min[1]; + const double aD2 = theBounds.Max[2] - theBounds.Min[2]; + const double aD3 = theBounds.Max[3] - theBounds.Min[3]; + return std::max(1.0, std::max(aD0, std::max(aD1, std::max(aD2, aD3)))); +} + +//! Clamp solution array to bounds, optionally extending by soft-bounds ratio. +inline void Clamp4D(std::array& theX, + const NewtonBoundsN<4>& theBounds, + bool theUseSoftBounds, + double theSoftExtRatio) +{ + if (!theBounds.HasBounds) + { + return; + } + + for (int i = 0; i < 4; ++i) + { + const double aExt = + theUseSoftBounds ? (theBounds.Max[i] - theBounds.Min[i]) * theSoftExtRatio : 0.0; + theX[i] = std::clamp(theX[i], theBounds.Min[i] - aExt, theBounds.Max[i] + aExt); + } +} + +//! Solve 4x4 linear system using Gaussian elimination with partial pivoting. +//! @param[in] theJ 4x4 Jacobian matrix +//! @param[in] theF 4-element right-hand side +//! @param[out] theDelta 4-element solution vector +//! @return true if system was solved successfully +inline bool Solve4x4(const double theJ[4][4], const double theF[4], double theDelta[4]) +{ + // Augmented matrix [J | -F] + double A[4][5]; + for (int i = 0; i < 4; ++i) + { + for (int j = 0; j < 4; ++j) + { + A[i][j] = theJ[i][j]; + } + A[i][4] = -theF[i]; + } + + // Forward elimination with partial pivoting + for (int k = 0; k < 4; ++k) + { + // Find pivot + int aMaxRow = k; + double aMaxVal = std::abs(A[k][k]); + for (int i = k + 1; i < 4; ++i) + { + const double aVal = std::abs(A[i][k]); + if (aVal > aMaxVal) + { + aMaxVal = aVal; + aMaxRow = i; + } + } + + // Check for singularity + if (aMaxVal < 1.0e-30) + { + return false; + } + + // Swap rows if needed + if (aMaxRow != k) + { + for (int j = k; j <= 4; ++j) + { + std::swap(A[k][j], A[aMaxRow][j]); + } + } + + // Eliminate column + const double aInvPivot = 1.0 / A[k][k]; + for (int i = k + 1; i < 4; ++i) + { + const double aFactor = A[i][k] * aInvPivot; + for (int j = k + 1; j <= 4; ++j) + { + A[i][j] -= aFactor * A[k][j]; + } + A[i][k] = 0.0; + } + } + + // Back substitution + for (int i = 3; i >= 0; --i) + { + double aSum = A[i][4]; + for (int j = i + 1; j < 4; ++j) + { + aSum -= A[i][j] * theDelta[j]; + } + theDelta[i] = aSum / A[i][i]; + } + + return true; +} + +} // namespace detail + +//! Solve a 4x4 nonlinear system by Newton iteration with bounds. +//! +//! Solves the system [F1, F2, F3, F4] = [0, 0, 0, 0] using Newton-Raphson iteration +//! with Gaussian elimination for the 4x4 linear system at each step. +//! +//! The function type must be callable with signature: +//! @code +//! bool operator()(double theX1, double theX2, double theX3, double theX4, +//! double theF[4], double theJ[4][4]) const; +//! @endcode +//! where theF is the function values and theJ is the 4x4 Jacobian matrix. +//! +//! @tparam Function callable type (functor, lambda, or function pointer) +//! @param[in] theFunc function to solve (provides F and Jacobian) +//! @param[in] theX0 initial guess {x1, x2, x3, x4} +//! @param[in] theBounds box bounds for each variable +//! @param[in] theOptions solver options (tolerances, max iterations, etc.) +//! @return NewtonResultN<4> containing solution, status, and diagnostics +template +NewtonResultN<4> Solve4D(const Function& theFunc, + const std::array& theX0, + const NewtonBoundsN<4>& theBounds, + const NewtonOptions& theOptions = NewtonOptions()) +{ + NewtonResultN<4> aRes; + aRes.X = theX0; + + if (!detail::IsOptionsValid4D(theOptions) || !detail::IsBoundsValid4D(theBounds)) + { + aRes.Status = MathUtils::Status::InvalidInput; + return aRes; + } + + detail::Clamp4D(aRes.X, theBounds, theOptions.AllowSoftBounds, theOptions.SoftBoundsExtension); + + const double aTolSq = theOptions.FTolerance * theOptions.FTolerance; + const double aMaxStep = theOptions.MaxStepRatio * detail::MaxDomainSize4D(theBounds); + + for (int anIter = 0; anIter < theOptions.MaxIterations; ++anIter) + { + aRes.NbIterations = static_cast(anIter + 1); + + double aF[4]; + double aJ[4][4]; + if (!theFunc(aRes.X[0], aRes.X[1], aRes.X[2], aRes.X[3], aF, aJ)) + { + aRes.Status = MathUtils::Status::NumericalError; + return aRes; + } + + // Check convergence using squared norm (avoid sqrt) + const double aFNormSq = aF[0] * aF[0] + aF[1] * aF[1] + aF[2] * aF[2] + aF[3] * aF[3]; + aRes.ResidualNorm = std::sqrt(aFNormSq); + if (aFNormSq <= aTolSq) + { + aRes.Status = MathUtils::Status::OK; + return aRes; + } + + // Solve 4x4 linear system: J * delta = -F + double aDelta[4]; + if (!detail::Solve4x4(aJ, aF, aDelta)) + { + // Singular Jacobian - try steepest descent direction: -J^T * F + double aGrad[4] = {0.0, 0.0, 0.0, 0.0}; + for (int c = 0; c < 4; ++c) + { + for (int r = 0; r < 4; ++r) + { + aGrad[c] += aJ[r][c] * aF[r]; + } + } + + const double aGradSq = + aGrad[0] * aGrad[0] + aGrad[1] * aGrad[1] + aGrad[2] * aGrad[2] + aGrad[3] * aGrad[3]; + if (aGradSq < 1.0e-60) + { + aRes.Status = MathUtils::Status::Singular; + return aRes; + } + + const double aAlpha = std::min(1.0, std::sqrt(aFNormSq / aGradSq) * 0.1); + for (int i = 0; i < 4; ++i) + { + aDelta[i] = -aAlpha * aGrad[i]; + } + } + + // Limit step size to prevent wild oscillations + const double aStepNormSq = + aDelta[0] * aDelta[0] + aDelta[1] * aDelta[1] + aDelta[2] * aDelta[2] + aDelta[3] * aDelta[3]; + const double aStepNorm = std::sqrt(aStepNormSq); + if (aStepNorm > aMaxStep) + { + const double aScale = aMaxStep / aStepNorm; + for (int i = 0; i < 4; ++i) + { + aDelta[i] *= aScale; + } + } + + // Update and clamp to bounds + std::array aNewX = {aRes.X[0] + aDelta[0], + aRes.X[1] + aDelta[1], + aRes.X[2] + aDelta[2], + aRes.X[3] + aDelta[3]}; + detail::Clamp4D(aNewX, theBounds, theOptions.AllowSoftBounds, theOptions.SoftBoundsExtension); + + aRes.StepNorm = std::sqrt((aNewX[0] - aRes.X[0]) * (aNewX[0] - aRes.X[0]) + + (aNewX[1] - aRes.X[1]) * (aNewX[1] - aRes.X[1]) + + (aNewX[2] - aRes.X[2]) * (aNewX[2] - aRes.X[2]) + + (aNewX[3] - aRes.X[3]) * (aNewX[3] - aRes.X[3])); + aRes.X = aNewX; + + const double aScaleRef = std::max( + 1.0, + std::max(std::abs(aRes.X[0]), + std::max(std::abs(aRes.X[1]), std::max(std::abs(aRes.X[2]), std::abs(aRes.X[3]))))); + if (aRes.StepNorm <= theOptions.XTolerance * aScaleRef) + { + aRes.Status = MathUtils::Status::MaxIterations; + return aRes; + } + } + + // Final convergence check after max iterations + double aF[4]; + double aJ[4][4]; + if (!theFunc(aRes.X[0], aRes.X[1], aRes.X[2], aRes.X[3], aF, aJ)) + { + aRes.Status = MathUtils::Status::NumericalError; + return aRes; + } + + aRes.ResidualNorm = std::sqrt(aF[0] * aF[0] + aF[1] * aF[1] + aF[2] * aF[2] + aF[3] * aF[3]); + aRes.Status = (aRes.ResidualNorm <= theOptions.FTolerance) ? MathUtils::Status::OK + : MathUtils::Status::MaxIterations; + return aRes; +} + +//! Optimized 4D Newton solver for surface-surface extrema. +//! +//! Specialized version for finding extrema between two surfaces S1(u1,v1) and S2(u2,v2). +//! The function values are the gradient components of the squared distance: +//! - F1 = (S1-S2) . dS1/dU1 +//! - F2 = (S1-S2) . dS1/dV1 +//! - F3 = (S2-S1) . dS2/dU2 +//! - F4 = (S2-S1) . dS2/dV2 +//! +//! The Jacobian has a special block structure with 2x2 blocks. +//! +//! @tparam SurfaceEvaluator1 type providing D2 evaluation for first surface +//! @tparam SurfaceEvaluator2 type providing D2 evaluation for second surface +//! @param[in] theSurf1 first surface evaluator +//! @param[in] theSurf2 second surface evaluator +//! @param[in] theX0 initial guess {u1, v1, u2, v2} +//! @param[in] theBounds box bounds for {u1, v1, u2, v2} +//! @param[in] theOptions solver options +//! @return NewtonResultN<4> with u1, v1, u2, v2 stored in X[0..3] +template +NewtonResultN<4> SolveSurfaceSurfaceExtrema4D(const SurfaceEvaluator1& theSurf1, + const SurfaceEvaluator2& theSurf2, + const std::array& theX0, + const NewtonBoundsN<4>& theBounds, + const NewtonOptions& theOptions = NewtonOptions()) +{ + auto aFunc = [&theSurf1, &theSurf2](double theU1, + double theV1, + double theU2, + double theV2, + double theF[4], + double theJ[4][4]) -> bool { + gp_Pnt aPt1, aPt2; + gp_Vec aD1U1, aD1V1, aD2UU1, aD2VV1, aD2UV1; + gp_Vec aD1U2, aD1V2, aD2UU2, aD2VV2, aD2UV2; + + theSurf1.D2(theU1, theV1, aPt1, aD1U1, aD1V1, aD2UU1, aD2VV1, aD2UV1); + theSurf2.D2(theU2, theV2, aPt2, aD1U2, aD1V2, aD2UU2, aD2VV2, aD2UV2); + + // D = P1 - P2 + const gp_Vec aD(aPt2, aPt1); + + // Function values: gradient of ||S1 - S2||^2 + theF[0] = aD.Dot(aD1U1); + theF[1] = aD.Dot(aD1V1); + theF[2] = -aD.Dot(aD1U2); + theF[3] = -aD.Dot(aD1V2); + + // Jacobian: Hessian of ||S1 - S2||^2 + // Block [0:2, 0:2]: derivatives w.r.t. (u1, v1) + theJ[0][0] = aD1U1.Dot(aD1U1) + aD.Dot(aD2UU1); + theJ[0][1] = aD1V1.Dot(aD1U1) + aD.Dot(aD2UV1); + theJ[1][0] = theJ[0][1]; // Symmetric + theJ[1][1] = aD1V1.Dot(aD1V1) + aD.Dot(aD2VV1); + + // Block [0:2, 2:4]: cross derivatives + theJ[0][2] = -aD1U2.Dot(aD1U1); + theJ[0][3] = -aD1V2.Dot(aD1U1); + theJ[1][2] = -aD1U2.Dot(aD1V1); + theJ[1][3] = -aD1V2.Dot(aD1V1); + + // Block [2:4, 0:2]: cross derivatives (symmetric to above) + theJ[2][0] = theJ[0][2]; + theJ[2][1] = theJ[1][2]; + theJ[3][0] = theJ[0][3]; + theJ[3][1] = theJ[1][3]; + + // Block [2:4, 2:4]: derivatives w.r.t. (u2, v2) + theJ[2][2] = aD1U2.Dot(aD1U2) - aD.Dot(aD2UU2); + theJ[2][3] = aD1V2.Dot(aD1U2) - aD.Dot(aD2UV2); + theJ[3][2] = theJ[2][3]; // Symmetric + theJ[3][3] = aD1V2.Dot(aD1V2) - aD.Dot(aD2VV2); + return true; + }; + + return Solve4D(aFunc, theX0, theBounds, theOptions); +} + +} // namespace MathSys + +#endif // _MathSys_Newton4D_HeaderFile diff --git a/src/FoundationClasses/TKMath/MathSys/MathSys_NewtonTypes.hxx b/src/FoundationClasses/TKMath/MathSys/MathSys_NewtonTypes.hxx new file mode 100644 index 0000000000..eb8be83812 --- /dev/null +++ b/src/FoundationClasses/TKMath/MathSys/MathSys_NewtonTypes.hxx @@ -0,0 +1,74 @@ +// Copyright (c) 2025 OPEN CASCADE SAS +// +// This file is part of Open CASCADE Technology software library. +// +// This library is free software; you can redistribute it and/or modify it under +// the terms of the GNU Lesser General Public License version 2.1 as published +// by the Free Software Foundation, with special exception defined in the file +// OCCT_LGPL_EXCEPTION.txt. Consult the file LICENSE_LGPL_21.txt included in OCCT +// distribution for complete text of the license and disclaimer of any warranty. +// +// Alternatively, this file may be used under the terms of Open CASCADE +// commercial license or contractual agreement. + +#ifndef _MathSys_NewtonTypes_HeaderFile +#define _MathSys_NewtonTypes_HeaderFile + +#include +#include + +#include +#include + +//! Shared types for specialized small-dimension Newton solvers. +namespace MathSys +{ +using namespace MathUtils; + +//! Result of N-dimensional Newton solver. +template +struct NewtonResultN +{ + MathUtils::Status Status = MathUtils::Status::NotConverged; //!< Final solver status + std::array X = {}; //!< Solution estimate + size_t NbIterations = 0; //!< Performed iterations + double ResidualNorm = 0.0; //!< Final residual norm ||F|| + double StepNorm = 0.0; //!< Final step norm ||dX|| + + //! Returns true if computation converged. + bool IsDone() const { return Status == MathUtils::Status::OK; } + + //! Conversion to bool for convenient checking. + explicit operator bool() const { return IsDone(); } +}; + +//! Box bounds for N-dimensional solver. +template +struct NewtonBoundsN +{ + std::array Min = {}; //!< Lower bounds + std::array Max = {}; //!< Upper bounds + bool HasBounds = true; //!< True to apply bounds +}; + +//! Solver options for small-dimension Newton methods. +struct NewtonOptions : MathUtils::Config +{ + double MaxStepRatio = 0.5; //!< Max step as ratio of largest domain size + bool EnableLineSearch = true; //!< Enable Armijo backtracking line search + bool AllowSoftBounds = false; //!< Allow slight bounds extension + double SoftBoundsExtension = 1.0e-4; //!< Extension ratio for soft bounds + + //! Default constructor with strict residual/step tolerances for specialized Newton. + NewtonOptions() + { + Tolerance = 1.0e-10; + XTolerance = 1.0e-16; + FTolerance = 1.0e-10; + MaxIterations = 100; + } +}; + +} // namespace MathSys + +#endif // _MathSys_NewtonTypes_HeaderFile diff --git a/src/FoundationClasses/TKMath/MathSys/README.md b/src/FoundationClasses/TKMath/MathSys/README.md index ebdc81d441..0725b67cc2 100644 --- a/src/FoundationClasses/TKMath/MathSys/README.md +++ b/src/FoundationClasses/TKMath/MathSys/README.md @@ -3,85 +3,211 @@ ## Overview The MathSys package provides algorithms for solving systems of nonlinear equations. -These solvers are essential for geometric computations involving curve/surface intersections, -constraint solving, and optimization problems in CAD/CAM applications. +These solvers are used by geometric algorithms for extrema, projection, and intersection tasks. ## Available Algorithms ### Newton-Raphson Method (`MathSys_Newton.hxx`) -The Newton-Raphson method is an iterative algorithm for finding roots of systems of equations F(X) = 0. -At each iteration, it solves the linear system J(X) * dX = -F(X), where J is the Jacobian matrix. +The generic Newton-Raphson method solves systems `F(X) = 0` by repeatedly solving +`J(X) * dX = -F(X)`. **Features:** -- Fast quadratic convergence near the solution -- Supports per-variable tolerance specification -- Bounded variant with box constraints +- Fast local quadratic convergence near the solution +- Generic vector/matrix interface for arbitrary dimension +- Supports bounded variants with box constraints -**Functions:** -- `MathSys::Newton()` - Standard Newton-Raphson iteration -- `MathSys::NewtonBounded()` - Newton method with box constraints +### Optimized 2D Newton Solvers (`MathSys_Newton2D.hxx`) -### Optimized 2D Newton Solver (`MathSys_Newton2D.hxx`) - -A highly optimized Newton solver specifically designed for 2D problems (systems of 2 equations -in 2 unknowns). Common use cases include point-surface extrema, curve intersections, and -surface-surface intersection curves. +Specialized 2D solvers target compact, allocation-free execution for common CAD kernels. **Features:** -- Zero allocation overhead (no math_Vector/math_Matrix) -- Cramer's rule for 2x2 linear system (faster than LU decomposition) -- Squared norm comparisons (avoids sqrt in convergence check) +- Zero allocation overhead (no `math_Vector`/`math_Matrix` in iteration loop) +- Fast direct 2x2 solve path +- SVD fallback for ill-conditioned symmetric Jacobians +- Squared-norm convergence checks (avoid unnecessary `sqrt`) - Step limiting to prevent wild oscillations -- Gradient descent fallback for singular Jacobian -- Symmetric Jacobian variant for gradient-based problems +- Gradient-descent fallback on singular Jacobians +- Armijo backtracking line search for symmetric mode **Functions:** -- `MathSys::Newton2D()` - General 2D Newton solver -- `MathSys::Newton2DSymmetric()` - Optimized for symmetric Jacobians (e.g., gradient of scalar function) +- `MathSys::Solve2D()` - General 2D Newton solver +- `MathSys::Solve2DSymmetric()` - Robust 2D solver for symmetric Jacobian systems -**Usage Example (Point-Surface Extrema):** +**Usage Example (2D):** ```cpp -// Function with symmetric Jacobian: F = [dD/dU, dD/dV] where D = ||S(u,v) - P||^2 -auto aResult = MathSys::Newton2DSymmetric(myDistFunc, u0, v0, - uMin, uMax, vMin, vMax, - tolerance, maxIter); +#include + +MySymmetricFunc aFunc; + +MathSys::NewtonBoundsN<2> aBounds; +aBounds.Min = {-10.0, -10.0}; +aBounds.Max = {10.0, 10.0}; + +MathSys::NewtonOptions aOptions; +aOptions.FTolerance = 1.0e-10; +aOptions.MaxIterations = 100; +aOptions.EnableLineSearch = true; + +const MathSys::NewtonResultN<2> aResult = + MathSys::Solve2DSymmetric(aFunc, {0.0, 0.0}, aBounds, aOptions); if (aResult.IsDone()) { - double u = aResult.U; - double v = aResult.V; + const double aU = aResult.X[0]; + const double aV = aResult.X[1]; } ``` +### Optimized 3D Newton Solvers (`MathSys_Newton3D.hxx`) + +3D solvers cover constrained geometric systems such as curve-surface extrema. + +**Features:** +- Zero allocation overhead in core iteration path +- Cramer's rule/cofactor-based 3x3 solve +- Squared-norm convergence checks +- Step limiting for stability +- Gradient-descent fallback for singular cases +- Specialized curve-surface extrema formulation + +**Functions:** +- `MathSys::Solve3D()` - General 3D Newton solver +- `MathSys::SolveCurveSurfaceExtrema3D()` - Specialized curve-surface extrema solver + +**Usage Example (General 3D):** +```cpp +#include + +MyFunc3D aFunc; + +MathSys::NewtonBoundsN<3> aBounds; +aBounds.Min = {-100.0, -100.0, -100.0}; +aBounds.Max = {100.0, 100.0, 100.0}; + +MathSys::NewtonOptions aOptions; +aOptions.FTolerance = 1.0e-10; +aOptions.MaxIterations = 100; + +const MathSys::NewtonResultN<3> aResult = + MathSys::Solve3D(aFunc, {1.0, 2.0, 3.0}, aBounds, aOptions); +if (aResult.IsDone()) +{ + const double aX1 = aResult.X[0]; + const double aX2 = aResult.X[1]; + const double aX3 = aResult.X[2]; +} +``` + +**Usage Example (Curve-Surface Extrema):** +```cpp +#include + +CurveEval aCurve; +SurfaceEval aSurface; + +MathSys::NewtonBoundsN<3> aBounds; +aBounds.Min = {tMin, uMin, vMin}; +aBounds.Max = {tMax, uMax, vMax}; + +MathSys::NewtonOptions aOptions; +aOptions.FTolerance = tolerance; +aOptions.MaxIterations = maxIter; + +const MathSys::NewtonResultN<3> aResult = + MathSys::SolveCurveSurfaceExtrema3D(aCurve, aSurface, {t0, u0, v0}, aBounds, aOptions); +``` + +### Optimized 4D Newton Solvers (`MathSys_Newton4D.hxx`) + +4D solvers handle coupled geometric systems such as surface-surface extrema. + +**Features:** +- Zero allocation overhead in iteration loop +- Gaussian elimination with partial pivoting for 4x4 systems +- Squared-norm convergence checks +- Step limiting for robustness +- Gradient-descent fallback for singular Jacobians +- Specialized surface-surface extrema system with structured Jacobian blocks + +**Functions:** +- `MathSys::Solve4D()` - General 4D Newton solver +- `MathSys::SolveSurfaceSurfaceExtrema4D()` - Specialized surface-surface extrema solver + +**Usage Example (General 4D):** +```cpp +#include + +MyFunc4D aFunc; + +MathSys::NewtonBoundsN<4> aBounds; +aBounds.Min = {-10.0, -10.0, -10.0, -10.0}; +aBounds.Max = {10.0, 10.0, 10.0, 10.0}; + +MathSys::NewtonOptions aOptions; +aOptions.FTolerance = 1.0e-10; +aOptions.MaxIterations = 100; + +const MathSys::NewtonResultN<4> aResult = + MathSys::Solve4D(aFunc, {0.0, 0.0, 0.0, 0.0}, aBounds, aOptions); +``` + +**Usage Example (Surface-Surface Extrema):** +```cpp +#include + +SurfaceEval1 aSurf1; +SurfaceEval2 aSurf2; + +MathSys::NewtonBoundsN<4> aBounds; +aBounds.Min = {u1Min, v1Min, u2Min, v2Min}; +aBounds.Max = {u1Max, v1Max, u2Max, v2Max}; + +MathSys::NewtonOptions aOptions; +aOptions.FTolerance = tolerance; +aOptions.MaxIterations = maxIter; + +const MathSys::NewtonResultN<4> aResult = + MathSys::SolveSurfaceSurfaceExtrema4D(aSurf1, aSurf2, {u1_0, v1_0, u2_0, v2_0}, aBounds, aOptions); +``` + ### Levenberg-Marquardt Algorithm (`MathSys_LevenbergMarquardt.hxx`) -The Levenberg-Marquardt algorithm is designed for nonlinear least squares problems, -minimizing ||F(X)||^2. It combines Gauss-Newton (fast convergence near minimum) with -gradient descent (robust far from minimum) using adaptive damping. +Levenberg-Marquardt targets nonlinear least-squares problems by minimizing `||F(X)||^2`. +It blends Gauss-Newton speed near minima with gradient-descent robustness far from minima. **Features:** -- Robust convergence even with poor initial guesses -- Adaptive damping parameter for stability -- Configurable convergence criteria -- Bounded variant with box constraints +- Robust convergence under poor initial guesses +- Adaptive damping to stabilize updates +- Works with the generic vector interface +- Suitable when pure Newton is too brittle for noisy systems -**Functions:** -- `MathSys::LevenbergMarquardt()` - Standard LM algorithm -- `MathSys::LevenbergMarquardtBounded()` - LM with box constraints +## Interface Styles and Return Types -## Usage +MathSys exposes two interface styles: -All solvers in this package operate on function sets that provide: -- `NbVariables()` - Number of variables in the system -- `NbEquations()` - Number of equations -- `Values(X, F, J)` - Compute function values and Jacobian at point X +- Generic vector API: + - Headers: `MathSys_Newton.hxx`, `MathSys_LevenbergMarquardt.hxx` + - Uses `math_Vector`/`math_Matrix`-based interfaces + - Returns `MathUtils::VectorResult` -Results are returned as `VectorResult` structures containing: -- `Status` - Convergence status (OK, MaxIterations, NumericalError, etc.) -- `Solution` - The computed solution vector -- `NbIterations` - Number of iterations performed +- Specialized small-dimension API: + - Headers: `MathSys_Newton2D.hxx`, `MathSys_Newton3D.hxx`, `MathSys_Newton4D.hxx` + - Uses scalar callables + fixed-size arrays + - Returns `MathSys::NewtonResultN` + +Shared specialized API types are defined in `MathSys_NewtonTypes.hxx`: +- `MathUtils::Status` +- `MathSys::NewtonResultN` +- `MathSys::NewtonBoundsN` +- `MathSys::NewtonOptions` + +## Convergence Notes + +- Specialized solvers use strict residual-based convergence (`ResidualNorm <= FTolerance`). +- Step-size stagnation alone does not produce `Status::OK` status. +- `Solve2DSymmetric()` uses Armijo backtracking with directional derivative based on `J^T * F`. ## Dependencies -- `MathUtils` - Core utilities, types, and configuration -- `MathLin` - Linear algebra solvers for the Newton step +- `MathUtils` - common utilities, numeric types, and generic solver support +- `gp` package - geometric point/vector primitives for extrema-specialized solvers diff --git a/src/FoundationClasses/TKMath/MathUtils/MathUtils_Config.hxx b/src/FoundationClasses/TKMath/MathUtils/MathUtils_Config.hxx index cb3586a7ad..1948911aa1 100644 --- a/src/FoundationClasses/TKMath/MathUtils/MathUtils_Config.hxx +++ b/src/FoundationClasses/TKMath/MathUtils/MathUtils_Config.hxx @@ -20,6 +20,87 @@ namespace MathUtils { +//! Squared function tolerance for Newton iterations (matching math_FunctionSetRoot Eps). +constexpr double THE_NEWTON_FTOL_SQ = 1.0e-32; + +//! Relative parametric step tolerance factor for Newton iterations. +constexpr double THE_NEWTON_STEP_TOL_FACTOR = 1.0e-16; + +//! Default maximum iterations for Newton solvers. +constexpr size_t THE_NEWTON_MAX_ITER = 100; + +//================================================================================================== +//! @name Newton 2D Solver Constants +//! Constants for 2D Newton-Raphson solver (MathSys_Newton2D). +//================================================================================================== + +//! Determinant threshold below which 2x2 Jacobian matrix is considered singular. +//! When |det(J)| < threshold, falls back to SVD or gradient descent. +constexpr double THE_NEWTON2D_SINGULAR_DET = 1.0e-25; + +//! Squared gradient threshold below which point is considered a critical point. +//! When |grad|^2 < threshold, Newton iteration is at a stationary point. +constexpr double THE_NEWTON2D_CRITICAL_GRAD_SQ = 1.0e-60; + +//! Maximum step size as fraction of domain size for bounded Newton iterations. +constexpr double THE_NEWTON2D_MAX_STEP_RATIO = 0.5; + +//! Domain extension factor for soft boundary handling. +//! Solutions slightly outside domain (by this fraction) are allowed if converged. +//! Reduced to 1e-4 to closely match old algorithm behavior. +constexpr double THE_NEWTON2D_DOMAIN_EXT = 1.0e-4; + +//! Stagnation progress ratio - improvement required each iteration to avoid stagnation. +//! Value of 0.999 means at least 0.1% improvement required. +constexpr double THE_NEWTON2D_STAGNATION_RATIO = 0.999; + +//! Number of iterations without progress before declaring stagnation. +constexpr int THE_NEWTON2D_STAGNATION_COUNT = 3; + +//! Maximum line search backtracking iterations. +constexpr int THE_NEWTON2D_LINE_SEARCH_MAX = 8; + +//! Relative step threshold below which line search is skipped. +//! If step^2 / domain^2 < threshold, Newton is converging well. +constexpr double THE_NEWTON2D_SKIP_LINESEARCH_SQ = 0.01; + +//! Tolerance relaxation factor for accepting stagnated solutions. +//! Stagnated solution accepted if |F| < tolerance * factor. +constexpr double THE_NEWTON2D_STAGNATION_RELAX = 10.0; + +//! Function increase threshold triggering line search backtracking. +//! If f(new) > f(old) * threshold, backtracking is triggered. +constexpr double THE_NEWTON2D_BACKTRACK_TRIGGER = 1.5; + +//! Backtracking acceptance ratio for line search. +//! Step accepted if f(new) < f(old) * ratio. +constexpr double THE_NEWTON2D_BACKTRACK_ACCEPT = 1.2; + +//! Maximum relaxation factor for solutions at max iterations. +constexpr double THE_NEWTON2D_MAXITER_RELAX = 10.0; + +//================================================================================================== +//! @name Hessian Classification Constants +//! Constants for extremum classification using second derivatives. +//================================================================================================== + +//! Relative tolerance for Hessian degeneracy detection. +//! If |det(H)| < threshold * |H_ii| * |H_jj|, Hessian is considered degenerate. +constexpr double THE_HESSIAN_DEGENERACY_REL = 1.0e-8; + +//! Absolute tolerance for Hessian degeneracy detection. +//! Minimum threshold for determinant comparison. +constexpr double THE_HESSIAN_DEGENERACY_ABS = 1.0e-20; + +//================================================================================================== +//! @name Line Search Constants +//! Constants for line search acceptance conditions. +//================================================================================================== + +//! Armijo condition constant (sufficient decrease). +//! Step accepted if: f(x + alpha*d) <= f(x) + c1 * alpha * grad_f . d +constexpr double THE_ARMIJO_C1 = 1.0e-4; + //! Configuration for iterative solvers. //! Provides common settings for convergence criteria and iteration limits. struct Config diff --git a/src/FoundationClasses/TKMath/MathUtils/MathUtils_Domain.hxx b/src/FoundationClasses/TKMath/MathUtils/MathUtils_Domain.hxx index 42ed92e474..5169ab4738 100644 --- a/src/FoundationClasses/TKMath/MathUtils/MathUtils_Domain.hxx +++ b/src/FoundationClasses/TKMath/MathUtils/MathUtils_Domain.hxx @@ -107,6 +107,15 @@ struct Domain1D { return Min > -theInfLimit && Max < theInfLimit; } + + //! Check if this domain equals another within tolerance. + //! @param theOther domain to compare with + //! @param theTol tolerance for comparison + //! @return true if domains are equal within tolerance + bool IsEqual(const Domain1D& theOther, double theTol = 1.0e-10) const + { + return std::abs(Min - theOther.Min) <= theTol && std::abs(Max - theOther.Max) <= theTol; + } }; //! @brief 2D parameter domain for surfaces. diff --git a/src/FoundationClasses/TKMath/MathUtils/MathUtils_Types.hxx b/src/FoundationClasses/TKMath/MathUtils/MathUtils_Types.hxx index 2e3f12e0bd..b95d541f08 100644 --- a/src/FoundationClasses/TKMath/MathUtils/MathUtils_Types.hxx +++ b/src/FoundationClasses/TKMath/MathUtils/MathUtils_Types.hxx @@ -36,7 +36,8 @@ enum class Status InfiniteSolutions, //!< Infinite number of solutions (degenerate case) NoSolution, //!< No solution exists NotPositiveDefinite, //!< Matrix not positive definite (for Cholesky, Newton, etc.) - Singular //!< Matrix is singular or nearly singular + Singular, //!< Matrix is singular or nearly singular + NonDescentDirection //!< Search direction is not a descent direction for merit function }; //! Result for scalar (1D) root finding and minimization. diff --git a/src/FoundationClasses/TKMath/MathUtils/README.md b/src/FoundationClasses/TKMath/MathUtils/README.md index ecfbdf602a..569c440e7c 100644 --- a/src/FoundationClasses/TKMath/MathUtils/README.md +++ b/src/FoundationClasses/TKMath/MathUtils/README.md @@ -16,12 +16,13 @@ The MathUtils package provides foundational utilities used by all other modern m ### Types and Configuration - `MathUtils_Types.hxx` - Common result types and status enums -- `MathUtils_Config.hxx` - Solver configuration structures +- `MathUtils_Config.hxx` - Solver configuration structures and shared Newton/line-search constants ### Core Utilities - `MathUtils_Core.hxx` - Mathematical constants and helper functions - `MathUtils_Convergence.hxx` - Convergence testing utilities - `MathUtils_Poly.hxx` - Polynomial evaluation and manipulation +- `MathUtils_Domain.hxx` - 1D/2D parameter domain helpers (contains/clamp/normalize/equality checks) - `MathUtils_Bracket.hxx` - Root and minimum bracketing algorithms - `MathUtils_Gauss.hxx` - Gauss-Legendre quadrature points and weights - `MathUtils_Deriv.hxx` - Numerical differentiation utilities @@ -72,13 +73,22 @@ auto result2 = MathRoot::Brent(func, 0.0, 2.0); ### Status Enum ```cpp enum class Status { - OK, // Success - Failed, // Generic failure - MaxIter, // Maximum iterations reached - NoConvergence // Algorithm did not converge + OK, + NotConverged, + MaxIterations, + NumericalError, + InvalidInput, + InfiniteSolutions, + NoSolution, + NotPositiveDefinite, + Singular, + NonDescentDirection }; ``` +Note: specialized `MathSys` 2D/3D/4D Newton solvers also use `MathUtils::Status` +via `MathSys_NewtonTypes.hxx`. + ### Result Types - `ScalarResult` - For 1D root finding results - `PolyResult` - For polynomial root results (up to 4 roots)