step/src/stbspline.cpp

//####//------------------------------------------------------------
//####//------------------------------------------------------------
//####//  MAGiC
//####//  Jean Christophe Cuilliere et Vincent FRANCOIS
//####//  Departement de Genie Mecanique - UQTR
//####//------------------------------------------------------------
//####//  MAGIC est un projet de recherche de l equipe ERICCA
//####//  du departement de genie mecanique de l Universite du Quebec a Trois Rivieres
//####//  http://www.uqtr.ca/ericca
//####//  http://www.uqtr.ca/
//####//------------------------------------------------------------
//####//------------------------------------------------------------
//####//
//####//       stbspline.cpp
//####//
//####//------------------------------------------------------------
//####//------------------------------------------------------------
//####//    COPYRIGHT 2000-2024
//####//  jeu 13 jun 2024 11:53:59 EDT
//####//------------------------------------------------------------
//####//------------------------------------------------------------


#include "stbspline.h"
#include <vector>
#include "st_gestionnaire.h"
#include "tpl_fonction.h"
#include "constantegeo.h"

#include <math.h>


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)
{
    int nb=bs_knots_multiplicities.size();
    for (int i=0;i<nb;i++)
    {
        for (int j=0;j<bs_knots_multiplicities[i];j++)
            knots.insert(knots.end(),bs_knots[i]);
    }
    nb_point=bs_indexptsctr.size();
    for (int i=0;i<nb_point;i++)
    {
        indexptsctr.insert(indexptsctr.end(),bs_indexptsctr[i]);
        //poids.insert(poids.end(),1.);
    }

}

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)
{
    int nb=vec_knots.size();
    for (int i=0;i<nb;i++)
        knots.insert(knots.end(),vec_knots[i]);
    nb_point=vec_point.size()/3;
    for (int i=0;i<nb_point;i++)
    {
        double w=vec_poids[i];
        double x=vec_point[3*i];
        double y=vec_point[3*i+1];
        double z=vec_point[3*i+2];
        OT_VECTEUR_4D pt(w*x,w*y,w*z,w);
        ptsctr.push_back(pt);
    }
    double xyz1[3];
    double xyz2[3];
    xyz1[0]=vec_point[0];
    xyz1[1]=vec_point[1];
    xyz1[2]=vec_point[2];
    xyz2[0]=vec_point[3*nb_point-3];
    xyz2[1]=vec_point[3*nb_point-2];
    xyz2[2]=vec_point[3*nb_point-1];
    periodique=0;
    if (OPERATEUR::egal (xyz1[0],xyz2[0],1E-6))
    {
        if (OPERATEUR::egal (xyz1[1],xyz2[1],1E-6))
        {
            if (OPERATEUR::egal (xyz1[2],xyz2[2],1E-6))
                periodique=1;
        }
    }

    if (periodique==1)
    {
        int i=knots.size();
        periode=(knots[i-1]-knots[0]);
    }
    else periode=0;
}

ST_B_SPLINE::~ST_B_SPLINE()
{
}

int  ST_B_SPLINE::get_intervalle(int nb_point, int degre, double t, std::vector<double> &knots)
{
    int inter;
    if (OPERATEUR::egal(t,knots[nb_point],1E-10)==1) inter=nb_point-1;
    else if (OPERATEUR::egal(t,knots[degre],1E-10)==1) inter=degre;
    else
    {
        int low=degre;
        int high=nb_point+1;
        int mid=((low+high)/2);
        while ((t<knots[mid-1]) || (t>=knots[mid]))
        {
            if (t<knots[mid-1]) high=mid;
            else low=mid;
            mid=(low+high)/2;
        }
        inter=mid-1;
    }
    return inter;
}

void ST_B_SPLINE::binomialCoef(double * Bin, int d) {
    int n,k;
    // Setup the first line
    Bin[0] = 1.0 ;
    for (k=d-1;k>0;--k)
        Bin[k] = 0.0 ;
    // Setup the other lines
    for (n=0;n<d-1;n++) {
        Bin[(n+1)*d+0] = 1.0 ;
        for (k=1;k<d;k++)
            if (n+1<k)
                Bin[(n+1)*d+k] = 0.0 ;
            else
                Bin[(n+1)*d+k] = Bin[n*d+k] + Bin[n*d+k-1] ;
    }
}

void  ST_B_SPLINE::get_valeur_fonction(int inter, double t, int degre, std::vector<double> &knots,double *grand_n)
{
    double saved;
    grand_n[0]=1.0;
    double gauche[16];
    double droite[16];
    for (int j=1;j<=degre;j++)
    {
        gauche[j-1]= t-knots[inter-j+1];
        droite[j-1]=knots[inter+j]-t;
        saved=0.0;
        for (int r=0;r<j;r++)
        {
            double temp=grand_n[r]/(droite[r]+ gauche[j-r-1]);
            grand_n[r]=saved+droite[r]* temp;
            saved=gauche[j-r-1]*temp;
        }
        grand_n[j]=saved;
    }
}


void  ST_B_SPLINE::evaluer(double t,double *xyz)
{
    if (est_periodique())
    {
        double tmin=get_tmin();
        double tmax=get_tmax();
        while (t>tmax) t -= periode;
        while (t<tmin) t += periode;
    }
    int  inter = get_intervalle(nb_point,degre,t, knots);
    double grand_n[256];
    get_valeur_fonction(inter,t,degre,knots,grand_n);

    OT_VECTEUR_4D sp(0,0,0,0) ;
    for (int j=0; j<=degre; j++)
    {
        sp += grand_n[j] * ptsctr[inter-degre+j];
    }

    // transform homogeneous coordinates to 3D coordinates
    for (int i=0; i<3; i++)
        xyz[i] = sp[i]/sp.w();
}
/*
void  ST_B_SPLINE::get_tout_fonction(int inter, double t, int degre, std::vector<double> &knots,double *grand_n)
{
#define grand_n(i,j) (*(grand_n+(i)*(degre+1)+j))
double saved;
grand_n(0,0)=1.0;
double gauche[16];
double droite[16];
for (int j=1;j<=degre;j++)
{
gauche[j-1]= t-knots[inter-j+1];
droite[j-1]=knots[inter+j]-t;
saved=0.0;
for (int r=0;r<j;r++)
{
grand_n(j,r)=(droite[r]+ gauche[j-r-1]);
double temp = grand_n(r,j-1)/grand_n(j,r);

grand_n(r,j)= saved+droite[r]*temp;
saved=gauche[j-r-1]*temp;
}
grand_n(j,j)=saved;
}
#undef grand_n // enlever (i,j)
}     */

void  ST_B_SPLINE::deriver_fonction(int inter,double t,int degre,int dd,std::vector<double> &knots,double *f_deriver)
{
#define f_deriver(i,j) (*(f_deriver+(i)*(degre+1)+j))
#define grand_n(i,j) (*(grand_n+(i)*(degre+1)+j))
#define a(i,j) (*(a+(i)*(dd+1)+j))
    double *grand_n=new double[(degre+1)*(degre+1)];
    double saved;
    grand_n(0,0)=1.0;
    double *gauche=new double[degre+1];
    double *droite=new double[degre+1];
    for (int j=1;j<=degre;j++)
    {
        gauche[j]= t-knots[inter-j+1];
        droite[j]=knots[inter+j]-t;
        saved=0.0;
        for (int r=0;r<j;r++)
        {
            grand_n(j,r)=(droite[r+1]+ gauche[j-r]);
            double temp = grand_n(r,j-1)/grand_n(j,r);

            grand_n(r,j)= saved+droite[r+1]*temp;
            saved=gauche[j-r]*temp;
        }
        grand_n(j,j)=saved;
    }
    for (int j=0;j<=degre;j++)
    {
        f_deriver(0,j)= grand_n(j,degre);
    }
    double *a=new double[(degre+1)*(degre+1)];
    for (int r=0;r<=degre;r++)
    {
        int s1=0;
        int s2=1;
        a(0,0)=1.0;
        for (int k=1;k<=dd; k++)
        {
            double d=0.0;
            int rk=r-k;
            int pk=degre-k;
            if (r>=k)
            {
                a(s2,0)=a(s1,0)/grand_n(pk+1,rk);
                d= a(s2,0)* grand_n(rk,pk);
            }
            int j1;
            int j2;
            if (rk>=-1)   j1=1;
            else j1= -rk;
            if (r-1<=pk)  j2=k-1;
            else j2=degre-r;
            for (int j=j1;j<=j2;j++)
            {
                a(s2,j) = (a(s1,j)-a(s1,j-1))/grand_n(pk+1,rk+j);
                d+=a(s2,j)*grand_n(rk+j,pk);
            }
            if (r<=pk)
            {
                a(s2,k) = -a(s1,k-1)/grand_n(pk+1,r);
                d+=a(s2,k)*grand_n(r,pk);
            }
            f_deriver(k,r)=d;
            int j=s1;
            s1=s2;
            s2=j;
        }
    }
    int r=degre;
    for (int k=1;k<=dd;k++)
    {
        for (int j=0;j<=degre;j++)
        {
            f_deriver(k,j)*=r;
        }
        r*=(degre-k);
    }
    delete [] a;
    delete [] grand_n;
    delete [] gauche;
    delete [] droite;
#undef f_deriver
#undef grand_n
#undef a
}

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)
{
    int du = std::min(d,degre);
    int  inter = get_intervalle(nb_point,degre,t, knots);
    double derF[256];
    deriver_fonction(inter, t,degre,du,knots,derF);

#define derF(i,j) (*(derF+(i)*(degre+1)+j))
    for (int k=du;k>=0;--k)
    {
        CK[k] = OT_VECTEUR_4D(0,0,0,0);
        for (int j=degre;j>=0;--j)
        {
            CK[k] += derF(k,j)*ptsctr_x[inter-degre+j];
        }
    }
#undef derF
}


void  ST_B_SPLINE::deriver_kieme(double t,int d, double *CK_x,double *CK_y,double *CK_z)
{
    int i,k;
    OT_VECTEUR_4D dersW[16], ders[16];
    OT_VECTEUR_4D v;
    deriver_bs_kieme(nb_point, degre, knots, ptsctr, t, d, dersW);

    double Bin[256];
    int dbin=d+1;
    binomialCoef(Bin, dbin);
#define Bin(i,j) (*(Bin+(i)*(dbin)+j))

    for (k=0;k<=d;k++)
    {
        ders[k] = dersW[k];
        for (i=k ;i>0 ;--i)
            ders[k] -= (Bin(k,i)*dersW[i].w())*ders[k-i];
        ders[k]/=dersW[0].w();
    }

    for (int i=0; i<=d; i++)
    {
        CK_x[i]=ders[i][0];
        CK_y[i]=ders[i][1];
        CK_z[i]=ders[i][2];
    }
}

void  ST_B_SPLINE::deriver(double t,double *dxyz)
{
    if (est_periodique())
    {
        double tmin=get_tmin();
        double tmax=get_tmax();
        while (t>tmax) t -= periode;
        while (t<tmin) t += periode;
    }
    double CK_x[2];
    double CK_y[2];
    double CK_z[2];
    deriver_kieme(t,1,CK_x,CK_y,CK_z);
    dxyz[0]=CK_x[1];
    dxyz[1]=CK_y[1];
    dxyz[2]=CK_z[1];
}

void  ST_B_SPLINE::deriver_seconde(double t,double *ddxyz,double* dxyz ,double* xyz )
{
    if (est_periodique())
    {
        double tmin=get_tmin();
        double tmax=get_tmax();
        while (t>tmax) t -= periode;
        while (t<tmin) t += periode;
    }
    double CK_x[3];
    double CK_y[3];
    double CK_z[3];
    deriver_kieme(t,2,CK_x,CK_y,CK_z);
    ddxyz[0]=CK_x[2];
    ddxyz[1]=CK_y[2];
    ddxyz[2]=CK_z[2];
    if (dxyz!=NULL)
    {
        dxyz[0]=CK_x[1];
        dxyz[1]=CK_y[1];
        dxyz[2]=CK_z[1];
    }
    if (xyz!=NULL)
    {
        xyz[0]=CK_x[0];
        xyz[1]=CK_y[0];
        xyz[2]=CK_z[0];
    }
}


void  ST_B_SPLINE::inverser(double& t,double *xyz,double precision)
{
    int code;
    int num_point=nb_point;
    do
    {
        code=inverser2(t,xyz,num_point,precision);
        num_point=num_point*2;
    }
    while (code==0 && num_point < 100000);
}

int  ST_B_SPLINE::inverser2(double& t,double *xyz,int num_test,double precision)
{
    double xyz1[3];
    double dxyz1[3];
    double ddxyz1[3];
    double ti;
    double eps;
    double tmin=get_tmin();
    double tmax=get_tmax();
    OT_VECTEUR_3D Pt(xyz[0],xyz[1],xyz[2]);
    double distance_ref=1e308;
    int ref;

    for (int i=0;i<num_test+1;i++)
    {
        double t=tmin+i*1./num_test*(tmax-tmin);
        evaluer(t,xyz1);
        OT_VECTEUR_3D Ct(xyz1[0],xyz1[1],xyz1[2]);
        OT_VECTEUR_3D Distance = Ct-Pt;
        double longueur=Distance.get_longueur2();
        if (longueur<distance_ref)
        {
            distance_ref=longueur;
            ref=i;
        }
    }

    double tii=tmin+ref*1./num_test*(tmax-tmin);
    int compteur=0;
    do
    {
        compteur++;
        ti=tii;
        deriver_seconde(ti,ddxyz1,dxyz1,xyz1);
        OT_VECTEUR_3D Ct(xyz1[0],xyz1[1],xyz1[2]);
        OT_VECTEUR_3D Ct_deriver(dxyz1[0],dxyz1[1],dxyz1[2]);
        OT_VECTEUR_3D Ct_deriver_seconde(ddxyz1[0],ddxyz1[1],ddxyz1[2]);
        OT_VECTEUR_3D Distance = Ct-Pt;
        tii=ti-Ct_deriver*Distance/(Ct_deriver_seconde*Distance+Ct_deriver.get_longueur2());
        if (periodique==1)
        {
            if (tii<get_tmin()) tii=get_tmax()-(get_tmin()-tii);
            if (tii>get_tmax()) tii=get_tmin()+(tii-get_tmax());
        }
        else
        {
            if (tii<get_tmin()) tii=get_tmin();
            if (tii>get_tmax()) tii=get_tmax();
        }
        eps=fabs(tii-ti);
        if (compteur>500) return 0;
    }
    while (eps>precision);
    t=ti;
    return 1;
}

double  ST_B_SPLINE::get_tmin()
{
    return knots[0];
}
double  ST_B_SPLINE::get_tmax()
{
    int i=knots.size();
    return knots[i-1];
}

double equation_longueur(ST_B_SPLINE& bsp,double t)
{
    double dxyz[3];
    bsp.deriver(t,dxyz);
    return sqrt(dxyz[0]*dxyz[0]+dxyz[1]*dxyz[1]+dxyz[2]*dxyz[2]);
}


double ST_B_SPLINE::get_longueur(double t1,double t2,double precis)
{
    TPL_FONCTION1<double,ST_B_SPLINE,double> longueur_bsp(*this,equation_longueur);
    return longueur_bsp.integrer_gauss_2(t1,t2);
}


int ST_B_SPLINE::est_periodique(void)
{
    return periodique;
}
double ST_B_SPLINE::get_periode(void)
{
    return periode;
}

int ST_B_SPLINE::get_type_geometrique(TPL_LISTE_ENTITE<double> &param)
{
    for (int i=0;i<nb_point-(degre+1);i++)
    {
        param.ajouter(knots[i]);
    }
    double xyz[3];
    for (int i=0;i<nb_point;i++)
    {
        xyz[0]=ptsctr[i].x()/ptsctr[i].w();
        xyz[1]=ptsctr[i].y()/ptsctr[i].w();
        xyz[2]=ptsctr[i].z()/ptsctr[i].w();
        param.ajouter(xyz[0]);
        param.ajouter(xyz[1]);
        param.ajouter(xyz[2]);
    }
    for (int i=0;i<nb_point;i++)
    {
        param.ajouter(ptsctr[i].w());
    }
    param.ajouter(degre);
    return GEOMETRIE::CONST::Co_BSPLINE;
}

void ST_B_SPLINE::initialiser(ST_GESTIONNAIRE *gest)
{
    int i=indexptsctr.size();
    ST_POINT* point1=gest->lst_point.getid(indexptsctr[0]);
    ST_POINT* point2=gest->lst_point.getid(indexptsctr[i-1]);
    double xyz1[3];
    double xyz2[3];
    point1->evaluer(xyz1);
    point2->evaluer(xyz2);
    periodique=0;
    if (OPERATEUR::egal (xyz1[0],xyz2[0],1E-6))
    {
        if (OPERATEUR::egal (xyz1[1],xyz2[1],1E-6))
        {
            if (OPERATEUR::egal (xyz1[2],xyz2[2],1E-6))
                periodique=1;
        }
    }

    if (periodique==1)
    {
        int i=knots.size();
        periode=(knots[i-1]-knots[0]);
    }
    else periode=0;

    int nbptsctr=indexptsctr.size();
    for (int i=0;i<nbptsctr;i++)
    {
        ST_POINT* stpoint=gest->lst_point.getid(indexptsctr[i]);
        double xyz[3];
        stpoint->evaluer(xyz);
        OT_VECTEUR_4D pt(xyz[0],xyz[1],xyz[2],1);
        ptsctr.insert(ptsctr.end(),pt);
    }
}


void ST_B_SPLINE::est_util(ST_GESTIONNAIRE* gest)
{
    util=true;
    for (int i=0;i<nb_point;i++)
        gest->lst_point.getid(indexptsctr[i])->est_util(gest);
}


void ST_B_SPLINE::get_param_NURBS(int& indx_premier_ptctr,TPL_LISTE_ENTITE<double> &param)
{


    param.ajouter(1);

    param.ajouter(degre+1);
    param.ajouter(0);


    param.ajouter(nb_point);
    param.ajouter(0);


    for (unsigned int i=0;i<knots.size();i++)
    {
        param.ajouter(knots[i]);
    }


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