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