7fd59977 |
1 | // File: Law.cxx |
2 | // Created: Mon Nov 20 09:43:45 1995 |
3 | // Author: Laurent BOURESCHE |
4 | // <lbo@phylox> |
5 | |
6 | |
7 | #include <Law.ixx> |
8 | |
9 | #include <Law_Interpolate.hxx> |
10 | #include <BSplCLib.hxx> |
11 | #include <TColStd_HArray1OfReal.hxx> |
12 | #include <TColStd_Array1OfReal.hxx> |
13 | #include <TColStd_HArray1OfBoolean.hxx> |
14 | #include <TColStd_Array1OfBoolean.hxx> |
15 | #include <gp_Pnt.hxx> |
16 | |
17 | Handle(Law_BSpFunc) Law::MixBnd(const Handle(Law_Linear)& Lin) |
18 | { |
19 | Standard_Real f,l; |
20 | Lin->Bounds(f,l); |
21 | TColStd_Array1OfReal Knots(1,4); |
22 | TColStd_Array1OfInteger Mults(1,4); |
23 | Knots(1) = f; Knots(4) = l; |
24 | Knots(2) = 0.75*f + 0.25*l; Knots(3) = 0.25*f + 0.75*l; |
25 | Mults(1) = Mults(4) = 4; |
26 | Mults(2) = Mults(3) = 1; |
27 | Handle(TColStd_HArray1OfReal) pol = Law::MixBnd(3,Knots,Mults,Lin); |
28 | Handle(Law_BSpline) bs = new Law_BSpline(pol->Array1(),Knots,Mults,3); |
29 | Handle(Law_BSpFunc) bsf = new Law_BSpFunc(); |
30 | bsf->SetCurve(bs); |
31 | return bsf; |
32 | } |
33 | |
34 | Handle(TColStd_HArray1OfReal) Law::MixBnd |
35 | (const Standard_Integer Degree, |
36 | const TColStd_Array1OfReal& Knots, |
37 | const TColStd_Array1OfInteger& Mults, |
38 | const Handle(Law_Linear)& Lin) |
39 | { |
40 | Standard_Integer nbpol = 0, nbfk = 0, i, j, k = 0; |
41 | for(i = Mults.Lower(); i <= Mults.Upper(); i++){ |
42 | nbfk += Mults(i); |
43 | } |
44 | TColStd_Array1OfReal fk(1,nbfk); |
45 | for(i = Mults.Lower(); i <= Mults.Upper(); i++){ |
46 | for(j = 1; j <= Mults(i); j++){ |
47 | fk(++k) = Knots(i); |
48 | } |
49 | } |
50 | nbpol = nbfk - Degree - 1; |
51 | TColStd_Array1OfReal par(1,nbpol); |
52 | BSplCLib::BuildSchoenbergPoints(Degree,fk,par); |
53 | Handle(TColStd_HArray1OfReal) res = new TColStd_HArray1OfReal(1,nbpol); |
54 | TColStd_Array1OfReal& pol = res->ChangeArray1(); |
55 | for(i = 1; i <= nbpol; i++){ |
56 | pol(i) = Lin->Value(par(i)); |
57 | } |
58 | TColStd_Array1OfInteger ord(1,nbpol); |
59 | ord.Init(0); |
60 | BSplCLib::Interpolate(Degree,fk,par,ord,1,pol(1),i); |
61 | if(nbpol >= 4){ |
62 | pol(2) = pol(1); |
63 | pol(nbpol - 1) = pol(nbpol); |
64 | } |
65 | return res; |
66 | } |
67 | |
68 | static Standard_Real eval1(const Standard_Real p, |
69 | const Standard_Real first, |
70 | const Standard_Real last, |
71 | const Standard_Real piv, |
72 | const Standard_Boolean nulr) |
73 | { |
74 | if((nulr && p >= piv) || (!nulr && p <= piv)) return 0.; |
75 | else if(nulr){ |
76 | Standard_Real a = piv - first; |
77 | a *= a; |
78 | a = 1./a; |
79 | Standard_Real b = p - first; |
80 | a *= b; |
81 | b = piv - p; |
82 | a *= b; |
83 | a *= b; |
84 | return a; |
85 | } |
86 | else{ |
87 | Standard_Real a = last - piv; |
88 | a *= a; |
89 | a = 1./a; |
90 | Standard_Real b = last - p; |
91 | a *= b; |
92 | b = p - piv; |
93 | a *= b; |
94 | a *= b; |
95 | return a; |
96 | } |
97 | } |
98 | |
99 | Handle(TColStd_HArray1OfReal) Law::MixTgt |
100 | (const Standard_Integer Degree, |
101 | const TColStd_Array1OfReal& Knots, |
102 | const TColStd_Array1OfInteger& Mults, |
103 | const Standard_Boolean NulOnTheRight, |
104 | const Standard_Integer Index) |
105 | { |
106 | Standard_Real first = Knots(Knots.Lower()); |
107 | Standard_Real last = Knots(Knots.Upper()); |
108 | Standard_Real piv = Knots(Index); |
109 | Standard_Integer nbpol = 0, nbfk = 0, i, j, k = 0; |
110 | for(i = Mults.Lower(); i <= Mults.Upper(); i++){ |
111 | nbfk += Mults(i); |
112 | } |
113 | TColStd_Array1OfReal fk(1,nbfk); |
114 | for(i = Mults.Lower(); i <= Mults.Upper(); i++){ |
115 | for(j = 1; j <= Mults(i); j++){ |
116 | fk(++k) = Knots(i); |
117 | } |
118 | } |
119 | nbpol = nbfk - Degree - 1; |
120 | TColStd_Array1OfReal par(1,nbpol); |
121 | BSplCLib::BuildSchoenbergPoints(Degree,fk,par); |
122 | Handle(TColStd_HArray1OfReal) res = new TColStd_HArray1OfReal(1,nbpol); |
123 | TColStd_Array1OfReal& pol = res->ChangeArray1(); |
124 | for(i = 1; i <= nbpol; i++){ |
125 | pol(i) = eval1(par(i),first,last,piv,NulOnTheRight); |
126 | } |
127 | TColStd_Array1OfInteger ord(1,nbpol); |
128 | ord.Init(0); |
129 | BSplCLib::Interpolate(Degree,fk,par,ord,1,pol(1),i); |
130 | return res; |
131 | } |
132 | |
133 | Handle(Law_BSpline) Law::Reparametrize(const Adaptor3d_Curve& Curve, |
134 | const Standard_Real First, |
135 | const Standard_Real Last, |
136 | const Standard_Boolean HasDF, |
137 | const Standard_Boolean HasDL, |
138 | const Standard_Real DFirst, |
139 | const Standard_Real DLast, |
140 | const Standard_Boolean Rev, |
141 | const Standard_Integer NbPoints) |
142 | { |
143 | // On evalue la longeur approximative de la courbe. |
144 | |
145 | Standard_Integer i; |
146 | Standard_Real DDFirst = DFirst, DDLast = DLast; |
147 | if(HasDF && Rev) DDFirst = -DFirst; |
148 | if(HasDL && Rev) DDLast = -DLast; |
149 | TColStd_Array1OfReal cumdist(1,2*NbPoints); |
150 | TColStd_Array1OfReal ucourbe(1,2*NbPoints); |
151 | gp_Pnt P1,P2; |
152 | Standard_Real U1 = Curve.FirstParameter(); |
153 | Standard_Real U2 = Curve.LastParameter(); |
154 | Standard_Real U, DU, Length = 0.; |
155 | if(!Rev){ |
156 | P1 = Curve.Value(U1); |
157 | U = U1; |
158 | DU = (U2 - U1) / ( 2*NbPoints - 1); |
159 | } |
160 | else{ |
161 | P1 = Curve.Value(U2); |
162 | U = U2; |
163 | DU = (U1 - U2) / ( 2*NbPoints - 1); |
164 | } |
165 | for ( i = 1; i <= 2*NbPoints ; i ++) { |
166 | P2 = Curve.Value(U); |
167 | Length += P2.Distance(P1); |
168 | cumdist(i) = Length; |
169 | ucourbe(i) = U; |
170 | U += DU; |
171 | P1 = P2; |
172 | } |
173 | if(Rev) ucourbe(2*NbPoints) = U1; |
174 | else ucourbe(2*NbPoints) = U2; |
175 | |
176 | Handle(TColStd_HArray1OfReal) point = new TColStd_HArray1OfReal(1,NbPoints); |
177 | Handle(TColStd_HArray1OfReal) param = new TColStd_HArray1OfReal(1,NbPoints); |
178 | |
179 | Standard_Real DCorde = Length / ( NbPoints - 1); |
180 | Standard_Real Corde = DCorde; |
181 | Standard_Integer Index = 1; |
182 | Standard_Real Alpha; |
183 | Standard_Real fac = 1./(NbPoints-1); |
184 | |
185 | point->SetValue(1,ucourbe(1)); |
186 | param->SetValue(1,First); |
187 | point->SetValue(NbPoints,ucourbe(2*NbPoints)); |
188 | param->SetValue(NbPoints,Last); |
189 | |
190 | for ( i = 2; i < NbPoints; i++) { |
191 | while (cumdist(Index) < Corde) Index ++; |
192 | |
193 | Alpha = (Corde - cumdist(Index - 1)) / (cumdist(Index) -cumdist(Index - 1)); |
194 | U = ucourbe(Index-1) + Alpha * (ucourbe(Index) - ucourbe(Index-1)); |
195 | point->SetValue(i,U); |
196 | param->SetValue(i,((NbPoints-i)*First+(i-1)*Last)*fac); |
197 | Corde = i*DCorde; |
198 | } |
199 | Law_Interpolate inter(point,param,0,1.e-9); |
200 | if(HasDF || HasDL){ |
201 | TColStd_Array1OfReal tgt(1,NbPoints); |
202 | Handle(TColStd_HArray1OfBoolean) |
203 | flag = new TColStd_HArray1OfBoolean(1,NbPoints); |
204 | flag->ChangeArray1().Init(0); |
205 | if(HasDF){ flag->SetValue(1,1); tgt.SetValue(1,DDFirst); } |
206 | if(HasDL){ flag->SetValue(NbPoints,1); tgt.SetValue(NbPoints,DDLast); } |
207 | inter.Load(tgt,flag); |
208 | } |
209 | inter.Perform(); |
210 | if(!inter.IsDone()) |
211 | Standard_Failure::Raise |
212 | ("Law::Reparametrize echec interpolation"); |
213 | Handle(Law_BSpline) bs = inter.Curve(); |
214 | return bs; |
215 | } |
216 | |
217 | static Standard_Real eval2(const Standard_Real p, |
218 | // const Standard_Real first, |
219 | const Standard_Real , |
220 | // const Standard_Real last, |
221 | const Standard_Real , |
222 | const Standard_Real mil, |
223 | const Standard_Boolean hasfirst, |
224 | const Standard_Boolean haslast, |
225 | const Handle(Law_BSpline)& bs1, |
226 | const Handle(Law_BSpline)& bs2) |
227 | { |
228 | if(hasfirst && p < mil) return bs1->Value(p); |
229 | else if(haslast && p > mil) return bs2->Value(p); |
230 | else return 1.; |
231 | } |
232 | |
233 | Handle(Law_BSpline) Law::Scale(const Standard_Real First, |
234 | const Standard_Real Last, |
235 | const Standard_Boolean HasF, |
236 | const Standard_Boolean HasL, |
237 | const Standard_Real VFirst, |
238 | const Standard_Real VLast) |
239 | { |
240 | Standard_Integer i; |
241 | Standard_Real Milieu = 0.5*(First+Last); |
242 | TColStd_Array1OfReal knot(1,3); |
243 | TColStd_Array1OfReal fknot(1,10); |
244 | TColStd_Array1OfInteger mult(1,3); |
245 | knot(1) = First; knot(2) = Milieu; knot(3) = Last; |
246 | fknot(1) = fknot(2) = fknot(3) = fknot(4) = First; |
247 | fknot(10) = fknot(9) = fknot(8) = fknot(7) = Last; |
248 | fknot(5) = fknot(6) = Milieu; |
249 | mult(1) = 4; mult(3) = 4; mult(2) = 2; |
250 | |
251 | TColStd_Array1OfReal pbs(1,4); |
252 | TColStd_Array1OfReal kbs(1,2); |
253 | TColStd_Array1OfInteger mbs(1,2); |
254 | mbs(1) = mbs(2) = 4; |
255 | Handle(Law_BSpline) bs1,bs2; |
256 | if(HasF){ |
257 | pbs(1) = pbs(2) = VFirst; |
258 | pbs(3) = pbs(4) = 1.; |
259 | kbs(1) = First; kbs(2) = Milieu; |
260 | bs1 = new Law_BSpline(pbs,kbs,mbs,3); |
261 | } |
262 | if(HasL){ |
263 | pbs(1) = pbs(2) = 1.; |
264 | pbs(3) = pbs(4) = VLast; |
265 | kbs(1) = Milieu; kbs(2) = Last; |
266 | bs2 = new Law_BSpline(pbs,kbs,mbs,3); |
267 | } |
268 | |
269 | TColStd_Array1OfReal pol(1,6); |
270 | TColStd_Array1OfReal par(1,6); |
271 | BSplCLib::BuildSchoenbergPoints(3,fknot,par); |
272 | for(i = 1; i <= 6; i++){ |
273 | pol(i) = eval2(par(i),First,Last,Milieu,HasF,HasL,bs1,bs2); |
274 | } |
275 | TColStd_Array1OfInteger ord(1,6); |
276 | ord.Init(0); |
277 | BSplCLib::Interpolate(3,fknot,par,ord,1,pol(1),i); |
278 | bs1 = new Law_BSpline(pol,knot,mult,3); |
279 | return bs1; |
280 | } |
281 | |
282 | |
283 | Handle(Law_BSpline) Law::ScaleCub(const Standard_Real First, |
284 | const Standard_Real Last, |
285 | const Standard_Boolean HasF, |
286 | const Standard_Boolean HasL, |
287 | const Standard_Real VFirst, |
288 | const Standard_Real VLast) |
289 | { |
290 | //Standard_Integer i; |
291 | Standard_Real Milieu = 0.5*(First+Last); |
292 | TColStd_Array1OfReal pol(1,5); |
293 | TColStd_Array1OfReal knot(1,3); |
294 | TColStd_Array1OfInteger mult(1,3); |
295 | knot(1) = First; knot(2) = Milieu; knot(3) = Last; |
296 | mult(1) = 4; mult(3) = 4; mult(2) = 1; |
297 | |
298 | Handle(Law_BSpline) bs; |
299 | if(HasF){ pol(1) = pol(2) = VFirst;} |
300 | else { pol(1) = pol(2) = 1.; } |
301 | if(HasL){ pol(4) = pol(5) = VLast;} |
302 | else { pol(4) = pol(5) = 1.; } |
303 | |
304 | pol(3) = 1.; |
305 | |
306 | bs = new Law_BSpline(pol,knot,mult,3); |
307 | return bs; |
308 | } |