|
0
|
1 /*
|
|
|
2 * DMLib
|
|
|
3 * -- Vector and matrix functions
|
|
|
4 * Programmed and designed by Matti 'ccr' Hamalainen
|
|
|
5 * (C) Copyright 2011 Tecnic Software productions (TNSP)
|
|
|
6 */
|
|
|
7 #include "dmvecmat.h"
|
|
|
8 #include <math.h>
|
|
|
9
|
|
|
10
|
|
|
11 void dm_vector_copy(DMVector *vd, DMVector *vs)
|
|
|
12 {
|
|
|
13 memcpy(vd, vs, sizeof(DMVector));
|
|
|
14 }
|
|
|
15
|
|
|
16
|
|
|
17 /* Basic vector operations
|
|
|
18 */
|
|
|
19 void dm_vector_add(DMVector *vr, const DMVector *v2)
|
|
|
20 {
|
|
|
21 vr->x += v2->x;
|
|
|
22 vr->y += v2->y;
|
|
|
23 vr->z += v2->z;
|
|
|
24 }
|
|
|
25
|
|
|
26
|
|
|
27 void dm_vector_add_r(DMVector *vr, const DMVector *v1, const DMVector *v2)
|
|
|
28 {
|
|
|
29 vr->x = v1->x + v2->x;
|
|
|
30 vr->y = v1->y + v2->y;
|
|
|
31 vr->z = v1->z + v2->z;
|
|
|
32 }
|
|
|
33
|
|
|
34
|
|
|
35 void dm_vector_sub(DMVector *vr, const DMVector *v2)
|
|
|
36 {
|
|
|
37 vr->x -= v2->x;
|
|
|
38 vr->y -= v2->y;
|
|
|
39 vr->z -= v2->z;
|
|
|
40 }
|
|
|
41
|
|
|
42
|
|
|
43 void dm_vector_sub_r(DMVector *vr, const DMVector *v1, const DMVector *v2)
|
|
|
44 {
|
|
|
45 vr->x = v1->x - v2->x;
|
|
|
46 vr->y = v1->y - v2->y;
|
|
|
47 vr->z = v1->z - v2->z;
|
|
|
48 }
|
|
|
49
|
|
|
50
|
|
|
51 /* Return vector length
|
|
|
52 */
|
|
|
53 DMFloat dm_vector_length(const DMVector *vs)
|
|
|
54 {
|
|
|
55 return sqrt((vs->x * vs->x) + (vs->y * vs->y) + (vs->z * vs->z));
|
|
|
56 }
|
|
|
57
|
|
|
58
|
|
|
59 /* Normalize vector
|
|
|
60 */
|
|
|
61 void dm_vector_normalize(DMVector *vec)
|
|
|
62 {
|
|
|
63 DMFloat l = dm_vector_length(vec);
|
|
|
64
|
|
|
65 if (l > 0.0f)
|
|
|
66 {
|
|
|
67 l = 1.0f / l;
|
|
|
68 vec->x *= l;
|
|
|
69 vec->y *= l;
|
|
|
70 vec->z *= l;
|
|
|
71 }
|
|
|
72 }
|
|
|
73
|
|
|
74
|
|
|
75 /* Scale given vector
|
|
|
76 */
|
|
|
77 void dm_vector_scale(DMVector * vec, const DMFloat k)
|
|
|
78 {
|
|
|
79 vec->x *= k;
|
|
|
80 vec->y *= k;
|
|
|
81 vec->z *= k;
|
|
|
82 }
|
|
|
83
|
|
|
84
|
|
|
85 /* Returns dot-product of two given vectors
|
|
|
86 */
|
|
|
87 DMFloat dm_vector_dot(const DMVector *v1, const DMVector *v2)
|
|
|
88 {
|
|
|
89 return (v1->x * v2->x) + (v1->y * v2->y) + (v1->z * v2->z);
|
|
|
90 }
|
|
|
91
|
|
|
92
|
|
|
93 /* Returns cross-product of two given vectors
|
|
|
94 */
|
|
|
95 void dm_vector_cross(DMVector *vr, const DMVector *v1, const DMVector *v2)
|
|
|
96 {
|
|
|
97 vr->x = (v1->y * v2->z) - (v1->z * v2->y);
|
|
|
98 vr->y = (v1->z * v2->x) - (v1->x * v2->z);
|
|
|
99 vr->z = (v1->x * v2->y) - (v1->y * v2->x);
|
|
|
100 }
|
|
|
101
|
|
|
102
|
|
|
103 /* Multiply given vector with a matrix
|
|
|
104 */
|
|
|
105 void dm_vector_mul_by_mat(DMVector *vd, const DMVector *vs, const DMMatrix *mat)
|
|
|
106 {
|
|
|
107 vd->x = (vs->x * mat->m[0][0]) + (vs->y * mat->m[1][0]) + (vs->z * mat->m[2][0]);
|
|
|
108 vd->y = (vs->x * mat->m[0][1]) + (vs->y * mat->m[1][1]) + (vs->z * mat->m[2][1]);
|
|
|
109 vd->z = (vs->x * mat->m[0][2]) + (vs->y * mat->m[1][2]) + (vs->z * mat->m[2][2]);
|
|
|
110 }
|
|
|
111
|
|
|
112
|
|
|
113 /* Multiply list of given vectors with given matrix.
|
|
|
114 */
|
|
|
115 void dm_vector_mul_by_mat_n(DMVector *list, const int nlist, const DMMatrix *mat)
|
|
|
116 {
|
|
|
117 int i;
|
|
|
118
|
|
|
119 for (i = 0; i < nlist; i++)
|
|
|
120 {
|
|
|
121 DMVector q;
|
|
|
122 memcpy(&q, &list[i], sizeof(DMVector));
|
|
|
123
|
|
|
124 list[i].x = (q.x * mat->m[0][0]) + (q.y * mat->m[1][0]) + (q.z * mat->m[2][0]);
|
|
|
125 list[i].y = (q.x * mat->m[0][1]) + (q.y * mat->m[1][1]) + (q.z * mat->m[2][1]);
|
|
|
126 list[i].z = (q.x * mat->m[0][2]) + (q.y * mat->m[1][2]) + (q.z * mat->m[2][2]);
|
|
|
127 }
|
|
|
128 }
|
|
|
129
|
|
|
130
|
|
|
131 /* Set matrix to unit-matrix
|
|
|
132 */
|
|
|
133 void dm_matrix_unit(DMMatrix *mat)
|
|
|
134 {
|
|
|
135 memset(mat, 0, sizeof(DMMatrix));
|
|
|
136 mat->m[0][0] = 1.0f;
|
|
|
137 mat->m[1][1] = 1.0f;
|
|
|
138 mat->m[2][2] = 1.0f;
|
|
|
139 }
|
|
|
140
|
|
|
141
|
|
|
142 /* Transpose the matrix mat2 to mat1
|
|
|
143 */
|
|
|
144 void dm_matrix_transpose(DMMatrix *mat1, const DMMatrix *mat2)
|
|
|
145 {
|
|
|
146 int i, j;
|
|
|
147
|
|
|
148 for (i = 0; i < DM_MATRIX_SIZE; i++)
|
|
|
149 for (j = 0; j < DM_MATRIX_SIZE; j++)
|
|
|
150 mat1->m[i][j] = mat2->m[j][i];
|
|
|
151 }
|
|
|
152
|
|
|
153
|
|
|
154 /* Multiply matrices mat1 and mat2, putting result into mat1
|
|
|
155 */
|
|
|
156 void dm_matrix_mul(DMMatrix *mat1, const DMMatrix *mat2)
|
|
|
157 {
|
|
|
158 int i, j;
|
|
|
159 DMMatrix tmpM;
|
|
|
160
|
|
|
161 for (i = 0; i < DM_MATRIX_SIZE; i++)
|
|
|
162 for (j = 0; j < DM_MATRIX_SIZE; j++)
|
|
|
163 tmpM.m[i][j] =
|
|
|
164 (mat1->m[i][0] * mat2->m[0][j]) +
|
|
|
165 (mat1->m[i][1] * mat2->m[1][j]) +
|
|
|
166 (mat1->m[i][2] * mat2->m[2][j]);
|
|
|
167
|
|
|
168 memcpy(mat1, &tmpM, sizeof(DMMatrix));
|
|
|
169 }
|
|
|
170
|
|
|
171
|
|
|
172 /* Multiply given list of matrices (size of nMatrices units) with given matrix.
|
|
|
173 */
|
|
|
174 void dm_matrix_mul_n(DMMatrix * list, const int nlist, const DMMatrix *mat)
|
|
|
175 {
|
|
|
176 int i;
|
|
|
177 for (i = 0; i < nlist; i++)
|
|
|
178 dm_matrix_mul(&list[i], mat);
|
|
|
179 }
|
|
|
180
|
|
|
181
|
|
|
182 /* Optimized rotation matrix creation
|
|
|
183 */
|
|
|
184 void dm_matrix_rot(DMMatrix *mat,
|
|
|
185 const DMFloat sx, const DMFloat sy, const DMFloat sz,
|
|
|
186 const DMFloat cx, const DMFloat cy, const DMFloat cz)
|
|
|
187 {
|
|
|
188 const DMFloat
|
|
|
189 q = cx * sz,
|
|
|
190 l = cx * cz,
|
|
|
191 i = sx * sz,
|
|
|
192 j = sx * cz;
|
|
|
193
|
|
|
194 mat->m[0][0] = cy * cz;
|
|
|
195 mat->m[0][1] = cy * sz;
|
|
|
196 mat->m[0][2] = -sy;
|
|
|
197
|
|
|
198
|
|
|
199 mat->m[1][0] = (sy * j) - q;
|
|
|
200 mat->m[1][1] = (sy * i) + l;
|
|
|
201 mat->m[1][2] = sx * cy;
|
|
|
202
|
|
|
203
|
|
|
204 mat->m[2][0] = (sy * l) + i;
|
|
|
205 mat->m[2][1] = (sy * q) - j;
|
|
|
206 mat->m[2][2] = cx * cy;
|
|
|
207 }
|
|
|
208
|
|
|
209
|
|
|
210 /* Make rotation matrix from given angles (radians)
|
|
|
211 */
|
|
|
212 void dm_matrix_rot_a(DMMatrix *mat, const DMFloat ax, const DMFloat ay, const DMFloat az)
|
|
|
213 {
|
|
|
214 dm_matrix_rot(mat, sin(ax), sin(ay), sin(az), cos(ax), cos(ay), cos(az));
|
|
|
215 }
|
|
|
216
|
|
|
217
|
