1 // SONIC ROBO BLAST 2
2 //-----------------------------------------------------------------------------
3 // Copyright (C) 1993-1996 by id Software, Inc.
4 // Copyright (C) 1998-2000 by DooM Legacy Team.
5 // Copyright (C) 1999-2020 by Sonic Team Junior.
6 //
7 // This program is free software distributed under the
8 // terms of the GNU General Public License, version 2.
9 // See the 'LICENSE' file for more details.
10 //-----------------------------------------------------------------------------
11 /// \file m_fixed.c
12 /// \brief Fixed point implementation
13
14 #if 0 //#ifndef NO_M
15 #include <math.h>
16 #define HAVE_SQRT
17 #if 0 //#ifndef _WIN32 // MSVCRT does not have *f() functions
18 #define HAVE_SQRTF
19 #endif
20 #endif
21 #include "doomdef.h"
22 #include "m_fixed.h"
23
24 #ifdef __USE_C_FIXEDMUL__
25
26 /** \brief The FixedMul function
27
28 \param a fixed_t number
29 \param b fixed_t number
30
31 \return a*b>>FRACBITS
32
33 */
FixedMul(fixed_t a,fixed_t b)34 fixed_t FixedMul(fixed_t a, fixed_t b)
35 {
36 // Need to cast to unsigned before shifting to avoid undefined behaviour
37 // for negative integers
38 return (fixed_t)(((UINT64)((INT64)a * b)) >> FRACBITS);
39 }
40
41 #endif //__USE_C_FIXEDMUL__
42
43 #ifdef __USE_C_FIXEDDIV__
44 /** \brief The FixedDiv2 function
45
46 \param a fixed_t number
47 \param b fixed_t number
48
49 \return a/b * FRACUNIT
50
51 */
FixedDiv2(fixed_t a,fixed_t b)52 fixed_t FixedDiv2(fixed_t a, fixed_t b)
53 {
54 INT64 ret;
55
56 if (b == 0)
57 I_Error("FixedDiv: divide by zero");
58
59 ret = (((INT64)a * FRACUNIT)) / b;
60
61 if ((ret > INT32_MAX) || (ret < INT32_MIN))
62 I_Error("FixedDiv: divide by zero");
63 return (fixed_t)ret;
64 }
65
66 #endif // __USE_C_FIXEDDIV__
67
FixedSqrt(fixed_t x)68 fixed_t FixedSqrt(fixed_t x)
69 {
70 #ifdef HAVE_SQRT
71 const float fx = FIXED_TO_FLOAT(x);
72 float fr;
73 #ifdef HAVE_SQRTF
74 fr = sqrtf(fx);
75 #else
76 fr = (float)sqrt(fx);
77 #endif
78 return FLOAT_TO_FIXED(fr);
79 #else
80 // The neglected art of Fixed Point arithmetic
81 // Jetro Lauha
82 // Seminar Presentation
83 // Assembly 2006, 3rd- 6th August 2006
84 // (Revised: September 13, 2006)
85 // URL: http://jet.ro/files/The_neglected_art_of_Fixed_Point_arithmetic_20060913.pdf
86 register UINT32 root, remHi, remLo, testDiv, count;
87 root = 0; /* Clear root */
88 remHi = 0; /* Clear high part of partial remainder */
89 remLo = x; /* Get argument into low part of partial remainder */
90 count = (15 + (FRACBITS >> 1)); /* Load loop counter */
91 do
92 {
93 remHi = (remHi << 2) | (remLo >> 30); remLo <<= 2; /* get 2 bits of arg */
94 root <<= 1; /* Get ready for the next bit in the root */
95 testDiv = (root << 1) + 1; /* Test radical */
96 if (remHi >= testDiv)
97 {
98 remHi -= testDiv;
99 root += 1;
100 }
101 } while (count-- != 0);
102 return root;
103 #endif
104 }
105
FixedHypot(fixed_t x,fixed_t y)106 fixed_t FixedHypot(fixed_t x, fixed_t y)
107 {
108 fixed_t ax, yx, yx2, yx1;
109 if (abs(y) > abs(x)) // |y|>|x|
110 {
111 ax = abs(y); // |y| => ax
112 yx = FixedDiv(x, y); // (x/y)
113 }
114 else // |x|>|y|
115 {
116 ax = abs(x); // |x| => ax
117 yx = FixedDiv(y, x); // (x/y)
118 }
119 yx2 = FixedMul(yx, yx); // (x/y)^2
120 yx1 = FixedSqrt(1 * FRACUNIT + yx2); // (1 + (x/y)^2)^1/2
121 return FixedMul(ax, yx1); // |x|*((1 + (x/y)^2)^1/2)
122 }
123
FV2_Load(vector2_t * vec,fixed_t x,fixed_t y)124 vector2_t *FV2_Load(vector2_t *vec, fixed_t x, fixed_t y)
125 {
126 vec->x = x;
127 vec->y = y;
128 return vec;
129 }
130
FV2_UnLoad(vector2_t * vec,fixed_t * x,fixed_t * y)131 vector2_t *FV2_UnLoad(vector2_t *vec, fixed_t *x, fixed_t *y)
132 {
133 *x = vec->x;
134 *y = vec->y;
135 return vec;
136 }
137
FV2_Copy(vector2_t * a_o,const vector2_t * a_i)138 vector2_t *FV2_Copy(vector2_t *a_o, const vector2_t *a_i)
139 {
140 return M_Memcpy(a_o, a_i, sizeof(vector2_t));
141 }
142
FV2_AddEx(const vector2_t * a_i,const vector2_t * a_c,vector2_t * a_o)143 vector2_t *FV2_AddEx(const vector2_t *a_i, const vector2_t *a_c, vector2_t *a_o)
144 {
145 a_o->x = a_i->x + a_c->x;
146 a_o->y = a_i->y + a_c->y;
147 return a_o;
148 }
149
FV2_Add(vector2_t * a_i,const vector2_t * a_c)150 vector2_t *FV2_Add(vector2_t *a_i, const vector2_t *a_c)
151 {
152 return FV2_AddEx(a_i, a_c, a_i);
153 }
154
FV2_SubEx(const vector2_t * a_i,const vector2_t * a_c,vector2_t * a_o)155 vector2_t *FV2_SubEx(const vector2_t *a_i, const vector2_t *a_c, vector2_t *a_o)
156 {
157 a_o->x = a_i->x - a_c->x;
158 a_o->y = a_i->y - a_c->y;
159 return a_o;
160 }
161
FV2_Sub(vector2_t * a_i,const vector2_t * a_c)162 vector2_t *FV2_Sub(vector2_t *a_i, const vector2_t *a_c)
163 {
164 return FV2_SubEx(a_i, a_c, a_i);
165 }
166
FV2_MulEx(const vector2_t * a_i,fixed_t a_c,vector2_t * a_o)167 vector2_t *FV2_MulEx(const vector2_t *a_i, fixed_t a_c, vector2_t *a_o)
168 {
169 a_o->x = FixedMul(a_i->x, a_c);
170 a_o->y = FixedMul(a_i->y, a_c);
171 return a_o;
172 }
173
FV2_Mul(vector2_t * a_i,fixed_t a_c)174 vector2_t *FV2_Mul(vector2_t *a_i, fixed_t a_c)
175 {
176 return FV2_MulEx(a_i, a_c, a_i);
177 }
178
FV2_DivideEx(const vector2_t * a_i,fixed_t a_c,vector2_t * a_o)179 vector2_t *FV2_DivideEx(const vector2_t *a_i, fixed_t a_c, vector2_t *a_o)
180 {
181 a_o->x = FixedDiv(a_i->x, a_c);
182 a_o->y = FixedDiv(a_i->y, a_c);
183 return a_o;
184 }
185
FV2_Divide(vector2_t * a_i,fixed_t a_c)186 vector2_t *FV2_Divide(vector2_t *a_i, fixed_t a_c)
187 {
188 return FV2_DivideEx(a_i, a_c, a_i);
189 }
190
191 // Vector Complex Math
FV2_Midpoint(const vector2_t * a_1,const vector2_t * a_2,vector2_t * a_o)192 vector2_t *FV2_Midpoint(const vector2_t *a_1, const vector2_t *a_2, vector2_t *a_o)
193 {
194 a_o->x = FixedDiv(a_2->x - a_1->x, 2 * FRACUNIT);
195 a_o->y = FixedDiv(a_2->y - a_1->y, 2 * FRACUNIT);
196 a_o->x = a_1->x + a_o->x;
197 a_o->y = a_1->y + a_o->y;
198 return a_o;
199 }
200
FV2_Distance(const vector2_t * p1,const vector2_t * p2)201 fixed_t FV2_Distance(const vector2_t *p1, const vector2_t *p2)
202 {
203 fixed_t xs = FixedMul(p2->x - p1->x, p2->x - p1->x);
204 fixed_t ys = FixedMul(p2->y - p1->y, p2->y - p1->y);
205 return FixedSqrt(xs + ys);
206 }
207
FV2_Magnitude(const vector2_t * a_normal)208 fixed_t FV2_Magnitude(const vector2_t *a_normal)
209 {
210 fixed_t xs = FixedMul(a_normal->x, a_normal->x);
211 fixed_t ys = FixedMul(a_normal->y, a_normal->y);
212 return FixedSqrt(xs + ys);
213 }
214
215 // Also returns the magnitude
FV2_NormalizeEx(const vector2_t * a_normal,vector2_t * a_o)216 fixed_t FV2_NormalizeEx(const vector2_t *a_normal, vector2_t *a_o)
217 {
218 fixed_t magnitude = FV2_Magnitude(a_normal);
219 a_o->x = FixedDiv(a_normal->x, magnitude);
220 a_o->y = FixedDiv(a_normal->y, magnitude);
221 return magnitude;
222 }
223
FV2_Normalize(vector2_t * a_normal)224 fixed_t FV2_Normalize(vector2_t *a_normal)
225 {
226 return FV2_NormalizeEx(a_normal, a_normal);
227 }
228
FV2_NegateEx(const vector2_t * a_1,vector2_t * a_o)229 vector2_t *FV2_NegateEx(const vector2_t *a_1, vector2_t *a_o)
230 {
231 a_o->x = -a_1->x;
232 a_o->y = -a_1->y;
233 return a_o;
234 }
235
FV2_Negate(vector2_t * a_1)236 vector2_t *FV2_Negate(vector2_t *a_1)
237 {
238 return FV2_NegateEx(a_1, a_1);
239 }
240
FV2_Equal(const vector2_t * a_1,const vector2_t * a_2)241 boolean FV2_Equal(const vector2_t *a_1, const vector2_t *a_2)
242 {
243 fixed_t Epsilon = FRACUNIT / FRACUNIT;
244
245 if ((abs(a_2->x - a_1->x) > Epsilon) ||
246 (abs(a_2->y - a_1->y) > Epsilon))
247 {
248 return true;
249 }
250
251 return false;
252 }
253
FV2_Dot(const vector2_t * a_1,const vector2_t * a_2)254 fixed_t FV2_Dot(const vector2_t *a_1, const vector2_t *a_2)
255 {
256 return (FixedMul(a_1->x, a_2->x) + FixedMul(a_1->y, a_2->y));
257 }
258
259 //
260 // Point2Vec
261 //
262 // Given two points, create a vector between them.
263 //
FV2_Point2Vec(const vector2_t * point1,const vector2_t * point2,vector2_t * a_o)264 vector2_t *FV2_Point2Vec(const vector2_t *point1, const vector2_t *point2, vector2_t *a_o)
265 {
266 a_o->x = point1->x - point2->x;
267 a_o->y = point1->y - point2->y;
268 return a_o;
269 }
270
FV3_Load(vector3_t * vec,fixed_t x,fixed_t y,fixed_t z)271 vector3_t *FV3_Load(vector3_t *vec, fixed_t x, fixed_t y, fixed_t z)
272 {
273 vec->x = x;
274 vec->y = y;
275 vec->z = z;
276 return vec;
277 }
278
FV3_UnLoad(vector3_t * vec,fixed_t * x,fixed_t * y,fixed_t * z)279 vector3_t *FV3_UnLoad(vector3_t *vec, fixed_t *x, fixed_t *y, fixed_t *z)
280 {
281 *x = vec->x;
282 *y = vec->y;
283 *z = vec->z;
284 return vec;
285 }
286
FV3_Copy(vector3_t * a_o,const vector3_t * a_i)287 vector3_t *FV3_Copy(vector3_t *a_o, const vector3_t *a_i)
288 {
289 return M_Memcpy(a_o, a_i, sizeof(vector3_t));
290 }
291
FV3_AddEx(const vector3_t * a_i,const vector3_t * a_c,vector3_t * a_o)292 vector3_t *FV3_AddEx(const vector3_t *a_i, const vector3_t *a_c, vector3_t *a_o)
293 {
294 a_o->x = a_i->x + a_c->x;
295 a_o->y = a_i->y + a_c->y;
296 a_o->z = a_i->z + a_c->z;
297 return a_o;
298 }
299
FV3_Add(vector3_t * a_i,const vector3_t * a_c)300 vector3_t *FV3_Add(vector3_t *a_i, const vector3_t *a_c)
301 {
302 return FV3_AddEx(a_i, a_c, a_i);
303 }
304
FV3_SubEx(const vector3_t * a_i,const vector3_t * a_c,vector3_t * a_o)305 vector3_t *FV3_SubEx(const vector3_t *a_i, const vector3_t *a_c, vector3_t *a_o)
306 {
307 a_o->x = a_i->x - a_c->x;
308 a_o->y = a_i->y - a_c->y;
309 a_o->z = a_i->z - a_c->z;
310 return a_o;
311 }
312
FV3_Sub(vector3_t * a_i,const vector3_t * a_c)313 vector3_t *FV3_Sub(vector3_t *a_i, const vector3_t *a_c)
314 {
315 return FV3_SubEx(a_i, a_c, a_i);
316 }
317
FV3_MulEx(const vector3_t * a_i,fixed_t a_c,vector3_t * a_o)318 vector3_t *FV3_MulEx(const vector3_t *a_i, fixed_t a_c, vector3_t *a_o)
319 {
320 a_o->x = FixedMul(a_i->x, a_c);
321 a_o->y = FixedMul(a_i->y, a_c);
322 a_o->z = FixedMul(a_i->z, a_c);
323 return a_o;
324 }
325
FV3_Mul(vector3_t * a_i,fixed_t a_c)326 vector3_t *FV3_Mul(vector3_t *a_i, fixed_t a_c)
327 {
328 return FV3_MulEx(a_i, a_c, a_i);
329 }
330
FV3_DivideEx(const vector3_t * a_i,fixed_t a_c,vector3_t * a_o)331 vector3_t *FV3_DivideEx(const vector3_t *a_i, fixed_t a_c, vector3_t *a_o)
332 {
333 a_o->x = FixedDiv(a_i->x, a_c);
334 a_o->y = FixedDiv(a_i->y, a_c);
335 a_o->z = FixedDiv(a_i->z, a_c);
336 return a_o;
337 }
338
FV3_Divide(vector3_t * a_i,fixed_t a_c)339 vector3_t *FV3_Divide(vector3_t *a_i, fixed_t a_c)
340 {
341 return FV3_DivideEx(a_i, a_c, a_i);
342 }
343
344 // Vector Complex Math
FV3_Midpoint(const vector3_t * a_1,const vector3_t * a_2,vector3_t * a_o)345 vector3_t *FV3_Midpoint(const vector3_t *a_1, const vector3_t *a_2, vector3_t *a_o)
346 {
347 a_o->x = FixedDiv(a_2->x - a_1->x, 2 * FRACUNIT);
348 a_o->y = FixedDiv(a_2->y - a_1->y, 2 * FRACUNIT);
349 a_o->z = FixedDiv(a_2->z - a_1->z, 2 * FRACUNIT);
350 a_o->x = a_1->x + a_o->x;
351 a_o->y = a_1->y + a_o->y;
352 a_o->z = a_1->z + a_o->z;
353 return a_o;
354 }
355
FV3_Distance(const vector3_t * p1,const vector3_t * p2)356 fixed_t FV3_Distance(const vector3_t *p1, const vector3_t *p2)
357 {
358 fixed_t xs = FixedMul(p2->x - p1->x, p2->x - p1->x);
359 fixed_t ys = FixedMul(p2->y - p1->y, p2->y - p1->y);
360 fixed_t zs = FixedMul(p2->z - p1->z, p2->z - p1->z);
361 return FixedSqrt(xs + ys + zs);
362 }
363
FV3_Magnitude(const vector3_t * a_normal)364 fixed_t FV3_Magnitude(const vector3_t *a_normal)
365 {
366 fixed_t xs = FixedMul(a_normal->x, a_normal->x);
367 fixed_t ys = FixedMul(a_normal->y, a_normal->y);
368 fixed_t zs = FixedMul(a_normal->z, a_normal->z);
369 return FixedSqrt(xs + ys + zs);
370 }
371
372 // Also returns the magnitude
FV3_NormalizeEx(const vector3_t * a_normal,vector3_t * a_o)373 fixed_t FV3_NormalizeEx(const vector3_t *a_normal, vector3_t *a_o)
374 {
375 fixed_t magnitude = FV3_Magnitude(a_normal);
376 a_o->x = FixedDiv(a_normal->x, magnitude);
377 a_o->y = FixedDiv(a_normal->y, magnitude);
378 a_o->z = FixedDiv(a_normal->z, magnitude);
379 return magnitude;
380 }
381
FV3_Normalize(vector3_t * a_normal)382 fixed_t FV3_Normalize(vector3_t *a_normal)
383 {
384 return FV3_NormalizeEx(a_normal, a_normal);
385 }
386
FV3_NegateEx(const vector3_t * a_1,vector3_t * a_o)387 vector3_t *FV3_NegateEx(const vector3_t *a_1, vector3_t *a_o)
388 {
389 a_o->x = -a_1->x;
390 a_o->y = -a_1->y;
391 a_o->z = -a_1->z;
392 return a_o;
393 }
394
FV3_Negate(vector3_t * a_1)395 vector3_t *FV3_Negate(vector3_t *a_1)
396 {
397 return FV3_NegateEx(a_1, a_1);
398 }
399
FV3_Equal(const vector3_t * a_1,const vector3_t * a_2)400 boolean FV3_Equal(const vector3_t *a_1, const vector3_t *a_2)
401 {
402 fixed_t Epsilon = FRACUNIT / FRACUNIT;
403
404 if ((abs(a_2->x - a_1->x) > Epsilon) ||
405 (abs(a_2->y - a_1->y) > Epsilon) ||
406 (abs(a_2->z - a_1->z) > Epsilon))
407 {
408 return true;
409 }
410
411 return false;
412 }
413
FV3_Dot(const vector3_t * a_1,const vector3_t * a_2)414 fixed_t FV3_Dot(const vector3_t *a_1, const vector3_t *a_2)
415 {
416 return (FixedMul(a_1->x, a_2->x) + FixedMul(a_1->y, a_2->y) + FixedMul(a_1->z, a_2->z));
417 }
418
FV3_Cross(const vector3_t * a_1,const vector3_t * a_2,vector3_t * a_o)419 vector3_t *FV3_Cross(const vector3_t *a_1, const vector3_t *a_2, vector3_t *a_o)
420 {
421 a_o->x = FixedMul(a_1->y, a_2->z) - FixedMul(a_1->z, a_2->y);
422 a_o->y = FixedMul(a_1->z, a_2->x) - FixedMul(a_1->x, a_2->z);
423 a_o->z = FixedMul(a_1->x, a_2->y) - FixedMul(a_1->y, a_2->x);
424 return a_o;
425 }
426
427 //
428 // ClosestPointOnLine
429 //
430 // Finds the point on a line closest
431 // to the specified point.
432 //
FV3_ClosestPointOnLine(const vector3_t * Line,const vector3_t * p,vector3_t * out)433 vector3_t *FV3_ClosestPointOnLine(const vector3_t *Line, const vector3_t *p, vector3_t *out)
434 {
435 // Determine t (the length of the vector from �Line[0]� to �p�)
436 vector3_t c, V;
437 fixed_t t, d = 0;
438 FV3_SubEx(p, &Line[0], &c);
439 FV3_SubEx(&Line[1], &Line[0], &V);
440 FV3_NormalizeEx(&V, &V);
441
442 d = FV3_Distance(&Line[0], &Line[1]);
443 t = FV3_Dot(&V, &c);
444
445 // Check to see if �t� is beyond the extents of the line segment
446 if (t < 0)
447 {
448 return FV3_Copy(out, &Line[0]);
449 }
450 if (t > d)
451 {
452 return FV3_Copy(out, &Line[1]);
453 }
454
455 // Return the point between �Line[0]� and �Line[1]�
456 FV3_Mul(&V, t);
457
458 return FV3_AddEx(&Line[0], &V, out);
459 }
460
461 //
462 // ClosestPointOnVector
463 //
464 // Similar to ClosestPointOnLine, but uses a vector instead of two points.
465 //
FV3_ClosestPointOnVector(const vector3_t * dir,const vector3_t * p,vector3_t * out)466 void FV3_ClosestPointOnVector(const vector3_t *dir, const vector3_t *p, vector3_t *out)
467 {
468 fixed_t t = FV3_Dot(dir, p);
469
470 // Return the point on the line closest
471 FV3_MulEx(dir, t, out);
472 return;
473 }
474
475 //
476 // ClosestPointOnTriangle
477 //
478 // Given a triangle and a point,
479 // the closest point on the edge of
480 // the triangle is returned.
481 //
FV3_ClosestPointOnTriangle(const vector3_t * tri,const vector3_t * point,vector3_t * result)482 void FV3_ClosestPointOnTriangle(const vector3_t *tri, const vector3_t *point, vector3_t *result)
483 {
484 UINT8 i;
485 fixed_t dist, closestdist;
486 vector3_t EdgePoints[3];
487 vector3_t Line[2];
488
489 FV3_Copy(&Line[0], &tri[0]);
490 FV3_Copy(&Line[1], &tri[1]);
491 FV3_ClosestPointOnLine(Line, point, &EdgePoints[0]);
492
493 FV3_Copy(&Line[0], &tri[1]);
494 FV3_Copy(&Line[1], &tri[2]);
495 FV3_ClosestPointOnLine(Line, point, &EdgePoints[1]);
496
497 FV3_Copy(&Line[0], &tri[2]);
498 FV3_Copy(&Line[1], &tri[0]);
499 FV3_ClosestPointOnLine(Line, point, &EdgePoints[2]);
500
501 // Find the closest one of the three
502 FV3_Copy(result, &EdgePoints[0]);
503 closestdist = FV3_Distance(point, &EdgePoints[0]);
504 for (i = 1; i < 3; i++)
505 {
506 dist = FV3_Distance(point, &EdgePoints[i]);
507
508 if (dist < closestdist)
509 {
510 closestdist = dist;
511 FV3_Copy(result, &EdgePoints[i]);
512 }
513 }
514
515 // We now have the closest point! Whee!
516 }
517
518 //
519 // Point2Vec
520 //
521 // Given two points, create a vector between them.
522 //
FV3_Point2Vec(const vector3_t * point1,const vector3_t * point2,vector3_t * a_o)523 vector3_t *FV3_Point2Vec(const vector3_t *point1, const vector3_t *point2, vector3_t *a_o)
524 {
525 a_o->x = point1->x - point2->x;
526 a_o->y = point1->y - point2->y;
527 a_o->z = point1->z - point2->z;
528 return a_o;
529 }
530
531 //
532 // Normal
533 //
534 // Calculates the normal of a polygon.
535 //
FV3_Normal(const vector3_t * a_triangle,vector3_t * a_normal)536 fixed_t FV3_Normal(const vector3_t *a_triangle, vector3_t *a_normal)
537 {
538 vector3_t a_1;
539 vector3_t a_2;
540
541 FV3_Point2Vec(&a_triangle[2], &a_triangle[0], &a_1);
542 FV3_Point2Vec(&a_triangle[1], &a_triangle[0], &a_2);
543
544 FV3_Cross(&a_1, &a_2, a_normal);
545
546 return FV3_NormalizeEx(a_normal, a_normal);
547 }
548
549 //
550 // Strength
551 //
552 // Measures the 'strength' of a vector in a particular direction.
553 //
FV3_Strength(const vector3_t * a_1,const vector3_t * dir)554 fixed_t FV3_Strength(const vector3_t *a_1, const vector3_t *dir)
555 {
556 vector3_t normal;
557 fixed_t dist = FV3_NormalizeEx(a_1, &normal);
558 fixed_t dot = FV3_Dot(&normal, dir);
559
560 FV3_ClosestPointOnVector(dir, a_1, &normal);
561
562 dist = FV3_Magnitude(&normal);
563
564 if (dot < 0) // Not facing same direction, so negate result.
565 dist = -dist;
566
567 return dist;
568 }
569
570 //
571 // PlaneDistance
572 //
573 // Calculates distance between a plane and the origin.
574 //
FV3_PlaneDistance(const vector3_t * a_normal,const vector3_t * a_point)575 fixed_t FV3_PlaneDistance(const vector3_t *a_normal, const vector3_t *a_point)
576 {
577 return -(FixedMul(a_normal->x, a_point->x) + FixedMul(a_normal->y, a_point->y) + FixedMul(a_normal->z, a_point->z));
578 }
579
FV3_IntersectedPlane(const vector3_t * a_triangle,const vector3_t * a_line,vector3_t * a_normal,fixed_t * originDistance)580 boolean FV3_IntersectedPlane(const vector3_t *a_triangle, const vector3_t *a_line, vector3_t *a_normal, fixed_t *originDistance)
581 {
582 fixed_t distance1 = 0, distance2 = 0;
583
584 FV3_Normal(a_triangle, a_normal);
585
586 *originDistance = FV3_PlaneDistance(a_normal, &a_triangle[0]);
587
588 distance1 = (FixedMul(a_normal->x, a_line[0].x) + FixedMul(a_normal->y, a_line[0].y)
589 + FixedMul(a_normal->z, a_line[0].z)) + *originDistance;
590
591 distance2 = (FixedMul(a_normal->x, a_line[1].x) + FixedMul(a_normal->y, a_line[1].y)
592 + FixedMul(a_normal->z, a_line[1].z)) + *originDistance;
593
594 // Positive or zero number means no intersection
595 if (FixedMul(distance1, distance2) >= 0)
596 return false;
597
598 return true;
599 }
600
601 //
602 // PlaneIntersection
603 //
604 // Returns the distance from
605 // rOrigin to where the ray
606 // intersects the plane. Assumes
607 // you already know it intersects
608 // the plane.
609 //
FV3_PlaneIntersection(const vector3_t * pOrigin,const vector3_t * pNormal,const vector3_t * rOrigin,const vector3_t * rVector)610 fixed_t FV3_PlaneIntersection(const vector3_t *pOrigin, const vector3_t *pNormal, const vector3_t *rOrigin, const vector3_t *rVector)
611 {
612 fixed_t d = -(FV3_Dot(pNormal, pOrigin));
613 fixed_t number = FV3_Dot(pNormal, rOrigin) + d;
614 fixed_t denom = FV3_Dot(pNormal, rVector);
615 return -FixedDiv(number, denom);
616 }
617
618 //
619 // IntersectRaySphere
620 // Input : rO - origin of ray in world space
621 // rV - vector describing direction of ray in world space
622 // sO - Origin of sphere
623 // sR - radius of sphere
624 // Notes : Normalized directional vectors expected
625 // Return: distance to sphere in world units, -1 if no intersection.
626 //
FV3_IntersectRaySphere(const vector3_t * rO,const vector3_t * rV,const vector3_t * sO,fixed_t sR)627 fixed_t FV3_IntersectRaySphere(const vector3_t *rO, const vector3_t *rV, const vector3_t *sO, fixed_t sR)
628 {
629 vector3_t Q;
630 fixed_t c, v, d;
631 FV3_SubEx(sO, rO, &Q);
632
633 c = FV3_Magnitude(&Q);
634 v = FV3_Dot(&Q, rV);
635 d = FixedMul(sR, sR) - (FixedMul(c, c) - FixedMul(v, v));
636
637 // If there was no intersection, return -1
638 if (d < 0 * FRACUNIT)
639 return (-1 * FRACUNIT);
640
641 // Return the distance to the [first] intersecting point
642 return (v - FixedSqrt(d));
643 }
644
645 //
646 // IntersectionPoint
647 //
648 // This returns the intersection point of the line that intersects the plane
649 //
FV3_IntersectionPoint(const vector3_t * vNormal,const vector3_t * vLine,fixed_t distance,vector3_t * ReturnVec)650 vector3_t *FV3_IntersectionPoint(const vector3_t *vNormal, const vector3_t *vLine, fixed_t distance, vector3_t *ReturnVec)
651 {
652 vector3_t vLineDir; // Variables to hold the point and the line's direction
653 fixed_t Numerator = 0, Denominator = 0, dist = 0;
654
655 // Here comes the confusing part. We need to find the 3D point that is actually
656 // on the plane. Here are some steps to do that:
657
658 // 1) First we need to get the vector of our line, Then normalize it so it's a length of 1
659 FV3_Point2Vec(&vLine[1], &vLine[0], &vLineDir); // Get the Vector of the line
660 FV3_NormalizeEx(&vLineDir, &vLineDir); // Normalize the lines vector
661
662
663 // 2) Use the plane equation (distance = Ax + By + Cz + D) to find the distance from one of our points to the plane.
664 // Here I just chose a arbitrary point as the point to find that distance. You notice we negate that
665 // distance. We negate the distance because we want to eventually go BACKWARDS from our point to the plane.
666 // By doing this is will basically bring us back to the plane to find our intersection point.
667 Numerator = -(FixedMul(vNormal->x, vLine[0].x) + // Use the plane equation with the normal and the line
668 FixedMul(vNormal->y, vLine[0].y) +
669 FixedMul(vNormal->z, vLine[0].z) + distance);
670
671 // 3) If we take the dot product between our line vector and the normal of the polygon,
672 // this will give us the cosine of the angle between the 2 (since they are both normalized - length 1).
673 // We will then divide our Numerator by this value to find the offset towards the plane from our arbitrary point.
674 Denominator = FV3_Dot(vNormal, &vLineDir); // Get the dot product of the line's vector and the normal of the plane
675
676 // Since we are using division, we need to make sure we don't get a divide by zero error
677 // If we do get a 0, that means that there are INFINITE points because the the line is
678 // on the plane (the normal is perpendicular to the line - (Normal.Vector = 0)).
679 // In this case, we should just return any point on the line.
680
681 if (Denominator == 0 * FRACUNIT) // Check so we don't divide by zero
682 {
683 ReturnVec->x = vLine[0].x;
684 ReturnVec->y = vLine[0].y;
685 ReturnVec->z = vLine[0].z;
686 return ReturnVec; // Return an arbitrary point on the line
687 }
688
689 // We divide the (distance from the point to the plane) by (the dot product)
690 // to get the distance (dist) that we need to move from our arbitrary point. We need
691 // to then times this distance (dist) by our line's vector (direction). When you times
692 // a scalar (single number) by a vector you move along that vector. That is what we are
693 // doing. We are moving from our arbitrary point we chose from the line BACK to the plane
694 // along the lines vector. It seems logical to just get the numerator, which is the distance
695 // from the point to the line, and then just move back that much along the line's vector.
696 // Well, the distance from the plane means the SHORTEST distance. What about in the case that
697 // the line is almost parallel with the polygon, but doesn't actually intersect it until half
698 // way down the line's length. The distance from the plane is short, but the distance from
699 // the actual intersection point is pretty long. If we divide the distance by the dot product
700 // of our line vector and the normal of the plane, we get the correct length. Cool huh?
701
702 dist = FixedDiv(Numerator, Denominator); // Divide to get the multiplying (percentage) factor
703
704 // Now, like we said above, we times the dist by the vector, then add our arbitrary point.
705 // This essentially moves the point along the vector to a certain distance. This now gives
706 // us the intersection point. Yay!
707
708 // Return the intersection point
709 ReturnVec->x = vLine[0].x + FixedMul(vLineDir.x, dist);
710 ReturnVec->y = vLine[0].y + FixedMul(vLineDir.y, dist);
711 ReturnVec->z = vLine[0].z + FixedMul(vLineDir.z, dist);
712 return ReturnVec;
713 }
714
715 //
716 // PointOnLineSide
717 //
718 // If on the front side of the line, returns 1.
719 // If on the back side of the line, returns 0.
720 // 2D only.
721 //
FV3_PointOnLineSide(const vector3_t * point,const vector3_t * line)722 UINT8 FV3_PointOnLineSide(const vector3_t *point, const vector3_t *line)
723 {
724 fixed_t s1 = FixedMul((point->y - line[0].y), (line[1].x - line[0].x));
725 fixed_t s2 = FixedMul((point->x - line[0].x), (line[1].y - line[0].y));
726 return (UINT8)(s1 - s2 < 0);
727 }
728
729 //
730 // PointInsideBox
731 //
732 // Given four points of a box,
733 // determines if the supplied point is
734 // inside the box or not.
735 //
FV3_PointInsideBox(const vector3_t * point,const vector3_t * box)736 boolean FV3_PointInsideBox(const vector3_t *point, const vector3_t *box)
737 {
738 vector3_t lastLine[2];
739
740 FV3_Load(&lastLine[0], box[3].x, box[3].y, box[3].z);
741 FV3_Load(&lastLine[1], box[0].x, box[0].y, box[0].z);
742
743 if (FV3_PointOnLineSide(point, &box[0])
744 || FV3_PointOnLineSide(point, &box[1])
745 || FV3_PointOnLineSide(point, &box[2])
746 || FV3_PointOnLineSide(point, lastLine))
747 return false;
748
749 return true;
750 }
751 //
752 // LoadIdentity
753 //
754 // Loads the identity matrix into a matrix
755 //
FM_LoadIdentity(matrix_t * matrix)756 void FM_LoadIdentity(matrix_t* matrix)
757 {
758 #define M(row,col) matrix->m[col * 4 + row]
759 memset(matrix, 0x00, sizeof(matrix_t));
760
761 M(0, 0) = FRACUNIT;
762 M(1, 1) = FRACUNIT;
763 M(2, 2) = FRACUNIT;
764 M(3, 3) = FRACUNIT;
765 #undef M
766 }
767
768 //
769 // CreateObjectMatrix
770 //
771 // Creates a matrix that can be used for
772 // adjusting the position of an object
773 //
FM_CreateObjectMatrix(matrix_t * matrix,fixed_t x,fixed_t y,fixed_t z,fixed_t anglex,fixed_t angley,fixed_t anglez,fixed_t upx,fixed_t upy,fixed_t upz,fixed_t radius)774 void FM_CreateObjectMatrix(matrix_t *matrix, fixed_t x, fixed_t y, fixed_t z, fixed_t anglex, fixed_t angley, fixed_t anglez, fixed_t upx, fixed_t upy, fixed_t upz, fixed_t radius)
775 {
776 vector3_t upcross;
777 vector3_t upvec;
778 vector3_t basevec;
779
780 FV3_Load(&upvec, upx, upy, upz);
781 FV3_Load(&basevec, anglex, angley, anglez);
782 FV3_Cross(&upvec, &basevec, &upcross);
783 FV3_Normalize(&upcross);
784
785 FM_LoadIdentity(matrix);
786
787 matrix->m[0] = upcross.x;
788 matrix->m[1] = upcross.y;
789 matrix->m[2] = upcross.z;
790 matrix->m[3] = 0 * FRACUNIT;
791
792 matrix->m[4] = upx;
793 matrix->m[5] = upy;
794 matrix->m[6] = upz;
795 matrix->m[7] = 0;
796
797 matrix->m[8] = anglex;
798 matrix->m[9] = angley;
799 matrix->m[10] = anglez;
800 matrix->m[11] = 0;
801
802 matrix->m[12] = x - FixedMul(upx, radius);
803 matrix->m[13] = y - FixedMul(upy, radius);
804 matrix->m[14] = z - FixedMul(upz, radius);
805 matrix->m[15] = FRACUNIT;
806 }
807
808 //
809 // MultMatrixVec
810 //
811 // Multiplies a vector by the specified matrix
812 //
FM_MultMatrixVec3(const matrix_t * matrix,const vector3_t * vec,vector3_t * out)813 void FM_MultMatrixVec3(const matrix_t *matrix, const vector3_t *vec, vector3_t *out)
814 {
815 #define M(row,col) matrix->m[col * 4 + row]
816 out->x = FixedMul(vec->x, M(0, 0))
817 + FixedMul(vec->y, M(0, 1))
818 + FixedMul(vec->z, M(0, 2))
819 + M(0, 3);
820
821 out->y = FixedMul(vec->x, M(1, 0))
822 + FixedMul(vec->y, M(1, 1))
823 + FixedMul(vec->z, M(1, 2))
824 + M(1, 3);
825
826 out->z = FixedMul(vec->x, M(2, 0))
827 + FixedMul(vec->y, M(2, 1))
828 + FixedMul(vec->z, M(2, 2))
829 + M(2, 3);
830 #undef M
831 }
832
833 //
834 // MultMatrix
835 //
836 // Multiples one matrix into another
837 //
FM_MultMatrix(matrix_t * dest,const matrix_t * multme)838 void FM_MultMatrix(matrix_t *dest, const matrix_t *multme)
839 {
840 matrix_t result;
841 UINT8 i, j;
842 #define M(row,col) multme->m[col * 4 + row]
843 #define D(row,col) dest->m[col * 4 + row]
844 #define R(row,col) result.m[col * 4 + row]
845
846 for (i = 0; i < 4; i++)
847 {
848 for (j = 0; j < 4; j++)
849 R(i, j) = FixedMul(D(i, 0), M(0, j)) + FixedMul(D(i, 1), M(1, j)) + FixedMul(D(i, 2), M(2, j)) + FixedMul(D(i, 3), M(3, j));
850 }
851
852 M_Memcpy(dest, &result, sizeof(matrix_t));
853
854 #undef R
855 #undef D
856 #undef M
857 }
858
859 //
860 // Translate
861 //
862 // Translates a matrix
863 //
FM_Translate(matrix_t * dest,fixed_t x,fixed_t y,fixed_t z)864 void FM_Translate(matrix_t *dest, fixed_t x, fixed_t y, fixed_t z)
865 {
866 matrix_t trans;
867 #define M(row,col) trans.m[col * 4 + row]
868
869 memset(&trans, 0x00, sizeof(matrix_t));
870
871 M(0, 0) = M(1, 1) = M(2, 2) = M(3, 3) = FRACUNIT;
872 M(0, 3) = x;
873 M(1, 3) = y;
874 M(2, 3) = z;
875
876 FM_MultMatrix(dest, &trans);
877 #undef M
878 }
879
880 //
881 // Scale
882 //
883 // Scales a matrix
884 //
FM_Scale(matrix_t * dest,fixed_t x,fixed_t y,fixed_t z)885 void FM_Scale(matrix_t *dest, fixed_t x, fixed_t y, fixed_t z)
886 {
887 matrix_t scale;
888 #define M(row,col) scale.m[col * 4 + row]
889
890 memset(&scale, 0x00, sizeof(matrix_t));
891
892 M(3, 3) = FRACUNIT;
893 M(0, 0) = x;
894 M(1, 1) = y;
895 M(2, 2) = z;
896
897 FM_MultMatrix(dest, &scale);
898 #undef M
899 }
900
901 #ifdef M_TESTCASE
902 //#define MULDIV_TEST
903 #define SQRT_TEST
904
M_print(INT64 a)905 static inline void M_print(INT64 a)
906 {
907 const fixed_t w = (a >> FRACBITS);
908 fixed_t f = a % FRACUNIT;
909 fixed_t d = FRACUNIT;
910
911 if (f == 0)
912 {
913 printf("%d", (fixed_t)w);
914 return;
915 }
916 else while (f != 1 && f / 2 == f >> 1)
917 {
918 d /= 2;
919 f /= 2;
920 }
921
922 if (w == 0)
923 printf("%d/%d", (fixed_t)f, d);
924 else
925 printf("%d+(%d/%d)", (fixed_t)w, (fixed_t)f, d);
926 }
927
FixedMulC(fixed_t a,fixed_t b)928 FUNCMATH FUNCINLINE static inline fixed_t FixedMulC(fixed_t a, fixed_t b)
929 {
930 return (fixed_t)((((INT64)a * b)) / FRACUNIT);
931 }
932
FixedDivC2(fixed_t a,fixed_t b)933 FUNCMATH FUNCINLINE static inline fixed_t FixedDivC2(fixed_t a, fixed_t b)
934 {
935 INT64 ret;
936
937 if (b == 0)
938 I_Error("FixedDiv: divide by zero");
939
940 ret = (((INT64)a * FRACUNIT)) / b;
941
942 if ((ret > INT32_MAX) || (ret < INT32_MIN))
943 I_Error("FixedDiv: divide by zero");
944 return (fixed_t)ret;
945 }
946
FixedDivC(fixed_t a,fixed_t b)947 FUNCMATH FUNCINLINE static inline fixed_t FixedDivC(fixed_t a, fixed_t b)
948 {
949 if ((abs(a) >> (FRACBITS - 2)) >= abs(b))
950 return (a^b) < 0 ? INT32_MIN : INT32_MAX;
951
952 return FixedDivC2(a, b);
953 }
954
FixedSqrtC(fixed_t x)955 FUNCMATH FUNCINLINE static inline fixed_t FixedSqrtC(fixed_t x)
956 {
957 const float fx = FIXED_TO_FLOAT(x);
958 float fr;
959 #ifdef HAVE_SQRTF
960 fr = sqrtf(fx);
961 #else
962 fr = (float)sqrt(fx);
963 #endif
964 return FLOAT_TO_FIXED(fr);
965 }
main(int argc,char ** argv)966 int main(int argc, char** argv)
967 {
968 int n = 10;
969 INT64 a, b;
970 fixed_t c, d;
971 (void)argc;
972 (void)argv;
973
974 #ifdef MULDIV_TEST
975 for (a = 1; a <= INT32_MAX; a += FRACUNIT)
976 for (b = 0; b <= INT32_MAX; b += FRACUNIT)
977 {
978 c = FixedMul(a, b);
979 d = FixedMulC(a, b);
980 if (c != d)
981 {
982 printf("(");
983 M_print(a);
984 printf(") * (");
985 M_print(b);
986 printf(") = (");
987 M_print(c);
988 printf(") != (");
989 M_print(d);
990 printf(") \n");
991 n--;
992 printf("%d != %d\n", c, d);
993 }
994 c = FixedDiv(a, b);
995 d = FixedDivC(a, b);
996 if (c != d)
997 {
998 printf("(");
999 M_print(a);
1000 printf(") / (");
1001 M_print(b);
1002 printf(") = (");
1003 M_print(c);
1004 printf(") != (");
1005 M_print(d);
1006 printf(")\n");
1007 n--;
1008 printf("%d != %d\n", c, d);
1009 }
1010 if (n <= 0)
1011 exit(-1);
1012 }
1013 #endif
1014
1015 #ifdef SQRT_TEST
1016 for (a = 0; a <= INT32_MAX; a += 1)
1017 {
1018 c = FixedSqrt(a);
1019 d = FixedSqrtC(a);
1020 b = abs(c - d);
1021 if (b > 1)
1022 {
1023 printf("sqrt(");
1024 M_print(a);
1025 printf(") = {(");
1026 M_print(c);
1027 printf(") != (");
1028 M_print(d);
1029 printf(")} \n");
1030 //n--;
1031 printf("%d != %d {", c, d);
1032 M_print(b);
1033 printf("}\n");
1034 }
1035 if (n <= 0)
1036 exit(-1);
1037 }
1038 #endif
1039 exit(0);
1040 }
1041
cpu_cpy(void * dest,const void * src,size_t n)1042 static void *cpu_cpy(void *dest, const void *src, size_t n)
1043 {
1044 return memcpy(dest, src, n);
1045 }
1046
1047 void *(*M_Memcpy)(void* dest, const void* src, size_t n) = cpu_cpy;
1048
I_Error(const char * error,...)1049 void I_Error(const char *error, ...)
1050 {
1051 (void)error;
1052 exit(-1);
1053 }
1054 #endif
1055