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
6 // This file is part of Open CASCADE Technology software library.
8 // This library is free software; you can redistribute it and / or modify it
9 // under the terms of the GNU Lesser General Public 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.
14 // Alternatively, this file may be used under the terms of Open CASCADE
15 // commercial license or contractual agreement.
18 #define No_Standard_OutOfRange
19 #define No_Standard_RangeError
25 #include <math_Vector.hxx>
26 #include <math_Matrix.hxx>
27 #include <TColgp_Array1OfPnt.hxx>
28 #include <TColgp_Array1OfPnt2d.hxx>
29 #include <gp_Pnt2d.hxx>
32 #include <gp_Vec2d.hxx>
33 #include <TColgp_Array1OfVec.hxx>
34 #include <TColgp_Array1OfVec2d.hxx>
35 #include <AppParCurves_MultiPoint.hxx>
36 #include <AppCont_ContMatrices.hxx>
42 //=======================================================================
43 //function : AppCont_LeastSquare
45 //=======================================================================
47 AppCont_LeastSquare::AppCont_LeastSquare
48 (const MultiLine& SSP,
49 const Standard_Real U0,
50 const Standard_Real U1,
51 const AppParCurves_Constraint FirstCons,
52 const AppParCurves_Constraint LastCons,
53 const Standard_Integer Deg,
54 const Standard_Integer NbPoints):
56 Points(1, NbPoints, 1, NbBColumns(SSP)),
57 Poles(1, Deg+1, 1, NbBColumns(SSP), 0.0),
59 VB(1, Deg+1, 1, NbPoints)
62 Done = Standard_False;
64 math_Matrix InvM(1, Deg+1, 1, Deg+1);
65 Standard_Integer i, j, k, c, i2;
66 Standard_Integer classe = Deg+1, cl1 = Deg;
67 Standard_Real U, dU, Coeff, Coeff2;
68 Standard_Real IBij, IBPij;
70 Standard_Integer FirstP = 1, LastP = NbPoints;
71 Standard_Integer nbcol = NbBColumns(SSP);
72 math_Matrix B(1, classe, 1, nbcol, 0.0);
73 Standard_Integer bdeb = 1, bfin = classe;
74 AppParCurves_Constraint myFirstC = FirstCons, myLastC = LastCons;
75 nbP = LineTool::NbP3d(SSP);
76 nbP2d = LineTool::NbP2d(SSP);
77 Standard_Integer mynbP = nbP, mynbP2d = nbP2d;
78 if (nbP == 0) mynbP = 1;
79 if (nbP2d == 0) mynbP2d = 1;
81 Standard_Integer i2plus1, i2plus2;
83 TColgp_Array1OfPnt TabP(1, mynbP);
84 TColgp_Array1OfVec TabV(1, mynbP);
85 TColgp_Array1OfPnt2d TabP2d(1, mynbP2d);
86 TColgp_Array1OfVec2d TabV2d(1, mynbP2d);
89 if (myFirstC == AppParCurves_TangencyPoint) {
90 if (nbP != 0 && nbP2d != 0) Ok=LineTool::D1(SSP, U0, TabV, TabV2d);
91 else if (nbP != 0) Ok=LineTool::D1(SSP, U0, TabV);
92 else Ok=LineTool::D1(SSP, U0, TabV2d);
93 if (!Ok) myFirstC = AppParCurves_PassPoint;
96 if (myLastC == AppParCurves_TangencyPoint) {
97 if (nbP != 0 && nbP2d != 0) Ok=LineTool::D1(SSP, U1, TabV, TabV2d);
98 else if (nbP != 0) Ok=LineTool::D1(SSP, U1, TabV);
99 else Ok=LineTool::D1(SSP, U1, TabV2d);
100 if (!Ok) myLastC = AppParCurves_PassPoint;
103 math_Vector GaussP(1, NbPoints), GaussW(1, NbPoints);
104 math::GaussPoints(NbPoints, GaussP);
105 math::GaussWeights(NbPoints, GaussW);
107 math_Vector TheWeights(1, NbPoints), VBParam(1, NbPoints);
111 // calcul et mise en ordre des parametres et des poids:
112 for (i = FirstP; i <= LastP; i++) {
113 U = 0.5*(U1+U0) + dU*GaussP(i);
114 if (i <= (NbPoints+1)/2) {
115 myParam(LastP-i+1) = U;
116 VBParam(LastP-i+1) = 0.5*(1 + GaussP(i));
117 TheWeights(LastP-i+1) = 0.5*GaussW(i);
120 VBParam(i-(NbPoints+1)/2) = 0.5*(1 + GaussP(i));
121 myParam(i-(NbPoints+1)/2) = U;
122 TheWeights(i-(NbPoints+1)/2) = 0.5*GaussW(i);
127 for (i = FirstP; i <= LastP; i++) {
129 if (nbP != 0 && nbP2d != 0) LineTool::Value(SSP, U, TabP, TabP2d);
130 else if (nbP != 0) LineTool::Value(SSP, U, TabP);
131 else LineTool::Value(SSP, U, TabP2d);
134 for (j = 1; j <= nbP; j++) {
135 (TabP(j)).Coord(Points(i, i2), Points(i, i2+1), Points(i, i2+2));
138 for (j = 1; j <= nbP2d; j++) {
139 (TabP2d(j)).Coord(Points(i, i2), Points(i, i2+1));
144 // Calcul du VB ( Valeur des fonctions de Bernstein):
146 // for (i = 1; i <= classe; i++) {
147 // for (j = 1; j <= NbPoints; j++) {
148 // VB(i,j)=PLib::Binomial(cl1,i-1)*Pow((1-VBParam(j)),classe-i)*
149 // Pow(VBParam(j),i-1);
153 VBernstein(classe, NbPoints, VB);
155 // Traitement du second membre:
157 Standard_Real *tmppoints, *tmpbis;
158 tmppoints = new Standard_Real[nbcol];
160 for (c = 1; c <= classe; c++) {
162 for (k = 1; k <= nbcol; k++, tmpbis++) {
165 for (i = 1; i <= NbPoints; i++) {
166 Coeff = TheWeights(i)*VB(c, i);
168 for (j = 1; j <= nbcol; j++, tmpbis++) {
169 *tmpbis += Points(i, j)*Coeff;
170 //B(c, j) += Points(i, j)*Coeff;
174 for (k = 1; k <= nbcol; k++, tmpbis++) {
181 if (myFirstC == AppParCurves_NoConstraint &&
182 myLastC == AppParCurves_NoConstraint) {
184 math_Matrix InvM(1, classe, 1, classe);
185 InvMMatrix(classe, InvM);
186 // Calcul direct des poles:
188 for (i = 1; i <= classe; i++) {
189 for (j = 1; j <= classe; j++) {
191 for (k = 1; k <= nbcol; k++) {
192 Poles(i, k) += IBij * B(j, k);
200 math_Matrix M(1, classe, 1, classe);
203 if (myFirstC == AppParCurves_PassPoint ||
204 myFirstC == AppParCurves_TangencyPoint) {
206 if (nbP != 0 && nbP2d != 0) LineTool::Value(SSP, U0, TabP, TabP2d);
207 else if (nbP != 0) LineTool::Value(SSP, U0, TabP);
208 else LineTool::Value(SSP, U0, TabP2d);
210 for (k = 1; k<= nbP; k++) {
211 (TabP(k)).Coord(Poles(1, i2), Poles(1, i2+1), Poles(1, i2+2));
214 for (k = 1; k<= nbP2d; k++) {
215 (TabP2d(k)).Coord(Poles(1, i2), Poles(1, i2+1));
221 if (myLastC == AppParCurves_PassPoint ||
222 myLastC == AppParCurves_TangencyPoint) {
225 if (nbP != 0 && nbP2d != 0) LineTool::Value(SSP, U1, TabP, TabP2d);
226 else if (nbP != 0) LineTool::Value(SSP, U1, TabP);
227 else LineTool::Value(SSP, U1, TabP2d);
228 for (k = 1; k<= nbP; k++) {
229 (TabP(k)).Coord(Poles(classe,i2),
234 for (k = 1; k<= nbP2d; k++) {
235 (TabP2d(k)).Coord(Poles(classe, i2), Poles(classe, i2+1));
242 if (myFirstC == AppParCurves_PassPoint) {
244 // mise a jour du second membre:
245 for (i = 1; i <= classe; i++) {
247 for (k = 1; k <= nbcol; k++) {
248 B(i, k) -= Poles(1, k)*Coeff;
254 if (myLastC == AppParCurves_PassPoint) {
256 for (i = 1; i <= classe; i++) {
257 Coeff = M(i, classe);
258 for (k = 1; k <= nbcol; k++) {
259 B(i, k) -= Poles(classe, k)*Coeff;
265 if (myFirstC == AppParCurves_TangencyPoint) {
266 // On fixe le second pole::
268 if (nbP != 0 && nbP2d != 0) LineTool::D1(SSP, U0, TabV, TabV2d);
269 else if (nbP != 0) LineTool::D1(SSP, U0, TabV);
270 else LineTool::D1(SSP, U0, TabV2d);
272 Coeff = (U1-U0)/Degre;
273 for (k = 1; k<= nbP; k++) {
274 i2plus1 = i2+1; i2plus2 = i2+2;
275 Poles(2, i2) = Poles(1, i2) + TabV(k).X()*Coeff;
276 Poles(2, i2plus1) = Poles(1, i2plus1) + TabV(k).Y()*Coeff;
277 Poles(2, i2plus2) = Poles(1, i2plus2) + TabV(k).Z()*Coeff;
280 for (k = 1; k<= nbP2d; k++) {
282 Poles(2, i2) = Poles(1, i2) + TabV2d(k).X()*Coeff;
283 Poles(2, i2plus1) = Poles(1, i2plus1) + TabV2d(k).Y()*Coeff;
288 for (i = 1; i <= classe; i++) {
289 Coeff = M(i, 1); Coeff2 = M(i, 2);
290 for (k = 1; k <= nbcol; k++) {
291 B(i, k) -= Poles(1, k)*Coeff+Poles(2, k)*Coeff2;
296 if (myLastC == AppParCurves_TangencyPoint) {
299 if (nbP != 0 && nbP2d != 0) LineTool::D1(SSP, U1, TabV, TabV2d);
300 else if (nbP != 0) LineTool::D1(SSP, U1, TabV);
301 else LineTool::D1(SSP, U1, TabV2d);
303 Coeff = (U1-U0)/Degre;
304 for (k = 1; k<= nbP; k++) {
305 i2plus1 = i2+1; i2plus2 = i2+2;
306 Poles(cl1,i2) = Poles(classe, i2) - TabV(k).X()*Coeff;
307 Poles(cl1,i2plus1) = Poles(classe, i2plus1) - TabV(k).Y()*Coeff;
308 Poles(cl1,i2plus2) = Poles(classe, i2plus2) - TabV(k).Z()*Coeff;
311 for (k = 1; k<= nbP2d; k++) {
313 Poles(cl1,i2) = Poles(classe, i2) - TabV2d(k).X()*Coeff;
314 Poles(cl1,i2plus1) = Poles(classe, i2plus1) - TabV2d(k).Y()*Coeff;
318 for (i = 1; i <= classe; i++) {
319 Coeff = M(i, classe); Coeff2 = M(i, cl1);
320 for (k = 1; k <= nbcol; k++) {
321 B(i, k) -= Poles(classe, k)*Coeff + Poles(cl1, k)*Coeff2;
327 math_Matrix B2(bdeb, bfin, 1, B.UpperCol(), 0.0);
329 for (i = bdeb; i <= bfin; i++) {
330 for (j = 1; j <= classe; j++) {
332 for (k = 1; k <= nbcol; k++) {
333 B2(i, k) += B(j, k)*Coeff;
341 math_Matrix IBP(bdeb, bfin, bdeb, bfin);
343 // dans IBPMatrix at IBTMatrix ne sont stockees que les resultats pour
344 // une classe inferieure ou egale a 26 (pour l instant du moins.)
346 if (bdeb == 2 && bfin == classe-1 && classe <= 26) {
347 IBPMatrix(classe, IBP);
349 else if (bdeb == 3 && bfin == classe-2 && classe <= 26) {
350 IBTMatrix(classe, IBP);
353 math_Matrix MP(1, classe, bdeb, bfin);
355 for (i = 1; i <= classe; i++) {
356 for (j = bdeb; j <= bfin; j++) {
360 math_Matrix IBP1(bdeb, bfin, bdeb, bfin);
361 IBP1 = MP.Transposed()*MP;
362 IBP = IBP1.Inverse();
366 Done = Standard_True;
367 for (i = bdeb; i <= bfin; i++) {
368 for (j = bdeb; j <= bfin; j++) {
370 for (k = 1; k<= nbcol; k++) {
371 Poles(i, k) += IBPij * B2(j, k);
382 //=======================================================================
383 //function : NbBColumns
385 //=======================================================================
387 Standard_Integer AppCont_LeastSquare::NbBColumns(
388 const MultiLine& SSP) const
390 Standard_Integer BCol;
391 BCol = (LineTool::NbP3d(SSP))*3 +
392 (LineTool::NbP2d(SSP))*2;
397 //=======================================================================
400 //=======================================================================
402 const AppParCurves_MultiCurve& AppCont_LeastSquare::Value()
405 Standard_Integer i, j, j2;
408 Standard_Integer ideb = 1, ifin = Degre+1;
410 // On met le resultat dans les curves correspondantes
411 for (i = ideb; i <= ifin; i++) {
413 AppParCurves_MultiPoint MPole(nbP, nbP2d);
414 for (j = 1; j <= nbP; j++) {
415 Pt.SetCoord(Poles(i, j2), Poles(i, j2+1), Poles(i,j2+2));
416 MPole.SetPoint(j, Pt);
419 for (j = nbP+1;j <= nbP+nbP2d; j++) {
420 Pt2d.SetCoord(Poles(i, j2), Poles(i, j2+1));
421 MPole.SetPoint2d(j, Pt2d);
424 SCU.SetValue(i, MPole);
431 //=======================================================================
434 //=======================================================================
436 void AppCont_LeastSquare::Error(Standard_Real& F,
437 Standard_Real& MaxE3d,
438 Standard_Real& MaxE2d) const
440 Standard_Integer i, j, k, c, i2, classe = Degre+1;
441 // Standard_Real Coeff, val = 0.0, err3d = 0.0, err2d =0.0;
442 Standard_Real Coeff, err3d = 0.0, err2d =0.0;
443 Standard_Integer ncol = Points.UpperCol()-Points.LowerCol()+1;
445 math_Matrix MyPoints(1, Nbdiscret, 1, ncol);
448 MaxE3d = MaxE2d = F = 0.0;
450 Standard_Real *tmppoles, *tmpbis;
451 tmppoles = new Standard_Real[ncol];
453 for (c = 1; c <= classe; c++) {
455 for (k = 1; k <= ncol; k++, tmpbis++) {
456 *tmpbis = Poles(c, k);
458 for (i = 1; i <= Nbdiscret; i++) {
461 for (j = 1; j <= ncol; j++, tmpbis++) {
462 MyPoints(i, j) -= (*tmpbis)*Coeff; // Poles(c, j)*Coeff;
468 Standard_Real e1, e2, e3;
469 for (i = 1; i <= Nbdiscret; i++) {
471 for (j = 1; j<= nbP; j++) {
472 e1 = MyPoints(i, i2);
473 e2 = MyPoints(i, i2+1);
474 e3 = MyPoints(i, i2+2);
475 err3d = e1*e1+e2*e2+e3*e3;
476 MaxE3d = Max(MaxE3d, err3d);
480 for (j = 1; j<= nbP2d; j++) {
481 e1 = MyPoints(i, i2);
482 e2 = MyPoints(i, i2+1);
484 MaxE2d = Max(MaxE2d, err2d);
490 MaxE3d = Sqrt(MaxE3d);
491 MaxE2d = Sqrt(MaxE2d);
496 //=======================================================================
499 //=======================================================================
501 Standard_Boolean AppCont_LeastSquare::IsDone() const