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