ViewVC Help
View File | Revision Log | Show Annotations | View Changeset | Root Listing
root/REPOS_ERICCA/magic/lib/step/src/stbspline.cpp
Revision: 283
Committed: Tue Sep 13 21:11:20 2011 UTC (13 years, 8 months ago) by francois
File size: 15781 byte(s)
Log Message: 
structure de l'écriture

File Contents

# User Rev Content
1 foucault 27 //------------------------------------------------------------
2     //------------------------------------------------------------
3     // MAGiC
4 francois 283 // Jean Christophe Cuilli�re et Vincent FRANCOIS
5     // D�partement de G�nie M�canique - UQTR
6 foucault 27 //------------------------------------------------------------
7 francois 283 // Le projet MAGIC est un projet de recherche du d�partement
8     // de g�nie m�canique de l'Universit� du Qu�bec �
9     // Trois Rivi�res
10     // Les librairies ne peuvent �tre utilis�es sans l'accord
11 foucault 27 // des auteurs (contact : francois@uqtr.ca)
12     //------------------------------------------------------------
13     //------------------------------------------------------------
14     //
15     // stbspline.cpp
16     //
17     //------------------------------------------------------------
18     //------------------------------------------------------------
19     // COPYRIGHT 2000
20 francois 283 // Version du 02/03/2006 � 11H24
21 foucault 27 //------------------------------------------------------------
22     //------------------------------------------------------------
23    
24    
25     #include "gestionversion.h"
26    
27     #include "stbspline.h"
28 francois 283 #include <vector>
29 foucault 27 #include "st_gestionnaire.h"
30     #include "tpl_fonction.h"
31     #include "constantegeo.h"
32    
33     #include <math.h>
34    
35    
36    
37     ST_B_SPLINE::ST_B_SPLINE(long LigneCourante,std::string idori,int bs_degre,std::vector<int> bs_indexptsctr,std::vector<int> bs_knots_multiplicities,std::vector<double> bs_knots):ST_COURBE(LigneCourante,idori),degre(bs_degre)
38     {
39 francois 283 int nb=bs_knots_multiplicities.size();
40     for (int i=0;i<nb;i++)
41     {
42 foucault 27 for (int j=0;j<bs_knots_multiplicities[i];j++)
43 francois 283 knots.insert(knots.end(),bs_knots[i]);
44     }
45     nb_point=bs_indexptsctr.size();
46     for (int i=0;i<nb_point;i++)
47     {
48 foucault 27 indexptsctr.insert(indexptsctr.end(),bs_indexptsctr[i]);
49     //poids.insert(poids.end(),1.);
50 francois 283 }
51 foucault 27
52     }
53    
54     ST_B_SPLINE::ST_B_SPLINE(int bs_degre,std::vector<double> &vec_knots,std::vector<double> &vec_point,std::vector<double> &vec_poids):ST_COURBE(),degre(bs_degre)
55     {
56 francois 283 int nb=vec_knots.size();
57     for (int i=0;i<nb;i++)
58 foucault 27 knots.insert(knots.end(),vec_knots[i]);
59 francois 283 nb_point=vec_point.size()/3;
60     for (int i=0;i<nb_point;i++)
61     {
62     double w=vec_poids[i];
63     double x=vec_point[3*i];
64     double y=vec_point[3*i+1];
65     double z=vec_point[3*i+2];
66     OT_VECTEUR_4D pt(w*x,w*y,w*z,w);
67     ptsctr.push_back(pt);
68     }
69     double xyz1[3];
70     double xyz2[3];
71     xyz1[0]=vec_point[0];
72     xyz1[1]=vec_point[1];
73     xyz1[2]=vec_point[2];
74     xyz2[0]=vec_point[3*nb_point-3];
75     xyz2[1]=vec_point[3*nb_point-2];
76     xyz2[2]=vec_point[3*nb_point-1];
77     periodique=0;
78     if (OPERATEUR::egal (xyz1[0],xyz2[0],1E-6))
79     {
80     if (OPERATEUR::egal (xyz1[1],xyz2[1],1E-6))
81 foucault 27 {
82 francois 283 if (OPERATEUR::egal (xyz1[2],xyz2[2],1E-6))
83 foucault 27 periodique=1;
84     }
85 francois 283 }
86 foucault 27
87 francois 283 if (periodique==1)
88     {
89 foucault 27 int i=knots.size();
90     periode=(knots[i-1]-knots[0]);
91 francois 283 }
92     else periode=0;
93 foucault 27 }
94    
95     ST_B_SPLINE::~ST_B_SPLINE()
96     {
97     }
98    
99     int ST_B_SPLINE::get_intervalle(int nb_point, int degre, double t, std::vector<double> &knots)
100     {
101 francois 283 int inter;
102     if (OPERATEUR::egal(t,knots[nb_point],1E-10)==1) inter=nb_point-1;
103     else if (OPERATEUR::egal(t,knots[degre],1E-10)==1) inter=degre;
104     else
105     {
106 foucault 27 int low=degre;
107     int high=nb_point+1;
108     int mid=((low+high)/2);
109     while ((t<knots[mid-1]) || (t>=knots[mid]))
110 francois 283 {
111     if (t<knots[mid-1]) high=mid;
112     else low=mid;
113     mid=(low+high)/2;
114     }
115 foucault 27 inter=mid-1;
116 francois 283 }
117     return inter;
118 foucault 27 }
119    
120 francois 283 void ST_B_SPLINE::binomialCoef(double * Bin, int d) {
121     int n,k;
122     // Setup the first line
123     Bin[0] = 1.0 ;
124     for (k=d-1;k>0;--k)
125     Bin[k] = 0.0 ;
126     // Setup the other lines
127     for (n=0;n<d-1;n++) {
128     Bin[(n+1)*d+0] = 1.0 ;
129     for (k=1;k<d;k++)
130     if (n+1<k)
131     Bin[(n+1)*d+k] = 0.0 ;
132     else
133     Bin[(n+1)*d+k] = Bin[n*d+k] + Bin[n*d+k-1] ;
134     }
135 foucault 27 }
136    
137     void ST_B_SPLINE::get_valeur_fonction(int inter, double t, int degre, std::vector<double> &knots,double *grand_n)
138     {
139 francois 283 double saved;
140     grand_n[0]=1.0;
141     double gauche[16];
142     double droite[16];
143     for (int j=1;j<=degre;j++)
144     {
145 foucault 27 gauche[j-1]= t-knots[inter-j+1];
146     droite[j-1]=knots[inter+j]-t;
147     saved=0.0;
148     for (int r=0;r<j;r++)
149 francois 283 {
150     double temp=grand_n[r]/(droite[r]+ gauche[j-r-1]);
151     grand_n[r]=saved+droite[r]* temp;
152     saved=gauche[j-r-1]*temp;
153     }
154 foucault 27 grand_n[j]=saved;
155 francois 283 }
156 foucault 27 }
157    
158    
159     void ST_B_SPLINE::evaluer(double t,double *xyz)
160     {
161 francois 283 if (est_periodique())
162     {
163     double tmin=get_tmin();
164     double tmax=get_tmax();
165     while (t>tmax) t -= periode;
166     while (t<tmin) t += periode;
167     }
168     int inter = get_intervalle(nb_point,degre,t, knots);
169     double grand_n[256];
170     get_valeur_fonction(inter,t,degre,knots,grand_n);
171 foucault 27
172 francois 283 OT_VECTEUR_4D sp(0,0,0,0) ;
173     for (int j=0; j<=degre; j++)
174     {
175     sp += grand_n[j] * ptsctr[inter-degre+j];
176     }
177    
178     // transform homogeneous coordinates to 3D coordinates
179     for (int i=0; i<3; i++)
180     xyz[i] = sp[i]/sp.w();
181 foucault 27 }
182 francois 283 /*
183 foucault 27 void ST_B_SPLINE::get_tout_fonction(int inter, double t, int degre, std::vector<double> &knots,double *grand_n)
184     {
185     #define grand_n(i,j) (*(grand_n+(i)*(degre+1)+j))
186     double saved;
187     grand_n(0,0)=1.0;
188     double gauche[16];
189     double droite[16];
190     for (int j=1;j<=degre;j++)
191 francois 283 {
192     gauche[j-1]= t-knots[inter-j+1];
193     droite[j-1]=knots[inter+j]-t;
194     saved=0.0;
195     for (int r=0;r<j;r++)
196     {
197     grand_n(j,r)=(droite[r]+ gauche[j-r-1]);
198     double temp = grand_n(r,j-1)/grand_n(j,r);
199 foucault 27
200 francois 283 grand_n(r,j)= saved+droite[r]*temp;
201     saved=gauche[j-r-1]*temp;
202     }
203     grand_n(j,j)=saved;
204     }
205 foucault 27 #undef grand_n // enlever (i,j)
206     } */
207    
208     void ST_B_SPLINE::deriver_fonction(int inter,double t,int degre,int dd,std::vector<double> &knots,double *f_deriver)
209     {
210     #define f_deriver(i,j) (*(f_deriver+(i)*(degre+1)+j))
211     #define grand_n(i,j) (*(grand_n+(i)*(degre+1)+j))
212     #define a(i,j) (*(a+(i)*(dd+1)+j))
213 francois 283 double *grand_n=new double[(degre+1)*(degre+1)];
214     double saved;
215     grand_n(0,0)=1.0;
216     double *gauche=new double[degre+1];
217     double *droite=new double[degre+1];
218     for (int j=1;j<=degre;j++)
219     {
220 foucault 27 gauche[j]= t-knots[inter-j+1];
221     droite[j]=knots[inter+j]-t;
222     saved=0.0;
223     for (int r=0;r<j;r++)
224 francois 283 {
225     grand_n(j,r)=(droite[r+1]+ gauche[j-r]);
226     double temp = grand_n(r,j-1)/grand_n(j,r);
227 foucault 27
228 francois 283 grand_n(r,j)= saved+droite[r+1]*temp;
229     saved=gauche[j-r]*temp;
230     }
231 foucault 27 grand_n(j,j)=saved;
232 francois 283 }
233     for (int j=0;j<=degre;j++)
234     {
235 foucault 27 f_deriver(0,j)= grand_n(j,degre);
236 francois 283 }
237     double *a=new double[(degre+1)*(degre+1)];
238     for (int r=0;r<=degre;r++)
239     {
240     int s1=0;
241     int s2=1;
242 foucault 27 a(0,0)=1.0;
243     for (int k=1;k<=dd; k++)
244 francois 283 {
245     double d=0.0;
246     int rk=r-k;
247     int pk=degre-k;
248     if (r>=k)
249     {
250     a(s2,0)=a(s1,0)/grand_n(pk+1,rk);
251     d= a(s2,0)* grand_n(rk,pk);
252     }
253     int j1;
254     int j2;
255     if (rk>=-1) j1=1;
256     else j1= -rk;
257     if (r-1<=pk) j2=k-1;
258     else j2=degre-r;
259     for (int j=j1;j<=j2;j++)
260     {
261     a(s2,j) = (a(s1,j)-a(s1,j-1))/grand_n(pk+1,rk+j);
262     d+=a(s2,j)*grand_n(rk+j,pk);
263     }
264     if (r<=pk)
265     {
266     a(s2,k) = -a(s1,k-1)/grand_n(pk+1,r);
267     d+=a(s2,k)*grand_n(r,pk);
268     }
269     f_deriver(k,r)=d;
270     int j=s1;
271     s1=s2;
272     s2=j;
273 foucault 27 }
274 francois 283 }
275     int r=degre;
276     for (int k=1;k<=dd;k++)
277     {
278     for (int j=0;j<=degre;j++)
279 foucault 27 {
280 francois 283 f_deriver(k,j)*=r;
281     }
282 foucault 27 r*=(degre-k);
283 francois 283 }
284     delete [] a;
285     delete [] grand_n;
286     delete [] gauche;
287     delete [] droite;
288 foucault 27 #undef f_deriver
289     #undef grand_n
290 francois 283 #undef a
291 foucault 27 }
292    
293     void ST_B_SPLINE::deriver_bs_kieme(int nb_point,int degre,std::vector<double> &knots,std::vector<OT_VECTEUR_4D> &ptsctr_x,double t,int d, OT_VECTEUR_4D * CK)
294     {
295 francois 283 int du = std::min(d,degre);
296     int inter = get_intervalle(nb_point,degre,t, knots);
297     double derF[256];
298     deriver_fonction(inter, t,degre,du,knots,derF);
299 foucault 27
300     #define derF(i,j) (*(derF+(i)*(degre+1)+j))
301 francois 283 for (int k=du;k>=0;--k)
302     {
303     CK[k] = OT_VECTEUR_4D(0,0,0,0);
304     for (int j=degre;j>=0;--j)
305 foucault 27 {
306 francois 283 CK[k] += derF(k,j)*ptsctr_x[inter-degre+j];
307 foucault 27 }
308 francois 283 }
309 foucault 27 #undef derF
310     }
311    
312    
313    
314     void ST_B_SPLINE::deriver_kieme(double t,int d, double *CK_x,double *CK_y,double *CK_z)
315     {
316 francois 283 int i,k;
317     OT_VECTEUR_4D dersW[16], ders[16];
318     OT_VECTEUR_4D v;
319     deriver_bs_kieme(nb_point, degre, knots, ptsctr, t, d, dersW);
320    
321     double Bin[256];
322     int dbin=d+1;
323     binomialCoef(Bin, dbin);
324 foucault 27 #define Bin(i,j) (*(Bin+(i)*(dbin)+j))
325    
326 francois 283 for (k=0;k<=d;k++)
327 foucault 27 {
328     ders[k] = dersW[k];
329 francois 283 for (i=k ;i>0 ;--i)
330 foucault 27 ders[k] -= (Bin(k,i)*dersW[i].w())*ders[k-i];
331     ders[k]/=dersW[0].w();
332     }
333    
334     for (int i=0; i<=d; i++)
335     {
336     CK_x[i]=ders[i][0];
337     CK_y[i]=ders[i][1];
338     CK_z[i]=ders[i][2];
339     }
340     }
341    
342     void ST_B_SPLINE::deriver(double t,double *dxyz)
343     {
344 francois 283 if (est_periodique())
345     {
346     double tmin=get_tmin();
347     double tmax=get_tmax();
348     while (t>tmax) t -= periode;
349     while (t<tmin) t += periode;
350     }
351     double CK_x[2];
352     double CK_y[2];
353     double CK_z[2];
354     deriver_kieme(t,1,CK_x,CK_y,CK_z);
355     dxyz[0]=CK_x[1];
356     dxyz[1]=CK_y[1];
357     dxyz[2]=CK_z[1];
358 foucault 27 }
359    
360     void ST_B_SPLINE::deriver_seconde(double t,double *ddxyz,double* dxyz ,double* xyz )
361     {
362 francois 283 if (est_periodique())
363     {
364     double tmin=get_tmin();
365     double tmax=get_tmax();
366     while (t>tmax) t -= periode;
367     while (t<tmin) t += periode;
368     }
369     double CK_x[3];
370     double CK_y[3];
371     double CK_z[3];
372     deriver_kieme(t,2,CK_x,CK_y,CK_z);
373     ddxyz[0]=CK_x[2];
374     ddxyz[1]=CK_y[2];
375     ddxyz[2]=CK_z[2];
376     if (dxyz!=NULL)
377     {
378 foucault 27 dxyz[0]=CK_x[1];
379     dxyz[1]=CK_y[1];
380     dxyz[2]=CK_z[1];
381 francois 283 }
382     if (xyz!=NULL)
383     {
384 foucault 27 xyz[0]=CK_x[0];
385     xyz[1]=CK_y[0];
386     xyz[2]=CK_z[0];
387 francois 283 }
388 foucault 27 }
389    
390    
391    
392     void ST_B_SPLINE::inverser(double& t,double *xyz,double precision)
393     {
394 francois 283 int code;
395     int num_point=nb_point;
396     do
397     {
398 foucault 27 code=inverser2(t,xyz,num_point,precision);
399     num_point=num_point*2;
400 francois 283 }
401     while (code==0 && num_point < 100000);
402 foucault 27 }
403    
404     int ST_B_SPLINE::inverser2(double& t,double *xyz,int num_test,double precision)
405     {
406 francois 283 double xyz1[3];
407     double dxyz1[3];
408     double ddxyz1[3];
409     double ti;
410     double eps;
411     double tmin=get_tmin();
412     double tmax=get_tmax();
413     OT_VECTEUR_3D Pt(xyz[0],xyz[1],xyz[2]);
414     double distance_ref=1e308;
415     int ref;
416 foucault 27
417 francois 283 for (int i=0;i<num_test+1;i++)
418     {
419 foucault 27 double t=tmin+i*1./num_test*(tmax-tmin);
420     evaluer(t,xyz1);
421     OT_VECTEUR_3D Ct(xyz1[0],xyz1[1],xyz1[2]);
422     OT_VECTEUR_3D Distance = Ct-Pt;
423     double longueur=Distance.get_longueur2();
424     if (longueur<distance_ref)
425 francois 283 {
426     distance_ref=longueur;
427     ref=i;
428 foucault 27 }
429 francois 283 }
430 foucault 27
431 francois 283 double tii=tmin+ref*1./num_test*(tmax-tmin);
432     int compteur=0;
433     do
434     {
435 foucault 27 compteur++;
436     ti=tii;
437     deriver_seconde(ti,ddxyz1,dxyz1,xyz1);
438     OT_VECTEUR_3D Ct(xyz1[0],xyz1[1],xyz1[2]);
439     OT_VECTEUR_3D Ct_deriver(dxyz1[0],dxyz1[1],dxyz1[2]);
440     OT_VECTEUR_3D Ct_deriver_seconde(ddxyz1[0],ddxyz1[1],ddxyz1[2]);
441     OT_VECTEUR_3D Distance = Ct-Pt;
442     tii=ti-Ct_deriver*Distance/(Ct_deriver_seconde*Distance+Ct_deriver.get_longueur2());
443     if (periodique==1)
444 francois 283 {
445     if (tii<get_tmin()) tii=get_tmax()-(get_tmin()-tii);
446     if (tii>get_tmax()) tii=get_tmin()+(tii-get_tmax());
447     }
448 foucault 27 else
449 francois 283 {
450     if (tii<get_tmin()) tii=get_tmin();
451     if (tii>get_tmax()) tii=get_tmax();
452     }
453 foucault 27 eps=fabs(tii-ti);
454     if (compteur>500) return 0;
455 francois 283 }
456     while (eps>precision);
457     t=ti;
458     return 1;
459 foucault 27 }
460    
461     double ST_B_SPLINE::get_tmin()
462     {
463 francois 283 return knots[0];
464 foucault 27 }
465     double ST_B_SPLINE::get_tmax()
466     {
467 francois 283 int i=knots.size();
468     return knots[i-1];
469 foucault 27 }
470    
471     double equation_longueur(ST_B_SPLINE& bsp,double t)
472 francois 283 {
473     double dxyz[3];
474     bsp.deriver(t,dxyz);
475     return sqrt(dxyz[0]*dxyz[0]+dxyz[1]*dxyz[1]+dxyz[2]*dxyz[2]);
476     }
477 foucault 27
478    
479     double ST_B_SPLINE::get_longueur(double t1,double t2,double precis)
480     {
481 francois 283 TPL_FONCTION1<double,ST_B_SPLINE,double> longueur_bsp(*this,equation_longueur);
482     return longueur_bsp.integrer_gauss_2(t1,t2);
483 foucault 27 }
484    
485    
486     int ST_B_SPLINE::est_periodique(void)
487     {
488 francois 283 return periodique;
489 foucault 27 }
490     double ST_B_SPLINE::get_periode(void)
491     {
492 francois 283 return periode;
493 foucault 27 }
494    
495     int ST_B_SPLINE::get_type_geometrique(TPL_LISTE_ENTITE<double> &param)
496     {
497 francois 283 for (int i=0;i<nb_point-(degre+1);i++)
498     {
499     param.ajouter(knots[i]);
500     }
501     double xyz[3];
502     for (int i=0;i<nb_point;i++)
503     {
504     xyz[0]=ptsctr[i].x()/ptsctr[i].w();
505     xyz[1]=ptsctr[i].y()/ptsctr[i].w();
506     xyz[2]=ptsctr[i].z()/ptsctr[i].w();
507     param.ajouter(xyz[0]);
508     param.ajouter(xyz[1]);
509     param.ajouter(xyz[2]);
510     }
511     for (int i=0;i<nb_point;i++)
512     {
513     param.ajouter(ptsctr[i].w());
514     }
515     param.ajouter(degre);
516     return MGCo_BSPLINE;
517 foucault 27 }
518    
519     void ST_B_SPLINE::initialiser(ST_GESTIONNAIRE *gest)
520     {
521 francois 283 int i=indexptsctr.size();
522     ST_POINT* point1=gest->lst_point.getid(indexptsctr[0]);
523     ST_POINT* point2=gest->lst_point.getid(indexptsctr[i-1]);
524     double xyz1[3];
525     double xyz2[3];
526     point1->evaluer(xyz1);
527     point2->evaluer(xyz2);
528     periodique=0;
529     if (OPERATEUR::egal (xyz1[0],xyz2[0],1E-6))
530     {
531     if (OPERATEUR::egal (xyz1[1],xyz2[1],1E-6))
532 foucault 27 {
533 francois 283 if (OPERATEUR::egal (xyz1[2],xyz2[2],1E-6))
534 foucault 27 periodique=1;
535     }
536 francois 283 }
537 foucault 27
538 francois 283 if (periodique==1)
539     {
540 foucault 27 int i=knots.size();
541     periode=(knots[i-1]-knots[0]);
542 francois 283 }
543     else periode=0;
544 foucault 27
545 francois 283 int nbptsctr=indexptsctr.size();
546     for (int i=0;i<nbptsctr;i++)
547     {
548 foucault 27 ST_POINT* stpoint=gest->lst_point.getid(indexptsctr[i]);
549     double xyz[3];
550     stpoint->evaluer(xyz);
551     OT_VECTEUR_4D pt(xyz[0],xyz[1],xyz[2],1);
552     ptsctr.insert(ptsctr.end(),pt);
553 francois 283 }
554 foucault 27 }
555    
556    
557    
558    
559    
560     void ST_B_SPLINE::est_util(ST_GESTIONNAIRE* gest)
561     {
562 francois 283 util=true;
563     for (int i=0;i<nb_point;i++)
564 foucault 27 gest->lst_point.getid(indexptsctr[i])->est_util(gest);
565     }
566    
567    
568    
569    
570     void ST_B_SPLINE::get_param_NURBS(int& indx_premier_ptctr,TPL_LISTE_ENTITE<double> &param)
571     {
572    
573     // the order stock of the parameters is:
574     // 1/ the variante type =1 (dimension one)
575     // 2/ the order of the spline in two dirction the second direction is zero
576     // 3/ the number of the control points
577     // 4/ the knots vector
578     // 5/ the control points cordinates in homogeneos forms
579    
580     // The first parameter indicate the code access
581 francois 283 param.ajouter(1);
582 foucault 27 // The follewing two parameters of the list indicate the orders of the net points
583    
584 francois 283 param.ajouter(degre+1);
585     param.ajouter(0);
586 foucault 27
587     // The follewing two parameters indicate the number of rows and colons of the control points
588     // respectively to the two parameters directions
589    
590 francois 283 param.ajouter(nb_point);
591     param.ajouter(0);
592 foucault 27
593     // this present the knot vector in the u-direction
594    
595 francois 283 for (unsigned int i=0;i<knots.size();i++)
596     {
597     param.ajouter(knots[i]);
598     }
599 foucault 27
600     // in the following we construct the control point vector
601    
602 francois 283 for (unsigned int pt=0;pt<ptsctr.size();pt++)
603     {
604     double w=ptsctr[pt].w();
605     double inv_w=1/w;
606     double x=ptsctr[pt].x()*inv_w;
607     double y=ptsctr[pt].y()*inv_w;
608     double z=ptsctr[pt].z()*inv_w;
609     param.ajouter(x);
610     param.ajouter(y);
611     param.ajouter(z);
612     param.ajouter(w);
613     }
614     indx_premier_ptctr=5+knots.size();
615     }