/* Given a set of data points, generate a spline curve that passes thru all points.
*
* Author - Marlow etc. Modified by - P. Ward Date - 3.10.1973
*
* Extracted from the ROOT package by O.Couet.
*
* Re-structured to remove most gotos by Dino Ferrero Merlino
* (the numerical part has not been touched)
*
* Changed to be ANSI-C compliant by Oliver Koch.
*
* Integrated into ploticus by Steve Grubb 10/3/03
*
* Usage:
*
* Helps to compute a smooth representation of a curve.
* The history of this code is quite long...
*
* This routine draws a smooth tangentially continuous curve through
* the sequence of data points P(I) I=1,N where P(I)=(X(I),Y(I))
* the curve is approximated by a polygonal arc of short vectors .
* the data points can represent open curves, P(1) != P(N) or closed
* curves P(2) == P(N) . If a tangential discontinuity at P(I) is
* required , then set P(I)=P(I+1) . loops are also allowed .
*
* Reference Marlow and Powell,Harwell report No.R.7092.1972
* MCCONALOGUE,Computer Journal VOL.13,NO4,NOV1970PP392 6
*
* The routine was then adapted as the IGRAP1 routine of HIGZ.
* The FORTRAN code was translated with f2c and adapted to ROOT.
*
*
* TODO:
*
*/
#include <stdio.h>
#include <stdlib.h>
#include <math.h>
#define false 0;
#define true 1;
static int npoints, k, kp, NpointsMax, banksize, npt;
static double xratio, yratio, sxmin, symin;
static int closed, flgis, iw;
static double P1, P2, P3, P4, P5, P6, W1, W2, W3, A, B, C, R, S, T, Z;
static double CO, SO, CT, ST, CTU, STU, XNT, DX1, DY1, DX2, DY2, XO, YO, DX, DY, XT, YT;
static double XA, XB, YA, YB, U1, U2, U3, TJ, SB, STH;
static double wsign, tsquare, tcube, *qlx, *qly, *x, *y;
static const int maxiterations = 20;
static const double delta = 0.00055;
static int QpSmootherInit( int nPoints, double in[][2] );
static int QpSmootherDone();
static int continousTangentAtEndPoints() ;
static int scaleRatio() ;
static int skipConsecutiveEqualPoints(int start) ;
static void computeDirectionCosine (int kMinus1, int kP, int kPlus1) ;
static int resizeSmoothArrays() ;
static void computeStraightLine(int kP) ;
static void newPoint() ;
static void dirCosL150() ;
static void dirCosL130() ;
static void computeCubicCoeffs() ;
static int smooth() ;
static void zero(int *kCode, double AZ, double BZ, double E2, double *X, double *Y, int maxiterations);
static void checkMaxIterations(int *kCode, int *IT, int J1, double *X, double X2, int maxiterations);
/* static int getNPoint() ; */
static double* getX ();
static double* getY ();
static double min( double f, double g );
static double max( double f, double g );
/* --------------------------------------------------- */
/* SMOOTHFIT - ploticus entry point */
int
PL_smoothfit( in, ninp, out, noutp )
double in[][2]; /* input vector */
int ninp; /* # input points */
double out[][2]; /* output vector */
int *noutp; /* # output points */
{
int i, stat;
double *resultx, *resulty;
stat = QpSmootherInit( ninp, in );
if( stat ) return( stat );
*noutp = smooth();
/* *noutp = getNPoint(); */
resultx = getX();
resulty = getY();
for( i = 0; i < *noutp; i++ ) {
out[i][0] = resultx[i];
out[i][1] = resulty[i];
}
stat = QpSmootherDone();
return( 0 );
}
/* ------------------------------- */
/* void QpSmootherInit(int nPoints, double *_x, double *_y) */
static int QpSmootherInit( nPoints, in )
int nPoints;
double in[][2];
{
int i;
C = T = CO = SO = CT = ST = CTU = STU = DX1 = DY1 = DX2 = DY2 = 0;
XT = YT = XA = XB = YA = YB = U1 = U2 = U3 = TJ = SB = 0;
/* make a local copy of the original polylines (smoother will scale them) */
npoints = nPoints;
x = (double *) malloc(sizeof(double) * npoints);
y = (double *) malloc(sizeof(double) * npoints);
if (x == NULL || y == NULL) return( 5720 ); /* Not enough memory for qpsmoother */
else {
for (i=0;i<npoints;i++) {
x[i] = in[i][0];
y[i] = in[i][1];
}
NpointsMax = npoints*10;
banksize = NpointsMax;
qlx = (double *) malloc(sizeof(double) * NpointsMax);
qly = (double *) malloc(sizeof(double) * NpointsMax);
if (qlx == NULL || qly == NULL) return( 5720 ); /* Not enough memory for qpsmoother */
else {
qlx[0] = x[0];
qly[0] = y[0];
/* Scale data and check whether endpoints collide */
closed = scaleRatio();
}
}
return( 0 );
}
/* ------------------------------- */
static int QpSmootherDone() {
free(x);
free(y);
free(qlx);
free(qly);
return( 0 );
}
/* ------------------------------- */
static int continousTangentAtEndPoints() {
int tangent = false;
if (x[0] != x[npoints-1] || y[0] != y[npoints-1])
tangent = true;
if (x[npoints-2] == x[npoints-1] && y[npoints-2] == y[npoints-1])
tangent = true;
if (x[0] == x[1] && y[0] == y[1])
tangent = true;
return tangent;
}
/* ------------------------------- */
/** Scale data to the range 0-ratio_signs in X, 0-1 in Y
where ratio_signs is the ratio between the number of changes
of sign in Y divided by the number of changes of sign in X.
Returns whether the endpoints are overlapping or not.
*/
static int scaleRatio() {
double ratio_signs;
double sxmax, symax;
int i;
double six;
double siy;
double dx1n;
double dy1n;
closed = false;
sxmin = x[0];
sxmax = x[0];
symin = y[0];
symax = y[0];
six = 1;
siy = 1;
for (i=1;i<npoints;i++) {
if (i > 1) {
if ((x[i]-x[i-1])*(x[i-1]-x[i-2]) < 0) six++;
if ((y[i]-y[i-1])*(y[i-1]-y[i-2]) < 0) siy++;
}
if (x[i] < sxmin) sxmin = x[i];
if (x[i] > sxmax) sxmax = x[i];
if (y[i] < symin) symin = y[i];
if (y[i] > symax) symax = y[i];
}
dx1n = fabs(x[npoints-1]-x[0]);
dy1n = fabs(y[npoints-1]-y[0]);
if (dx1n < 0.01*(sxmax-sxmin) && dy1n < 0.01*(symax-symin))
closed = true;
if (sxmin == sxmax)
xratio = 1;
else {
if (six > 1)
ratio_signs = siy/six;
else
ratio_signs = 20;
xratio = ratio_signs/(sxmax-sxmin);
}
if (symin == symax)
yratio = 1;
else
yratio = 1/(symax-symin);
for (i=0;i<npoints;i++) {
x[i] = (x[i]-sxmin)*xratio;
y[i] = (y[i]-symin)*yratio;
}
return closed;
}
/* ------------------------------- */
static int skipConsecutiveEqualPoints(int start) {
k = start;
while((k < npoints) && (x[k] == x[k-1] && y[k] == y[k-1])) {
k++;
}
return k;
}
/* ------------------------------- */
/* Calculate the direction cosines at P(k) obtained from P(kMinus1),P(k),P(kPlus1).
* If k=1 then N-1 is used for kMinus1. If k=N then 2 is used for kPlus1
*/
static void computeDirectionCosine (int kMinus1, int kP, int kPlus1) {
double DK1,DK2;
DX1 = x[kP] - x[kMinus1];
DY1 = y[kP] - y[kMinus1];
DX2 = x[kPlus1] - x[kP];
DY2 = y[kPlus1] - y[kP];
DK1 = DX1*DX1 + DY1*DY1;
DK2 = DX2*DX2 + DY2*DY2;
CTU = DX1*DK2 + DX2*DK1;
STU = DY1*DK2 + DY2*DK1;
XNT = CTU*CTU + STU*STU;
/* If both ctu and stu are zero,then default.This can
* occur when P(k)=P(k+1). I.E. A loop.
*/
if (XNT < 1.E-25) {
CTU = DY1;
STU =-DX1;
XNT = DK1;
}
/* Normalise direction cosines. */
CT = CTU/sqrt(XNT);
ST = STU/sqrt(XNT);
}
/* ------------------------------- */
static int resizeSmoothArrays() {
int i;
int newsize = banksize + NpointsMax;
double *qt1=qlx;
double *qt2=qly;
qlx = (double *) malloc(sizeof(double) * newsize);
qly = (double *) malloc(sizeof(double) * newsize);
for (i=0;i<banksize;i++) {
qlx[i] = qt1[i];
qly[i] = qt2[i];
}
free(qt1);
free(qt2);
banksize = newsize;
return newsize;
}
/* ------------------------------- */
static void computeStraightLine(int kP) {
/* Compute a straight line between P(k-1) and P(k) in Fortran
* (XT,YT) will contain the coordinate of the endpoint
*/
XT = x[kP] ;
YT = y[kP] ;
}
/* ------------------------------- */
static void newPoint() {
/* Convert coordinates to original system */
qlx[npt] = sxmin + XT/xratio;
qly[npt] = symin + YT/yratio;
npt++;
/* If the arrays are filled up, enlarge them. */
if (npt >= banksize)
banksize = resizeSmoothArrays();
}
/* ------------------------------- */
/* Direction cosines at P(k) obtained from P(k-2),P(k-1),P(k). */
static void dirCosL150() {
W3 = 2*(DX1*DY2-DX2*DY1);
CT = CTU-W3*DY2;
ST = STU+W3*DX2;
XNT = 1/sqrt(CT*CT+ST*ST);
CT = CT*XNT;
ST = ST*XNT;
flgis = 0;
}
/* ------------------------------- */
/* Direction cosines at P(k-1) obtained from P(k-1),P(k),P(k+1). */
static void dirCosL130() {
W3 = 2*(DX1*DY2-DX2*DY1);
CO = CTU+W3*DY1;
SO = STU-W3*DX1;
XNT = 1/sqrt(CO*CO+SO*SO);
CO = CO*XNT;
SO = SO*XNT;
flgis = 1;
}
/* ------------------------------- */
/* For the arc between P(k-1) and P(k) with direction cosines CO,
* SO and CT,ST respectively, calculate the coefficients of the
* parametric cubic represented by X(T) and Y(T) where
* X(T)=XA*T**3 + XB*T**2 + CO*T + XO
* Y(T)=YA*T**3 + YB*T**2 + SO*T + YO
*/
static void computeCubicCoeffs() {
double CC ;
double ERR ;
int zeroLoop;
XO = x[k-2];
YO = y[k-2];
DX = x[k-1] - XO;
DY = y[k-1] - YO;
/* Initialise the values of X(TI),Y(TI) in XT and YT respectively. */
XT = XO;
YT = YO;
C = DX*DX+DY*DY;
A = CO+CT;
B = SO+ST;
R = DX*A+DY*B;
T = C*6/(sqrt(R*R+2*(7-CO*CT-SO*ST)*C)+R);
tsquare = T*T;
tcube = T*tsquare;
XA = (A*T-2*DX)/tcube;
XB = (3*DX-(CO+A)*T)/tsquare;
YA = (B*T-2*DY)/tcube;
YB = (3*DY-(SO+B)*T)/tsquare;
/* If the curve is close to a straight line then use a straight
* line between (XO,YO) and (XT,YT).
*/
if (.75*max(fabs(DX*SO-DY*CO),fabs(DX*ST-DY*CT)) <= delta) {
computeStraightLine(k-1);
newPoint();
} else {
/* Calculate a set of values 0 == T(0).LTCT(1) < ... < T(M)=TC
* such that polygonal arc joining X(T(J)),Y(T(J)) (J=0,1,..M)
* is within the required accuracy of the curve
*/
TJ = 0;
U1 = YA*XB-YB*XA;
U2 = YB*CO-XB*SO;
U3 = SO*XA-YA*CO;
/* Given T(J), calculate T(J+1). The values of X(T(J)), */
/* Y(T(J)) T(J) are contained in XT,YT and TJ respectively. */
do {
S = T - TJ;
iw = -2;
/* Define iw here later. */
P1 = (2*U1)*TJ-U3;
P2 = (U1*TJ-U3)*3*TJ+U2;
P3 = 3*TJ*YA+YB;
P4 = (P3+YB)*TJ+SO;
P5 = 3*TJ*XA+XB;
P6 = (P5+XB)*TJ+CO;
CC = 0.8209285;
ERR = 0.1209835;
iw -= 2;
L200:
/* Test D(TJ,THETA). A is set to (Y(TJ+S)-Y(TJ))/S.B is
* set to (X(TJ+S)-X(TJ))/S.
*/
A = (S*YA+P3)*S+P4;
B = (S*XA+P5)*S+P6;
/* Set Z to PSI(D/delta)-CC. */
W1 = -S*(S*U1+P1);
W2 = S*S*U1-P2;
W3 = 1.5*W1+W2;
/* Set the estimate of (THETA-TJ)/S.Then set the numerator
* of the expression (EQUATION 4.4)/S. Then set the square
* of D(TJ,TJ+S)/delta. Then replace Z by PSI(D/delta)-CC.
*/
if (W3 > 0)
wsign = fabs(W1);
else
wsign = -fabs(W1);
STH = 0.5+wsign/(3.4*fabs(W1)+5.2*fabs(W3));
Z = S*STH*(S-S*STH)*(W1*STH+W1+W2);
Z = Z*Z/((A*A+B*B)*(delta*delta));
Z = (Z+2.642937)*Z/((.3715652*Z+3.063444)*Z+.2441889)-CC;
/* Branch if Z has been calculated */
zeroLoop=true;
if (iw <= 0) {
if (Z > ERR) {
kp = 0;
C = Z;
SB = S;
} else {
L220:
if (iw+2 == 0) {
/* goto L190; */
iw -= 2;
goto L200;
}
if (iw+2 > 0) {
/* Update TJ,XT and YT. */
XT = XT + S*B;
YT = YT + S*A;
TJ = S + TJ;
} else {
/* Last part of arc. */
XT = x[k-1];
YT = y[k-1];
S = 0;
}
newPoint();
zeroLoop=false; /* passato qui. S>0 */
}
}
/* Z(S). find a value of S where 0 <= S <= SB such that */
/* fabs(Z(S)) < ERR */
while(zeroLoop) {
zero(&kp,0,SB,ERR,&S,&Z,maxiterations);
if (kp == 2) {
/* goto L210; */
iw -= 2;
goto L220;
}
if (kp > 2) {
fprintf( stderr, "Attempt to plot outside plot limits\n" );
XT = x[k-1];
YT = y[k-1];
newPoint();
/* goto L310; */
break;
}
if (iw > 0) goto L200;
/* Set Z=Z(S) for S=0. */
if (iw < 0) {
Z = -CC;
iw = 0;
} else {
/* Set Z=Z(S) for S=SB. */
Z = C;
iw = 1;
}
} /* End while (zeroLoop) */
} while ( S > 0 );
}
}
/* ------------------------------- */
static int smooth() {
npt = 1;
k = 1;
/* flgis is true in the main loop, but is false if there is
* a deviation from the main loop (cusp or endpoint? DinoFM)
*/
/* The curve is open if :
* the two endpoints are not overlapping (closed == false)
* AND there is a continuous tangent at the endpoints */
if (!closed && continousTangentAtEndPoints()) {
closed = true;
flgis = 0;
k++;
} else {
closed = false;
flgis = 1;
/* Calculate direction cosines at P(0) using P(N-1),P(0),P(1). */
computeDirectionCosine(npoints-2,0,1);
/* Carry forward the direction cosines from the previous arc. */
CO = CT;
SO = ST;
k++;
}
while (k <= npoints) {
k=skipConsecutiveEqualPoints(k);
/* More arcs to compute */
if (k < npoints) {
/* Test whether P(k) is a cusp. */
if (x[k-1] == x[k] && y[k-1] == y[k]) {
if (flgis) {
dirCosL150();
computeCubicCoeffs();
} else {
/* Make a straight line from P(k-1) to P(k). */
computeStraightLine(k-1);
newPoint();
}
} else {
/* k is always >= 2 ! */
computeDirectionCosine(k-2,k-1,k);
if (!flgis)
dirCosL130();
computeCubicCoeffs();
}
} else {
/* k == npoints. Last arc ! */
if (!closed) {
/* k is always >= 2 ! */
computeDirectionCosine(k-2,k-1,1);
if (!flgis)
dirCosL130();
computeCubicCoeffs();
} else {
if (flgis) {
dirCosL150();
computeCubicCoeffs();
} else {
/* Make a straight line from P(k-1) to P(k). */
computeStraightLine(k-1);
newPoint();
}
}
}
/* Branch if the next section of the curve begins at a cusp. */
if (flgis) {
/* Carry forward the direction cosines from the previous arc. */
CO = CT;
SO = ST;
}
k++;
}
return npt;
}
/* ------------------------------- */
static void zero(int *kCode, double AZ, double BZ, double E2, double *X, double *Y, int maxiterations)
{
static double AA, BB, YAA, ytest, Y1, X1, H;
static int J1, IT, J3, J2;
double YBB, X2;
int exitLoop;
YBB = 0;
exitLoop=false;
/* Calculate Y(X) at X=AZ. */
if ((*kCode) <= 0) {
AA = AZ;
BB = BZ;
(*X) = A;
J1 = 1;
IT = 1;
(*kCode) = J1;
return;
}
/* Test whether Y(X) is sufficiently small. */
if (fabs((*Y)) <= E2) { (*kCode) = 2; return; }
/* Calculate Y(X) at X=BZ. */
if (J1 == 1) {
YAA = (*Y);
(*X) = BB;
J1 = 2;
return;
}
/* Test whether the signs of Y(AZ) and Y(BZ) are different.
* if not, begin the binary subdivision.
*/
if ((J1 == 2) && (YAA*(*Y) >= 0)) {
X1 = AA;
Y1 = YAA;
J1 = 3;
H = BB - AA;
J2 = 1;
X2 = AA + 0.5*H;
J3 = 1;
checkMaxIterations(&k,&IT,J1,X,X2,maxiterations);
return;
}
/* Test whether a bracket has been found .
* If not,continue the search
*/
if ((J1 != 2) || (YAA*(*Y) >= 0)) {
if (J1 > 3) goto L170;
if (YAA*(*Y) >= 0) {
if (J3 >= J2) {
H = 0.5*H; J2 = 2*J2;
AA = X1; YAA = Y1; X2 = AA + 0.5*H; J3 = 1;
}
else {
AA = (*X); YAA = (*Y); X2 = (*X) + H; J3++;
}
checkMaxIterations(&k,&IT,J1,X,X2,maxiterations);
return;
}
}
/* The first bracket has been found.calculate the next X by the
* secant method based on the bracket.
*/
BB = (*X);
YBB = (*Y);
J1 = 4;
if (fabs(YAA) > fabs(YBB)) {
X1 = AA; Y1 = YAA; (*X) = BB; (*Y) = YBB;
} else {
X1 = BB; Y1 = YBB; (*X) = AA; (*Y) = YAA;
}
/* Use the secant method based on the function values Y1 and Y.
* check that X2 is inside the interval (A,B).
*/
do {
X2 = (*X)-(*Y)*((*X)-X1)/((*Y)-Y1);
X1 = (*X);
Y1 = (*Y);
ytest = 0.5*min(fabs(YAA),fabs(YBB));
if ((X2-AA)*(X2-BB) < 0) {
checkMaxIterations(kCode,&IT,J1,X,X2,maxiterations);
return;
}
/* Calculate the next value of X by bisection . Check whether
* the maximum accuracy has been achieved.
*/
X2 = 0.5*(AA+BB);
ytest = 0;
if ((X2-AA)*(X2-BB) >= 0) {
(*kCode) = 2; return;
}
checkMaxIterations(kCode,&IT,J1,X,X2,maxiterations);
return;
/* Revise the bracket (A,B). */
L170:
if (J1 != 4) return;
if (YAA*(*Y) < 0) {
BB = (*X); YBB = (*Y);
} else {
AA = (*X); YAA = (*Y);
}
/* Use ytest to decide the method for the next value of X. */
if (ytest <= 0) {
if (fabs(YAA) > fabs(YBB)) {
X1 = AA; Y1 = YAA; (*X) = BB; (*Y) = YBB;
} else {
X1 = BB; Y1 = YBB; (*X) = AA; (*Y) = YAA;
}
} else {
if (fabs((*Y))-ytest > 0)
exitLoop=true;
}
} while (!exitLoop);
X2 = 0.5*(AA+BB);
ytest = 0;
if ((X2-AA)*(X2-BB) >= 0) {
k = 2;
return;
}
checkMaxIterations(&k,&IT,J1,X,X2,maxiterations);
}
/* ------------------------------- */
/* Check whether (maxiterations) function values have been calculated. */
static void checkMaxIterations(int *kCode, int *IT, int J1, double *X, double X2, int maxiterations)
{
(*IT)++;
if ((*IT) >= maxiterations) {
(*kCode) = J1;
} else {
(*X) = X2;
}
}
/* ------------------------------- */
/*
* static int getNPoint() {
* return(npoints);
* }
*/
/* ------------------------------- */
static double* getX () {
return(qlx);
}
/* ------------------------------- */
static double* getY () {
return(qly);
}
/* ------------------------------- */
static double min( f, g )
double f, g;
{
if( f < g ) return( f );
else return( g );
}
/* ------------------------------- */
static double max( f, g )
double f, g;
{
if( f > g ) return( f );
else return( g );
}
syntax highlighted by Code2HTML, v. 0.9.1