0031007: Coding - eliminate warnings issued while compiling with -pedantic flag
[occt.git] / src / AppCont / AppCont_LeastSquare.cxx
1 // Created on: 1995-03-14
2 // Created by: Modelistation
3 // Copyright (c) 1995-1999 Matra Datavision
4 // Copyright (c) 1999-2014 OPEN CASCADE SAS
5 //
6 // This file is part of Open CASCADE Technology software library.
7 //
8 // This library is free software; you can redistribute it and/or modify it under
9 // the terms of the GNU Lesser General Public License version 2.1 as published
10 // by the Free Software Foundation, with special exception defined in the file
11 // OCCT_LGPL_EXCEPTION.txt. Consult the file LICENSE_LGPL_21.txt included in OCCT
12 // distribution for complete text of the license and disclaimer of any warranty.
13 //
14 // Alternatively, this file may be used under the terms of Open CASCADE
15 // commercial license or contractual agreement.
16
17 #ifndef OCCT_DEBUG
18 #define No_Standard_OutOfRange
19 #define No_Standard_RangeError
20 #endif
21 #include <AppCont_LeastSquare.hxx>
22
23 #include <math.hxx>
24 #include <AppParCurves_MultiPoint.hxx>
25 #include <AppCont_ContMatrices.hxx>
26 #include <PLib.hxx>
27 #include <Precision.hxx>
28
29
30 //=======================================================================
31 //function : AppCont_LeastSquare
32 //purpose  : 
33 //=======================================================================
34 void AppCont_LeastSquare::FixSingleBorderPoint(const AppCont_Function&       theSSP,
35                                                const Standard_Real           theU,
36                                                const Standard_Real           theU0,
37                                                const Standard_Real           theU1,
38                                                NCollection_Array1<gp_Pnt2d>& theFix2d,
39                                                NCollection_Array1<gp_Pnt>&   theFix)
40 {
41   Standard_Integer aMaxIter = 15;
42   Standard_Integer j, i2;
43   NCollection_Array1<gp_Pnt>   aTabP(1, Max (myNbP, 1)),     aPrevP(1, Max (myNbP, 1));
44   NCollection_Array1<gp_Pnt2d> aTabP2d(1,  Max (myNbP2d, 1)), aPrevP2d(1,  Max (myNbP2d, 1));
45   Standard_Real aMult = ((theU - theU0) > (theU1 - theU)) ? 1.0: -1.0;
46   Standard_Real aStartParam = theU,
47                 aCurrParam, aPrevDist = 1.0, aCurrDist = 1.0;
48
49   Standard_Real du = -(theU1 - theU0) / 2.0 * aMult;
50   Standard_Real eps = Epsilon(1.);
51   Standard_Real dd = du, dec = .1;
52   for (Standard_Integer anIter = 1; anIter < aMaxIter; anIter++)
53   {
54     dd *=  dec;
55     aCurrParam = aStartParam + dd;
56     theSSP.Value(aCurrParam, aTabP2d, aTabP);
57
58     // from second iteration
59     if (anIter > 1)
60     {
61       aCurrDist = 0.0;
62
63       i2 = 1;
64       for (j = 1; j <= myNbP; j++)
65       {
66         aCurrDist += aTabP(j).Distance(aPrevP(j));
67         i2 += 3;
68       }
69       for (j = 1; j <= myNbP2d; j++)
70       {
71         aCurrDist += aTabP2d(j).Distance(aPrevP2d(j));
72         i2 += 2;
73       }
74
75       // from the third iteration
76       if (anIter > 2 && aCurrDist / aPrevDist > 10.0)
77         break;
78     }
79     aPrevP = aTabP;
80     aPrevP2d = aTabP2d;
81     aPrevDist = aCurrDist;
82     if(aPrevDist <= eps)
83       break;
84   }
85   theFix2d = aPrevP2d;
86   theFix   = aPrevP;
87 }
88
89
90 //=======================================================================
91 //function : AppCont_LeastSquare
92 //purpose  : 
93 //=======================================================================
94
95 AppCont_LeastSquare::AppCont_LeastSquare(const AppCont_Function&       SSP,
96                                          const Standard_Real           U0,
97                                          const Standard_Real           U1,
98                                          const AppParCurves_Constraint FirstCons,
99                                          const AppParCurves_Constraint LastCons,
100                                          const Standard_Integer        Deg,
101                                          const Standard_Integer        myNbPoints)
102 : mySCU(Deg+1),
103   myPoints(1, myNbPoints, 1, 3 * SSP.GetNbOf3dPoints() + 2 * SSP.GetNbOf2dPoints()),
104   myPoles(1, Deg + 1, 1, 3 * SSP.GetNbOf3dPoints() + 2 * SSP.GetNbOf2dPoints(), 0.0),
105   myParam(1, myNbPoints),
106   myVB(1, Deg+1, 1, myNbPoints),
107   myPerInfo(1, 3 * SSP.GetNbOf3dPoints() + 2 * SSP.GetNbOf2dPoints() )
108 {
109   myDone = Standard_False;
110   myDegre = Deg;
111   Standard_Integer i, j, k, c, i2;
112   Standard_Integer classe = Deg + 1, cl1 = Deg;
113   Standard_Real U, dU, Coeff, Coeff2;
114   Standard_Real IBij, IBPij;
115
116   Standard_Integer FirstP = 1, LastP = myNbPoints;
117   Standard_Integer nbcol = 3 * SSP.GetNbOf3dPoints() + 2 * SSP.GetNbOf2dPoints();
118   math_Matrix B(1, classe, 1, nbcol, 0.0);
119   Standard_Integer bdeb = 1, bfin = classe;
120   AppParCurves_Constraint myFirstC = FirstCons, myLastC = LastCons;
121   SSP.GetNumberOfPoints(myNbP, myNbP2d);
122
123   Standard_Integer i2plus1, i2plus2;
124   myNbdiscret = myNbPoints;
125   NCollection_Array1<gp_Pnt>   aTabP(1, Max (myNbP, 1));
126   NCollection_Array1<gp_Pnt2d> aTabP2d(1, Max (myNbP2d, 1));
127   NCollection_Array1<gp_Vec>   aTabV(1, Max (myNbP, 1));
128   NCollection_Array1<gp_Vec2d> aTabV2d(1, Max (myNbP2d, 1));
129
130   for(Standard_Integer aDimIdx = 1; aDimIdx <= myNbP * 3 + myNbP2d * 2; aDimIdx++)
131   {
132     SSP.PeriodInformation(aDimIdx, 
133                           myPerInfo(aDimIdx).isPeriodic,
134                           myPerInfo(aDimIdx).myPeriod);
135   }
136
137   Standard_Boolean Ok;
138   if (myFirstC == AppParCurves_TangencyPoint) 
139   {
140     Ok = SSP.D1(U0, aTabV2d, aTabV);
141     if (!Ok) myFirstC = AppParCurves_PassPoint;
142   }
143
144   if (myLastC == AppParCurves_TangencyPoint)
145   {
146     Ok = SSP.D1(U1, aTabV2d, aTabV);
147     if (!Ok) myLastC = AppParCurves_PassPoint;
148   }
149
150   // Compute control points params on which approximation will be built.
151   math_Vector GaussP(1, myNbPoints), GaussW(1, myNbPoints);
152   math::GaussPoints(myNbPoints, GaussP);
153   math::GaussWeights(myNbPoints, GaussW);
154   math_Vector TheWeights(1, myNbPoints), VBParam(1, myNbPoints);
155   dU = 0.5*(U1-U0);
156   for (i = FirstP; i <= LastP; i++)
157   {
158     U  = 0.5 * (U1 + U0) + dU * GaussP(i);
159     if (i <=  (myNbPoints+1)/2)
160     {
161       myParam(LastP - i + 1)  = U;
162       VBParam(LastP - i + 1)  = 0.5 * (1 + GaussP(i));
163       TheWeights(LastP - i + 1) = 0.5 * GaussW(i);
164     }
165     else
166     {
167       VBParam(i - (myNbPoints + 1) / 2)  = 0.5*(1 + GaussP(i));
168       myParam(i - (myNbPoints + 1) / 2) = U;
169       TheWeights(i - (myNbPoints+ 1) / 2) = 0.5 * GaussW(i);
170     }
171   }
172
173   // Compute control points.
174   for (i = FirstP; i <= LastP; i++)
175   {
176     U = myParam(i);
177     SSP.Value(U, aTabP2d, aTabP);
178
179     i2 = 1;
180     for (j = 1; j <= myNbP; j++)
181     {
182       (aTabP(j)).Coord(myPoints(i, i2), myPoints(i, i2+1), myPoints(i, i2+2));
183       i2 += 3;
184     }
185     for (j = 1; j <= myNbP2d; j++)
186     {
187       (aTabP2d(j)).Coord(myPoints(i, i2), myPoints(i, i2+1));
188       i2 += 2;
189     }
190   }
191
192   // Fix possible "period jump".
193   Standard_Integer aMaxDim = 3 * myNbP + 2 * myNbP2d;
194   for(Standard_Integer aDimIdx = 1; aDimIdx <= aMaxDim; aDimIdx++)
195   {
196     if (myPerInfo(aDimIdx).isPeriodic &&
197         Abs (myPoints(1, aDimIdx) - myPoints(2, aDimIdx)) > myPerInfo(aDimIdx).myPeriod / 2.01 &&
198         Abs (myPoints(2, aDimIdx) - myPoints(3, aDimIdx)) < myPerInfo(aDimIdx).myPeriod / 2.01)
199     {
200       Standard_Real aPeriodMult = (myPoints(1, aDimIdx) < myPoints(2, aDimIdx)) ? 1.0 : -1.0;
201       Standard_Real aNewParam = myPoints(1, aDimIdx) + aPeriodMult * myPerInfo(aDimIdx).myPeriod;
202       myPoints(1, aDimIdx) = aNewParam;
203     }
204   }
205   for (Standard_Integer aPntIdx = 1; aPntIdx < myNbPoints; aPntIdx++)
206   {
207     for(Standard_Integer aDimIdx = 1; aDimIdx <= aMaxDim; aDimIdx++)
208     {
209       if (myPerInfo(aDimIdx).isPeriodic &&
210         Abs ( myPoints(aPntIdx, aDimIdx) - myPoints(aPntIdx + 1, aDimIdx) ) > myPerInfo(aDimIdx).myPeriod / 2.01)
211       {
212         Standard_Real aPeriodMult = (myPoints(aPntIdx, aDimIdx) > myPoints(aPntIdx + 1, aDimIdx)) ? 1.0 : -1.0;
213         Standard_Real aNewParam = myPoints(aPntIdx + 1, aDimIdx) + aPeriodMult * myPerInfo(aDimIdx).myPeriod;
214         myPoints(aPntIdx + 1, aDimIdx) = aNewParam;
215       }
216     }
217   }
218
219   VBernstein(classe, myNbPoints, myVB);
220
221   // Traitement du second membre:
222   NCollection_Array1<Standard_Real> tmppoints(1, nbcol);
223
224   for (c = 1; c <= classe; c++) 
225   {
226     tmppoints.Init(0.0);
227     for (i = 1; i <= myNbPoints; i++)
228     {
229       Coeff = TheWeights(i) * myVB(c, i);
230       for (j = 1; j <= nbcol; j++)
231       {
232         tmppoints(j) += myPoints(i, j)*Coeff;
233       }
234     }
235     for (k = 1; k <= nbcol; k++)
236     {
237       B(c, k) += tmppoints(k);
238     }
239   }
240
241   if (myFirstC == AppParCurves_NoConstraint &&
242       myLastC  == AppParCurves_NoConstraint) {
243
244     math_Matrix InvM(1, classe, 1, classe);
245     InvMMatrix(classe, InvM);
246     // Calcul direct des poles:
247
248     for (i = 1; i <= classe; i++) {
249       for (j = 1; j <= classe; j++) {
250         IBij = InvM(i, j);
251         for (k = 1; k <= nbcol; k++) {
252           myPoles(i, k)   += IBij * B(j, k);
253         }
254       }
255     }
256   }
257
258
259   else
260   {
261     math_Matrix M(1, classe, 1, classe);
262     MMatrix(classe, M);
263     NCollection_Array1<gp_Pnt2d> aFixP2d(1, Max (myNbP2d, 1));
264     NCollection_Array1<gp_Pnt>   aFixP(1, Max (myNbP, 1));
265
266     if (myFirstC == AppParCurves_PassPoint ||
267         myFirstC == AppParCurves_TangencyPoint)
268     {
269       SSP.Value(U0, aTabP2d, aTabP);
270       FixSingleBorderPoint(SSP, U0, U0, U1, aFixP2d, aFixP);
271
272       i2 = 1;
273       for (k = 1; k<= myNbP; k++)
274       {
275         if (aFixP(k).Distance(aTabP(k)) > 0.1)
276           (aFixP(k)).Coord(myPoles(1, i2), myPoles(1, i2 + 1), myPoles(1, i2 + 2));
277         else
278           (aTabP(k)).Coord(myPoles(1, i2), myPoles(1, i2 + 1), myPoles(1, i2 + 2));
279         i2 += 3;
280       }
281       for (k = 1; k<= myNbP2d; k++)
282       {
283         if (aFixP2d(k).Distance(aTabP2d(k)) > 0.1)
284           (aFixP2d(k)).Coord(myPoles(1, i2), myPoles(1, i2 + 1));
285         else
286           (aTabP2d(k)).Coord(myPoles(1, i2), myPoles(1, i2 + 1));
287         i2 += 2;
288       }
289
290       for (Standard_Integer aDimIdx = 1; aDimIdx <= aMaxDim; aDimIdx++) 
291       {
292         if (myPerInfo(aDimIdx).isPeriodic && 
293             Abs ( myPoles(1, aDimIdx) - myPoints(1, aDimIdx) ) > myPerInfo(aDimIdx).myPeriod / 2.01 )
294         {
295           Standard_Real aMult = myPoles(1, aDimIdx) < myPoints(1, aDimIdx)? 1.0: -1.0;
296           myPoles(1,aDimIdx) += aMult * myPerInfo(aDimIdx).myPeriod;
297         }
298       }
299     }
300
301     if (myLastC == AppParCurves_PassPoint ||
302         myLastC == AppParCurves_TangencyPoint)
303     {
304       SSP.Value(U1, aTabP2d, aTabP);
305       FixSingleBorderPoint(SSP, U1, U0, U1, aFixP2d, aFixP);
306
307       i2 = 1;
308       for (k = 1; k<= myNbP; k++)
309       {
310         if (aFixP(k).Distance(aTabP(k)) > 0.1)
311           (aFixP(k)).Coord(myPoles(classe, i2), myPoles(classe, i2 + 1), myPoles(classe, i2 + 2));
312         else
313           (aTabP(k)).Coord(myPoles(classe, i2), myPoles(classe, i2 + 1), myPoles(classe, i2 + 2));
314         i2 += 3;
315       }
316       for (k = 1; k<= myNbP2d; k++)
317       {
318         if (aFixP2d(k).Distance(aTabP2d(k)) > 0.1)
319           (aFixP2d(k)).Coord(myPoles(classe, i2), myPoles(classe, i2 + 1));
320         else
321           (aTabP2d(k)).Coord(myPoles(classe, i2), myPoles(classe, i2 + 1));
322         i2 += 2;
323       }
324
325
326       for (Standard_Integer aDimIdx = 1; aDimIdx <= 2; aDimIdx++) 
327       {
328         if (myPerInfo(aDimIdx).isPeriodic && 
329           Abs ( myPoles(classe, aDimIdx) - myPoints(myNbPoints, aDimIdx) ) > myPerInfo(aDimIdx).myPeriod / 2.01 )
330         {
331           Standard_Real aMult = myPoles(classe, aDimIdx) < myPoints(myNbPoints, aDimIdx)? 1.0: -1.0;
332           myPoles(classe,aDimIdx) += aMult * myPerInfo(aDimIdx).myPeriod;
333         }
334       }
335     }
336
337     if (myFirstC == AppParCurves_PassPoint) {
338       bdeb = 2;
339       // mise a jour du second membre:
340       for (i = 1; i <= classe; i++) {
341         Coeff = M(i, 1);
342         for (k = 1; k <= nbcol; k++) {
343           B(i, k) -= myPoles(1, k)*Coeff;
344         }
345       }
346     }
347
348     if (myLastC == AppParCurves_PassPoint) {
349       bfin = cl1;
350       for (i = 1; i <= classe; i++) {
351         Coeff = M(i, classe);
352         for (k = 1; k <= nbcol; k++) {
353           B(i, k) -= myPoles(classe, k)*Coeff;
354         }
355       }
356     }
357
358     if (myFirstC == AppParCurves_TangencyPoint) {
359       // On fixe le second pole::
360       bdeb = 3;
361       SSP.D1(U0, aTabV2d, aTabV);
362
363       i2 = 1;
364       Coeff = (U1-U0)/myDegre;
365       for (k = 1; k<= myNbP; k++) {
366         i2plus1 = i2+1; i2plus2 = i2+2;
367         myPoles(2, i2)      = myPoles(1, i2)      + aTabV(k).X()*Coeff;
368         myPoles(2, i2plus1) = myPoles(1, i2plus1) + aTabV(k).Y()*Coeff;
369         myPoles(2, i2plus2) = myPoles(1, i2plus2) + aTabV(k).Z()*Coeff;
370         i2 += 3;
371       }
372       for (k = 1; k<= myNbP2d; k++) {
373         i2plus1 = i2+1;
374         myPoles(2, i2)      = myPoles(1, i2)      + aTabV2d(k).X()*Coeff;
375         myPoles(2, i2plus1) = myPoles(1, i2plus1) + aTabV2d(k).Y()*Coeff;
376         i2 += 2;
377       }
378
379       for (i = 1; i <= classe; i++) {
380         Coeff = M(i, 1); Coeff2 = M(i, 2);
381         for (k = 1; k <= nbcol; k++) {
382           B(i, k) -= myPoles(1, k)*Coeff+myPoles(2, k)*Coeff2;
383         }
384       }
385     }
386
387     if (myLastC == AppParCurves_TangencyPoint) {
388       bfin = classe-2;
389       SSP.D1(U1, aTabV2d, aTabV);
390       i2 = 1;
391       Coeff = (U1-U0)/myDegre;
392       for (k = 1; k<= myNbP; k++) {
393         i2plus1 = i2+1; i2plus2 = i2+2;
394         myPoles(cl1,i2)      = myPoles(classe, i2)      - aTabV(k).X()*Coeff;
395         myPoles(cl1,i2plus1) = myPoles(classe, i2plus1) - aTabV(k).Y()*Coeff;
396         myPoles(cl1,i2plus2) = myPoles(classe, i2plus2) - aTabV(k).Z()*Coeff;
397         i2 += 3;
398       }
399       for (k = 1; k<= myNbP2d; k++) {
400         i2plus1 = i2+1; 
401         myPoles(cl1,i2) = myPoles(classe, i2) - aTabV2d(k).X()*Coeff;
402         myPoles(cl1,i2plus1) = myPoles(classe, i2plus1) - aTabV2d(k).Y()*Coeff;
403         i2 += 2;
404       }
405
406       for (i = 1; i <= classe; i++) {
407         Coeff = M(i, classe); Coeff2 = M(i, cl1);
408         for (k = 1; k <= nbcol; k++) {
409           B(i, k) -= myPoles(classe, k)*Coeff + myPoles(cl1, k)*Coeff2;
410         }
411       }
412     }
413
414
415     if (bdeb <= bfin) {
416       math_Matrix B2(bdeb, bfin, 1, B.UpperCol(), 0.0);
417       
418       for (i = bdeb; i <= bfin; i++) {
419         for (j = 1; j <= classe; j++) {
420           Coeff = M(i, j);
421           for (k = 1; k <= nbcol; k++) {
422             B2(i, k) += B(j, k)*Coeff;
423           }
424         }
425       }
426
427       // Resolution:
428       // ===========
429       math_Matrix IBP(bdeb, bfin, bdeb, bfin);
430
431       // dans IBPMatrix at IBTMatrix ne sont stockees que les resultats pour
432       // une classe inferieure ou egale a 26 (pour l instant du moins.)
433
434       if (bdeb == 2 && bfin == classe-1 && classe <= 26) {
435         IBPMatrix(classe, IBP);
436       }
437       else if (bdeb == 3 && bfin == classe-2 && classe <= 26) {
438         IBTMatrix(classe, IBP);
439       }
440       else {
441         math_Matrix MP(1, classe, bdeb, bfin);
442         for (i = 1; i <= classe; i++) {
443           for (j = bdeb; j <= bfin; j++) {
444             MP(i, j) = M(i, j);
445           }
446         }
447         math_Matrix IBP1(bdeb, bfin, bdeb, bfin);
448         IBP1 = MP.Transposed()*MP;
449         IBP = IBP1.Inverse();
450       }
451
452       myDone = Standard_True;
453       for (i = bdeb; i <= bfin; i++) {
454         for (j = bdeb; j <= bfin; j++) {
455           IBPij = IBP(i, j);
456           for (k = 1; k<= nbcol; k++) {
457             myPoles(i, k)   += IBPij * B2(j, k);
458           }
459         }
460       }
461     }
462   }
463 }
464
465 //=======================================================================
466 //function : Value
467 //purpose  : 
468 //=======================================================================
469
470 const AppParCurves_MultiCurve& AppCont_LeastSquare::Value() 
471 {
472
473   Standard_Integer i, j, j2;
474   gp_Pnt Pt;
475   gp_Pnt2d Pt2d;
476   Standard_Integer ideb = 1, ifin = myDegre+1;
477
478   // On met le resultat dans les curves correspondantes
479   for (i = ideb; i <= ifin; i++) {
480     j2 = 1;
481     AppParCurves_MultiPoint MPole(myNbP, myNbP2d);
482     for (j = 1; j <= myNbP; j++) {
483       Pt.SetCoord(myPoles(i, j2), myPoles(i, j2+1), myPoles(i,j2+2));
484       MPole.SetPoint(j, Pt);
485       j2 += 3;
486     }
487     for (j = myNbP+1;j <= myNbP+myNbP2d; j++) {
488       Pt2d.SetCoord(myPoles(i, j2), myPoles(i, j2+1));
489       MPole.SetPoint2d(j, Pt2d);
490       j2 += 2;
491     }
492     mySCU.SetValue(i, MPole);
493   }
494   return mySCU;
495 }
496
497
498
499 //=======================================================================
500 //function : Error
501 //purpose  : 
502 //=======================================================================
503
504 void AppCont_LeastSquare::Error(Standard_Real& F, 
505                                 Standard_Real& MaxE3d,
506                                 Standard_Real& MaxE2d) const
507 {
508   Standard_Integer i, j, k, c, i2, classe = myDegre + 1;
509   Standard_Real Coeff, err3d = 0.0, err2d = 0.0;
510   Standard_Integer ncol = myPoints.UpperCol() - myPoints.LowerCol() + 1;
511
512   math_Matrix MyPoints(1, myNbdiscret, 1, ncol);
513   MyPoints = myPoints;
514
515   MaxE3d = MaxE2d = F = 0.0;
516
517   NCollection_Array1<Standard_Real> tmppoles(1, ncol);
518
519   for (c = 1; c <= classe; c++)
520   {
521     for (k = 1; k <= ncol; k++)
522     {
523       tmppoles(k) = myPoles(c, k);
524     }
525     for (i = 1; i <= myNbdiscret; i++)
526     {
527       Coeff = myVB(c, i);
528       for (j = 1; j <= ncol; j++)
529       {
530         MyPoints(i, j) -= tmppoles(j) * Coeff;
531       }
532     }
533   }
534
535   Standard_Real e1, e2, e3;
536   for (i = 1; i <= myNbdiscret; i++)
537   {
538     i2 = 1;
539     for (j = 1; j<= myNbP; j++) {
540       e1 = MyPoints(i, i2);
541       e2 = MyPoints(i, i2+1);
542       e3 = MyPoints(i, i2+2);
543       err3d = e1*e1+e2*e2+e3*e3;
544       MaxE3d = Max(MaxE3d, err3d);
545       F += err3d;
546       i2 += 3;
547     }
548     for (j = 1; j<= myNbP2d; j++) {
549       e1 = MyPoints(i, i2);
550       e2 = MyPoints(i, i2+1);
551       err2d = e1*e1+e2*e2;
552       MaxE2d = Max(MaxE2d, err2d);
553       F += err2d;
554       i2 += 2;
555     }
556   }
557
558   MaxE3d = Sqrt(MaxE3d);
559   MaxE2d = Sqrt(MaxE2d);
560
561 }
562
563
564 //=======================================================================
565 //function : IsDone
566 //purpose  : 
567 //=======================================================================
568
569 Standard_Boolean AppCont_LeastSquare::IsDone() const
570 {
571   return myDone;
572 }