1 /* -*- c -*- */
2
3 /*
4 * vector.c
5 *
6 * metapixel
7 *
8 * Copyright (C) 1997-1999 Mark Probst
9 *
10 * This program is free software; you can redistribute it and/or
11 * modify it under the terms of the GNU General Public License
12 * as published by the Free Software Foundation; either version 2
13 * of the License, or (at your option) any later version.
14 *
15 * This program is distributed in the hope that it will be useful,
16 * but WITHOUT ANY WARRANTY; without even the implied warranty of
17 * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
18 * GNU General Public License for more details.
19 *
20 * You should have received a copy of the GNU General Public License
21 * along with this program; if not, write to the Free Software
22 * Foundation, Inc., 675 Mass Ave, Cambridge, MA 02139, USA.
23 */
24
25 #include <math.h>
26
27 #include "vector.h"
28
29 Vector2D
MakeVector2D(double x,double y)30 MakeVector2D (double x, double y)
31 {
32 Vector2D vec = { x, y };
33
34 return vec;
35 }
36
37 Vector2D*
InitVector2D(Vector2D * vec,double x,double y)38 InitVector2D (Vector2D *vec, double x, double y)
39 {
40 vec->x = x;
41 vec->y = y;
42
43 return vec;
44 }
45
46 Vector2D*
CopyVector2D(Vector2D * dest,const Vector2D * src)47 CopyVector2D (Vector2D *dest, const Vector2D *src)
48 {
49 dest->x = src->x;
50 dest->y = src->y;
51
52 return dest;
53 }
54
55 double
Abs2D(const Vector2D * v)56 Abs2D (const Vector2D *v)
57 {
58 return sqrt(v->x * v->x + v->y * v->y);
59 }
60
61 Vector2D*
AddVectors2D(Vector2D * dest,const Vector2D * vec1,const Vector2D * vec2)62 AddVectors2D (Vector2D *dest, const Vector2D *vec1, const Vector2D *vec2)
63 {
64 dest->x = vec1->x + vec2->x;
65 dest->y = vec1->y + vec2->y;
66
67 return dest;
68 }
69
70 Vector2D*
SubVectors2D(Vector2D * dest,const Vector2D * vec1,const Vector2D * vec2)71 SubVectors2D (Vector2D *dest, const Vector2D *vec1, const Vector2D *vec2)
72 {
73 dest->x = vec1->x - vec2->x;
74 dest->y = vec1->y - vec2->y;
75
76 return dest;
77 }
78
79 Vector2D*
MultScalar2D(Vector2D * dest,double d,const Vector2D * vec)80 MultScalar2D (Vector2D *dest, double d, const Vector2D *vec)
81 {
82 dest->x = d * vec->x;
83 dest->y = d * vec->y;
84
85 return dest;
86 }
87
88 Vector2D*
MultVectors2D(Vector2D * dest,const Vector2D * vec1,const Vector2D * vec2)89 MultVectors2D (Vector2D *dest, const Vector2D *vec1, const Vector2D *vec2)
90 {
91 dest->x = vec1->x * vec2->x;
92 dest->y = vec1->y * vec2->y;
93
94 return dest;
95 }
96
97 Vector2D*
Unity2D(Vector2D * dest,Vector2D * vec)98 Unity2D (Vector2D *dest, Vector2D *vec)
99 {
100 return MultScalar2D(dest, 1 / Abs2D(vec), vec);
101 }
102
103 Vector2D*
Rectangular2DToPolar(Vector2D * dest,const Vector2D * src)104 Rectangular2DToPolar (Vector2D *dest, const Vector2D *src)
105 {
106 dest->y = sqrt(src->x * src->x + src->y * src->y);
107 if (src->y == 0.0)
108 {
109 if (src->x > 0)
110 dest->x = 0.0;
111 else
112 dest->x = M_PI;
113 }
114 else
115 {
116 dest->x = atan(src->y / src->x);
117 if (src->x < 0.0)
118 dest->x = M_PI + dest->x;
119 }
120
121 dest->x = fmod(dest->x, 2 * M_PI);
122
123 return dest;
124 }
125
126 Vector2D*
Polar2DToRectangular(Vector2D * dest,const Vector2D * src)127 Polar2DToRectangular (Vector2D *dest, const Vector2D *src)
128 {
129 dest->x = cos(src->x) * src->y;
130 dest->y = sin(src->x) * src->y;
131
132 return dest;
133 }
134
135 Vector3D
MakeVector3D(double x,double y,double z)136 MakeVector3D (double x, double y, double z)
137 {
138 Vector3D vec = { x, y, z };
139
140 return vec;
141 }
142
143 Vector3D*
InitVector3D(Vector3D * vec,double x,double y,double z)144 InitVector3D (Vector3D *vec, double x, double y, double z)
145 {
146 vec->x = x;
147 vec->y = y;
148 vec->z = z;
149
150 return vec;
151 }
152
153 Vector3D*
CopyVector3D(Vector3D * dest,const Vector3D * src)154 CopyVector3D (Vector3D *dest, const Vector3D *src)
155 {
156 dest->x = src->x;
157 dest->y = src->y;
158 dest->z = src->z;
159
160 return dest;
161 }
162
163 double
Abs3D(const Vector3D * v)164 Abs3D (const Vector3D *v)
165 {
166 return sqrt(v->x * v->x + v->y * v->y + v->z * v->z);
167 }
168
169 Vector3D*
AddVectors3D(Vector3D * vec,const Vector3D * vec1,const Vector3D * vec2)170 AddVectors3D (Vector3D *vec, const Vector3D *vec1, const Vector3D *vec2)
171 {
172 vec->x = vec1->x + vec2->x;
173 vec->y = vec1->y + vec2->y;
174 vec->z = vec1->z + vec2->z;
175
176 return vec;
177 }
178
179 Vector3D*
SubVectors3D(Vector3D * vec,const Vector3D * vec1,const Vector3D * vec2)180 SubVectors3D (Vector3D *vec, const Vector3D *vec1, const Vector3D *vec2)
181 {
182 vec->x = vec1->x - vec2->x;
183 vec->y = vec1->y - vec2->y;
184 vec->z = vec1->z - vec2->z;
185
186 return vec;
187 }
188
189 Vector3D*
MultScalar3D(Vector3D * dest,double d,const Vector3D * vec)190 MultScalar3D (Vector3D *dest, double d, const Vector3D *vec)
191 {
192 dest->x = d * vec->x;
193 dest->y = d * vec->y;
194 dest->z = d * vec->z;
195
196 return dest;
197 }
198
199 Vector3D*
MultVectors3D(Vector3D * vec,const Vector3D * vec1,const Vector3D * vec2)200 MultVectors3D (Vector3D *vec, const Vector3D *vec1, const Vector3D *vec2)
201 {
202 vec->x = vec1->x * vec2->x;
203 vec->y = vec1->y * vec2->y;
204 vec->z = vec1->z * vec2->z;
205
206 return vec;
207 }
208
209 double
DotProduct3D(const Vector3D * vec1,const Vector3D * vec2)210 DotProduct3D (const Vector3D *vec1, const Vector3D *vec2)
211 {
212 return vec1->x * vec2->x + vec1->y * vec2->y + vec1->z * vec2->z;
213 }
214
215 Vector3D*
CrossProduct3D(Vector3D * vec,const Vector3D * vec1,const Vector3D * vec2)216 CrossProduct3D (Vector3D *vec, const Vector3D *vec1, const Vector3D *vec2)
217 {
218 vec->x = vec1->y * vec2->z - vec2->y * vec1->z;
219 vec->y = vec1->z * vec2->x - vec2->z * vec1->x;
220 vec->z = vec1->x * vec2->y - vec2->x * vec1->y;
221
222 return vec;
223 }
224
225 Vector3D*
Unity3D(Vector3D * dest,const Vector3D * vec)226 Unity3D (Vector3D *dest, const Vector3D *vec)
227 {
228 return MultScalar3D(dest, 1 / Abs3D(vec), vec);
229 }
230
231 Matrix3D*
InitMatrix3D(Matrix3D * mat)232 InitMatrix3D (Matrix3D *mat)
233 {
234 int x,
235 y;
236
237 for (x = 0; x < 3; ++x)
238 for (y = 0; y < 3; ++y)
239 (*mat)[x][y] = 0.0;
240
241 return mat;
242 }
243
244 Matrix3D*
CopyMatrix3D(Matrix3D * dest,const Matrix3D * src)245 CopyMatrix3D (Matrix3D *dest, const Matrix3D *src)
246 {
247 int x,
248 y;
249
250 for (x = 0; x < 3; ++x)
251 for (y = 0; y < 3; ++y)
252 (*dest)[x][y] = (*src)[x][y];
253
254 return dest;
255 }
256
257 Vector3D*
MultMatrixVector3D(Vector3D * dest,const Matrix3D * mat,const Vector3D * vec)258 MultMatrixVector3D (Vector3D *dest, const Matrix3D *mat, const Vector3D *vec)
259 {
260 dest->x = (*mat)[0][0] * vec->x + (*mat)[0][1] * vec->y + (*mat)[0][2] * vec->z;
261 dest->y = (*mat)[1][0] * vec->x + (*mat)[1][1] * vec->y + (*mat)[1][2] * vec->z;
262 dest->z = (*mat)[2][0] * vec->x + (*mat)[2][1] * vec->y + (*mat)[2][2] * vec->z;
263
264 return dest;
265 }
266
267 Matrix4D*
InitMatrix4D(Matrix4D * mat)268 InitMatrix4D (Matrix4D *mat)
269 {
270 int x,
271 y;
272
273 for (x = 0; x < 4; ++x)
274 for (y = 0; y < 4; ++y)
275 (*mat)[x][y] = 0.0;
276
277 return mat;
278 }
279
280 Matrix4D*
CopyMatrix4D(Matrix4D * dest,const Matrix4D * src)281 CopyMatrix4D (Matrix4D *dest, const Matrix4D *src)
282 {
283 int x,
284 y;
285
286 for (x = 0; x < 4; ++x)
287 for (y = 0; y < 4; ++y)
288 (*dest)[x][y] = (*src)[x][y];
289
290 return dest;
291 }
292
293 Matrix4D*
MultMatrix4D(Matrix4D * dest,const Matrix4D * mat1,const Matrix4D * mat2)294 MultMatrix4D (Matrix4D *dest, const Matrix4D *mat1, const Matrix4D *mat2)
295 {
296 int x,
297 y,
298 i;
299
300 for (x = 0; x < 4; ++x)
301 for (y = 0; y < 4; ++y)
302 {
303 (*dest)[x][y] = 0.0;
304 for (i = 0; i < 4; ++i)
305 (*dest)[x][y] += (*mat1)[x][i] * (*mat2)[i][y];
306 }
307
308 return dest;
309 }
310
311 Vector3D*
ApplyTransformation(Vector3D * dest,const Matrix4D * mat,const Vector3D * vec)312 ApplyTransformation (Vector3D *dest, const Matrix4D *mat, const Vector3D *vec)
313 {
314 dest->x = (*mat)[0][0] * vec->x + (*mat)[0][1] * vec->y +
315 (*mat)[0][2] * vec->z + (*mat)[0][3];
316 dest->y = (*mat)[1][0] * vec->x + (*mat)[1][1] * vec->y +
317 (*mat)[1][2] * vec->z + (*mat)[1][3];
318 dest->z = (*mat)[2][0] * vec->x + (*mat)[2][1] * vec->y +
319 (*mat)[2][2] * vec->z + (*mat)[2][3];
320
321 return dest;
322 }
323
324 Matrix4D*
InitTranslationMatrix(Matrix4D * mat,double x,double y,double z)325 InitTranslationMatrix (Matrix4D *mat, double x, double y, double z)
326 {
327 int i;
328
329 InitMatrix4D(mat);
330
331 for (i = 0; i < 4; ++i)
332 (*mat)[i][i] = 1.0;
333
334 (*mat)[0][3] = x;
335 (*mat)[1][3] = y;
336 (*mat)[2][3] = z;
337
338 return mat;
339 }
340
341 Matrix4D*
InitXRotationMatrix(Matrix4D * mat,double theta)342 InitXRotationMatrix (Matrix4D *mat, double theta)
343 {
344 double sine = sin(theta),
345 cosine = cos(theta);
346
347 InitMatrix4D(mat);
348
349 (*mat)[0][0] = 1.0;
350 (*mat)[3][3] = 1.0;
351 (*mat)[1][1] = cosine;
352 (*mat)[1][2] = -sine;
353 (*mat)[2][1] = sine;
354 (*mat)[2][2] = cosine;
355
356 return mat;
357 }
358
359 Matrix4D*
InitYRotationMatrix(Matrix4D * mat,double theta)360 InitYRotationMatrix (Matrix4D *mat, double theta)
361 {
362 double sine = sin(theta),
363 cosine = cos(theta);
364
365 InitMatrix4D(mat);
366
367 (*mat)[1][1] = 1.0;
368 (*mat)[3][3] = 1.0;
369 (*mat)[0][0] = cosine;
370 (*mat)[0][2] = sine;
371 (*mat)[2][0] = -sine;
372 (*mat)[2][2] = cosine;
373
374 return mat;
375 }
376
377 Matrix4D*
InitZRotationMatrix(Matrix4D * mat,double theta)378 InitZRotationMatrix (Matrix4D *mat, double theta)
379 {
380 double sine = sin(theta),
381 cosine = cos(theta);
382
383 InitMatrix4D(mat);
384
385 (*mat)[2][2] = 1.0;
386 (*mat)[3][3] = 1.0;
387 (*mat)[0][0] = cosine;
388 (*mat)[0][1] = -sine;
389 (*mat)[1][0] = sine;
390 (*mat)[1][1] = cosine;
391
392 return mat;
393 }
394
395 Matrix4x3*
InitMatrix4x3(Matrix4x3 * mat)396 InitMatrix4x3 (Matrix4x3 *mat)
397 {
398 int i,
399 j;
400
401 for (i = 0; i < 4; ++i)
402 for (j = 0; j < 3; ++j)
403 (*mat)[i][j] = 0.0;
404
405 return mat;
406 }
407
408 Matrix4x3*
MakeMatrix4x3(Matrix4x3 * dest,const Vector3D * vec1,const Vector3D * vec2,const Vector3D * vec3,const Vector3D * vec4)409 MakeMatrix4x3 (Matrix4x3 *dest,
410 const Vector3D *vec1, const Vector3D *vec2,
411 const Vector3D *vec3, const Vector3D *vec4)
412 {
413 (*dest)[0][0] = vec1->x;
414 (*dest)[0][1] = vec1->y;
415 (*dest)[0][2] = vec1->z;
416
417 (*dest)[1][0] = vec2->x;
418 (*dest)[1][1] = vec2->y;
419 (*dest)[1][2] = vec2->z;
420
421 (*dest)[2][0] = vec3->x;
422 (*dest)[2][1] = vec3->y;
423 (*dest)[2][2] = vec3->z;
424
425 (*dest)[3][0] = vec4->x;
426 (*dest)[3][1] = vec4->y;
427 (*dest)[3][2] = vec4->z;
428
429 return dest;
430 }
431
432 Matrix4x3*
CopyMatrix4x3(Matrix4x3 * dest,const Matrix4x3 * src)433 CopyMatrix4x3 (Matrix4x3 *dest, const Matrix4x3 *src)
434 {
435 int i,
436 j;
437
438 for (i = 0; i < 4; ++i)
439 for (j = 0; j < 3; ++j)
440 (*dest)[i][j] = (*src)[i][j];
441
442 return dest;
443 }
444
445 Matrix4x3*
MultMatrix4x3(Matrix4x3 * dest,const Matrix4D * mat1,const Matrix4x3 * mat2)446 MultMatrix4x3 (Matrix4x3 *dest, const Matrix4D *mat1, const Matrix4x3 *mat2)
447 {
448 int i,
449 j,
450 k;
451
452 for (i = 0; i < 4; ++i)
453 for (j = 0; j < 3; ++j)
454 {
455 (*dest)[i][j] = 0.0;
456 for (k = 0; k < 4; ++k)
457 (*dest)[i][j] += (*mat1)[i][k] * (*mat2)[k][j];
458 }
459
460 return dest;
461 }
462
463 Vector3D*
MultVector4DMatrix4x3(Vector3D * dest,const Vector4D * vec,const Matrix4x3 * mat)464 MultVector4DMatrix4x3 (Vector3D *dest,
465 const Vector4D *vec, const Matrix4x3 *mat)
466 {
467 dest->x = (*vec)[0] * (*mat)[0][0] + (*vec)[1] * (*mat)[1][0] +
468 (*vec)[2] * (*mat)[2][0] + (*vec)[3] * (*mat)[3][0];
469 dest->y = (*vec)[0] * (*mat)[0][1] + (*vec)[1] * (*mat)[1][1] +
470 (*vec)[2] * (*mat)[2][1] + (*vec)[3] * (*mat)[3][1];
471 dest->z = (*vec)[0] * (*mat)[0][2] + (*vec)[1] * (*mat)[1][2] +
472 (*vec)[2] * (*mat)[2][2] + (*vec)[3] * (*mat)[3][2];
473
474 return dest;
475 }
476