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