7fd59977 |
1 | // xab : modified 15-Mar-94 added EvalBSplineMatrix, FactorBandedMatrix, SolveBandedSystem |
2 | // Eval, BuildCache, cacheD0, cacheD1, cacheD2, cacheD3, LocalMatrix, |
3 | // EvalPolynomial, RationalDerivatives. |
4 | // xab : 22-Mar-94 : fixed rational problem in BuildCache |
5 | // xab : 12-Mar-96 : added MovePointAndTangent |
6 | // |
7 | #include <TColStd_Array1OfInteger.hxx> |
8 | #include <TColStd_Array1OfReal.hxx> |
9 | #include <gp_Vec2d.hxx> |
10 | #include <Standard_ConstructionError.hxx> |
11 | #include <PLib.hxx> |
12 | #include <math_Matrix.hxx> |
13 | |
14 | #define No_Standard_RangeError |
15 | #define No_Standard_OutOfRange |
16 | |
17 | //======================================================================= |
18 | //struct : BSplCLib_DataContainer |
19 | //purpose: Auxiliary structure providing buffers for poles and knots used in |
20 | // evaluation of bspline (allocated in the stack) |
21 | //======================================================================= |
22 | |
23 | struct BSplCLib_DataContainer |
24 | { |
25 | BSplCLib_DataContainer(Standard_Integer Degree) |
26 | { |
27 | if ( Degree > BSplCLib::MaxDegree() || BSplCLib::MaxDegree() > 25 ) |
28 | Standard_OutOfRange::Raise ("BSplCLib: bspline degree is greater than maximum supported"); |
29 | } |
30 | |
31 | Standard_Real poles[(25+1)*(Dimension_gen+1)]; |
32 | Standard_Real knots[2*25]; |
33 | Standard_Real ders[Dimension_gen*4]; |
34 | }; |
35 | |
36 | //======================================================================= |
37 | //function : Reverse |
38 | //purpose : |
39 | //======================================================================= |
40 | |
41 | void BSplCLib::Reverse(Array1OfPoints& Poles, |
42 | const Standard_Integer L) |
43 | { |
44 | Standard_Integer i, l = L; |
45 | l = Poles.Lower() + (l-Poles.Lower()) % (Poles.Upper()-Poles.Lower()+1); |
46 | |
47 | Array1OfPoints temp(0,Poles.Length()-1); |
48 | |
49 | for (i = Poles.Lower(); i <= l; i++) |
50 | temp(l-i) = Poles(i); |
51 | |
52 | for (i = l+1; i <= Poles.Upper(); i++) |
53 | temp(l-Poles.Lower()+Poles.Upper()-i+1) = Poles(i); |
54 | |
55 | for (i = Poles.Lower(); i <= Poles.Upper(); i++) |
56 | Poles(i) = temp(i-Poles.Lower()); |
57 | } |
58 | |
59 | // |
60 | // CURVES MODIFICATIONS |
61 | // |
62 | |
63 | //======================================================================= |
64 | //function : RemoveKnot |
65 | //purpose : |
66 | //======================================================================= |
67 | |
68 | Standard_Boolean BSplCLib::RemoveKnot |
69 | (const Standard_Integer Index, |
70 | const Standard_Integer Mult, |
71 | const Standard_Integer Degree, |
72 | const Standard_Boolean Periodic, |
73 | const Array1OfPoints& Poles, |
74 | const TColStd_Array1OfReal& Weights, |
75 | const TColStd_Array1OfReal& Knots, |
76 | const TColStd_Array1OfInteger& Mults, |
77 | Array1OfPoints& NewPoles, |
78 | TColStd_Array1OfReal& NewWeights, |
79 | TColStd_Array1OfReal& NewKnots, |
80 | TColStd_Array1OfInteger& NewMults, |
81 | const Standard_Real Tolerance) |
82 | { |
83 | Standard_Boolean rational = &Weights != NULL; |
84 | Standard_Integer dim; |
85 | dim = Dimension_gen; |
86 | if (rational) dim++; |
87 | |
88 | TColStd_Array1OfReal poles(1,dim*Poles.Length()); |
89 | TColStd_Array1OfReal newpoles(1,dim*NewPoles.Length()); |
90 | |
91 | if (rational) PLib::SetPoles(Poles,Weights,poles); |
92 | else PLib::SetPoles(Poles,poles); |
93 | |
94 | if (!RemoveKnot(Index,Mult,Degree,Periodic,dim, |
95 | poles,Knots,Mults,newpoles,NewKnots,NewMults,Tolerance)) |
96 | return Standard_False; |
97 | |
98 | if (rational) PLib::GetPoles(newpoles,NewPoles,NewWeights); |
99 | else PLib::GetPoles(newpoles,NewPoles); |
100 | return Standard_True; |
101 | } |
102 | |
103 | //======================================================================= |
104 | //function : InsertKnots |
105 | //purpose : insert an array of knots and multiplicities |
106 | //======================================================================= |
107 | |
108 | void BSplCLib::InsertKnots |
109 | (const Standard_Integer Degree, |
110 | const Standard_Boolean Periodic, |
111 | const Array1OfPoints& Poles, |
112 | const TColStd_Array1OfReal& Weights, |
113 | const TColStd_Array1OfReal& Knots, |
114 | const TColStd_Array1OfInteger& Mults, |
115 | const TColStd_Array1OfReal& AddKnots, |
116 | const TColStd_Array1OfInteger& AddMults, |
117 | Array1OfPoints& NewPoles, |
118 | TColStd_Array1OfReal& NewWeights, |
119 | TColStd_Array1OfReal& NewKnots, |
120 | TColStd_Array1OfInteger& NewMults, |
121 | const Standard_Real Epsilon, |
122 | const Standard_Boolean Add) |
123 | { |
124 | Standard_Boolean rational = &Weights != NULL; |
125 | Standard_Integer dim; |
126 | dim = Dimension_gen; |
127 | if (rational) dim++; |
128 | |
129 | TColStd_Array1OfReal poles(1,dim*Poles.Length()); |
130 | TColStd_Array1OfReal newpoles(1,dim*NewPoles.Length()); |
131 | |
132 | if (rational) PLib::SetPoles(Poles,Weights,poles); |
133 | else PLib::SetPoles(Poles,poles); |
134 | |
135 | InsertKnots(Degree,Periodic,dim,poles,Knots,Mults, |
136 | AddKnots,AddMults,newpoles,NewKnots,NewMults,Epsilon,Add); |
137 | |
138 | if (rational) PLib::GetPoles(newpoles,NewPoles,NewWeights); |
139 | else PLib::GetPoles(newpoles,NewPoles); |
140 | } |
141 | |
142 | //======================================================================= |
143 | //function : InsertKnot |
144 | //purpose : |
145 | //======================================================================= |
146 | |
147 | void BSplCLib::InsertKnot(const Standard_Integer , |
148 | const Standard_Real U, |
149 | const Standard_Integer UMult, |
150 | const Standard_Integer Degree, |
151 | const Standard_Boolean Periodic, |
152 | const Array1OfPoints& Poles, |
153 | const TColStd_Array1OfReal& Weights, |
154 | const TColStd_Array1OfReal& Knots, |
155 | const TColStd_Array1OfInteger& Mults, |
156 | Array1OfPoints& NewPoles, |
157 | TColStd_Array1OfReal& NewWeights) |
158 | { |
159 | TColStd_Array1OfReal k(1,1); |
160 | k(1) = U; |
161 | TColStd_Array1OfInteger m(1,1); |
162 | m(1) = UMult; |
163 | TColStd_Array1OfReal nk(1,Knots.Length()+1); |
164 | TColStd_Array1OfInteger nm(1,Knots.Length()+1); |
165 | InsertKnots(Degree,Periodic,Poles,Weights,Knots,Mults, |
166 | k,m,NewPoles,NewWeights,nk,nm,Epsilon(U)); |
167 | } |
168 | |
169 | //======================================================================= |
170 | //function : RaiseMultiplicity |
171 | //purpose : |
172 | //======================================================================= |
173 | |
174 | void BSplCLib::RaiseMultiplicity(const Standard_Integer KnotIndex, |
175 | const Standard_Integer Mult, |
176 | const Standard_Integer Degree, |
177 | const Standard_Boolean Periodic, |
178 | const Array1OfPoints& Poles, |
179 | const TColStd_Array1OfReal& Weights, |
180 | const TColStd_Array1OfReal& Knots, |
181 | const TColStd_Array1OfInteger& Mults, |
182 | Array1OfPoints& NewPoles, |
183 | TColStd_Array1OfReal& NewWeights) |
184 | { |
185 | TColStd_Array1OfReal k(1,1); |
186 | k(1) = Knots(KnotIndex); |
187 | TColStd_Array1OfInteger m(1,1); |
188 | m(1) = Mult - Mults(KnotIndex); |
189 | TColStd_Array1OfReal nk(1,Knots.Length()); |
190 | TColStd_Array1OfInteger nm(1,Knots.Length()); |
191 | InsertKnots(Degree,Periodic,Poles,Weights,Knots,Mults, |
192 | k,m,NewPoles,NewWeights,nk,nm,Epsilon(k(1))); |
193 | } |
194 | |
195 | //======================================================================= |
196 | //function : IncreaseDegree |
197 | //purpose : |
198 | //======================================================================= |
199 | |
200 | void BSplCLib::IncreaseDegree |
201 | (const Standard_Integer Degree, |
202 | const Standard_Integer NewDegree, |
203 | const Standard_Boolean Periodic, |
204 | const Array1OfPoints& Poles, |
205 | const TColStd_Array1OfReal& Weights, |
206 | const TColStd_Array1OfReal& Knots, |
207 | const TColStd_Array1OfInteger& Mults, |
208 | Array1OfPoints& NewPoles, |
209 | TColStd_Array1OfReal& NewWeights, |
210 | TColStd_Array1OfReal& NewKnots, |
211 | TColStd_Array1OfInteger& NewMults) |
212 | { |
213 | Standard_Boolean rational = &Weights != NULL; |
214 | Standard_Integer dim; |
215 | dim = Dimension_gen; |
216 | if (rational) dim++; |
217 | |
218 | TColStd_Array1OfReal poles(1,dim*Poles.Length()); |
219 | TColStd_Array1OfReal newpoles(1,dim*NewPoles.Length()); |
220 | |
221 | if (rational) PLib::SetPoles(Poles,Weights,poles); |
222 | else PLib::SetPoles(Poles,poles); |
223 | |
224 | IncreaseDegree(Degree,NewDegree,Periodic,dim,poles,Knots,Mults, |
225 | newpoles,NewKnots,NewMults); |
226 | |
227 | if (rational) PLib::GetPoles(newpoles,NewPoles,NewWeights); |
228 | else PLib::GetPoles(newpoles,NewPoles); |
229 | } |
230 | |
231 | //======================================================================= |
232 | //function : Unperiodize |
233 | //purpose : |
234 | //======================================================================= |
235 | |
236 | void BSplCLib::Unperiodize |
237 | (const Standard_Integer Degree, |
238 | const TColStd_Array1OfInteger& Mults, |
239 | const TColStd_Array1OfReal& Knots, |
240 | const Array1OfPoints& Poles, |
241 | const TColStd_Array1OfReal& Weights, |
242 | TColStd_Array1OfInteger& NewMults, |
243 | TColStd_Array1OfReal& NewKnots, |
244 | Array1OfPoints& NewPoles, |
245 | TColStd_Array1OfReal& NewWeights) |
246 | { |
247 | Standard_Boolean rational = &Weights != NULL; |
248 | Standard_Integer dim; |
249 | dim = Dimension_gen; |
250 | if (rational) dim++; |
251 | |
252 | TColStd_Array1OfReal poles(1,dim*Poles.Length()); |
253 | TColStd_Array1OfReal newpoles(1,dim*NewPoles.Length()); |
254 | |
255 | if (rational) PLib::SetPoles(Poles,Weights,poles); |
256 | else PLib::SetPoles(Poles,poles); |
257 | |
258 | Unperiodize(Degree, dim, Mults, Knots, poles, |
259 | NewMults,NewKnots, newpoles); |
260 | |
261 | if (rational) PLib::GetPoles(newpoles,NewPoles,NewWeights); |
262 | else PLib::GetPoles(newpoles,NewPoles); |
263 | } |
264 | |
265 | //======================================================================= |
266 | //function : Trimming |
267 | //purpose : |
268 | //======================================================================= |
269 | |
270 | void BSplCLib::Trimming(const Standard_Integer Degree, |
271 | const Standard_Boolean Periodic, |
272 | const TColStd_Array1OfReal& Knots, |
273 | const TColStd_Array1OfInteger& Mults, |
274 | const Array1OfPoints& Poles, |
275 | const TColStd_Array1OfReal& Weights, |
276 | const Standard_Real U1, |
277 | const Standard_Real U2, |
278 | TColStd_Array1OfReal& NewKnots, |
279 | TColStd_Array1OfInteger& NewMults, |
280 | Array1OfPoints& NewPoles, |
281 | TColStd_Array1OfReal& NewWeights) |
282 | { |
283 | Standard_Boolean rational = &Weights != NULL; |
284 | Standard_Integer dim; |
285 | dim = Dimension_gen; |
286 | if (rational) dim++; |
287 | |
288 | TColStd_Array1OfReal poles(1,dim*Poles.Length()); |
289 | TColStd_Array1OfReal newpoles(1,dim*NewPoles.Length()); |
290 | |
291 | if (rational) PLib::SetPoles(Poles,Weights,poles); |
292 | else PLib::SetPoles(Poles,poles); |
293 | |
294 | Trimming(Degree, Periodic, dim, Knots, Mults, poles, U1, U2, |
295 | NewKnots, NewMults, newpoles); |
296 | |
297 | if (rational) PLib::GetPoles(newpoles,NewPoles,NewWeights); |
298 | else PLib::GetPoles(newpoles,NewPoles); |
299 | } |
300 | |
301 | //-------------------------------------------------------------------------- |
302 | //ELEMENTARY COMPUTATIONS |
303 | //-------------------------------------------------------------------------- |
304 | // |
305 | // All the following methods are methods of low level used in BSplCLib |
306 | // but also in BSplSLib (but not in Geom) |
307 | // At the creation time of this package there is no possibility |
308 | // to declare private methods of package and to declare friend |
309 | // methods of package. It could interesting to declare that BSplSLib |
310 | // is a package friend to BSplCLib because it uses all the basis methods |
311 | // of BSplCLib. |
312 | //-------------------------------------------------------------------------- |
313 | |
314 | //======================================================================= |
315 | //function : BuildEval |
316 | //purpose : builds the local array for evaluation |
317 | //======================================================================= |
318 | |
319 | void BSplCLib::BuildEval(const Standard_Integer Degree, |
320 | const Standard_Integer Index, |
321 | const Array1OfPoints& Poles, |
322 | const TColStd_Array1OfReal& Weights, |
323 | Standard_Real& LP) |
324 | { |
325 | Standard_Real w, *pole = &LP; |
326 | Standard_Integer PLower = Poles.Lower(); |
327 | Standard_Integer PUpper = Poles.Upper(); |
328 | Standard_Integer i; |
329 | Standard_Integer ip = PLower + Index - 1; |
330 | if (&Weights == NULL) { |
331 | for (i = 0; i <= Degree; i++) { |
332 | ip++; |
333 | if (ip > PUpper) ip = PLower; |
334 | const Point& P = Poles(ip); |
335 | PointToCoords (pole,P,+0); |
336 | pole += Dimension_gen; |
337 | } |
338 | } |
339 | else { |
340 | for (i = 0; i <= Degree; i++) { |
341 | ip++; |
342 | if (ip > PUpper) ip = PLower; |
343 | const Point& P = Poles(ip); |
344 | pole[Dimension_gen] = w = Weights(ip); |
345 | PointToCoords (pole, P, * w); |
346 | pole += Dimension_gen + 1; |
347 | } |
348 | } |
349 | } |
350 | |
351 | //======================================================================= |
352 | //function : PrepareEval |
353 | //purpose : stores data for Eval in the local arrays |
354 | // dc.poles and dc.knots |
355 | //======================================================================= |
356 | |
357 | static void PrepareEval |
358 | (Standard_Real& u, |
359 | Standard_Integer& index, |
360 | Standard_Integer& dim, |
361 | Standard_Boolean& rational, |
362 | const Standard_Integer Degree, |
363 | const Standard_Boolean Periodic, |
364 | const Array1OfPoints& Poles, |
365 | const TColStd_Array1OfReal& Weights, |
366 | const TColStd_Array1OfReal& Knots, |
367 | const TColStd_Array1OfInteger& Mults, |
368 | BSplCLib_DataContainer& dc) |
369 | { |
370 | // Set the Index |
371 | BSplCLib::LocateParameter(Degree,Knots,Mults,u,Periodic,index,u); |
372 | |
373 | // make the knots |
374 | BSplCLib::BuildKnots(Degree,index,Periodic,Knots,Mults,*dc.knots); |
375 | if (&Mults == NULL) |
376 | index -= Knots.Lower() + Degree; |
377 | else |
378 | index = BSplCLib::PoleIndex(Degree,index,Periodic,Mults); |
379 | |
380 | // check truly rational |
381 | rational = (&Weights != NULL); |
382 | if (rational) { |
383 | Standard_Integer WLower = Weights.Lower() + index; |
384 | rational = BSplCLib::IsRational(Weights, WLower, WLower + Degree); |
385 | } |
386 | |
387 | // make the poles |
388 | if (rational) { |
389 | dim = Dimension_gen + 1; |
390 | BSplCLib::BuildEval(Degree, index, Poles, Weights , *dc.poles); |
391 | } |
392 | else { |
393 | dim = Dimension_gen; |
394 | BSplCLib::BuildEval(Degree, index, Poles, BSplCLib::NoWeights(), *dc.poles); |
395 | } |
396 | } |
397 | |
398 | //======================================================================= |
399 | //function : D0 |
400 | //purpose : |
401 | //======================================================================= |
402 | |
403 | void BSplCLib::D0 |
404 | (const Standard_Real U, |
405 | const Standard_Integer Index, |
406 | const Standard_Integer Degree, |
407 | const Standard_Boolean Periodic, |
408 | const Array1OfPoints& Poles, |
409 | const TColStd_Array1OfReal& Weights, |
410 | const TColStd_Array1OfReal& Knots, |
411 | const TColStd_Array1OfInteger& Mults, |
412 | Point& P) |
413 | { |
414 | // Standard_Integer k,dim,index = Index; |
415 | Standard_Integer dim,index = Index; |
416 | Standard_Real u = U; |
417 | Standard_Boolean rational; |
418 | BSplCLib_DataContainer dc(Degree); |
419 | PrepareEval(u,index,dim,rational,Degree,Periodic,Poles,Weights,Knots,Mults,dc); |
420 | BSplCLib::Eval(u,Degree,*dc.knots,dim,*dc.poles); |
421 | |
422 | if (rational) { |
423 | Standard_Real w = dc.poles[Dimension_gen]; |
424 | CoordsToPoint (P, dc.poles, / w); |
425 | } |
426 | else |
427 | CoordsToPoint (P, dc.poles, ); |
428 | } |
429 | |
430 | //======================================================================= |
431 | //function : D1 |
432 | //purpose : |
433 | //======================================================================= |
434 | |
435 | void BSplCLib::D1 |
436 | (const Standard_Real U, |
437 | const Standard_Integer Index, |
438 | const Standard_Integer Degree, |
439 | const Standard_Boolean Periodic, |
440 | const Array1OfPoints& Poles, |
441 | const TColStd_Array1OfReal& Weights, |
442 | const TColStd_Array1OfReal& Knots, |
443 | const TColStd_Array1OfInteger& Mults, |
444 | Point& P, |
445 | Vector& V) |
446 | { |
447 | Standard_Integer dim,index = Index; |
448 | Standard_Real u = U; |
449 | Standard_Boolean rational; |
450 | BSplCLib_DataContainer dc(Degree); |
451 | PrepareEval(u,index,dim,rational,Degree,Periodic,Poles,Weights,Knots,Mults,dc); |
452 | BSplCLib::Bohm(u,Degree,1,*dc.knots,dim,*dc.poles); |
453 | Standard_Real *result = dc.poles; |
454 | if (rational) { |
455 | PLib::RationalDerivative(Degree,1,Dimension_gen,*dc.poles,*dc.ders); |
456 | result = dc.ders; |
457 | } |
458 | |
459 | CoordsToPoint (P, result, ); |
460 | CoordsToPoint (V, result + Dimension_gen, ); |
461 | } |
462 | |
463 | //======================================================================= |
464 | //function : D2 |
465 | //purpose : |
466 | //======================================================================= |
467 | |
468 | void BSplCLib::D2 |
469 | (const Standard_Real U, |
470 | const Standard_Integer Index, |
471 | const Standard_Integer Degree, |
472 | const Standard_Boolean Periodic, |
473 | const Array1OfPoints& Poles, |
474 | const TColStd_Array1OfReal& Weights, |
475 | const TColStd_Array1OfReal& Knots, |
476 | const TColStd_Array1OfInteger& Mults, |
477 | Point& P, |
478 | Vector& V1, |
479 | Vector& V2) |
480 | { |
481 | Standard_Integer dim,index = Index; |
482 | Standard_Real u = U; |
483 | Standard_Boolean rational; |
484 | BSplCLib_DataContainer dc(Degree); |
485 | PrepareEval(u,index,dim,rational,Degree,Periodic,Poles,Weights,Knots,Mults,dc); |
486 | BSplCLib::Bohm(u,Degree,2,*dc.knots,dim,*dc.poles); |
487 | Standard_Real *result = dc.poles; |
488 | if (rational) { |
489 | PLib::RationalDerivative(Degree,2,Dimension_gen,*dc.poles,*dc.ders); |
490 | result = dc.ders; |
491 | } |
492 | |
493 | CoordsToPoint (P, result, ); |
494 | CoordsToPoint (V1, result + Dimension_gen, ); |
495 | if (!rational && (Degree < 2)) |
496 | NullifyPoint (V2); |
497 | else |
498 | CoordsToPoint (V2, result + 2 * Dimension_gen, ); |
499 | } |
500 | |
501 | //======================================================================= |
502 | //function : D3 |
503 | //purpose : |
504 | //======================================================================= |
505 | |
506 | void BSplCLib::D3 |
507 | (const Standard_Real U, |
508 | const Standard_Integer Index, |
509 | const Standard_Integer Degree, |
510 | const Standard_Boolean Periodic, |
511 | const Array1OfPoints& Poles, |
512 | const TColStd_Array1OfReal& Weights, |
513 | const TColStd_Array1OfReal& Knots, |
514 | const TColStd_Array1OfInteger& Mults, |
515 | Point& P, |
516 | Vector& V1, |
517 | Vector& V2, |
518 | Vector& V3) |
519 | { |
520 | Standard_Integer dim,index = Index; |
521 | Standard_Real u = U; |
522 | Standard_Boolean rational; |
523 | BSplCLib_DataContainer dc(Degree); |
524 | PrepareEval(u,index,dim,rational,Degree,Periodic,Poles,Weights,Knots,Mults,dc); |
525 | BSplCLib::Bohm(u,Degree,3,*dc.knots,dim,*dc.poles); |
526 | Standard_Real *result = dc.poles; |
527 | if (rational) { |
528 | PLib::RationalDerivative(Degree,3,Dimension_gen,*dc.poles,*dc.ders); |
529 | result = dc.ders; |
530 | } |
531 | |
532 | CoordsToPoint (P, result, ); |
533 | CoordsToPoint (V1, result + Dimension_gen, ); |
534 | if (!rational && (Degree < 2)) |
535 | NullifyPoint (V2); |
536 | else |
537 | CoordsToPoint (V2, result + 2 * Dimension_gen, ); |
538 | if (!rational && (Degree < 3)) |
539 | NullifyPoint (V3); |
540 | else |
541 | CoordsToPoint (V3, result + 3 * Dimension_gen, ); |
542 | } |
543 | |
544 | //======================================================================= |
545 | //function : DN |
546 | //purpose : |
547 | //======================================================================= |
548 | |
549 | void BSplCLib::DN |
550 | (const Standard_Real U, |
551 | const Standard_Integer N, |
552 | const Standard_Integer Index, |
553 | const Standard_Integer Degree, |
554 | const Standard_Boolean Periodic, |
555 | const Array1OfPoints& Poles, |
556 | const TColStd_Array1OfReal& Weights, |
557 | const TColStd_Array1OfReal& Knots, |
558 | const TColStd_Array1OfInteger& Mults, |
559 | Vector& VN) |
560 | { |
561 | Standard_Integer dim,index = Index; |
562 | Standard_Real u = U; |
563 | Standard_Boolean rational; |
564 | BSplCLib_DataContainer dc(Degree); |
565 | PrepareEval(u,index,dim,rational,Degree,Periodic,Poles,Weights,Knots,Mults,dc); |
566 | BSplCLib::Bohm(u,Degree,N,*dc.knots,dim,*dc.poles); |
567 | |
568 | if (rational) { |
569 | Standard_Real v[Dimension_gen]; |
570 | PLib::RationalDerivative(Degree,N,Dimension_gen,*dc.poles,v[0],Standard_False); |
571 | CoordsToPoint (VN, v, ); |
572 | } |
573 | else { |
574 | if (N > Degree) |
575 | NullifyPoint (VN); |
576 | else { |
577 | Standard_Real *DN = dc.poles + N * Dimension_gen; |
578 | CoordsToPoint (VN, DN, ); |
579 | } |
580 | } |
581 | } |
582 | |
583 | //======================================================================= |
584 | //function : Solves a LU factored Matrix |
585 | //purpose : |
586 | //======================================================================= |
587 | |
588 | Standard_Integer |
589 | BSplCLib::SolveBandedSystem(const math_Matrix& Matrix, |
590 | const Standard_Integer UpperBandWidth, |
591 | const Standard_Integer LowerBandWidth, |
592 | Array1OfPoints& PolesArray) |
593 | { |
594 | Standard_Real *PArray ; |
595 | PArray = (Standard_Real *) &PolesArray(PolesArray.Lower()) ; |
596 | |
597 | return BSplCLib::SolveBandedSystem(Matrix, |
598 | UpperBandWidth, |
599 | LowerBandWidth, |
600 | Dimension_gen, |
601 | PArray[0]) ; |
602 | } |
603 | |
604 | //======================================================================= |
605 | //function : Solves a LU factored Matrix |
606 | //purpose : if HomogeneousFlag is 1 then the input and the output |
607 | // will be homogeneous that is no division or multiplication |
608 | // by weigths will happen. On the contrary if HomogeneousFlag |
609 | // is 0 then the poles will be multiplied first by the weights |
610 | // and after interpolation they will be devided by the weights |
611 | //======================================================================= |
612 | |
613 | Standard_Integer |
614 | BSplCLib::SolveBandedSystem(const math_Matrix& Matrix, |
615 | const Standard_Integer UpperBandWidth, |
616 | const Standard_Integer LowerBandWidth, |
617 | const Standard_Boolean HomogeneousFlag, |
618 | Array1OfPoints& PolesArray, |
619 | TColStd_Array1OfReal& WeightsArray) |
620 | { |
621 | Standard_Real *PArray, |
622 | * WArray ; |
623 | PArray = (Standard_Real *) &PolesArray(PolesArray.Lower()) ; |
624 | WArray = (Standard_Real *) &WeightsArray(WeightsArray.Lower()) ; |
625 | return BSplCLib::SolveBandedSystem(Matrix, |
626 | UpperBandWidth, |
627 | LowerBandWidth, |
628 | HomogeneousFlag, |
629 | Dimension_gen, |
630 | PArray[0], |
631 | WArray[0]) ; |
632 | } |
633 | |
634 | //======================================================================= |
635 | //function : Evaluates a Bspline function : uses the ExtrapMode |
636 | //purpose : the function is extrapolated using the Taylor expansion |
637 | // of degree ExtrapMode[0] to the left and the Taylor |
638 | // expansion of degree ExtrapMode[1] to the right |
639 | // if the HomogeneousFlag == True than the Poles are supposed |
640 | // to be stored homogeneously and the result will also be homogeneous |
641 | // Valid only if Weights |
642 | //======================================================================= |
643 | void BSplCLib::Eval(const Standard_Real Parameter, |
644 | const Standard_Boolean PeriodicFlag, |
645 | const Standard_Boolean HomogeneousFlag, |
646 | Standard_Integer& ExtrapMode, |
647 | const Standard_Integer Degree, |
648 | const TColStd_Array1OfReal& FlatKnots, |
649 | const Array1OfPoints& PolesArray, |
650 | const TColStd_Array1OfReal& WeightsArray, |
651 | Point& aPoint, |
652 | Standard_Real& aWeight) |
653 | { |
654 | Standard_Real Inverse, |
655 | P[Dimension_gen], |
656 | *PArray, |
657 | *WArray ; |
658 | Standard_Integer kk ; |
659 | PArray = (Standard_Real *) &PolesArray(PolesArray.Lower()) ; |
660 | WArray = (Standard_Real *) &WeightsArray(WeightsArray.Lower()) ; |
661 | if (HomogeneousFlag) { |
662 | BSplCLib::Eval(Parameter, |
663 | PeriodicFlag, |
664 | 0, |
665 | ExtrapMode, |
666 | Degree, |
667 | FlatKnots, |
668 | Dimension_gen, |
669 | PArray[0], |
670 | P[0]) ; |
671 | BSplCLib::Eval(Parameter, |
672 | PeriodicFlag, |
673 | 0, |
674 | ExtrapMode, |
675 | Degree, |
676 | FlatKnots, |
677 | 1, |
678 | WArray[0], |
679 | aWeight) ; |
680 | } |
681 | else { |
682 | BSplCLib::Eval(Parameter, |
683 | PeriodicFlag, |
684 | 0, |
685 | ExtrapMode, |
686 | Degree, |
687 | FlatKnots, |
688 | Dimension_gen, |
689 | PArray[0], |
690 | WArray[0], |
691 | P[0], |
692 | aWeight) ; |
693 | Inverse = 1.0e0 / aWeight ; |
694 | |
695 | for (kk = 0 ; kk < Dimension_gen ; kk++) { |
696 | P[kk] *= Inverse ; |
697 | } |
698 | } |
699 | |
700 | for (kk = 0 ; kk < Dimension_gen ; kk++) |
701 | aPoint.SetCoord(kk + 1, P[kk]) ; |
702 | } |
703 | |
704 | //======================================================================= |
705 | //function : CacheD0 |
706 | //purpose : Evaluates the polynomial cache of the Bspline Curve |
707 | // |
708 | //======================================================================= |
709 | void BSplCLib::CacheD0(const Standard_Real Parameter, |
710 | const Standard_Integer Degree, |
711 | const Standard_Real CacheParameter, |
712 | const Standard_Real SpanLenght, |
713 | const Array1OfPoints& PolesArray, |
714 | const TColStd_Array1OfReal& WeightsArray, |
715 | Point& aPoint) |
716 | { |
717 | // |
718 | // the CacheParameter is where the cache polynomial was evaluated in homogeneous |
719 | // form |
720 | // the SpanLenght is the normalizing factor so that the polynomial is between |
721 | // 0 and 1 |
722 | Standard_Real NewParameter, Inverse; |
723 | Standard_Real * PArray = (Standard_Real *) &(PolesArray(PolesArray.Lower())) ; |
724 | Standard_Real * myPoint = (Standard_Real *) &aPoint ; |
725 | NewParameter = (Parameter - CacheParameter) / SpanLenght ; |
726 | PLib::NoDerivativeEvalPolynomial(NewParameter, |
727 | Degree, |
728 | Dimension_gen, |
729 | Degree * Dimension_gen, |
730 | PArray[0], |
731 | myPoint[0]) ; |
732 | if (&WeightsArray != NULL) { |
733 | Standard_Real * |
734 | WArray = (Standard_Real *) &WeightsArray(WeightsArray.Lower()) ; |
735 | PLib::NoDerivativeEvalPolynomial(NewParameter, |
736 | Degree, |
737 | 1, |
738 | Degree, |
739 | WArray[0], |
740 | Inverse) ; |
741 | |
742 | Inverse = 1.0e0 / Inverse; |
743 | ModifyCoords (myPoint, *= Inverse); |
744 | } |
745 | } |
746 | |
747 | //======================================================================= |
748 | //function : CacheD1 |
749 | //purpose : Evaluates the polynomial cache of the Bspline Curve |
750 | // |
751 | //======================================================================= |
752 | void BSplCLib::CacheD1(const Standard_Real Parameter, |
753 | const Standard_Integer Degree, |
754 | const Standard_Real CacheParameter, |
755 | const Standard_Real SpanLenght, |
756 | const Array1OfPoints& PolesArray, |
757 | const TColStd_Array1OfReal& WeightsArray, |
758 | Point& aPoint, |
759 | Vector& aVector) |
760 | { |
761 | // |
762 | // the CacheParameter is where the cache polynomial was evaluated in homogeneous |
763 | // form |
764 | // the SpanLenght is the normalizing factor so that the polynomial is between |
765 | // 0 and 1 |
766 | Standard_Real LocalPDerivatives[Dimension_gen << 1] ; |
767 | // Standard_Real LocalWDerivatives[2], NewParameter, Inverse ; |
768 | Standard_Real LocalWDerivatives[2], NewParameter ; |
769 | |
770 | Standard_Real * |
771 | PArray = (Standard_Real *) &(PolesArray(PolesArray.Lower())) ; |
772 | Standard_Real * |
773 | myPoint = (Standard_Real *) &aPoint ; |
774 | Standard_Real * |
775 | myVector = (Standard_Real *) &aVector ; |
776 | NewParameter = (Parameter - CacheParameter) / SpanLenght ; |
777 | PLib::EvalPolynomial(NewParameter, |
778 | 1, |
779 | Degree, |
780 | Dimension_gen, |
781 | PArray[0], |
782 | LocalPDerivatives[0]) ; |
783 | // |
784 | // unormalize derivatives since those are computed normalized |
785 | // |
786 | |
787 | ModifyCoords (LocalPDerivatives + Dimension_gen, /= SpanLenght); |
788 | |
789 | if (&WeightsArray != NULL) { |
790 | Standard_Real * |
791 | WArray = (Standard_Real *) &WeightsArray(WeightsArray.Lower()) ; |
792 | PLib::EvalPolynomial(NewParameter, |
793 | 1, |
794 | Degree, |
795 | 1, |
796 | WArray[0], |
797 | LocalWDerivatives[0]) ; |
798 | // |
799 | // unormalize the result since the polynomial stored in the cache |
800 | // is normalized between 0 and 1 |
801 | // |
802 | LocalWDerivatives[1] /= SpanLenght ; |
803 | |
804 | PLib::RationalDerivatives(1, |
805 | Dimension_gen, |
806 | LocalPDerivatives[0], |
807 | LocalWDerivatives[0], |
808 | LocalPDerivatives[0]) ; |
809 | } |
810 | |
811 | CopyCoords (myPoint, LocalPDerivatives); |
812 | CopyCoords (myVector, LocalPDerivatives + Dimension_gen); |
813 | } |
814 | |
815 | //======================================================================= |
816 | //function : CacheD2 |
817 | //purpose : Evaluates the polynomial cache of the Bspline Curve |
818 | // |
819 | //======================================================================= |
820 | void BSplCLib::CacheD2(const Standard_Real Parameter, |
821 | const Standard_Integer Degree, |
822 | const Standard_Real CacheParameter, |
823 | const Standard_Real SpanLenght, |
824 | const Array1OfPoints& PolesArray, |
825 | const TColStd_Array1OfReal& WeightsArray, |
826 | Point& aPoint, |
827 | Vector& aVector1, |
828 | Vector& aVector2) |
829 | { |
830 | // |
831 | // the CacheParameter is where the cache polynomial was evaluated in homogeneous |
832 | // form |
833 | // the SpanLenght is the normalizing factor so that the polynomial is between |
834 | // 0 and 1 |
835 | Standard_Integer ii,Index,EndIndex; |
836 | Standard_Real LocalPDerivatives[(Dimension_gen << 1) + Dimension_gen] ; |
837 | // Standard_Real LocalWDerivatives[3], NewParameter, Factor, Inverse ; |
838 | Standard_Real LocalWDerivatives[3], NewParameter, Factor ; |
839 | Standard_Real * PArray = (Standard_Real *) &(PolesArray(PolesArray.Lower())) ; |
840 | Standard_Real * myPoint = (Standard_Real *) &aPoint ; |
841 | Standard_Real * myVector1 = (Standard_Real *) &aVector1 ; |
842 | Standard_Real * myVector2 = (Standard_Real *) &aVector2 ; |
843 | NewParameter = (Parameter - CacheParameter) / SpanLenght ; |
844 | PLib::EvalPolynomial(NewParameter, |
845 | 2, |
846 | Degree, |
847 | Dimension_gen, |
848 | PArray[0], |
849 | LocalPDerivatives[0]) ; |
850 | // |
851 | // unormalize derivatives since those are computed normalized |
852 | // |
853 | Factor = 1.0e0/ SpanLenght ; |
854 | Index = Dimension_gen ; |
855 | EndIndex = Min (2, Degree) ; |
856 | |
857 | for (ii = 1 ; ii <= EndIndex ; ii++) { |
858 | ModifyCoords (LocalPDerivatives + Index, *= Factor); |
859 | Factor /= SpanLenght ; |
860 | Index += Dimension_gen; |
861 | } |
862 | |
863 | Index = (Degree + 1) * Dimension_gen; |
864 | for (ii = Degree ; ii < 2 ; ii++) { |
865 | NullifyCoords (LocalPDerivatives + Index); |
866 | Index += Dimension_gen; |
867 | } |
868 | |
869 | if (&WeightsArray != NULL) { |
870 | Standard_Real * |
871 | WArray = (Standard_Real *) &WeightsArray(WeightsArray.Lower()) ; |
872 | |
873 | PLib::EvalPolynomial(NewParameter, |
874 | 2, |
875 | Degree, |
876 | 1, |
877 | WArray[0], |
878 | LocalWDerivatives[0]) ; |
879 | |
880 | for (ii = Degree + 1 ; ii <= 2 ; ii++) { |
881 | LocalWDerivatives[ii] = 0.0e0 ; |
882 | } |
883 | // |
884 | // unormalize the result since the polynomial stored in the cache |
885 | // is normalized between 0 and 1 |
886 | // |
887 | Factor = 1.0e0 / SpanLenght ; |
888 | |
889 | for (ii = 1 ; ii <= EndIndex ; ii++) { |
890 | LocalWDerivatives[ii] *= Factor ; |
891 | Factor /= SpanLenght ; |
892 | } |
893 | PLib::RationalDerivatives(2, |
894 | Dimension_gen, |
895 | LocalPDerivatives[0], |
896 | LocalWDerivatives[0], |
897 | LocalPDerivatives[0]) ; |
898 | } |
899 | |
900 | CopyCoords (myPoint, LocalPDerivatives); |
901 | CopyCoords (myVector1, LocalPDerivatives + Dimension_gen); |
902 | CopyCoords (myVector2, LocalPDerivatives + Dimension_gen * 2); |
903 | } |
904 | |
905 | //======================================================================= |
906 | //function : CacheD3 |
907 | //purpose : Evaluates the polynomial cache of the Bspline Curve |
908 | // |
909 | //======================================================================= |
910 | void BSplCLib::CacheD3(const Standard_Real Parameter, |
911 | const Standard_Integer Degree, |
912 | const Standard_Real CacheParameter, |
913 | const Standard_Real SpanLenght, |
914 | const Array1OfPoints& PolesArray, |
915 | const TColStd_Array1OfReal& WeightsArray, |
916 | Point& aPoint, |
917 | Vector& aVector1, |
918 | Vector& aVector2, |
919 | Vector& aVector3) |
920 | { |
921 | // |
922 | // the CacheParameter is where the cache polynomial was evaluated in homogeneous |
923 | // form |
924 | // the SpanLenght is the normalizing factor so that the polynomial is between |
925 | // 0 and 1 |
926 | Standard_Integer ii, Index, EndIndex; |
927 | Standard_Real LocalPDerivatives[Dimension_gen << 2] ; |
928 | // Standard_Real LocalWDerivatives[4], Factor, NewParameter, Inverse ; |
929 | Standard_Real LocalWDerivatives[4], Factor, NewParameter ; |
930 | Standard_Real * PArray = (Standard_Real *) &(PolesArray(PolesArray.Lower())) ; |
931 | Standard_Real * myPoint = (Standard_Real *) &aPoint ; |
932 | Standard_Real * myVector1 = (Standard_Real *) &aVector1 ; |
933 | Standard_Real * myVector2 = (Standard_Real *) &aVector2 ; |
934 | Standard_Real * myVector3 = (Standard_Real *) &aVector3 ; |
935 | NewParameter = (Parameter - CacheParameter) / SpanLenght ; |
936 | PLib::EvalPolynomial(NewParameter, |
937 | 3, |
938 | Degree, |
939 | Dimension_gen, |
940 | PArray[0], |
941 | LocalPDerivatives[0]) ; |
942 | |
943 | Index = (Degree + 1) * Dimension_gen; |
944 | for (ii = Degree ; ii < 3 ; ii++) { |
945 | NullifyCoords (LocalPDerivatives + Index); |
946 | Index += Dimension_gen; |
947 | } |
948 | |
949 | // |
950 | // unormalize derivatives since those are computed normalized |
951 | // |
952 | Factor = 1.0e0 / SpanLenght ; |
953 | Index = Dimension_gen ; |
954 | EndIndex = Min(3,Degree) ; |
955 | |
956 | for (ii = 1 ; ii <= EndIndex ; ii++) { |
957 | ModifyCoords (LocalPDerivatives + Index, *= Factor); |
958 | Factor /= SpanLenght; |
959 | Index += Dimension_gen; |
960 | } |
961 | |
962 | if (&WeightsArray != NULL) { |
963 | Standard_Real * |
964 | WArray = (Standard_Real *) &WeightsArray(WeightsArray.Lower()) ; |
965 | |
966 | PLib::EvalPolynomial(NewParameter, |
967 | 3, |
968 | Degree, |
969 | 1, |
970 | WArray[0], |
971 | LocalWDerivatives[0]) ; |
972 | // |
973 | // unormalize the result since the polynomial stored in the cache |
974 | // is normalized between 0 and 1 |
975 | // |
976 | Factor = 1.0e0 / SpanLenght ; |
977 | |
978 | for (ii = 1 ; ii <= EndIndex ; ii++) { |
979 | LocalWDerivatives[ii] *= Factor ; |
980 | Factor /= SpanLenght ; |
981 | } |
982 | |
983 | for (ii = (Degree + 1) ; ii <= 3 ; ii++) { |
984 | LocalWDerivatives[ii] = 0.0e0 ; |
985 | } |
986 | PLib::RationalDerivatives(3, |
987 | Dimension_gen, |
988 | LocalPDerivatives[0], |
989 | LocalWDerivatives[0], |
990 | LocalPDerivatives[0]) ; |
991 | } |
992 | |
993 | CopyCoords (myPoint, LocalPDerivatives); |
994 | CopyCoords (myVector1, LocalPDerivatives + Dimension_gen); |
995 | CopyCoords (myVector2, LocalPDerivatives + Dimension_gen * 2); |
996 | CopyCoords (myVector3, LocalPDerivatives + Dimension_gen * 3); |
997 | } |
998 | |
999 | //======================================================================= |
1000 | //function : BuildCache |
1001 | //purpose : Stores theTaylor expansion normalized between 0,1 in the |
1002 | // Cache : in case of a rational function the Poles are |
1003 | // stored in homogeneous form |
1004 | //======================================================================= |
1005 | |
1006 | void BSplCLib::BuildCache |
1007 | (const Standard_Real U, |
1008 | const Standard_Real SpanDomain, |
1009 | const Standard_Boolean Periodic, |
1010 | const Standard_Integer Degree, |
1011 | const TColStd_Array1OfReal& FlatKnots, |
1012 | const Array1OfPoints& Poles, |
1013 | const TColStd_Array1OfReal& Weights, |
1014 | Array1OfPoints& CachePoles, |
1015 | TColStd_Array1OfReal& CacheWeights) |
1016 | { |
1017 | Standard_Integer ii, |
1018 | Dimension, |
1019 | LocalIndex, |
1020 | index = 0 ; |
1021 | Standard_Real u = U, |
1022 | LocalValue ; |
1023 | Standard_Boolean rational; |
1024 | |
1025 | BSplCLib_DataContainer dc(Degree); |
1026 | PrepareEval(u, |
1027 | index, |
1028 | Dimension, |
1029 | rational, |
1030 | Degree, |
1031 | Periodic, |
1032 | Poles, |
1033 | Weights, |
1034 | FlatKnots, |
1035 | (BSplCLib::NoMults()), |
1036 | dc); |
1037 | // |
1038 | // Watch out : PrepareEval checks if locally the Bspline is polynomial |
1039 | // therefore rational flag could be set to False even if there are |
1040 | // Weights. The Dimension is set accordingly. |
1041 | // |
1042 | |
1043 | BSplCLib::Bohm(u,Degree,Degree,*dc.knots,Dimension,*dc.poles); |
1044 | |
1045 | LocalValue = 1.0e0 ; |
1046 | LocalIndex = 0 ; |
1047 | |
1048 | if (rational) { |
1049 | |
1050 | for (ii = 1 ; ii <= Degree + 1 ; ii++) { |
1051 | CoordsToPoint (CachePoles(ii), dc.poles + LocalIndex, * LocalValue); |
1052 | LocalIndex += Dimension_gen + 1; |
1053 | LocalValue *= SpanDomain / (Standard_Real) ii ; |
1054 | } |
1055 | |
1056 | LocalIndex = Dimension_gen; |
1057 | LocalValue = 1.0e0 ; |
1058 | for (ii = 1 ; ii <= Degree + 1 ; ii++) { |
1059 | CacheWeights(ii) = dc.poles[LocalIndex] * LocalValue ; |
1060 | LocalIndex += Dimension_gen + 1; |
1061 | LocalValue *= SpanDomain / (Standard_Real) ii ; |
1062 | } |
1063 | } |
1064 | else { |
1065 | |
1066 | for (ii = 1 ; ii <= Degree + 1 ; ii++) { |
1067 | CoordsToPoint (CachePoles(ii), dc.poles + LocalIndex, * LocalValue); |
1068 | LocalIndex += Dimension_gen; |
1069 | LocalValue *= SpanDomain / (Standard_Real) ii ; |
1070 | } |
1071 | |
1072 | if (&Weights != NULL) { |
1073 | for (ii = 1 ; ii <= Degree + 1 ; ii++) |
1074 | CacheWeights(ii) = 0.0e0 ; |
1075 | CacheWeights(1) = 1.0e0 ; |
1076 | } |
1077 | } |
1078 | } |
1079 | |
1080 | //======================================================================= |
1081 | //function : Interpolate |
1082 | //purpose : |
1083 | //======================================================================= |
1084 | |
1085 | void BSplCLib::Interpolate(const Standard_Integer Degree, |
1086 | const TColStd_Array1OfReal& FlatKnots, |
1087 | const TColStd_Array1OfReal& Parameters, |
1088 | const TColStd_Array1OfInteger& ContactOrderArray, |
1089 | Array1OfPoints& Poles, |
1090 | Standard_Integer& InversionProblem) |
1091 | |
1092 | { |
1093 | Standard_Real *PArray ; |
1094 | |
1095 | PArray = (Standard_Real *) &Poles(Poles.Lower()) ; |
1096 | |
1097 | BSplCLib::Interpolate(Degree, |
1098 | FlatKnots, |
1099 | Parameters, |
1100 | ContactOrderArray, |
1101 | Dimension_gen, |
1102 | PArray[0], |
1103 | InversionProblem) ; |
1104 | } |
1105 | |
1106 | //======================================================================= |
1107 | //function : Interpolate |
1108 | //purpose : |
1109 | //======================================================================= |
1110 | |
1111 | void BSplCLib::Interpolate(const Standard_Integer Degree, |
1112 | const TColStd_Array1OfReal& FlatKnots, |
1113 | const TColStd_Array1OfReal& Parameters, |
1114 | const TColStd_Array1OfInteger& ContactOrderArray, |
1115 | Array1OfPoints& Poles, |
1116 | TColStd_Array1OfReal& Weights, |
1117 | Standard_Integer& InversionProblem) |
1118 | { |
1119 | Standard_Real *PArray, |
1120 | * WArray ; |
1121 | PArray = (Standard_Real *) &Poles(Poles.Lower()) ; |
1122 | WArray = (Standard_Real *) &Weights(Weights.Lower()) ; |
1123 | BSplCLib::Interpolate(Degree, |
1124 | FlatKnots, |
1125 | Parameters, |
1126 | ContactOrderArray, |
1127 | Dimension_gen, |
1128 | PArray[0], |
1129 | WArray[0], |
1130 | InversionProblem) ; |
1131 | } |
1132 | |
1133 | //======================================================================= |
1134 | //function : MovePoint |
1135 | //purpose : Find the new poles which allows an old point (with a |
1136 | // given u as parameter) to reach a new position |
1137 | //======================================================================= |
1138 | |
1139 | void BSplCLib::MovePoint (const Standard_Real U, |
1140 | const Vector& Displ, |
1141 | const Standard_Integer Index1, |
1142 | const Standard_Integer Index2, |
1143 | const Standard_Integer Degree, |
1144 | const Standard_Boolean Rational, |
1145 | const Array1OfPoints& Poles, |
1146 | const TColStd_Array1OfReal& Weights, |
1147 | const TColStd_Array1OfReal& FlatKnots, |
1148 | Standard_Integer& FirstIndex, |
1149 | Standard_Integer& LastIndex, |
1150 | Array1OfPoints& NewPoles) |
1151 | { |
1152 | // calculate the BSplineBasis in the parameter U |
1153 | Standard_Integer FirstNonZeroBsplineIndex; |
1154 | math_Matrix BSplineBasis(1, 1, |
1155 | 1, Degree+1); |
1156 | Standard_Integer ErrorCode = |
1157 | BSplCLib::EvalBsplineBasis(1, |
1158 | 0, |
1159 | Degree+1, |
1160 | FlatKnots, |
1161 | U, |
1162 | FirstNonZeroBsplineIndex, |
1163 | BSplineBasis); |
1164 | if (ErrorCode != 0) { |
1165 | FirstIndex = 0; |
1166 | LastIndex = 0; |
1167 | |
1168 | for (Standard_Integer i=Poles.Lower(); i<=Poles.Upper(); i++) { |
1169 | NewPoles(i) = Poles(i); |
1170 | } |
1171 | return; |
1172 | } |
1173 | |
1174 | // find the span which is predominant for parameter U |
1175 | FirstIndex = FirstNonZeroBsplineIndex; |
1176 | LastIndex = FirstNonZeroBsplineIndex + Degree ; |
1177 | if (FirstIndex < Index1) FirstIndex = Index1; |
1178 | if (LastIndex > Index2) LastIndex = Index2; |
1179 | |
1180 | Standard_Real maxValue = 0.0; |
1181 | Standard_Integer i, kk1=0, kk2, ii; |
1182 | |
1183 | for (i = FirstIndex-FirstNonZeroBsplineIndex+1; |
1184 | i <= LastIndex-FirstNonZeroBsplineIndex+1; i++) { |
1185 | if (BSplineBasis(1,i) > maxValue) { |
1186 | kk1 = i + FirstNonZeroBsplineIndex - 1; |
1187 | maxValue = BSplineBasis(1,i); |
1188 | } |
1189 | } |
1190 | |
1191 | // find a kk2 if symetriy |
1192 | kk2 = kk1; |
1193 | i = kk1 - FirstNonZeroBsplineIndex + 2; |
1194 | if ((kk1+1) <= LastIndex) { |
1195 | if (Abs(BSplineBasis(1, kk1-FirstNonZeroBsplineIndex+2) - maxValue) < 1.e-10) { |
1196 | kk2 = kk1+1; |
1197 | } |
1198 | } |
1199 | |
1200 | // compute the vector of displacement |
1201 | Standard_Real D1 = 0.0; |
1202 | Standard_Real D2 = 0.0; |
1203 | Standard_Real hN, Coef, Dval; |
1204 | |
1205 | for (i = 1; i <= Degree+1; i++) { |
1206 | ii = i + FirstNonZeroBsplineIndex - 1; |
1207 | if (Rational) { |
1208 | hN = Weights(ii)*BSplineBasis(1, i); |
1209 | D2 += hN; |
1210 | } |
1211 | else { |
1212 | hN = BSplineBasis(1, i); |
1213 | } |
1214 | if (ii >= FirstIndex && ii <= LastIndex) { |
1215 | if (ii < kk1) { |
1216 | Dval = kk1-ii; |
1217 | } |
1218 | else if (ii > kk2) { |
1219 | Dval = ii - kk2; |
1220 | } |
1221 | else { |
1222 | Dval = 0.0; |
1223 | } |
1224 | D1 += 1./(Dval + 1.) * hN; |
1225 | } |
1226 | } |
1227 | |
1228 | if (Rational) { |
1229 | Coef = D2/D1; |
1230 | } |
1231 | else { |
1232 | Coef = 1./D1; |
1233 | } |
1234 | |
1235 | // compute the new poles |
1236 | |
1237 | for (i=Poles.Lower(); i<=Poles.Upper(); i++) { |
1238 | if (i >= FirstIndex && i <= LastIndex) { |
1239 | if (i < kk1) { |
1240 | Dval = kk1-i; |
1241 | } |
1242 | else if (i > kk2) { |
1243 | Dval = i - kk2; |
1244 | } |
1245 | else { |
1246 | Dval = 0.0; |
1247 | } |
1248 | NewPoles(i) = Poles(i).Translated((Coef/(Dval+1.))*Displ); |
1249 | } |
1250 | else { |
1251 | NewPoles(i) = Poles(i); |
1252 | } |
1253 | } |
1254 | } |
1255 | |
1256 | //======================================================================= |
1257 | //function : MovePoint |
1258 | //purpose : Find the new poles which allows an old point (with a |
1259 | // given u as parameter) to reach a new position |
1260 | //======================================================================= |
1261 | |
1262 | //======================================================================= |
1263 | //function : MovePointAndTangent |
1264 | //purpose : |
1265 | //======================================================================= |
1266 | |
1267 | void BSplCLib::MovePointAndTangent (const Standard_Real U, |
1268 | const Vector& Delta, |
1269 | const Vector& DeltaDerivatives, |
1270 | const Standard_Real Tolerance, |
1271 | const Standard_Integer Degree, |
1272 | const Standard_Boolean Rational, |
1273 | const Standard_Integer StartingCondition, |
1274 | const Standard_Integer EndingCondition, |
1275 | const Array1OfPoints& Poles, |
1276 | const TColStd_Array1OfReal& Weights, |
1277 | const TColStd_Array1OfReal& FlatKnots, |
1278 | Array1OfPoints& NewPoles, |
1279 | Standard_Integer & ErrorStatus) |
1280 | { |
1281 | Standard_Real *delta_array, |
1282 | *delta_derivative_array, |
1283 | *poles_array, |
1284 | *new_poles_array ; |
1285 | |
1286 | Standard_Integer num_poles ; |
1287 | num_poles = Poles.Length() ; |
1288 | |
1289 | if (NewPoles.Length() != num_poles) { |
1290 | Standard_ConstructionError::Raise(); |
1291 | } |
1292 | delta_array = (Standard_Real *) &Delta ; |
1293 | delta_derivative_array = (Standard_Real *)&DeltaDerivatives ; |
1294 | poles_array = (Standard_Real *) &Poles(Poles.Lower()) ; |
1295 | |
1296 | new_poles_array = (Standard_Real *) &NewPoles(NewPoles.Lower()) ; |
1297 | MovePointAndTangent(U, |
1298 | Dimension_gen, |
1299 | delta_array[0], |
1300 | delta_derivative_array[0], |
1301 | Tolerance, |
1302 | Degree, |
1303 | Rational, |
1304 | StartingCondition, |
1305 | EndingCondition, |
1306 | poles_array[0], |
1307 | Weights, |
1308 | FlatKnots, |
1309 | new_poles_array[0], |
1310 | ErrorStatus) ; |
1311 | } |
1312 | |
1313 | //======================================================================= |
1314 | //function : Resolution |
1315 | //purpose : |
1316 | //======================================================================= |
1317 | |
1318 | void BSplCLib::Resolution(const Array1OfPoints& Poles, |
1319 | const TColStd_Array1OfReal& Weights, |
1320 | const Standard_Integer NumPoles, |
1321 | const TColStd_Array1OfReal& FlatKnots, |
1322 | const Standard_Integer Degree, |
1323 | const Standard_Real Tolerance3D, |
1324 | Standard_Real& UTolerance) |
1325 | { |
1326 | Standard_Real *PolesArray ; |
1327 | PolesArray = (Standard_Real *) &Poles(Poles.Lower()) ; |
1328 | BSplCLib::Resolution(PolesArray[0], |
1329 | Dimension_gen, |
1330 | NumPoles, |
1331 | Weights, |
1332 | FlatKnots, |
1333 | Degree, |
1334 | Tolerance3D, |
1335 | UTolerance) ; |
1336 | } |
1337 | |
1338 | //======================================================================= |
1339 | //function : FunctionMultiply |
1340 | //purpose : |
1341 | //======================================================================= |
1342 | |
1343 | void BSplCLib::FunctionMultiply |
1344 | (const BSplCLib_EvaluatorFunction & FunctionPtr, |
1345 | const Standard_Integer BSplineDegree, |
1346 | const TColStd_Array1OfReal & BSplineFlatKnots, |
1347 | const Array1OfPoints & Poles, |
1348 | const TColStd_Array1OfReal & FlatKnots, |
1349 | const Standard_Integer NewDegree, |
1350 | Array1OfPoints & NewPoles, |
1351 | Standard_Integer & Status) |
1352 | { |
1353 | Standard_Integer num_bspline_poles = |
1354 | BSplineFlatKnots.Length() - BSplineDegree - 1 ; |
1355 | Standard_Integer num_new_poles = |
1356 | FlatKnots.Length() - NewDegree - 1 ; |
1357 | |
1358 | if (Poles.Length() != num_bspline_poles || |
1359 | NewPoles.Length() != num_new_poles) { |
1360 | Standard_ConstructionError::Raise(); |
1361 | } |
1362 | Standard_Real * array_of_poles = |
1363 | (Standard_Real *) &Poles(Poles.Lower()) ; |
1364 | Standard_Real * array_of_new_poles = |
1365 | (Standard_Real *) &NewPoles(NewPoles.Lower()) ; |
1366 | BSplCLib::FunctionMultiply(FunctionPtr, |
1367 | BSplineDegree, |
1368 | BSplineFlatKnots, |
1369 | Dimension_gen, |
1370 | array_of_poles[0], |
1371 | FlatKnots, |
1372 | NewDegree, |
1373 | array_of_new_poles[0], |
1374 | Status) ; |
1375 | } |
1376 | |
1377 | //======================================================================= |
1378 | //function : FunctionReparameterise |
1379 | //purpose : |
1380 | //======================================================================= |
1381 | |
1382 | void BSplCLib::FunctionReparameterise |
1383 | (const BSplCLib_EvaluatorFunction & FunctionPtr, |
1384 | const Standard_Integer BSplineDegree, |
1385 | const TColStd_Array1OfReal & BSplineFlatKnots, |
1386 | const Array1OfPoints & Poles, |
1387 | const TColStd_Array1OfReal & FlatKnots, |
1388 | const Standard_Integer NewDegree, |
1389 | Array1OfPoints & NewPoles, |
1390 | Standard_Integer & Status) |
1391 | { |
1392 | Standard_Integer num_bspline_poles = |
1393 | BSplineFlatKnots.Length() - BSplineDegree - 1 ; |
1394 | Standard_Integer num_new_poles = |
1395 | FlatKnots.Length() - NewDegree - 1 ; |
1396 | |
1397 | if (Poles.Length() != num_bspline_poles || |
1398 | NewPoles.Length() != num_new_poles) { |
1399 | Standard_ConstructionError::Raise(); |
1400 | } |
1401 | Standard_Real * array_of_poles = |
1402 | (Standard_Real *) &Poles(Poles.Lower()) ; |
1403 | Standard_Real * array_of_new_poles = |
1404 | (Standard_Real *) &NewPoles(NewPoles.Lower()) ; |
1405 | BSplCLib::FunctionReparameterise(FunctionPtr, |
1406 | BSplineDegree, |
1407 | BSplineFlatKnots, |
1408 | Dimension_gen, |
1409 | array_of_poles[0], |
1410 | FlatKnots, |
1411 | NewDegree, |
1412 | array_of_new_poles[0], |
1413 | Status) ; |
1414 | } |
1415 | |