matrix.c

学习c++必备

#include "matrix.h"
#include "math.h"

void init_matrix(Matrix *m)
{
    int i,j;
    for (i=0;i<4;i++)
	for (j=0;j<4;j++)
	    m->index[i][j] = (i==j?1:0);
}

void init_vector(Vector *v)
{
    int i;
    for (i=0;i<3;i++)
	v->index[i] = 0;
    v->index[3] = 1;
}

void copy_vector(Vector *v1, Vector *v2)
{
    int i;
    for (i=0;i<4;i++)
	v1->index[i] = v2->index[i];
}

void copy_matrix(Matrix *m1, Matrix *m2)
{
    int i,j;
    for (i=0;i<4;i++)
	for (j=0;j<4;j++)
	    m1->index[i][j] = m2->index[i][j];
}

void multiply_vector_matrix(Matrix *m, Vector *v)
{
    int i,j;
    Vector t;
    for (i=0;i<4;i++)
    {
	t.index[i] = 0;
	for (j=0;j<4;j++)
	    t.index[i] += m->index[i][j] * v->index[j];
    }
    copy_vector(v, &t);
}

void multiply_matrix_vector(Matrix *m, Vector *v)
{
    int i,j;
    Vector t;
    for (i=0;i<4;i++)
    {
	t.index[i] = 0;
	for (j=0;j<4;j++)
	    t.index[i] += m->index[j][i] * v->index[j];
    }
    copy_vector(v, &t);
}

void multiply_matrix(Matrix *m2, Matrix *m1)
{
    int i,j,k;
    Matrix m;
    for (i=0;i<4;i++)
    {
	for (j=0;j<4;j++)
	{
	    m.index[i][j] = 0;
	    for (k=0;k<4;k++)
		m.index[i][j] += m1->index[i][k]* m2->index[k][j];
	}
    }
    copy_matrix(m2, &m);
}

void rotate_x(double angle, Matrix *m)
{
    Matrix r;
    double c = cos(angle), s = sin(angle);
    init_matrix(&r);
    r.index[1][1] = c;
    r.index[1][2] = s;
    r.index[2][1] = -s;
    r.index[2][2] = c;
    multiply_matrix(m, &r);
}

void rotate_y(double angle, Matrix *m)
{
    Matrix r;
    double c = cos(angle), s = sin(angle);
    init_matrix(&r);
    r.index[0][0] = c;
    r.index[0][2] = -s;
    r.index[2][0] = s;
    r.index[2][2] = c;
    multiply_matrix(m, &r);
}

void rotate_z(double angle, Matrix *m)
{
    Matrix r;
    double c = cos(angle), s = sin(angle);
    init_matrix(&r);
    r.index[0][0] = c;
    r.index[0][1] = s;
    r.index[1][0] = -s;
    r.index[1][1] = c;
    multiply_matrix(m, &r);
}

void mcount() {}