diff --git a/Makefile b/Makefile index 2cd1a5c..0639f92 100644 --- a/Makefile +++ b/Makefile @@ -3,7 +3,7 @@ INCLUDES=-I. CFLAGS= -O3 $(INCLUDES) -OBJS= main.o image.o dsp.o filter.o reg.o fcolor.o +OBJS= main.o image.o dsp.o filter.o reg.o fcolor.o dres.o atpdec: $(OBJS) $(CC) -o $@ $(OBJS) -lm -lsndfile -lpng @@ -13,6 +13,7 @@ dsp.o: dsp.c filtercoeff.h filter.h filter.o: filter.c filter.h image.o: image.c satcal.h fcolor.o : fcolor.c +dres.o : dres.c clean: rm -f *.o atpdec diff --git a/dsp.c b/dsp.c index d023a58..86bc087 100755 --- a/dsp.c +++ b/dsp.c @@ -1,207 +1,195 @@ -/* - * Atpdec - * Copyright (c) 2003 by Thierry Leconte (F4DWV) - * - * $Id$ - * - * This library is free software; you can redistribute it and/or modify - * it under the terms of the GNU Library General Public License as - * published by the Free Software Foundation; either version 2 of - * the License, or (at your option) any later version. - * - * This program is distributed in the hope that it will be useful, - * but WITHOUT ANY WARRANTY; without even the implied warranty of - * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the - * GNU Library General Public License for more details. - * - * You should have received a copy of the GNU Library General Public - * License along with this library; if not, write to the Free Software - * Foundation, Inc., 675 Mass Ave, Cambridge, MA 02139, USA. - * - */ -#include -#ifndef M_PI -#define M_PI 3.14159265358979323846 /* for OS that don't know it */ -#endif -#include "filter.h" -#include "filtercoeff.h" - -#define Fe 11025.0 -#define Fc 2400.0 -#define DFc 50.0 -#define PixelLine 2080 -#define Fp (2*PixelLine) -#define RSMULT 10 -#define Fi (Fp*RSMULT) - -static double FreqOsc=Fc/Fe; -static double FreqLine=1.0; -extern int getsample(float *inbuff ,int nb); - -static float pll(float In) -{ -/* pll coeff */ -#define K1 5e-3 -#define K2 3e-6 - -static double PhaseOsc=0.0; -static iirbuff_t Ifilterbuff,Qfilterbuff; - -double Io,Qo; -double Ip,Qp; -double DPhi; -double DF; - -/* quadrature oscillator */ -Io=cos(PhaseOsc); Qo=sin(PhaseOsc); - -/* phase detector */ -Ip=iir(In*Io,&Ifilterbuff,&PhaseFilterCf); -Qp=iir(In*Qo,&Qfilterbuff,&PhaseFilterCf); -DPhi=-atan2(Qp,Ip)/M_PI; - -/* loop filter */ -DF=K1*DPhi+FreqOsc; -FreqOsc+=K2*DPhi; - -if(FreqOsc>((Fc+DFc)/Fe)) FreqOsc=(Fc+DFc)/Fe; -if(FreqOsc<((Fc-DFc)/Fe)) FreqOsc=(Fc-DFc)/Fe; - -PhaseOsc+=2.0*M_PI*DF; -if (PhaseOsc > M_PI) PhaseOsc-=2.0*M_PI; -if (PhaseOsc <= -M_PI) PhaseOsc+=2.0*M_PI; - -return (float)(In*Io); -} - -static double fr=Fc/Fe; -static double offset=0.0; - -getamp(float *ambuff,int nb) -{ -#define BLKIN 1024 -float inbuff[BLKIN]; -int n; -int res; - - res=getsample(inbuff,nb>BLKIN?BLKIN:nb); - - for(n=0;n0) - memmove(pixelv,pixels,npv*sizeof(float)); - - if(npvmax) { - mshift=shift; - max=corr; - } - } - if(mshift !=0) { - memmove(pixelv,&(pixelv[mshift]),(npv-mshift)*sizeof(float)); - npv-=mshift; - synced=0; - FreqLine=1.0; - } else - synced+=1; - } - - if(npv +#ifndef M_PI +#define M_PI 3.14159265358979323846 /* for OS that don't know it */ +#endif /* */ +#include "filter.h" +#include "filtercoeff.h" + +#define Fe 11025.0 +#define Fc 2400.0 +#define DFc 50.0 +#define PixelLine 2080 +#define Fp (2*PixelLine) +#define RSMULT 10 +#define Fi (Fp*RSMULT) +static double FreqOsc = Fc / Fe; +static double FreqLine = 1.0; +extern int getsample (float *inbuff, int nb); +static float +pll (float In) +{ + +/* pll coeff */ +#define K1 5e-3 +#define K2 3e-6 + static double PhaseOsc = 0.0; + static iirbuff_t Ifilterbuff, Qfilterbuff; + double Io, Qo; + double Ip, Qp; + double DPhi; + double DF; + +/* quadrature oscillator */ + Io = cos (PhaseOsc); + Qo = sin (PhaseOsc); + +/* phase detector */ + Ip = iir (In * Io, &Ifilterbuff, &PhaseFilterCf); + Qp = iir (In * Qo, &Qfilterbuff, &PhaseFilterCf); + DPhi = -atan2 (Qp, Ip) / M_PI; + +/* loop filter */ + DF = K1 * DPhi + FreqOsc; + FreqOsc += K2 * DPhi; + if (FreqOsc > ((Fc + DFc) / Fe)) + FreqOsc = (Fc + DFc) / Fe; + if (FreqOsc < ((Fc - DFc) / Fe)) + FreqOsc = (Fc - DFc) / Fe; + PhaseOsc += 2.0 * M_PI * DF; + if (PhaseOsc > M_PI) + PhaseOsc -= 2.0 * M_PI; + if (PhaseOsc <= -M_PI) + PhaseOsc += 2.0 * M_PI; + return (float) (In * Io); +} static double fr = Fc / Fe; +static double offset = 0.0; +getamp (float *ambuff, int nb) +{ + +#define BLKIN 1024 + float inbuff[BLKIN]; + int n; + int res; + res = getsample (inbuff, nb > BLKIN ? BLKIN : nb); + for (n = 0; n < res; n++) { + ambuff[n] = pll (inbuff[n]); + fr = 0.25 * FreqOsc + 0.75 * fr; + } + return (res); +} +int +getpixelv (float *pvbuff, int nb) +{ + +#define BLKAMP 256 + static float ambuff[BLKAMP]; + static int nam = 0; + static int idxam = 0; + int n; + for (n = 0; n < nb; n++) { + double mult; + int shift; + if (nam < BLKAMP) { + int res; + memmove (ambuff, &(ambuff[idxam]), nam * sizeof (float)); + idxam = 0; + res = getamp (&(ambuff[nam]), BLKAMP - nam); + nam += res; + if (nam < BLKAMP) + return (n); + } + mult = (double) Fi *fr / Fc * FreqLine; + pvbuff[n] = + rsfir (&(ambuff[idxam]), rsfilter, RSFilterLen, offset, + mult) * mult * 2 * 256.0; + shift = (int) ((RSMULT - offset + mult - 1) / mult); + offset = shift * mult + offset - RSMULT; + idxam += shift; + nam -= shift; + } return (nb); +} +int +getpixelrow (float *pixelv) +{ + static float pixels[PixelLine + SyncFilterLen]; + static int npv = 0; + static int synced = 0; + static double max = 0.0; + double corr, ecorr, lcorr; + double ph; + int n, res; + if (npv > 0) + memmove (pixelv, pixels, npv * sizeof (float)); + if (npv < SyncFilterLen + 2) { + res = getpixelv (&(pixelv[npv]), SyncFilterLen + 2 - npv); + npv += res; + if (npv < SyncFilterLen + 2) + return (0); + } + + /* test sync */ + corr = fir (&(pixelv[1]), Sync, SyncFilterLen); + ecorr = fir (pixelv, Sync, SyncFilterLen); + lcorr = fir (&(pixelv[2]), Sync, SyncFilterLen); + FreqLine = 1.0 + (ecorr - lcorr) / corr / PixelLine / 4.0; + if (corr < 0.75 * max) { + synced = 0; + FreqLine = 1.0; + } + max = corr; + if (synced < 8) { + int shift, mshift; + if (npv < PixelLine + SyncFilterLen) { + res = getpixelv (&(pixelv[npv]), PixelLine + SyncFilterLen - npv); + npv += res; + if (npv < PixelLine + SyncFilterLen) + return (0); + } + + /* lookup sync start */ + mshift = 0; + for (shift = 1; shift < PixelLine; shift++) { + double corr; + corr = fir (&(pixelv[shift + 1]), Sync, SyncFilterLen); + if (corr > max) { + mshift = shift; + max = corr; + } + } + if (mshift != 0) { + memmove (pixelv, &(pixelv[mshift]), (npv - mshift) * sizeof (float)); + npv -= mshift; + synced = 0; + FreqLine = 1.0; + } + else + synced += 1; + } + if (npv < PixelLine) { + res = getpixelv (&(pixelv[npv]), PixelLine - npv); + npv += res; + if (npv < PixelLine) + return (0); + } + if (npv == PixelLine) { + npv = 0; + } + else { + memmove (pixels, &(pixelv[PixelLine]), + (npv - PixelLine) * sizeof (float)); + npv -= PixelLine; + } return (1); +} + + diff --git a/filter.c b/filter.c index de98a3b..4c9cd64 100755 --- a/filter.c +++ b/filter.c @@ -18,77 +18,65 @@ * License along with this library; if not, write to the Free Software * Foundation, Inc., 675 Mass Ave, Cambridge, MA 02139, USA. * - */ + */ #include "filter.h" - -float fir( float *buff, const float *coeff, const int len) -{ -int i; -double r; - -r=0.0; -for(i=0;ix[4]=buff->x[3]; -buff->x[3]=buff->x[2]; -buff->x[2]=buff->x[1]; -buff->x[1]=buff->x[0]; -buff->x[0]=x/coeff->G; - -buff->y[2]=buff->y[1]; -buff->y[1]=buff->y[0]; -buff->y[0] = buff->x[4] - + coeff->x[2] * buff->x[3] - + coeff->x[1] * buff->x[2] - + coeff->x[0] * buff->x[1] - + buff->x[0] - + coeff->y[1] * buff->y[2] - + coeff->y[0] * buff->y[1]; - -return (buff->y[0]); -} - - + float +fir (float *buff, const float *coeff, const int len) +{ + int i; + double r; + r = 0.0; + for (i = 0; i < len; i++) { + r += buff[i] * coeff[i]; + } + return r; + } + void +iqfir (float *buff, const float *Icoeff, const float *Qcoeff, const int len, + float *Iptr, float *Qptr) +{ + int i; + float I, Q; + I = Q = 0.0; + for (i = 0; i < len; i++) { + double v; + v = buff[i]; + I += v * Icoeff[i]; + Q += v * Qcoeff[i]; + } *Iptr = I; + *Qptr = Q; + } float +rsfir (float *buff, const float *coeff, const int len, const double offset, + const double delta) +{ + int i; + double n; + double out; + out = 0.0; + for (i = 0, n = offset; n < len - 1; n += delta, i++) { + int k; + double alpha; + k = (int) n; + alpha = n - k; + out += buff[i] * (coeff[k] * (1.0 - alpha) + coeff[k + 1] * alpha); + } return out; + } + double +iir (double x, iirbuff_t * buff, const iircoeff_t * coeff) +{ + buff->x[4] = buff->x[3]; + buff->x[3] = buff->x[2]; + buff->x[2] = buff->x[1]; + buff->x[1] = buff->x[0]; + buff->x[0] = x / coeff->G; + buff->y[2] = buff->y[1]; + buff->y[1] = buff->y[0]; + buff->y[0] = buff->x[4] + +coeff->x[2] * buff->x[3] + +coeff->x[1] * buff->x[2] + +coeff->x[0] * buff->x[1] + +buff->x[0] +coeff->y[1] * buff->y[2] +coeff->y[0] * buff->y[1]; + return (buff->y[0]); + } + + diff --git a/image.c b/image.c index e333d65..e41bc75 100644 --- a/image.c +++ b/image.c @@ -26,29 +26,34 @@ #include #define REGORDER 3 -typedef struct { -double cf[REGORDER+1] ; +typedef struct +{ + double cf[REGORDER + 1]; } rgparam; -static void rgcomp(double x[16], rgparam *rgpr) +static void +rgcomp (double x[16], rgparam * rgpr) { /*{ 0.106,0.215,0.324,0.433,0.542,0.652,0.78,0.87 ,0.0 }; */ -const double y[9] = { 31.07,63.02,94.96,126.9,158.86,191.1,228.62,255.0, 0.0 }; -extern void polyreg(const int m,const int n,const double x[],const double y[],double c[]); + const double y[9] = + { 31.07, 63.02, 94.96, 126.9, 158.86, 191.1, 228.62, 255.0, 0.0 }; + extern void polyreg (const int m, const int n, const double x[], + const double y[], double c[]); -polyreg(REGORDER,9,x,y,rgpr->cf); + polyreg (REGORDER, 9, x, y, rgpr->cf); } -static double rgcal(float x,rgparam *rgpr) +static double +rgcal (float x, rgparam * rgpr) { -double y,p; -int i; - -for(i=0,y=0.0,p=1.0;icf[i]*p; - p=p*x; -} -return(y); + double y, p; + int i; + + for (i = 0, y = 0.0, p = 1.0; i < REGORDER + 1; i++) { + y += rgpr->cf[i] * p; + p = p * x; + } + return (y); } @@ -56,245 +61,272 @@ static double tele[16]; static double Cs; static nbtele; -int Calibrate(float **prow,int nrow,int offset) +int +Calibrate (float **prow, int nrow, int offset) { -double teleline[3000]; -double wedge[16]; -rgparam regr[30]; -int n; -int k; -int mtelestart,telestart; -int channel=-1; -float max; + double teleline[3000]; + double wedge[16]; + rgparam regr[30]; + int n; + int k; + int mtelestart, telestart; + int channel = -1; + float max; -printf("Calibration "); -fflush(stdout); + printf ("Calibration "); + fflush (stdout); /* build telemetry values lines */ -for(n=0;n max) { + mtelestart = n; + max = df; + } + } + + mtelestart -= 64; + telestart = mtelestart % 128; + + if (mtelestart < 0 || nrow < telestart + 128) { + fprintf (stderr, " impossible, not enought row\n"); + return (0); + } - df=(teleline[n-4]+teleline[n-3]+teleline[n-2]+teleline[n-1])/(teleline[n]+teleline[n+1]+teleline[n+2]+teleline[n+3]); - if(df> max){ mtelestart=n; max=df; } -} +/* compute wedges and regression */ + for (n = telestart, k = 0; n < nrow - 128; n += 128, k++) { + int j; -mtelestart-=64; -telestart=mtelestart%128; + for (j = 0; j < 16; j++) { + int i; -if(mtelestart <0 || nrow50.0) { - Cs+=csline; - i++; - } - } - Cs/=i; - Cs=rgcal(Cs,&(regr[k])); + /* channel ID */ + for (j = 0, max = 10000.0, channel = -1; j < 6; j++) { + float df; + df = wedge[15] - wedge[j]; + df = df * df; + if (df < max) { + channel = j; + max = df; } -} -nbtele=k; + } + + /* Cs computation */ + for (Cs = 0.0, i = 0, j = n; j < n + 128; j++) { + double csline; + + for (csline = 0.0, l = 3; l < 43; l++) + csline += prow[n][l + offset]; + csline /= 40.0; + if (csline > 50.0) { + Cs += csline; + i++; + } + } + Cs /= i; + Cs = rgcal (Cs, &(regr[k])); + } + } + nbtele = k; /* calibrate */ -for(n=0;n=nbtele) k=nbtele-1; - kof=(n-telestart)%128; - if(kof<64) { - if(k<1) { - pv=rgcal(pv,&(regr[k])); - } else { - pv=rgcal(pv,&(regr[k]))*(64+kof)/128.0+ - rgcal(pv,&(regr[k-1]))*(64-kof)/128.0; - } - } else { - if((k+1)>=nbtele) { - pv=rgcal(pv,&(regr[k])); - } else { - pv=rgcal(pv,&(regr[k]))*(192-kof)/128.0+ - rgcal(pv,&(regr[k+1]))*(kof-64)/128.0; - } - } - - if(pv>255.0) pv=255.0; - if(pv<0.0) pv=0.0; - pixelv[i+offset]=pv; + for (n = 0; n < nrow; n++) { + float *pixelv; + int i, c; + + pixelv = prow[n]; + for (i = 0; i < 1001; i++) { + float pv; + int k, kof; + + pv = pixelv[i + offset]; + + k = (n - telestart) / 128; + if (k >= nbtele) + k = nbtele - 1; + kof = (n - telestart) % 128; + if (kof < 64) { + if (k < 1) { + pv = rgcal (pv, &(regr[k])); } -} -printf("Done\n"); -return(channel); + else { + pv = rgcal (pv, &(regr[k])) * (64 + kof) / 128.0 + + rgcal (pv, &(regr[k - 1])) * (64 - kof) / 128.0; + } + } + else { + if ((k + 1) >= nbtele) { + pv = rgcal (pv, &(regr[k])); + } + else { + pv = rgcal (pv, &(regr[k])) * (192 - kof) / 128.0 + + rgcal (pv, &(regr[k + 1])) * (kof - 64) / 128.0; + } + } + + if (pv > 255.0) + pv = 255.0; + if (pv < 0.0) + pv = 0.0; + pixelv[i + offset] = pv; + } + } + printf ("Done\n"); + return (channel); } /* ------------------------------temperature calibration -----------------------*/ extern int satnum; -const struct { - float d[4][3]; - struct { - float vc,A,B; - } rad[3]; - struct { - float Ns; - float b[3]; - } cor[3]; -} satcal[2]= +const struct +{ + float d[4][3]; + struct + { + float vc, A, B; + } rad[3]; + struct + { + float Ns; + float b[3]; + } cor[3]; +} satcal[2] = #include "satcal.h" - -typedef struct { -double Nbb; -double Cs; -double Cb; -int ch; + typedef struct +{ + double Nbb; + double Cs; + double Cb; + int ch; } tempparam; /* temperature compensation for IR channel */ -static void tempcomp(double t[16], int ch,tempparam *tpr) +static void +tempcomp (double t[16], int ch, tempparam * tpr) { -double Tbb,T[4]; -double C; -double r; -int n; - - tpr->ch=ch-3; - - /* compute equivalent T black body */ - for (n=0;n<4;n++) { - float d0,d1,d2; - - C=t[9+n]*4.0; - d0=satcal[satnum].d[n][0]; - d1=satcal[satnum].d[n][1]; - d2=satcal[satnum].d[n][2]; - T[n]=d0; - T[n]+=d1*C; C=C*C; - T[n]+=d2*C; - } - Tbb=(T[0]+T[1]+T[2]+T[3])/4.0; - Tbb=satcal[satnum].rad[tpr->ch].A+satcal[satnum].rad[tpr->ch].B*Tbb; - - /* compute radiance Black body */ - C=satcal[satnum].rad[tpr->ch].vc; - tpr->Nbb=c1*C*C*C/(exp(c2*C/Tbb)-1.0); - /* store Count Blackbody and space */ - tpr->Cs=Cs*4.0; - tpr->Cb=t[14]*4.0; + double Tbb, T[4]; + double C; + double r; + int n; + + tpr->ch = ch - 3; + + /* compute equivalent T black body */ + for (n = 0; n < 4; n++) { + float d0, d1, d2; + + C = t[9 + n] * 4.0; + d0 = satcal[satnum].d[n][0]; + d1 = satcal[satnum].d[n][1]; + d2 = satcal[satnum].d[n][2]; + T[n] = d0; + T[n] += d1 * C; + C = C * C; + T[n] += d2 * C; + } + Tbb = (T[0] + T[1] + T[2] + T[3]) / 4.0; + Tbb = satcal[satnum].rad[tpr->ch].A + satcal[satnum].rad[tpr->ch].B * Tbb; + + /* compute radiance Black body */ + C = satcal[satnum].rad[tpr->ch].vc; + tpr->Nbb = c1 * C * C * C / (exp (c2 * C / Tbb) - 1.0); + /* store Count Blackbody and space */ + tpr->Cs = Cs * 4.0; + tpr->Cb = t[14] * 4.0; } -static double tempcal(float Ce,tempparam *rgpr) +static double +tempcal (float Ce, tempparam * rgpr) { -double Nl,Nc,Ns,Ne; -double T,vc; + double Nl, Nc, Ns, Ne; + double T, vc; - Ns=satcal[satnum].cor[rgpr->ch].Ns; - Nl=Ns+(rgpr->Nbb-Ns)*(rgpr->Cs-Ce*4.0)/(rgpr->Cs-rgpr->Cb); - Nc=satcal[satnum].cor[rgpr->ch].b[0]+ - satcal[satnum].cor[rgpr->ch].b[1]*Nl+ - satcal[satnum].cor[rgpr->ch].b[2]*Nl*Nl; + Ns = satcal[satnum].cor[rgpr->ch].Ns; + Nl = Ns + (rgpr->Nbb - Ns) * (rgpr->Cs - Ce * 4.0) / (rgpr->Cs - rgpr->Cb); + Nc = satcal[satnum].cor[rgpr->ch].b[0] + + satcal[satnum].cor[rgpr->ch].b[1] * Nl + + satcal[satnum].cor[rgpr->ch].b[2] * Nl * Nl; - Ne=Nl+Nc; + Ne = Nl + Nc; - vc=satcal[satnum].rad[rgpr->ch].vc; - T=c2*vc/log(c1*vc*vc*vc/Ne+1.0); - T=(T-satcal[satnum].rad[rgpr->ch].A)/satcal[satnum].rad[rgpr->ch].B; + vc = satcal[satnum].rad[rgpr->ch].vc; + T = c2 * vc / log (c1 * vc * vc * vc / Ne + 1.0); + T = (T - satcal[satnum].rad[rgpr->ch].A) / satcal[satnum].rad[rgpr->ch].B; - /* rescale to range 0-255 for +40-60 °C */ - T=(-T+273.15+40)/100.0*255.0; + /* rescale to range 0-255 for +40-60 °C */ + T = (-T + 273.15 + 40) / 100.0 * 255.0; - return(T); + return (T); } -int Temperature(float **prow,int nrow,int channel,int offset) +int +Temperature (float **prow, int nrow, int channel, int offset) { -tempparam temp; -int n; + tempparam temp; + int n; -printf("Temperature "); + printf ("Temperature "); - tempcomp(tele,channel,&temp); + tempcomp (tele, channel, &temp); -for(n=0;n255.0) pv=255.0; - if(pv<0.0) pv=0.0; - pixelv[i+offset]=pv; - } -} -printf("Done\n"); + if (pv > 255.0) + pv = 255.0; + if (pv < 0.0) + pv = 0.0; + pixelv[i + offset] = pv; + } + } + printf ("Done\n"); } diff --git a/reg.c b/reg.c index 4c50109..1942fe4 100644 --- a/reg.c +++ b/reg.c @@ -8,161 +8,144 @@ Prentice Hall, International Editions: ISBN 0-13-625047-5 This free software is compliments of the author. E-mail address: in%"mathews@fullerton.edu" -*/ - +*/ + #include - + #define DMAX 5 /* Maximum degree of polynomial */ #define NMAX 10 /* Maximum number of points */ - -static void FactPiv(int N, double A[DMAX][DMAX], double B[], double Cf[]); - -void polyreg(const int M, const int N, const double X[], const double Y[], double C[]) -{ - int R, K, J; /* Loop counters */ - double A[DMAX][DMAX]; /* A */ - double B[DMAX]; - double P[2*DMAX+1]; - double x, y; - double p; - - /* Zero the array */ - - for (R = 0; R < M+1; R++) B[R] = 0; - - /* Compute the column vector */ - - for (K = 0; K < N; K++) - { - y = Y[K]; - x = X[K]; - p = 1.0; - - for( R = 0; R < M+1; R++ ) - { - B[R] += y * p; - p = p*x; - } - } - - /* Zero the array */ - - for (J = 1; J <= 2*M; J++) P[J] = 0; - - P[0] = N; - - /* Compute the sum of powers of x_(K-1) */ - - for (K = 0; K < N; K++) - { - x = X[K]; - p = X[K]; - - for (J = 1; J <= 2*M; J++) - { - P[J] += p; - p = p * x; - } - } - - /* Determine the matrix entries */ - - for (R = 0; R < M+1; R++) - { - for( K = 0; K < M+1; K++) A[R][K] = P[R+K]; - } - + static void FactPiv (int N, double A[DMAX][DMAX], double B[], double Cf[]); + void +polyreg (const int M, const int N, const double X[], const double Y[], + double C[]) +{ + int R, K, J; /* Loop counters */ + double A[DMAX][DMAX]; /* A */ + double B[DMAX]; + double P[2 * DMAX + 1]; + double x, y; + double p; + + /* Zero the array */ + for (R = 0; R < M + 1; R++) + B[R] = 0; + + /* Compute the column vector */ + for (K = 0; K < N; K++) + { + y = Y[K]; + x = X[K]; + p = 1.0; + for (R = 0; R < M + 1; R++) + { + B[R] += y * p; + p = p * x; + } + } + + /* Zero the array */ + for (J = 1; J <= 2 * M; J++) + P[J] = 0; + P[0] = N; + + /* Compute the sum of powers of x_(K-1) */ + for (K = 0; K < N; K++) + { + x = X[K]; + p = X[K]; + for (J = 1; J <= 2 * M; J++) + { + P[J] += p; + p = p * x; + } + } + + /* Determine the matrix entries */ + for (R = 0; R < M + 1; R++) + { + for (K = 0; K < M + 1; K++) + A[R][K] = P[R + K]; + } + /* Solve the linear system of M + 1 equations : A*C = B - for the coefficient vector C = (c_1,c_2,..,c_M,c_(M+1)) */ - FactPiv(M+1, A, B, C); - -} /* end main */ - -/*--------------------------------------------------------*/ - -static void FactPiv(int N, double A[DMAX][DMAX], double B[], double Cf[]) -{ - - int K, P, C, J; /* Loop counters */ - int Row[NMAX]; /* Field with row-number */ - double X[DMAX], Y[DMAX]; - double SUM, DET = 1.0; - int T; - - /* Initialize the pointer vector */ - - for (J = 0; J< N; J++) Row[J] = J; - - /* Start LU factorization */ - - for (P = 0; P < N - 1; P++) { - - /* Find pivot element */ - - for (K = P + 1; K < N; K++) { - if ( fabs(A[Row[K]][P]) > fabs(A[Row[P]][P]) ) { - /* Switch the index for the p-1 th pivot row if necessary */ - T = Row[P]; - Row[P] = Row[K]; - Row[K] = T; - DET = - DET; - } - - } /* End of simulated row interchange */ - - if (A[Row[P]][P] == 0) { - printf("The matrix is SINGULAR !\n"); - printf("Cannot use algorithm --> exit\n"); - exit(1); - } - - /* Multiply the diagonal elements */ - - DET = DET * A[Row[P]][P]; - - /* Form multiplier */ - - for (K = P + 1; K < N; K++) { - A[Row[K]][P]= A[Row[K]][P] / A[Row[P]][P]; - - /* Eliminate X_(p-1) */ - - for (C = P + 1; C < N + 1; C++) { - A[Row[K]][C] -= A[Row[K]][P] * A[Row[P]][C]; - } - } - - } /* End of L*U factorization routine */ - - DET = DET * A[Row[N-1]][N-1]; - - /* Start the forward substitution */ - - for(K = 0; K < N; K++) Y[K] = B[K]; - - Y[0] = B[Row[0]]; - for ( K = 1; K < N; K++) { - SUM =0; - for ( C = 0; C <= K -1; C++) SUM += A[Row[K]][C] * Y[C]; - Y[K] = B[Row[K]] - SUM; - } - - /* Start the back substitution */ - - X[N-1] = Y[N-1] / A[Row[N-1]][N-1]; - - for (K = N - 2; K >= 0; K--) { - SUM = 0; - for (C = K + 1; C < N; C++) { - SUM += A[Row[K]][C] * X[C]; - } - - X[K] = ( Y[K] - SUM) / A[Row[K]][K]; - - } /* End of back substitution */ - - /* Output */ - for( K = 0; K < N; K++) - Cf[K]=X[K]; -} - + for the coefficient vector C = (c_1,c_2,..,c_M,c_(M+1)) */ + FactPiv (M + 1, A, B, C); + } /* end main */ + + +/*--------------------------------------------------------*/ + static void +FactPiv (int N, double A[DMAX][DMAX], double B[], double Cf[]) +{ + int K, P, C, J; /* Loop counters */ + int Row[NMAX]; /* Field with row-number */ + double X[DMAX], Y[DMAX]; + double SUM, DET = 1.0; + int T; + + /* Initialize the pointer vector */ + for (J = 0; J < N; J++) + Row[J] = J; + + /* Start LU factorization */ + for (P = 0; P < N - 1; P++) { + + /* Find pivot element */ + for (K = P + 1; K < N; K++) { + if (fabs (A[Row[K]][P]) > fabs (A[Row[P]][P])) { + + /* Switch the index for the p-1 th pivot row if necessary */ + T = Row[P]; + Row[P] = Row[K]; + Row[K] = T; + DET = -DET; + } + } /* End of simulated row interchange */ + if (A[Row[P]][P] == 0) { + printf ("The matrix is SINGULAR !\n"); + printf ("Cannot use algorithm --> exit\n"); + exit (1); + } + + /* Multiply the diagonal elements */ + DET = DET * A[Row[P]][P]; + + /* Form multiplier */ + for (K = P + 1; K < N; K++) { + A[Row[K]][P] = A[Row[K]][P] / A[Row[P]][P]; + + /* Eliminate X_(p-1) */ + for (C = P + 1; C < N + 1; C++) { + A[Row[K]][C] -= A[Row[K]][P] * A[Row[P]][C]; + } + } + } /* End of L*U factorization routine */ + DET = DET * A[Row[N - 1]][N - 1]; + + /* Start the forward substitution */ + for (K = 0; K < N; K++) + Y[K] = B[K]; + Y[0] = B[Row[0]]; + for (K = 1; K < N; K++) { + SUM = 0; + for (C = 0; C <= K - 1; C++) + SUM += A[Row[K]][C] * Y[C]; + Y[K] = B[Row[K]] - SUM; + } + + /* Start the back substitution */ + X[N - 1] = Y[N - 1] / A[Row[N - 1]][N - 1]; + for (K = N - 2; K >= 0; K--) { + SUM = 0; + for (C = K + 1; C < N; C++) { + SUM += A[Row[K]][C] * X[C]; + } + X[K] = (Y[K] - SUM) / A[Row[K]][K]; + } /* End of back substitution */ + + /* Output */ + for (K = 0; K < N; K++) + Cf[K] = X[K]; + } + +