projects / occt.git / blame
? search:
summary | shortlog | log | commit | commitdiff | tree
blob | blame (incremental) | history | HEAD
0026937: Eliminate NO_CXX_EXCEPTION macro support
[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>
ad121848 27#include <Precision.hxx>
368cdde6 28
29
30//=======================================================================
31//function : AppCont_LeastSquare
32//purpose :
33//=======================================================================
34void 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{
ad121848 41 Standard_Integer aMaxIter = 15;
368cdde6 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;
ad121848 46 Standard_Real aStartParam = theU,
368cdde6 47 aCurrParam, aPrevDist = 1.0, aCurrDist = 1.0;
48
ad121848 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++)
368cdde6 53 {
ad121848 54 dd *= dec;
55 aCurrParam = aStartParam + dd;
368cdde6 56 theSSP.Value(aCurrParam, aTabP2d, aTabP);
57
58 // from second iteration
ad121848 59 if (anIter > 1)
368cdde6 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
ad121848 76 if (anIter > 2 && aCurrDist / aPrevDist > 10.0)
368cdde6 77 break;
78 }
79 aPrevP = aTabP;
80 aPrevP2d = aTabP2d;
81 aPrevDist = aCurrDist;
ad121848 82 if(aPrevDist <= eps)
83 break;
368cdde6 84 }
85 theFix2d = aPrevP2d;
86 theFix = aPrevP;
87}
88
89
90//=======================================================================
91//function : AppCont_LeastSquare
92//purpose :
93//=======================================================================
94
95AppCont_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;
368cdde6 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
470const 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
504void 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
569Standard_Boolean AppCont_LeastSquare::IsDone() const
570{
571 return myDone;
572}
Open CASCADE Technology repository