0026937: Eliminate NO_CXX_EXCEPTION macro support
[occt.git] / src / Law / Law_Interpolate.cxx
1 // Created on: 1995-11-16
2 // Created by: Laurent BOURESCHE
3 // Copyright (c) 1995-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 // Programme cree 
18
19 #include <BSplCLib.hxx>
20 #include <gp_Pnt.hxx>
21 #include <Law_BSpline.hxx>
22 #include <Law_Interpolate.hxx>
23 #include <PLib.hxx>
24 #include <Standard_ConstructionError.hxx>
25 #include <StdFail_NotDone.hxx>
26 #include <TColStd_Array1OfBoolean.hxx>
27 #include <TColStd_Array1OfInteger.hxx>
28
29 //=======================================================================
30 //function : CheckParameters
31 //purpose  : 
32 //=======================================================================
33 static Standard_Boolean CheckParameters
34 (const TColStd_Array1OfReal&  Parameters) 
35 {
36   Standard_Integer ii;
37   Standard_Real distance;
38   Standard_Boolean result = Standard_True;
39   for (ii = Parameters.Lower() ; result && ii < Parameters.Upper() ; ii++) {
40     distance = Parameters.Value(ii+1) - Parameters.Value(ii);
41     result = (distance >= RealSmall());
42   }
43   return result;
44 }       
45
46 //=======================================================================
47 //function : BuildParameters
48 //purpose  : 
49 //=======================================================================
50 static void  BuildParameters(const Standard_Boolean         PeriodicFlag,
51                              const TColStd_Array1OfReal&    PointsArray,
52                              Handle(TColStd_HArray1OfReal)& ParametersPtr)
53 {
54   Standard_Integer ii, index = 2;
55   Standard_Real distance;
56   Standard_Integer num_parameters = PointsArray.Length();
57   if (PeriodicFlag) {
58     num_parameters += 1;
59   }
60   ParametersPtr = new TColStd_HArray1OfReal(1, num_parameters);
61   ParametersPtr->SetValue(1,0.);
62   for (ii = PointsArray.Lower(); ii < PointsArray.Upper(); ii++) {
63     distance = Abs(PointsArray.Value(ii) - PointsArray.Value(ii+1));
64     ParametersPtr->SetValue(index, ParametersPtr->Value(ii) + distance);
65     index += 1 ;
66   }
67   if (PeriodicFlag) {
68     distance = Abs(PointsArray.Value(PointsArray.Upper()) - 
69                    PointsArray.Value(PointsArray.Lower()));
70     ParametersPtr->SetValue(index, ParametersPtr->Value(ii) + distance);
71   }
72 }
73
74 //=======================================================================
75 //function : BuildPeriodicTangents
76 //purpose  : 
77 //=======================================================================
78
79 static void BuildPeriodicTangent
80 (const TColStd_Array1OfReal& PointsArray,
81  TColStd_Array1OfReal&       TangentsArray,
82  TColStd_Array1OfBoolean&    TangentFlags,
83  const TColStd_Array1OfReal& ParametersArray)
84 {
85   Standard_Real point_array[3], parameter_array[3], eval_result[2];
86   
87   if (PointsArray.Length() < 2) {
88     TangentFlags.SetValue(1,Standard_True);
89     TangentsArray.SetValue(1,0.);
90   }   
91   else if (!TangentFlags.Value(1)){
92     //Pour les periodiques on evalue la tangente du point de fermeture
93     //par une interpolation de degre 2 entre le dernier point, le point
94     //de fermeture et le deuxieme point.
95     Standard_Integer degree = 2;
96     Standard_Real period = (ParametersArray.Value(ParametersArray.Upper()) -
97                             ParametersArray.Value(ParametersArray.Lower()));
98     point_array [0] = PointsArray.Value(PointsArray.Upper());
99     point_array [1] = PointsArray.Value(PointsArray.Lower());
100     point_array [2] = PointsArray.Value(PointsArray.Lower()+1);
101     parameter_array[0] = ParametersArray.Value(ParametersArray.Upper() - 1) - period;
102     parameter_array[1] = ParametersArray.Value(ParametersArray.Lower());
103     parameter_array[2] = ParametersArray.Value(ParametersArray.Lower() + 1);
104     TangentFlags.SetValue(1,Standard_True);
105     PLib::EvalLagrange(parameter_array[1],
106                        1,
107                        degree,
108                        1,
109                        point_array[0],
110                        parameter_array[0],
111                        eval_result[0]);
112     TangentsArray.SetValue(1,eval_result[1]);
113   }
114 }
115
116 //=======================================================================
117 //function : BuildTangents
118 //purpose  : 
119 //=======================================================================
120
121 static void BuildTangents(const TColStd_Array1OfReal&  PointsArray,
122                           TColStd_Array1OfReal&        TangentsArray,
123                           TColStd_Array1OfBoolean&     TangentFlags,
124                           const TColStd_Array1OfReal&  ParametersArray)
125 {
126   Standard_Integer  degree = 3;//,ii;
127   Standard_Real *point_array, *parameter_array, eval_result[2];
128   
129   if ( PointsArray.Length() < 3) {
130     throw Standard_ConstructionError();
131   }   
132   if (PointsArray.Length() == 3) {
133     degree = 2;
134   }
135   if (!TangentFlags.Value(1)) {
136     point_array = (Standard_Real *) &PointsArray.Value(PointsArray.Lower()); 
137     parameter_array = (Standard_Real *) &ParametersArray.Value(1);
138     TangentFlags.SetValue(1,Standard_True);
139     PLib::EvalLagrange(ParametersArray.Value(1),
140                        1,
141                        degree,
142                        1,
143                        point_array[0],
144                        parameter_array[0],
145                        eval_result[0]);
146     TangentsArray.SetValue(1,eval_result[1]);
147   }
148   if (! TangentFlags.Value(TangentFlags.Upper())) {
149     point_array = 
150       (Standard_Real *) &PointsArray.Value(PointsArray.Upper() - degree);
151     TangentFlags.SetValue(TangentFlags.Upper(),Standard_True);
152     Standard_Integer iup = ParametersArray.Upper() - degree;
153     parameter_array = (Standard_Real *) &ParametersArray.Value(iup);
154     PLib::EvalLagrange(ParametersArray.Value(ParametersArray.Upper()),
155                        1,
156                        degree,
157                        1,
158                        point_array[0],
159                        parameter_array[0],
160                        eval_result[0]);
161     TangentsArray.SetValue(TangentsArray.Upper(),eval_result[1]);
162   }
163
164
165 //=======================================================================
166 //function : Law_Interpolate
167 //purpose  : 
168 //=======================================================================
169
170 Law_Interpolate::Law_Interpolate
171 (const Handle(TColStd_HArray1OfReal)& PointsPtr,
172  const Standard_Boolean               PeriodicFlag,
173  const Standard_Real                  Tolerance) :
174  myTolerance(Tolerance),
175  myPoints(PointsPtr),
176  myIsDone(Standard_False),
177  myPeriodic(PeriodicFlag),
178  myTangentRequest(Standard_False) 
179      
180 {
181 //Standard_Integer ii;
182   myTangents = new TColStd_HArray1OfReal (myPoints->Lower(),
183                                           myPoints->Upper());
184   myTangentFlags = new TColStd_HArray1OfBoolean(myPoints->Lower(),
185                                                 myPoints->Upper());
186
187   BuildParameters(PeriodicFlag,
188                   PointsPtr->Array1(),
189                   myParameters);
190   myTangentFlags->Init(Standard_False);
191 }
192
193 //=======================================================================
194 //function : Law_Interpolate
195 //purpose  : 
196 //=======================================================================
197
198 Law_Interpolate::Law_Interpolate
199 (const Handle(TColStd_HArray1OfReal)& PointsPtr,
200  const Handle(TColStd_HArray1OfReal)& ParametersPtr,
201  const Standard_Boolean               PeriodicFlag,
202  const Standard_Real                  Tolerance) :
203  myTolerance(Tolerance),
204  myPoints(PointsPtr),
205  myIsDone(Standard_False),
206  myParameters(ParametersPtr),
207  myPeriodic(PeriodicFlag),
208  myTangentRequest(Standard_False) 
209 {
210 //Standard_Integer ii;
211   if (PeriodicFlag) {
212     if ((PointsPtr->Length()) + 1 != ParametersPtr->Length()) {
213       throw Standard_ConstructionError();
214     }
215   }
216   myTangents = new TColStd_HArray1OfReal(myPoints->Lower(),
217                                        myPoints->Upper());
218   myTangentFlags = new TColStd_HArray1OfBoolean(myPoints->Lower(),
219                                                 myPoints->Upper());
220   Standard_Boolean result = CheckParameters(ParametersPtr->Array1());
221   if (!result) {
222     throw Standard_ConstructionError();
223   }
224   myTangentFlags->Init(Standard_False);
225 }
226
227 //=======================================================================
228 //function : Load
229 //purpose  : 
230 //=======================================================================
231
232 void Law_Interpolate::Load
233 (const TColStd_Array1OfReal&             Tangents,
234  const Handle(TColStd_HArray1OfBoolean)& TangentFlagsPtr) 
235 {
236 //Standard_Boolean result;
237   Standard_Integer ii;
238   myTangentRequest = Standard_True;
239   myTangentFlags = TangentFlagsPtr;
240   if (Tangents.Length() != myPoints->Length() ||
241       TangentFlagsPtr->Length() != myPoints->Length()) {
242     throw Standard_ConstructionError();
243   }
244   myTangents = new TColStd_HArray1OfReal(Tangents.Lower(),Tangents.Upper());
245   for (ii = Tangents.Lower() ; ii <= Tangents.Upper() ; ii++ ) {
246     myTangents->SetValue(ii,Tangents.Value(ii));
247   }
248 }
249
250 //=======================================================================
251 //function : Load
252 //purpose  : 
253 //=======================================================================
254
255 void Law_Interpolate::Load(const Standard_Real InitialTangent,
256                            const Standard_Real FinalTangent) 
257 {
258 //Standard_Boolean result;
259   myTangentRequest = Standard_True;
260   myTangentFlags->SetValue(1,Standard_True);
261   myTangents->SetValue(1,InitialTangent);
262   myTangentFlags->SetValue(myPoints->Length(),Standard_True);
263   myTangents->SetValue(myPoints->Length(),FinalTangent) ;
264 }
265
266 //=======================================================================
267 //function : Perform
268 //purpose  : 
269 //=======================================================================
270
271 void Law_Interpolate::Perform() 
272 {
273   if (myPeriodic) {
274     PerformPeriodic();
275   }
276   else {
277     PerformNonPeriodic();
278   }
279 }
280
281 //=======================================================================
282 //function : PerformPeriodic
283 //purpose  : 
284 //=======================================================================
285
286 void Law_Interpolate::PerformPeriodic()
287
288   Standard_Integer degree,
289   ii,
290 //jj,
291   index,
292   index1,
293 //index2,
294   mult_index,
295   half_order,
296   inversion_problem,
297   num_points,
298   num_distinct_knots,
299   num_poles;
300   
301   Standard_Real period;
302   
303 //gp_Pnt a_point;
304   
305   num_points = myPoints->Length();
306   period = myParameters->Value(myParameters->Upper()) -
307     myParameters->Value(myParameters->Lower())  ;
308   num_poles = num_points + 1 ;
309   num_distinct_knots = num_points + 1;
310   half_order = 2;  
311   degree = 3;
312   num_poles += 2;
313   if (myTangentRequest) {
314     for (ii = myTangentFlags->Lower() + 1; 
315          ii <= myTangentFlags->Upper(); ii++) {
316       if (myTangentFlags->Value(ii)) {
317         num_poles += 1;
318       }
319     }
320   }
321   TColStd_Array1OfReal     parameters(1,num_poles);  
322   TColStd_Array1OfReal     flatknots(1,num_poles + degree + 1);
323   TColStd_Array1OfInteger  mults(1,num_distinct_knots);
324   TColStd_Array1OfInteger  contact_order_array(1, num_poles);
325   TColStd_Array1OfReal     poles(1,num_poles);
326   
327   for (ii = 1 ; ii <= half_order ; ii++) {
328     flatknots.SetValue(ii,myParameters->Value(myParameters->Upper() -1) -
329                        period);
330     flatknots.SetValue(ii + half_order,myParameters->
331                        Value(myParameters->Lower()));
332     flatknots.SetValue(num_poles + ii,
333                        myParameters->Value(myParameters->Upper()));
334     flatknots.SetValue(num_poles + half_order + ii,
335                        myParameters->Value(half_order) + period);
336   }
337   for (ii = 1 ; ii <= num_poles ; ii++) {
338     contact_order_array.SetValue(ii,0) ;
339   }
340   for (ii = 2; ii < num_distinct_knots; ii++) {
341     mults.SetValue(ii,1); 
342   }
343   mults.SetValue(1,half_order);
344   mults.SetValue(num_distinct_knots ,half_order);
345   
346   BuildPeriodicTangent(myPoints->Array1(),
347                        myTangents->ChangeArray1(),
348                        myTangentFlags->ChangeArray1(),
349                        myParameters->Array1());
350   
351     contact_order_array.SetValue(2,1);
352   parameters.SetValue(1,myParameters->Value(1));
353   parameters.SetValue(2,myParameters->Value(1));
354   poles.SetValue(1,myPoints->Value(1));
355   poles.SetValue(2,myTangents->Value(1));
356   mult_index = 2;
357   index = 3;
358   index1 = degree + 2;
359   if (myTangentRequest) {
360     for (ii = myTangentFlags->Lower() + 1; 
361          ii <= myTangentFlags->Upper(); ii++) {
362       parameters.SetValue(index,myParameters->Value(ii));
363       flatknots.SetValue(index1,myParameters->Value(ii));
364       poles.SetValue(index,myPoints->Value(ii));
365       index += 1;
366       index1 += 1;
367       if (myTangentFlags->Value(ii)) {
368         mults.SetValue(mult_index,mults.Value(mult_index)  + 1);
369         contact_order_array(index) = 1;
370         
371         parameters.SetValue(index,
372                             myParameters->Value(ii));
373         flatknots.SetValue(index1,myParameters->Value(ii));
374         poles.SetValue(index,myTangents->Value(ii));
375         index  += 1;
376         index1 += 1;
377       }
378       mult_index += 1;
379     }
380   }
381   else {
382     index = degree + 1;
383     index1 = 2 ;
384     for(ii = myParameters->Lower(); ii <= myParameters->Upper(); ii++) {
385       parameters.SetValue(index1, myParameters->Value(ii));
386       flatknots.SetValue(index, myParameters->Value(ii));
387       index += 1;
388       index1 += 1;
389     }
390     index = 3;
391     for (ii = myPoints->Lower() + 1; ii <= myPoints->Upper(); ii++) {
392       //
393       // copy all the given points since the last one will be initialized
394       // below by the first point in the array myPoints
395       //
396       poles.SetValue(index, myPoints->Value(ii));
397       index += 1;
398     }
399   }
400   contact_order_array.SetValue(num_poles - 1, 1);
401   parameters.SetValue(num_poles-1,
402                       myParameters->Value(myParameters->Upper()));
403   //
404   // for the periodic curve ONLY  the tangent of the first point
405   // will be used since the curve should close itself at the first
406   // point See BuildPeriodicTangent
407   //
408   poles.SetValue(num_poles-1,myTangents->Value(1));
409   parameters.SetValue(num_poles,
410                       myParameters->Value(myParameters->Upper()));
411   poles.SetValue(num_poles,
412                  myPoints->Value(1));
413   
414   BSplCLib::Interpolate(degree,
415                         flatknots,
416                         parameters,
417                         contact_order_array,
418                         1,
419                         poles(1),
420                         inversion_problem);
421   if (!inversion_problem) {
422     TColStd_Array1OfReal  newpoles(poles.Value(1),
423                                    1,
424                                    num_poles - 2) ;
425     myCurve =   new Law_BSpline(newpoles,
426                                 myParameters->Array1(),
427                                 mults,
428                                 degree,
429                                 myPeriodic);
430     myIsDone = Standard_True;
431   }
432 }
433
434
435 //=======================================================================
436 //function : PerformNonPeriodic
437 //purpose  : 
438 //=======================================================================
439
440 void Law_Interpolate::PerformNonPeriodic() 
441 {
442   Standard_Integer degree,
443   ii,
444 //jj,
445   index,
446   index1,
447   index2,
448   index3,
449   mult_index,
450   inversion_problem,
451   num_points,
452   num_distinct_knots,
453   num_poles;
454   
455   num_points =
456     num_distinct_knots =
457       num_poles = myPoints->Length();
458   if (num_poles == 2 &&   !myTangentRequest)  {
459     degree = 1;
460   } 
461   else if (num_poles == 3 && !myTangentRequest) {
462     degree = 2;
463     num_distinct_knots = 2;
464   }
465   else {
466     degree = 3;
467     num_poles += 2;
468     if (myTangentRequest) {
469       for (ii = myTangentFlags->Lower() + 1; 
470            ii < myTangentFlags->Upper(); ii++) {
471         if (myTangentFlags->Value(ii)) {
472           num_poles += 1;
473         }
474       }
475     }
476   }
477   TColStd_Array1OfReal     parameters(1,num_poles) ;  
478   TColStd_Array1OfReal     flatknots(1,num_poles + degree + 1) ;
479   TColStd_Array1OfInteger  mults(1,num_distinct_knots) ;
480   TColStd_Array1OfReal     knots(1,num_distinct_knots) ;
481   TColStd_Array1OfInteger  contact_order_array(1, num_poles) ;
482   TColStd_Array1OfReal     poles(1,num_poles) ;
483   
484   for (ii = 1 ; ii <= degree + 1 ; ii++) {
485     flatknots.SetValue(ii,myParameters->Value(1));
486     flatknots.SetValue(ii + num_poles,
487                        myParameters->Value(num_points));
488   }
489   for (ii = 1 ; ii <= num_poles ; ii++) {
490     contact_order_array.SetValue(ii,0);
491   }
492   for (ii = 2 ; ii < num_distinct_knots ; ii++) {
493     mults.SetValue(ii,1); 
494   }
495   mults.SetValue(1,degree + 1);
496   mults.SetValue(num_distinct_knots ,degree + 1);
497   
498   switch (degree) {
499   case 1:
500     for (ii = 1 ; ii <= num_poles ; ii++) {
501       poles.SetValue(ii ,myPoints->Value(ii));
502     }
503     myCurve =
504       new Law_BSpline(poles,
505                       myParameters->Array1(),
506                       mults,
507                       degree) ;
508     myIsDone = Standard_True ;
509     break ;
510   case 2:
511     knots.SetValue(1,myParameters->Value(1)) ;
512     knots.SetValue(2,myParameters->Value(num_poles)) ;
513     for (ii = 1 ; ii <= num_poles ; ii++) {
514       poles.SetValue(ii,myPoints->Value(ii)) ;
515       
516     }
517     BSplCLib::Interpolate(degree,
518                           flatknots,
519                           myParameters->Array1(),
520                           contact_order_array,
521                           1,
522                           poles(1),
523                           inversion_problem) ;
524     if (!inversion_problem) {
525       myCurve = new Law_BSpline(poles,
526                                 knots,
527                                 mults,
528                                 degree);
529       myIsDone = Standard_True;
530     }
531     break;
532   case 3:
533     //
534     // check if the boundary conditions are set
535     //
536     if (num_points >= 3) {
537       //
538       // cannot build the tangents with degree 3 with only 2 points
539       // if those where not given in advance 
540       //
541       BuildTangents(myPoints->Array1(),
542                     myTangents->ChangeArray1(),
543                     myTangentFlags->ChangeArray1(),
544                     myParameters->Array1()) ;
545     }
546     contact_order_array.SetValue(2,1);
547     parameters.SetValue(1,myParameters->Value(1));
548     parameters.SetValue(2,myParameters->Value(1));
549     poles.SetValue(1,myPoints->Value(1));
550     poles.SetValue(2,myTangents->Value(1));
551     mult_index = 2 ;
552     index = 3 ;
553     index1 = 2 ;
554     index2 = myPoints->Lower() + 1 ;
555     index3 = degree + 2 ;
556     if (myTangentRequest) {
557       for (ii = myParameters->Lower() + 1 ; 
558            ii < myParameters->Upper() ; ii++) {
559         parameters.SetValue(index,myParameters->Value(ii)) ;
560         poles.SetValue(index,myPoints->Value(index2)) ;
561         flatknots.SetValue(index3,myParameters->Value(ii)) ; 
562         index += 1 ;
563         index3 += 1 ;
564         if (myTangentFlags->Value(index1)) {
565           //
566           // set the multiplicities, the order of the contact, the 
567           // the flatknots,
568           //
569           mults.SetValue(mult_index,mults.Value(mult_index) + 1) ;
570           contact_order_array(index) = 1 ;
571           flatknots.SetValue(index3, myParameters->Value(ii)) ;
572           parameters.SetValue(index,
573                               myParameters->Value(ii));
574           poles.SetValue(index,myTangents->Value(ii));
575           index += 1  ;
576           index3 += 1 ;
577         }
578         mult_index += 1 ;
579         index1 += 1 ;
580         index2 += 1 ;
581       }
582     }
583     else {
584       index1 = 2 ;
585       for(ii = myParameters->Lower(); ii <= myParameters->Upper(); ii++) {
586         parameters.SetValue(index1, myParameters->Value(ii));
587         index1 += 1;
588       }
589       index = 3 ;
590       for (ii = myPoints->Lower() + 1; ii <= myPoints->Upper() - 1; ii++){
591         poles.SetValue(index, myPoints->Value(ii));
592         index += 1 ;
593       }
594       index = degree + 1;
595       for(ii = myParameters->Lower(); ii <= myParameters->Upper(); ii++){
596         flatknots.SetValue(index, myParameters->Value(ii));
597         index += 1 ;
598       }
599     }
600     poles.SetValue(num_poles-1, myTangents->Value(num_points));
601     
602     contact_order_array.SetValue(num_poles - 1, 1);
603     parameters.SetValue(num_poles,
604                         myParameters->Value(myParameters->Upper())) ;
605     parameters.SetValue(num_poles -1,
606                         myParameters->Value(myParameters->Upper())) ;
607     
608     poles.SetValue(num_poles,
609                    myPoints->Value(num_points)) ;
610     
611     BSplCLib::Interpolate(degree,
612                           flatknots,
613                           parameters,
614                           contact_order_array,
615                           1,
616                           poles(1),
617                           inversion_problem) ;
618     if (!inversion_problem) {
619       myCurve = new Law_BSpline(poles,
620                                 myParameters->Array1(),
621                                 mults,
622                                 degree) ;
623       myIsDone = Standard_True;
624     }
625     break ;
626   }
627 }
628
629 //=======================================================================
630 //function : Handle(Geom_BSplineCurve)&
631 //purpose  : 
632 //=======================================================================
633
634 const Handle(Law_BSpline)& Law_Interpolate::Curve() const 
635 {
636   if ( !myIsDone) 
637     throw StdFail_NotDone(" ");
638   return myCurve;
639 }
640
641 //=======================================================================
642 //function : IsDone
643 //purpose  : 
644 //=======================================================================
645
646 Standard_Boolean Law_Interpolate::IsDone() const
647 {
648   return myIsDone;
649 }