-
Notifications
You must be signed in to change notification settings - Fork 0
/
rmath.c
148 lines (133 loc) · 4.54 KB
/
rmath.c
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
114
115
116
117
118
119
120
121
122
123
124
125
126
127
128
129
130
131
132
133
134
135
136
137
138
139
140
141
142
143
144
145
146
147
148
#include"rmath.h"
#include<malloc.h>
#include<math.h>
struct Matrix * create_matrix(const unsigned int height, const unsigned int width, const double num)
{
struct Matrix * matrix = (struct Matrix*)malloc(sizeof(struct Matrix));
matrix->height = height;
matrix->width = width;
matrix->matrix = (double**)malloc(sizeof(double)*height);
for(unsigned int i = 0; i < height; i += 1)
matrix->matrix[i] = (double*)malloc(sizeof(double)*width);
for(unsigned int i = 0; i < height; i += 1)
for(unsigned int j = 0; j < width; j += 1)
matrix->matrix[i][j] = num;
return matrix;
}
void free_matrix(struct Matrix * matrix)
{
if(matrix == NULL || matrix->matrix == NULL)
return;
for(unsigned int i = 0; i < matrix->height; i += 1)
{
free(matrix->matrix[i]);
matrix->matrix[i] = NULL;
}
free(matrix->matrix);
matrix->matrix = NULL;
free(matrix);
matrix = NULL;
return;
}
struct Matrix * matrix_mult(const struct Matrix * matrix1, const struct Matrix * matrix2)
{
if(matrix2->height != matrix1->width)
return NULL;
if(matrix1 == NULL || matrix2 == NULL || matrix1->matrix == NULL || matrix2->matrix == NULL)
return NULL;
struct Matrix * resMatrix = create_matrix(matrix1->height, matrix2->width, 0.0);
for(unsigned int i = 0; i < matrix1->height; i += 1)
for(unsigned int j = 0; j < matrix2->width; j += 1)
for(unsigned int k = 0; k < matrix2->height; k += 1)
resMatrix->matrix[i][j] += matrix1->matrix[i][k]*matrix2->matrix[k][j];
return resMatrix;
}
struct Matrix * matrix_mult_num(struct Matrix * matrix, const double num)
{
if(matrix == NULL || matrix->matrix == NULL)
return NULL;
for(unsigned int i = 0; i < matrix->height; i += 1)
for(unsigned int j = 0; j < matrix->width; j += 1)
matrix->matrix[i][j] *= num;
return matrix;
}
struct Matrix * matrix_add_num(struct Matrix * matrix, const double num)
{
if(matrix == NULL || matrix->matrix == NULL)
return NULL;
for(unsigned int i = 0; i < matrix->height; i += 1)
for(unsigned int j = 0; j < matrix->width; j += 1)
matrix->matrix[i][j] += num;
return matrix;
}
struct Matrix * matrix_add(const struct Matrix * matrix1, const struct Matrix * matrix2)
{
if(matrix1 == NULL || matrix2 == NULL || matrix1->matrix == NULL || matrix2->matrix == NULL)
return NULL;
if(matrix1->height != matrix2->height || matrix2->width != matrix2->width)
return NULL;
struct Matrix * resMatrix = create_matrix(matrix1->height, matrix2->width, 0.0);
for(unsigned int i = 0; i < resMatrix->height; i += 1)
for(unsigned int j = 0; j < resMatrix->width; j += 1)
resMatrix->matrix[i][j] += matrix1->matrix[i][j] + matrix2->matrix[i][j];
return resMatrix;
}
struct Matrix * matrix_Tr(struct Matrix * matrix)
{
if(matrix == NULL || matrix->matrix == NULL)
return NULL;
struct Matrix * resMatrix = create_matrix(matrix->width, matrix->height, 0.0);
for(unsigned int i = 0; i < resMatrix->height; i += 1)
for(unsigned int j = 0; j < resMatrix->width; j += 1)
resMatrix->matrix[i][j] = matrix->matrix[j][i];
free_matrix(matrix);
return resMatrix;
}
struct Matrix * neg_matrix(struct Matrix * matrix)
{
if(matrix == NULL || matrix->matrix == NULL)
return NULL;
for(unsigned int i = 0; i < matrix->height; i += 1)
for(unsigned int j = 0; j < matrix->width; j += 1)
matrix->matrix[i][j] *= -1;
return matrix;
}
struct Matrix * mult_x(struct Matrix * matrix, const double theta)
{
register struct Matrix * Rx = create_matrix(3, 3, 0.0);
Rx->matrix[0][0] = 1.0;
Rx->matrix[1][1] = cos(theta);
Rx->matrix[1][2] = -sin(theta);
Rx->matrix[2][1] = sin(theta);
Rx->matrix[2][2] = cos(theta);
struct Matrix * Res = matrix_mult(Rx, matrix);
free_matrix(Rx);
free_matrix(matrix);
return Res;
}
struct Matrix * mult_y(struct Matrix * matrix, const double theta)
{
register struct Matrix * Ry = create_matrix(3, 3, 0.0);
Ry->matrix[0][0] = cos(theta);
Ry->matrix[0][2] = sin(theta);
Ry->matrix[1][1] = 1.0;
Ry->matrix[2][0] = -sin(theta);
Ry->matrix[2][2] = cos(theta);
struct Matrix * Res = matrix_mult(Ry, matrix);
free_matrix(Ry);
free_matrix(matrix);
return Res;
}
struct Matrix * mult_z(struct Matrix * matrix, const double theta)
{
register struct Matrix * Rz = create_matrix(3, 3, 0.0);
Rz->matrix[0][0] = cos(theta);
Rz->matrix[0][1] = -sin(theta);
Rz->matrix[1][0] = sin(theta);
Rz->matrix[1][1] = cos(theta);
Rz->matrix[2][2] = 1.0;
struct Matrix * Res = matrix_mult(Rz, matrix);
free_matrix(Rz);
free_matrix(matrix);
return Res;
}