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> 17static const Standard_Real TolFactor = 1.e-12; 18static const Standard_Real MinTol = 1.e-20; 19static const Standard_Real MinStep = 1e-7; 20 21static const Standard_Integer MaxOrder = 3; 22 23 24 25/*----------------------------------------------------------------------------- 26Fonction permettant de rechercher une distance extremale entre un point P et 27une courbe C (en partant d'un point approche C(u0)). 28Cette classe herite de math_FunctionWithDerivative et est utilisee par 29les algorithmes math_FunctionRoot et math_FunctionRoots. 30Si 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 40Standard_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 76Extrema_FuncExtPC::Extrema_FuncExtPC(): 77myU(0.), 78myD1f(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 91Extrema_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 120void 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 149void 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 161Standard_Boolean Extrema_FuncExtPC::Value (const Standard_Real U, Standard_Real& F) 162{ 163 if (!myPinit || !myCinit) 164 throw Standard_TypeMismatch("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 272Standard_Boolean Extrema_FuncExtPC::Derivative (const Standard_Real U, Standard_Real& D1f) 273{ 274 if (!myPinit || !myCinit) throw Standard_TypeMismatch(); 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 281Standard_Boolean Extrema_FuncExtPC::Values (const Standard_Real U, 282 Standard_Real& F, 283 Standard_Real& D1f) 284{ 285 if (!myPinit || !myCinit) 286 throw Standard_TypeMismatch("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 378Standard_Integer Extrema_FuncExtPC::GetStateNumber () 379{ 380 if (!myPinit || !myCinit) throw Standard_TypeMismatch(); 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 400Standard_Integer Extrema_FuncExtPC::NbExt () const { return mySqDist.Length(); } 401//============================================================================= 402 403Standard_Real Extrema_FuncExtPC::SquareDistance (const Standard_Integer N) const 404{ 405 if (!myPinit || !myCinit) throw Standard_TypeMismatch(); 406 return mySqDist.Value(N); 407} 408//============================================================================= 409Standard_Boolean Extrema_FuncExtPC::IsMin (const Standard_Integer N) const 410{ 411 if (!myPinit || !myCinit) throw Standard_TypeMismatch(); 412 return (myIsMin.Value(N) == 1); 413} 414//============================================================================= 415const POnC & Extrema_FuncExtPC::Point (const Standard_Integer N) const 416{ 417 if (!myPinit || !myCinit) throw Standard_TypeMismatch(); 418 return myPoint.Value(N); 419} 420//============================================================================= 421 422void Extrema_FuncExtPC::SubIntervalInitialize(const Standard_Real theUfirst, const Standard_Real theUlast) 423{ 424 myUinfium = theUfirst; 425 myUsupremum = theUlast; 426} 427