EIC code displayed by LXR master/​include/​opencascade/​MathRoot_Trig.hxx	Source navigation Diff markup Identifier search General search
	Version: master test Revert   Change

File indexing completed on 2026-09-28 09:20:52

 // 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 _MathRoot_Trig_HeaderFile
 #define _MathRoot_Trig_HeaderFile
 
 #include <MathUtils_Types.hxx>
 #include <MathUtils_Config.hxx>
 #include <MathUtils_Core.hxx>
 #include <MathPoly_Quadratic.hxx>
 #include <MathPoly_Quartic.hxx>
 
 #include <cmath>
 #include <algorithm>
 
 namespace MathRoot
 {
 using namespace MathUtils;
 
 //! Result for trigonometric equation solver.
 struct TrigResult
 {
   MathUtils::Status     Status        = MathUtils::Status::NotConverged;
   std::array<double, 4> Roots         = {0.0, 0.0, 0.0, 0.0};
   int                   NbRoots       = 0;
   bool                  InfiniteRoots = false;
 
   bool IsDone() const { return Status == MathUtils::Status::OK; }
 
   explicit operator bool() const { return IsDone(); }
 };
 
 //! Solve trigonometric equation: a*cos^2(x) + 2*b*cos(x)*sin(x) + c*cos(x) + d*sin(x) + e = 0.
 //!
 //! Uses half-angle substitution t = tan(x/2) to convert to polynomial:
 //! - cos(x) = (1-t^2)/(1+t^2)
 //! - sin(x) = 2t/(1+t^2)
 //!
 //! Resulting polynomial is of degree 4, 3, or 2 depending on coefficients.
 //! Roots are filtered to lie within [theInfBound, theSupBound].
 //!
 //! @param theA coefficient of cos^2(x)
 //! @param theB coefficient of cos(x)*sin(x) (equation uses 2*b)
 //! @param theC coefficient of cos(x)
 //! @param theD coefficient of sin(x)
 //! @param theE constant term
 //! @param theInfBound lower bound for roots (default 0)
 //! @param theSupBound upper bound for roots (default 2*PI)
 //! @param theEps tolerance for coefficient comparison
 //! @return TrigResult containing roots in specified interval
 inline TrigResult Trigonometric(double theA,
                                 double theB,
                                 double theC,
                                 double theD,
                                 double theE,
                                 double theInfBound = 0.0,
                                 double theSupBound = THE_2PI,
                                 double theEps      = 1.5e-12)
 {
   TrigResult aResult;
   aResult.Status = MathUtils::Status::OK;
 
   // Compute working interval
   double aMyBorneInf, aDelta, aMod;
   if (theInfBound <= std::numeric_limits<double>::lowest() / 2.0
       && theSupBound >= std::numeric_limits<double>::max() / 2.0)
   {
     aMyBorneInf = 0.0;
     aDelta      = THE_2PI;
     aMod        = 0.0;
   }
   else if (theSupBound >= std::numeric_limits<double>::max() / 2.0)
   {
     aMyBorneInf = theInfBound;
     aDelta      = THE_2PI;
     aMod        = aMyBorneInf / THE_2PI;
   }
   else if (theInfBound <= std::numeric_limits<double>::lowest() / 2.0)
   {
     aMyBorneInf = theSupBound - THE_2PI;
     aDelta      = THE_2PI;
     aMod        = aMyBorneInf / THE_2PI;
   }
   else
   {
     aMyBorneInf = theInfBound;
     aDelta      = theSupBound - theInfBound;
     aMod        = theInfBound / THE_2PI;
     if (aDelta > THE_2PI)
     {
       aDelta = THE_2PI;
     }
   }
 
   std::array<double, 4> aZer       = {0.0, 0.0, 0.0, 0.0};
   size_t                aNZer      = 0;
   const double          aDelta_Eps = std::numeric_limits<double>::epsilon() * std::abs(aDelta);
 
   // Case: A = B = 0 (degree <= 2 in cos/sin)
   if (std::abs(theA) <= theEps && std::abs(theB) <= theEps)
   {
     if (std::abs(theC) <= theEps)
     {
       if (std::abs(theD) <= theEps)
       {
         if (std::abs(theE) <= theEps)
         {
           aResult.InfiniteRoots = true;
           return aResult;
         }
         else
         {
           aResult.NbRoots = 0;
           return aResult;
         }
       }
       else
       {
         // d*sin(x) + e = 0  =>  sin(x) = -e/d
         double aVal = -theE / theD;
         if (std::abs(aVal) > 1.0)
         {
           aResult.NbRoots = 0;
           return aResult;
         }
 
         aZer[0] = std::asin(aVal);
         aZer[1] = THE_PI - aZer[0];
         aNZer   = 2;
 
         // Adjust to positive range
         for (size_t i = 0; i < aNZer; ++i)
         {
           if (aZer[i] <= -theEps)
           {
             aZer[i] = THE_2PI - std::abs(aZer[i]);
           }
           aZer[i] += std::trunc(aMod) * THE_2PI;
           double aX = aZer[i] - aMyBorneInf;
           if (aX >= -aDelta_Eps && aX <= aDelta + aDelta_Eps)
           {
             aResult.Roots[aResult.NbRoots++] = aZer[i];
           }
         }
         return aResult;
       }
     }
     else if (std::abs(theD) <= theEps)
     {
       // c*cos(x) + e = 0  =>  cos(x) = -e/c
       double aVal = -theE / theC;
       if (std::abs(aVal) > 1.0)
       {
         aResult.NbRoots = 0;
         return aResult;
       }
 
       double aPrincipal = std::acos(aVal);
       aZer[0]           = aPrincipal;  // acos gives [0, PI]
       aZer[1]           = -aPrincipal; // Negative angle
       aNZer             = 2;
 
       // For each solution, find the representative in or near the given bounds
       for (size_t i = 0; i < aNZer; ++i)
       {
         double aAngle = aZer[i];
         // Shift angle to be near the lower bound
         double aK = std::floor((theInfBound - aAngle) / THE_2PI);
         aAngle += (aK + 1) * THE_2PI; // Start from a value >= theInfBound - 2*PI
 
         // Check both this value and the next period
         for (int aPeriod = 0; aPeriod < 2; ++aPeriod)
         {
           double aTestAngle = aAngle + aPeriod * THE_2PI;
           if (aTestAngle >= theInfBound - aDelta_Eps && aTestAngle <= theSupBound + aDelta_Eps)
           {
             // Avoid duplicates
             bool aDup = false;
             for (int k = 0; k < aResult.NbRoots; ++k)
             {
               if (std::abs(aTestAngle - aResult.Roots[k]) < theEps)
               {
                 aDup = true;
                 break;
               }
             }
             if (!aDup && aResult.NbRoots < 4)
             {
               aResult.Roots[aResult.NbRoots++] = aTestAngle;
             }
           }
         }
       }
       return aResult;
     }
     else
     {
       // c*cos(x) + d*sin(x) + e = 0
       // Using t = tan(x/2): (e-c)*t^2 + 2d*t + (e+c) = 0
       double aAA = theE - theC;
       double aBB = 2.0 * theD;
       double aCC = theE + theC;
 
       MathPoly::PolyResult aPoly = MathPoly::Quadratic(aAA, aBB, aCC);
       if (!aPoly.IsDone())
       {
         aResult.Status = aPoly.Status;
         return aResult;
       }
       if (aPoly.Status == MathUtils::Status::InfiniteSolutions)
       {
         aResult.InfiniteRoots = true;
         return aResult;
       }
 
       aNZer = aPoly.NbRoots;
       for (size_t i = 0; i < aNZer; ++i)
       {
         aZer[i] = aPoly.Roots[i];
       }
     }
   }
   else
   {
     // Special case: A = E = 0
     if (std::abs(theA) <= theEps && std::abs(theE) <= theEps)
     {
       if (std::abs(theC) <= theEps)
       {
         // 2*B*sin*cos + D*sin = 0  =>  sin(x)*(2*B*cos(x) + D) = 0
         aZer[0] = 0.0;
         aZer[1] = THE_PI;
         aNZer   = 2;
 
         double aVal = -theD / (theB * 2.0);
         if (std::abs(aVal) <= 1.0 + 1.0e-10)
         {
           if (aVal >= 1.0)
           {
             aZer[2] = 0.0;
             aZer[3] = 0.0;
           }
           else if (aVal <= -1.0)
           {
             aZer[2] = THE_PI;
             aZer[3] = THE_PI;
           }
           else
           {
             aZer[2] = std::acos(aVal);
             aZer[3] = THE_2PI - aZer[2];
           }
           aNZer = 4;
         }
 
         for (size_t i = 0; i < aNZer; ++i)
         {
           if (aZer[i] <= aMyBorneInf - theEps)
           {
             aZer[i] += THE_2PI;
           }
           aZer[i] += std::trunc(aMod) * THE_2PI;
           double aX = aZer[i] - aMyBorneInf;
           if (aX >= -1.0e-10 && aX <= aDelta + 1.0e-10)
           {
             aZer[i] = std::max(theInfBound, std::min(theSupBound, aZer[i]));
             aResult.Roots[aResult.NbRoots++] = aZer[i];
           }
         }
         return aResult;
       }
       if (std::abs(theD) <= theEps)
       {
         // 2*B*sin*cos + C*cos = 0  =>  cos(x)*(2*B*sin(x) + C) = 0
         aZer[0] = THE_PI / 2.0;
         aZer[1] = THE_PI * 3.0 / 2.0;
         aNZer   = 2;
 
         double aVal = -theC / (theB * 2.0);
         if (std::abs(aVal) <= 1.0 + 1.0e-10)
         {
           if (aVal >= 1.0)
           {
             aZer[2] = THE_PI / 2.0;
             aZer[3] = THE_PI / 2.0;
           }
           else if (aVal <= -1.0)
           {
             aZer[2] = THE_PI * 3.0 / 2.0;
             aZer[3] = THE_PI * 3.0 / 2.0;
           }
           else
           {
             aZer[2] = std::asin(aVal);
             aZer[3] = THE_PI - aZer[2];
           }
           aNZer = 4;
         }
 
         for (size_t i = 0; i < aNZer; ++i)
         {
           if (aZer[i] <= aMyBorneInf - theEps)
           {
             aZer[i] += THE_2PI;
           }
           aZer[i] += std::trunc(aMod) * THE_2PI;
           double aX = aZer[i] - aMyBorneInf;
           if (aX >= -1.0e-10 && aX <= aDelta + 1.0e-10)
           {
             aZer[i] = std::max(theInfBound, std::min(theSupBound, aZer[i]));
             aResult.Roots[aResult.NbRoots++] = aZer[i];
           }
         }
         return aResult;
       }
     }
 
     // General case: degree 4 polynomial
     // t = tan(x/2), then:
     // ko[0]*t^4 + ko[1]*t^3 + ko[2]*t^2 + ko[3]*t + ko[4] = 0
     double ko0 = theA - theC + theE;
     double ko1 = 2.0 * theD - 4.0 * theB;
     double ko2 = 2.0 * theE - 2.0 * theA;
     double ko3 = 4.0 * theB + 2.0 * theD;
     double ko4 = theA + theC + theE;
 
     MathPoly::PolyResult aPoly = MathPoly::Quartic(ko0, ko1, ko2, ko3, ko4);
     if (!aPoly.IsDone())
     {
       if (aPoly.Status == MathUtils::Status::InfiniteSolutions)
       {
         aResult.InfiniteRoots = true;
       }
       else
       {
         aResult.Status = aPoly.Status;
       }
       return aResult;
     }
 
     // NbRoots is bounded by the fixed storage capacity; clamp defensively.
     aNZer = std::min<size_t>(aPoly.NbRoots, aZer.size());
     for (size_t i = 0; i < aNZer; ++i)
     {
       aZer[i] = aPoly.Roots[i];
     }
 
     // Sort roots
     std::sort(aZer.begin(), aZer.begin() + aNZer);
   }
 
   // Convert t values to angles and filter by bounds
   for (size_t i = 0; i < aNZer; ++i)
   {
     double aTeta = 2.0 * std::atan(aZer[i]);
     if (aZer[i] <= -theEps)
     {
       aTeta = THE_2PI - std::abs(aTeta);
     }
     aTeta += std::trunc(aMod) * THE_2PI;
     if (aTeta - aMyBorneInf < 0.0)
     {
       aTeta += THE_2PI;
     }
 
     double aX = aTeta - aMyBorneInf;
     if (aX >= -aDelta_Eps && aX <= aDelta + aDelta_Eps)
     {
       // Newton refinement with Halley's method fallback for double roots
       auto aRefineRoot = [&](double theX) -> double {
         constexpr int    THE_MAX_ITER = 20;
         constexpr double THE_TOL      = 1.0e-14;
 
         for (int anIter = 0; anIter < THE_MAX_ITER; ++anIter)
         {
           double aCos  = std::cos(theX);
           double aSin  = std::sin(theX);
           double aCos2 = aCos * aCos;
           double aSin2 = aSin * aSin;
           double aCS   = aCos * aSin;
 
           double aF  = theA * aCos2 + 2.0 * theB * aCS + theC * aCos + theD * aSin + theE;
           double aDF = -2.0 * theA * aCS + 2.0 * theB * (aCos2 - aSin2) - theC * aSin + theD * aCos;
 
           // Check if already converged
           if (std::abs(aF) < 1.0e-15)
           {
             break;
           }
 
           double aDelta;
           if (std::abs(aDF) < 1.0e-10 * (std::abs(aF) + 1.0))
           {
             // Near double root: use Halley's method for better convergence
             // F'' = -2*a*(cos^2-sin^2) - 4*b*cos*sin - c*cos - d*sin
             double aD2F =
               -2.0 * theA * (aCos2 - aSin2) - 4.0 * theB * aCS - theC * aCos - theD * aSin;
             double aDenom = 2.0 * aDF * aDF - aF * aD2F;
             if (std::abs(aDenom) < 1.0e-30)
             {
               // Can't improve further
               break;
             }
             aDelta = 2.0 * aF * aDF / aDenom;
           }
           else
           {
             // Standard Newton step
             aDelta = aF / aDF;
           }
 
           // Limit step size to avoid overshooting
           constexpr double THE_MAX_STEP = 0.5;
           if (std::abs(aDelta) > THE_MAX_STEP)
           {
             aDelta = (aDelta > 0) ? THE_MAX_STEP : -THE_MAX_STEP;
           }
 
           theX -= aDelta;
 
           if (std::abs(aDelta) < THE_TOL)
           {
             break;
           }
         }
         return theX;
       };
 
       double aTetaRefined = aRefineRoot(aTeta);
 
       // Check if Newton didn't diverge too far
       double aDeltaNewton = std::abs(aTetaRefined - aTeta);
       double aSupmInfs100 = (theSupBound - theInfBound) * 0.01;
       if (aDeltaNewton <= aSupmInfs100)
       {
         aTeta = aTetaRefined;
       }
 
       // Insert in sorted order, avoiding duplicates
       bool aFound = false;
       for (int k = 0; k < aResult.NbRoots; ++k)
       {
         if (std::abs(aTeta - aResult.Roots[k]) < theEps)
         {
           aFound = true;
           break;
         }
       }
 
       if (!aFound && aResult.NbRoots < 4)
       {
         // Insert sorted
         int aPos = aResult.NbRoots;
         for (int k = 0; k < aResult.NbRoots; ++k)
         {
           if (aTeta < aResult.Roots[k])
           {
             aPos = k;
             break;
           }
         }
         for (int k = aResult.NbRoots; k > aPos; --k)
         {
           aResult.Roots[k] = aResult.Roots[k - 1];
         }
         aResult.Roots[aPos] = aTeta;
         aResult.NbRoots++;
       }
     }
   }
 
   // Special case: check if PI is a root (when A - C + E = 0)
   if (aResult.NbRoots < 4 && std::abs(theA - theC + theE) <= theEps)
   {
     double aTeta = THE_PI + std::trunc(aMod) * THE_2PI;
     double aX    = aTeta - aMyBorneInf;
     if (aX >= -aDelta_Eps && aX <= aDelta + aDelta_Eps)
     {
       bool aFound = false;
       for (int k = 0; k < aResult.NbRoots; ++k)
       {
         if (std::abs(aTeta - aResult.Roots[k]) <= theEps)
         {
           aFound = true;
           break;
         }
       }
       if (!aFound)
       {
         int aPos = aResult.NbRoots;
         for (int k = 0; k < aResult.NbRoots; ++k)
         {
           if (aTeta < aResult.Roots[k])
           {
             aPos = k;
             break;
           }
         }
         for (int k = aResult.NbRoots; k > aPos; --k)
         {
           aResult.Roots[k] = aResult.Roots[k - 1];
         }
         aResult.Roots[aPos] = aTeta;
         aResult.NbRoots++;
       }
     }
   }
 
   return aResult;
 }
 
 //! Solve linear trigonometric equation: d*sin(x) + e = 0.
 //!
 //! @param theD coefficient of sin(x)
 //! @param theE constant term
 //! @param theInfBound lower bound for roots
 //! @param theSupBound upper bound for roots
 //! @return TrigResult containing roots
 inline TrigResult TrigonometricLinear(double theD,
                                       double theE,
                                       double theInfBound = 0.0,
                                       double theSupBound = THE_2PI)
 {
   return Trigonometric(0.0, 0.0, 0.0, theD, theE, theInfBound, theSupBound);
 }
 
 //! Solve trigonometric equation: c*cos(x) + d*sin(x) + e = 0.
 //!
 //! @param theC coefficient of cos(x)
 //! @param theD coefficient of sin(x)
 //! @param theE constant term
 //! @param theInfBound lower bound for roots
 //! @param theSupBound upper bound for roots
 //! @return TrigResult containing roots
 inline TrigResult TrigonometricCDE(double theC,
                                    double theD,
                                    double theE,
                                    double theInfBound = 0.0,
                                    double theSupBound = THE_2PI)
 {
   return Trigonometric(0.0, 0.0, theC, theD, theE, theInfBound, theSupBound);
 }
 
 } // namespace MathRoot
 
 #endif // _MathRoot_Trig_HeaderFile

This page was automatically generated by the 2.3.7 LXR engine. The LXR team