1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
92
93
94
95
96
97
98
99
100
101
102
103
104
105
106
107
108
109
110
111
112
113
|
#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() {}
|
