void matmul_strassen(volatile float * a, volatile float * b, volatile float * c, int NN)
{
float **as, **bs, **cs;
int i, j;
int LEAF_SIZE;
as = allocate_real_matrix(NN, -1);
bs = allocate_real_matrix(NN, -1);
cs = allocate_real_matrix(NN, -1);
for (i=0; i<NN; i++)
for (j=0; j<NN; j++)
{
as[i][j] = a[i*NN+j];
bs[i][j] = b[i*NN+j];
}
LEAF_SIZE = 32;
strassen(as, bs, cs, NN, LEAF_SIZE);
for (i=0; i<NN; i++)
for (j=0; j<NN; j++)
{
c[i*NN+j] = cs[i][j];
}
as = free_real_matrix(as, NN);
bs = free_real_matrix(bs, NN);
cs = free_real_matrix(cs, NN);
return;
}
int LLL(long n, double **b)
{
/* Lattice reduction algorithm. */
double *B = allocate_real_vector(n);
double **bs = allocate_real_matrix(n, n);
double **mu = allocate_real_matrix(n, n);
double C, t, temp, x, y;
long i, j, k, l;
for (i = 0; i < n; i++) bs[0][i] = b[0][i];
B[0] = Scalar(n, bs[0], bs[0]);
for (i = 1; i < n; i++) {
for (j = 0; j < n; j++) bs[i][j] = b[i][j];
for (j = 0; j < i; j++) {
mu[i][j] = Scalar(n, b[i], bs[j]) / B[j];
for (k = 0; k < n; k++)
bs[i][k] -= mu[i][j] * bs[j][k];
}
B[i] = Scalar(n, bs[i], bs[i]);
}
L3:
k = 1;
L4:
l = k - 1;
Reduce(k, l, n, b, mu);
x = mu[k][l];
y = 0.75 - x * x;
if (B[k] < y * B[l]) {
C = B[k] + x * x * B[l];
mu[k][l] = x * B[l] / C;
B[k] *= B[l] / C;
B[l] = C;
for (i = 0; i < n; i++) {
temp = b[k][i];
b[k][i] = b[l][i];
b[l][i] = temp;
}
if (k > 1) {
for (j = 0; j < k - 1; j++) {
temp = mu[k][j];
mu[k][j] = mu[l][j];
mu[l][j] = temp;
}
}
for (i = k + 1; i < n; i++) {
t = mu[i][k];
mu[i][k] = mu[i][l] - x * t;
mu[i][l] = t + mu[k][l] * mu[i][k];
}
k = max(1, k - 1);
goto L4;
}
for (l = k - 2; l >= 0; l--) Reduce(k, l, n, b, mu);
k++;
if (k < n) goto L4;
free_real_matrix(bs, n);
free_real_matrix(mu, n);
free_real_vector(B);
return 1;
}
int SubsetSum(long n, double s, double *a, double *x)
{
long n1 = n + 1;
double **b = allocate_real_matrix(n1, n1);
double sum;
long i, j, m = ceil(sqrt(n) / 2.0);
for (i = 0; i < n1; i++) {
if (i < n) {
for (j = 0; j < n1; j++) b[i][j] = 0.0;
b[i][i] = 1.0;
b[i][n1 - 1] = m * a[i];
}
else {
for (j = 0; j < n; j++) b[i][j] = 0.5;
b[i][n1 - 1] = m * s;
}
}
printf("the matrix to be reduced is:\n\n");
for (i = 0; i < n1; i++) {
for (j = 0; j < n1; j++)
printf("%6.2f ", b[i][j]);
printf("\n");
}
printf("\n");
if (!LLL(n1, b)) {
free_real_matrix(b, n1);
return 0;
}
printf("the reduced matrix is:\n\n");
for (i = 0; i < n1; i++) {
for (j = 0; j < n1; j++)
printf("%6.2f ", b[i][j]);
printf("\n");
}
printf("\n");
for (i = 0; i < n1; i++) {
for (j = 0; j < n; j++) x[j] = b[i][j] + 0.5;
sum = 0.0;
for (j = 0; j < n; j++) sum += a[j] * x[j];
if (sum == s) {
free_real_matrix(b, n1);
return 1;
}
for (j = 0; j < n; j++) x[j] = - b[i][j] + 0.5;
sum = 0.0;
for (j = 0; j < n; j++) sum += a[j] * x[j];
if (sum == s) {
free_real_matrix(b, n1);
return 1;
}
}
free_real_matrix(b, n1);
return 0;
}
void main ()
{
real_t **allocate_real_matrix(int, int, int, int);
void free_real_matrix(real_t **, int, int, int);
void liniger1vs(real_t *, real_t, int, real_t [], real_t *,
void (*)(int, real_t[], real_t *), real_t **,
void (*)(int, real_t **, real_t [], real_t *),
int, real_t, real_t, real_t, real_t, real_t [],
void (*)(real_t, real_t, int, real_t [], real_t,
real_t **, real_t []));
int i,itmax;
real_t x,sigma,reta,y[3],**j,info[10];
j=allocate_real_matrix(1,2,1,2);
printf("The results with LINIGER1VS are:\n\n");
reta=1.0;
for (i=1; i<=3; i++) {
reta *= 1.0e-2;
x=y[2]=0.0;
y[1]=1.0;
liniger1vs(&x,50.0,2,y,&sigma,f,j,jacobian,10,0.1,50.0,
reta,reta,info,out);
}
printf("\n");
reta = -1.0;
for (i=1; i<=3; i++) {
reta *= 1.0e-2;
x=y[2]=0.0;
y[1]=1.0;
liniger1vs(&x,50.0,2,y,&sigma,f,j,jacobian,10,0.1,1.0,
reta,reta,info,out);
}
free_real_matrix(j,1,2,1);
}
void main ()
{
real_t **allocate_real_matrix(int, int, int, int);
void free_real_matrix(real_t **, int, int, int);
void arkmat(real_t *, real_t, int, int, real_t **,
void (*)(int, int, real_t, real_t **, real_t **),
int, int *, real_t *,
void (*)(real_t, real_t, int, int, real_t **, int, int, real_t *));
int i,j,n,m,typ,orde;
real_t **u,t,te,cos1,spr;
u=allocate_real_matrix(1,20,1,10);
hpi=2.0*atan(1.0);
h2=1.0/9.0;
h1=(2.0*hpi)/9.0;
n=m=10;
h1k=h1*h1;
h2k=h2*h2;
tel=0;
t=0.0;
te=1.0;
for (j=1; j<=m; j++) u[n][j]=sin(h1*(j-1));
for (i=1; i<=n; i++) {
cos1=cos(h2*hpi*(i-1));
for (j=1; j<=m; j++) u[i][j]=u[n][j]*cos1;
}
inimat(n+1,n+n,1,m,u,0.0);
typ=3;
orde=2;
spr=80.0;
arkmat(&t,te,m,n+n,u,der,typ,&orde,&spr,out);
free_real_matrix(u,1,20,1);
}
void main ()
{
real_t *allocate_real_vector(int, int);
real_t **allocate_real_matrix(int, int, int, int);
void free_real_vector(real_t *, int);
void free_real_matrix(real_t **, int, int, int);
void eigsym1(real_t [], int, int, real_t [], real_t **, real_t []);
int i,j;
real_t a[11],val[3],em[10],**vec;
vec=allocate_real_matrix(1,4,1,2);
em[0]=1.0e-6; em[2]=1.0e-5; em[4]=1.0e-3;
em[6]=1.0e-5; em[8]=5.0;
for (i=1; i<=4; i++)
for (j=i; j<=4; j++) a[(j*j-j)/2+i]=1.0/(i+j-1);
eigsym1(a,4,2,val,vec,em);
printf("The eigenvalues:\n %12.5e %12.5e\n\nThe eigenvectors:\n"
" %12.5e %12.5e\n %12.5e %12.5e\n %12.5e %12.5e\n"
" %12.5e %12.5e\n\nEM[1] = %e\n"
"EM[7] = %e\nEM[3] =%3.0f\nEM[5] =%3.0f\nEM[9] =%3.0f\n",
val[1],val[2],vec[1][1],vec[1][2],vec[2][1],vec[2][2],
vec[3][1],vec[3][2],vec[4][1],vec[4][2],
em[1],em[7],em[3],em[5],em[9]);
free_real_matrix(vec,1,4,1);
}
void main ()
{
real_t **allocate_real_matrix(int, int, int, int);
void free_real_matrix(real_t **, int, int, int);
void marquardt(int, int, real_t [], real_t [], real_t **,
int (*)(int, int, real_t[], real_t[]),
void (*)(int, int, real_t[], real_t[], real_t **),
real_t [], real_t []);
real_t in[7],out[8],rv[7],par[4],**jjinv;
jjinv=allocate_real_matrix(1,3,1,3);
in[0]=1.0e-6; in[3]=1.0e-4; in[4]=1.0e-1; in[5]=75.0;
in[6]=1.0e-2;
x[1] = -5.0; x[2] = -3.0; x[3] = -1.0; x[4]=1.0;
x[5]=3.0; x[6]=5.0;
y[1]=127.0; y[2]=151.0; y[3]=379.0; y[4]=421.0;
y[5]=460.0; y[6]=426.0;
par[1]=580.0; par[2] = -180.0; par[3] = -0.160;
marquardt(6,3,par,rv,jjinv,expfunct,jacobian,in,out);
printf("Parameters:\n %9.4e %9.4e %9.4e\n\nOUT:\n"
" %14.6e\n %14.6e\n %14.6e\n %14.6e\n %14.6e\n %14.6e\n"
" %14.6e\n\nLast residual vector:\n"
" %6.1f %6.1f %6.1f %6.1f %6.1f %6.1f\n",
par[1],par[2],par[3],out[7],out[2],out[6],out[3],out[4],
out[5],out[1],rv[1],rv[2],rv[3],rv[4],rv[5],rv[6]);
free_real_matrix(jjinv,1,3,1);
}
void main ()
{
real_t **allocate_real_matrix(int, int, int, int);
void free_real_matrix(real_t **, int, int, int);
void qzi(int, real_t **, real_t **, real_t **, real_t [],
real_t [], real_t [], int [], real_t []);
int k,l,iter[5];
real_t **a,**b,**x,alfr[5],alfi[5],beta[5],em[2];
a=allocate_real_matrix(1,4,1,4);
b=allocate_real_matrix(1,4,1,4);
x=allocate_real_matrix(1,4,1,4);
a[1][1]=2.0; a[1][2]=3.0; a[1][3] = -3.0; a[1][4]=4.0;
a[2][1]=1.0; a[2][2] = -1.0; a[2][3]=5.0; a[2][4]=1.0;
a[3][1]=0.0; a[3][2]=2.0; a[3][3]=6.0; a[3][4]=8.0;
a[4][1]=1.0; a[4][2]=1.0; a[4][3]=0.0; a[4][4]=4.0;
b[1][1]=1.0; b[1][2]=5.0; b[1][3]=9.0; b[1][4]=0.0;
b[2][1]=2.0; b[2][2]=6.0; b[2][3]=10.0; b[2][4]=2.0;
b[3][1]=3.0; b[3][2]=7.0; b[3][3]=11.0; b[3][4] = -1.0;
b[4][1]=4.0; b[4][2]=8.0; b[4][3]=12.0; b[4][4]=3.0;
for (k=1; k<=4; k++)
for (l=1; l<=4; l++) x[k][l] = (k == l) ? 1.0 : 0.0;
em[0]=1.0e-35;
em[1]=1.0e-6;
qzi(4,a,b,x,alfr,alfi,beta,iter,em);
for (k=1; k<=4; k++)
printf("ITER[%1d]=%3d\n",k,iter[k]);
printf("\nEigenvectors:\n");
for (k=1; k<=4; k++)
printf(" %12.6e %12.6e %12.6e %12.6e\n",
x[k][1],x[k][2],x[k][3],x[k][4]);
printf("\nALFA(real part) ALFA(imaginary part) BETA\n");
for (k=1; k<=4; k++)
printf(" %12.6e %16.6e %21.6e\n",alfr[k],alfi[k],beta[k]);
printf("\nLAMBDA(real part) LAMBDA(imaginary part)\n");
for (k=1; k<=4; k++)
if (beta[k] == 0.0)
printf(" INFINITE INDEFINITE\n");
else
printf(" %12.6e %16.6e\n",
alfr[k]/beta[k],alfi[k]/beta[k]);
free_real_matrix(a,1,4,1);
free_real_matrix(b,1,4,1);
free_real_matrix(x,1,4,1);
}
void main ()
{
real_t *allocate_real_vector(int, int);
real_t **allocate_real_matrix(int, int, int, int);
void free_real_vector(real_t *, int);
void free_real_matrix(real_t **, int, int, int);
void chldecsol2(real_t **, int, real_t [], real_t []);
real_t chldeterm2(real_t **, int);
void chldecinv2(real_t **, int, real_t []);
int i,j;
real_t determinant,**pascal2,*b,*aux;
pascal2=allocate_real_matrix(1,4,1,4);
b=allocate_real_vector(1,4);
aux=allocate_real_vector(2,3);
for (j=1; j<=4; j++) {
pascal2[1][j]=1.0;
for (i=2; i<=j; i++)
pascal2[i][j] = (i == j) ?
pascal2[i-1][j]*2.0 : pascal2[i][j-1]+pascal2[i-1][j];
b[j]=pow(2.0,j);
}
aux[2]=1.0e-11;
chldecsol2(pascal2,4,aux,b);
if (aux[3] == 4)
determinant=chldeterm2(pascal2,4);
else
printf("Matrix not positive definite");
printf("Solution with CHLDECSOL2:\n %e %e %e %e\n",
b[1],b[2],b[3],b[4]);
printf("\nDeterminant with CHLDETERM2: %e\n",determinant);
for (j=1; j<=4; j++) {
pascal2[1][j]=1.0;
for (i=2; i<=j; i++)
pascal2[i][j] = (i == j) ?
pascal2[i-1][j]*2.0 : pascal2[i][j-1]+pascal2[i-1][j];
}
chldecinv2(pascal2,4,aux);
printf("\nInverse matrix with CHLDECINV2:\n");
for (i=1; i<=4; i++) {
for (j=1; j<=4; j++)
if (j < i)
printf(" ");
else
printf("%11.5f",pascal2[i][j]);
printf("\n");
}
free_real_matrix(pascal2,1,4,1);
free_real_vector(b,1);
free_real_vector(aux,2);
}
void main ()
{
real_t **allocate_real_matrix(int, int, int, int);
void free_real_matrix(real_t **, int, int, int);
void rotcomcol(int, int, int, int, real_t **, real_t **,
real_t, real_t, real_t);
int i,j;
real_t **ar,**ai;
ar=allocate_real_matrix(1,2,1,2);
ai=allocate_real_matrix(1,2,1,2);
ar[1][1]=4.0; ar[1][2]=5.0; ar[2][1] = -5.0; ar[2][2]=4.0;
ai[1][1]=3.0; ai[1][2]=ai[2][1]=0.0; ai[2][2] = -3.0;
rotcomcol(1,2,1,2,ar,ai,0.08,0.06,-0.1);
printf("After postmultiplication:\n"
" %+3.1f%+3.1f*I %+3.1f%+3.1f*I\n"
" %+3.1f%+3.1f*I %+3.1f%+3.1f*I\n",
ar[1][1],ai[1][1],ar[1][2],ai[1][2],
ar[2][1],ai[2][1],ar[2][2],ai[2][2]);
free_real_matrix(ar,1,2,1);
free_real_matrix(ai,1,2,1);
}
void main ()
{
real_t **allocate_real_matrix(int, int, int, int);
void free_real_matrix(real_t **, int, int, int);
void richardson(real_t **, int, int, int, int,
int, void (*)(int, int, int, int, real_t **),
real_t, real_t, int *, real_t [], int *, real_t *, real_t *,
void (*)(real_t **, int, int, int, int, int *, real_t [],
int, real_t, real_t));
void elimination(real_t **, int, int, int, int,
void (*)(int, int, int, int, real_t **),
real_t, real_t, int *, real_t [], int *, real_t *, real_t *,
void (*)(real_t **, int, int, int, int, int *, real_t [],
int, real_t, real_t));
int j,l,lj,uj,ll,ul,n,p,k;
real_t pi,domeigval,rateconvr,rateconve,rateconv,a,b,discr[3],**u;
u=allocate_real_matrix(0,11,0,11);
printf("RICHARDSON and ELIMINATION deliver:\n\n");
pi=3.14159265358979;
lj=0; uj=11; ll=0; ul=11; n=50;
a=0.326; b=7.83;
h=pi/(uj-lj);
h2=h*h;
for (j=lj; j<=uj; j++)
for (l=ll; l<=ul; l++)
u[j][l] = (j==lj || j==uj || l==ll || l==ul) ?
(j*h)*(j*h)*(l*h)*(l*h) : 1.0;
nn=n;
richardson(u,lj,uj,ll,ul,1,residual,a,b,&n,discr,&k,&rateconv,
&domeigval,out1);
rateconvr=rateconv;
printf("\n dominant eigenvalue: %e\n\n",domeigval);
elimination(u,lj,uj,ll,ul,residual,a,b,&p,discr,&k,&rateconv,
&domeigval,out2);
rateconve=rateconv;
nn=n+p;
printf("\nTotal number of iterations: %2d\n"
"Rate of convergence with respect to\n"
" the zeroth iterand of RICHARDSON: %e\n",
nn,(n*rateconvr+p*rateconve)/nn);
free_real_matrix(u,0,11,0);
}
void main ()
{
real_t **allocate_real_matrix(int, int, int, int);
void free_real_matrix(real_t **, int, int, int);
void liniger2(real_t *, real_t, int, real_t [], real_t *, real_t *,
real_t (*)(int, real_t[], int, real_t *, real_t *),
int (*)(int), real_t **,
void (*)(int, real_t **, real_t [], real_t *, real_t *),
int *, int, real_t, real_t, real_t,
void (*)(real_t, real_t, int, real_t [], real_t, real_t,
real_t **, int));
int i,k,itmax;
real_t x,sigma1,sigma2,step,y[3],**j;
j=allocate_real_matrix(1,2,1,2);
printf("The results with LINIGER2 (second order) are:\n"
" K DER.EV. JAC.EV. Y[1] Y[2]\n");
for (i=1; i<=2; i++) {
step = (i == 1) ? 10.0 : 1.0;
for (itmax=1; itmax<=3; itmax += 2) {
passes=pasjac=0;
x=y[2]=0.0;
y[1]=1.0;
sigma2=0.0;
liniger2(&x,50.0,2,y,&sigma1,&sigma2,f,evaluate1,j,
jacobian,&k,itmax,step,1.0e-4,1.0e-4,out);
}
}
printf("\nThe results with LINIGER2 (third order) are:\n"
" K DER.EV. JAC.EV. Y[1] Y[2]\n");
for (i=1; i<=2; i++) {
step = (i == 1) ? 10.0 : 1.0;
for (itmax=1; itmax<=3; itmax += 2) {
passes=pasjac=0;
x=y[2]=0.0;
y[1]=1.0;
sigma2=0.0;
liniger2(&x,50.0,2,y,&sigma1,&sigma2,f,evaluate2,j,
jacobian,&k,itmax,step,1.0e-4,1.0e-4,out);
}
}
free_real_matrix(j,1,2,1);
}
void main ()
{
real_t **allocate_real_matrix(int, int, int, int);
void free_real_matrix(real_t **, int, int, int);
void decsol(real_t **, int, real_t [], real_t []);
int i,j;
real_t **a,b[5],aux[4];
a=allocate_real_matrix(1,4,1,4);
for (i=1; i<=4; i++) {
for (j=1; j<=4; j++) a[i][j]=1.0/(i+j-1);
b[i]=a[i][3];
}
aux[2]=1.0e-5;
decsol(a,4,aux,b);
printf("Solution: %e %e %e %e\n",b[1],b[2],b[3],b[4]);
printf("Sign(Det) =%3.0f\nNumber of eliminations =%3.0f\n",
aux[1],aux[3]);
free_real_matrix(a,1,4,1);
}
void main ()
{
real_t **allocate_real_matrix(int, int, int, int);
void free_real_matrix(real_t **, int, int, int);
void gssinverb(real_t **, int, real_t []);
int i;
real_t **a,aux[12];
a=allocate_real_matrix(1,4,1,4);
a[1][1]=4.0; a[1][2]=2.0; a[1][3]=4.0; a[1][4]=1.0;
a[2][1]=30.0; a[2][2]=20.0; a[2][3]=45.0; a[2][4]=12.0;
a[3][1]=20.0; a[3][2]=15.0; a[3][3]=36.0; a[3][4]=10.0;
a[4][1]=35.0; a[4][2]=28.0; a[4][3]=70.0; a[4][4]=20.0;
aux[0]=aux[2]=aux[6]=1.0e-14;
aux[4]=8;
gssinverb(a,4,aux);
printf("Calculated inverse:\n");
for (i=1; i<=4; i++)
printf(" %4.0f%4.0f%4.0f%4.0f\n",a[i][1],a[i][2],a[i][3],a[i][4]);
printf("\nAUX elements:\n%e\n%e\n%e\n%e\n%e\n%e\n",
aux[1],aux[3],aux[5],aux[7],aux[9],aux[11]);
free_real_matrix(a,1,4,1);
}
void main ()
{
real_t **allocate_real_matrix(int, int, int, int);
void free_real_matrix(real_t **, int, int, int);
void hshhrmtri(real_t **, int, real_t [], real_t [], real_t [],
real_t [], real_t [], real_t []);
void inimat(int, int, int, int, real_t **, real_t);
real_t **a,d[5],b[5],bb[5],tr[4],ti[4],em[2];
a=allocate_real_matrix(1,4,1,4);
inimat(1,4,1,4,a,0.0);
a[1][1]=a[2][2]=3.0;
a[1][2]=a[3][3]=a[3][4]=a[4][4]=1.0;
a[3][2]=2.0;
a[4][1] = -2.0;
em[0]=1.0e-6;
hshhrmtri(a,4,d,b,bb,em,tr,ti);
printf("HSHHRMTRI delivers\n\nD[1:4]: %7.3f %7.3f %7.3f %7.3f\n"
"B[1:3]: %7.3f %7.3f %7.3f\n"
"BB[1:3]: %7.3f %7.3f %7.3f\n"
"EM[1]: %7.3f\n",
d[1],d[2],d[3],d[4],b[1],b[2],b[3],bb[1],bb[2],bb[3],em[1]);
free_real_matrix(a,1,4,1);
}
int peidefunct(int nrow, int ncol, real_t par[], real_t res[],
int n, int m, int nobs, int *nbp, int first, int *sec,
int *max, int *nis, real_t eps1, int weight, int bp[],
real_t save[], real_t ymax[], real_t y[], real_t **yp,
real_t **fy, real_t **fp, int cobs[], real_t tobs[],
real_t obs[], real_t in[], real_t aux[], int clean,
int (*deriv)(int,int,real_t [],real_t [],real_t,real_t []),
int (*jacdfdy)(int,int,real_t [],real_t [],real_t,real_t **),
int (*jacdfdp)(int,int,real_t [],real_t [],real_t,real_t **),
void (*callystart)(int,int,real_t [],real_t [],real_t[]),
void (*monitor)(int,int,int,real_t [],real_t [],int,int))
{
/* this function is internally used by PEIDE */
void peidereset(int, int, real_t, real_t, real_t, real_t, real_t [],
real_t [], real_t *, real_t *, real_t *, int *);
void peideorder(int, int, real_t, real_t [], real_t [],
real_t *, real_t *, real_t *, real_t *, real_t *, int *);
void peidestep(int, int, int, real_t, real_t, real_t, real_t,
real_t [], real_t [], real_t [], real_t [], int *, real_t *);
real_t peideinterpol(int, int, int, real_t, real_t []);
int l,k,knew,fails,same,kpold,n6,nnpar,j5n,cobsii,*p,evaluate,
evaluated,decompose,conv,extra,npar,i,j,jj,ii;
real_t xold,hold,a0,tolup,tol,toldwn,tolconv,h,ch,chnew,error,
dfi,tobsdif,a[6],*delta,*lastdelta,*df,*y0,**jacob,xend,
hmax,hmin,eps,s,aa,x,t,c;
p=allocate_integer_vector(1,n);
delta=allocate_real_vector(1,n);
lastdelta=allocate_real_vector(1,n);
df=allocate_real_vector(1,n);
y0=allocate_real_vector(1,n);
jacob=allocate_real_matrix(1,n,1,n);
if (*sec) {
*sec=0;
goto Finish;
}
xend=tobs[nobs];
eps=in[2];
npar=m;
extra=(*nis)=0;
ii=1;
jj = (*nbp == 0) ? 0 : 1;
n6=n*6;
inivec(-3,-1,save,0.0);
inivec(n6+1,(6+m)*n,y,0.0);
inimat(1,nobs+(*nbp),1,m+(*nbp),yp,0.0);
t=tobs[1];
x=tobs[0];
(*callystart)(n,m,par,y,ymax);
hmax=tobs[1]-tobs[0];
hmin=hmax*in[1];
/* evaluate jacobian */
evaluate=0;
decompose=evaluated=1;
if (!(*jacdfdy)(n,m,par,y,x,fy)) {
save[-3]=4.0;
goto Finish;
}
nnpar=n*npar;
Newstart:
k=1;
kpold=0;
same=2;
peideorder(n,k,eps,a,save,&tol,&tolup,&toldwn,&tolconv,
&a0,&decompose);
if (!(*deriv)(n,m,par,y,x,df)) {
save[-3]=3.0;
goto Finish;
}
s=FLT_MIN;
for (i=1; i<=n; i++) {
aa=matvec(1,n,i,fy,df)/ymax[i];
s += aa*aa;
}
h=sqrt(2.0*eps/sqrt(s));
if (h > hmax)
h=hmax;
else
if (h < hmin) h=hmin;
xold=x;
hold=h;
ch=1.0;
for (i=1; i<=n; i++) {
save[i]=y[i];
save[n+i]=y[n+i]=df[i]*h;
}
fails=0;
while (x < xend) {
if (x+h <= xend)
x += h;
else {
h=xend-x;
x=xend;
ch=h/hold;
c=1.0;
for (j=n; j<=k*n; j += n) {
c *= ch;
for (i=j+1; i<=j+n; i++) y[i] *= c;
}
same = (same < 3) ? 3 : same+1;
}
/* prediction */
for (l=1; l<=n; l++) {
for (i=l; i<=(k-1)*n+l; i += n)
for (j=(k-1)*n+l; j>=i; j -= n) y[j] += y[j+n];
delta[l]=0.0;
}
evaluated=0;
/* correction and estimation local error */
for (l=1; l<=3; l++) {
if (!(*deriv)(n,m,par,y,x,df)) {
save[-3]=3;
goto Finish;
}
for (i=1; i<=n; i++) df[i]=df[i]*h-y[n+i];
if (evaluate) {
/* evaluate jacobian */
evaluate=0;
decompose=evaluated=1;
if (!(*jacdfdy)(n,m,par,y,x,fy)) {
save[-3]=4.0;
goto Finish;
}
}
if (decompose) {
/* decompose jacobian */
decompose=0;
c = -a0*h;
for (j=1; j<=n; j++) {
for (i=1; i<=n; i++) jacob[i][j]=fy[i][j]*c;
jacob[j][j] += 1.0;
}
dec(jacob,n,aux,p);
}
sol(jacob,n,p,df);
conv=1;
for (i=1; i<=n; i++) {
dfi=df[i];
y[i] += a0*dfi;
y[n+i] += dfi;
delta[i] += dfi;
conv=(conv && (fabs(dfi) < tolconv*ymax[i]));
}
if (conv) {
s=FLT_MIN;
for (i=1; i<=n; i++) {
aa=delta[i]/ymax[i];
s += aa*aa;
}
error=s;
break;
}
}
/* acceptance or rejection */
if (!conv) {
if (!evaluated)
evaluate=1;
else {
ch /= 4.0;
if (h < 4.0*hmin) {
save[-1] += 10.0;
hmin /= 10.0;
if (save[-1] > 40.0) goto Finish;
}
}
peidereset(n,k,hmin,hmax,hold,xold,y,save,&ch,&x,
&h,&decompose);
} else if (error > tol) {
fails++;
if (h > 1.1*hmin) {
if (fails > 2) {
peidereset(n,k,hmin,hmax,hold,xold,y,save,&ch,&x,
&h,&decompose);
goto Newstart;
} else {
/* calculate step and order */
peidestep(n,k,fails,tolup,toldwn,tol,error,delta,
lastdelta,y,ymax,&knew,&chnew);
if (knew != k) {
k=knew;
peideorder(n,k,eps,a,save,&tol,&tolup,
&toldwn,&tolconv,&a0,&decompose);
}
ch *= chnew;
peidereset(n,k,hmin,hmax,hold,xold,y,save,&ch,&x,
&h,&decompose);
}
} else {
if (k == 1) {
/* violate eps criterion */
save[-2] += 1.0;
same=4;
goto Errortestok;
}
k=1;
peidereset(n,k,hmin,hmax,hold,xold,y,save,&ch,&x,
&h,&decompose);
peideorder(n,k,eps,a,save,&tol,&tolup,
&toldwn,&tolconv,&a0,&decompose);
same=2;
}
} else {
Errortestok:
fails=0;
for (i=1; i<=n; i++) {
c=delta[i];
for (l=2; l<=k; l++) y[l*n+i] += a[l]*c;
if (fabs(y[i]) > ymax[i]) ymax[i]=fabs(y[i]);
}
same--;
if (same == 1)
dupvec(1,n,0,lastdelta,delta);
else if (same == 0) {
/* calculate step and order */
peidestep(n,k,fails,tolup,toldwn,tol,error,delta,
lastdelta,y,ymax,&knew,&chnew);
if (chnew > 1.1) {
if (k != knew) {
if (knew > k)
mulvec(knew*n+1,knew*n+n,-knew*n,y,delta,
a[k]/knew);
k=knew;
peideorder(n,k,eps,a,save,&tol,&tolup,
&toldwn,&tolconv,&a0,&decompose);
}
same=k+1;
if (chnew*h > hmax) chnew=hmax/h;
h *= chnew;
c=1.0;
for (j=n; j<=k*n; j += n) {
c *= chnew;
mulvec(j+1,j+n,0,y,y,c);
}
decompose=1;
} else
same=10;
}
(*nis)++;
/* start of an integration step of yp */
if (clean) {
hold=h;
xold=x;
kpold=k;
ch=1.0;
dupvec(1,k*n+n,0,save,y);
} else {
if (h != hold) {
ch=h/hold;
c=1.0;
for (j=n6+nnpar; j<=kpold*nnpar+n6; j += nnpar) {
c *= ch;
for (i=j+1; i<=j+nnpar; i++) y[i] *= c;
}
hold=h;
}
if (k > kpold)
inivec(n6+k*nnpar+1,n6+k*nnpar+nnpar,y,0.0);
xold=x;
kpold=k;
ch=1.0;
dupvec(1,k*n+n,0,save,y);
/* evaluate jacobian */
evaluate=0;
decompose=evaluated=1;
if (!(*jacdfdy)(n,m,par,y,x,fy)) {
save[-3]=4.0;
goto Finish;
}
/* decompose jacobian */
decompose=0;
c = -a0*h;
for (j=1; j<=n; j++) {
for (i=1; i<=n; i++) jacob[i][j]=fy[i][j]*c;
jacob[j][j] += 1.0;
}
dec(jacob,n,aux,p);
if (!(*jacdfdp)(n,m,par,y,x,fp)) {
save[-3]=5.0;
goto Finish;
}
if (npar > m) inimat(1,n,m+1,npar,fp,0.0);
/* prediction */
for (l=0; l<=k-1; l++)
for (j=k-1; j>=l; j--)
elmvec(j*nnpar+n6+1,j*nnpar+n6+nnpar,nnpar,
y,y,1.0);
/* correction */
for (j=1; j<=npar; j++) {
j5n=(j+5)*n;
dupvec(1,n,j5n,y0,y);
for (i=1; i<=n; i++)
df[i]=h*(fp[i][j]+matvec(1,n,i,fy,y0))-
y[nnpar+j5n+i];
sol(jacob,n,p,df);
for (l=0; l<=k; l++) {
i=l*nnpar+j5n;
elmvec(i+1,i+n,-i,y,df,a[l]);
}
}
}
while (x >= t) {
/* calculate a row of the jacobian matrix and an
element of the residual vector */
tobsdif=(tobs[ii]-x)/h;
cobsii=cobs[ii];
res[ii]=peideinterpol(cobsii,n,k,tobsdif,y)-obs[ii];
if (!clean) {
for (i=1; i<=npar; i++)
yp[ii][i]=peideinterpol(cobsii+(i+5)*n,nnpar,k,
tobsdif,y);
/* introducing break-points */
if (bp[jj] != ii) {
} else if (first && fabs(res[ii]) < eps1) {
(*nbp)--;
for (i=jj; i<=(*nbp); i++) bp[i]=bp[i+1];
bp[*nbp+1]=0;
} else {
extra++;
if (first) par[m+jj]=obs[ii];
/* introducing a jacobian row and a residual
vector element for continuity requirements */
yp[nobs+jj][m+jj] = -weight;
mulrow(1,npar,nobs+jj,ii,yp,yp,weight);
res[nobs+jj]=weight*(res[ii]+obs[ii]-par[m+jj]);
}
}
if (ii == nobs)
goto Finish;
else {
t=tobs[ii+1];
if (bp[jj] == ii && jj < *nbp) jj++;
hmax=t-tobs[ii];
hmin=hmax*in[1];
ii++;
}
}
/* break-points introduce new initial values for y & yp */
if (extra > 0) {
for (i=1; i<=n; i++) {
y[i]=peideinterpol(i,n,k,tobsdif,y);
for (j=1; j<=npar; j++)
y[i+(j+5)*n]=peideinterpol(i+(j+5)*n,nnpar,
k,tobsdif,y);
}
for (l=1; l<=extra; l++) {
cobsii=cobs[bp[npar-m+l]];
y[cobsii]=par[npar+l];
for (i=1; i<=npar+extra; i++) y[cobsii+(5+i)*n]=0.0;
inivec(1+nnpar+(l+5)*n,nnpar+(l+6)*n,y,0.0);
y[cobsii+(5+npar+l)*n]=1.0;
}
npar += extra;
extra=0;
x=tobs[ii-1];
/* evaluate jacobian */
evaluate=0;
decompose=evaluated=1;
if (!(*jacdfdy)(n,m,par,y,x,fy)) {
save[-3]=4.0;
goto Finish;
}
nnpar=n*npar;
goto Newstart;
}
}
}
Finish:
if (save[-2] > *max) *max=save[-2];
if (!first) (*monitor)(1,ncol,nrow,par,res,weight,*nis);
free_integer_vector(p,1);
free_real_vector(delta,1);
free_real_vector(lastdelta,1);
free_real_vector(df,1);
free_real_vector(y0,1);
free_real_matrix(jacob,1,n,1);
return (save[-1] <= 40.0 && save[-3] == 0.0);
}
void peide(int n, int m, int nobs, int *nbp, real_t par[],
real_t res[], int bp[], real_t **jtjinv,
real_t in[], real_t out[],
int (*deriv)(int,int,real_t [],real_t [],real_t,real_t []),
int (*jacdfdy)(int,int,real_t [],real_t [],real_t,real_t **),
int (*jacdfdp)(int,int,real_t [],real_t [],real_t,real_t **),
void (*callystart)(int,int,real_t [],real_t [],real_t[]),
void (*data)(int,real_t [],real_t [],int[]),
void (*monitor)(int,int,int,real_t [],real_t [],int,int))
{
int i,j,weight,ncol,nrow,away,max,nfe,nis,*cobs,
first,sec,clean,nbpold,maxfe,fe,it,err,emergency;
real_t eps1,res1,in3,in4,fac3,fac4,aux[4],*obs,*save,*tobs,
**yp,*ymax,*y,**fy,**fp,w,**aid,temp,
vv,ww,w2,mu,res2,fpar,fparpres,lambda,lambdamin,p,pw,
reltolres,abstolres,em[8],*val,*b,*bb,*parpres,**jaco;
static real_t save1[35]={1.0, 1.0, 9.0, 4.0, 0.0, 2.0/3.0, 1.0,
1.0/3.0, 36.0, 20.25, 1.0, 6.0/11.0, 1.0, 6.0/11.0,
1.0/11.0, 84.028, 53.778, 0.25, 0.48, 1.0, 0.7, 0.2,
0.02, 156.25, 108.51, 0.027778, 120.0/274.0, 1.0,
225.0/274.0, 85.0/274.0, 15.0/274.0, 1.0/274.0, 0.0,
187.69, 0.0047361};
nbpold=(*nbp);
cobs=allocate_integer_vector(1,nobs);
obs=allocate_real_vector(1,nobs);
save=allocate_real_vector(-38,6*n);
tobs=allocate_real_vector(0,nobs);
ymax=allocate_real_vector(1,n);
y=allocate_real_vector(1,6*n*(nbpold+m+1));
yp=allocate_real_matrix(1,nbpold+nobs,1,nbpold+m);
fy=allocate_real_matrix(1,n,1,n);
fp=allocate_real_matrix(1,n,1,m+nbpold);
aid=allocate_real_matrix(1,m+nbpold,1,m+nbpold);
for (i=0; i<=34; i++) save[-38+i]=save1[i];
(*data)(nobs,tobs,obs,cobs);
weight=1;
first=sec=0;
clean=(*nbp > 0);
aux[2]=FLT_EPSILON;
eps1=1.0e10;
out[1]=0.0;
bp[0]=max=0;
/* smooth integration without break-points */
if (!peidefunct(nobs,m,par,res,
n,m,nobs,nbp,first,&sec,&max,&nis,eps1,weight,bp,
save,ymax,y,yp,fy,fp,cobs,tobs,obs,in,aux,clean,deriv,
jacdfdy,jacdfdp,callystart,monitor)) goto Escape;
res1=sqrt(vecvec(1,nobs,0,res,res));
nfe=1;
if (in[5] == 1.0) {
out[1]=1.0;
goto Escape;
}
if (clean) {
first=1;
clean=0;
fac3=sqrt(sqrt(in[3]/res1));
fac4=sqrt(sqrt(in[4]/res1));
eps1=res1*fac4;
if (!peidefunct(nobs,m,par,res,
n,m,nobs,nbp,first,&sec,&max,&nis,eps1,weight,bp,
save,ymax,y,yp,fy,fp,cobs,tobs,obs,in,aux,clean,deriv,
jacdfdy,jacdfdp,callystart,monitor)) goto Escape;
first=0;
} else
nfe=0;
ncol=m+(*nbp);
nrow=nobs+(*nbp);
sec=1;
in3=in[3];
in4=in[4];
in[3]=res1;
weight=away=0;
out[4]=out[5]=w=0.0;
temp=sqrt(weight)+1.0;
weight=temp*temp;
while (weight != 16 && *nbp > 0) {
if (away == 0 && w != 0.0) {
/* if no break-points were omitted then one function
function evaluation is saved */
w=weight/w;
for (i=nobs+1; i<=nrow; i++) {
for (j=1; j<=ncol; j++) yp[i][j] *= w;
res[i] *= w;
}
sec=1;
nfe--;
}
in[3] *= fac3*weight;
in[4]=eps1;
(*monitor)(2,ncol,nrow,par,res,weight,nis);
/* marquardt's method */
val=allocate_real_vector(1,ncol);
b=allocate_real_vector(1,ncol);
bb=allocate_real_vector(1,ncol);
parpres=allocate_real_vector(1,ncol);
jaco=allocate_real_matrix(1,nrow,1,ncol);
vv=10.0;
w2=0.5;
mu=0.01;
ww = (in[6] < 1.0e-7) ? 1.0e-8 : 1.0e-1*in[6];
em[0]=em[2]=em[6]=in[0];
em[4]=10*ncol;
reltolres=in[3];
abstolres=in[4]*in[4];
maxfe=in[5];
err=0;
fe=it=1;
p=fpar=res2=0.0;
pw = -log(ww*in[0])/2.30;
if (!peidefunct(nrow,ncol,par,res,
n,m,nobs,nbp,first,&sec,&max,&nis,eps1,
weight,bp,save,ymax,y,yp,fy,fp,cobs,tobs,obs,
in,aux,clean,deriv,jacdfdy,jacdfdp,
callystart,monitor))
err=3;
else {
fpar=vecvec(1,nrow,0,res,res);
out[3]=sqrt(fpar);
emergency=0;
it=1;
do {
dupmat(1,nrow,1,ncol,jaco,yp);
i=qrisngvaldec(jaco,nrow,ncol,val,aid,em);
if (it == 1)
lambda=in[6]*vecvec(1,ncol,0,val,val);
else
if (p == 0.0) lambda *= w2;
for (i=1; i<=ncol; i++)
b[i]=val[i]*tamvec(1,nrow,i,jaco,res);
while (1) {
for (i=1; i<=ncol; i++)
bb[i]=b[i]/(val[i]*val[i]+lambda);
for (i=1; i<=ncol; i++)
parpres[i]=par[i]-matvec(1,ncol,i,aid,bb);
fe++;
if (fe >= maxfe)
err=1;
else
if (!peidefunct(nrow,ncol,parpres,res,
n,m,nobs,nbp,first,&sec,&max,&nis,
eps1,weight,bp,save,ymax,y,yp,fy,fp,
cobs,tobs,obs,in,aux,clean,deriv,
jacdfdy,jacdfdp,callystart,monitor))
err=2;
if (err != 0) {
emergency=1;
break;
}
fparpres=vecvec(1,nrow,0,res,res);
res2=fpar-fparpres;
if (res2 < mu*vecvec(1,ncol,0,b,bb)) {
p += 1.0;
lambda *= vv;
if (p == 1.0) {
lambdamin=ww*vecvec(1,ncol,0,val,val);
if (lambda < lambdamin) lambda=lambdamin;
}
if (p >= pw) {
err=4;
emergency=1;
break;
}
} else {
dupvec(1,ncol,0,par,parpres);
fpar=fparpres;
break;
}
}
if (emergency) break;
it++;
} while (fpar>abstolres &&
res2>reltolres*fpar+abstolres);
for (i=1; i<=ncol; i++)
mulcol(1,ncol,i,i,jaco,aid,1.0/(val[i]+in[0]));
for (i=1; i<=ncol; i++)
for (j=1; j<=i; j++)
aid[i][j]=aid[j][i]=mattam(1,ncol,i,j,jaco,jaco);
lambda=lambdamin=val[1];
for (i=2; i<=ncol; i++)
if (val[i] > lambda)
lambda=val[i];
else
if (val[i] < lambdamin) lambdamin=val[i];
temp=lambda/(lambdamin+in[0]);
out[7]=temp*temp;
out[2]=sqrt(fpar);
out[6]=sqrt(res2+fpar)-out[2];
}
out[4]=fe;
out[5]=it-1;
out[1]=err;
free_real_vector(val,1);
free_real_vector(b,1);
free_real_vector(bb,1);
free_real_vector(parpres,1);
free_real_matrix(jaco,1,nrow,1);
if (out[1] > 0.0) goto Escape;
/* the relative starting value of lambda is adjusted
to the last value of lambda used */
away=out[4]-out[5]-1.0;
in[6] *= pow(5.0,away)*pow(2.0,away-out[5]);
nfe += out[4];
w=weight;
temp=sqrt(weight)+1.0;
eps1=temp*temp*in[4]*fac4;
away=0;
/* omit useless break-points */
for (j=1; j<=(*nbp); j++)
if (fabs(obs[bp[j]]+res[bp[j]]-par[j+m]) < eps1) {
(*nbp)--;
for (i=j; i<=(*nbp); i++) bp[i]=bp[i+1];
dupvec(j+m,(*nbp)+m,1,par,par);
j--;
away++;
bp[*nbp+1]=0;
}
ncol -= away;
nrow -= away;
temp=sqrt(weight)+1.0;
weight=temp*temp;
}
in[3]=in3;
in[4]=in4;
*nbp=0;
weight=1;
(*monitor)(2,m,nobs,par,res,weight,nis);
/* marquardt's method */
val=allocate_real_vector(1,m);
b=allocate_real_vector(1,m);
bb=allocate_real_vector(1,m);
parpres=allocate_real_vector(1,m);
jaco=allocate_real_matrix(1,nobs,1,m);
vv=10.0;
w2=0.5;
mu=0.01;
ww = (in[6] < 1.0e-7) ? 1.0e-8 : 1.0e-1*in[6];
em[0]=em[2]=em[6]=in[0];
em[4]=10*m;
reltolres=in[3];
abstolres=in[4]*in[4];
maxfe=in[5];
err=0;
fe=it=1;
p=fpar=res2=0.0;
pw = -log(ww*in[0])/2.30;
if (!peidefunct(nobs,m,par,res,
n,m,nobs,nbp,first,&sec,&max,&nis,eps1,weight,bp,
save,ymax,y,yp,fy,fp,cobs,tobs,obs,in,aux,clean,
deriv,jacdfdy,jacdfdp,callystart,monitor))
err=3;
else {
fpar=vecvec(1,nobs,0,res,res);
out[3]=sqrt(fpar);
emergency=0;
it=1;
do {
dupmat(1,nobs,1,m,jaco,yp);
i=qrisngvaldec(jaco,nobs,m,val,jtjinv,em);
if (it == 1)
lambda=in[6]*vecvec(1,m,0,val,val);
else
if (p == 0.0) lambda *= w2;
for (i=1; i<=m; i++)
b[i]=val[i]*tamvec(1,nobs,i,jaco,res);
while (1) {
for (i=1; i<=m; i++)
bb[i]=b[i]/(val[i]*val[i]+lambda);
for (i=1; i<=m; i++)
parpres[i]=par[i]-matvec(1,m,i,jtjinv,bb);
fe++;
if (fe >= maxfe)
err=1;
else
if (!peidefunct(nobs,m,parpres,res,
n,m,nobs,nbp,first,&sec,&max,&nis,eps1,
weight,bp,save,ymax,y,yp,fy,fp,cobs,tobs,
obs,in,aux,clean,deriv,jacdfdy,jacdfdp,
callystart,monitor))
err=2;
if (err != 0) {
emergency=1;
break;
}
fparpres=vecvec(1,nobs,0,res,res);
res2=fpar-fparpres;
if (res2 < mu*vecvec(1,m,0,b,bb)) {
p += 1.0;
lambda *= vv;
if (p == 1.0) {
lambdamin=ww*vecvec(1,m,0,val,val);
if (lambda < lambdamin) lambda=lambdamin;
}
if (p >= pw) {
err=4;
emergency=1;
break;
}
} else {
dupvec(1,m,0,par,parpres);
fpar=fparpres;
break;
}
}
if (emergency) break;
it++;
} while (fpar>abstolres && res2>reltolres*fpar+abstolres);
for (i=1; i<=m; i++)
mulcol(1,m,i,i,jaco,jtjinv,1.0/(val[i]+in[0]));
for (i=1; i<=m; i++)
for (j=1; j<=i; j++)
jtjinv[i][j]=jtjinv[j][i]=mattam(1,m,i,j,jaco,jaco);
lambda=lambdamin=val[1];
for (i=2; i<=m; i++)
if (val[i] > lambda)
lambda=val[i];
else
if (val[i] < lambdamin) lambdamin=val[i];
temp=lambda/(lambdamin+in[0]);
out[7]=temp*temp;
out[2]=sqrt(fpar);
out[6]=sqrt(res2+fpar)-out[2];
}
out[4]=fe;
out[5]=it-1;
out[1]=err;
free_real_vector(val,1);
free_real_vector(b,1);
free_real_vector(bb,1);
free_real_vector(parpres,1);
free_real_matrix(jaco,1,nobs,1);
nfe += out[4];
Escape:
if (out[1] == 3.0)
out[1]=2.0;
else
if (out[1] == 4.0) out[1]=6.0;
if (save[-3] != 0.0) out[1]=save[-3];
out[3]=res1;
out[4]=nfe;
out[5]=max;
free_integer_vector(cobs,1);
free_real_vector(obs,1);
free_real_vector(save,-38);
free_real_vector(tobs,0);
free_real_vector(ymax,1);
free_real_vector(y,1);
free_real_matrix(yp,1,nbpold+nobs,1);
free_real_matrix(fy,1,n,1);
free_real_matrix(fp,1,n,1);
free_real_matrix(aid,1,m+nbpold,1);
}
void efsirk(real_t *x, real_t xe, int m, real_t y[],
real_t *delta, void (*derivative)(int, real_t[], real_t *),
void (*jacobian)(int, real_t **, real_t [], real_t *),
real_t **j, int *n, real_t aeta, real_t reta, real_t hmin,
real_t hmax, int linear,
void (*output)(real_t, real_t, int, real_t [],
real_t, real_t **, int))
{
int *allocate_integer_vector(int, int);
real_t *allocate_real_vector(int, int);
real_t **allocate_real_matrix(int, int, int, int);
void free_integer_vector(int *, int);
void free_real_vector(real_t *, int);
void free_real_matrix(real_t **, int, int, int);
real_t vecvec(int, int, int, real_t [], real_t []);
real_t matmat(int, int, int, int, real_t **, real_t **);
real_t matvec(int, int, int, real_t **, real_t []);
void gsselm(real_t **, int, real_t [], int [], int []);
void solelm(real_t **, int, int [], int [], real_t []);
int k,l,lin,*ri,*ci;
real_t step,h,mu0,mu1,mu2,theta0,theta1,nu1,nu2,nu3,yk,fk,c1,c2,
d,*f,*k0,*labda,**j1,aux[8],discr,eta,s,z1,z2,e,alpha1,a,b;
ri=allocate_integer_vector(1,m);
ci=allocate_integer_vector(1,m);
f=allocate_real_vector(1,m);
k0=allocate_real_vector(1,m);
labda=allocate_real_vector(1,m);
j1=allocate_real_matrix(1,m,1,m);
aux[2]=FLT_EPSILON;
aux[4]=8.0;
for (k=1; k<=m; k++) f[k]=y[k];
*n = 0;
(*output)(*x,xe,m,y,*delta,j,*n);
step=0.0;
do {
(*n)++;
/* difference scheme */
(*derivative)(m,f,delta);
/* step size */
if (linear)
s=h=hmax;
else
if (*n == 1 || hmin == hmax)
s=h=hmin;
else {
eta=aeta+reta*sqrt(vecvec(1,m,0,y,y));
c1=nu3*step;
for (k=1; k<=m; k++) labda[k] += c1*f[k]-y[k];
discr=sqrt(vecvec(1,m,0,labda,labda));
s=h=(eta/(0.75*(eta+discr))+0.33)*h;
if (h < hmin)
s=h=hmin;
else
if (h > hmax) s=h=hmax;
}
if ((*x)+s > xe) s=xe-(*x);
lin=((step == s) && linear);
step=s;
if (!linear || *n == 1) (*jacobian)(m,j,y,delta);
if (!lin) {
/* coefficient */
z1=step*(*delta);
if (*n == 1) z2=z1+z1;
if (fabs(z2-z1) > 1.0e-6*fabs(z1) || z2 > -1.0) {
a=z1*z1+12.0;
b=6.0*z1;
if (fabs(z1) < 0.1)
alpha1=(z1*z1/140.0-1.0)*z1/30.0;
else if (z1 < 1.0e-14)
alpha1=1.0/3.0;
else if (z1 < -33.0)
alpha1=(a+b)/(3.0*z1*(2.0+z1));
else {
e=((z1 < 230.0) ? exp(z1) : FLT_MAX);
alpha1=((a-b)*e-a-b)/(((2.0-z1)*e-2.0-z1)*3.0*z1);
}
mu2=(1.0/3.0+alpha1)*0.25;
mu1 = -(1.0+alpha1)*0.5;
mu0=(6.0*mu1+2.0)/9.0;
theta0=0.25;
theta1=0.75;
a=3.0*alpha1;
nu3=(1.0+a)/(5.0-a)*0.5;
a=nu3+nu3;
nu1=0.5-a;
nu2=(1.0+a)*0.75;
z2=z1;
}
c1=step*mu1;
d=step*step*mu2;
for (k=1; k<=m; k++) {
for (l=1; l<=m; l++)
j1[k][l]=d*matmat(1,m,k,l,j,j)+c1*j[k][l];
j1[k][k] += 1.0;
}
gsselm(j1,m,aux,ri,ci);
}
c1=step*step*mu0;
d=step*2.0/3.0;
for (k=1; k<=m; k++) {
k0[k]=fk=f[k];
labda[k]=d*fk+c1*matvec(1,m,k,j,f);
}
solelm(j1,m,ri,ci,labda);
for (k=1; k<=m; k++) f[k]=y[k]+labda[k];
(*derivative)(m,f,delta);
c1=theta0*step;
c2=theta1*step;
d=nu1*step;
for (k=1; k<=m; k++) {
yk=y[k];
fk=f[k];
labda[k]=yk+d*fk+nu2*labda[k];
y[k]=f[k]=yk+c1*k0[k]+c2*fk;
}
(*x) += step;
(*output)(*x,xe,m,y,*delta,j,*n);
} while (*x < xe);
free_integer_vector(ri,1);
free_integer_vector(ci,1);
free_real_vector(f,1);
free_real_vector(k0,1);
free_real_vector(labda,1);
free_real_matrix(j1,1,m,1);
}
void ark(real_t *t, real_t *te, int *m0, int *m, real_t u[],
void (*derivative)(int *, int *, real_t *, real_t[]),
real_t data[],
void (*out)(int *, int *, real_t *, real_t *, real_t [],
real_t []))
{
real_t *allocate_real_vector(int, int);
real_t **allocate_real_matrix(int, int, int, int);
void free_real_vector(real_t *, int);
void free_real_matrix(real_t **, int, int, int);
void inivec(int, int, real_t [], real_t);
void mulvec(int, int, int, real_t [], real_t [], real_t);
void dupvec(int, int, int, real_t [], real_t []);
real_t vecvec(int, int, int, real_t [], real_t []);
void elmvec(int, int, int, real_t [], real_t [], real_t);
void decsol(real_t **, int, real_t [], real_t []);
real_t arkmui(int, int, int, real_t []);
real_t arklabda(int, int, int, int, real_t []);
static real_t th1[8] = {1.0, 0.5, 1.0/6.0, 1.0/3.0, 1.0/24.0,
1.0/12.0, 0.125, 0.25};
static real_t ec0,ec1,ec2,tau0,tau1,tau2,taus,t2;
int p,n,q,start,step1,last,i,j,k,l,n1,m00;
real_t thetanm1,tau,betan,qinv,eta,*mu,*lambda,*thetha,*ro,*r,
**alfa,th[9],aux[4],s,ss,theta0,tauacc,taustab,
aa,bb,cc,ec,mt,lt;
n=data[1];
m00=(*m0);
mu=allocate_real_vector(1,n);
lambda=allocate_real_vector(1,n);
thetha=allocate_real_vector(0,n);
ro=allocate_real_vector(m00,*m);
r=allocate_real_vector(m00,*m);
alfa=allocate_real_matrix(1,8,1,n+1);
p=data[2];
ec1=ec2=0.0;
betan=data[3];
thetanm1 = (p == 3) ? 0.75 : 1.0;
theta0=1.0-thetanm1;
s=1.0;
for (j=n-1; j>=1; j--) {
s = -s*theta0+data[n+10-j];
mu[j]=data[n+11-j]/s;
lambda[j]=mu[j]-theta0;
}
for (i=1; i<=8; i++)
for (j=0; j<=n; j++)
if (i == 1) alfa[i][j+1]=1.0;
else if (j == 0) alfa[i][j+1]=0.0;
else if (i == 2 || i == 4 || i == 8)
alfa[i][j+1]=pow(arkmui(j,n,p,lambda),(i+2)/3);
else if ((i == 3 || i == 6) && j > 1) {
s=0.0;
for (l=1; l<=j-1; l++)
s += arklabda(j,l,n,p,lambda)*
pow(arkmui(l,n,p,lambda),i/3);
alfa[i][j+1]=s;
}
else if (i == 5 && j > 2) {
s=0.0;
for (l=2; l<=j-1; l++) {
ss=0.0;
for (k=1; k<=l-1; k++)
ss += arklabda(l,k,n,p,lambda)*
arkmui(k,n,p,lambda);
s += arklabda(j,l,n,p,lambda)*ss;
}
alfa[i][j+1]=s;
}
else if (i == 7 && j > 1) {
s=0.0;
for (l=1; l<=j-1; l++)
s += arklabda(j,l,n,p,lambda)*arkmui(l,n,p,lambda);
alfa[i][j+1]=s*arkmui(j,n,p,lambda);
}
else alfa[i][j+1]=0.0;
n1 = ((n < 4) ? n+1 : ((n < 7) ? 4 : 8));
for (i=1; i<=8; i++) th[i]=th1[i-1];
if (p == 3 && n < 7) th[1]=th[2]=0.0;
aux[2]=FLT_EPSILON;
decsol(alfa,n1,aux,th);
inivec(0,n,thetha,0.0);
dupvec(0,n1-1,1,thetha,th);
if (!(p == 3 && n < 7)) {
thetha[0] -= theta0;
thetha[n-1] -= thetanm1;
q=p+1;
} else
q=3;
qinv=1.0/q;
start=(data[8] == 0.0);
data[10]=0.0;
last=0;
dupvec(*m0,*m,0,r,u);
(*derivative)(m0,m,t,r);
do {
/* stepsize */
eta=sqrt(vecvec(*m0,*m,0,u,u))*data[7]+data[6];
if (eta > 0.0) {
if (start) {
if (data[8] == 0) {
tauacc=data[5];
step1=1;
} else
if (step1) {
tauacc=pow(eta/ec2,qinv);
if (tauacc > 10.0*tau2)
tauacc=10.0*tau2;
else
step1=0;
} else {
bb=(ec2-ec1)/tau1;
cc = -bb*t2+ec2;
ec=bb*(*t)+cc;
tauacc = (ec < 0.0) ? tau2 : pow(eta/ec,qinv);
start=0;
}
} else {
aa=((ec0-ec1)/tau0+(ec2-ec1)/tau1)/(tau1+tau0);
bb=(ec2-ec1)/tau1-(2.0*t2-tau1)*aa;
cc = -(aa*t2+bb)*t2+ec2;
ec=(aa*(*t)+bb)*(*t)+cc;
tauacc = ((ec < 0.0) ? taus : pow(eta/ec,qinv));
if (tauacc > 2.0*taus) tauacc=2.0*taus;
if (tauacc < taus/2.0) tauacc=taus/2.0;
}
} else
tauacc=data[5];
if (tauacc < data[5]) tauacc=data[5];
taustab=betan/data[4];
if (taustab < data[5]) {
data[10]=1.0;
break;
}
tau = ((tauacc > taustab) ? taustab : tauacc);
taus=tau;
if (tau >= (*te)-(*t)) {
tau=(*te)-(*t);
last=1;
}
tau0=tau1;
tau1=tau2;
tau2=tau;
/* difference scheme */
mulvec(*m0,*m,0,ro,r,thetha[0]);
if (p == 3) elmvec(*m0,*m,0,u,r,0.25*tau);
for (i=1; i<=n-1; i++) {
mt=mu[i]*tau;
lt=lambda[i]*tau;
for (j=(*m0); j<=(*m); j++) r[j]=lt*r[j]+u[j];
s=(*t)+mt;
(*derivative)(m0,m,&s,r);
if (thetha[i] != 0.0) elmvec(*m0,*m,0,ro,r,thetha[i]);
if (i == n) {
data[9]=sqrt(vecvec(*m0,*m,0,ro,ro))*tau;
ec0=ec1;
ec1=ec2;
ec2=data[9]/pow(tau,q);
}
}
elmvec(*m0,*m,0,u,r,thetanm1*tau);
dupvec(*m0,*m,0,r,u);
s=(*t)+tau;
(*derivative)(m0,m,&s,r);
if (thetha[n] != 0.0) elmvec(*m0,*m,0,ro,r,thetha[n]);
data[9]=sqrt(vecvec(*m0,*m,0,ro,ro))*tau;
ec0=ec1;
ec1=ec2;
ec2=data[9]/pow(tau,q);
t2=(*t);
if (last) {
last=0;
(*t)=(*te);
} else
(*t) += tau;
data[8] += 1.0;
(*out)(m0,m,t,te,u,data);
} while ((*t) != (*te));
free_real_vector(mu,1);
free_real_vector(lambda,1);
free_real_vector(thetha,0);
free_real_vector(ro,m00);
free_real_vector(r,m00);
free_real_matrix(alfa,1,8,1);
}
void arkmat(real_t *t, real_t te, int m, int n, real_t **u,
void (*der)(int, int, real_t, real_t **, real_t **),
int type, int *order, real_t *spr,
void (*out)(real_t, real_t, int, int, real_t **, int,
int, real_t *))
{
real_t **allocate_real_matrix(int, int, int, int);
void free_real_matrix(real_t **, int, int, int);
void elmcol(int, int, int, int, real_t **, real_t **, real_t);
void dupmat(int, int, int, int, real_t **, real_t **);
int sig,l,last,ta,tb,i;
real_t tau,lambda[10],**uh,**du,mlt;
static real_t lbd1[9]={1.0/9.0, 1.0/8.0, 1.0/7.0, 1.0/6.0,
1.0/5.0, 1.0/4.0, 1.0/3.0, 1.0/2.0, 4.3};
static real_t lbd2[9]={0.1418519249e-2, 0.3404154076e-2,
0.0063118569, 0.01082794375, 0.01842733851,
0.03278507942, 0.0653627415, 0.1691078577, 156.0};
static real_t lbd3[9]={0.3534355908e-2, 0.8532600867e-2,
0.015956206, 0.02772229155, 0.04812587964,
0.08848689452, 0.1863578961, 0.5, 64.0};
static real_t lbd4[9]={1.0/8.0, 1.0/20.0, 5.0/32.0, 2.0/17.0,
17.0/80.0, 5.0/22.0, 11.0/32.0, 1.0/2.0, 8.0};
uh=allocate_real_matrix(1,n,1,m);
du=allocate_real_matrix(1,n,1,m);
/* initialize */
if (type != 2 && type != 3) type=1;
if (type != 2)
*order = 2;
else
if (*order != 2) *order = 1;
switch ((type == 1) ? 1 : type+(*order)-1) {
case 1: for (i=0; i<=8; i++) lambda[i+1]=lbd1[i]; break;
case 2: for (i=0; i<=8; i++) lambda[i+1]=lbd2[i]; break;
case 3: for (i=0; i<=8; i++) lambda[i+1]=lbd3[i]; break;
case 4: for (i=0; i<=8; i++) lambda[i+1]=lbd4[i]; break;
}
sig = ((te == *t) ? 0 : ((te > *t) ? 1 : -1));
last=0;
do {
tau=((*spr == 0.0) ? fabs(te-(*t)) :
fabs(lambda[9]/(*spr)))*sig;
ta = (*t)+tau >= te;
tb = tau >= 0.0;
if ((ta && tb) || (!(ta || tb))) {
tau=te-(*t);
last=1;
}
/* difference scheme */
(*der)(m,n,*t,u,du);
for (i=1; i<=8; i++) {
mlt=lambda[i]*tau;
dupmat(1,n,1,m,uh,u);
for (l=1; l<=m; l++) elmcol(1,n,l,l,uh,du,mlt);
(*der)(m,n,(*t)+mlt,uh,du);
}
for (l=1; l<=m; l++) elmcol(1,n,l,l,u,du,tau);
*t = (last ? te : (*t)+tau);
(*out)(*t,te,m,n,u,type,*order,spr);
} while (!last);
free_real_matrix(uh,1,n,1);
free_real_matrix(du,1,n,1);
}
int Ti_Optimization::D2Circle_fitting(int m, // number of points
double**g_pnt, /*point array*/
double* const par) /*array of unknown variables
Par[1--3], circle center
Par[4--6], circle plane vector
Par[7], circle radius*/
{
/*/begin parameter initialization*/
int i;
//initialize the center point
par[1] = par[2] = par[3] = 0;
for (int i=1; i <= m; i++)
{
par[1] += g_pnt[i][1];
par[2] += g_pnt[i][2];
}
par[1] /= m;
par[2] /= m;
/*initialize the circle radius*/
par[3] = sqrt((g_pnt[1][1] - par[1])*(g_pnt[1][1] - par[1]) +
(g_pnt[1][2] - par[2])*(g_pnt[1][2] - par[2]));
/* par[1] = 0.1;
par[2] = 0;
par[3] = 0;
*/
// end initialization
//Ti_Optimization algorithm;
double temp = 0;
double in[7],out[8],*rv,**jjinv;
rv = allocate_real_vector(1,m);
jjinv = allocate_real_matrix(1,3,1,3);//7 stand for the number of variables
in[0]=1.0e-30;
in[3]=1.0e-10;
in[4]=1.0e-10;
in[5]=2000;
in[6]=1.0e-6;
marquardt(
m,
3,
g_pnt,
par,
rv,
jjinv,
Evaluatefor2DCircle,
Jacobianfor2DCircle,
in,
out);
/*calculate the average errors*/
double ave_error=0, temp_center[3];
temp_center[0] = par[1];
temp_center[1] = par[2];
for (i=1; i <= m; i++)
ave_error += sqrt((g_pnt[i][1] - par[1])*(g_pnt[i][1] - par[1]) +
(g_pnt[i][2] - par[2])*(g_pnt[i][2] - par[2])) - par[3];
ave_error /= m;
/*free the memory*/
if (jjinv != NULL)
{
free_real_matrix(jjinv,1,3,1);///
jjinv = NULL;
}
if( rv != NULL)
{
free_real_vector(rv,1);
rv = NULL;
}
return 0;
}
void praxis( int n, double *x, int *data, double (*funct)(double *, void *data), double *in, double *out) {
int illc,i,j,k,k2,nl,maxf,nf,kl,kt,ktm,emergency;
double s,sl,dn,dmin,fx,f1,lds,ldt,sf,df,qf1,qd0,qd1,qa,qb,qc,m2,m4,
small,vsmall,large,vlarge,scbd,ldfac,t2,macheps,reltol,
abstol,h,**v,*d,*y,*z,*q0,*q1,**a,em[8],l;
/*
* Seed random number generator
*/
#ifdef MSWIN
srand(34084320);
#else
srand48(34084320);
#endif
// for (i=0; i<8; ++i) x[i+1] = (double)data->x[i];
d=allocate_real_vector(1,n);
y=allocate_real_vector(1,n);
z=allocate_real_vector(1,n);
q0=allocate_real_vector(1,n);
q1=allocate_real_vector(1,n);
v=allocate_real_matrix(1,n,1,n);
a=allocate_real_matrix(1,n,1,n);
// heuristic numbers:
//
// If the axes may be badly scaled (which is to be avoided if
// possible), then set scbd = 10. otherwise set scbd=1.
//
// If the problem is known to be ill-conditioned, set ILLC = true.
//
// KTM is the number of iterations without improvement before the
// algorithm terminates. KTM = 4 is very cautious; usually KTM = 1
// is satisfactory.
//
macheps=in[0];
reltol=in[1];
abstol=in[2];
maxf=in[5];
h=in[6];
scbd=in[7];
ktm=in[8];
illc = in[9] < 0.0;
small=macheps*macheps;
vsmall=small*small;
large=1.0/small;
vlarge=1.0/vsmall;
m2=reltol;
m4=sqrt(m2);
srand(1);
ldfac = (illc ? 0.1 : 0.01);
kt=nl=0;
nf=1;
out[3]=qf1=fx=(*funct)(x, data);
abstol=t2=small+fabs(abstol);
dmin=small;
if (h < abstol*100.0) h=abstol*100;
ldt=h;
inimat(1,n,1,n,v,0.0);
for (i=1; i<=n; i++) v[i][i]=1.0;
d[1]=qd0=qd1=0.0;
dupvec(1,n,0,q1,x);
inivec(1,n,q0,0.0);
emergency=0;
while (1) {
sf=d[1];
d[1]=s=0.0;
praxismin(1,2,&(d[1]),&s,&fx,0,
n,x,v,&qa,&qb,&qc,qd0,qd1,q0,q1,&nf,
&nl,&fx,m2,m4,dmin,ldt,reltol,abstol,small,h,funct, data);
if (s <= 0.0) mulcol(1,n,1,1,v,v,-1.0);
if (sf <= 0.9*d[1] || 0.9*sf >= d[1]) inivec(2,n,d,0.0);
for (k=2; k<=n; k++) {
dupvec(1,n,0,y,x);
sf=fx;
illc = (illc || kt > 0);
while (1) {
kl=k;
df=0.0;
if (illc) {
/* random stop to get off resulting valley */
for (i=1; i<=n; i++) {
s=z[i]=(0.1*ldt+t2*pow(10.0,kt))*
#ifdef MSWIN
((double)(rand())/RAND_MAX-0.5);
#else
(drand48()-0.5);
#endif
elmveccol(1,n,i,x,v,s);
}
fx=(*funct)(x, data);
nf++;
}
for (k2=k; k2<=n; k2++) {
sl=fx;
s=0.0;
praxismin(k2,2,&(d[k2]),&s,&fx,0,
n,x,v,&qa,&qb,&qc,qd0,qd1,q0,q1,&nf,
&nl,&fx,m2,m4,dmin,ldt,reltol,abstol,small,h,funct, data);
s = illc ? d[k2]*(s+z[k2])*(s+z[k2]) : sl-fx;
if (df < s) {
df=s;
kl=k2;
}
}
if (!illc && df < fabs(100.0*macheps*fx))
illc=1;
else
break;
}
for (k2=1; k2<=k-1; k2++) {
s=0.0;
praxismin(k2,2,&(d[k2]),&s,&fx,0,
n,x,v,&qa,&qb,&qc,qd0,qd1,q0,q1,&nf,
&nl,&fx,m2,m4,dmin,ldt,reltol,abstol,small,h,funct, data);
}
f1=fx;
fx=sf;
lds=0.0;
for (i=1; i<=n; i++) {
sl=x[i];
x[i]=y[i];
y[i] = sl -= y[i];
lds += sl*sl;
}
lds=sqrt(lds);
if (lds > small) {
for (i=kl-1; i>=k; i--) {
for (j=1; j<=n; j++) v[j][i+1]=v[j][i];
d[i+1]=d[i];
}
d[k]=0.0;
dupcolvec(1,n,k,v,y);
mulcol(1,n,k,k,v,v,1.0/lds);
praxismin(k,4,&(d[k]),&lds,&f1,1,
n,x,v,&qa,&qb,&qc,qd0,qd1,q0,q1,&nf,
&nl,&fx,m2,m4,dmin,ldt,reltol,abstol,small,h,funct, data);
if (lds <= 0.0) {
lds = -lds;
mulcol(1,n,k,k,v,v,-1.0);
}
}
ldt *= ldfac;
if (ldt < lds) ldt=lds;
t2=m2*sqrt(vecvec(1,n,0,x,x))+abstol;
kt = (ldt > 0.5*t2) ? 0 : kt+1;
if (kt > ktm) {
out[1]=0.0;
emergency=1;
}
}
if (emergency) break;
/* quad */
s=fx;
fx=qf1;
qf1=s;
qd1=0.0;
for (i=1; i<=n; i++) {
s=x[i];
x[i]=l=q1[i];
q1[i]=s;
qd1 += (s-l)*(s-l);
}
l=qd1=sqrt(qd1);
s=0.0;
if ((qd0*qd1 > DBL_MIN) && (nl >=3*n*n)) {
praxismin(0,2,&s,&l,&qf1,1,
n,x,v,&qa,&qb,&qc,qd0,qd1,q0,q1,&nf,
&nl,&fx,m2,m4,dmin,ldt,reltol,abstol,small,h,funct, data);
qa=l*(l-qd1)/(qd0*(qd0+qd1));
qb=(l+qd0)*(qd1-l)/(qd0*qd1);
qc=l*(l+qd0)/(qd1*(qd0+qd1));
} else {
fx=qf1;
qa=qb=0.0;
qc=1.0;
}
qd0=qd1;
for (i=1; i<=n; i++) {
s=q0[i];
q0[i]=x[i];
x[i]=qa*s+qb*x[i]+qc*q1[i];
}
/* end of quad */
dn=0.0;
for (i=1; i<=n; i++) {
d[i]=1.0/sqrt(d[i]);
if (dn < d[i]) dn=d[i];
}
for (j=1; j<=n; j++) {
s=d[j]/dn;
mulcol(1,n,j,j,v,v,s);
}
if (scbd > 1.0) {
s=vlarge;
for (i=1; i<=n; i++) {
sl=z[i]=sqrt(mattam(1,n,i,i,v,v));
if (sl < m4) z[i]=m4;
if (s > sl) s=sl;
}
for (i=1; i<=n; i++) {
sl=s/z[i];
z[i]=1.0/sl;
if (z[i] > scbd) {
sl=1.0/scbd;
z[i]=scbd;
}
mulrow(1,n,i,i,v,v,sl);
}
}
for (i=1; i<=n; i++) ichrowcol(i+1,n,i,i,v);
em[0]=em[2]=macheps;
em[4]=10*n;
em[6]=vsmall;
dupmat(1,n,1,n,a,v);
if (qrisngvaldec(a,n,n,d,v,em) != 0) {
out[1]=2.0;
emergency=1;
}
if (emergency) break;
if (scbd > 1.0) {
for (i=1; i<=n; i++) mulrow(1,n,i,i,v,v,z[i]);
for (i=1; i<=n; i++) {
s=sqrt(tammat(1,n,i,i,v,v));
d[i] *= s;
s=1.0/s;
mulcol(1,n,i,i,v,v,s);
}
}
for (i=1; i<=n; i++) {
s=dn*d[i];
d[i] = (s > large) ? vsmall :
((s < small) ? vlarge : 1.0/(s*s));
}
/* sort */
for (i=1; i<=n-1; i++) {
k=i;
s=d[i];
for (j=i+1; j<=n; j++)
if (d[j] > s) {
k=j;
s=d[j];
}
if (k > i) {
d[k]=d[i];
d[i]=s;
for (j=1; j<=n; j++) {
s=v[j][i];
v[j][i]=v[j][k];
v[j][k]=s;
}
}
}
/* end of sort */
dmin=d[n];
if (dmin < small) dmin=small;
illc = (m2*d[1]) > dmin;
if (nf >= maxf) {
out[1]=1.0;
break;
}
}
out[2]=fx;
out[4]=nf;
out[5]=nl;
out[6]=ldt;
free_real_vector(d,1);
free_real_vector(y,1);
free_real_vector(z,1);
free_real_vector(q0,1);
free_real_vector(q1,1);
free_real_matrix(v,1,n,1);
free_real_matrix(a,1,n,1);
// for (i=0; i<40; ++i) data->x[i] = (double)x[i+1];
}
void strassen(double **a, double **b, double **c, int tam) {
/*trivial case: when the matrix is 1 X 1:
*/
if (tam <= BREAK){
if (tam == 1) {
c[0][0] = a[0][0] * b[0][0];
return;
}
int i, j, k;
for (i = 0; i < tam; i++) {
for (k = 0; k < tam; k++) {
for (j = 0; j < tam; j++) {
c[i][j] += a[i][k] * b[k][j];
}
}
}
return;
}
// other cases are treated here:
int newTam = tam/2;
double **a11, **a12, **a21, **a22;
double **b11, **b12, **b21, **b22;
double **c11, **c12, **c21, **c22;
double **p1, **p2, **p3, **p4, **p5, **p6, **p7;
// memory allocation:
a11 = allocate_real_matrix(newTam, 0);
a12 = allocate_real_matrix(newTam, 0);
a21 = allocate_real_matrix(newTam, 0);
a22 = allocate_real_matrix(newTam, 0);
b11 = allocate_real_matrix(newTam, 0);
b12 = allocate_real_matrix(newTam, 0);
b21 = allocate_real_matrix(newTam, 0);
b22 = allocate_real_matrix(newTam, 0);
c11 = allocate_real_matrix(newTam, 0);
c12 = allocate_real_matrix(newTam, 0);
c21 = allocate_real_matrix(newTam, 0);
c22 = allocate_real_matrix(newTam, 0);
p1 = allocate_real_matrix(newTam, 0);
p2 = allocate_real_matrix(newTam, 0);
p3 = allocate_real_matrix(newTam, 0);
p4 = allocate_real_matrix(newTam, 0);
p5 = allocate_real_matrix(newTam, 0);
p6 = allocate_real_matrix(newTam, 0);
p7 = allocate_real_matrix(newTam, 0);
double **aResult = allocate_real_matrix(newTam, 0);
double **bResult = allocate_real_matrix(newTam, 0);
int i, j;
//dividing the matrices in 4 sub-matrices:
for (i = 0; i < newTam; i++) {
for (j = 0; j < newTam; j++) {
a11[i][j] = a[i][j];
a12[i][j] = a[i][j + newTam];
a21[i][j] = a[i + newTam][j];
a22[i][j] = a[i + newTam][j + newTam];
b11[i][j] = b[i][j];
b12[i][j] = b[i][j + newTam];
b21[i][j] = b[i + newTam][j];
b22[i][j] = b[i + newTam][j + newTam];
}
}
// Calculating p1 to p7:
sum(a11, a22, aResult, newTam); // a11 + a22
sum(b11, b22, bResult, newTam); // b11 + b22
strassen(aResult, bResult, p1, newTam); // p1 = (a11+a22) * (b11+b22)
sum(a21, a22, aResult, newTam); // a21 + a22
strassen(aResult, b11, p2, newTam); // p2 = (a21+a22) * (b11)
subtract(b12, b22, bResult, newTam); // b12 - b22
strassen(a11, bResult, p3, newTam); // p3 = (a11) * (b12 - b22)
subtract(b21, b11, bResult, newTam); // b21 - b11
strassen(a22, bResult, p4, newTam); // p4 = (a22) * (b21 - b11)
sum(a11, a12, aResult, newTam); // a11 + a12
strassen(aResult, b22, p5, newTam); // p5 = (a11+a12) * (b22)
subtract(a21, a11, aResult, newTam); // a21 - a11
sum(b11, b12, bResult, newTam); // b11 + b12
strassen(aResult, bResult, p6, newTam); // p6 = (a21-a11) * (b11+b12)
subtract(a12, a22, aResult, newTam); // a12 - a22
sum(b21, b22, bResult, newTam); // b21 + b22
strassen(aResult, bResult, p7, newTam); // p7 = (a12-a22) * (b21+b22)
// calculating c21, c21, c11 e c22:
sum(p3, p5, c12, newTam); // c12 = p3 + p5
sum(p2, p4, c21, newTam); // c21 = p2 + p4
sum(p1, p4, aResult, newTam); // p1 + p4
sum(aResult, p7, bResult, newTam); // p1 + p4 + p7
subtract(bResult, p5, c11, newTam); // c11 = p1 + p4 - p5 + p7
sum(p1, p3, aResult, newTam); // p1 + p3
sum(aResult, p6, bResult, newTam); // p1 + p3 + p6
subtract(bResult, p2, c22, newTam); // c22 = p1 + p3 - p2 + p6
// Grouping the results obtained in a single matrix:
for (i = 0; i < newTam ; i++) {
for (j = 0 ; j < newTam ; j++) {
c[i][j] = c11[i][j];
c[i][j + newTam] = c12[i][j];
c[i + newTam][j] = c21[i][j];
c[i + newTam][j + newTam] = c22[i][j];
}
}
// deallocating memory (free):
a11 = free_real_matrix(a11, newTam);
a12 = free_real_matrix(a12, newTam);
a21 = free_real_matrix(a21, newTam);
a22 = free_real_matrix(a22, newTam);
b11 = free_real_matrix(b11, newTam);
b12 = free_real_matrix(b12, newTam);
b21 = free_real_matrix(b21, newTam);
b22 = free_real_matrix(b22, newTam);
c11 = free_real_matrix(c11, newTam);
c12 = free_real_matrix(c12, newTam);
c21 = free_real_matrix(c21, newTam);
c22 = free_real_matrix(c22, newTam);
p1 = free_real_matrix(p1, newTam);
p2 = free_real_matrix(p2, newTam);
p3 = free_real_matrix(p3, newTam);
p4 = free_real_matrix(p4, newTam);
p5 = free_real_matrix(p5, newTam);
p6 = free_real_matrix(p6, newTam);
p7 = free_real_matrix(p7, newTam);
aResult = free_real_matrix(aResult, newTam);
bResult = free_real_matrix(bResult, newTam);
} // end of Strassen function
void gssnewton(int m, int n, real_t par[], real_t rv[], real_t **jjinv,
int (*funct)(int, int, real_t[], real_t[]),
void (*jacobian)(int, int, real_t[], real_t[], real_t **),
real_t in[], real_t out[])
{
int *allocate_integer_vector(int, int);
real_t *allocate_real_vector(int, int);
real_t **allocate_real_matrix(int, int, int, int);
void free_integer_vector(int *, int);
void free_real_vector(real_t *, int);
void free_real_matrix(real_t **, int, int, int);
real_t vecvec(int, int, int, real_t [], real_t []);
void dupvec(int, int, int, real_t [], real_t []);
void elmvec(int, int, int, real_t [], real_t [], real_t);
void lsqortdec(real_t **, int, int, real_t [], real_t [], int []);
void lsqsol(real_t **, int, int, real_t [], int [], real_t []);
void lsqinv(real_t **, int, real_t [], int []);
int i,j,inr,mit,text,it,itmax,inrmax,tim,feval,fevalmax,conv,
testthf,dampingon,*ci,fail;
real_t rho,res1,res2,rn,reltolpar,abstolpar,abstolres,stap,normx,
**jac,*pr,*aid,*sol,*fu2,aux[6];
ci=allocate_integer_vector(1,n);
pr=allocate_real_vector(1,n);
aid=allocate_real_vector(1,n);
sol=allocate_real_vector(1,n);
fu2=allocate_real_vector(1,m);
jac=allocate_real_matrix(1,m+1,1,n);
itmax=fevalmax=in[5];
aux[2]=n*in[0];
tim=in[7];
reltolpar=in[1]*in[1];
abstolpar=in[2]*in[2];
abstolres=in[4]*in[4];
inrmax=in[6];
dupvec(1,n,0,pr,par);
if (m < n)
for (i=1; i<=n; i++) jac[m+1][i]=0.0;
text=4;
mit=0;
testthf=1;
res2=stap=out[5]=out[6]=out[7]=0.0;
(*funct)(m,n,par,fu2);
rn=vecvec(1,m,0,fu2,fu2);
out[3]=sqrt(rn);
feval=1;
dampingon=0;
fail=0;
it=1;
do {
out[5]=it;
(*jacobian)(m,n,par,fu2,jac);
if (!testthf) {
text=7;
fail=1;
break;
}
lsqortdec(jac,m,n,aux,aid,ci);
if (aux[3] != n) {
text=5;
fail=1;
break;
}
lsqsol(jac,m,n,aid,ci,fu2);
dupvec(1,n,0,sol,fu2);
stap=vecvec(1,n,0,sol,sol);
rho=2.0;
normx=vecvec(1,n,0,par,par);
if (stap > reltolpar*normx+abstolpar || it == 1 && stap > 0.0) {
inr=0;
do {
rho /= 2.0;
if (inr > 0) {
res1=res2;
dupvec(1,m,0,rv,fu2);
dampingon = inr > 1;
}
for (i=1; i<=n; i++) pr[i]=par[i]-sol[i]*rho;
feval++;
if (!(*funct)(m,n,pr,fu2)) {
text=6;
fail=1;
break;
}
res2=vecvec(1,m,0,fu2,fu2);
conv = inr >= inrmax;
inr++;
} while ((inr == 1) ? (dampingon || res2 >= rn) :
(!conv && (rn <= res1 || res2 < res1)));
if (fail) break;
if (conv) {
mit++;
if (mit < tim) conv=0;
} else
mit=0;
if (inr > 1) {
rho *= 2.0;
elmvec(1,n,0,par,sol,-rho);
rn=res1;
if (inr > 2) out[7]=it;
} else {
dupvec(1,n,0,par,pr);
rn=res2;
dupvec(1,m,0,rv,fu2);
}
if (rn <= abstolres) {
text=1;
itmax=it;
} else
if (conv && inrmax > 0) {
text=3;
itmax=it;
} else
dupvec(1,m,0,fu2,rv);
} else {
text=2;
rho=1.0;
itmax=it;
}
it++;
} while (it <= itmax && feval < fevalmax);
if (!fail) {
lsqinv(jac,n,aid,ci);
for (i=1; i<=n; i++) {
jjinv[i][i]=jac[i][i];
for (j=i+1; j<=n; j++) jjinv[i][j]=jjinv[j][i]=jac[i][j];
}
}
out[6]=sqrt(stap)*rho;
out[2]=sqrt(rn);
out[4]=feval;
out[1]=text;
out[8]=aux[3];
out[9]=aux[5];
free_integer_vector(ci,1);
free_real_vector(pr,1);
free_real_vector(aid,1);
free_real_vector(sol,1);
free_real_vector(fu2,1);
free_real_matrix(jac,1,m+1,1);
}
/*-------------------------------------------------------------------------------
calculate the least squares solution of an overdetermined system of nonlinear equations
with Marquardt's method
-------------------------------------------------------------------------------*/
void Ti_Optimization::MarquardtforCylinderFitting(
int m,
int n,
double**g_pnt,
double* const par,
double*& g,
double**v,
int (*funct)(int m, int n, double* const par, double* g,double**g_pnt),
void (*jacobian)(int m, int n, double* const par, double*& g, double **jac,double**g_pnt),
double in[],
double out[]
)
{
int maxfe,fe,it,i,j,err,emergency;
double vv,ww,w,mu,res,fpar,fparpres,lambda,lambdamin,p,pw,reltolres,
abstolres,em[8],*val,*b,*bb,*parpres,**jac,temp;
val = allocate_real_vector(1,n);
b = allocate_real_vector(1,n);
bb = allocate_real_vector(1,n);
parpres = allocate_real_vector(1,n);
jac = allocate_real_matrix(1,m,1,n);
assert( (val != NULL) &&
(b != NULL) &&
(bb != NULL) &&
(parpres!= NULL)&&
(jac != NULL)
);
vv = 10.0;
w = 0.5;
mu = 0.01;
ww = (in[6] < 1.0e-7) ? 1.0e-8 : 1.0e-1*in[6];
em[0] = em[2] = em[6] = in[0];
em[4] = 10*n;
reltolres =in[3];
abstolres=in[4]*in[4];
maxfe=(int)in[5];
err=0;
fe=it=1;
p=fpar=res=0.0;
pw = -log(ww*in[0])/2.30;
if (!(*funct)(m,n,par,g,g_pnt))
{
err=3;
out[4]=fe;
out[5]=it-1;
out[1]=err;
free_real_vector(val,1);
free_real_vector(b,1);
free_real_vector(bb,1);
free_real_vector(parpres,1);
free_real_matrix(jac,1,m,1);
return;
}
fpar=vecvec(1,m,0,g,g);// norm of residual vector
out[3]=sqrt(fpar);
emergency=0;
it=1;
do {
(*jacobian)(m,n,par,g,jac,g_pnt);
i = qrisngvaldec(jac,m,n,val,v,em);
if (it == 1)
lambda = in[6]*vecvec(1,n,0,val,val);
else
if (p == 0.0)
lambda *= w;
for (i=1; i<=n; i++)
b[i] = val[i]*tamvec(1,m,i,jac,g);
while (1)
{
for (i=1; i<=n; i++)
bb[i]=b[i]/(val[i]*val[i]+lambda);
for (i=1; i<=n; i++)
parpres[i]=par[i]-matvec(1,n,i,v,bb);
//normalization ,this section only used for cylinder fitting,
//when it is used in other situations,it should be removed
temp = sqrt(parpres[4]*parpres[4]+parpres[5]*parpres[5]+parpres[6]*parpres[6]);
parpres[4] /= temp;
parpres[5] /= temp;
parpres[6] /= temp;
//end normalization
fe++;
if (fe >= maxfe)
err=1;
else
if (!(*funct)(m,n,parpres,g,g_pnt))
err=2;
if (err != 0)
{
emergency = 1;
break;
}
fparpres=vecvec(1,m,0,g,g);
res=fpar-fparpres;
if (res < mu*vecvec(1,n,0,b,bb))
{
p += 1.0;
lambda *= vv;
if (p == 1.0)
{
lambdamin=ww*vecvec(1,n,0,val,val);
if (lambda < lambdamin)
lambda=lambdamin;
}
if (p >= pw)
{
err=4;
emergency=1;
break;
}
} // end if
else
{
dupvec(1,n,0,par,parpres);
fpar=fparpres;
break;
} // end else
} // end while
if (emergency)
break;
it++;
}
while (
(fpar > abstolres) &&
(res > reltolres*fpar+abstolres)
);
for (i=1; i<=n; i++)
mulcol(1,n,i,i,jac,v,1.0/(val[i]+in[0]));
for (i=1; i<=n; i++)
{
for (j=1; j<=i; j++)
v[i][j]=v[j][i]=mattam(1,n,i,j,jac,jac);
lambda=lambdamin=val[1];
}
for (i=2; i<=n; i++)
{
if (val[i] > lambda)
lambda=val[i];
else
{
if (val[i] < lambdamin)
lambdamin=val[i];
}
}
temp=lambda/(lambdamin+in[0]);
out[7]=temp*temp;
out[2]=sqrt(fpar);
out[6]=sqrt(res+fpar)-out[2];
out[4]=fe;
out[5]=it-1;
out[1]=err;
if(val != NULL)
{
free_real_vector(val,1);
val = NULL;
}
if (b != NULL)
{
free_real_vector(b,1);
b = NULL;
}
if(bb!=NULL)
{
free_real_vector(bb,1);
bb = NULL;
}
if(parpres != NULL)
{
free_real_vector(parpres,1);
parpres = NULL;
}
if (jac != NULL)
{
free_real_matrix(jac,1,m,1);
jac = NULL;
}
}
