Осуществление заданных движений динамических систем с минимальной энергией

goto m200;

if(fabs(dt) > fabs(h))

goto m150;

output=True;

h=dt;

goto m200;

m150: h=0.5*dt;

/*Внутренний одношаговый интегратор*/

m200: if(nfe <= maxnfe)

goto m220;

iflag=4;

kflag=4;

goto mexit;

/*Продолжить приближенное решение на один шаг длины h*/

m220: FEHL(neqn, t, y, f_p, &h, yp, f1, f2, f3, f4, f5, f1);

nfe=nfe+5;

eeoet=0.0;

for (k=0; k<neqn; k++)

{ et=fabs(y[k])+fabs(f1[k])+ae;

if (et > 0.0)

goto m240;

/*Неправильная граница погрешности*/

iflag=5;

goto mexit;

m240: ee=fabs((-2090.0*yp[k]+(21970.0*f3[k]-15048.0*f4[k]))+

(22528.0*f2[k]-27360.0*f5[k]));

rab1=ee/et;

if (eeoet < rab1)

eeoet=rab1;

}

esttol=fabs(h)*eeoet*scale/752400.0;

if (esttol <= 1.0)

goto m260;

/*Неудачный шаг; уменьшить его величину*/

hfaild=True;

output=False;

s=0.1;

if (esttol < 59049.0)

s=0.9/(exp(0.2*log(esttol)));

h=s*h;

if (fabs(h) > hmin)

goto m200;

/*Неудача даже при минимальном шаге*/

iflag=6;

kflag=6;

goto mexit;

/*Успешный шаг*/

m260: t=t+h;

for (k=0; k<neqn; k++)

y[k]=f1[k];

a=t;

fun(a,y,yp);

nfe=nfe+1;

s=5.0;

if (esttol > 1.889568e-4)

s=0.9/(exp(0.2*log(esttol)));

if (hfaild)

{ if (s > 1.0)

s=1.0;

}

rab1=s*fabs(h);

if (rab1 < hmin)

rab1=hmin;

if (h > 0.0)

h=fabs(rab1);

else

h=-fabs(rab1);

if (output)

goto m300;

if (iflag > 0)

goto m100;

/*Интегрирование успешно завершено*/

iflag=-2;

goto mexit;

m300: iflag=2;

mexit: *iflagc=iflag; *nfec=nfe; *kopc=kop; *initc=init; *jflagc=jflag;

*kflagc=kflag;

*hc=h; *savrec=savre; *savaec=savae;

*relerrc = relerr;

*abserrc = abserr;

return;

}

void prifl0 ( int *iflc,

double *rerrc,

double *aerrc )

{

int

ifl;

double

rerr,

aerr;

ifl = *iflc;

rerr = *rerrc;

aerr = *aerrc;

switch (ifl)

{

case 3: break;

case 4: printf ("много fp()\n ifl=%d\n", ifl);

break;

case 5: aerr = 1.0e-9;

break;

case 6: rerr *= 10.0;

printf ("ifl=%d\n rerr=%.12lf aerr=%.12lf\n", ifl, rerr, aerr);

ifl = 2;

break;

case 7: printf ("много вых. точек\n ifl=%d\n", ifl);

ifl = 2;

break;

case 8: printf ("ifl=%d\n", ifl);

break;

}

*iflc = ifl;

*rerrc = rerr;

*aerrc = aerr;

}



Приложение Б

Текст программы 2

#pragma hdrstop

# include <stdio.h>

# include <math.h>

# include <string.h>

# include <process.h>

# include <alloc.h>

# include <conio.h>

#define M 2

#define N 25

#define N_M 23

#define TBEG0.0

#define TETA 5.0

#define L 3.0

#define Uf -2.0

#define NX 2

double tau,

x[NX] = { 2.0, 4.0 },

yx[NX],

z0[NX] = { 2.0, 3.0 },

b[M], A[M][N], dn[N], dw[N],

eps1 = 0.00001, eps2=0.00001,

P[M][M], **Pr, Q[M][M],

**D,v[N_M], **Ma,u[N],

Del[N], J, a1, a2;

int Jop[M], Jn[N_M], kn,

Joc[N_M], koc, Jnn[N_M], knn,

kod4;

void callocPr ( int );

void freePr ( int );

void puN1NIntU2 ( double );

void puIntU2 ( double );

void formParam ( double );

void formElem ( void );

void procsogl( void );

int gs ( int, double**, /* Метод Гаусса */

double*, int* );

void f_x ( double, double*,/* f1(t) = x2(t) + u(t) */

double* ); /* f2(t) = -x1(t) - u(t) */

void FEHL(int, double,

double*, void (f_p)(double, double*, double*),

double*, double*,

double*, double*,

double*, double*,

double*, double*);

void RKF45 (int, double, /* Метод Рунге-Кутта-Фельдберга */

double, double*,

void (f_p)(double, double*, double*), double*,

double*, double*,

double*, double*,

double*, double*,

int*, int*,

int*, int*,

int*, int*,

double*, double*,

double*, double*);

void prifl0 (int*, double*, double*);

FILE *d;

FILE *r;

FILE *ru; /* Файл.dat : t, u(t) */

FILE *rJ; /* Файл.dat : t, J(t) */

FILE *rx; /* Файл.dat : t, x(t) */

void callocPr ( int n )

{

int i;

Pr = (double**)calloc( n, sizeof(double) );

for ( i=0; i<n; i++ )

Pr[i] = (double*)calloc( n, sizeof(double) );

}

void freePr ( int n )

{

int i;

for ( i=0; i<n; i++ )

free ( Pr[i] );

free ( Pr );

}

void puN1NIntU2 ( double h )/* Прогр.упр.задачи пер.стр.*/

{

int i;

/* Программн. упр-ие миним. энергии: */

kod4 = 0;

puIntU2 ( h );

if ( kod4==0 )

{

J = 0.0;

for ( i=0; i<N; i++ )

J += u[i]*u[i]/2.0;

J *= h;

printf ( "J = %.12lf\n", J );

}

else

{

printf ( "Нет решения !!!\n" );

getchar();

}

} /* puN1NIntU2 */

void puIntU2 ( double h )/* Прогр.упр.мин.эн. */

{

double ur, **H;

int i, j, k, kod, nn, UslSogl, UslOpt;

H = (double**)calloc( N_M, sizeof(double) );

for ( j=0; j<N_M; j++ )

H[j] = (double*)calloc( N_M, sizeof(double) );

/* Формирование параметров конечномерн. задачи: */

formParam ( h );

/* Формирование элементов опорного псевдоплана: */

formElem ();

/* Пров-ка согласов-ти оп.пс-плана: */

UslSogl = 1;

for ( j=0; j<knn; j++ )

if ( ( ( Del[ Jnn[j] ] < -eps1 ) &&

( fabs( u[ Jnn[j] ] - dn[ Jnn[j] ]) < eps2 ) ) ||

( ( Del[ Jnn[j] ] > eps1 ) &&

( fabs( u[ Jnn[j] ] - dw[ Jnn[j] ]) < eps2 ) ) )

{

UslSogl = 0;

break;

}

printf ( "UslSogl = %d\n", UslSogl );

if ( !UslSogl )

procsogl ();

/*printf ( "A * u - b = (" );

UslOpt = 1;

for ( j=0; j<M; j++ )

if ( ( u[Jop[j]] < dn[Jop[j]] - eps2 ) ||

( u[Jop[j]] > dw[Jop[j]] + eps2 ) )

{

UslOpt = 0;

break;

}

if ( !UslOpt )

{

/* Реш.конечномерн.з-чи дв. методом: */

/* Открытие файла для передачи в прогр. *.for (двойств.метод) */

if ( ( d = fopen ( "dat", "wt" ) ) == NULL )

{

perror ( "dat" );

exit (-1);

}

/* Выч.обр.м. к м.D */

callocPr ( koc );

for ( j=0; j<koc; j++ )

{

for ( i=0; i<koc; i++ )

{

for ( k=0; k<koc; k++ )

Pr[i][k] = D[i][k];

v[i] = 0.0;

}

v[j] = 1.0;

gs ( koc, Pr, v, &kod );

if ( kod )

{

printf ( "\nОпорная матрица (цел.ф-ии) вырождена !!!\n" );

getchar();

}

for ( i=0; i<koc; i++ )

H[i][j] = v[i];

}

freePr ( koc );

/* Печать параметров задачи в файл */

fprintf ( d, "%d %d\n", M, N );/* размеры M,N */

for ( j=0; j<N; j++ ) /* матрица A */

{

for ( i=0; i<M; i++ )

fprintf ( d, "%lf ", A[i][j] );

fprintf ( d, "\n" );

}

for ( i=0; i<M; i++ ) /* вектор b */

fprintf ( d, "%lf ", b[i] );

fprintf ( d, "\n" );

for ( i=0; i<N; i++ ) /* векторы dn, dw */

fprintf ( d, "%lf %lf\n", dn[i], dw[i] );

fprintf ( d, "%d\n", M );/* размер оп.огр. M */

for ( i=0; i<M; i++ ) /* обр.оп.м. Q */

{

for ( j=0; j<M; j++ )

fprintf ( d, "%lf ", Q[i][j] );

fprintf ( d, "\n" );

}

fprintf ( d, "%d\n", koc ); /* разм.оп.ц.ф. koc */

for ( j=0; j<koc; j++ )/* верх.треуг.м. H */

for ( i=0; i<=j; i++ )

fprintf ( d, "%lf\n", H[i][j] );

for ( i=0; i<M; i++ ) /* IX=(Jop,Joc,Jnn) */

fprintf ( d, "%d\n", Jop[i] + 1 );

for ( i=0; i<koc; i++ )

fprintf ( d, "%d\n", Joc[i] + 1 );

for ( i=0; i<knn; i++ )

fprintf ( d, "%d\n", Jnn[i] + 1 );

for (j=0; j<N; j++ )

fprintf ( d, "%lf\n", u[j] );

fclose (d);

system ( "errfile.exe" );

system ( "dmqp.exe" );

/* Открытие файла для чтения из прогр. *.for (двойств.метода) */

if ( ( r = fopen ( "res", "r" ) ) == NULL )

{

perror ( "res" );

exit (-1);

}

fscanf( r, "%d", &kod4 );

printf ( "Результаты:\nkod(4) = %d\n", kod4 );

if ( kod4==0 )

{

printf ( "Jop = (" );

fscanf( r, "%d", &i );

nn = i;

for ( j=0; j<i; j++ )

{

fscanf( r, "%d", &k );

Jop [j] = k - 1;

printf ( "%d ", Jop[j] );

}

printf ( " )\n" );

printf ( "Joc = (" );

fscanf( r, "%d", &i );

nn += i;

for ( j=0; j<i; j++ )

{

fscanf( r, "%d", &k );

Joc[j] = k - 1;

printf ( "%d ", Joc[j] );

}

printf ( " )\n" );

printf ( "Jnn = (" );

for ( j=0; j<N-nn; j++ )

{

fscanf( r, "%d", &k );

Jnn[j] = k - 1;

printf ( "%d ", Jnn[j] );

}

printf ( " )\n" );

for ( k=0; k<N; k++ )

{

fscanf( r, "%lf", &ur );

u[k] = ur;

}

printf ( "u =\n" );

for ( i=0; i<10; i++ )

printf ( "%7.1lf", u[i] );

printf ( "\n" );

for ( i=10; i<20; i++ )

printf ( "%7.1lf", u[i] );

printf ( "\n" );

for ( i=20; i<N; i++ )

printf ( "%7.1lf", u[i] );

printf ( "\n" );

} /* if ( kod4==0 ) */

fclose (r);

} /* if ( !UslOpt ) */

for ( j=0; j<N_M; j++ )

free ( H[j] );

free ( H );

} /* puIntU2 */

void formParam ( double h )/* Форм.парам.конечном.з-чи */

{

double t_tn, t_tw,

te;

int i, j;

for ( j=0; j<N; j++ )

{

t_tn = N*h - j*h;

t_tw = N*h - (j+1)*h;

A[0][j] = sin( t_tn ) - sin( t_tw ) + cos( t_tn ) - cos( t_tw );

A[1][j] = cos( t_tn ) - cos( t_tw ) + sin( t_tw ) - sin( t_tn );

}

te = N*h;

yx[0] = x[0] - ( z0[1] + Uf )*sin( tau ) - ( z0[0] + Uf )*cos( tau ) + Uf;

yx[1] = x[1] + ( z0[0] + Uf )*sin( tau ) - ( z0[1] + Uf )*cos( tau ) + Uf;

b[0] = -cos( te )*yx[0] - sin( te )*yx[1];

b[1] = sin( te )*yx[0] - cos( te )*yx[1];

for ( i=0; i<N; i++ ) /* в-ры dn, dw */

{

dn[i] = -L-Uf;

dw[i] = L-Uf;

}

} /* formParam */

void formElem ( void ) /* Форм.эл-тов оп.пс-плана */

{

int i, j, k, jr, kod, PrOp;

/* Опора огр. Jop */

Jop[0] = 0;

Jop[1] = 24;

kn = N - M; /* неопорн.инд. Jn */

k = 0;

for ( j=0; j<N; j++ )

{

PrOp = 0;

for ( i=0; i<M; i++ )

if ( j==Jop[i] )

{

PrOp = 1;

break;

}

if ( !PrOp )

{

Jn[k] = j;

k++;

}

}

koc = 0;

knn = kn; /* дв.неоп.инд. Jnn */

for ( j=0; j<knn; j++ )

Jnn[j] = Jn[j];

for ( i=0; i<M; i++ ) /* P=A(I,Jop) */

for ( j=0; j<M; j++ )

P[i][j] = A[i][Jop[j]];

callocPr ( M );

for ( j=0; j<M; j++ ) /* м. Q -- обр.к P */

{

for ( i=0; i<M; i++ )

{

for ( k=0; k<M; k++ )

Pr[i][k] = P[i][k];

v[i] = 0.0;

}

v[j] = 1.0;

gs ( M, Pr, v, &kod );

if ( kod )

{

printf ( "\nОпорная матрица (ограничений) вырождена !!!\n" );

getchar();

}

for ( i=0; i<M; i++ )

Q[i][j] = v[i];

}

freePr ( M );

/* псевдоплан u */

for ( j=0; j<knn; j++ ) /* unn */

u[Jnn[j]] = dn[Jnn[j]];

if ( koc>0 ) /* uoc */

{

/* uoc */

}

if ( koc>0 ) /* U=(*,Uoc,Unn) */

for ( i=0; i<koc; i++ )

u[Joc[i]] = v[i];

callocPr ( M );

for ( i=0; i<M; i++ ) /* uop */

{

for ( j=0; j<M; j++ )

Pr[i][j] = P[i][j];

v[i] = b[i];

for ( j=0; j<kn; j++ )

v[i] -= A[i][Jn[j]] * u[Jn[j]];

}

gs ( M, Pr, v, &kod );

freePr ( M );

if ( kod )

{

printf ( "\nОпорная матрица (ограничений) вырождена !!!\n" );

getchar();

}

for ( i=0; i<M; i++ ) /* U=(Uop,Uoc,Unn) */

u[Jop[i]] = v[i];

callocPr ( M );

for ( i=0; i<M; i++ ) /* Вект.потенц. Up */

{

for ( k=0; k<M; k++ )

Pr[i][k] = P[k][i];

v[i] = u[Jop[i]];

}

gs ( M, Pr, v, &kod );

freePr ( M );

if ( kod )

{

printf ( "\nОпорная матрица (ограничений) вырождена !!!\n" );

getchar();

}

for ( j=0; j<N; j++ ) /* вект.оц. Delta */

{

Del[j] = u[j];

for ( i=0; i<M; i++ )

Del[j] -= v[i] * A[i][j];

}

} /* formElem */

void procsogl( void )

{

double l[N], Qb[M], Th, Thj;

int i, j, k, kod, iterS, prTh, j0;

iterS = 0;

while (1)

{

printf ( "iterS = %d\n", iterS + 1 );

/* l */

for ( j=0; j<knn; j++ ) /* lnn */

if ( Del[Jnn[j]] > eps1 )

l[Jnn[j]] = dn[Jnn[j]] - u[Jnn[j]];

else

if ( Del[Jnn[j]] < -eps1 )

l[Jnn[j]] = dw[Jnn[j]] - u[Jnn[j]];

else

l[Jnn[j]] = 0.0;

if ( koc>0 ) /* loc */

{

callocPr ( M );

for ( j=0; j<koc; j++ ) /* Выч.матр. Q*Aoc */

{

for ( i=0; i<M; i++ )

{

for ( k=0; k<M; k++ )

Pr[i][k] = P[i][k];

v[i] = A[i][Joc[j]];

}

gs ( M, Pr, v, &kod );

if ( kod )

{

printf ( "\nОпорная матрица (ограничений) вырождена !!!\n" );

getchar();

}

for ( i=0; i<M; i++ )

Ma[i][j] = v[i];

}

freePr ( M );

for ( i=0; i<koc; i++ ) /* D=E+Aoc'Q'QAoc */

for ( j=0; j<koc; j++ )

{

D[i][j] = 0.0;

for ( k=0; k<M; k++ )

D[i][j] += Ma[k][i] * Ma[k][j];

}

for ( i=0; i<koc; i++ )

D[i][i] += 1.0;

callocPr ( M );

for ( i=0; i<M; i++ ) /* Qb=Q*Ann*lnn */

{

for ( k=0; k<M; k++ )

Pr[i][k] = P[i][k];

Qb[i] = 0.0;

for ( j=0; j<knn; j++ )

Qb[i] += A[i][Jnn[j]] * l[Jnn[j]];

}

gs ( M, Pr, Qb, &kod );

freePr ( M );

if ( kod )

{

printf ( "\nОпорная матрица (ограничений) вырождена !!!\n" );

getchar();

}

for ( i=0; i<koc; i++ ) /* boc=Aoc'Q'Qb */

{

v[i] = 0.0;

for ( j=0; j<M; j++ )

v[i] -= Ma[j][i] * Qb[j];

}

callocPr ( koc );

for ( i=0; i<koc; i++ ) /* Выч. loc */

for ( k=0; k<koc; k++ )

Pr[i][k] = D[i][k];

gs ( koc, Pr, v, &kod );

freePr ( koc );

if ( kod )

{

printf ( "\nОпорная матрица (цел.ф-ии) вырождена !!!\n" );

getchar();

}

}

for ( j=0; j<koc; j++ )/* l=(*,loc,lnn) */

l[Joc[j]] = v[j];

callocPr ( M );

for ( i=0; i<M; i++ )/* lop */

{

for ( j=0; j<M; j++ )

Pr[i][j] = P[i][j];

v[i] = 0.0;

for ( j=0; j<kn; j++ )

v[i] -= A[i][Jn[j]] * l[Jn[j]];

}

gs ( M, Pr, v, &kod );

freePr ( M );

if ( kod )

{

printf ( "\nОпорная матрица (ограничений) вырождена !!!\n" );

getchar();

}

for ( j=0; j<M; j++ ) /* l=(lop,loc,lnn) */

l[Jop[j]] = v[j];

/* tnn */

callocPr ( M );

for ( i=0; i<M; i++ ) /* Qb=Q'*lop */

{

for ( k=0; k<M; k++ )

Pr[i][k] = P[k][i];

Qb[i] = l[Jop[i]];

}

gs ( M, Pr, Qb, &kod );

freePr ( M );

if ( kod )

{

printf ( "\nОпорная матрица (ограничений) вырождена !!!\n" );

getchar();

}

for ( j=0; j<knn; j++ ) /* tnn */

{

v[j] = l[Jnn[j]];

for ( i=0; i<M; i++ )

v[j] -= Qb[i] * A[i][Jnn[j]];

}

Th = 1; /* Theta */

prTh = -1;

for ( j=0; j<koc; j++ ) /* Theta(Joc) */

if ( l[Joc[j]] < -eps2 )

{

Thj = ( dn[Joc[j]] - u[Joc[j]] ) / l[Joc[j]];

if ( Thj < Th )

{

Th = Thj;

j0 = j; /* ?????? */

prTh = 0;

}

}

else

if ( l[Joc[j]] > eps2 )

{

Thj = ( dw[Joc[j]] - u[Joc[j]] ) / l[Joc[j]];

if ( Thj < Th )

{

Th = Thj;

j0 = j;

prTh = 0;

}

}

for ( j=0; j<knn; j++ ) /* Th(Jnn) */

if ( Del[Jnn[j]] * v[j] < -eps1 )

{

Thj = -Del[Jnn[j]] / v[j];

if ( Thj < Th )

{

Th = Thj;

j0 = j;

prTh = 1;

}

}

for ( j=0; j<N; j++ ) /* u=u+Th*l */

u[j] += Th*l[j];

if ( Th == 1 )

{

break;

}

else

if ( prTh )

{

Joc[koc] = Jnn[j0];

koc++;

knn--;

for ( i=j0; i<knn; i++ )

Jnn[i] = Jnn[i+1];

}

else

{

Jnn[knn] = Joc[j0];

knn++;

koc--;

for ( i=j0; i<koc; i++ )

Joc[i] = Joc[i+1];

}

callocPr ( M );

for ( i=0; i<M; i++ ) /* Вект.потенц. Up */

{

for ( k=0; k<M; k++ )

Pr[i][k] = P[k][i];

v[i] = u[Jop[i]];

}

gs ( M, Pr, v, &kod );

freePr ( M );

if ( kod )

{

printf ( "\nОпорная матрица (ограничений) вырождена !!!\n" );

getchar();

}

for ( j=0; j<N; j++ ) /* вект.оц. Delta */

{

Del[j] = u[j];

for ( i=0; i<M; i++ )

Del[j] -= v[i] * A[i][j];

}

iterS++;

} /* while(1) */

}

int gs ( int m, double **pX,

double copX[], int *kod )

{

double tol = 1E-5,

bigpX, pXpX,

sav;

int i, j, k, ky, imax,

ix, jx, i0, ib;

for ( j=0; j<m; j++ )

{

bigpX = 0.;

for ( i=j; i<m; i++ )

{

pXpX = fabs ( pX [i] [j] );

if ( fabs ( bigpX ) < pXpX )

{

bigpX = pX [i] [j];

imax = i;

}

}

if ( fabs ( bigpX ) <= tol )

{

*kod = 1;

return 1;

}

for ( k=j; k<m; k++ )

{

sav = pX [imax] [k];

pX [imax] [k] = pX [j] [k];

pX [j] [k] = sav / bigpX;

}

sav = copX [imax];

copX [imax] = copX [j];

copX [j] = sav / bigpX;

if ( j==m-1 )

break;

for ( ix=j+1; ix < m; ix++ )

{

for ( jx=j+1; jx < m; jx++ )

pX [ix] [jx] -= pX [ix] [j] * pX [j] [jx];

copX [ix] -= copX [j] * pX [ix] [j];

}

}

for ( ky=0; ky < m-1; ky++ )

{

ib = m-2-ky;

i0 = m-1;

for ( k=0; k <= ky; k++ )

{

copX [ib] -= pX [ib] [i0] * copX [i0];

i0--;

}

}

*kod=0;

return 0;

}

void f_x ( double t, /* Выч. f1(t) = x2(t) + u(t) */

double y[], /* f2(t) = -x1(t) - u(t) */

double f[] )

{

f[0] = y[1] + ( u[0] + Uf );

f[1] = -y[0] - ( u[0] + Uf ) + a1*sin( a2*t );

} /* f_x() */

# define True 1

# define False 0

void FEHL(int neqn, double t,

double y[], void (f_p)(double, double*, double*),

double *hc, double yp[],

double f1[], double f2[],

double f3[], double f4[],

double f5[], double s[])

{ double h;

double ch,rab;

int k;

void (*fun)(double, double*, double*);

h=*hc;

fun = f_p;

ch = h/4.0;

for (k=0; k<neqn; k++)

f5[k] = y[k] + ch * yp[k];

rab = t + ch;

fun(rab,f5,f1);

ch = 3.0 * h / 32.0;

for (k=0; k<neqn; k++)

f5[k] = y[k] + ch * (yp[k] + 3.0 * f1[k]);

rab = t + 3.0 * h / 8.0;

fun(rab,f5,f2);

ch = h / 2197.0;

for (k=0; k<neqn; k++)

f5[k] = y[k] + ch * (1932.0 * yp[k] + (7296.0 * f2[k] - 7200.0 * f1[k]));

rab = t + 12.0 * h / 13.0;

fun(rab,f5,f3);

ch = h / 4104.0;

for (k=0; k<neqn; k++)

f5[k] = y[k] + ch * ((8341.0 * yp[k] - 845.0 * f3[k]) +

(29440.0 * f2[k] - 32832.0 * f1[k]));

rab = t + h;

fun(rab,f5,f4);

ch = h / 20520.0;

for (k=0; k<neqn; k++)

f1[k] = y[k] + ch * ((-6080.0 * yp[k] + (9295.0 * f3[k] -

5643.0 * f4[k])) + (41040.0 * f1[k] - 28352.0 * f2[k]));

rab = t + h / 2.0;

fun(rab,f1,f5);

/*ВЫЧИСЛИТЬ ПРИБЛИЖЕННОЕ РЕШЕНИЕ В ТОЧКЕ t + h*/

ch = h / 7618050.0;

for (k=0; k<neqn; k++)

s[k] = y[k] + ch * ((902880.0 * yp[k] + (3855735.0 * f3[k] -

1371249.0 * f4[k])) + (3953664.0 * f2[k] + 277020.0 * f5[k]));

*hc=h;

return;

}

void RKF45 (int neqn, double t,

double tout, double y[],

void (f_p)(double, double*, double*), double *hc,

double yp[], double f1[],

double f2[], double f3[],

double f4[], double f5[],

int *iflagc, int *nfec,

int *kopc, int *initc,

int *jflagc, int *kflagc,

double *relerrc, double *abserrc,

double *savrec, double *savaec)

{

int iflag,nfe,kop,init,jflag,kflag;

double h,savre,savae,

relerr, abserr;

int maxnfe = 30000;

double remin = 1.0e-15;

int hfaild, output;

double a,ae,dt,ee,eeoet,esttol,et,hmin,eps,u26,

rer,s,scale,tol,toln,ypk,rab1;

int k, mflag;

void (*fun)(double, double*, double*);

fun = f_p;

iflag=*iflagc; nfe=*nfec; kop=*kopc; init=*initc; jflag=*jflagc;

kflag=*kflagc;

h=*hc; savre=*savrec; savae=*savaec;

relerr = *relerrc;

abserr = *abserrc;

/*Проверить входные параметры*/

if (neqn < 1)

goto m10;

if (relerr < 0.0 || abserr < 0.0)

goto m10;

mflag=abs(iflag);

if (mflag == 0 || mflag > 8)

goto m10;

if (mflag != 1)

goto m20;

/*Первый вызов, вычислить машинное эпсилон*/

eps = 1.0;

while (eps+1.0 > 1.0)

eps = eps/2.0;

u26 = 26.0*eps;

goto m50;

/*Ошибки во входной информации*/

m10: iflag = 8;

goto mexit;

/*Проверить возможность продолжения*/

m20: if (t==tout && kflag != 3)

goto m10;

if (mflag != 2)

goto m25;

if (kflag == 3 || init==0)

goto m45;

if (kflag == 4)

goto m40;

if (kflag == 5 && abserr == 0.0)

goto m30;

if (kflag == 6 && relerr <= savre && abserr <= savae)

goto m30;

goto m50;

m25: if (iflag == 3)

goto m45;

if (iflag == 4)

goto m40;

if (iflag == 5 && abserr > 0.0)

goto m45;

m30: goto mexit;

m40: nfe = 0;

if (mflag == 2)

goto m50;

m45: iflag = jflag;

if (kflag == 3)

mflag = abs(iflag);

m50: jflag = iflag;

kflag = 0;

savre = relerr;

savae = abserr;

rer = 2.0*eps+remin;

if (relerr >= rer)

goto m55;

/*Заданная граница относительной погрешности слишком мала*/

relerr = rer;

iflag = 3;

kflag = 3;

goto mexit;

m55: dt = tout-t;

if (mflag==1)

goto m60;

if (init==0)

goto m65;

goto m80;

m60: init = 0;

kop = 0;

a = t;

fun(a,y,yp);

nfe = 1;

if (t != tout)

goto m65;

iflag = 2;

goto mexit;

m65: init = 1;

h = fabs(dt);

toln = 0.0;

for (k=0; k<neqn; k++)

{ tol = relerr*fabs(y[k])+abserr;

if (tol > 0.0)

{ toln=tol;

ypk=fabs(yp[k]);

rab1=h*h*h*h*h;

if (ypk*rab1 > tol)

h=exp(0.2*log(tol/ypk));

}

}

if (toln <= 0.0)

h=0.0;

rab1=u26*fabs(t);

if (fabs(t) < fabs(dt))

rab1=u26*fabs(dt);

if (h < rab1)

h=rab1;

jflag=-2;

if (iflag > 0)

jflag=2;

m80: if (dt < 0.0)

h=-h;

if (fabs(h) >= 2.0*fabs(dt))

kop=kop+1;

if (kop != 100 )

goto m85;

/*Много выходов*/

kop=0;

iflag=7;

goto mexit;

m85: if (fabs(dt) > u26*fabs(t))

goto m95;

/*Близко к выходной точке, проэкстраполировать и вернуться по месту вызова*/

for (k=0; k<neqn; k++)

y[k] = y[k] + dt * yp[k];

a=tout;

fun(a,y,yp);

nfe=nfe+1;

goto m300;

m95: output=False;

scale=2.0/relerr;

ae=scale*abserr;

/*Пошаговое интегрирование*/

m100: hfaild=False;

hmin=u26*fabs(t);

dt=tout-t;

if(fabs(dt) >= 2.0*fabs(h))

goto m200;

if(fabs(dt) > fabs(h))

goto m150;

output=True;

h=dt;

goto m200;

m150: h=0.5*dt;

/*Внутренний одношаговый интегратор*/

m200: if(nfe <= maxnfe)

goto m220;

iflag=4;

kflag=4;

goto mexit;

/*Продолжить приближенное решение на один шаг длины h*/

m220: FEHL(neqn, t, y, f_p, &h, yp, f1, f2, f3, f4, f5, f1);

nfe=nfe+5;

eeoet=0.0;

for (k=0; k<neqn; k++)

{ et=fabs(y[k])+fabs(f1[k])+ae;

if (et > 0.0)

goto m240;

/*Неправильная граница погрешности*/

iflag=5;

goto mexit;

m240: ee=fabs((-2090.0*yp[k]+(21970.0*f3[k]-15048.0*f4[k]))+

(22528.0*f2[k]-27360.0*f5[k]));

rab1=ee/et;

if (eeoet < rab1)

eeoet=rab1;

}

esttol=fabs(h)*eeoet*scale/752400.0;

if (esttol <= 1.0)

goto m260;

/*Неудачный шаг; уменьшить его величину*/

hfaild=True;

output=False;

s=0.1;

if (esttol < 59049.0)

s=0.9/(exp(0.2*log(esttol)));

h=s*h;

if (fabs(h) > hmin)

goto m200;

/*Неудача даже при минимальном шаге*/

iflag=6;

kflag=6;

goto mexit;

/*Успешный шаг*/

m260: t=t+h;

for (k=0; k<neqn; k++)

y[k]=f1[k];

a=t;

fun(a,y,yp);

nfe=nfe+1;

s=5.0;

if (esttol > 1.889568e-4)

s=0.9/(exp(0.2*log(esttol)));

if (hfaild)

{ if (s > 1.0)

s=1.0;

}

rab1=s*fabs(h);

if (rab1 < hmin)

rab1=hmin;

if (h > 0.0)

h=fabs(rab1);

else

h=-fabs(rab1);

if (output)

goto m300;

if (iflag > 0)

goto m100;

/*Интегрирование успешно завершено*/

iflag=-2;

goto mexit;

m300: iflag=2;

mexit: *iflagc=iflag; *nfec=nfe; *kopc=kop; *initc=init; *jflagc=jflag;

*kflagc=kflag;

*hc=h; *savrec=savre; *savaec=savae;

*relerrc = relerr;

*abserrc = abserr;

return;

}

void prifl0 ( int *iflc,

double *rerrc,

double *aerrc )

{

int ifl;

double rerr, aerr;

ifl = *iflc;

rerr = *rerrc;

aerr = *aerrc;

switch (ifl)

{

case 3: break;

case 4: printf ("много fp()\n ifl=%d\n", ifl);

break;

case 5: aerr = 1.0e-9;

break;

case 6: rerr *= 10.0;

printf ("ifl=%d\n rerr=%.12lf aerr=%.12lf\n", ifl, rerr, aerr);

ifl = 2;

break;

case 7: printf ("много вых. точек\n ifl=%d\n", ifl);

ifl = 2;

break;

case 8: printf ("ifl=%d\n", ifl);

break;

}

*iflc = ifl;

*rerrc = rerr;

*aerrc = aerr;

}

#pragma argsused

int main(int argc, char* argv[])

{ char f_Name_u[13],/* Имя файла.dat : t, u(t) */

f_Name_J[13],/* Имя файла.dat : t, J(t) */

f_Name_x[13];/* Имя файла.dat : t, x(t) */

double h, t, tout, ypx [NX], hx=0, f1x [NX], f2x [NX], f3x [NX], f4x [NX], f5x [NX], rerr = 1.0e-12, aerr = 0.0,

savrex = 0, savaex = 0;

int j, Ostanov, inte, iflx, jflx, kflx, nfex, kopx, initx;

D = (double**)calloc( N_M, sizeof(double) );

for ( j=0; j<N_M; j++ )

D[j] = (double*)calloc( N_M, sizeof(double) );

Ma = (double**)calloc( N_M, sizeof(double) );

for ( j=0; j<N_M; j++ )

Ma[j] = (double*)calloc( N_M, sizeof(double) );

/* Задание возмущения: */

printf ( "Возмущение: a1 * sin ( a2 * t )\n" );

printf ( "a1=" ); scanf ( "%lf", &a1 );

printf ( "a2=" ); scanf ( "%lf", &a2 );

/* Открытие файлов: */

printf ( "\nFileName: " ); scanf ( "%s", f_Name_u );

strcpy ( f_Name_x, f_Name_u );

strcpy ( f_Name_J, f_Name_u );

strcat ( f_Name_u, "_u.dat" );

strcat ( f_Name_J, "_J.dat" );

strcat ( f_Name_x, "_x.dat" );

if ( ( ru = fopen ( f_Name_u, "wt" ) ) == NULL )

{

perror ( f_Name_u );

exit (-1);

}

if ( ( rJ = fopen ( f_Name_J, "wt" ) ) == NULL )

{

perror ( f_Name_J );

exit (-1);

}

if ( ( rx = fopen ( f_Name_x, "wt" ) ) == NULL )

{

perror ( f_Name_x );

exit (-1);

}

tau = TBEG;

/* Печать в файл и на экран x0: */

fprintf ( rx, "%lf ", tau );

printf ( "\ntau = %lf ", tau );

printf ( "x = ( " );

for ( j=0; j<NX; j++ )

{

fprintf ( rx, "%lf ", x[j] );

printf ( "%lf ", x[j] );

}

fprintf ( rx, "\n" );

printf ( ")\n" );

/* Начальн. формирование некотор.параметров конечномерн.задачи: */

h = TETA/N; /* шаг(длина перем.)*/

printf ( "h = %lf\n", h );

getchar();

/* Работа регулятора: */

Ostanov = 0;

while ( !Ostanov )

{

/* Программн. упpавл. задачи с перем. структурой: */

puN1NIntU2 ( h );

/* Печ. в файл u(t), x(t) на [tau,tau+h]: */

fprintf ( ru, "%lf %lf\n", tau, u[0] + Uf );

fprintf ( ru, "%lf %lf\n", tau+h, u[0] + Uf );

fprintf ( rJ, "%lf %lf\n", tau, J );

tout = tau;

printf ( "tau = %lf\n", tau );

while ( tout < tau+h-0.000001 )

{

t = tout;

if ( tau + h - tout > 0.1 )

tout += 0.1;

else

tout = tau + h;

printf ( "\nt = %lf", t );

printf ( " tout = %lf", tout );

iflx = 1;

inte = 1;

while (inte)

{

RKF45 ( NX, t, tout, x, f_x, &hx, ypx, f1x, f2x, f3x, f4x, f5x,

&iflx, &nfex, &kopx, &initx, &jflx, &kflx,

&rerr, &aerr, &savrex, &savaex);

if ( iflx==2 )

inte = 0;

else

{

printf ("x\n");

prifl0 (&iflx, &rerr, &aerr);

}

}

fprintf ( rx, "%lf ", tout );

printf ( "x = ( " );

for ( j=0; j<NX; j++ )

{

fprintf ( rx, "%lf ", x[j] );

printf ( "%lf ", x[j] );

}

fprintf ( rx, "\n" );

printf ( ") " );

}

tau += h;

printf ( "\n\n\n tau = %lf\n\n\n", tau );

while (1)

{

printf ( "|(x1-2)^2 + (x2-2)^2 - 1| = %lf\n",

fabs( (x[0]-2.0)*(x[0]-2.0) + (x[1]-2.0)*(x[1]-2.0)-1.0 ) );

printf ( "Остановить процесс ? ( 1 -- да, 0 -- нет )" );

scanf ( "%d", &Ostanov );

if ( ( Ostanov==0 ) || ( Ostanov==1 ) )

break;

}

} /* while( !Ostanov ) */

fcloseall();

for ( j=0; j<N_M; j++ )

free ( D[j] );

free ( D );

for ( j=0; j<N_M; j++ )

free ( Ma[j] );

free ( Ma );

return 0;

}

Размещено на Allbest.ru