0027275: Unused formal parameter in BSplCLib::EvalBsplineBasis
[occt.git] / src / FairCurve / FairCurve_DistributionOfJerk.cxx
1 // Created on: 1996-04-01
2 // Created by: Philippe MANGIN
3 // Copyright (c) 1996-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 // 01-04-1996 : PMN Version originale
18
19 #ifndef OCCT_DEBUG
20 #define No_Standard_RangeError
21 #define No_Standard_OutOfRange
22 #endif
23
24
25 #include <BSplCLib.hxx>
26 #include <FairCurve_BattenLaw.hxx>
27 #include <FairCurve_DistributionOfJerk.hxx>
28 #include <gp_Pnt2d.hxx>
29 #include <gp_XY.hxx>
30 #include <math_Matrix.hxx>
31 #include <math_Vector.hxx>
32
33 FairCurve_DistributionOfJerk::FairCurve_DistributionOfJerk(const Standard_Integer BSplOrder, 
34                                                            const Handle(TColStd_HArray1OfReal)& FlatKnots,
35                                                            const Handle(TColgp_HArray1OfPnt2d)& Poles,
36                                                            const Standard_Integer DerivativeOrder,
37                                                            const FairCurve_BattenLaw& Law,
38                                                            const Standard_Integer NbValAux) :
39                                   FairCurve_DistributionOfEnergy( BSplOrder,  
40                                                                   FlatKnots, 
41                                                                   Poles, 
42                                                                   DerivativeOrder,
43                                                                   NbValAux) , 
44                                    MyLaw (Law)                                                          
45 {
46 }
47
48
49 Standard_Boolean FairCurve_DistributionOfJerk::Value(const math_Vector& TParam,
50                                                            math_Vector& Jerk)
51 {
52   Standard_Boolean Ok = Standard_True;
53   Standard_Integer ier, ii, jj, kk;
54   gp_XY CPrim  (0., 0.), CSecn (0., 0.), CTroi(0., 0.);
55   Standard_Integer  LastGradientIndex, FirstNonZero, LastZero; 
56
57   // (0.0) initialisations generales
58   Jerk.Init(0.0);
59   math_Matrix Base(1, 5, 1, MyBSplOrder ); // On shouhaite utiliser la derive troisieme
60                                            // Dans EvalBsplineBasis C"' <=> DerivOrder = 4
61                                            // et il faut ajouter 1 rang dans la matrice Base => 5 rangs
62    
63   ier  =  BSplCLib::EvalBsplineBasis(3,  MyBSplOrder, 
64                                      MyFlatKnots->Array1(), TParam(TParam.Lower()),
65                                      FirstNonZero, Base );
66   if (ier != 0) return Standard_False;
67   LastZero = FirstNonZero - 1;
68   FirstNonZero = 2*LastZero+1;
69
70
71   // (0.1) evaluation de CPrim et CScn
72   for (ii= 1; ii<= MyBSplOrder; ii++) {
73       CPrim += Base(2, ii) * MyPoles->Value(ii+LastZero).Coord();
74       CSecn += Base(3, ii) * MyPoles->Value(ii+LastZero).Coord();
75       CTroi += Base(4, ii) * MyPoles->Value(ii+LastZero).Coord();
76       }
77
78   // (1) Evaluation de la secousse locale = W*W
79   Standard_Real NormeCPrim = CPrim.Modulus();
80   Standard_Real InvNormeCPrim = 1 / NormeCPrim;
81   Standard_Real InvNormeCPrim2 = InvNormeCPrim *InvNormeCPrim;
82   Standard_Real ProduitC1C2 =  CPrim*CSecn; 
83   Standard_Real DeriveNormeCPrim = ProduitC1C2 * InvNormeCPrim2;
84
85   Standard_Real Hauteur, WVal, Mesure;
86   Standard_Real NumRho = CPrim ^ CSecn;
87   Standard_Real Numerateur = (CPrim ^ CTroi) - 3 * NumRho * DeriveNormeCPrim; 
88   Standard_Real Denominateur = pow ( NormeCPrim, 2.5);
89
90   Ok = MyLaw.Value (TParam(TParam.Lower()), Hauteur);
91   if (!Ok) return Ok;
92
93   Mesure =  pow(Hauteur, 3) / 12;
94   WVal   =  Numerateur / Denominateur;
95   Jerk(Jerk.Lower()) = Mesure * pow(WVal, 2);
96
97   if (MyDerivativeOrder >= 1) {
98   // (2) Evaluation du gradient de la secousse locale.
99
100       math_Vector WGrad (1, 2*MyBSplOrder+MyNbValAux), 
101                   NumGrad(1, 2*MyBSplOrder), 
102                   NGrad1(1, 2*MyBSplOrder),
103                   NGrad2(1, 2*MyBSplOrder),
104                   GradNormeCPrim(1, 2*MyBSplOrder),
105                   NGDNCPrim(1, 2*MyBSplOrder),
106                   GradDeriveNormeCPrim(1, 2*MyBSplOrder),
107                   GradNumRho(1, 2*MyBSplOrder),
108                   NumduGrad(1, 2*MyBSplOrder);
109       Standard_Real Facteur;
110       Standard_Real XPrim = CPrim.X();
111       Standard_Real YPrim = CPrim.Y();
112       Standard_Real XSecn = CSecn.X();
113       Standard_Real YSecn = CSecn.Y();
114       Standard_Real XTroi = CTroi.X();
115       Standard_Real YTroi = CTroi.Y();
116       Standard_Real InvDenominateur = 1 / Denominateur;
117       Standard_Real Aux, AuxBis;
118
119       Facteur = 2 * Mesure * WVal;
120       Aux = 2.5 * Numerateur * InvNormeCPrim;
121       AuxBis = 2 * ProduitC1C2 * InvNormeCPrim;
122       kk = Jerk.Lower() + FirstNonZero;
123       
124
125       jj = 1;
126       for (ii=1; ii<=MyBSplOrder; ii++) {
127
128    //     (2.1) Derivation en X
129              GradNormeCPrim(jj) =  XPrim * Base(2, ii) * InvNormeCPrim;
130              NGDNCPrim(jj) = (XPrim * Base(3, ii) + XSecn * Base(2, ii)) 
131                            -  AuxBis * GradNormeCPrim(jj);
132              GradDeriveNormeCPrim(jj) =  NGDNCPrim(jj) * InvNormeCPrim2;
133              GradNumRho(jj) = YSecn * Base(2, ii) - YPrim * Base(3, ii);
134              NGrad1(jj) = YTroi * Base(2, ii) - YPrim * Base(4, ii);
135              NGrad2(jj) =  - 3 * (  NumRho         * GradDeriveNormeCPrim(jj) 
136                                   + GradNumRho(jj) * DeriveNormeCPrim);
137              NumGrad(jj) =  NGrad1(jj) + NGrad2(jj);
138              NumduGrad(jj) =  NumGrad(jj) - Aux * GradNormeCPrim(jj);
139              WGrad(jj)   = InvDenominateur * NumduGrad(jj);
140              Jerk(kk) = Facteur *  WGrad(jj);
141
142              jj +=1;
143
144    //     (2.2) Derivation en Y                                                             
145
146              GradNormeCPrim(jj) = YPrim * Base(2, ii) * InvNormeCPrim;
147              NGDNCPrim(jj) = (YPrim * Base(3, ii) + YSecn * Base(2, ii))  
148                            -  AuxBis * GradNormeCPrim(jj);
149              GradDeriveNormeCPrim(jj) =  NGDNCPrim(jj)  * InvNormeCPrim2;
150              GradNumRho(jj) =  - XSecn * Base(2, ii) + XPrim * Base(3, ii);
151              NGrad1(jj) =  - XTroi * Base(2, ii) + XPrim * Base(4, ii);
152              NGrad2(jj) =  - 3 * (  NumRho         * GradDeriveNormeCPrim(jj) 
153                                  +  GradNumRho(jj) * DeriveNormeCPrim);
154              NumGrad(jj) =  NGrad1(jj) + NGrad2(jj);
155              NumduGrad(jj) =  NumGrad(jj) - Aux * GradNormeCPrim(jj);
156              WGrad(jj)   = InvDenominateur * NumduGrad(jj);
157              Jerk(kk+1) = Facteur *  WGrad(jj);
158
159              jj += 1;
160              kk += 2;
161       }
162       if (MyNbValAux == 1) {
163     //    (2.3) Gestion de la variable de glissement
164              LastGradientIndex = Jerk.Lower() + 2*MyPoles->Length() + 1;
165              WGrad( WGrad.Upper()) = 0.0; 
166              Jerk(LastGradientIndex) = 0.0;       
167        }
168
169       else { LastGradientIndex = Jerk.Lower() + 2*MyPoles->Length(); }
170
171
172       if (MyDerivativeOrder >= 2) { 
173    
174 // (3) Evaluation du Hessien de la tension locale ---------------------- 
175
176
177          Standard_Real FacteurX =  (1 - Pow(XPrim * InvNormeCPrim,2)) * InvNormeCPrim;
178          Standard_Real FacteurY =  (1 - Pow(YPrim * InvNormeCPrim,2)) * InvNormeCPrim;
179          Standard_Real FacteurXY = - (XPrim*InvNormeCPrim)
180                                  * (YPrim*InvNormeCPrim) * InvNormeCPrim;
181          Standard_Real FacteurW = WVal * InvNormeCPrim;
182
183          Standard_Real Produit, Produit2, ProduitV, HNumRho, DSeconde, NSeconde, 
184                        HDeriveNormeCPrim, Aux1, DeriveAuxBis;
185          Standard_Real VIntermed;
186          Standard_Integer k1, k2, II, JJ;
187    
188          Facteur = 2 * Mesure;
189
190          kk = FirstNonZero;
191          k2 = LastGradientIndex + (kk-1)*kk/2;
192
193          for (ii=2; ii<= 2*MyBSplOrder; ii+=2) {
194            II = ii/2;
195            k1 = k2+FirstNonZero;
196            k2 = k1+kk;
197            kk += 2;              
198            for (jj=2; jj< ii; jj+=2) {
199              JJ = jj/2;
200              Produit =  Base(2, II) *  Base(2, JJ);
201              Produit2 = Base(2, II) *  Base(3, JJ) +  Base(3, II) *  Base(2, JJ);           
202              ProduitV =   Base(2, II) *  Base(4, JJ)
203                         - Base(4, II) *  Base(2, JJ);
204              HNumRho  =   Base(2, II) *  Base(3, JJ)
205                         - Base(3, II) *  Base(2, JJ);
206
207              // derivation en XiXj
208              Aux1 =  InvNormeCPrim2 * GradNormeCPrim(jj-1);
209              DeriveAuxBis = 2 * ( (XPrim * Base(3, JJ) + XSecn * Base(2, JJ))*InvNormeCPrim
210                                  - ProduitC1C2 * Aux1);
211              HDeriveNormeCPrim =  ( Produit2 - DeriveAuxBis*GradNormeCPrim(ii-1) 
212                                   - AuxBis * ( Produit * InvNormeCPrim 
213                                   - XPrim * Base(2, II)*Aux1) )*InvNormeCPrim2
214                                - 2*NGDNCPrim(ii-1) * Aux1 *InvNormeCPrim;
215              NSeconde = - 3 * (
216                         GradNumRho(ii-1) * GradDeriveNormeCPrim(jj-1)
217                       + NumRho * HDeriveNormeCPrim
218                       + GradNumRho(jj-1) * GradDeriveNormeCPrim(ii-1) );
219
220              DSeconde =  FacteurX * Produit;
221              Aux = NormeCPrim * NSeconde
222                  + NumGrad(ii-1)*GradNormeCPrim(jj-1)
223                  - 2.5 * ( NumGrad(jj-1)*GradNormeCPrim(ii-1)
224                          + DSeconde * Numerateur );
225              VIntermed = InvDenominateur 
226                        * (Aux - 3.5*GradNormeCPrim(jj-1)*NumduGrad(ii-1));
227
228              Jerk(k1) = Facteur * ( WGrad(ii-1)*WGrad(jj-1)
229                                   + FacteurW * VIntermed); 
230              k1++;
231  
232              // derivation en XiYj
233              Aux1 =  InvNormeCPrim2 * GradNormeCPrim(jj);
234              DeriveAuxBis = 2 * ( (YPrim * Base(3, JJ) + YSecn * Base(2, JJ))*InvNormeCPrim
235                                  - ProduitC1C2 * Aux1);
236              HDeriveNormeCPrim =  (- DeriveAuxBis*GradNormeCPrim(ii-1) 
237                                    + AuxBis *  XPrim * Base(2, II) * Aux1) * InvNormeCPrim2
238                                - 2*NGDNCPrim(ii-1)*Aux1*InvNormeCPrim;
239              NSeconde = ProduitV 
240                       - 3 * (
241                         GradNumRho(ii-1) * GradDeriveNormeCPrim(jj)
242                       + NumRho * HDeriveNormeCPrim
243                       + GradNumRho(jj) * GradDeriveNormeCPrim(ii-1)
244                       + HNumRho * DeriveNormeCPrim );
245              DSeconde =  FacteurXY * Produit;   
246              Aux = NormeCPrim * NSeconde
247                  + NumGrad(ii-1)*GradNormeCPrim(jj)
248                  - 2.5 * ( NumGrad(jj)*GradNormeCPrim(ii-1)
249                          + DSeconde * Numerateur );
250              VIntermed = InvDenominateur 
251                        * (Aux - 3.5*GradNormeCPrim(jj)*NumduGrad(ii-1));
252              Jerk(k1) = Facteur * ( WGrad(ii-1)*WGrad(jj)
253                                      + FacteurW * VIntermed);
254              k1++;
255
256              // derivation en YiXj
257              // DSeconde calcule ci-dessus
258              Aux1 =  InvNormeCPrim2 * GradNormeCPrim(jj-1);
259              DeriveAuxBis = 2 * ( (XPrim * Base(3, JJ) + XSecn * Base(2, JJ))*InvNormeCPrim
260                                  - ProduitC1C2 * Aux1);
261              HDeriveNormeCPrim =  (- DeriveAuxBis*GradNormeCPrim(ii) 
262                                   + AuxBis * YPrim * Base(2, II)*Aux1) * InvNormeCPrim2
263                                - 2*NGDNCPrim(ii)*Aux1 *InvNormeCPrim;
264              NSeconde = - ProduitV 
265                       - 3 * (
266                         GradNumRho(ii) * GradDeriveNormeCPrim(jj-1)
267                       + NumRho * HDeriveNormeCPrim
268                       + GradNumRho(jj-1) * GradDeriveNormeCPrim(ii) 
269                       - HNumRho * DeriveNormeCPrim);
270              Aux = NormeCPrim * NSeconde
271                    + NumGrad(ii)*GradNormeCPrim(jj-1)
272                    - 2.5 * ( NumGrad(jj-1)*GradNormeCPrim(ii)
273                          + DSeconde * Numerateur );
274              VIntermed = InvDenominateur 
275                        * (Aux - 3.5*GradNormeCPrim(jj-1)*NumduGrad(ii));
276
277              Jerk(k2) = Facteur * ( WGrad(ii)*WGrad(jj-1)       
278                                      + FacteurW * VIntermed);
279              k2++;
280
281              // derivation en YiYj
282              Aux1 =  InvNormeCPrim2 * GradNormeCPrim(jj);
283              DeriveAuxBis = 2 * ( (YPrim * Base(3, JJ) + YSecn * Base(2, JJ))*InvNormeCPrim
284                                  - ProduitC1C2 * Aux1);
285              HDeriveNormeCPrim =  ( Produit2 - DeriveAuxBis*GradNormeCPrim(ii) 
286                                   - AuxBis * ( Produit * InvNormeCPrim 
287                                   - YPrim * Base(2, II)*Aux1) )*InvNormeCPrim2
288                                - 2*NGDNCPrim(ii)*Aux1 *InvNormeCPrim;
289              NSeconde = - 3 * (
290                         GradNumRho(ii) * GradDeriveNormeCPrim(jj)
291                       + NumRho * HDeriveNormeCPrim
292                       + GradNumRho(jj) * GradDeriveNormeCPrim(ii) );
293
294              DSeconde =  FacteurY * Produit;
295              Aux = NSeconde * NormeCPrim
296                  + NumGrad(ii)*GradNormeCPrim(jj)
297                  - 2.5 * ( NumGrad(jj)*GradNormeCPrim(ii)
298                          + DSeconde * Numerateur );
299              VIntermed = InvDenominateur 
300                        * (Aux - 3.5*GradNormeCPrim(jj)*NumduGrad(ii));
301              Jerk(k2) = Facteur * ( WGrad(ii)*WGrad(jj)
302                                      + FacteurW * VIntermed); 
303              k2++;
304              }
305
306          // cas ou jj = ii : remplisage en triangle
307              Produit =  pow (Base(2, II), 2);
308              Produit2 = 2 * Base(2, II) *  Base(3, II); 
309   //           ProduitV2 = Base         
310
311              
312              // derivation en XiXi
313              Aux1 =  InvNormeCPrim2 * GradNormeCPrim(ii-1);
314              DeriveAuxBis = 2 * ( (XPrim * Base(3, II) + XSecn * Base(2, II))*InvNormeCPrim
315                                  - ProduitC1C2 * Aux1);
316              HDeriveNormeCPrim =  ( Produit2 - DeriveAuxBis*GradNormeCPrim(ii-1) 
317                                   - AuxBis * ( Produit * InvNormeCPrim 
318                                   - XPrim * Base(2, II)*Aux1) )*InvNormeCPrim2
319                                - 2*NGDNCPrim(ii-1)*Aux1 *InvNormeCPrim;
320              NSeconde = - 3 * (
321                               2 * GradNumRho(ii-1) * GradDeriveNormeCPrim(ii-1)
322                               + NumRho * HDeriveNormeCPrim );
323              DSeconde = FacteurX * Produit;
324              Aux =  NSeconde * NormeCPrim 
325                   - 1.5 * NumGrad(ii-1)*GradNormeCPrim(ii-1)
326                   - 2.5 * DSeconde * Numerateur;
327              VIntermed = InvDenominateur 
328                        * (Aux - 3.5*GradNormeCPrim(ii-1)*NumduGrad(ii-1));
329              Jerk(k1) = Facteur * ( WGrad(ii-1)*WGrad(ii-1)
330                                      + FacteurW * VIntermed );
331              // derivation en XiYi
332              Aux1 =  InvNormeCPrim2 * GradNormeCPrim(ii);
333              DeriveAuxBis = 2 * ( (YPrim * Base(3, II) + YSecn * Base(2, II))*InvNormeCPrim
334                                  - ProduitC1C2 * Aux1);
335              HDeriveNormeCPrim =  ( - DeriveAuxBis * GradNormeCPrim(ii-1) 
336                                     + AuxBis * XPrim * Base(2, II) * Aux1 ) * InvNormeCPrim2
337                                - 2*NGDNCPrim(ii-1) * Aux1 *InvNormeCPrim;
338              NSeconde = -3 * (
339                                 GradNumRho(ii-1) * GradDeriveNormeCPrim(ii)
340                               + NumRho * HDeriveNormeCPrim 
341                               + GradNumRho(ii) * GradDeriveNormeCPrim(ii-1) );
342              DSeconde = FacteurXY * Produit;
343              Aux = NSeconde * NormeCPrim 
344                  + NumGrad(ii)*GradNormeCPrim(ii-1)
345                  - 2.5 * ( NumGrad(ii-1)*GradNormeCPrim(ii)
346                          + DSeconde * Numerateur );
347              VIntermed = InvDenominateur 
348                        * (Aux- 3.5*GradNormeCPrim(ii-1)*NumduGrad(ii));
349              Jerk(k2) = Facteur * ( WGrad(ii)*WGrad(ii-1)
350                                      + FacteurW * VIntermed);
351              k2++;
352
353              // derivation en YiYi
354              // Aux1 et DeriveAuxBis calcules au pas precedent ...
355              HDeriveNormeCPrim =  ( Produit2 - DeriveAuxBis*GradNormeCPrim(ii) 
356                                   - AuxBis * ( Produit * InvNormeCPrim 
357                                   - YPrim * Base(2, II)*Aux1) )*InvNormeCPrim2
358                                - 2*NGDNCPrim(ii)*Aux1 *InvNormeCPrim;
359              NSeconde = - 3 * (
360                               2 * GradNumRho(ii) * GradDeriveNormeCPrim(ii)
361                               + NumRho * HDeriveNormeCPrim );
362              DSeconde = FacteurY * Produit;
363              Aux =  NSeconde * NormeCPrim
364                  - 1.5 * NumGrad(ii)*GradNormeCPrim(ii) 
365                  - 2.5 * DSeconde * Numerateur; 
366              VIntermed = InvDenominateur 
367                        * (Aux - 3.5*GradNormeCPrim(ii)*NumduGrad(ii));
368              Jerk(k2) = Facteur * ( WGrad(ii)*WGrad(ii)
369                                      + FacteurW * VIntermed);
370          }
371        }     
372   }
373
374 // sortie standard           
375   return Ok;
376 }
377