1 /** @file vector1.c Vector math (2D, 3D, 4D).
2 *
3 * @authors Copyright © 2003-2017 Jaakko Keränen <jaakko.keranen@iki.fi>
4 * @authors Copyright © 2006-2013 Daniel Swanson <danij@dengine.net>
5 *
6 * @par License
7 * GPL: http://www.gnu.org/licenses/gpl.html
8 *
9 * <small>This program is free software; you can redistribute it and/or modify
10 * it under the terms of the GNU General Public License as published by the
11 * Free Software Foundation; either version 2 of the License, or (at your
12 * option) any later version. This program is distributed in the hope that it
13 * will be useful, but WITHOUT ANY WARRANTY; without even the implied warranty
14 * of MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General
15 * Public License for more details. You should have received a copy of the GNU
16 * General Public License along with this program; if not, write to the Free
17 * Software Foundation, Inc., 51 Franklin St, Fifth Floor, Boston, MA
18 * 02110-1301 USA</small>
19 */
20
21 #include <math.h>
22 #include <float.h>
23
24 #include "de/vector1.h"
25
V2x_Set(fixed_t vec[],fixed_t x,fixed_t y)26 void V2x_Set(fixed_t vec[], fixed_t x, fixed_t y)
27 {
28 vec[VX] = x;
29 vec[VY] = y;
30 }
31
V2x_Intersection(fixed_t const v1[],fixed_t const v1Delta[],fixed_t const v2[],fixed_t const v2Delta[])32 fixed_t V2x_Intersection(fixed_t const v1[], fixed_t const v1Delta[],
33 fixed_t const v2[], fixed_t const v2Delta[])
34 {
35 fixed_t r, div = FixedMul(v1Delta[VY] >> 8, v2Delta[VX]) -
36 FixedMul(v1Delta[VX] >> 8, v2Delta[VY]);
37 if (div == 0)
38 {
39 // Parallel.
40 r = 0;
41 }
42 else
43 {
44 r = FixedMul((v1[VX] - v2[VX]) >> 8, v1Delta[VY]) +
45 FixedMul((v2[VY] - v1[VY]) >> 8, v1Delta[VX]);
46 r = FixedDiv(r, div);
47 }
48 return r;
49 }
50
V2x_PointOnLineSide(fixed_t const point[],fixed_t const lineOrigin[],fixed_t const lineDirection[])51 int V2x_PointOnLineSide(fixed_t const point[], fixed_t const lineOrigin[], fixed_t const lineDirection[])
52 {
53 if (!lineDirection[VX])
54 {
55 return (point[VX] <= lineOrigin[VX])? lineDirection[VY] > 0 : lineDirection[VY] < 0;
56 }
57 else if (!lineDirection[VY])
58 {
59 return (point[VY] <= lineOrigin[VY])? lineDirection[VX] < 0 : lineDirection[VX] > 0;
60 }
61 else
62 {
63 fixed_t dX = point[VX] - lineOrigin[VX];
64 fixed_t dY = point[VY] - lineOrigin[VY];
65
66 // Try to quickly decide by comparing signs.
67 if ((lineDirection[VY] ^ lineDirection[VX] ^ dX ^ dY) & 0x80000000)
68 {
69 // Left is negative.
70 return ((lineDirection[VY] ^ dX) & 0x80000000)? 1 : 0;
71 }
72 else
73 {
74 // if left >= right return 1 else 0.
75 return FixedMul(dY >> 8, lineDirection[VX] >> 8) >=
76 FixedMul(lineDirection[VY] >> 8, dX >> 8);
77 }
78 }
79 }
80
V2f_Set(pvec2f_t vec,vectorcompf_t x,vectorcompf_t y)81 void V2f_Set(pvec2f_t vec, vectorcompf_t x, vectorcompf_t y)
82 {
83 vec[VX] = x;
84 vec[VY] = y;
85 }
86
V2f_SetFixed(pvec2f_t vec,fixed_t x,fixed_t y)87 void V2f_SetFixed(pvec2f_t vec, fixed_t x, fixed_t y)
88 {
89 vec[VX] = FIX2FLT(x);
90 vec[VY] = FIX2FLT(y);
91 }
92
V2f_Length(const_pvec2f_t vec)93 float V2f_Length(const_pvec2f_t vec)
94 {
95 if (vec[VX] == 0 && vec[VY] == 0) return 0;
96 return (float) sqrt(vec[VX] * vec[VX] + vec[VY] * vec[VY]);
97 }
98
V2f_Distance(const_pvec2f_t a,const_pvec2f_t b)99 float V2f_Distance(const_pvec2f_t a, const_pvec2f_t b)
100 {
101 vec2f_t vec;
102 V2f_Subtract(vec, b, a);
103 return V2f_Length(vec);
104 }
105
V2f_Normalize(pvec2f_t vec)106 float V2f_Normalize(pvec2f_t vec)
107 {
108 float len = V2f_Length(vec);
109 if (len != 0)
110 {
111 vec[VX] /= len;
112 vec[VY] /= len;
113 }
114 return len;
115 }
116
V2f_Copy(pvec2f_t dest,const_pvec2f_t src)117 void V2f_Copy(pvec2f_t dest, const_pvec2f_t src)
118 {
119 dest[VX] = src[VX];
120 dest[VY] = src[VY];
121 }
122
V2f_Copyd(pvec2f_t dest,const_pvec2d_t src)123 void V2f_Copyd(pvec2f_t dest, const_pvec2d_t src)
124 {
125 vec2f_t other;
126 V2f_Set(other, (float) src[VX], (float) src[VY]);
127 V2f_Copy(dest, other);
128 }
129
V2f_Scale(pvec2f_t vec,float scalar)130 void V2f_Scale(pvec2f_t vec, float scalar)
131 {
132 vec[VX] *= scalar;
133 vec[VY] *= scalar;
134 }
135
V2f_Rotate(pvec2f_t vec,float radians)136 void V2f_Rotate(pvec2f_t vec, float radians)
137 {
138 float const c = (float) cos(radians);
139 float const s = (float) sin(radians);
140 float const x = c * vec[VX] - s * vec[VY];
141 float const y = s * vec[VX] + c * vec[VY];
142
143 vec[VX] = x;
144 vec[VY] = y;
145 }
146
V2f_Sum(pvec2f_t dest,const_pvec2f_t src1,const_pvec2f_t src2)147 void V2f_Sum(pvec2f_t dest, const_pvec2f_t src1, const_pvec2f_t src2)
148 {
149 dest[VX] = src1[VX] + src2[VX];
150 dest[VY] = src1[VY] + src2[VY];
151 }
152
V2f_Subtract(pvec2f_t dest,const_pvec2f_t src1,const_pvec2f_t src2)153 void V2f_Subtract(pvec2f_t dest, const_pvec2f_t src1, const_pvec2f_t src2)
154 {
155 dest[VX] = src1[VX] - src2[VX];
156 dest[VY] = src1[VY] - src2[VY];
157 }
158
V2f_DotProduct(const_pvec2f_t a,const_pvec2f_t b)159 float V2f_DotProduct(const_pvec2f_t a, const_pvec2f_t b)
160 {
161 return a[VX] * b[VX] + a[VY] * b[VY];
162 }
163
V2f_ScalarProject(const_pvec2f_t a,const_pvec2f_t b)164 float V2f_ScalarProject(const_pvec2f_t a, const_pvec2f_t b)
165 {
166 float dot, len = V2f_Length(b);
167 if (len == 0) return 0;
168
169 dot = V2f_DotProduct(a, b);
170 return dot / len;
171 }
172
V2f_Project(pvec2f_t dest,const_pvec2f_t a,const_pvec2f_t b)173 float V2f_Project(pvec2f_t dest, const_pvec2f_t a, const_pvec2f_t b)
174 {
175 float div = V2f_DotProduct(b, b);
176 if (div == 0)
177 {
178 dest[VX] = dest[VY] = 0;
179 return 0;
180 }
181
182 V2f_Copy(dest, b);
183 V2f_Scale(dest, V2f_DotProduct(a, b) / div);
184 return div;
185 }
186
V2f_IsParallel(const_pvec2f_t a,const_pvec2f_t b)187 dd_bool V2f_IsParallel(const_pvec2f_t a, const_pvec2f_t b)
188 {
189 #define EPSILON .9999f
190
191 float aLen = V2f_Length(a);
192 float bLen = V2f_Length(b);
193 float dot;
194
195 // Both must be non-zero vectors.
196 if (aLen == 0 || bLen == 0) return true;
197
198 dot = V2f_DotProduct(a, b) / aLen / bLen;
199
200 // If it's close enough, we'll consider them parallel.
201 return dot > EPSILON || dot < -EPSILON;
202
203 #undef EPSILON
204 }
205
V2f_IsZero(const_pvec2f_t vec)206 dd_bool V2f_IsZero(const_pvec2f_t vec)
207 {
208 return vec[VX] == 0 && vec[VY] == 0;
209 }
210
V2f_PointUnitLineDistance(const_pvec2f_t point,const_pvec2f_t linePoint,const_pvec2f_t lineDirection)211 float V2f_PointUnitLineDistance(const_pvec2f_t point, const_pvec2f_t linePoint, const_pvec2f_t lineDirection)
212 {
213 return (float) fabs(((linePoint[VY] - point[VY]) * (lineDirection[VX] - linePoint[VX]) -
214 (linePoint[VX] - point[VX]) * (lineDirection[VY] - linePoint[VY])));
215 }
216
V2f_Intersection(const_pvec2f_t p1,const_pvec2f_t delta1,const_pvec2f_t p2,const_pvec2f_t delta2,pvec2f_t point)217 float V2f_Intersection(const_pvec2f_t p1, const_pvec2f_t delta1, const_pvec2f_t p2,
218 const_pvec2f_t delta2, pvec2f_t point)
219 {
220 /*
221 * (YA-YC)(XD-XC)-(XA-XC)(YD-YC)
222 * r = -----------------------------
223 * (XB-XA)(YD-YC)-(YB-YA)(XD-XC)
224 */
225
226 float r, div;
227 int i;
228
229 div = delta1[VX] * delta2[VY] - delta1[VY] * delta2[VX];
230
231 if (div == 0)
232 {
233 // Special case: lines are parallel.
234 r = 0;
235 }
236 else
237 {
238 r = ((p1[VY] - p2[VY]) * delta2[VX] -
239 (p1[VX] - p2[VX]) * delta2[VY]) / div;
240 }
241
242 /*
243 * XI=XA+r(XB-XA)
244 * YI=YA+r(YB-YA)
245 */
246
247 if (point)
248 {
249 // Calculate the intersection point.
250 for (i = 0; i < 2; ++i)
251 point[i] = p1[i] + r * delta1[i];
252 }
253
254 // Return the scaling factor.
255 return r;
256 }
257
V2f_Intercept(const_pvec2f_t a,const_pvec2f_t b,const_pvec2f_t c,const_pvec2f_t d,pvec2f_t point)258 float V2f_Intercept(const_pvec2f_t a, const_pvec2f_t b, const_pvec2f_t c,
259 const_pvec2f_t d, pvec2f_t point)
260 {
261 vec2f_t ab, cd;
262
263 ab[0] = b[VX] - a[VX];
264 ab[1] = b[VY] - a[VY];
265 cd[0] = d[VX] - c[VX];
266 cd[1] = d[VY] - c[VY];
267
268 return V2f_Intersection(a, ab, c, cd, point);
269 }
270
V2f_Intercept2(const_pvec2f_t a,const_pvec2f_t b,const_pvec2f_t c,const_pvec2f_t d,pvec2f_t point,float * abFrac,float * cdFrac)271 dd_bool V2f_Intercept2(const_pvec2f_t a, const_pvec2f_t b, const_pvec2f_t c,
272 const_pvec2f_t d, pvec2f_t point, float* abFrac, float* cdFrac)
273 {
274 float ab, cd;
275
276 ab = V2f_Intercept(a, b, c, d, point);
277 cd = V2f_Intercept(c, d, a, b, NULL);
278
279 if (abFrac) *abFrac = ab;
280 if (cdFrac) *cdFrac = cd;
281
282 return (ab >= 0 && ab <= 1 && cd >= 0 && cd <= 1);
283 }
284
V2f_Lerp(pvec2f_t dest,const_pvec2f_t a,const_pvec2f_t b,float c)285 void V2f_Lerp(pvec2f_t dest, const_pvec2f_t a, const_pvec2f_t b, float c)
286 {
287 uint i;
288 for (i = 0; i < 2; ++i)
289 {
290 dest[i] = a[i] + c * (b[i] - a[i]);
291 }
292 }
293
V2f_InitBox(arvec2f_t box,const_pvec2f_t point)294 void V2f_InitBox(arvec2f_t box, const_pvec2f_t point)
295 {
296 V2f_Copy(box[0], point);
297 V2f_Copy(box[1], point);
298 }
299
V2f_AddToBox(arvec2f_t box,const_pvec2f_t point)300 void V2f_AddToBox(arvec2f_t box, const_pvec2f_t point)
301 {
302 if (point[VX] < box[0][VX])
303 box[0][VX] = point[VX];
304 if (point[VX] > box[1][VX])
305 box[1][VX] = point[VX];
306
307 if (point[VY] < box[0][VY])
308 box[0][VY] = point[VY];
309 if (point[VY] > box[1][VY])
310 box[1][VY] = point[VY];
311 }
312
V2f_UniteBox(arvec2f_t box,arvec2f_t const other)313 void V2f_UniteBox(arvec2f_t box, arvec2f_t const other)
314 {
315 if (other[0][VX] < box[0][VX])
316 box[0][VX] = other[0][VX];
317
318 if (other[1][VX] > box[1][VX])
319 box[1][VX] = other[1][VX];
320
321 if (other[0][VY] < box[0][VY])
322 box[0][VY] = other[0][VY];
323
324 if (other[1][VY] > box[1][VY])
325 box[1][VY] = other[1][VY];
326 }
327
V2f_CopyBox(arvec2f_t dest,arvec2f_t const src)328 void V2f_CopyBox(arvec2f_t dest, arvec2f_t const src)
329 {
330 V2f_Copy(dest[0], src[0]);
331 V2f_Copy(dest[1], src[1]);
332 }
333
V2f_CopyBoxd(arvec2f_t dest,arvec2d_t const src)334 void V2f_CopyBoxd(arvec2f_t dest, arvec2d_t const src)
335 {
336 vec2f_t other[2];
337 V2f_Set(other[0], (float) src[0][VX], (float) src[0][VY]);
338 V2f_Set(other[1], (float) src[1][VX], (float) src[1][VY]);
339 V2f_CopyBox(dest, other);
340 }
341
V2d_Set(pvec2d_t vec,vectorcompd_t x,vectorcompd_t y)342 void V2d_Set(pvec2d_t vec, vectorcompd_t x, vectorcompd_t y)
343 {
344 vec[VX] = x;
345 vec[VY] = y;
346 }
347
V2d_SetFixed(pvec2d_t vec,fixed_t x,fixed_t y)348 void V2d_SetFixed(pvec2d_t vec, fixed_t x, fixed_t y)
349 {
350 vec[VX] = FIX2FLT(x);
351 vec[VY] = FIX2FLT(y);
352 }
353
V2d_Length(const_pvec2d_t vec)354 double V2d_Length(const_pvec2d_t vec)
355 {
356 if (vec[VX] == 0 && vec[VY] == 0) return 0;
357 return sqrt(vec[VX] * vec[VX] + vec[VY] * vec[VY]);
358 }
359
V2d_Distance(const_pvec2d_t a,const_pvec2d_t b)360 double V2d_Distance(const_pvec2d_t a, const_pvec2d_t b)
361 {
362 vec2d_t vec;
363 V2d_Subtract(vec, b, a);
364 return V2d_Length(vec);
365 }
366
V2d_Normalize(pvec2d_t vec)367 double V2d_Normalize(pvec2d_t vec)
368 {
369 double len = V2d_Length(vec);
370 if (len != 0)
371 {
372 vec[VX] /= len;
373 vec[VY] /= len;
374 }
375 return len;
376 }
377
V2d_Copy(pvec2d_t dest,const_pvec2d_t src)378 void V2d_Copy(pvec2d_t dest, const_pvec2d_t src)
379 {
380 dest[VX] = src[VX];
381 dest[VY] = src[VY];
382 }
383
V2d_Copyf(pvec2d_t dest,const_pvec2f_t srcf)384 void V2d_Copyf(pvec2d_t dest, const_pvec2f_t srcf)
385 {
386 V2d_Set(dest, srcf[VX], srcf[VY]);
387 }
388
V2d_Scale(pvec2d_t vec,double scalar)389 void V2d_Scale(pvec2d_t vec, double scalar)
390 {
391 vec[VX] *= scalar;
392 vec[VY] *= scalar;
393 }
394
V2d_Rotate(pvec2d_t vec,double radians)395 void V2d_Rotate(pvec2d_t vec, double radians)
396 {
397 double const c = cos(radians);
398 double const s = sin(radians);
399 double const x = c * vec[VX] - s * vec[VY];
400 double const y = s * vec[VX] + c * vec[VY];
401
402 vec[VX] = x;
403 vec[VY] = y;
404 }
405
V2d_Sum(pvec2d_t dest,const_pvec2d_t src1,const_pvec2d_t src2)406 void V2d_Sum(pvec2d_t dest, const_pvec2d_t src1, const_pvec2d_t src2)
407 {
408 dest[VX] = src1[VX] + src2[VX];
409 dest[VY] = src1[VY] + src2[VY];
410 }
411
V2d_Subtract(pvec2d_t dest,const_pvec2d_t src1,const_pvec2d_t src2)412 void V2d_Subtract(pvec2d_t dest, const_pvec2d_t src1, const_pvec2d_t src2)
413 {
414 dest[VX] = src1[VX] - src2[VX];
415 dest[VY] = src1[VY] - src2[VY];
416 }
417
V2d_PointLineDistance(const_pvec2d_t point,const_pvec2d_t linePoint,const_pvec2d_t lineDirection,double * offset)418 double V2d_PointLineDistance(const_pvec2d_t point, const_pvec2d_t linePoint,
419 const_pvec2d_t lineDirection, double *offset)
420 {
421 vec2d_t delta;
422 double len;
423
424 V2d_Subtract(delta, lineDirection, linePoint);
425 len = V2d_Length(delta);
426 if (len == 0)
427 {
428 if (offset) *offset = 0;
429 return 0;
430 }
431
432 if (offset)
433 {
434 *offset = ((linePoint[VY] - point[VY]) * (linePoint[VY] - lineDirection[VY]) -
435 (linePoint[VX] - point[VX]) * (lineDirection[VX] - linePoint[VX]) ) / len;
436 }
437
438 return ((linePoint[VY] - point[VY]) * (lineDirection[VX] - linePoint[VX]) -
439 (linePoint[VX] - point[VX]) * (lineDirection[VY] - linePoint[VY])) / len;
440 }
441
V2d_PointLineParaDistance(const_pvec2d_t point,const_pvec2d_t lineDirection,double linePara,double lineLength)442 double V2d_PointLineParaDistance(const_pvec2d_t point, const_pvec2d_t lineDirection,
443 double linePara, double lineLength)
444 {
445 return (point[VX] * lineDirection[VX] + point[VY] * lineDirection[VY] + linePara) / lineLength;
446 }
447
V2d_PointLinePerpDistance(const_pvec2d_t point,const_pvec2d_t lineDirection,double linePerp,double lineLength)448 double V2d_PointLinePerpDistance(const_pvec2d_t point, const_pvec2d_t lineDirection,
449 double linePerp, double lineLength)
450 {
451 return (point[VX] * lineDirection[VY] - point[VY] * lineDirection[VX] + linePerp) / lineLength;
452 }
453
V2d_PointOnLineSide(const_pvec2d_t point,const_pvec2d_t lineOrigin,const_pvec2d_t lineDirection)454 double V2d_PointOnLineSide(const_pvec2d_t point, const_pvec2d_t lineOrigin, const_pvec2d_t lineDirection)
455 {
456 return (lineOrigin[VY] - point[VY]) * lineDirection[VX] - (lineOrigin[VX] - point[VX]) * lineDirection[VY];
457 }
458
V2d_PointOnLineSide2(const_pvec2d_t point,const_pvec2d_t lineDirection,double linePerp,double lineLength,double epsilon)459 double V2d_PointOnLineSide2(const_pvec2d_t point, const_pvec2d_t lineDirection,
460 double linePerp, double lineLength, double epsilon)
461 {
462 double perp = V2d_PointLinePerpDistance(point, lineDirection, linePerp, lineLength);
463 if (fabs(perp) <= epsilon) return 0;
464 return perp;
465 }
466
V2d_DotProduct(const_pvec2d_t a,const_pvec2d_t b)467 double V2d_DotProduct(const_pvec2d_t a, const_pvec2d_t b)
468 {
469 return a[VX] * b[VX] + a[VY] * b[VY];
470 }
471
V2d_ScalarProject(const_pvec2d_t a,const_pvec2d_t b)472 double V2d_ScalarProject(const_pvec2d_t a, const_pvec2d_t b)
473 {
474 double dot, len = V2d_Length(b);
475 if (len == 0) return 0;
476
477 dot = V2d_DotProduct(a, b);
478 return dot / len;
479 }
480
V2d_Project(pvec2d_t dest,const_pvec2d_t a,const_pvec2d_t b)481 double V2d_Project(pvec2d_t dest, const_pvec2d_t a, const_pvec2d_t b)
482 {
483 double div = V2d_DotProduct(b, b);
484 if (div == 0)
485 {
486 if (dest)
487 {
488 dest[VX] = dest[VY] = 0;
489 }
490 return 0;
491 }
492
493 if (dest)
494 {
495 V2d_Copy(dest, b);
496 V2d_Scale(dest, V2d_DotProduct(a, b) / div);
497 }
498
499 return div;
500 }
501
V2d_ProjectOnLine(pvec2d_t dest,const_pvec2d_t point,const_pvec2d_t lineOrigin,const_pvec2d_t lineDirection)502 double V2d_ProjectOnLine(pvec2d_t dest, const_pvec2d_t point,
503 const_pvec2d_t lineOrigin, const_pvec2d_t lineDirection)
504 {
505 double div = V2d_DotProduct(lineDirection, lineDirection);
506 double pointVec[2];
507
508 if (div == 0)
509 {
510 if (dest)
511 {
512 dest[VX] = dest[VY] = 0;
513 }
514 return 0;
515 }
516
517 V2d_Subtract(pointVec, point, lineOrigin);
518 div = V2d_DotProduct(pointVec, lineDirection) / div;
519
520 if (dest)
521 {
522 dest[VX] = lineOrigin[VX] + lineDirection[VX] * div;
523 dest[VY] = lineOrigin[VY] + lineDirection[VY] * div;
524 }
525
526 return div;
527 }
528
V2d_IsParallel(const_pvec2d_t a,const_pvec2d_t b)529 dd_bool V2d_IsParallel(const_pvec2d_t a, const_pvec2d_t b)
530 {
531 #define EPSILON .99999999
532
533 double aLen = V2d_Length(a);
534 double bLen = V2d_Length(b);
535 double dot;
536
537 // Both must be non-zero vectors.
538 if (aLen == 0 || bLen == 0) return true;
539
540 dot = V2d_DotProduct(a, b) / aLen / bLen;
541
542 // If it's close enough, we'll consider them parallel.
543 return dot > EPSILON || dot < -EPSILON;
544
545 #undef EPSILON
546 }
547
V2d_IsZero(const_pvec2d_t vec)548 dd_bool V2d_IsZero(const_pvec2d_t vec)
549 {
550 return vec[VX] == 0 && vec[VY] == 0;
551 }
552
V2d_Intersection(const_pvec2d_t linePointA,const_pvec2d_t lineDirectionA,const_pvec2d_t linePointB,const_pvec2d_t lineDirectionB,pvec2d_t point)553 double V2d_Intersection(const_pvec2d_t linePointA, const_pvec2d_t lineDirectionA,
554 const_pvec2d_t linePointB, const_pvec2d_t lineDirectionB, pvec2d_t point)
555 {
556 /*
557 * (YA-YC)(XD-XC)-(XA-XC)(YD-YC)
558 * r = -----------------------------
559 * (XB-XA)(YD-YC)-(YB-YA)(XD-XC)
560 */
561
562 double r, div;
563
564 div = lineDirectionA[VX] * lineDirectionB[VY] - lineDirectionA[VY] * lineDirectionB[VX];
565
566 if (div == 0)
567 {
568 // Special case: lines are parallel.
569 r = 0;
570 }
571 else
572 {
573 r = ((linePointA[VY] - linePointB[VY]) * lineDirectionB[VX] -
574 (linePointA[VX] - linePointB[VX]) * lineDirectionB[VY]) / div;
575 }
576
577 /*
578 * XI = XA + r(XB-XA)
579 * YI = YA + r(YB-YA)
580 */
581
582 if (point)
583 {
584 // Calculate the intersection point.
585 point[VX] = linePointA[VX] + r * lineDirectionA[VX];
586 point[VY] = linePointA[VY] + r * lineDirectionA[VY];
587 }
588
589 // Return the scaling factor.
590 return r;
591 }
592
V2d_Intercept(const_pvec2d_t a,const_pvec2d_t b,const_pvec2d_t c,const_pvec2d_t d,pvec2d_t point)593 double V2d_Intercept(const_pvec2d_t a, const_pvec2d_t b, const_pvec2d_t c,
594 const_pvec2d_t d, pvec2d_t point)
595 {
596 vec2d_t ab, cd;
597
598 ab[0] = b[VX] - a[VX];
599 ab[1] = b[VY] - a[VY];
600 cd[0] = d[VX] - c[VX];
601 cd[1] = d[VY] - c[VY];
602
603 return V2d_Intersection(a, ab, c, cd, point);
604 }
605
V2d_Intercept2(const_pvec2d_t a,const_pvec2d_t b,const_pvec2d_t c,const_pvec2d_t d,pvec2d_t point,double * abFrac,double * cdFrac)606 dd_bool V2d_Intercept2(const_pvec2d_t a, const_pvec2d_t b, const_pvec2d_t c,
607 const_pvec2d_t d, pvec2d_t point, double* abFrac, double* cdFrac)
608 {
609 double ab, cd;
610
611 ab = V2d_Intercept(a, b, c, d, point);
612 cd = V2d_Intercept(c, d, a, b, NULL);
613
614 if (abFrac) *abFrac = ab;
615 if (cdFrac) *cdFrac = cd;
616
617 return (ab >= 0 && ab <= 1 && cd >= 0 && cd <= 1);
618 }
619
V2d_Lerp(pvec2d_t dest,const_pvec2d_t a,const_pvec2d_t b,double c)620 void V2d_Lerp(pvec2d_t dest, const_pvec2d_t a, const_pvec2d_t b, double c)
621 {
622 uint i;
623 for (i = 0; i < 2; ++i)
624 {
625 dest[i] = a[i] + c * (b[i] - a[i]);
626 }
627 }
628
V2d_InitBox(arvec2d_t box,const_pvec2d_t point)629 void V2d_InitBox(arvec2d_t box, const_pvec2d_t point)
630 {
631 V2d_Copy(box[0], point);
632 V2d_Copy(box[1], point);
633 }
634
V2d_InitBoxXY(arvec2d_t box,double x,double y)635 void V2d_InitBoxXY(arvec2d_t box, double x, double y)
636 {
637 vec2d_t point; V2d_Set(point, x, y);
638 V2d_InitBox(box, point);
639 }
640
V2d_AddToBox(arvec2d_t box,const_pvec2d_t point)641 void V2d_AddToBox(arvec2d_t box, const_pvec2d_t point)
642 {
643 if (point[VX] < box[0][VX])
644 box[0][VX] = point[VX];
645 if (point[VX] > box[1][VX])
646 box[1][VX] = point[VX];
647
648 if (point[VY] < box[0][VY])
649 box[0][VY] = point[VY];
650 if (point[VY] > box[1][VY])
651 box[1][VY] = point[VY];
652 }
653
V2d_AddToBoxXY(arvec2d_t box,double x,double y)654 void V2d_AddToBoxXY(arvec2d_t box, double x, double y)
655 {
656 vec2d_t point; V2d_Set(point, x, y);
657 V2d_AddToBox(box, point);
658 }
659
V2d_UniteBox(arvec2d_t box,const_arvec2d_t other)660 void V2d_UniteBox(arvec2d_t box, const_arvec2d_t other)
661 {
662 if (other[0][VX] < box[0][VX])
663 box[0][VX] = other[0][VX];
664
665 if (other[1][VX] > box[1][VX])
666 box[1][VX] = other[1][VX];
667
668 if (other[0][VY] < box[0][VY])
669 box[0][VY] = other[0][VY];
670
671 if (other[1][VY] > box[1][VY])
672 box[1][VY] = other[1][VY];
673 }
674
V2d_CopyBox(arvec2d_t dest,const_arvec2d_t src)675 void V2d_CopyBox(arvec2d_t dest, const_arvec2d_t src)
676 {
677 V2d_Copy(dest[0], src[0]);
678 V2d_Copy(dest[1], src[1]);
679 }
680
V3f_Set(pvec3f_t vec,vectorcompf_t x,vectorcompf_t y,vectorcompf_t z)681 void V3f_Set(pvec3f_t vec, vectorcompf_t x, vectorcompf_t y, vectorcompf_t z)
682 {
683 vec[VX] = x;
684 vec[VY] = y;
685 vec[VZ] = z;
686 }
687
V3f_SetFixed(pvec3f_t vec,fixed_t x,fixed_t y,fixed_t z)688 void V3f_SetFixed(pvec3f_t vec, fixed_t x, fixed_t y, fixed_t z)
689 {
690 vec[VX] = FIX2FLT(x);
691 vec[VY] = FIX2FLT(y);
692 vec[VZ] = FIX2FLT(z);
693 }
694
V3f_Length(const_pvec3f_t vec)695 float V3f_Length(const_pvec3f_t vec)
696 {
697 if (vec[VX] == 0 && vec[VY] == 0 && vec[VZ] == 0) return 0;
698 return (float) sqrt(vec[VX] * vec[VX] + vec[VY] * vec[VY] + vec[VZ] * vec[VZ]);
699 }
700
V3f_Distance(const_pvec3f_t a,const_pvec3f_t b)701 float V3f_Distance(const_pvec3f_t a, const_pvec3f_t b)
702 {
703 vec3f_t vec;
704 V3f_Subtract(vec, b, a);
705 return V3f_Length(vec);
706 }
707
V3f_Normalize(pvec3f_t vec)708 float V3f_Normalize(pvec3f_t vec)
709 {
710 float len = V3f_Length(vec);
711 if (len)
712 {
713 vec[VX] /= len;
714 vec[VY] /= len;
715 vec[VZ] /= len;
716 }
717 return len;
718 }
719
V3f_Copy(pvec3f_t dest,const_pvec3f_t src)720 void V3f_Copy(pvec3f_t dest, const_pvec3f_t src)
721 {
722 dest[VX] = src[VX];
723 dest[VY] = src[VY];
724 dest[VZ] = src[VZ];
725 }
726
V3f_Copyd(pvec3f_t dest,const_pvec3d_t src)727 void V3f_Copyd(pvec3f_t dest, const_pvec3d_t src)
728 {
729 V3f_Set(dest, (float) src[VX], (float) src[VY], (float) src[VZ]);
730 }
731
V3f_Scale(pvec3f_t vec,float scalar)732 void V3f_Scale(pvec3f_t vec, float scalar)
733 {
734 vec[VX] *= scalar;
735 vec[VY] *= scalar;
736 vec[VZ] *= scalar;
737 }
738
V3f_Sum(pvec3f_t dest,const_pvec3f_t src1,const_pvec3f_t src2)739 void V3f_Sum(pvec3f_t dest, const_pvec3f_t src1, const_pvec3f_t src2)
740 {
741 dest[VX] = src1[VX] + src2[VX];
742 dest[VY] = src1[VY] + src2[VY];
743 dest[VZ] = src1[VZ] + src2[VZ];
744 }
745
V3f_Subtract(pvec3f_t dest,const_pvec3f_t src1,const_pvec3f_t src2)746 void V3f_Subtract(pvec3f_t dest, const_pvec3f_t src1, const_pvec3f_t src2)
747 {
748 dest[VX] = src1[VX] - src2[VX];
749 dest[VY] = src1[VY] - src2[VY];
750 dest[VZ] = src1[VZ] - src2[VZ];
751 }
752
V3f_DotProduct(const_pvec3f_t a,const_pvec3f_t b)753 float V3f_DotProduct(const_pvec3f_t a, const_pvec3f_t b)
754 {
755 return a[VX] * b[VX] + a[VY] * b[VY] + a[VZ] * b[VZ];
756 }
757
V3f_CrossProduct(pvec3f_t dest,const_pvec3f_t a,const_pvec3f_t b)758 void V3f_CrossProduct(pvec3f_t dest, const_pvec3f_t a, const_pvec3f_t b)
759 {
760 dest[VX] = a[VY] * b[VZ] - a[VZ] * b[VY];
761 dest[VY] = a[VZ] * b[VX] - a[VX] * b[VZ];
762 dest[VZ] = a[VX] * b[VY] - a[VY] * b[VX];
763 }
764
V3f_CrossProductd(pvec3f_t dest,const_pvec3d_t src1d,const_pvec3d_t src2d)765 void V3f_CrossProductd(pvec3f_t dest, const_pvec3d_t src1d, const_pvec3d_t src2d)
766 {
767 vec3f_t src1, src2;
768 V3f_Copyd(src1, src1d);
769 V3f_Copyd(src2, src2d);
770 V3f_CrossProduct(dest, src1, src2);
771 }
772
V3f_PointCrossProduct(pvec3f_t dest,const_pvec3f_t v1,const_pvec3f_t v2,const_pvec3f_t v3)773 void V3f_PointCrossProduct(pvec3f_t dest, const_pvec3f_t v1, const_pvec3f_t v2,
774 const_pvec3f_t v3)
775 {
776 vec3f_t a, b;
777 V3f_Subtract(a, v2, v1);
778 V3f_Subtract(b, v3, v1);
779 V3f_CrossProduct(dest, a, b);
780 }
781
V3f_ClosestPointOnPlane(pvec3f_t dest,const_pvec3f_t planeNormal,const_pvec3f_t planePoint,const_pvec3f_t arbPoint)782 float V3f_ClosestPointOnPlane(pvec3f_t dest, const_pvec3f_t planeNormal,
783 const_pvec3f_t planePoint, const_pvec3f_t arbPoint)
784 {
785 vec3f_t pvec;
786 float distance;
787
788 V3f_Subtract(pvec, arbPoint, planePoint);
789 distance = V3f_DotProduct(planeNormal, pvec);
790
791 V3f_Copy(dest, planeNormal);
792 V3f_Scale(dest, distance);
793 V3f_Subtract(dest, arbPoint, dest);
794
795 return distance;
796 }
797
V3f_MajorAxis(const_pvec3f_t vec)798 int V3f_MajorAxis(const_pvec3f_t vec)
799 {
800 vec3f_t fn;
801 int axis;
802
803 V3f_Set(fn, fabsf(vec[VX]), fabsf(vec[VY]), fabsf(vec[VZ]));
804
805 axis = VX;
806 if (fn[VY] > fn[axis])
807 axis = VY;
808 if (fn[VZ] > fn[axis])
809 axis = VZ;
810
811 return axis;
812 }
813
V3f_IsZero(const_pvec3f_t vec)814 dd_bool V3f_IsZero(const_pvec3f_t vec)
815 {
816 return vec[VX] == 0 && vec[VY] == 0 && vec[VZ] == 0;
817 }
818
V3f_Lerp(pvec3f_t dest,const_pvec3f_t a,const_pvec3f_t b,float c)819 void V3f_Lerp(pvec3f_t dest, const_pvec3f_t a, const_pvec3f_t b, float c)
820 {
821 uint i;
822 for (i = 0; i < 3; ++i)
823 {
824 dest[i] = a[i] + c * (b[i] - a[i]);
825 }
826 }
827
V3f_BuildTangents(pvec3f_t tangent,pvec3f_t bitangent,const_pvec3f_t normal)828 void V3f_BuildTangents(pvec3f_t tangent, pvec3f_t bitangent, const_pvec3f_t normal)
829 {
830 vec3f_t const rotm[3] = {
831 { 0.f, 0.f, 1.f },
832 { 0.f, 0.f, 1.f },
833 { 0.f, 0.f, 1.f }
834 };
835 int axis = VX;
836 vec3f_t fn;
837
838 V3f_Set(fn, fabsf(normal[VX]), fabsf(normal[VY]), fabsf(normal[VZ]));
839
840 if (fn[VY] > fn[axis])
841 axis = VY;
842 if (fn[VZ] > fn[axis])
843 axis = VZ;
844
845 if (fabsf(fn[VX] - 1.0f) < FLT_EPSILON ||
846 fabsf(fn[VY] - 1.0f) < FLT_EPSILON ||
847 fabsf(fn[VZ] - 1.0f) < FLT_EPSILON)
848 {
849 // We must build the tangent vector manually.
850 if (axis == VX && normal[VX] > 0.f)
851 {
852 V3f_Set(tangent, 0.f, 1.f, 0.f);
853 }
854 else if (axis == VX)
855 {
856 V3f_Set(tangent, 0.f, -1.f, 0.f);
857 }
858
859 if (axis == VY && normal[VY] > 0.f)
860 {
861 V3f_Set(tangent, -1.f, 0.f, 0.f);
862 }
863 else if (axis == VY)
864 {
865 V3f_Set(tangent, 1.f, 0.f, 0.f);
866 }
867
868 if (axis == VZ)
869 {
870 V3f_Set(tangent, 1.f, 0.f, 0.f);
871 }
872 }
873 else
874 {
875 // Can use a cross product of the normal.
876 V3f_CrossProduct(tangent, (pvec3f_t)rotm[axis], normal);
877 V3f_Normalize(tangent);
878 }
879
880 V3f_CrossProduct(bitangent, tangent, normal);
881 V3f_Normalize(bitangent);
882 }
883
V3d_Set(pvec3d_t vec,vectorcompd_t x,vectorcompd_t y,vectorcompd_t z)884 void V3d_Set(pvec3d_t vec, vectorcompd_t x, vectorcompd_t y, vectorcompd_t z)
885 {
886 vec[VX] = x;
887 vec[VY] = y;
888 vec[VZ] = z;
889 }
890
V3d_SetFixed(pvec3d_t vec,fixed_t x,fixed_t y,fixed_t z)891 void V3d_SetFixed(pvec3d_t vec, fixed_t x, fixed_t y, fixed_t z)
892 {
893 vec[VX] = FIX2FLT(x);
894 vec[VY] = FIX2FLT(y);
895 vec[VZ] = FIX2FLT(z);
896 }
897
V3d_Length(const_pvec3d_t vec)898 double V3d_Length(const_pvec3d_t vec)
899 {
900 if (vec[VX] == 0 && vec[VY] == 0 && vec[VZ] == 0) return 0;
901 return sqrt(vec[VX] * vec[VX] + vec[VY] * vec[VY] + vec[VZ] * vec[VZ]);
902 }
903
V3d_Distance(const_pvec3d_t a,const_pvec3d_t b)904 double V3d_Distance(const_pvec3d_t a, const_pvec3d_t b)
905 {
906 vec3d_t vec;
907 V3d_Subtract(vec, b, a);
908 return V3d_Length(vec);
909 }
910
V3d_Normalize(pvec3d_t vec)911 double V3d_Normalize(pvec3d_t vec)
912 {
913 double len = V3d_Length(vec);
914 if (len != 0)
915 {
916 vec[VX] /= len;
917 vec[VY] /= len;
918 vec[VZ] /= len;
919 }
920 return len;
921 }
922
V3d_Copy(pvec3d_t dest,const_pvec3d_t src)923 void V3d_Copy(pvec3d_t dest, const_pvec3d_t src)
924 {
925 dest[VX] = src[VX];
926 dest[VY] = src[VY];
927 dest[VZ] = src[VZ];
928 }
929
V3d_Copyf(pvec3d_t dest,const_pvec3f_t srcf)930 void V3d_Copyf(pvec3d_t dest, const_pvec3f_t srcf)
931 {
932 V3d_Set(dest, srcf[VX], srcf[VY], srcf[VZ]);
933 }
934
V3d_Scale(pvec3d_t vec,double scalar)935 void V3d_Scale(pvec3d_t vec, double scalar)
936 {
937 vec[VX] *= scalar;
938 vec[VY] *= scalar;
939 vec[VZ] *= scalar;
940 }
941
V3d_Sum(pvec3d_t dest,const_pvec3d_t src1,const_pvec3d_t src2)942 void V3d_Sum(pvec3d_t dest, const_pvec3d_t src1, const_pvec3d_t src2)
943 {
944 dest[VX] = src1[VX] + src2[VX];
945 dest[VY] = src1[VY] + src2[VY];
946 dest[VZ] = src1[VZ] + src2[VZ];
947 }
948
V3d_Subtract(pvec3d_t dest,const_pvec3d_t src1,const_pvec3d_t src2)949 void V3d_Subtract(pvec3d_t dest, const_pvec3d_t src1, const_pvec3d_t src2)
950 {
951 dest[VX] = src1[VX] - src2[VX];
952 dest[VY] = src1[VY] - src2[VY];
953 dest[VZ] = src1[VZ] - src2[VZ];
954 }
955
V3d_DotProduct(const_pvec3d_t a,const_pvec3d_t b)956 double V3d_DotProduct(const_pvec3d_t a, const_pvec3d_t b)
957 {
958 return a[VX] * b[VX] + a[VY] * b[VY] + a[VZ] * b[VZ];
959 }
960
V3d_DotProductf(const_pvec3d_t a,const_pvec3f_t bf)961 double V3d_DotProductf(const_pvec3d_t a, const_pvec3f_t bf)
962 {
963 vec3d_t b;
964 V3d_Copyf(b, bf);
965 return V3d_DotProduct(a, b);
966 }
967
V3d_CrossProduct(pvec3d_t dest,const_pvec3d_t src1,const_pvec3d_t src2)968 void V3d_CrossProduct(pvec3d_t dest, const_pvec3d_t src1, const_pvec3d_t src2)
969 {
970 dest[VX] = src1[VY] * src2[VZ] - src1[VZ] * src2[VY];
971 dest[VY] = src1[VZ] * src2[VX] - src1[VX] * src2[VZ];
972 dest[VZ] = src1[VX] * src2[VY] - src1[VY] * src2[VX];
973 }
974
V3d_PointCrossProduct(pvec3d_t dest,const_pvec3d_t v1,const_pvec3d_t v2,const_pvec3d_t v3)975 void V3d_PointCrossProduct(pvec3d_t dest, const_pvec3d_t v1, const_pvec3d_t v2,
976 const_pvec3d_t v3)
977 {
978 vec3d_t a, b;
979 V3d_Subtract(a, v2, v1);
980 V3d_Subtract(b, v3, v1);
981 V3d_CrossProduct(dest, a, b);
982 }
983
V3d_ClosestPointOnPlane(pvec3d_t dest,const_pvec3d_t planeNormal,const_pvec3d_t planePoint,const_pvec3d_t arbPoint)984 double V3d_ClosestPointOnPlane(pvec3d_t dest, const_pvec3d_t planeNormal,
985 const_pvec3d_t planePoint, const_pvec3d_t arbPoint)
986 {
987 vec3d_t pvec;
988 double distance;
989
990 V3d_Subtract(pvec, arbPoint, planePoint);
991 distance = V3d_DotProduct(planeNormal, pvec);
992
993 V3d_Copy(dest, planeNormal);
994 V3d_Scale(dest, distance);
995 V3d_Subtract(dest, arbPoint, dest);
996
997 return distance;
998 }
999
V3d_ClosestPointOnPlanef(pvec3d_t dest,const_pvec3f_t planeNormalf,const_pvec3d_t planePoint,const_pvec3d_t arbPoint)1000 double V3d_ClosestPointOnPlanef(pvec3d_t dest, const_pvec3f_t planeNormalf,
1001 const_pvec3d_t planePoint, const_pvec3d_t arbPoint)
1002 {
1003 vec3d_t planeNormal;
1004 V3d_Copyf(planeNormal, planeNormalf);
1005 return V3d_ClosestPointOnPlane(dest, planeNormal, planePoint, arbPoint);
1006 }
1007
V3d_MajorAxis(const_pvec3d_t vec)1008 int V3d_MajorAxis(const_pvec3d_t vec)
1009 {
1010 vec3d_t fn;
1011 int axis;
1012
1013 V3d_Set(fn, fabs(vec[VX]), fabs(vec[VY]), fabs(vec[VZ]));
1014
1015 axis = VX;
1016 if (fn[VY] > fn[axis])
1017 axis = VY;
1018 if (fn[VZ] > fn[axis])
1019 axis = VZ;
1020
1021 return axis;
1022 }
1023
V3d_IsZero(const_pvec3d_t vec)1024 dd_bool V3d_IsZero(const_pvec3d_t vec)
1025 {
1026 return vec[VX] == 0 && vec[VY] == 0 && vec[VZ] == 0;
1027 }
1028
V3d_Lerp(pvec3d_t dest,const_pvec3d_t a,const_pvec3d_t b,double c)1029 void V3d_Lerp(pvec3d_t dest, const_pvec3d_t a, const_pvec3d_t b, double c)
1030 {
1031 uint i;
1032 for (i = 0; i < 3; ++i)
1033 {
1034 dest[i] = a[i] + c * (b[i] - a[i]);
1035 }
1036 }
1037
V3d_BuildTangents(pvec3d_t tangent,pvec3d_t bitangent,const_pvec3d_t normal)1038 void V3d_BuildTangents(pvec3d_t tangent, pvec3d_t bitangent, const_pvec3d_t normal)
1039 {
1040 vec3d_t const rotm[3] = {
1041 { 0.f, 0.f, 1.f },
1042 { 0.f, 0.f, 1.f },
1043 { 0.f, 0.f, 1.f }
1044 };
1045 int axis = VX;
1046 vec3d_t fn;
1047
1048 V3d_Set(fn, fabs(normal[VX]), fabs(normal[VY]), fabs(normal[VZ]));
1049
1050 if (fn[VY] > fn[axis])
1051 axis = VY;
1052 if (fn[VZ] > fn[axis])
1053 axis = VZ;
1054
1055 if (fabs(fn[VX] - 1.0) < FLT_EPSILON ||
1056 fabs(fn[VY] - 1.0) < FLT_EPSILON ||
1057 fabs(fn[VZ] - 1.0) < FLT_EPSILON)
1058 {
1059 // We must build the tangent vector manually.
1060 if (axis == VX && normal[VX] > 0.f)
1061 {
1062 V3d_Set(tangent, 0.f, 1.f, 0.f);
1063 }
1064 else if (axis == VX)
1065 {
1066 V3d_Set(tangent, 0.f, -1.f, 0.f);
1067 }
1068
1069 if (axis == VY && normal[VY] > 0.f)
1070 {
1071 V3d_Set(tangent, -1.f, 0.f, 0.f);
1072 }
1073 else if (axis == VY)
1074 {
1075 V3d_Set(tangent, 1.f, 0.f, 0.f);
1076 }
1077
1078 if (axis == VZ)
1079 {
1080 V3d_Set(tangent, 1.f, 0.f, 0.f);
1081 }
1082 }
1083 else
1084 {
1085 // Can use a cross product of the normal.
1086 V3d_CrossProduct(tangent, (pvec3d_t)rotm[axis], normal);
1087 V3d_Normalize(tangent);
1088 }
1089
1090 V3d_CrossProduct(bitangent, tangent, normal);
1091 V3d_Normalize(bitangent);
1092 }
1093
V4f_Set(pvec4f_t vec,vectorcompf_t x,vectorcompf_t y,vectorcompf_t z,vectorcompf_t w)1094 void V4f_Set(pvec4f_t vec, vectorcompf_t x, vectorcompf_t y, vectorcompf_t z, vectorcompf_t w)
1095 {
1096 vec[0] = x;
1097 vec[1] = y;
1098 vec[2] = z;
1099 vec[3] = w;
1100 }
1101
V4f_SetFixed(pvec4f_t vec,fixed_t x,fixed_t y,fixed_t z,fixed_t w)1102 void V4f_SetFixed(pvec4f_t vec, fixed_t x, fixed_t y, fixed_t z, fixed_t w)
1103 {
1104 vec[0] = FIX2FLT(x);
1105 vec[1] = FIX2FLT(y);
1106 vec[2] = FIX2FLT(z);
1107 vec[3] = FIX2FLT(w);
1108 }
1109
V4f_Length(const_pvec4f_t vec)1110 float V4f_Length(const_pvec4f_t vec)
1111 {
1112 if (vec[0] == 0 && vec[1] == 0 && vec[2] == 0 && vec[3] == 0) return 0;
1113 return (float) sqrt(vec[0] * vec[0] + vec[1] * vec[1] +
1114 vec[2] * vec[2] + vec[3] * vec[3]);
1115 }
1116
V4f_Distance(const_pvec4f_t a,const_pvec4f_t b)1117 float V4f_Distance(const_pvec4f_t a, const_pvec4f_t b)
1118 {
1119 vec4f_t vec;
1120 V4f_Subtract(vec, b, a);
1121 return V4f_Length(vec);
1122 }
1123
V4f_Normalize(pvec4f_t vec)1124 float V4f_Normalize(pvec4f_t vec)
1125 {
1126 float len = V4f_Length(vec);
1127 if (len != 0)
1128 {
1129 vec[0] /= len;
1130 vec[1] /= len;
1131 vec[2] /= len;
1132 vec[3] /= len;
1133 }
1134 return len;
1135 }
1136
V4f_Copy(pvec4f_t dest,const_pvec4f_t src)1137 void V4f_Copy(pvec4f_t dest, const_pvec4f_t src)
1138 {
1139 dest[0] = src[0];
1140 dest[1] = src[1];
1141 dest[2] = src[2];
1142 dest[3] = src[3];
1143 }
1144
V4f_Scale(pvec4f_t vec,float scalar)1145 void V4f_Scale(pvec4f_t vec, float scalar)
1146 {
1147 vec[0] *= scalar;
1148 vec[1] *= scalar;
1149 vec[2] *= scalar;
1150 vec[3] *= scalar;
1151 }
1152
V4f_Sum(pvec4f_t dest,const_pvec4f_t src1,const_pvec4f_t src2)1153 void V4f_Sum(pvec4f_t dest, const_pvec4f_t src1, const_pvec4f_t src2)
1154 {
1155 dest[0] = src1[0] + src2[0];
1156 dest[1] = src1[1] + src2[1];
1157 dest[2] = src1[2] + src2[2];
1158 dest[3] = src1[3] + src2[3];
1159 }
1160
V4f_Subtract(pvec4f_t dest,const_pvec4f_t src1,const_pvec4f_t src2)1161 void V4f_Subtract(pvec4f_t dest, const_pvec4f_t src1, const_pvec4f_t src2)
1162 {
1163 dest[0] = src1[0] - src2[0];
1164 dest[1] = src1[1] - src2[1];
1165 dest[2] = src1[2] - src2[2];
1166 dest[3] = src1[3] - src2[3];
1167 }
1168
V4f_IsZero(const_pvec4f_t vec)1169 dd_bool V4f_IsZero(const_pvec4f_t vec)
1170 {
1171 return vec[0] == 0 && vec[1] == 0 && vec[2] == 0 && vec[3] == 0;
1172 }
1173
V4f_Lerp(pvec4f_t dest,const_pvec4f_t a,const_pvec4f_t b,float c)1174 void V4f_Lerp(pvec4f_t dest, const_pvec4f_t a, const_pvec4f_t b, float c)
1175 {
1176 uint i;
1177 for (i = 0; i < 4; ++i)
1178 {
1179 dest[i] = a[i] + c * (b[i] - a[i]);
1180 }
1181 }
1182
V4d_Set(pvec4d_t vec,vectorcompd_t x,vectorcompd_t y,vectorcompd_t z,vectorcompd_t w)1183 void V4d_Set(pvec4d_t vec, vectorcompd_t x, vectorcompd_t y, vectorcompd_t z, vectorcompd_t w)
1184 {
1185 vec[0] = x;
1186 vec[1] = y;
1187 vec[2] = z;
1188 vec[3] = w;
1189 }
1190
V4d_SetFixed(pvec4d_t vec,fixed_t x,fixed_t y,fixed_t z,fixed_t w)1191 void V4d_SetFixed(pvec4d_t vec, fixed_t x, fixed_t y, fixed_t z, fixed_t w)
1192 {
1193 vec[0] = FIX2FLT(x);
1194 vec[1] = FIX2FLT(y);
1195 vec[2] = FIX2FLT(z);
1196 vec[3] = FIX2FLT(w);
1197 }
1198
V4d_Length(const_pvec4d_t vec)1199 double V4d_Length(const_pvec4d_t vec)
1200 {
1201 if (vec[0] == 0 && vec[1] == 0 && vec[2] == 0 && vec[3] == 0) return 0;
1202 return sqrt(vec[0] * vec[0] + vec[1] * vec[1] +
1203 vec[2] * vec[2] + vec[3] * vec[3]);
1204 }
1205
V4d_Distance(const_pvec4d_t a,const_pvec4d_t b)1206 double V4d_Distance(const_pvec4d_t a, const_pvec4d_t b)
1207 {
1208 vec4d_t vec;
1209 V4d_Subtract(vec, b, a);
1210 return V4d_Length(vec);
1211 }
1212
V4d_Normalize(pvec4d_t vec)1213 double V4d_Normalize(pvec4d_t vec)
1214 {
1215 double len = V4d_Length(vec);
1216 if (len != 0)
1217 {
1218 vec[0] /= len;
1219 vec[1] /= len;
1220 vec[2] /= len;
1221 vec[3] /= len;
1222 }
1223 return len;
1224 }
1225
V4d_Copy(pvec4d_t dest,const_pvec4d_t src)1226 void V4d_Copy(pvec4d_t dest, const_pvec4d_t src)
1227 {
1228 dest[0] = src[0];
1229 dest[1] = src[1];
1230 dest[2] = src[2];
1231 dest[3] = src[3];
1232 }
1233
V4d_Scale(pvec4d_t vec,double scalar)1234 void V4d_Scale(pvec4d_t vec, double scalar)
1235 {
1236 vec[0] *= scalar;
1237 vec[1] *= scalar;
1238 vec[2] *= scalar;
1239 vec[3] *= scalar;
1240 }
1241
V4d_Sum(pvec4d_t dest,const_pvec4d_t src1,const_pvec4d_t src2)1242 void V4d_Sum(pvec4d_t dest, const_pvec4d_t src1, const_pvec4d_t src2)
1243 {
1244 dest[0] = src1[0] + src2[0];
1245 dest[1] = src1[1] + src2[1];
1246 dest[2] = src1[2] + src2[2];
1247 dest[3] = src1[3] + src2[3];
1248 }
1249
V4d_Subtract(pvec4d_t dest,const_pvec4d_t src1,const_pvec4d_t src2)1250 void V4d_Subtract(pvec4d_t dest, const_pvec4d_t src1, const_pvec4d_t src2)
1251 {
1252 dest[0] = src1[0] - src2[0];
1253 dest[1] = src1[1] - src2[1];
1254 dest[2] = src1[2] - src2[2];
1255 dest[3] = src1[3] - src2[3];
1256 }
1257
V4d_IsZero(const_pvec4d_t vec)1258 dd_bool V4d_IsZero(const_pvec4d_t vec)
1259 {
1260 return vec[0] == 0 && vec[1] == 0 && vec[2] == 0 && vec[3] == 0;
1261 }
1262
V4d_Lerp(pvec4d_t dest,const_pvec4d_t a,const_pvec4d_t b,double c)1263 void V4d_Lerp(pvec4d_t dest, const_pvec4d_t a, const_pvec4d_t b, double c)
1264 {
1265 uint i;
1266 for (i = 0; i < 4; ++i)
1267 {
1268 dest[i] = a[i] + c * (b[i] - a[i]);
1269 }
1270 }
1271