QR分解的C语言实现

#include <stdio.h>
#include <math.h>

int n = 3;

double calculate_mode(int r, double x[r][1])
{
    double mode2 = 0;
    for (int i = 0; i < r; i++)
    {
        mode2 = mode2 + x[i][0]*x[i][0];
    }
     double mode = sqrt(mode2);
    return mode;
}

void print_matrix(double matrix[n][n])
{
    for(int i = 0;i < n;i ++)
    {
        for(int j = 0;j < n;j ++)
        {
            printf("%lf\t",matrix[i][j]);
        }    
        printf("\n");
    }
    printf("----------------------------------------------");
    printf("\n");
}

int main()
{
    double A[3][3] = {{4, -1, -1}, {16, -2, -3}, {16, -3, -1}};//A[n][n]
    double Ao[n][n];
    for (int i = 0; i < n; i++)//Ao = A
    {
        for (int j = 0; j < n; j++)
        {
            Ao[i][j] = A[i][j];
        }        
    }
    double Q[n][n];
    for (int i = 0; i < n; i++)//Q = I
    {
        for (int j = 0; j < n; j++)
        {
            if (j == i)
            {
                Q[i][j] = 1;
            }
            else
            {
                Q[i][j] = 0;
            }
        }        
    }
    double R[n][n];
    
    for (int i = 0; i < n-1; i++)
    {
        int r = n - i;
        double x[r][1], y[r][1], vo[r][1], v[r][1], H[r][r];
        
        for (int j = 0; j < r; j++)//x
        {
            x[j][0] = A[i+j][i];
        }
        
        double Ho[n][n];
        for (int j = 0; j < n; j++)//Ho = I
        {
            for (int k = 0; k < n; k++)
            {
                if (k == j)
                {
                    Ho[j][k] = 1;
                }
                else
                {
                    Ho[j][k] = 0;
                }
            }        
        }
        
        for (int j = 0; j < r; j++)//vo
        {
            if (j == 0)
            {
                y[j][0] = calculate_mode(r, x);                
            }
            else
            {
                y[j][0] = 0;
            }
            vo[j][0] = x[j][0] - y[j][0];
        }
        double mode_vo = calculate_mode(r, vo);
        for (int j = 0; j < r; j++)//v
        {
            v[j][0] = vo[j][0]/mode_vo;
        }
        
        for (int j = 0; j < r; j++)//H = I - 2vv.'
        {
            for (int k = 0; k < r; k++)
            {
                if (k == j)
                {
                    H[j][k] = 1 - 2*v[j][0]*v[j][0];
                }
                else
                {
                    H[j][k] = -2*v[j][0]*v[k][0];
                }
            }            
        }
        
        for (int j = i; j < n; j++)//Ho = (I H)
        {
            for (int k = i; k < n; k++)
            {
                Ho[j][k] = H[j-i][k-i];
            }            
        }
        
        double result[n][n];
        for (int j = 0; j < n; j++)//result = 0
        {
            for (int k = 0; k < n; k++)
            {           
                result[j][k] = 0;       
            } 
        }
        for (int m = 0; m < n; m++)//result = Q*Ho
        {
            for (int j = 0; j < n; j++)
            {
                for (int k = 0; k < n; k++)
                {
                    result[m][j] = result[m][j] + Q[m][k]*Ho[k][j];
                }   
            }     
        }

        for (int j = 0; j < n; j++)//Q = result
        {
            for (int k = 0; k < n; k++)
            {
                Q[j][k] = result[j][k];
            }   
        }

        for (int j = 0; j < n; j++)//R = 0
        {
            for (int k = 0; k < n; k++)
            {           
                R[j][k] = 0;       
            } 
        }
        for (int m = 0; m < n; m++)//R = Ho*A
        {
            for (int j = 0; j < n; j++)
            {
                for (int k = 0; k < n; k++)
                {
                    R[m][j] = R[m][j] + Ho[m][k]*A[k][j];
                }   
            }     
        }

        for (int j = 0; j < n; j++)//A = R
        {
            for (int k = 0; k < n; k++)
            {
                A[j][k] = R[j][k];
            }   
        }

        printf("Try %d\n", i+1);
        printf("H%d = \n", i+1);
        print_matrix(Ho); 
        printf("A <-- H%dA = \n", i+1);
        print_matrix(A);      

    }
    
    printf("=====================RESULT=====================\n");
    printf("Q = \n");
    print_matrix(Q);

    printf("R = \n");
    print_matrix(R);

    return 0;
}