projects / occt.git / blame_incremental
? search:
summary | shortlog | log | commit | commitdiff | tree
blob | blame (non-incremental) | history | HEAD
0024624: Lost word in license statement in source files
[occt.git] / src / AppCont / AppCont_LeastSquare.gxx
... / ...
CommitLineData
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 DEB
18#define No_Standard_OutOfRange
19#define No_Standard_RangeError
20#endif
21
22
23
24#include <math.hxx>
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>
30#include <gp_Pnt.hxx>
31#include <gp_Vec.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>
37#include <PLib.hxx>
38
39
40
41
42//=======================================================================
43//function : AppCont_LeastSquare
44//purpose :
45//=======================================================================
46
47AppCont_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):
55 SCU(Deg+1),
56 Points(1, NbPoints, 1, NbBColumns(SSP)),
57 Poles(1, Deg+1, 1, NbBColumns(SSP), 0.0),
58 myParam(1, NbPoints),
59 VB(1, Deg+1, 1, NbPoints)
60
61{
62 Done = Standard_False;
63 Degre = Deg;
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;
69
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;
80
81 Standard_Integer i2plus1, i2plus2;
82 Nbdiscret = NbPoints;
83 TColgp_Array1OfPnt TabP(1, mynbP);
84 TColgp_Array1OfVec TabV(1, mynbP);
85 TColgp_Array1OfPnt2d TabP2d(1, mynbP2d);
86 TColgp_Array1OfVec2d TabV2d(1, mynbP2d);
87
88 Standard_Boolean Ok;
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;
94 }
95
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;
101 }
102
103 math_Vector GaussP(1, NbPoints), GaussW(1, NbPoints);
104 math::GaussPoints(NbPoints, GaussP);
105 math::GaussWeights(NbPoints, GaussW);
106
107 math_Vector TheWeights(1, NbPoints), VBParam(1, NbPoints);
108
109 dU = 0.5*(U1-U0);
110
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);
118 }
119 else {
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);
123 }
124 }
125
126
127 for (i = FirstP; i <= LastP; i++) {
128 U = myParam(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);
132
133 i2 = 1;
134 for (j = 1; j <= nbP; j++) {
135 (TabP(j)).Coord(Points(i, i2), Points(i, i2+1), Points(i, i2+2));
136 i2 += 3;
137 }
138 for (j = 1; j <= nbP2d; j++) {
139 (TabP2d(j)).Coord(Points(i, i2), Points(i, i2+1));
140 i2 += 2;
141 }
142 }
143
144 // Calcul du VB ( Valeur des fonctions de Bernstein):
145
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);
150// }
151// }
152
153 VBernstein(classe, NbPoints, VB);
154
155 // Traitement du second membre:
156
157 Standard_Real *tmppoints, *tmpbis;
158 tmppoints = new Standard_Real[nbcol];
159
160 for (c = 1; c <= classe; c++) {
161 tmpbis = tmppoints;
162 for (k = 1; k <= nbcol; k++, tmpbis++) {
163 *tmpbis = 0.0;
164 }
165 for (i = 1; i <= NbPoints; i++) {
166 Coeff = TheWeights(i)*VB(c, i);
167 tmpbis = tmppoints;
168 for (j = 1; j <= nbcol; j++, tmpbis++) {
169 *tmpbis += Points(i, j)*Coeff;
170 //B(c, j) += Points(i, j)*Coeff;
171 }
172 }
173 tmpbis = tmppoints;
174 for (k = 1; k <= nbcol; k++, tmpbis++) {
175 B(c, k) += *tmpbis;
176 }
177 }
178
179 delete [] tmppoints;
180
181 if (myFirstC == AppParCurves_NoConstraint &&
182 myLastC == AppParCurves_NoConstraint) {
183
184 math_Matrix InvM(1, classe, 1, classe);
185 InvMMatrix(classe, InvM);
186 // Calcul direct des poles:
187
188 for (i = 1; i <= classe; i++) {
189 for (j = 1; j <= classe; j++) {
190 IBij = InvM(i, j);
191 for (k = 1; k <= nbcol; k++) {
192 Poles(i, k) += IBij * B(j, k);
193 }
194 }
195 }
196 }
197
198
199 else {
200 math_Matrix M(1, classe, 1, classe);
201 MMatrix(classe, M);
202
203 if (myFirstC == AppParCurves_PassPoint ||
204 myFirstC == AppParCurves_TangencyPoint) {
205
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);
209 i2 =1;
210 for (k = 1; k<= nbP; k++) {
211 (TabP(k)).Coord(Poles(1, i2), Poles(1, i2+1), Poles(1, i2+2));
212 i2 += 3;
213 }
214 for (k = 1; k<= nbP2d; k++) {
215 (TabP2d(k)).Coord(Poles(1, i2), Poles(1, i2+1));
216 i2 += 2;
217 }
218
219 }
220
221 if (myLastC == AppParCurves_PassPoint ||
222 myLastC == AppParCurves_TangencyPoint) {
223
224 i2 = 1;
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),
230 Poles(classe,i2+1),
231 Poles(classe,i2+2));
232 i2 += 3;
233 }
234 for (k = 1; k<= nbP2d; k++) {
235 (TabP2d(k)).Coord(Poles(classe, i2), Poles(classe, i2+1));
236 i2 += 2;
237 }
238 }
239
240
241
242 if (myFirstC == AppParCurves_PassPoint) {
243 bdeb = 2;
244 // mise a jour du second membre:
245 for (i = 1; i <= classe; i++) {
246 Coeff = M(i, 1);
247 for (k = 1; k <= nbcol; k++) {
248 B(i, k) -= Poles(1, k)*Coeff;
249 }
250 }
251 }
252
253
254 if (myLastC == AppParCurves_PassPoint) {
255 bfin = cl1;
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;
260 }
261 }
262 }
263
264
265 if (myFirstC == AppParCurves_TangencyPoint) {
266 // On fixe le second pole::
267 bdeb = 3;
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);
271 i2 = 1;
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;
278 i2 += 3;
279 }
280 for (k = 1; k<= nbP2d; k++) {
281 i2plus1 = i2+1;
282 Poles(2, i2) = Poles(1, i2) + TabV2d(k).X()*Coeff;
283 Poles(2, i2plus1) = Poles(1, i2plus1) + TabV2d(k).Y()*Coeff;
284 i2 += 2;
285 }
286
287
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;
292 }
293 }
294 }
295
296 if (myLastC == AppParCurves_TangencyPoint) {
297 bfin = classe-2;
298
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);
302 i2 = 1;
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;
309 i2 += 3;
310 }
311 for (k = 1; k<= nbP2d; k++) {
312 i2plus1 = i2+1;
313 Poles(cl1,i2) = Poles(classe, i2) - TabV2d(k).X()*Coeff;
314 Poles(cl1,i2plus1) = Poles(classe, i2plus1) - TabV2d(k).Y()*Coeff;
315 i2 += 2;
316 }
317
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;
322 }
323 }
324 }
325
326 if (bdeb <= bfin) {
327 math_Matrix B2(bdeb, bfin, 1, B.UpperCol(), 0.0);
328
329 for (i = bdeb; i <= bfin; i++) {
330 for (j = 1; j <= classe; j++) {
331 Coeff = M(i, j);
332 for (k = 1; k <= nbcol; k++) {
333 B2(i, k) += B(j, k)*Coeff;
334 }
335 }
336 }
337
338
339 // Resolution:
340 // ===========
341 math_Matrix IBP(bdeb, bfin, bdeb, bfin);
342
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.)
345
346 if (bdeb == 2 && bfin == classe-1 && classe <= 26) {
347 IBPMatrix(classe, IBP);
348 }
349 else if (bdeb == 3 && bfin == classe-2 && classe <= 26) {
350 IBTMatrix(classe, IBP);
351 }
352 else {
353 math_Matrix MP(1, classe, bdeb, bfin);
354
355 for (i = 1; i <= classe; i++) {
356 for (j = bdeb; j <= bfin; j++) {
357 MP(i, j) = M(i, j);
358 }
359 }
360 math_Matrix IBP1(bdeb, bfin, bdeb, bfin);
361 IBP1 = MP.Transposed()*MP;
362 IBP = IBP1.Inverse();
363 }
364
365
366 Done = Standard_True;
367 for (i = bdeb; i <= bfin; i++) {
368 for (j = bdeb; j <= bfin; j++) {
369 IBPij = IBP(i, j);;
370 for (k = 1; k<= nbcol; k++) {
371 Poles(i, k) += IBPij * B2(j, k);
372 }
373 }
374 }
375 }
376 }
377}
378
379
380
381
382//=======================================================================
383//function : NbBColumns
384//purpose :
385//=======================================================================
386
387Standard_Integer AppCont_LeastSquare::NbBColumns(
388 const MultiLine& SSP) const
389{
390 Standard_Integer BCol;
391 BCol = (LineTool::NbP3d(SSP))*3 +
392 (LineTool::NbP2d(SSP))*2;
393 return BCol;
394}
395
396
397//=======================================================================
398//function : Value
399//purpose :
400//=======================================================================
401
402const AppParCurves_MultiCurve& AppCont_LeastSquare::Value()
403{
404
405 Standard_Integer i, j, j2;
406 gp_Pnt Pt;
407 gp_Pnt2d Pt2d;
408 Standard_Integer ideb = 1, ifin = Degre+1;
409
410 // On met le resultat dans les curves correspondantes
411 for (i = ideb; i <= ifin; i++) {
412 j2 = 1;
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);
417 j2 += 3;
418 }
419 for (j = nbP+1;j <= nbP+nbP2d; j++) {
420 Pt2d.SetCoord(Poles(i, j2), Poles(i, j2+1));
421 MPole.SetPoint2d(j, Pt2d);
422 j2 += 2;
423 }
424 SCU.SetValue(i, MPole);
425 }
426 return SCU;
427}
428
429
430
431//=======================================================================
432//function : Error
433//purpose :
434//=======================================================================
435
436void AppCont_LeastSquare::Error(Standard_Real& F,
437 Standard_Real& MaxE3d,
438 Standard_Real& MaxE2d) const
439{
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;
444
445 math_Matrix MyPoints(1, Nbdiscret, 1, ncol);
446 MyPoints = Points;
447
448 MaxE3d = MaxE2d = F = 0.0;
449
450 Standard_Real *tmppoles, *tmpbis;
451 tmppoles = new Standard_Real[ncol];
452
453 for (c = 1; c <= classe; c++) {
454 tmpbis = tmppoles;
455 for (k = 1; k <= ncol; k++, tmpbis++) {
456 *tmpbis = Poles(c, k);
457 }
458 for (i = 1; i <= Nbdiscret; i++) {
459 Coeff = VB(c, i);
460 tmpbis = tmppoles;
461 for (j = 1; j <= ncol; j++, tmpbis++) {
462 MyPoints(i, j) -= (*tmpbis)*Coeff; // Poles(c, j)*Coeff;
463 }
464 }
465 }
466 delete [] tmppoles;
467
468 Standard_Real e1, e2, e3;
469 for (i = 1; i <= Nbdiscret; i++) {
470 i2 = 1;
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);
477 F += err3d;
478 i2 += 3;
479 }
480 for (j = 1; j<= nbP2d; j++) {
481 e1 = MyPoints(i, i2);
482 e2 = MyPoints(i, i2+1);
483 err2d = e1*e1+e2*e2;
484 MaxE2d = Max(MaxE2d, err2d);
485 F += err2d;
486 i2 += 2;
487 }
488 }
489
490 MaxE3d = Sqrt(MaxE3d);
491 MaxE2d = Sqrt(MaxE2d);
492
493}
494
495
496//=======================================================================
497//function : IsDone
498//purpose :
499//=======================================================================
500
501Standard_Boolean AppCont_LeastSquare::IsDone() const
502{
503 return Done;
504}
Open CASCADE Technology repository