#include <stdlib.h>
#include <math.h>
#include <stdio.h>
#define N 6
#define pqs 2
#define pvs 1
#define ep 1e-5
int brinv(double a[], int n)
{
int *is,*js,i,j,k,l,u,v;
double d,p;
is=malloc(n*sizeof(int));
js=malloc(n*sizeof(int));
for (k=0; k<=n-1; k++)
{ d=0.0;
for (i=k; i<=n-1; i++)
for (j=k; j<=n-1; j++)
{ l=i*n+j; p=fabs(a[l]);
if (p>d) { d=p; is[k]=i; js[k]=j;}
}
if (d+1.0==1.0)
{
free(is); free(js); printf("err**not inv\n");
return(0);
}
if (is[k]!=k)
for (j=0; j<=n-1; j++)
{
u=k*n+j; v=is[k]*n+j;
p=a[u]; a[u]=a[v]; a[v]=p;
}
if (js[k]!=k)
for (i=0; i<=n-1; i++)
{
u=i*n+k; v=i*n+js[k];
p=a[u]; a[u]=a[v]; a[v]=p;
}
l=k*n+k;
a[l]=1.0/a[l];
for (j=0; j<=n-1; j++)
if (j!=k)
{ u=k*n+j; a[u]=a[u]*a[l];}
for (i=0; i<=n-1; i++)
if (i!=k)
for (j=0; j<=n-1; j++)
if (j!=k)
{ u=i*n+j;
a[u]=a[u]-a[i*n+k]*a[k*n+j];
}
for (i=0; i<=n-1; i++)
if (i!=k)
{ u=i*n+k; a[u]=-a[u]*a[l];}
}
for (k=n-1; k>=0; k--)
{
if (js[k]!=k)
for (j=0; j<=n-1; j++)
{ u=k*n+j; v=js[k]*n+j;
p=a[u]; a[u]=a[v]; a[v]=p;
}
if (is[k]!=k)
for (i=0; i<=n-1; i++)
{ u=i*n+k; v=i*n+is[k];
p=a[u]; a[u]=a[v]; a[v]=p;
}
}
free(is); free(js);
return(1);
}
void matrixmul(double g[N][N],double h[N],double xx[N])
{ int i,j;
xx[0]=0;xx[1]=0;xx[2]=0;xx[3]=0;xx[4]=0;xx[5]=0;
for(i=0;i<=N-1;i++)
for(j=0;j<=N-1;j++)
xx[i]+=g[i][j]*h[j];
}
void funfe(double fe[N],double x[N])
{int i;
for(i=0;i<N;i++)
fe[i]+=x[i];
/*fe[0]=fe[0]+x[0]; fe[1]=fe[1]+x[1];
fe[2]=fe[2]+x[2]; fe[3]=fe[3]+x[3];
fe[4]=fe[4]+x[4]; fe[5]=fe[5]+x[5];*/
}
void matrixpq(double pq[N],double G[4][4],double B[4][4],double e[4],double f[4])
{int i,j;
double pp1,pp2,qq1,qq2;
double p[pqs+pvs]={-0.3,-0.55,0.5};
double q[pqs]={-0.18,-0.13};
double u=1.1;
for(i=0;i<=pqs;i++)
{ pp1=0;pp2=0;qq1=0;qq2=0;
for(j=0;j<4;j++)
{ pp1=pp1+G[i][j]*e[j]-B[i][j]*f[j];
pp2=pp2+G[i][j]*f[j]+B[i][j]*e[j];
if (i<pqs)
{qq1=qq1+G[i][j]*e[j]-B[i][j]*f[j];
qq2=qq2+G[i][j]*f[j]+B[i][j]*e[j];
}
}
pq[2*i]=p[i]-(pp1*e[i]+pp2*f[i]);
if (i<pqs)
pq[2*i+1]=q[i]-(qq1*f[i]-qq2*e[i]);
else
pq[2*pqs+pvs]=u*u-(e[2]*e[2]+f[2]*f[2]);
}
}
void matrixef(double e[4],double f[4],double fe[N])
{
e[0]=fe[1]; f[0]=fe[0];
e[1]=fe[3]; f[1]=fe[2];
e[2]=fe[5]; f[2]=fe[4];
e[3]=1.05; f[3]=0;
}
void matrixjcb(double G[4][4],double B[4][4],double e[4],double f[4],double jcbjz[N][N])
{int i,j,k,m,n;
double bb;double aa;
double H[3][3],M[3][3],J[2][3],L[2][3];
for(i=0;i<3;i++)
{ for(j=0;j<3;j++)
{ if (i!=j)
{H[i][j]=G[i][j]*f[i]-B[i][j]*e[i];
M[i][j]=G[i][j]*e[i]+B[i][j]*f[i];
if (i!=2)
{J[i][j]=-M[i][j];
L[i][j]=H[i][j];
}
}
else
{ bb=0;aa=0;
for(k=0;k<4;k++)
{bb=bb+G[i][k]*f[k]+B[i][k]*e[k];
aa=aa+G[i][k]*e[k]-B[i][j]*f[k];
}
H[i][i]=G[i][i]*f[i]-B[i][i]*e[i]+bb;
M[i][i]=G[i][i]*e[i]+B[i][i]*f[i]+aa;
if (i!=2)
{J[i][i]=(-G[i][i])*e[i]-B[i][i]*f[i]+aa;
L[i][i]=(-B[i][i])*e[i]+G[i][i]*f[i]-bb;
}
}
}
}
n=0;
for(i=0;i<5;i+=2)
{ m=0;
for(j=0;j<5;j+=2)
{ jcbjz[i][j]=H[n][m];
jcbjz[i][j+1]=M[n][m];
if(n<2)
{jcbjz[i+1][j]=J[n][m];
jcbjz[i+1][j+1]=L[n][m];
}
else
{jcbjz[5][0]=0; jcbjz[5][1]=0; jcbjz[5][2]=0;
jcbjz[5][3]=0; jcbjz[5][4]=2*f[3]; jcbjz[5][5]=2*e[3];
}
m++;
}
n++;
}
}
void voltage(double U[3],double e[4],double f[4])
{int i;
for(i=0;i<3;i++)
U[i]=sqrt(e[i]*e[i]+f[i]*f[i]);
}
double mulRe(double b1,double b2,double b3,double b4)
{double ar;
ar=b1*b3-b2*b4;
return(ar);
}
double mulIm(double b1,double b2,double b3,double b4)
{double ai;
ai=b2*b3+b1*b4;
return(ai);
}
double divRe(double a1,double a2,double a3,double a4)
{double br;
br=(a1*a3+a2*a4)/(a3*a3+a4*a4);
return(br);
}
double divIm(double a1,double a2,double a3,double a4)
{double bi;
bi=(a2*a3-a1*a4)/(a3*a3+a4*a4);
return(bi);
}
void zhilupower(double x1,double x2,double y01,double e1,double f1,double G01,double B01)
{double uy[2],duy[2],uuy[2];
uy[0]=mulRe(x1,x2,0,y01);
uy[1]=mulIm(x1,x2,0,y01);
duy[0]=divRe(x1-e1,x2-f1,G01,B01);
duy[1]=divIm(x1-e1,x2-f1,G01,B01);
uuy[0]=mulRe(x1,x2,uy[0]+duy[0],-uy[1]-duy[1]);
uuy[1]=mulIm(x1,x2,uy[0]+duy[0],-uy[1]-duy[1]);
printf("%6.5f+j%6.5f\n",uuy[0],uuy[1]);
}
int main()
{
int i,j,h,num;
double yur=0,yui=0,S4[2];
double fe[N]={0,1,0,1,0,1};
double jcbjz[N][N],b[N][N],pq[N],x[N],e[4],f[4],U[3];
double G[4][4]={{1.0421,-0.5882,0,-0.4539},
{-0.5822,1.069,0,-0.4808},
{0,0,0,0},
{-0.4539,-0.4808,0,0.9346}};
double B[4][4]={{-8.2429,2.3529,3.6666,1.8911},
{2.3529,-4.7274,0,2.4038},
{3.6666,0,-3.3333,0},
{1.8911,2.4038,0,-4.2616}};
double y[4][4]={{0,0.01528,0,0.0192},
{0.01528,0,0,0.01413},
{0,0,0,0},
{0.0192,0.01413,0,0}};
num=0;
do{
h=0;
matrixef(e,f,fe);
matrixjcb(G,B,e,f,jcbjz);
matrixpq(pq,G,B,e,f);
for (i=0; i<=N-1; i++)
for (j=0; j<=N-1; j++)
b[i][j]=jcbjz[i][j];
i=0;
i=brinv(b,N);
if (i!=0)
{ num++;
printf("\n%d",num);
printf("\n MAX pq :");
for(i=0;i<6;i++)
printf("%6.5e ",pq[i]);
matrixmul(b,pq,x);
for(i=0;i<=N-1;i++)
if (fabs(pq[i])<ep)
h++;
funfe(fe,x);
printf("\nMAT X-fe IS:");
for(i=0;i<=N-1;i++)
printf("%6.5f ",x[i]);
printf("\nMAT fe IS:");
for(i=0;i<=N-1;i++)
printf("%6.5f ",fe[i]);
}
}
while(h!=6);
printf("\nnum is: %d\n",num);
voltage(U,e,f);
for(i=0;i<3;i++)
printf("i=%d,Ui=%6.5f,Ai=%6.5f\n",i+1,U[i],atan(f[i]/e[i])*180/3.14159);
for(j=0;j<4;j++)
{ yur+=mulRe(G[3][j],B[3][j],e[j],f[j]);
yui+=mulIm(G[3][j],B[3][j],e[j],f[j]);
}
S4[0]=mulRe(e[3],f[3],yur,-yui);
S4[1]=mulIm(e[3],f[3],yur,-yui);
printf("S4Re+jS4Im=%6.5f+j%6.5f\n",S4[0],S4[1]);
printf("S12=");
zhilupower(e[0],f[0],y[0][1],e[1],f[1],0.1,0.4);
printf("S21=");
zhilupower(e[1],f[1],y[1][0],e[0],f[0],0.1,0.4);
printf("S13=");
zhilupower(e[0]/0.9091,f[0]/0.9091,y[0][2],e[2],f[2],0,0.3);
printf("S31=");
zhilupower(e[2],f[2],y[2][0],e[0]/0.9091,f[0]/0.9091,0,0.3);
printf("S14=");
zhilupower(e[0],f[0],y[0][3],e[3],f[3],0.12,0.5);
printf("S41=");
zhilupower(e[3],f[3],y[3][0],e[0],f[0],0.12,0.5);
printf("S24=");
zhilupower(e[1],f[1],y[1][3],e[3],f[3],0.08,0.4);
printf("S42=");
zhilupower(e[3],f[3],y[3][1],e[1],f[1],0.08,0.4);
}