src/Extrema/Extrema_FuncExtPC.gxx

   1 // Copyright (c) 1995-1999 Matra Datavision
   2 // Copyright (c) 1999-2014 OPEN CASCADE SAS
   3 //
   4 // This file is part of Open CASCADE Technology software library.
   5 //
   6 // This library is free software; you can redistribute it and/or modify it under
   7 // the terms of the GNU Lesser General Public License version 2.1 as published
   8 // by the Free Software Foundation, with special exception defined in the file
   9 // OCCT_LGPL_EXCEPTION.txt. Consult the file LICENSE_LGPL_21.txt included in OCCT
  10 // distribution for complete text of the license and disclaimer of any warranty.
  11 //
  12 // Alternatively, this file may be used under the terms of Open CASCADE
  13 // commercial license or contractual agreement.
  14
  15 #include <Standard_TypeMismatch.hxx>
  16 #include <Precision.hxx>
  17 static const Standard_Real TolFactor = 1.e-12;
  18 static const Standard_Real MinTol    = 1.e-20;
  19 static const Standard_Real MinStep   = 1e-7;
  20
  21 static const Standard_Integer MaxOrder = 3;
  22
  23
  24
  25 /*-----------------------------------------------------------------------------
  26 Fonction permettant de rechercher une distance extremale entre un point P et
  27 une courbe C (en partant d'un point approche C(u0)).
  28 Cette classe herite de math_FunctionWithDerivative et est utilisee par
  29 les algorithmes math_FunctionRoot et math_FunctionRoots.
  30 Si on note D1c et D2c les derivees premiere et seconde:
  31 { F(u) = (C(u)-P).D1c(u)/ ||Du||}
  32 { DF(u) = ||Du|| + (C(u)-P).D2c(u)/||Du|| - F(u)*Duu*Du/||Du||**2
  33
  34
  35 { F(u) = (C(u)-P).D1c(u) }
  36 { DF(u) = D1c(u).D1c(u) + (C(u)-P).D2c(u)
  37 = ||D1c(u)|| ** 2 + (C(u)-P).D2c(u) }
  38 ----------------------------------------------------------------------------*/
  39
  40 Standard_Real Extrema_FuncExtPC::SearchOfTolerance()
  41 {
  42   const Standard_Integer NPoint = 10;
  43   const Standard_Real aStep = (myUsupremum - myUinfium)/(Standard_Real)NPoint;
  44
  45   Standard_Integer aNum = 0;
  46   Standard_Real aMax = -Precision::Infinite();  //Maximum value of 1st derivative
  47   //(it is computed with using NPoint point)
  48
  49   do
  50   {
  51     Standard_Real u = myUinfium + aNum*aStep;    //parameter for every point
  52     if(u > myUsupremum)
  53       u = myUsupremum;
  54
  55     Pnt Ptemp;  //empty point (is not used below)
  56     Vec VDer;   // 1st derivative vector
  57     Tool::D1(*((Curve*)myC), u, Ptemp, VDer);
  58
  59     if(Precision::IsInfinite(VDer.X()) || Precision::IsInfinite(VDer.Y()))
  60     {
  61       continue;
  62     }
  63
  64     Standard_Real vm = VDer.Magnitude();
  65     if(vm > aMax)
  66       aMax = vm;
  67   }
  68   while(++aNum < NPoint+1);
  69
  70   return Max(aMax*TolFactor,MinTol);
  71
  72 }
  73
  74 //=============================================================================
  75
  76 Extrema_FuncExtPC::Extrema_FuncExtPC():
  77 myU(0.),
  78 myD1f(0.)
  79 {
  80   myPinit = Standard_False;
  81   myCinit = Standard_False;
  82   myD1Init = Standard_False;
  83
  84   SubIntervalInitialize(0.0,0.0);
  85   myMaxDerivOrder = 0;
  86   myTol=MinTol;
  87 }
  88
  89 //=============================================================================
  90
  91 Extrema_FuncExtPC::Extrema_FuncExtPC (const Pnt& P,
  92                                       const Curve& C): myU(0.), myD1f(0.)
  93 {
  94   myP = P;
  95   myC = (Standard_Address)&C;
  96   myPinit = Standard_True;
  97   myCinit = Standard_True;
  98   myD1Init = Standard_False;
  99
 100   SubIntervalInitialize(Tool::FirstParameter(*((Curve*)myC)),
 101     Tool::LastParameter(*((Curve*)myC)));
 102
 103   switch(Tool::GetType(*((Curve*)myC)))
 104   {
 105   case GeomAbs_BezierCurve:
 106   case GeomAbs_BSplineCurve:
 107   case GeomAbs_OffsetCurve:
 108   case GeomAbs_OtherCurve:
 109     myMaxDerivOrder = MaxOrder;
 110     myTol = SearchOfTolerance();
 111     break;
 112   default:
 113     myMaxDerivOrder = 0;
 114     myTol=MinTol;
 115     break;
 116   }
 117 }
 118 //=============================================================================
 119
 120 void Extrema_FuncExtPC::Initialize(const Curve& C)
 121 {
 122   myC = (Standard_Address)&C;
 123   myCinit = Standard_True;
 124   myPoint.Clear();
 125   mySqDist.Clear();
 126   myIsMin.Clear();
 127
 128   SubIntervalInitialize(Tool::FirstParameter(*((Curve*)myC)),
 129     Tool::LastParameter(*((Curve*)myC)));
 130
 131   switch(Tool::GetType(*((Curve*)myC)))
 132   {
 133   case GeomAbs_BezierCurve:
 134   case GeomAbs_BSplineCurve:
 135   case GeomAbs_OffsetCurve:
 136   case GeomAbs_OtherCurve:
 137     myMaxDerivOrder = MaxOrder;
 138     myTol = SearchOfTolerance();
 139     break;
 140   default:
 141     myMaxDerivOrder = 0;
 142     myTol=MinTol;
 143     break;
 144   }
 145 }
 146
 147 //=============================================================================
 148
 149 void Extrema_FuncExtPC::SetPoint(const Pnt& P)
 150 {
 151   myP = P;
 152   myPinit = Standard_True;
 153   myPoint.Clear();
 154   mySqDist.Clear();
 155   myIsMin.Clear();
 156 }
 157
 158 //=============================================================================
 159
 160
 161 Standard_Boolean Extrema_FuncExtPC::Value (const Standard_Real U, Standard_Real& F)
 162 {
 163   if (!myPinit || !myCinit)
 164     Standard_TypeMismatch::Raise("No init");
 165
 166   myU = U;
 167   Vec D1c;
 168   Tool::D1(*((Curve*)myC),myU,myPc,D1c);
 169
 170   if(Precision::IsInfinite(D1c.X()) || Precision::IsInfinite(D1c.Y()))
 171   {
 172     F = Precision::Infinite();
 173     return Standard_False;
 174   }
 175
 176   Standard_Real Ndu = D1c.Magnitude();
 177
 178   if(myMaxDerivOrder != 0)
 179   {
 180     if (Ndu <= myTol) // Cas Singulier (PMN 22/04/1998)
 181     {
 182       const Standard_Real DivisionFactor = 1.e-3;
 183       Standard_Real du;
 184       if((myUsupremum >= RealLast()) || (myUinfium <= RealFirst()))
 185         du = 0.0;
 186       else
 187         du = myUsupremum-myUinfium;
 188
 189       const Standard_Real aDelta = Max(du*DivisionFactor,MinStep);
 190       //Derivative is approximated by Taylor-series
 191
 192       Standard_Integer n = 1; //Derivative order
 193       Vec V;
 194       Standard_Boolean IsDeriveFound;
 195
 196       do
 197       {
 198         V = Tool::DN(*((Curve*)myC),myU,++n);
 199         Ndu = V.Magnitude();
 200         IsDeriveFound = (Ndu > myTol);
 201       }
 202       while(!IsDeriveFound && n < myMaxDerivOrder);
 203
 204       if(IsDeriveFound)
 205       {
 206         Standard_Real u;
 207
 208         if(myU-myUinfium < aDelta)
 209           u = myU+aDelta;
 210         else
 211           u = myU-aDelta;
 212
 213         Pnt P1, P2;
 214         Tool::D0(*((Curve*)myC),Min(myU, u),P1);
 215         Tool::D0(*((Curve*)myC),Max(myU, u),P2);
 216
 217         Vec V1(P1,P2);
 218         Standard_Real aDirFactor = V.Dot(V1);
 219
 220         if(aDirFactor < 0.0)
 221           D1c = -V;
 222         else
 223           D1c = V;
 224       }//if(IsDeriveFound)
 225       else
 226       {
 227         //Derivative is approximated by three points
 228
 229         Pnt Ptemp; //(0,0,0)-coordinate
 230         Pnt P1, P2, P3;
 231         Standard_Boolean IsParameterGrown;
 232
 233         if(myU-myUinfium < 2*aDelta)
 234         {
 235           Tool::D0(*((Curve*)myC),myU,P1);
 236           Tool::D0(*((Curve*)myC),myU+aDelta,P2);
 237           Tool::D0(*((Curve*)myC),myU+2*aDelta,P3);
 238           IsParameterGrown = Standard_True;
 239         }
 240         else
 241         {
 242           Tool::D0(*((Curve*)myC),myU-2*aDelta,P1);
 243           Tool::D0(*((Curve*)myC),myU-aDelta,P2);
 244           Tool::D0(*((Curve*)myC),myU,P3);
 245           IsParameterGrown = Standard_False;
 246         }
 247
 248         Vec V1(Ptemp,P1), V2(Ptemp,P2), V3(Ptemp,P3);
 249
 250         if(IsParameterGrown)
 251           D1c=-3*V1+4*V2-V3;
 252         else
 253           D1c=V1-4*V2+3*V3;
 254       }
 255       Ndu = D1c.Magnitude();
 256     }//(if (Ndu <= myTol)) condition
 257   }//if(myMaxDerivOrder != 0)
 258
 259   if (Ndu <= MinTol)
 260   {
 261     //Warning: 1st derivative is equal to zero!
 262     return Standard_False;
 263   }
 264
 265   Vec PPc (myP,myPc);
 266   F = PPc.Dot(D1c)/Ndu;
 267   return Standard_True;
 268 }
 269
 270 //=============================================================================
 271
 272 Standard_Boolean Extrema_FuncExtPC::Derivative (const Standard_Real U, Standard_Real& D1f)
 273 {
 274   if (!myPinit || !myCinit) Standard_TypeMismatch::Raise();
 275   Standard_Real F;
 276   return Values(U,F,D1f);  /* on fait appel a Values pour simplifier la
 277                            sauvegarde de l'etat. */
 278 }
 279 //=============================================================================
 280
 281 Standard_Boolean Extrema_FuncExtPC::Values (const Standard_Real U,
 282                                             Standard_Real& F,
 283                                             Standard_Real& D1f)
 284 {
 285   if (!myPinit || !myCinit)
 286     Standard_TypeMismatch::Raise("No init");
 287
 288   Pnt myPc_old = myPc, myP_old = myP;
 289
 290   if(Value(U,F) == Standard_False)
 291   {
 292     //Warning: No function value found!;
 293
 294     myD1Init = Standard_False;
 295     return Standard_False;
 296   }
 297
 298   myU = U;
 299   myPc = myPc_old;
 300   myP = myP_old;
 301
 302   Vec D1c,D2c;
 303   Tool::D2(*((Curve*)myC),myU,myPc,D1c,D2c);
 304
 305   Standard_Real Ndu = D1c.Magnitude();
 306   if (Ndu <= myTol) // Cas Singulier (PMN 22/04/1998)
 307   {
 308     //Derivative is approximated by three points
 309
 310     //Attention:  aDelta value must be greater than same value for "Value(...)"
 311     //            function to avoid of points' collisions.
 312     const Standard_Real DivisionFactor = 0.01;
 313     Standard_Real du;
 314     if((myUsupremum >= RealLast()) || (myUinfium <= RealFirst()))
 315       du = 0.0;
 316     else
 317       du = myUsupremum-myUinfium;
 318
 319     const Standard_Real aDelta = Max(du*DivisionFactor,MinStep);
 320
 321     Standard_Real F1, F2, F3;
 322
 323     if(myU-myUinfium < 2*aDelta)
 324     {
 325       F1=F;
 326       //const Standard_Real U1 = myU;
 327       const Standard_Real U2 = myU + aDelta;
 328       const Standard_Real U3 = myU + aDelta * 2.0;
 329
 330       if(!((Value(U2,F2)) && (Value(U3,F3))))
 331       {
 332         //There are many points close to singularity points and
 333         //which have zero-derivative. Try to decrease aDelta variable's value.
 334
 335         myD1Init = Standard_False;
 336         return Standard_False;
 337       }
 338
 339       //After calling of Value(...) function variable myU will be redeterminated.
 340       //So we must return it previous value.
 341       D1f=(-3*F1+4*F2-F3)/(2.0*aDelta);
 342     }
 343     else
 344     {
 345       F3 = F;
 346       const Standard_Real U1 = myU - aDelta * 2.0;
 347       const Standard_Real U2 = myU - aDelta;
 348       //const Standard_Real U3 = myU;
 349
 350       if(!((Value(U2,F2)) && (Value(U1,F1))))
 351       {
 352         //There are many points close to singularity points and
 353         //which have zero-derivative. Try to decrease aDelta variable's value.
 354         myD1Init = Standard_False;
 355         return Standard_False;
 356       }
 357       //After calling of Value(...) function variable myU will be redeterminated.
 358       //So we must return it previous value.
 359       D1f=(F1-4*F2+3*F3)/(2.0*aDelta);
 360     }
 361     myU = U;
 362     myPc = myPc_old;
 363     myP = myP_old;
 364   }
 365   else
 366   {
 367     Vec PPc (myP,myPc);
 368     D1f = Ndu + (PPc.Dot(D2c)/Ndu) - F*(D1c.Dot(D2c))/(Ndu*Ndu);
 369   }
 370
 371   myD1f = D1f;
 372
 373   myD1Init = Standard_True;
 374   return Standard_True;
 375 }
 376 //=============================================================================
 377
 378 Standard_Integer Extrema_FuncExtPC::GetStateNumber ()
 379 {
 380   if (!myPinit || !myCinit) Standard_TypeMismatch::Raise();
 381   mySqDist.Append(myPc.SquareDistance(myP));
 382
 383   // It is necessary to always compute myD1f.
 384   myD1Init = Standard_True;
 385   Standard_Real FF, DD;
 386   Values(myU, FF, DD);
 387
 388   Standard_Integer IntVal = 0;
 389   if (myD1f > 0.0)
 390   {
 391     IntVal = 1;
 392   }
 393
 394   myIsMin.Append(IntVal);
 395   myPoint.Append(POnC(myU,myPc));
 396   return 0;
 397 }
 398 //=============================================================================
 399
 400 Standard_Integer Extrema_FuncExtPC::NbExt () const { return mySqDist.Length(); }
 401 //=============================================================================
 402
 403 Standard_Real Extrema_FuncExtPC::SquareDistance (const Standard_Integer N) const
 404 {
 405   if (!myPinit || !myCinit) Standard_TypeMismatch::Raise();
 406   return mySqDist.Value(N);
 407 }
 408 //=============================================================================
 409 Standard_Boolean Extrema_FuncExtPC::IsMin (const Standard_Integer N) const
 410 {
 411   if (!myPinit || !myCinit) Standard_TypeMismatch::Raise();
 412   return (myIsMin.Value(N) == 1);
 413 }
 414 //=============================================================================
 415 const POnC & Extrema_FuncExtPC::Point (const Standard_Integer N) const
 416 {
 417   if (!myPinit || !myCinit) Standard_TypeMismatch::Raise();
 418   return myPoint.Value(N);
 419 }
 420 //=============================================================================
 421
 422 void Extrema_FuncExtPC::SubIntervalInitialize(const Standard_Real theUfirst, const Standard_Real theUlast)
 423 {
 424   myUinfium = theUfirst;
 425   myUsupremum = theUlast;
 426 }