0024428: Implementation of LGPL license
[occt.git] / src / AppCont / AppCont_SurfLeastSquare.gxx
1 // Copyright (c) 1995-1999 Matra Datavision
2 // Copyright (c) 1999-2014 OPEN CASCADE SAS
3 //
4 // This file is part of Open CASCADE Technology software library.
5 //
6 // This library is free software; you can redistribute it and / or modify it
7 // under the terms of the GNU Lesser General Public version 2.1 as published
8 // by the Free Software Foundation, with special exception defined in the file
9 // OCCT_LGPL_EXCEPTION.txt. Consult the file LICENSE_LGPL_21.txt included in OCCT
10 // distribution for complete text of the license and disclaimer of any warranty.
11 //
12 // Alternatively, this file may be used under the terms of Open CASCADE
13 // commercial license or contractual agreement.
14
15 // Lpa, le 19/05/93
16
17
18 #ifndef DEB
19 #define No_Standard_OutOfRange
20 #define No_Standard_RangeError
21 #endif
22
23 #include <math_GaussPoints.hxx>
24 #include <math_Vector.hxx>
25 #include <math_Matrix.hxx>
26 #include <gp_Pnt.hxx>
27 #include <gp_Vec.hxx>
28 #include <AppParCurves_MultiPoint.hxx>
29 #include <AppCont_ContMatrices.hxx>
30 #include <Standard_ConstructionError.hxx>
31 #include <Standard_NotImplemented.hxx>
32 #include <StdFail_NotDone.hxx>
33 #include <PLib.hxx>
34
35
36
37
38 static void InvMSurfMatrix(const Standard_Integer classeU, 
39                            const Standard_Integer classeV,
40                            math_Matrix&           InvM)
41 {
42   math_Matrix Inv1(1, classeU);
43   InvMMatrix(classeU, Inv1);
44   math_Matrix Inv2(1, classeV);
45   InvMMatrix(classeV, Inv2);
46
47   // math_Matrix InvM(1, classeU*classeV, 1, classeU*classeV);
48   Standard_Integer i, j, k, l;
49   
50   for (i = 1; i <= classeU; i++) {
51     for (j= 1; j <= classeU; j++) {
52       for (k =1; k<= classeV; k++) {
53         for (l = 1; l<= classeV; l++) {
54           InvM(k+(i-1)*classeV,l+(j-1)*classeV) = Inv1(i,j)*Inv2(k,l);
55         }
56       }
57     }
58   }
59 }
60
61
62 static void MSurfMatrix(const Standard_Integer classeU, 
63                         const Standard_Integer classeV,
64                         math_Matrix&           M)
65 {
66   math_Matrix M1(1, classeU, 1, classeU);
67   MMatrix(classeU, M1);
68   math_Matrix M2(1, classeV, 1, classeV);
69   MMatrix(classeV, M2);
70
71   // math_Matrix M(1, classeU*classeV, 1, classeU*classeV);
72   Standard_Integer i, j, k, l;
73   
74   for (i = 1; i <= classeU; i++) {
75     for (j= 1; j <= classeU; j++) {
76       for (k =1; k<= classeV; k++) {
77         for (l = 1; l<= classeV; l++) {
78           M(k+(i-1)*classeV,l+(j-1)*classeV) = M1(i,j)*M2(k,l);
79         }
80       }
81     }
82   }
83 }
84
85
86
87
88
89
90 AppCont_SurfLeastSquare::
91   AppCont_SurfLeastSquare(const Surface& Surf,
92                           const Standard_Real U0,
93                           const Standard_Real U1,
94                           const Standard_Real V0,
95                           const Standard_Real V1,
96                           const AppParCurves_Constraint FirstCons,
97                           const AppParCurves_Constraint LastUCons,
98                           const AppParCurves_Constraint LastVCons,
99                           const AppParCurves_Constraint LastCons,
100                           const Standard_Integer DegU,
101                           const Standard_Integer DegV,
102                           const Standard_Integer NbPoints):
103                           PointsX(1, NbPoints, 1 , NbPoints),
104                           PointsY(1, NbPoints, 1 , NbPoints),
105                           PointsZ(1, NbPoints, 1 , NbPoints),
106                           PolesX(1, (DegU+1)*(DegV+1), 0.0),
107                           PolesY(1, (DegU+1)*(DegV+1), 0.0),
108                           PolesZ(1, (DegU+1)*(DegV+1), 0.0),
109                           myUParam(1, NbPoints),
110                           myVParam(1, NbPoints),
111                           VBU(1, DegU+1, 1, NbPoints),
112                           VBV(1, DegV+1, 1, NbPoints)
113 {
114   DegreU = DegU;
115   DegreV = DegV;
116   Nbdiscret = NbPoints;
117   Standard_Integer c, c1, c2, classeU = DegU+1, classeV = DegV+1;
118   Standard_Integer cla = classeU*classeV;
119   Standard_Integer bdeb = 1, bfin = cla, clav = cla-classeV+1;
120   Standard_Integer bint = 0, bint1 = 0, bint2 = 0, bintfin = 0;
121   Standard_Integer bint3 = 0, bint4 = 0, bint5 = 0, bint6 = 0;
122   Standard_Integer bint7 = 0, bint8 = 0;
123   math_Vector GaussP(1, NbPoints), GaussW(1, NbPoints);
124   Standard_Integer i, j, FirstP = 1, LastP = NbPoints;
125   Standard_Real U, dU, V, dV, ISS, Coeff, Coeff2;
126   Done = Standard_False;
127   gp_Pnt Pt;
128   gp_Vec VU, VV;
129   math_Vector BX(1, cla, 0.0),
130               BY(1, cla, 0.0),
131               BZ(1, cla, 0.0);
132
133   GaussP = GPoints(NbPoints);
134   GaussW = GWeights(NbPoints);
135   math_Vector TheWeights(1, NbPoints), VBParam(1, NbPoints);
136
137   dU = 0.5*(U1-U0);
138   dV = 0.5*(V1-V0);
139
140   // calcul et mise en ordre des parametres et des poids:
141   for (i = FirstP; i <= LastP; i++) {
142     U  = 0.5*(U1+U0) + dU*GaussP(i);
143     V  = 0.5*(V1+V0) + dV*GaussP(i);
144     if (i <=  (NbPoints+1)/2) {
145       myUParam(LastP-i+1)  = U;
146       myVParam(LastP-i+1)  = V;
147       VBParam(LastP-i+1)  = 0.5*(1 + GaussP(i));
148       TheWeights(LastP-i+1) = 0.5*GaussW(i);
149     }
150     else {
151       myUParam(i-(NbPoints+1)/2) = U;
152       myVParam(i-(NbPoints+1)/2) = V;
153       VBParam(i-(NbPoints+1)/2)  = 0.5*(1 + GaussP(i));
154       TheWeights(i-(NbPoints+1)/2) = 0.5*GaussW(i);
155     }
156   }
157
158
159
160   // Stockage des Points de Gauss:
161   for (i = FirstP; i <= LastP; i++) {
162     U = myUParam(i);
163     for (j = FirstP; j <= LastP; j++) {
164       V = myVParam(j);
165       SurfTool::D0(Surf, U, V, Pt);
166       Pt.Coord(PointsX(i, j), PointsY(i, j), PointsZ(i, j));
167     }
168   }
169
170   
171   // Calcul des VB ( fonctions de Bernstein):
172   for (i = 1; i <= classeU; i++) {
173     for (j = 1; j <= NbPoints; j++) {
174       VBU(i,j) = PLib::Binomial(classeU-1,i-1)*
175         Pow((1-VBParam(j)),classeU-i)*Pow(VBParam(j),i-1);
176     }
177   }
178   
179   for (i = 1; i <= classeV; i++) {
180     for (j = 1; j <= NbPoints; j++) {
181       VBV(i,j) = PLib::Binomial(classeV-1,i-1)*
182         Pow((1-VBParam(j)),classeV-i)*Pow(VBParam(j),i-1);
183     }
184   }
185
186   // Traitement du second membre:
187   c = 0;
188   for (c1 = 1; c1 <= classeU; c1++) {
189     for (c2 = 1; c2 <= classeV; c2++) {
190       c++;
191       for (i = 1; i <= NbPoints; i++) {
192         for (j = 1; j <= NbPoints; j++) {
193           Coeff = TheWeights(i)*TheWeights(j)*VBU(c1, i)*VBV(c2, j);
194           BX(c) += PointsX(i, j)*Coeff;
195           BY(c) += PointsY(i, j)*Coeff;
196           BZ(c) += PointsZ(i, j)*Coeff;
197         }
198       }
199     }
200   }
201
202   math_Matrix InvM(1, classeU*classeV, 1, classeU*classeV);
203   InvMSurfMatrix(classeU, classeV, InvM);
204   TColgp_Array2OfPnt Poles(1, classeU, 1, classeV);
205
206   // Calcul des poles:
207   // =================
208   if (FirstCons == AppParCurves_NoConstraint &&
209       LastCons  == AppParCurves_NoConstraint &&
210       LastUCons == AppParCurves_NoConstraint &&
211       LastVCons == AppParCurves_NoConstraint) {
212
213     for (i = 1; i <= cla; i++) {
214       for (j = 1; j <= cla; j++) {
215         ISS = InvM(i, j);
216         PolesX(i) += ISS * BX(j);
217         PolesY(i) += ISS * BY(j);
218         PolesZ(i) += ISS * BZ(j);
219       }
220     }
221   }
222   
223   else {
224     // Traitement du second membre:
225     math_Matrix M(1, classeU*classeV, 1, classeU*classeV);
226     MSurfMatrix(classeU, classeV, M);
227     
228     
229     if (FirstCons == AppParCurves_PassPoint ||
230         FirstCons == AppParCurves_TangencyPoint) {
231       bdeb = 2;
232       SurfTool::D0(Surf, U0, V0, Pt);
233       Pt.Coord(PolesX(1), PolesY(1), PolesZ(1));
234         
235       for (c = 1; c <= cla; c++) {
236         Coeff = M(c, 1);
237         BX(c) = BX(c) - PolesX(1)*Coeff;
238         BY(c) = BY(c) - PolesY(1)*Coeff;
239         BZ(c) = BZ(c) - PolesZ(1)*Coeff;
240       }
241     }
242
243     if (LastCons == AppParCurves_PassPoint ||
244         LastCons == AppParCurves_TangencyPoint) {
245       bfin = cla-1;
246       SurfTool::D0(Surf, U1, V1, Pt);
247       Pt.Coord(PolesX(cla), PolesY(cla), PolesZ(cla));
248         
249       for (c = 1; c <= cla; c++) {
250         Coeff = M(c, cla);
251         BX(c) = BX(c) - PolesX(cla)*Coeff;
252         BY(c) = BY(c) - PolesY(cla)*Coeff;
253         BZ(c) = BZ(c) - PolesZ(cla)*Coeff;
254       }
255     }
256
257     if (LastUCons == AppParCurves_PassPoint ||
258         LastUCons == AppParCurves_TangencyPoint) {
259       bint++; bint1 = clav;
260       SurfTool::D0(Surf, U1, V0, Pt);
261       Pt.Coord(PolesX(clav), PolesY(clav), PolesZ(clav));
262         
263       for (c = 1; c <= cla; c++) {
264         Coeff = M(c, clav);
265         BX(c) = BX(c) - PolesX(clav)*Coeff;
266         BY(c) = BY(c) - PolesY(clav)*Coeff;
267         BZ(c) = BZ(c) - PolesZ(clav)*Coeff;
268       }
269     }
270
271     if (LastVCons == AppParCurves_PassPoint ||
272         LastVCons == AppParCurves_TangencyPoint) {
273       bint++; bint2 = classeV;
274       SurfTool::D0(Surf, U0, V1, Pt);
275       Pt.Coord(PolesX(classeV), PolesY(classeV), PolesZ(classeV));
276         
277       for (c = 1; c <= cla; c++) {
278         Coeff = M(c, classeV);
279         BX(c) = BX(c) - PolesX(classeV)*Coeff;
280         BY(c) = BY(c) - PolesY(classeV)*Coeff;
281         BZ(c) = BZ(c) - PolesZ(classeV)*Coeff;
282       }
283     }
284
285     
286
287
288     
289     if (FirstCons == AppParCurves_TangencyPoint) {
290       SurfTool::D1(Surf, U0, V0, Pt, VU, VV);
291       bdeb = 3; bint++; bint3 = classeV+1;
292         
293       PolesX(bint3) = PolesX(1) + VU.X()/DegU*(U1-U0);
294       PolesY(bint3) = PolesY(1) + VU.Y()/DegU*(U1-U0);
295       PolesZ(bint3) = PolesZ(1) + VU.Z()/DegU*(U1-U0);
296         
297       PolesX(2) = PolesX(1) + VV.X()/DegV*(V1-V0);
298       PolesY(2) = PolesY(1) + VV.Y()/DegV*(V1-V0);
299       PolesZ(2) = PolesZ(1) + VV.Z()/DegV*(V1-V0);
300         
301       for (c = 1; c <= cla; c++) {
302         Coeff = M(c, 2); Coeff2 = M(c, bint3);
303         BX(c) = BX(c) - PolesX(2)*Coeff - PolesX(bint3)*Coeff2;
304         BY(c) = BY(c) - PolesY(2)*Coeff - PolesY(bint3)*Coeff2;
305         BZ(c) = BZ(c) - PolesZ(2)*Coeff - PolesZ(bint3)*Coeff2;
306       }
307     }
308
309
310     if (LastCons == AppParCurves_TangencyPoint) {
311       SurfTool::D1(Surf, U1, V1, Pt, VU, VV);
312       bfin = cla-2; bint++; bint4 = cla-classeV;
313       
314       PolesX(bint4) = PolesX(cla) - VU.X()/DegU*(U1-U0);
315       PolesY(bint4) = PolesY(cla) - VU.Y()/DegU*(U1-U0);
316       PolesZ(bint4) = PolesZ(cla) - VU.Z()/DegU*(U1-U0);
317       
318       PolesX(cla-1) = PolesX(cla) - VV.X()/DegV*(V1-V0);
319       PolesY(cla-1) = PolesY(cla) - VV.Y()/DegV*(V1-V0);
320       PolesZ(cla-1) = PolesZ(cla) - VV.Z()/DegV*(V1-V0);
321       
322       for (c = 1; c <= cla; c++) {
323         Coeff = M(c, cla-1); Coeff2 = M(c, bint4);
324         BX(c) = BX(c) - PolesX(cla-1)*Coeff - PolesX(bint4)*Coeff2;
325         BY(c) = BY(c) - PolesY(cla-1)*Coeff - PolesY(bint4)*Coeff2;
326         BZ(c) = BZ(c) - PolesZ(cla-1)*Coeff - PolesZ(bint4)*Coeff2;
327       }
328     }
329
330
331     if (LastVCons == AppParCurves_TangencyPoint) {
332       SurfTool::D1(Surf, U0, V1, Pt, VU, VV);
333       bint += 2; bint5 = classeV-1; bint6 = 2*classeV;
334       
335       PolesX(bint5) = PolesX(classeV) - VV.X()/DegV*(V1-V0);
336       PolesY(bint5) = PolesY(classeV) - VV.Y()/DegV*(V1-V0);
337       PolesZ(bint5) = PolesZ(classeV) - VV.Z()/DegV*(V1-V0);
338       
339       PolesX(bint6) = PolesX(classeV) + VU.X()/DegU*(U1-U0);
340       PolesY(bint6) = PolesY(classeV) + VU.Y()/DegU*(U1-U0);
341       PolesZ(bint6) = PolesZ(classeV) + VU.Z()/DegU*(U1-U0);
342       
343       for (c = 1; c <= cla; c++) {
344         Coeff = M(c, bint5); Coeff2 = M(c, bint6);
345         BX(c) = BX(c) - PolesX(bint5)*Coeff - PolesX(bint6)*Coeff2;
346         BY(c) = BY(c) - PolesY(bint5)*Coeff - PolesY(bint6)*Coeff2;
347         BZ(c) = BZ(c) - PolesZ(bint5)*Coeff - PolesZ(bint6)*Coeff2;
348       }
349     }
350
351
352     if (LastUCons == AppParCurves_TangencyPoint) {
353       SurfTool::D1(Surf, U1, V0, Pt, VU, VV);
354       bint += 2; bint7 = clav-classeV; bint8 = clav+1;
355       
356       PolesX(bint8) = PolesX(clav) + VV.X()/DegV*(V1-V0);
357       PolesY(bint8) = PolesY(clav) + VV.Y()/DegV*(V1-V0);
358       PolesZ(bint8) = PolesZ(clav) + VV.Z()/DegV*(V1-V0);
359       
360       PolesX(bint7) = PolesX(clav) - VU.X()/DegU*(U1-U0);
361       PolesY(bint7) = PolesY(clav) - VU.Y()/DegU*(U1-U0);
362       PolesZ(bint7) = PolesZ(clav) - VU.Z()/DegU*(U1-U0);
363       
364       for (c = 1; c <= cla; c++) {
365         Coeff = M(c, bint8); Coeff2 = M(c, bint7);
366         BX(c) = BX(c)- PolesX(bint8)*Coeff - PolesX(bint7)*Coeff2;
367         BY(c) = BY(c)- PolesY(bint8)*Coeff - PolesY(bint7)*Coeff2;
368         BZ(c) = BZ(c)- PolesZ(bint8)*Coeff - PolesZ(bint7)*Coeff2;
369       }
370     }
371
372
373     math_Vector B2X(bdeb, bfin-bint, 0.0);
374     math_Vector B2Y(bdeb, bfin-bint, 0.0);
375     math_Vector B2Z(bdeb, bfin-bint, 0.0);
376
377     Standard_Integer i2 = bdeb;
378     for (i = bdeb; i <= bfin; i++) {
379       if (i != bint1 && i != bint2 && i != bint3 && i != bint4 &&
380           i != bint5 && i != bint6 && i != bint7 && i != bint8) {
381         for (j = 1; j <= cla; j++) {
382           Coeff = M(i, j);
383           B2X(i2) = B2X(i2) + BX(j)*Coeff;
384           B2Y(i2) = B2Y(i2) + BY(j)*Coeff;
385           B2Z(i2) = B2Z(i2) + BZ(j)*Coeff;
386         }
387         i2 ++;
388       }
389     }
390
391     math_Matrix MP(1, cla, bdeb, bfin-bint);
392     math_Matrix IBP(bdeb, bfin-bint, bdeb, bfin-bint);
393
394     Standard_Integer j2 = bdeb;
395     for (i = 1; i <= cla; i++) {
396       j2 = bdeb;
397       for (j = bdeb; j <= bfin; j++) {
398         if (j != bint1 && j != bint2 && j != bint3 && j != bint4 &&
399             j != bint5 && j != bint6 && j != bint7 && j != bint8) {
400           MP(i, j2) = M(i, j);
401           j2++;
402         }
403       }
404     }
405     math_Matrix IBP1 = MP.Transposed()*MP;
406     IBP = IBP1.Inverse();
407
408     i2 = bdeb;
409     for (i = bdeb; i <= bfin; i++) {
410       if (i != bint1 && i != bint2 && i != bint3 && i != bint4 &&
411           i != bint5 && i != bint6 && i != bint7 && i != bint8) {
412         for (j = bdeb; j <= bfin-bint; j++) {
413           ISS = IBP(i2, j);
414           PolesX(i) += ISS * B2X(j);
415           PolesY(i) += ISS * B2Y(j);
416           PolesZ(i) += ISS * B2Z(j);
417         }
418         i2++;
419       }
420     }
421   }
422
423   for (j = 1; j <= classeV; j++) {
424     for (i = 1; i <= classeU; i++) {
425       Poles(i, j).SetCoord(PolesX(j+ (i-1)*classeV), 
426                            PolesY(j+ (i-1)*classeV), 
427                            PolesZ(j+ (i-1)*classeV));
428     }
429   }
430
431   SCU = new Geom_BezierSurface(Poles);
432   Done = Standard_True;
433 }
434
435
436
437 Standard_Boolean AppCont_SurfLeastSquare::IsDone() const
438 {
439   return Done;
440 }
441
442
443 const Handle(Geom_BezierSurface)& AppCont_SurfLeastSquare::Value()
444 {
445   return SCU;
446 }
447
448
449
450 void AppCont_SurfLeastSquare::Error(Standard_Real& F,
451                                     Standard_Real& MaxE3d) const
452 {
453
454   Standard_Integer i, j, c, cu, cv, classeU = DegreU+1, classeV = DegreV+1;
455   Standard_Real Coeff, err3d = 0.0;
456   math_Matrix MyPointsX(1, Nbdiscret, 1, Nbdiscret);
457   math_Matrix MyPointsY(1, Nbdiscret, 1, Nbdiscret);
458   math_Matrix MyPointsZ(1, Nbdiscret, 1, Nbdiscret);
459   MyPointsX = PointsX;
460   MyPointsY = PointsY;
461   MyPointsZ = PointsZ;
462   MaxE3d = 0.0;
463   F = 0.0;
464   c = 0;
465
466   for (cu = 1; cu <= classeU; cu++) {
467     for (cv = 1; cv <= classeV; cv++) {
468       c++;
469       for (i = 1; i <= Nbdiscret; i++) {
470         for (j = 1; j <= Nbdiscret; j++) {
471           Coeff = VBU(cu, i)*VBV(cv, j);
472           MyPointsX(i, j) = MyPointsX(i, j) - PolesX(c)*Coeff;
473           MyPointsY(i, j) = MyPointsY(i, j) - PolesY(c)*Coeff;
474           MyPointsZ(i, j) = MyPointsZ(i, j) - PolesZ(c)*Coeff;
475         }
476       }
477     }
478   }
479
480
481   for (i = 1; i <= Nbdiscret; i++) {
482     for (j = 1; j <= Nbdiscret; j++) {
483       err3d = MyPointsX(i, j) * MyPointsX(i, j) +
484               MyPointsY(i, j) * MyPointsY(i, j) +
485               MyPointsZ(i, j) * MyPointsZ(i, j);
486       MaxE3d = Max(MaxE3d, err3d);
487       F += err3d;
488     }
489   }
490
491   MaxE3d = Sqrt(MaxE3d);
492
493 }