int main(){


	//declare matrix
	int mat1[3][3];
	int mat2[3][3];
	int mat_add[3][3]; //for addition

	int mat_mul[3][3]; //for multiplication
	int mat_trp[3][3]; //for transpose


	printf("enter the first matrix: \n");
	create(mat1);
	
	printf("enter the second matrix: \n");

	create(mat2);
	
	printf("display matrix 1:\n");
	display(mat1);
	
	printf("display matrix 2:\n");
	display(mat2);

	printf("addition of mat1 and mat2: \n");
	
	matadd(mat1,mat2,mat_add);
	display(mat_add);


	printf("multiplication of mat1 and mat2: \n");
	
	matadd(mat1,mat2,mat_mul);
	display(mat_mul);

	printf("transpose of mat1 and mat2: \n");
	
	transpose(mat1,mat_trp);
	display(mat_trp);


	return 0;	
}
void pure_vel_update(void)
{
	int i;
	double temp[3];

	matmul(3, 3, (double *)p_dcm, 3, 1, (double *)p_velo_20ms, (double *)pure_vel);

	//earth rate correction
	cross(omg_dub, pure_v_old, temp);
	matmulint(3,1,(double *)temp, eight_delt, (double *)temp);
	matsub(3,1,(double *)pure_vel, (double *)temp, (double *)pure_vel);
	matadd(3,1,(double *)pure_vel, (double *)pure_ecef_gravity, (double *)pure_vel);

	matadd(3,1,(double *)pure_vel, (double *)pure_v_old, (double *)pure_vel);

	matadd(3,1,(double *)pure_vel, (double *)pure_v_old, (double *)pure_vav);
	matmulint(3,1,(double *)pure_vav, 0.5, (double *)pure_vav);

	for (i = 0; i < 3; i++)
		pure_v_old[i] = pure_vel[i];


}
示例#3
0
文件: mgfas.c 项目: gnovak/bin
void mgfas(double **u, int n, int maxcyc)
{
	double anorm2(double **a, int n);
	void copy(double **aout, double **ain, int n);
	void interp(double **uf, double **uc, int nf);
	void lop(double **out, double **u, int n);
	void matadd(double **a, double **b, double **c, int n);
	void matsub(double **a, double **b, double **c, int n);
	void relax2(double **u, double **rhs, int n);
	void rstrct(double **uc, double **uf, int nc);
	void slvsm2(double **u, double **rhs);
	unsigned int j,jcycle,jj,jm1,jpost,jpre,nf,ng=0,ngrid,nn;
	double **irho[NGMAX+1],**irhs[NGMAX+1],**itau[NGMAX+1],
		**itemp[NGMAX+1],**iu[NGMAX+1];
	double res,trerr;

	nn=n;
	while (nn >>= 1) ng++;
	if (n != 1+(1L << ng)) nrerror("n-1 must be a power of 2 in mgfas.");
	if (ng > NGMAX) nrerror("increase NGMAX in mglin.");
	nn=n/2+1;
	ngrid=ng-1;
	irho[ngrid]=dmatrix(1,nn,1,nn);
	rstrct(irho[ngrid],u,nn);
	while (nn > 3) {
		nn=nn/2+1;
		irho[--ngrid]=dmatrix(1,nn,1,nn);
		rstrct(irho[ngrid],irho[ngrid+1],nn);
	}
	nn=3;
	iu[1]=dmatrix(1,nn,1,nn);
	irhs[1]=dmatrix(1,nn,1,nn);
	itau[1]=dmatrix(1,nn,1,nn);
	itemp[1]=dmatrix(1,nn,1,nn);
	slvsm2(iu[1],irho[1]);
	free_dmatrix(irho[1],1,nn,1,nn);
	ngrid=ng;
	for (j=2;j<=ngrid;j++) {
		nn=2*nn-1;
		iu[j]=dmatrix(1,nn,1,nn);
		irhs[j]=dmatrix(1,nn,1,nn);
		itau[j]=dmatrix(1,nn,1,nn);
		itemp[j]=dmatrix(1,nn,1,nn);
		interp(iu[j],iu[j-1],nn);
		copy(irhs[j],(j != ngrid ? irho[j] : u),nn);
		for (jcycle=1;jcycle<=maxcyc;jcycle++) {
		nf=nn;
			for (jj=j;jj>=2;jj--) {
				for (jpre=1;jpre<=NPRE;jpre++)
					relax2(iu[jj],irhs[jj],nf);
				lop(itemp[jj],iu[jj],nf);
				nf=nf/2+1;
				jm1=jj-1;
				rstrct(itemp[jm1],itemp[jj],nf);
				rstrct(iu[jm1],iu[jj],nf);
				lop(itau[jm1],iu[jm1],nf);
				matsub(itau[jm1],itemp[jm1],itau[jm1],nf);
				if (jj == j)
					trerr=ALPHA*anorm2(itau[jm1],nf);
				rstrct(irhs[jm1],irhs[jj],nf);
				matadd(irhs[jm1],itau[jm1],irhs[jm1],nf);
			}
			slvsm2(iu[1],irhs[1]);
			nf=3;
			for (jj=2;jj<=j;jj++) {
			jm1=jj-1;
			rstrct(itemp[jm1],iu[jj],nf);
			matsub(iu[jm1],itemp[jm1],itemp[jm1],nf);
			nf=2*nf-1;
			interp(itau[jj],itemp[jm1],nf);
			matadd(iu[jj],itau[jj],iu[jj],nf);
			for (jpost=1;jpost<=NPOST;jpost++)
				relax2(iu[jj],irhs[jj],nf);
			}
			lop(itemp[j],iu[j],nf);
			matsub(itemp[j],irhs[j],itemp[j],nf);
			res=anorm2(itemp[j],nf);
			if (res < trerr) break;
		}
	}
	copy(u,iu[ngrid],n);
	for (nn=n,j=ng;j>=1;j--,nn=nn/2+1) {
		free_dmatrix(itemp[j],1,nn,1,nn);
		free_dmatrix(itau[j],1,nn,1,nn);
		free_dmatrix(irhs[j],1,nn,1,nn);
		free_dmatrix(iu[j],1,nn,1,nn);
		if (j != ng && j != 1) free_dmatrix(irho[j],1,nn,1,nn);
	}
}
示例#4
0
void matmul_strassen(double* a, double* b, double* c, int n)
{
    double* tmp1 = (double*) malloc((n*n/4)*sizeof(double));
    double* tmp2 = (double*) malloc((n*n/4)*sizeof(double));
    
    double* a11 = (double*) malloc((n*n/4)*sizeof(double));
    double* a12 = (double*) malloc((n*n/4)*sizeof(double));
    double* a21 = (double*) malloc((n*n/4)*sizeof(double));
    double* a22 = (double*) malloc((n*n/4)*sizeof(double));
    
    double* b11 = (double*) malloc((n*n/4)*sizeof(double));
    double* b12 = (double*) malloc((n*n/4)*sizeof(double));
    double* b21 = (double*) malloc((n*n/4)*sizeof(double));
    double* b22 = (double*) malloc((n*n/4)*sizeof(double));
    
    double* c11 = (double*) malloc((n*n/4)*sizeof(double));
    double* c12 = (double*) malloc((n*n/4)*sizeof(double));
    double* c21 = (double*) malloc((n*n/4)*sizeof(double));
    double* c22 = (double*) malloc((n*n/4)*sizeof(double));
    

    double* m1 = (double*) malloc((n*n/4)*sizeof(double));
    double* m2 = (double*) malloc((n*n/4)*sizeof(double));
    double* m3 = (double*) malloc((n*n/4)*sizeof(double));
    double* m4 = (double*) malloc((n*n/4)*sizeof(double));
    double* m5 = (double*) malloc((n*n/4)*sizeof(double));
    double* m6 = (double*) malloc((n*n/4)*sizeof(double));
    double* m7 = (double*) malloc((n*n/4)*sizeof(double));
	int i, j, k, ii, jj, kk, N;
	double tmp;
    

    // partition A and B
    N = n/2;
    for (i=0; i< N; i++) 
    {

        for (j=0; j< N; j++) 
        {
            a11[i*N +j ] = a[i*n+j];
            b11[i*N +j ] = b[i*n+j];
            
            a12[i*N +j ] = a[i*n+j+N];
            b12[i*N +j ] = b[i*n+j+N];
            
            a21[i*N +j ] = a[(i+N)*n+j];
            b21[i*N +j ] = b[(i+N)*n+j];

            a22[i*N +j ] = a[(i+N)*n+j+N];
            b22[i*N +j ] = b[(i+N)*n+j+N];
        }
    }
    //print(a, n);
    //print(a11, N);
    //print(a12, N);
    //print(a21, N);
    //print(a22, N);
    
    //form m1 = (a11 + a22)(b11 + b22)
    matadd(a11, a22, tmp1, N);
    matadd(b11, b22, tmp2, N);
    matmul(tmp1, tmp2, m1, N);
    
    //form m2 = (a21 + a22)b11
    matadd(a21, a22, tmp1, N);
    matmul(tmp1, b11, m2, N);

    //form m3 = a11(b12 - b22)
    matsub(b12, b22, tmp1, N);
    matmul(a11, tmp1, m3, N);

    //form m4 = a22(b21 - b11)
    matsub(b21, b11, tmp1, N);
    matmul(a22, tmp1, m4, N);
    
    //form m5 = (a11 +a12)b22
    matadd(a11, a12, tmp1, N);
    matmul(tmp1, b22, m5, N);
    
    //form m6 = (a21 -a11)(b11 + b12)
    matsub(a21, a11, tmp1, N);
    matadd(b11, b12, tmp2, N);
    matmul(tmp1, tmp2, m6, N);

    //form m7 = (a12 -a22)(b21 + b22)
    matsub(a12, a22, tmp1, N);
    matadd(b21, b22, tmp2, N);
    matmul(tmp1, tmp2, m7, N);    



    //============================
    //form c11 = m1 + m4 - m5 + m7
    matadd(m1, m4, tmp1, N);
    matsub(tmp1, m5, tmp2, N);
    matadd(tmp2, m7, c11, N);

    //form c12 = m3 + m5
    matadd(m3, m5, c12, N);

    //form c21 = m2 + m4
    matadd(m2, m4, c21, N);

    //fomr c22 = m1 - m2 + m3 + m6
    matsub(m1, m2, tmp1, N);
    matadd(tmp1, m3, tmp2, N);
    matadd(tmp2, m6, c22, N);

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

        for (j=0; j< N; j++) 
        {
            c[i*n+j] = c11[i*N +j ];
            c[i*n+j+N] = c12[i*N +j ];
            c[(i+N)*n+j] = c21[i*N +j ];
            c[(i+N)*n+j+N] = c22[i*N +j ]; 
        }
    }
    free(tmp1);
    free(tmp2);

    free(a11);
    free(a12);
    free(a21);
    free(a22);
    
    free(b11);
    free(b12);
    free(b21);
    free(b22);

    free(c11);
    free(c12);
    free(c21);
    free(c22);

    free(m1);
    free(m2);
    free(m3);
    free(m4);
    free(m5);
    free(m6);
    free(m7);


}