b311480e |
1 | // Created on: 1996-04-01 |
2 | // Created by: Philippe MANGIN |
3 | // Copyright (c) 1996-1999 Matra Datavision |
973c2be1 |
4 | // Copyright (c) 1999-2014 OPEN CASCADE SAS |
b311480e |
5 | // |
973c2be1 |
6 | // This file is part of Open CASCADE Technology software library. |
b311480e |
7 | // |
d5f74e42 |
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 |
973c2be1 |
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. |
b311480e |
13 | // |
973c2be1 |
14 | // Alternatively, this file may be used under the terms of Open CASCADE |
15 | // commercial license or contractual agreement. |
b311480e |
16 | |
7fd59977 |
17 | // 01-04-1996 : PMN Version originale |
18 | |
0797d9d3 |
19 | #ifndef OCCT_DEBUG |
7fd59977 |
20 | #define No_Standard_RangeError |
21 | #define No_Standard_OutOfRange |
22 | #endif |
23 | |
7fd59977 |
24 | |
42cf5bc1 |
25 | #include <BSplCLib.hxx> |
26 | #include <FairCurve_BattenLaw.hxx> |
27 | #include <FairCurve_DistributionOfJerk.hxx> |
7fd59977 |
28 | #include <gp_Pnt2d.hxx> |
42cf5bc1 |
29 | #include <gp_XY.hxx> |
7fd59977 |
30 | #include <math_Matrix.hxx> |
42cf5bc1 |
31 | #include <math_Vector.hxx> |
7fd59977 |
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 | |
6143f12f |
63 | ier = BSplCLib::EvalBsplineBasis(3, MyBSplOrder, |
7fd59977 |
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 | |