1 /*
2 * Copyright (C) Volition, Inc. 1999. All rights reserved.
3 *
4 * All source code herein is the property of Volition, Inc. You may not sell
5 * or otherwise commercially exploit the source or things you created based on the
6 * source.
7 *
8 */
9
10
11
12 #include <cstdio>
13 #if _M_IX86_FP >= 1
14 #include <xmmintrin.h>
15 #endif
16
17 #include "math/vecmat.h"
18 #include "utils/RandomRange.h"
19
20
21 #define SMALL_NUM 1e-7
22 #define SMALLER_NUM 1e-20
23 #define CONVERT_RADIANS 0.017453 // conversion factor from degrees to radians
24
25 vec3d vmd_zero_vector = ZERO_VECTOR;
26 vec3d vmd_scale_identity_vector = SCALE_IDENTITY_VECTOR;
27 vec3d vmd_x_vector = { { { 1.0f, 0.0f, 0.0f } } };
28 vec3d vmd_y_vector = { { { 0.0f, 1.0f, 0.0f } } };
29 vec3d vmd_z_vector = { { { 0.0f, 0.0f, 1.0f } } };
30 matrix vmd_zero_matrix = ZERO_MATRIX;
31 matrix4 vmd_zero_matrix4 = ZERO_MATRIX4;
32 matrix vmd_identity_matrix = IDENTITY_MATRIX;
33 angles vmd_zero_angles = { 0.0f, 0.0f, 0.0f };
34
35 #define UNINITIALIZED_VALUE -12345678.9f
36
vm_vec_equal(const vec4 & self,const vec4 & other)37 bool vm_vec_equal(const vec4 &self, const vec4 &other)
38 {
39 return fl_equal(self.a1d[0], other.a1d[0]) && fl_equal(self.a1d[1], other.a1d[1]) && fl_equal(self.a1d[2], other.a1d[2]) && fl_equal(self.a1d[3], other.a1d[3]);
40 }
41
vm_vec_equal(const vec3d & self,const vec3d & other)42 bool vm_vec_equal(const vec3d &self, const vec3d &other)
43 {
44 return fl_equal(self.a1d[0], other.a1d[0]) && fl_equal(self.a1d[1], other.a1d[1]) && fl_equal(self.a1d[2], other.a1d[2]);
45 }
46
vm_vec_equal(const vec2d & self,const vec2d & other)47 bool vm_vec_equal(const vec2d &self, const vec2d &other)
48 {
49 return fl_equal(self.x, other.x) && fl_equal(self.y, other.y);
50 }
51
vm_matrix_equal(const matrix & self,const matrix & other)52 bool vm_matrix_equal(const matrix &self, const matrix &other)
53 {
54 return vm_vec_equal(self.vec.fvec, other.vec.fvec) && vm_vec_equal(self.vec.uvec, other.vec.uvec) && vm_vec_equal(self.vec.rvec, other.vec.rvec);
55 }
56
vm_matrix_equal(const matrix4 & self,const matrix4 & other)57 bool vm_matrix_equal(const matrix4 &self, const matrix4 &other)
58 {
59 return vm_vec_equal(self.vec.fvec, other.vec.fvec) &&
60 vm_vec_equal(self.vec.rvec, other.vec.rvec) &&
61 vm_vec_equal(self.vec.uvec, other.vec.uvec) &&
62 vm_vec_equal(self.vec.pos, other.vec.pos);
63 }
64
65 // -----------------------------------------------------------
66 // atan2_safe()
67 //
68 // Wrapper around atan2() that used atanf() to calculate angle. Safe
69 // for optimized builds. Handles special cases when x == 0.
70 //
atan2_safe(float y,float x)71 float atan2_safe(float y, float x)
72 {
73 float ang;
74
75 // special case, x == 0
76 if ( x == 0.0f ) {
77 if ( y == 0.0f )
78 ang = 0.0f;
79 else if ( y > 0.0f )
80 ang = PI_2;
81 else
82 ang = -PI_2;
83
84 return ang;
85 }
86
87 ang = atanf(y/x);
88 if ( x < 0.0f ){
89 ang += PI;
90 }
91
92 return ang;
93 }
94
95 // ---------------------------------------------------------------------
96 // vm_vec_component()
97 //
98 // finds projection of a vector along a unit (normalized) vector
99 //
vm_vec_projection_parallel(vec3d * component,const vec3d * src,const vec3d * unit_vec)100 float vm_vec_projection_parallel(vec3d *component, const vec3d *src, const vec3d *unit_vec)
101 {
102 float mag;
103 Assertion(vm_vec_is_normalized(unit_vec), "unit_vec must be normalized!");
104
105 mag = vm_vec_dot(src, unit_vec);
106 vm_vec_copy_scale(component, unit_vec, mag);
107 return mag;
108 }
109
110 // ---------------------------------------------------------------------
111 // vm_vec_projection_onto_plane()
112 //
113 // finds projection of a vector onto a plane specified by a unit normal vector
114 //
vm_vec_projection_onto_plane(vec3d * projection,const vec3d * src,const vec3d * unit_normal)115 void vm_vec_projection_onto_plane(vec3d *projection, const vec3d *src, const vec3d *unit_normal)
116 {
117 float mag;
118 Assertion(vm_vec_is_normalized(unit_normal), "unit_normal must be normalized!");
119
120 mag = vm_vec_dot(src, unit_normal);
121 *projection = *src;
122 vm_vec_scale_add2(projection, unit_normal, -mag);
123 }
124
125 // ---------------------------------------------------------------------
126 // vm_vec_project_point_onto_plane()
127 //
128 // finds the point on a plane closest to a given point
129 // moves the point in the direction of the plane normal until it is on the plane
130 //
vm_project_point_onto_plane(vec3d * new_point,const vec3d * point,const vec3d * plane_normal,const vec3d * plane_point)131 void vm_project_point_onto_plane(vec3d *new_point, const vec3d *point, const vec3d *plane_normal, const vec3d *plane_point)
132 {
133 float D; // plane constant in Ax+By+Cz+D = 0 or dot(X,n) - dot(Xp,n) = 0, so D = -dot(Xp,n)
134 float dist;
135 Assertion(vm_vec_is_normalized(plane_normal), "plane_normal must be normalized!");
136
137 D = -vm_vec_dot(plane_point, plane_normal);
138 dist = vm_vec_dot(point, plane_normal) + D;
139
140 *new_point = *point;
141 vm_vec_scale_add2(new_point, plane_normal, -dist);
142 }
143
144 // Take abs(x), then sqrt. Could insert warning message if desired.
asqrt(float x)145 float asqrt(float x)
146 {
147 if (x < 0.0f)
148 return fl_sqrt(-x);
149 else
150 return fl_sqrt(x);
151 }
152
vm_set_identity(matrix * m)153 void vm_set_identity(matrix *m)
154 {
155 m->vec.rvec.xyz.x = 1.0f; m->vec.rvec.xyz.y = 0.0f; m->vec.rvec.xyz.z = 0.0f;
156 m->vec.uvec.xyz.x = 0.0f; m->vec.uvec.xyz.y = 1.0f; m->vec.uvec.xyz.z = 0.0f;
157 m->vec.fvec.xyz.x = 0.0f; m->vec.fvec.xyz.y = 0.0f; m->vec.fvec.xyz.z = 1.0f;
158 }
159
vm_vec_new(float x,float y,float z)160 vec3d vm_vec_new(float x, float y, float z)
161 {
162 vec3d vec;
163
164 vec.xyz.x = x;
165 vec.xyz.y = y;
166 vec.xyz.z = z;
167
168 return vec;
169 }
170
171 //adds two vectors, fills in dest, returns ptr to dest
172 //ok for dest to equal either source, but should use vm_vec_add2() if so
173 //dest = src0 + src1
vm_vec_add(vec3d * dest,const vec3d * src0,const vec3d * src1)174 void vm_vec_add(vec3d *dest, const vec3d *src0, const vec3d *src1)
175 {
176 dest->xyz.x = src0->xyz.x + src1->xyz.x;
177 dest->xyz.y = src0->xyz.y + src1->xyz.y;
178 dest->xyz.z = src0->xyz.z + src1->xyz.z;
179 }
180
181 //subs two vectors, fills in dest, returns ptr to dest
182 //ok for dest to equal either source, but should use vm_vec_sub2() if so
183 //dest = src0 - src1
vm_vec_sub(vec3d * dest,const vec3d * src0,const vec3d * src1)184 void vm_vec_sub(vec3d *dest, const vec3d *src0, const vec3d *src1)
185 {
186 dest->xyz.x = src0->xyz.x - src1->xyz.x;
187 dest->xyz.y = src0->xyz.y - src1->xyz.y;
188 dest->xyz.z = src0->xyz.z - src1->xyz.z;
189 }
190
191
192 //adds one vector to another. returns ptr to dest
193 //dest can equal source
194 //dest += src
vm_vec_add2(vec3d * dest,const vec3d * src)195 void vm_vec_add2(vec3d *dest, const vec3d *src)
196 {
197 dest->xyz.x += src->xyz.x;
198 dest->xyz.y += src->xyz.y;
199 dest->xyz.z += src->xyz.z;
200 }
201
202 //subs one vector from another, returns ptr to dest
203 //dest can equal source
204 //dest -= src
vm_vec_sub2(vec3d * dest,const vec3d * src)205 void vm_vec_sub2(vec3d *dest, const vec3d *src)
206 {
207 dest->xyz.x -= src->xyz.x;
208 dest->xyz.y -= src->xyz.y;
209 dest->xyz.z -= src->xyz.z;
210 }
211
212 //averages n vectors. returns ptr to dest
213 //dest can equal any vector in src[]
214 //dest = sum(src[]) / n
vm_vec_avg_n(vec3d * dest,int n,const vec3d src[])215 vec3d *vm_vec_avg_n(vec3d *dest, int n, const vec3d src[])
216 {
217 float x = 0.0f, y = 0.0f, z = 0.0f;
218 float inv_n = 1.0f / (float) n;;
219
220 for(int i = 0; i<n; i++){
221 x += src[i].xyz.x;
222 y += src[i].xyz.y;
223 z += src[i].xyz.z;
224 }
225
226 dest->xyz.x = x * inv_n;
227 dest->xyz.y = y * inv_n;
228 dest->xyz.z = z * inv_n;
229
230 return dest;
231 }
232
233
234 //averages two vectors. returns ptr to dest
235 //dest can equal either source
236 //dest = (src0 + src1) * 0.5
vm_vec_avg(vec3d * dest,const vec3d * src0,const vec3d * src1)237 vec3d *vm_vec_avg(vec3d *dest, const vec3d *src0, const vec3d *src1)
238 {
239 dest->xyz.x = (src0->xyz.x + src1->xyz.x) * 0.5f;
240 dest->xyz.y = (src0->xyz.y + src1->xyz.y) * 0.5f;
241 dest->xyz.z = (src0->xyz.z + src1->xyz.z) * 0.5f;
242
243 return dest;
244 }
245
246 //averages three vectors. returns ptr to dest
247 //dest can equal any source
248 //dest = (src0 + src1 + src2) *0.33
vm_vec_avg3(vec3d * dest,const vec3d * src0,const vec3d * src1,const vec3d * src2)249 vec3d *vm_vec_avg3(vec3d *dest, const vec3d *src0, const vec3d *src1, const vec3d *src2)
250 {
251 dest->xyz.x = (src0->xyz.x + src1->xyz.x + src2->xyz.x) * 0.333333333f;
252 dest->xyz.y = (src0->xyz.y + src1->xyz.y + src2->xyz.y) * 0.333333333f;
253 dest->xyz.z = (src0->xyz.z + src1->xyz.z + src2->xyz.z) * 0.333333333f;
254 return dest;
255 }
256
257 //averages four vectors. returns ptr to dest
258 //dest can equal any source
259 //dest = (src0 + src1 + src2 + src3) * 0.25
vm_vec_avg4(vec3d * dest,const vec3d * src0,const vec3d * src1,const vec3d * src2,const vec3d * src3)260 vec3d *vm_vec_avg4(vec3d *dest, const vec3d *src0, const vec3d *src1, const vec3d *src2, const vec3d *src3)
261 {
262 dest->xyz.x = (src0->xyz.x + src1->xyz.x + src2->xyz.x + src3->xyz.x) * 0.25f;
263 dest->xyz.y = (src0->xyz.y + src1->xyz.y + src2->xyz.y + src3->xyz.y) * 0.25f;
264 dest->xyz.z = (src0->xyz.z + src1->xyz.z + src2->xyz.z + src3->xyz.z) * 0.25f;
265 return dest;
266 }
267
268
269 //scales a vector in place.
270 //dest *= s
vm_vec_scale(vec3d * dest,float s)271 void vm_vec_scale(vec3d *dest, float s)
272 {
273 dest->xyz.x = dest->xyz.x * s;
274 dest->xyz.y = dest->xyz.y * s;
275 dest->xyz.z = dest->xyz.z * s;
276 }
277
278 //scales a 4-component vector in place.
279 // dest *= s
vm_vec_scale(vec4 * dest,float s)280 void vm_vec_scale(vec4 *dest, float s)
281 {
282 dest->xyzw.x = dest->xyzw.x * s;
283 dest->xyzw.y = dest->xyzw.y * s;
284 dest->xyzw.z = dest->xyzw.z * s;
285 dest->xyzw.w = dest->xyzw.w * s;
286 }
287
288 //scales and copies a vector.
289 // dest = src * s
vm_vec_copy_scale(vec3d * dest,const vec3d * src,float s)290 void vm_vec_copy_scale(vec3d *dest, const vec3d *src, float s)
291 {
292 dest->xyz.x = src->xyz.x*s;
293 dest->xyz.y = src->xyz.y*s;
294 dest->xyz.z = src->xyz.z*s;
295 }
296
297 //scales a vector, adds it to another, and stores in a 3rd vector
298 //dest = src1 + k * src2
vm_vec_scale_add(vec3d * dest,const vec3d * src1,const vec3d * src2,float k)299 void vm_vec_scale_add(vec3d *dest, const vec3d *src1, const vec3d *src2, float k)
300 {
301 dest->xyz.x = src1->xyz.x + src2->xyz.x*k;
302 dest->xyz.y = src1->xyz.y + src2->xyz.y*k;
303 dest->xyz.z = src1->xyz.z + src2->xyz.z*k;
304 }
305
306 //scales a vector, subtracts it from another, and stores in a 3rd vector
307 //dest = src1 - (k * src2)
vm_vec_scale_sub(vec3d * dest,const vec3d * src1,const vec3d * src2,float k)308 void vm_vec_scale_sub(vec3d *dest, const vec3d *src1, const vec3d *src2, float k)
309 {
310 dest->xyz.x = src1->xyz.x - src2->xyz.x*k;
311 dest->xyz.y = src1->xyz.y - src2->xyz.y*k;
312 dest->xyz.z = src1->xyz.z - src2->xyz.z*k;
313 }
314
315 //scales a vector and adds it to another
316 //dest += k * src
vm_vec_scale_add2(vec3d * dest,const vec3d * src,float k)317 void vm_vec_scale_add2(vec3d *dest, const vec3d *src, float k)
318 {
319 dest->xyz.x += src->xyz.x*k;
320 dest->xyz.y += src->xyz.y*k;
321 dest->xyz.z += src->xyz.z*k;
322 }
323
324 //scales a vector and subtracts it from another
325 //dest -= k * src
vm_vec_scale_sub2(vec3d * dest,const vec3d * src,float k)326 void vm_vec_scale_sub2(vec3d *dest, const vec3d *src, float k)
327 {
328 dest->xyz.x -= src->xyz.x*k;
329 dest->xyz.y -= src->xyz.y*k;
330 dest->xyz.z -= src->xyz.z*k;
331 }
332
333 //scales a vector in place, taking n/d for scale.
334 //dest *= n/d
vm_vec_scale2(vec3d * dest,float n,float d)335 void vm_vec_scale2(vec3d *dest, float n, float d)
336 {
337 d = 1.0f/d;
338
339 dest->xyz.x = dest->xyz.x* n * d;
340 dest->xyz.y = dest->xyz.y* n * d;
341 dest->xyz.z = dest->xyz.z* n * d;
342 }
343
344 //returns dot product of 2 vectors
vm_vec_dot(const vec3d * v0,const vec3d * v1)345 float vm_vec_dot(const vec3d *v0, const vec3d *v1)
346 {
347 return (v1->xyz.x*v0->xyz.x)+(v1->xyz.y*v0->xyz.y)+(v1->xyz.z*v0->xyz.z);
348 }
349
350
351 //returns dot product of <x,y,z> and vector
vm_vec_dot3(float x,float y,float z,const vec3d * v)352 float vm_vec_dot3(float x, float y, float z, const vec3d *v)
353 {
354 return (x*v->xyz.x)+(y*v->xyz.y)+(z*v->xyz.z);
355 }
356
357 //returns magnitude of a vector
vm_vec_mag(const vec3d * v)358 float vm_vec_mag(const vec3d *v)
359 {
360 float mag1;
361
362 mag1 = (v->xyz.x * v->xyz.x) + (v->xyz.y * v->xyz.y) + (v->xyz.z * v->xyz.z);
363
364 if (mag1 <= 0.0f) {
365 return 0.0f;
366 }
367
368 return fl_sqrt(mag1);
369 }
370
371 //returns squared magnitude of a vector, useful if you want to compare distances
vm_vec_mag_squared(const vec3d * v)372 float vm_vec_mag_squared(const vec3d *v)
373 {
374 return ((v->xyz.x * v->xyz.x) + (v->xyz.y * v->xyz.y) + (v->xyz.z * v->xyz.z));
375 }
376
377 //returns the square of the difference between v0 and v1 (the distance, squared)
378 //just like vm_vec_mag_squared, but the distance between two points instead.
vm_vec_dist_squared(const vec3d * v0,const vec3d * v1)379 float vm_vec_dist_squared(const vec3d *v0, const vec3d *v1)
380 {
381 float dx, dy, dz;
382
383 dx = v0->xyz.x - v1->xyz.x;
384 dy = v0->xyz.y - v1->xyz.y;
385 dz = v0->xyz.z - v1->xyz.z;
386 return dx*dx + dy*dy + dz*dz;
387 }
388
389 //computes the distance between two points. (does sub and mag)
vm_vec_dist(const vec3d * v0,const vec3d * v1)390 float vm_vec_dist(const vec3d *v0, const vec3d *v1)
391 {
392 float t1;
393 vec3d t;
394
395 vm_vec_sub(&t,v0,v1);
396
397 t1 = vm_vec_mag(&t);
398
399 return t1;
400 }
401
vm_vec_is_normalized(const vec3d * v)402 bool vm_vec_is_normalized(const vec3d *v)
403 {
404 // By the standards of FSO, it is sufficient to check that the magnitude is close to 1.
405 return vm_vec_mag(v) > 0.999f && vm_vec_mag(v) < 1.001f;
406 }
407
408 //normalize a vector. returns mag of source vec (always greater than zero)
vm_vec_copy_normalize(vec3d * dest,const vec3d * src)409 float vm_vec_copy_normalize(vec3d *dest, const vec3d *src)
410 {
411 float m;
412
413 m = vm_vec_mag(src);
414
415 // Mainly here to trap attempts to normalize a null vector.
416 if (m <= 0.0f) {
417 mprintf(("Null vec3d in vec3d normalize.\n"
418 "Trace out of vecmat.cpp and find offending code.\n"));
419
420 dest->xyz.x = 1.0f;
421 dest->xyz.y = 0.0f;
422 dest->xyz.z = 0.0f;
423
424 return 1.0f;
425 }
426
427 float im = 1.0f / m;
428
429 dest->xyz.x = src->xyz.x * im;
430 dest->xyz.y = src->xyz.y * im;
431 dest->xyz.z = src->xyz.z * im;
432
433 return m;
434 }
435
436 //normalize a vector. returns mag of source vec (always greater than zero)
vm_vec_normalize(vec3d * v)437 float vm_vec_normalize(vec3d *v)
438 {
439 float t;
440 t = vm_vec_copy_normalize(v,v);
441 return t;
442 }
443
444 // Normalize a vector.
445 // If vector is 0,0,0, return 1.0f, and change v to 1,0,0.
446 // Otherwise return the magnitude.
447 // No warning() generated for null vector.
vm_vec_normalize_safe(vec3d * v)448 float vm_vec_normalize_safe(vec3d *v)
449 {
450 float m;
451
452 m = vm_vec_mag(v);
453
454 // Mainly here to trap attempts to normalize a null vector.
455 if (m <= 0.0f) {
456 v->xyz.x = 1.0f;
457 v->xyz.y = 0.0f;
458 v->xyz.z = 0.0f;
459 return 1.0f;
460 }
461
462 float im = 1.0f / m;
463
464 v->xyz.x *= im;
465 v->xyz.y *= im;
466 v->xyz.z *= im;
467
468 return m;
469
470 }
471
472 //return the normalized direction vector between two points
473 //dest = normalized(end - start). Returns mag of direction vector
474 //NOTE: the order of the parameters matches the vector subtraction
vm_vec_normalized_dir(vec3d * dest,const vec3d * end,const vec3d * start)475 float vm_vec_normalized_dir(vec3d *dest, const vec3d *end, const vec3d *start)
476 {
477 float t;
478
479 vm_vec_sub(dest,end,start);
480 // VECMAT-ERROR: NULL VEC3D (end == start)
481 t = vm_vec_normalize_safe(dest);
482 return t;
483 }
484
485 //computes surface normal from three points. result is normalized
486 //returns ptr to dest
487 //dest CANNOT equal either source
vm_vec_normal(vec3d * dest,const vec3d * p0,const vec3d * p1,const vec3d * p2)488 vec3d *vm_vec_normal(vec3d *dest, const vec3d *p0, const vec3d *p1, const vec3d *p2)
489 {
490 Assert(dest != p0);
491 Assert(dest != p1);
492 Assert(dest != p2);
493
494 vm_vec_perp(dest,p0,p1,p2);
495
496 vm_vec_normalize(dest);
497
498 return dest;
499 }
500
501
502 //computes cross product of two vectors.
503 //Note: this magnitude of the resultant vector is the
504 //product of the magnitudes of the two source vectors. This means it is
505 //quite easy for this routine to overflow and underflow. Be careful that
506 //your inputs are ok.
vm_vec_cross(vec3d * dest,const vec3d * src0,const vec3d * src1)507 vec3d *vm_vec_cross(vec3d *dest, const vec3d *src0, const vec3d *src1)
508 {
509 dest->xyz.x = (src0->xyz.y * src1->xyz.z) - (src0->xyz.z * src1->xyz.y);
510 dest->xyz.y = (src0->xyz.z * src1->xyz.x) - (src0->xyz.x * src1->xyz.z);
511 dest->xyz.z = (src0->xyz.x * src1->xyz.y) - (src0->xyz.y * src1->xyz.x);
512
513 return dest;
514 }
515
vm_test_parallel(const vec3d * src0,const vec3d * src1)516 int vm_test_parallel(const vec3d *src0, const vec3d *src1)
517 {
518 vec3d partial1;
519 vec3d partial2;
520
521 /*
522 * To test if two vectors are parallel, calculate their cross product.
523 * If the result is zero, then the vectors are parallel. It is better
524 * to compare the two cross product "partials" (for lack of a better
525 * word) against each other instead of the final cross product against
526 * zero.
527 */
528
529 partial1.xyz.x = (src0->xyz.y * src1->xyz.z);
530 partial1.xyz.y = (src0->xyz.z * src1->xyz.x);
531 partial1.xyz.z = (src0->xyz.x * src1->xyz.y);
532
533 partial2.xyz.x = (src0->xyz.z * src1->xyz.y);
534 partial2.xyz.y = (src0->xyz.x * src1->xyz.z);
535 partial2.xyz.z = (src0->xyz.y * src1->xyz.x);
536
537 return vm_vec_equal(partial1, partial2);
538 }
539
540 //computes non-normalized surface normal from three points.
541 //returns ptr to dest
542 //dest CANNOT equal either source
vm_vec_perp(vec3d * dest,const vec3d * p0,const vec3d * p1,const vec3d * p2)543 vec3d *vm_vec_perp(vec3d *dest, const vec3d *p0, const vec3d *p1,const vec3d *p2)
544 {
545 Assert(dest != p0);
546 Assert(dest != p1);
547 Assert(dest != p2);
548
549 vec3d t0,t1;
550
551 vm_vec_sub(&t0,p1,p0);
552 vm_vec_sub(&t1,p2,p1);
553
554 return vm_vec_cross(dest,&t0,&t1);
555 }
556
557
558 //computes the delta angle between two vectors.
559 //vectors need not be normalized. if they are, call vm_vec_delta_ang_norm()
560 //the up vector (third parameter) can be NULL, in which case the absolute
561 //value of the angle in returned.
562 //Otherwise, the delta ang will be positive if the v0 -> v1 direction from the
563 //point of view of uvec is clockwise, negative if counterclockwise.
564 //This vector should be orthogonal to v0 and v1
vm_vec_delta_ang(const vec3d * v0,const vec3d * v1,const vec3d * uvec)565 float vm_vec_delta_ang(const vec3d *v0, const vec3d *v1, const vec3d *uvec)
566 {
567 float t;
568 vec3d t0,t1,t2;
569
570 vm_vec_copy_normalize(&t0,v0);
571 vm_vec_copy_normalize(&t1,v1);
572
573 if (uvec == nullptr) {
574 t = vm_vec_delta_ang_norm(&t0, &t1, NULL);
575 } else {
576 vm_vec_copy_normalize(&t2,uvec);
577 t = vm_vec_delta_ang_norm(&t0,&t1,&t2);
578 }
579
580 return t;
581 }
582
583 //computes the delta angle between two normalized vectors.
vm_vec_delta_ang_norm(const vec3d * v0,const vec3d * v1,const vec3d * uvec)584 float vm_vec_delta_ang_norm(const vec3d *v0, const vec3d *v1, const vec3d *uvec)
585 {
586 float a;
587 vec3d t;
588
589 a = acosf_safe(vm_vec_dot(v0,v1));
590
591 if (uvec) {
592 vm_vec_cross(&t,v0,v1);
593 if ( vm_vec_dot(&t,uvec) < 0.0 ) {
594 a = -a;
595 }
596 }
597
598 return a;
599 }
600
601 // helper function that fills in matrix m based on provided sine and cosine values.
sincos_2_matrix(matrix * m,float sinp,float cosp,float sinb,float cosb,float sinh,float cosh)602 static matrix *sincos_2_matrix(matrix *m, float sinp, float cosp, float sinb, float cosb, float sinh, float cosh)
603 {
604 float sbsh,cbch,cbsh,sbch;
605
606
607 sbsh = sinb*sinh;
608 cbch = cosb*cosh;
609 cbsh = cosb*sinh;
610 sbch = sinb*cosh;
611
612 m->vec.rvec.xyz.x = cbch + sinp*sbsh; //m1
613 m->vec.uvec.xyz.z = sbsh + sinp*cbch; //m8
614
615 m->vec.uvec.xyz.x = sinp*cbsh - sbch; //m2
616 m->vec.rvec.xyz.z = sinp*sbch - cbsh; //m7
617
618 m->vec.fvec.xyz.x = sinh*cosp; //m3
619 m->vec.rvec.xyz.y = sinb*cosp; //m4
620 m->vec.uvec.xyz.y = cosb*cosp; //m5
621 m->vec.fvec.xyz.z = cosh*cosp; //m9
622
623 m->vec.fvec.xyz.y = -sinp; //m6
624
625
626 return m;
627
628 }
629
630 //computes a matrix from a set of three angles. returns ptr to matrix
vm_angles_2_matrix(matrix * m,const angles * a)631 matrix *vm_angles_2_matrix(matrix *m, const angles *a)
632 {
633 matrix * t;
634 float sinp,cosp,sinb,cosb,sinh,cosh;
635
636 sinp = sinf(a->p); cosp = cosf(a->p);
637 sinb = sinf(a->b); cosb = cosf(a->b);
638 sinh = sinf(a->h); cosh = cosf(a->h);
639
640 t = sincos_2_matrix(m,sinp,cosp,sinb,cosb,sinh,cosh);
641
642 return t;
643 }
644
645 //computes a matrix from one angle.
646 // angle_index = 0,1,2 for p,b,h
vm_angle_2_matrix(matrix * m,float a,int angle_index)647 matrix *vm_angle_2_matrix(matrix *m, float a, int angle_index)
648 {
649 matrix * t;
650 float sinp,cosp,sinb,cosb,sinh,cosh;
651
652 /*
653 * Initialize sin and cos variables using an initial angle of
654 * zero degrees. Recall that sin(0) = 0 and cos(0) = 1.
655 */
656
657 sinp = 0.0f; cosp = 1.0f;
658 sinb = 0.0f; cosb = 1.0f;
659 sinh = 0.0f; cosh = 1.0f;
660
661 switch (angle_index) {
662 case 0:
663 sinp = sinf(a); cosp = cosf(a);
664 break;
665 case 1:
666 sinb = sinf(a); cosb = cosf(a);
667 break;
668 case 2:
669 sinh = sinf(a); cosh = cosf(a);
670 break;
671 }
672
673 t = sincos_2_matrix(m,sinp,cosp,sinb,cosb,sinh,cosh);
674
675 return t;
676 }
677
678
679 //computes a matrix from a forward vector and an angle
vm_vec_ang_2_matrix(matrix * m,const vec3d * v,float a)680 matrix *vm_vec_ang_2_matrix(matrix *m, const vec3d *v, float a)
681 {
682 matrix * t;
683 float sinb,cosb,sinp,cosp,sinh,cosh;
684
685 sinb = sinf(a); cosb = cosf(a);
686
687 sinp = -v->xyz.y;
688 cosp = fl_sqrt(1.0f - sinp*sinp);
689
690 sinh = v->xyz.x / cosp;
691 cosh = v->xyz.z / cosp;
692
693 t = sincos_2_matrix(m,sinp,cosp,sinb,cosb,sinh,cosh);
694
695 return t;
696 }
697
698 /**
699 * @brief Generates a matrix from a normalized fvec.
700 *
701 * @param[in,out] matrix The matrix to generate
702 *
703 * @details The matrix's fvec is used to generate the uvec and rvec
704 *
705 * @sa vm_vector_2_matrix(), vm_vector_2_matrix_norm()
706 */
vm_vector_2_matrix_gen_vectors(matrix * m)707 void vm_vector_2_matrix_gen_vectors(matrix *m)
708 {
709 vec3d *xvec=&m->vec.rvec;
710 vec3d *yvec=&m->vec.uvec;
711 vec3d *zvec=&m->vec.fvec;
712
713 if ((zvec->xyz.x==0.0f) && (zvec->xyz.z==0.0f)) { //forward vec is straight up or down
714 m->vec.rvec.xyz.x = 1.0f;
715 m->vec.uvec.xyz.z = (zvec->xyz.y<0.0f)?1.0f:-1.0f;
716
717 m->vec.rvec.xyz.y = m->vec.rvec.xyz.z = m->vec.uvec.xyz.x = m->vec.uvec.xyz.y = 0.0f;
718 }
719 else { //not straight up or down
720
721 xvec->xyz.x = zvec->xyz.z;
722 xvec->xyz.y = 0.0f;
723 xvec->xyz.z = -zvec->xyz.x;
724
725 vm_vec_normalize(xvec);
726
727 vm_vec_cross(yvec,zvec,xvec);
728
729 }
730 }
731
vm_vector_2_matrix(matrix * m,const vec3d * fvec,const vec3d * uvec,const vec3d * rvec)732 matrix *vm_vector_2_matrix(matrix *m, const vec3d *fvec, const vec3d *uvec, const vec3d *rvec)
733 {
734 vec3d fvec_norm;
735 vm_vec_copy_normalize(&fvec_norm, fvec);
736 fvec = &fvec_norm;
737
738 vec3d uvec_norm;
739 if (uvec != nullptr) {
740 vm_vec_copy_normalize(&uvec_norm, uvec);
741 uvec = &uvec_norm;
742 }
743
744 vec3d rvec_norm;
745 if (rvec != nullptr) {
746 vm_vec_copy_normalize(&rvec_norm, rvec);
747 rvec = &rvec_norm;
748 }
749
750 // Call the actuall function for normalized vectors
751 return vm_vector_2_matrix_norm(m, fvec, uvec, rvec);
752 }
753
vm_vector_2_matrix_norm(matrix * m,const vec3d * fvec,const vec3d * uvec,const vec3d * rvec)754 matrix *vm_vector_2_matrix_norm(matrix *m, const vec3d *fvec, const vec3d *uvec, const vec3d *rvec)
755 {
756 matrix temp = vmd_identity_matrix;
757
758 vec3d *xvec=&temp.vec.rvec;
759 vec3d *yvec=&temp.vec.uvec;
760 vec3d *zvec=&temp.vec.fvec;
761
762 Assert(fvec != NULL);
763
764 *zvec = *fvec;
765
766 if (uvec == NULL) {
767 if (rvec == NULL) { //just forward vec
768 vm_vector_2_matrix_gen_vectors(&temp);
769 }
770 else { //use right vec
771 *xvec = *rvec;
772
773 vm_vec_cross(yvec,zvec,xvec);
774
775 //normalize new perpendicular vector
776 vm_vec_normalize(yvec);
777
778 //now recompute right vector, in case it wasn't entirely perpendiclar
779 vm_vec_cross(xvec,yvec,zvec);
780 }
781 }
782 else { //use up vec
783 *yvec = *uvec;
784
785 vm_vec_cross(xvec,yvec,zvec);
786
787 if (vm_vec_equal(*xvec, vmd_zero_vector)) {
788 // uvec was bogus (either same as fvec or -fvec)
789 // Reset temp to the original values and do the setup again
790 temp = *m;
791
792 temp.vec.fvec = *fvec;
793
794 vm_vector_2_matrix_gen_vectors(&temp);
795 }
796 else {
797 //normalize new perpendicular vector
798 vm_vec_normalize(xvec);
799
800 //now recompute up vector, in case it wasn't entirely perpendiclar
801 vm_vec_cross(yvec,zvec,xvec);
802 }
803 }
804
805 // Copy the computed values into the output parameter
806 *m = temp;
807 return m;
808 }
809
810
811 // rotates a vector through a matrix, writes to *dest and returns the pointer
812 // if m is a rotation matrix it will preserve the length of *src, so normalised vectors will remain normalised
vm_vec_rotate(vec3d * dest,const vec3d * src,const matrix * m)813 vec3d *vm_vec_rotate(vec3d *dest, const vec3d *src, const matrix *m)
814 {
815 *dest = (*m) * (*src);
816
817 return dest;
818 }
819
820 // like vm_vec_rotate, but uses the transpose matrix instead. for rotations, this is an inverse.
vm_vec_unrotate(vec3d * dest,const vec3d * src,const matrix * m)821 vec3d *vm_vec_unrotate(vec3d *dest, const vec3d *src, const matrix *m)
822 {
823 matrix mt;
824
825 vm_copy_transpose(&mt, m);
826 *dest = mt * (*src);
827
828 return dest;
829 }
830
831 //transpose a matrix in place. returns ptr to matrix
vm_transpose(matrix * m)832 matrix *vm_transpose(matrix *m)
833 {
834 float t;
835
836 t = m->vec.uvec.xyz.x; m->vec.uvec.xyz.x = m->vec.rvec.xyz.y; m->vec.rvec.xyz.y = t;
837 t = m->vec.fvec.xyz.x; m->vec.fvec.xyz.x = m->vec.rvec.xyz.z; m->vec.rvec.xyz.z = t;
838 t = m->vec.fvec.xyz.y; m->vec.fvec.xyz.y = m->vec.uvec.xyz.z; m->vec.uvec.xyz.z = t;
839
840 return m;
841 }
842
843 //copy and transpose a matrix. returns ptr to matrix
844 //dest CANNOT equal source. use vm_transpose() if this is the case
vm_copy_transpose(matrix * dest,const matrix * src)845 matrix *vm_copy_transpose(matrix *dest, const matrix *src)
846 {
847 Assert(dest != src);
848
849 dest->vec.rvec.xyz.x = src->vec.rvec.xyz.x;
850 dest->vec.rvec.xyz.y = src->vec.uvec.xyz.x;
851 dest->vec.rvec.xyz.z = src->vec.fvec.xyz.x;
852
853 dest->vec.uvec.xyz.x = src->vec.rvec.xyz.y; //-V537
854 dest->vec.uvec.xyz.y = src->vec.uvec.xyz.y;
855 dest->vec.uvec.xyz.z = src->vec.fvec.xyz.y; //-V537
856
857 dest->vec.fvec.xyz.x = src->vec.rvec.xyz.z;
858 dest->vec.fvec.xyz.y = src->vec.uvec.xyz.z; //-V537
859 dest->vec.fvec.xyz.z = src->vec.fvec.xyz.z;
860
861
862 return dest;
863 }
864
operator *(const matrix & A,const vec3d & v)865 inline vec3d operator*(const matrix& A, const vec3d& v) {
866 vec3d out;
867
868 out.xyz.x = vm_vec_dot(&A.vec.rvec, &v);
869 out.xyz.y = vm_vec_dot(&A.vec.uvec, &v);
870 out.xyz.z = vm_vec_dot(&A.vec.fvec, &v);
871
872 return out;
873 }
874
operator *(const matrix & A,const matrix & B)875 inline matrix operator*(const matrix& A, const matrix& B) {
876 matrix BT, out;
877
878 // we transpose B here for concision and also potential vectorisation opportunities
879 vm_copy_transpose(&BT, &B);
880
881 out.vec.rvec = BT * A.vec.rvec;
882 out.vec.uvec = BT * A.vec.uvec;
883 out.vec.fvec = BT * A.vec.fvec;
884
885 return out;
886 }
887
888 // Old matrix multiplication routine. Note that the order of multiplication is inverted
889 // compared to the mathematical standard: formally, this calculates src1 * src0
vm_matrix_x_matrix(matrix * dest,const matrix * src0,const matrix * src1)890 matrix *vm_matrix_x_matrix(matrix *dest, const matrix *src0, const matrix *src1)
891 {
892 *dest = (*src1) * (*src0);
893
894 return dest;
895 }
896
897 //extract angles from a matrix
vm_extract_angles_matrix(angles * a,const matrix * m)898 angles *vm_extract_angles_matrix(angles *a, const matrix *m)
899 {
900 float sinh,cosh,cosp;
901
902 a->h = atan2_safe(m->vec.fvec.xyz.x,m->vec.fvec.xyz.z);
903
904 sinh = sinf(a->h); cosh = cosf(a->h);
905
906 if (fl_abs(sinh) > fl_abs(cosh)) //sine is larger, so use it
907 cosp = m->vec.fvec.xyz.x*sinh;
908 else //cosine is larger, so use it
909 cosp = m->vec.fvec.xyz.z*cosh;
910
911 //using the fvec_xz_distance extracts the correct pitch from the matrix --wookieejedi
912 //previously cosp was used as the denominator, but this resulted in some incorrect pitch extractions
913 float fvec_xz_distance;
914
915 fvec_xz_distance = fl_sqrt( ( (m->vec.fvec.xyz.x)*(m->vec.fvec.xyz.x) ) + ( (m->vec.fvec.xyz.z)*(m->vec.fvec.xyz.z) ) );
916
917 a->p = atan2_safe(-m->vec.fvec.xyz.y, fvec_xz_distance);
918
919 if (cosp == 0.0f) //the cosine of pitch is zero. we're pitched straight up. say no bank
920
921 a->b = 0.0f;
922
923 else {
924 float sinb,cosb;
925
926 sinb = m->vec.rvec.xyz.y/cosp;
927 cosb = m->vec.uvec.xyz.y/cosp;
928
929 a->b = atan2_safe(sinb,cosb);
930 }
931
932
933 return a;
934 }
935
936 // alternate method for extracting angles which seems to be
937 // less susceptible to rounding errors -- see section 8.7.2
938 // (pages 278-281) of 3D Math Primer for Graphics and Game
939 // Development, 2nd Edition
940 // http://books.google.com/books?id=X3hmuhBoFF0C&printsec=frontcover#v=onepage&q&f=false
vm_extract_angles_matrix_alternate(angles * a,const matrix * m)941 angles *vm_extract_angles_matrix_alternate(angles *a, const matrix *m)
942 {
943 Assert(a != NULL);
944 Assert(m != NULL);
945
946 a->p = asinf_safe(-m->vec.fvec.xyz.y);
947
948 // Check for the Gimbal lock case, giving a slight tolerance
949 // for numerical imprecision
950 if (fabs(m->vec.fvec.xyz.y) > 0.9999f) {
951 // We are looking straight up or down.
952 // Slam bank to zero and just set heading
953 a->b = 0.0f;
954 a->h = atan2(-m->vec.rvec.xyz.z, m->vec.rvec.xyz.x);
955 } else {
956 // Compute heading
957 a->h = atan2(m->vec.fvec.xyz.x, m->vec.fvec.xyz.z);
958
959 // Compute bank
960 a->b = atan2(m->vec.rvec.xyz.y, m->vec.uvec.xyz.y);
961 }
962
963 return a;
964 }
965
966
967 //extract heading and pitch from a vector, assuming bank==0
vm_extract_angles_vector_normalized(angles * a,const vec3d * v)968 static angles *vm_extract_angles_vector_normalized(angles *a, const vec3d *v)
969 {
970
971 a->b = 0.0f; //always zero bank
972
973 a->p = asinf_safe(-v->xyz.y);
974
975 a->h = atan2_safe(v->xyz.z,v->xyz.x);
976
977 return a;
978 }
979
980 //extract heading and pitch from a vector, assuming bank==0
vm_extract_angles_vector(angles * a,const vec3d * v)981 angles *vm_extract_angles_vector(angles *a, const vec3d *v)
982 {
983 vec3d t;
984
985 vm_vec_copy_normalize(&t,v);
986 vm_extract_angles_vector_normalized(a,&t);
987
988 return a;
989 }
990
991 //compute the distance from a point to a plane. takes the normalized normal
992 //of the plane (ebx), a point on the plane (edi), and the point to check (esi).
993 //returns distance in eax
994 //distance is signed, so negative dist is on the back of the plane
vm_dist_to_plane(const vec3d * checkp,const vec3d * norm,const vec3d * planep)995 float vm_dist_to_plane(const vec3d *checkp, const vec3d *norm, const vec3d *planep)
996 {
997 float t1;
998 vec3d t;
999
1000 vm_vec_sub(&t,checkp,planep);
1001
1002 t1 = vm_vec_dot(&t,norm);
1003
1004 return t1;
1005
1006 }
1007
1008 // Given mouse movement in dx, dy, returns a 3x3 rotation matrix in RotMat.
1009 // Taken from Graphics Gems III, page 51, "The Rolling Ball"
1010 // Example:
1011 //if ( (Mouse.dx!=0) || (Mouse.dy!=0) ) {
1012 // GetMouseRotation( Mouse.dx, Mouse.dy, &MouseRotMat );
1013 // vm_matrix_x_matrix(&tempm,&LargeView.ev_matrix,&MouseRotMat);
1014 // LargeView.ev_matrix = tempm;
1015 //}
1016
1017
vm_trackball(int idx,int idy,matrix * RotMat)1018 void vm_trackball( int idx, int idy, matrix * RotMat )
1019 {
1020 float dr, cos_theta, sin_theta, denom, cos_theta1;
1021 float Radius = 100.0f;
1022 float dx,dy;
1023 float dxdr,dydr;
1024
1025 idy *= -1;
1026
1027 dx = (float)idx; dy = (float)idy;
1028
1029 dr = fl_sqrt(dx*dx+dy*dy);
1030
1031 denom = fl_sqrt(Radius*Radius+dr*dr);
1032
1033 cos_theta = Radius/denom;
1034 sin_theta = dr/denom;
1035
1036 cos_theta1 = 1.0f - cos_theta;
1037
1038 dxdr = dx/dr;
1039 dydr = dy/dr;
1040
1041 RotMat->vec.rvec.xyz.x = cos_theta + (dydr*dydr)*cos_theta1;
1042 RotMat->vec.uvec.xyz.x = - ((dxdr*dydr)*cos_theta1);
1043 RotMat->vec.fvec.xyz.x = (dxdr*sin_theta);
1044
1045 RotMat->vec.rvec.xyz.y = RotMat->vec.uvec.xyz.x;
1046 RotMat->vec.uvec.xyz.y = cos_theta + ((dxdr*dxdr)*cos_theta1);
1047 RotMat->vec.fvec.xyz.y = (dydr*sin_theta);
1048
1049 RotMat->vec.rvec.xyz.z = -RotMat->vec.fvec.xyz.x;
1050 RotMat->vec.uvec.xyz.z = -RotMat->vec.fvec.xyz.y;
1051 RotMat->vec.fvec.xyz.z = cos_theta;
1052 }
1053
1054 // Compute the outer product of A = A * transpose(A). 1x3 vector becomes 3x3 matrix.
vm_vec_outer_product(matrix * mat,const vec3d * vec)1055 static void vm_vec_outer_product(matrix *mat, const vec3d *vec)
1056 {
1057 mat->vec.rvec.xyz.x = vec->xyz.x * vec->xyz.x;
1058 mat->vec.rvec.xyz.y = vec->xyz.x * vec->xyz.y;
1059 mat->vec.rvec.xyz.z = vec->xyz.x * vec->xyz.z;
1060
1061 mat->vec.uvec.xyz.x = vec->xyz.y * vec->xyz.x; //-V537
1062 mat->vec.uvec.xyz.y = vec->xyz.y * vec->xyz.y;
1063 mat->vec.uvec.xyz.z = vec->xyz.y * vec->xyz.z; //-V537
1064
1065 mat->vec.fvec.xyz.x = vec->xyz.z * vec->xyz.x;
1066 mat->vec.fvec.xyz.y = vec->xyz.z * vec->xyz.y; //-V537
1067 mat->vec.fvec.xyz.z = vec->xyz.z * vec->xyz.z;
1068 }
1069
1070 // Find the point on the line between p0 and p1 that is nearest to int_pnt.
1071 // Stuff result in nearest_point.
1072 // Uses algorithm from page 148 of Strang, Linear Algebra and Its Applications.
1073 // Returns value indicating whether *nearest_point is between *p0 and *p1.
1074 // 0.0f means *nearest_point is *p0, 1.0f means it's *p1. 2.0f means it's beyond p1 by 2x.
1075 // -1.0f means it's "before" *p0 by 1x.
find_nearest_point_on_line(vec3d * nearest_point,const vec3d * p0,const vec3d * p1,const vec3d * int_pnt)1076 float find_nearest_point_on_line(vec3d *nearest_point, const vec3d *p0, const vec3d *p1, const vec3d *int_pnt)
1077 {
1078 vec3d norm, xlated_int_pnt, projected_point;
1079 matrix mat;
1080 float mag, dot;
1081
1082 vm_vec_sub(&norm, p1, p0);
1083 vm_vec_sub(&xlated_int_pnt, int_pnt, p0);
1084
1085 if (IS_VEC_NULL_SQ_SAFE(&norm)) {
1086 *nearest_point = *int_pnt;
1087 return 9999.9f;
1088 }
1089
1090 mag = vm_vec_normalize(&norm); // Normalize vector so we don't have to divide by dot product.
1091
1092 if (mag < 0.01f) {
1093 *nearest_point = *int_pnt;
1094 return 9999.9f;
1095 // Warning(LOCATION, "Very small magnitude in find_nearest_point_on_line.\n");
1096 }
1097
1098 vm_vec_outer_product(&mat, &norm);
1099
1100 vm_vec_rotate(&projected_point, &xlated_int_pnt, &mat);
1101 vm_vec_add(nearest_point, &projected_point, p0);
1102
1103 dot = vm_vec_dot(&norm, &projected_point);
1104
1105 return dot/mag;
1106 }
1107
find_intersection(float * s,const vec3d * p0,const vec3d * p1,const vec3d * v0,const vec3d * v1)1108 int find_intersection(float* s, const vec3d* p0, const vec3d* p1, const vec3d* v0, const vec3d* v1)
1109 {
1110 // Vector v2 forms an edge between v0 and v1, thus forming a triangle.
1111 // An intersection exists between v0 and v1 if their cross product is parallel with the cross product of v2 and v1
1112 // The scalar of v0 can then be found by the ratio between the two cross products
1113 vec3d v2, crossA, crossB;
1114
1115 vm_vec_sub(&v2, p1, p0);
1116 vm_vec_cross(&crossA, v0, v1);
1117 vm_vec_cross(&crossB, &v2, v1);
1118
1119 if (vm_vec_equal(crossA, vmd_zero_vector)) {
1120 // Colinear
1121 return -1;
1122 }
1123
1124 if (!vm_test_parallel(&crossA, &crossB)) {
1125 // The two cross products are not parallel, so no intersection between v0 and v1
1126 return -2;
1127 }
1128
1129 *s = vm_vec_mag(&crossB) / vm_vec_mag(&crossA);
1130 return 0;
1131 }
1132
find_point_on_line_nearest_skew_line(vec3d * dest,const vec3d * p1,const vec3d * d1,const vec3d * p2,const vec3d * d2)1133 void find_point_on_line_nearest_skew_line(vec3d *dest, const vec3d *p1, const vec3d *d1, const vec3d *p2, const vec3d *d2)
1134 {
1135 vec3d n, n2, pdiff;
1136
1137 // The cross product of the direction vectors is perpendicular to both lines
1138 vm_vec_cross(&n, d1, d2);
1139
1140 // The plane formed by the translations of Line 2 along n contains the point p2 and is perpendicular to n2 = d2 x n
1141 vm_vec_cross(&n2, d2, &n);
1142
1143 // So now we find the intersection of Line 1 with that plane, which is apparently this gibberish
1144 vm_vec_sub(&pdiff, p2, p1);
1145 float numerator = vm_vec_dot(&pdiff, &n2);
1146 float denominator = vm_vec_dot(d1, &n2);
1147 vm_vec_scale_add(dest, p1, d1, numerator / denominator);
1148 }
1149
1150
1151 // normalizes only if above a threshold, returns if normalized or not
vm_maybe_normalize(vec3d * dst,const vec3d * src,float threshold)1152 bool vm_maybe_normalize(vec3d* dst, const vec3d* src, float threshold) {
1153 float mag = vm_vec_mag(src);
1154 if (mag < threshold) return false;
1155 vm_vec_copy_scale(dst, src, 1 / mag);
1156 return true;
1157 }
1158
1159 // Produce a vector perpendicular to the normalized input vector unit_normal,
1160 // in the direction preference (if not null). If that direction doesn't work it picks the z or y direction,
1161 // so that an output perpendicular vector is guaranteed.
vm_orthogonalize_one_vec(vec3d * dst,const vec3d * unit_normal,const vec3d * preference)1162 void vm_orthogonalize_one_vec(vec3d* dst, const vec3d* unit_normal, const vec3d* preference) {
1163 if (preference != nullptr) {
1164 vm_vec_projection_onto_plane(dst, preference, unit_normal);
1165 if (vm_maybe_normalize(dst, dst)) {
1166 // The process of rescaling dst may have exaggerated floating point inaccuracy
1167 // so that dst is no longer approximately orthogonal to unit_normal,
1168 // so project it again.
1169 if (fabs(vm_vec_dot(dst, unit_normal)) > 1e-4f) {
1170 vm_vec_projection_onto_plane(dst, dst, unit_normal);
1171 vm_vec_normalize(dst);
1172 }
1173 return;
1174 }
1175 }
1176 vm_vec_projection_onto_plane(dst, &vmd_z_vector, unit_normal);
1177 if (vm_maybe_normalize(dst, dst)) return;
1178 vm_vec_projection_onto_plane(dst, &vmd_y_vector, unit_normal);
1179 }
1180
1181 // Produce two vectors perpendicular to each other from two arbitrary vectors src1, src2.
1182 // In the normal case dst1 will point in the direction of src1 and dst2 will be in the plane
1183 // of src1, src2 and perpendicular to dst1, but in the case of degeneracy it tries to
1184 // give useful results. The vector preference is a third vector (which may be null)
1185 // that will be considered in case the first two vectors are zero.
vm_orthogonalize_two_vec(vec3d * dst1,vec3d * dst2,const vec3d * src1,const vec3d * src2,const vec3d * preference)1186 void vm_orthogonalize_two_vec(vec3d* dst1, vec3d* dst2, const vec3d* src1, const vec3d* src2, const vec3d* preference) {
1187 if (vm_maybe_normalize(dst1, src1))
1188 vm_orthogonalize_one_vec(dst2, dst1, src2);
1189 else if (vm_maybe_normalize(dst2, src2))
1190 vm_orthogonalize_one_vec(dst1, dst2, src1);
1191 else {
1192 if (preference == nullptr || !vm_maybe_normalize(dst1, preference))
1193 vm_vec_make(dst1, 1, 0, 0);
1194 vm_orthogonalize_one_vec(dst2, dst1, src2);
1195 }
1196 }
1197
1198 // make sure matrix is orthogonal
1199 // computes a matrix from one or more vectors. The forward vector is required,
1200 // with the other two being optional. If both up & right vectors are passed,
1201 // the up vector is used. If only the forward vector is passed, a bank of
1202 // zero is assumed
vm_orthogonalize_matrix(matrix * m_src)1203 void vm_orthogonalize_matrix(matrix *m_src)
1204 {
1205 vec3d fvec, uvec;
1206 vm_orthogonalize_two_vec(&fvec, &uvec, &m_src->vec.fvec, &m_src->vec.uvec, &m_src->vec.rvec);
1207 vm_vec_cross(&m_src->vec.rvec, &uvec, &fvec);
1208 m_src->vec.fvec = fvec;
1209 m_src->vec.uvec = uvec;
1210 }
1211
1212 // like vm_orthogonalize_matrix(), except that zero vectors can exist within the
1213 // matrix without causing problems. Valid vectors will be created where needed.
vm_fix_matrix(matrix * m)1214 void vm_fix_matrix(matrix *m)
1215 {
1216 float fmag, umag, rmag;
1217
1218 fmag = vm_vec_mag(&m->vec.fvec);
1219 umag = vm_vec_mag(&m->vec.uvec);
1220 rmag = vm_vec_mag(&m->vec.rvec);
1221 if (fmag <= 0.0f) {
1222 if ((umag > 0.0f) && (rmag > 0.0f) && !vm_test_parallel(&m->vec.uvec, &m->vec.rvec)) {
1223 vm_vec_cross(&m->vec.fvec, &m->vec.uvec, &m->vec.rvec);
1224 vm_vec_normalize(&m->vec.fvec);
1225
1226 } else if (umag > 0.0f) {
1227 if (!m->vec.uvec.xyz.x && !m->vec.uvec.xyz.y && m->vec.uvec.xyz.z) // z vector
1228 vm_vec_make(&m->vec.fvec, 1.0f, 0.0f, 0.0f);
1229 else
1230 vm_vec_make(&m->vec.fvec, 0.0f, 0.0f, 1.0f);
1231 }
1232
1233 } else
1234 vm_vec_normalize(&m->vec.fvec);
1235
1236 // we now have a valid and normalized forward vector
1237
1238 if ((umag <= 0.0f) || vm_test_parallel(&m->vec.fvec, &m->vec.uvec)) { // no up vector to use..
1239 if ((rmag <= 0.0f) || vm_test_parallel(&m->vec.fvec, &m->vec.rvec)) { // no right vector either, so make something up
1240 if (!m->vec.fvec.xyz.x && m->vec.fvec.xyz.y && !m->vec.fvec.xyz.z) // vertical vector
1241 vm_vec_make(&m->vec.uvec, 0.0f, 0.0f, -1.0f);
1242 else
1243 vm_vec_make(&m->vec.uvec, 0.0f, 1.0f, 0.0f);
1244
1245 } else { // use the right vector to figure up vector
1246 vm_vec_cross(&m->vec.uvec, &m->vec.fvec, &m->vec.rvec);
1247 vm_vec_normalize(&m->vec.uvec);
1248 }
1249
1250 } else
1251 vm_vec_normalize(&m->vec.uvec);
1252
1253 // we now have both valid and normalized forward and up vectors
1254
1255 vm_vec_cross(&m->vec.rvec, &m->vec.uvec, &m->vec.fvec);
1256
1257 //normalize new perpendicular vector
1258 vm_vec_normalize(&m->vec.rvec);
1259
1260 //now recompute up vector, in case it wasn't entirely perpendiclar
1261 vm_vec_cross(&m->vec.uvec, &m->vec.fvec, &m->vec.rvec);
1262 }
1263
1264 //Rotates the orient matrix by the angles in tangles and then
1265 //makes sure that the matrix is orthogonal.
vm_rotate_matrix_by_angles(matrix * orient,const angles * tangles)1266 void vm_rotate_matrix_by_angles( matrix *orient, const angles *tangles )
1267 {
1268 matrix rotmat,new_orient;
1269 vm_angles_2_matrix(&rotmat,tangles);
1270 vm_matrix_x_matrix(&new_orient,orient,&rotmat);
1271 *orient = new_orient;
1272 vm_orthogonalize_matrix(orient);
1273 }
1274
1275 // dir must be normalized!
vm_vec_dot_to_point(const vec3d * dir,const vec3d * p1,const vec3d * p2)1276 float vm_vec_dot_to_point(const vec3d *dir, const vec3d *p1, const vec3d *p2)
1277 {
1278 vec3d tvec;
1279
1280 vm_vec_sub(&tvec, p2, p1);
1281 // VECMAT-ERROR: NULL VEC3D (p1 == p2)
1282 vm_vec_normalize_safe(&tvec);
1283
1284 return vm_vec_dot(dir, &tvec);
1285
1286 }
1287
1288 /////////////////////////////////////////////////////////
1289 // Given a plane and a point, return the point on the plane closest the the point.
1290 // Result returned in q.
compute_point_on_plane(vec3d * q,const plane * planep,const vec3d * p)1291 void compute_point_on_plane(vec3d *q, const plane *planep, const vec3d *p)
1292 {
1293 float k;
1294 vec3d normal;
1295
1296 normal.xyz.x = planep->A;
1297 normal.xyz.y = planep->B;
1298 normal.xyz.z = planep->C;
1299
1300 k = (planep->D + vm_vec_dot(&normal, p)) / vm_vec_dot(&normal, &normal);
1301
1302 vm_vec_scale_add(q, p, &normal, -k);
1303 }
1304
1305 // Generate a fairly random vector that's normalized.
vm_vec_rand_vec(vec3d * rvec)1306 void vm_vec_rand_vec(vec3d *rvec)
1307 {
1308 rvec->xyz.x = (frand() - 0.5f) * 2;
1309 rvec->xyz.y = (frand() - 0.5f) * 2;
1310 rvec->xyz.z = (frand() - 0.5f) * 2;
1311
1312 if (IS_VEC_NULL_SQ_SAFE(rvec))
1313 rvec->xyz.x = 1.0f;
1314
1315 vm_vec_normalize(rvec);
1316 }
1317
1318 // Given an point "in" rotate it by "angle" around an
1319 // arbritary line defined by a point on the line "line_point"
1320 // and the normalized line direction, "line_dir"
1321 // Returns the rotated point in "out".
vm_rot_point_around_line(vec3d * out,const vec3d * in,float angle,const vec3d * line_point,const vec3d * line_dir)1322 void vm_rot_point_around_line(vec3d *out, const vec3d *in, float angle, const vec3d *line_point, const vec3d *line_dir)
1323 {
1324 vec3d tmp, tmp1;
1325 matrix m, r;
1326 angles ta;
1327
1328 vm_vector_2_matrix_norm(&m, line_dir, NULL, NULL );
1329
1330 ta.p = ta.h = 0.0f;
1331 ta.b = angle;
1332 vm_angles_2_matrix(&r,&ta);
1333
1334 vm_vec_sub( &tmp, in, line_point ); // move relative to a point on line
1335 vm_vec_rotate( &tmp1, &tmp, &m); // rotate into line's base
1336 vm_vec_rotate( &tmp, &tmp1, &r); // rotate around Z
1337 vm_vec_unrotate( &tmp1, &tmp, &m); // unrotate out of line's base
1338 vm_vec_add( out, &tmp1, line_point ); // move back to world coordinates
1339 }
1340
1341 // Given two position vectors, return 0 if the same, else non-zero.
vm_vec_cmp(const vec3d * a,const vec3d * b)1342 int vm_vec_cmp( const vec3d * a, const vec3d * b )
1343 {
1344 float diff = vm_vec_dist(a,b);
1345 //mprintf(( "Diff=%.32f\n", diff ));
1346 if ( diff > 0.005f )
1347 return 1;
1348 else
1349 return 0;
1350 }
1351
1352 // Given two orientation matrices, return 0 if the same, else non-zero.
vm_matrix_cmp(const matrix * a,const matrix * b)1353 int vm_matrix_cmp(const matrix * a, const matrix * b)
1354 {
1355 float tmp1,tmp2,tmp3;
1356 tmp1 = fl_abs(vm_vec_dot( &a->vec.uvec, &b->vec.uvec ) - 1.0f);
1357 tmp2 = fl_abs(vm_vec_dot( &a->vec.fvec, &b->vec.fvec ) - 1.0f);
1358 tmp3 = fl_abs(vm_vec_dot( &a->vec.rvec, &b->vec.rvec ) - 1.0f);
1359 // mprintf(( "Mat=%.16f, %.16f, %.16f\n", tmp1, tmp2, tmp3 ));
1360
1361 if ( tmp1 > 0.0000005f ) return 1;
1362 if ( tmp2 > 0.0000005f ) return 1;
1363 if ( tmp3 > 0.0000005f ) return 1;
1364 return 0;
1365 }
1366
1367
1368 // Moves angle 'h' towards 'desired_angle', taking the shortest
1369 // route possible. It will move a maximum of 'step_size' radians
1370 // each call. All angles in radians.
vm_interp_angle(float * h,float desired_angle,float step_size,bool force_front)1371 float vm_interp_angle( float *h, float desired_angle, float step_size, bool force_front )
1372 {
1373 float delta;
1374 float abs_delta;
1375
1376 if ( desired_angle < 0.0f ) desired_angle += PI2;
1377 if ( desired_angle > PI2 ) desired_angle -= PI2;
1378
1379 delta = desired_angle - *h;
1380 abs_delta = fl_abs(delta);
1381
1382 if ((force_front) && ((desired_angle > PI) ^ (*h > PI)) ) {
1383 // turn away from PI
1384 if ( *h > PI )
1385 delta = abs_delta;
1386 else
1387 delta = -abs_delta;
1388 } else {
1389 if ( abs_delta > PI ) {
1390 // Go the other way, since it will be shorter.
1391 if ( delta > 0.0f ) {
1392 delta = delta - PI2;
1393 } else {
1394 delta = PI2 - delta;
1395 }
1396 }
1397 }
1398
1399 if ( delta > step_size )
1400 *h += step_size;
1401 else if ( delta < -step_size )
1402 *h -= step_size;
1403 else
1404 *h = desired_angle;
1405
1406 // If we wrap outside of 0 to 2*PI, then put the
1407 // angle back in the range 0 to 2*PI.
1408 if ( *h > PI2 ) *h -= PI2;
1409 if ( *h < 0.0f ) *h += PI2;
1410
1411 return delta;
1412 }
1413
vm_delta_from_interp_angle(float current_angle,float desired_angle)1414 float vm_delta_from_interp_angle( float current_angle, float desired_angle )
1415 {
1416 float delta;
1417 if ( desired_angle < 0.0f ) desired_angle += PI2;
1418 if ( desired_angle > PI2 ) desired_angle -= PI2;
1419
1420 delta = desired_angle - current_angle;
1421
1422 if ( fl_abs(delta) > PI ) {
1423 if ( delta > 0.0f ) {
1424 delta = delta - PI2;
1425 } else {
1426 delta = PI2 - delta;
1427 }
1428 }
1429 return delta;
1430 }
1431
1432 // check a matrix for zero rows and columns
vm_check_matrix_for_zeros(const matrix * m)1433 int vm_check_matrix_for_zeros(const matrix *m)
1434 {
1435 if (!m->vec.fvec.xyz.x && !m->vec.fvec.xyz.y && !m->vec.fvec.xyz.z)
1436 return 1;
1437 if (!m->vec.rvec.xyz.x && !m->vec.rvec.xyz.y && !m->vec.rvec.xyz.z)
1438 return 1;
1439 if (!m->vec.uvec.xyz.x && !m->vec.uvec.xyz.y && !m->vec.uvec.xyz.z)
1440 return 1;
1441
1442 if (!m->vec.fvec.xyz.x && !m->vec.rvec.xyz.x && !m->vec.uvec.xyz.x)
1443 return 1;
1444 if (!m->vec.fvec.xyz.y && !m->vec.rvec.xyz.y && !m->vec.uvec.xyz.y)
1445 return 1;
1446 if (!m->vec.fvec.xyz.z && !m->vec.rvec.xyz.z && !m->vec.uvec.xyz.z)
1447 return 1;
1448
1449 return 0;
1450 }
1451
1452 // see if two vectors are the same
vm_vec_same(const vec3d * v1,const vec3d * v2)1453 int vm_vec_same(const vec3d *v1, const vec3d *v2)
1454 {
1455 if ( v1->xyz.x == v2->xyz.x && v1->xyz.y == v2->xyz.y && v1->xyz.z == v2->xyz.z )
1456 return 1;
1457
1458 return 0;
1459 }
1460
1461 // see if two matrices are the same
vm_matrix_same(matrix * m1,matrix * m2)1462 int vm_matrix_same(matrix *m1, matrix *m2)
1463 {
1464 int i;
1465 for (i = 0; i < 9; i++)
1466 if (m1->a1d[i] != m2->a1d[i])
1467 return 0;
1468
1469 return 1;
1470 }
1471
1472
1473 // --------------------------------------------------------------------------------------
1474
vm_quaternion_rotate(matrix * M,float theta,const vec3d * u)1475 void vm_quaternion_rotate(matrix *M, float theta, const vec3d *u)
1476 // given an arbitrary rotation axis and rotation angle, function generates the
1477 // corresponding rotation matrix
1478 //
1479 // M is the return rotation matrix theta is the angle of rotation
1480 // u is the direction of the axis.
1481 // this is adapted from Computer Graphics (Hearn and Bker 2nd ed.) p. 420
1482 //
1483 {
1484 float a,b,c, s;
1485 float sin_theta = sinf(theta * 0.5f);
1486
1487 a = (u->xyz.x * sin_theta);
1488 b = (u->xyz.y * sin_theta);
1489 c = (u->xyz.z * sin_theta);
1490 s = cosf(theta * 0.5f);
1491
1492 // 1st ROW vector
1493 M->vec.rvec.xyz.x = 1.0f - 2.0f*b*b - 2.0f*c*c;
1494 M->vec.rvec.xyz.y = 2.0f*a*b + 2.0f*s*c;
1495 M->vec.rvec.xyz.z = 2.0f*a*c - 2.0f*s*b;
1496 // 2nd ROW vector
1497 M->vec.uvec.xyz.x = 2.0f*a*b - 2.0f*s*c;
1498 M->vec.uvec.xyz.y = 1.0f - 2.0f*a*a - 2.0f*c*c;
1499 M->vec.uvec.xyz.z = 2.0f*b*c + 2.0f*s*a;
1500 // 3rd ROW vector
1501 M->vec.fvec.xyz.x = 2.0f*a*c + 2.0f*s*b;
1502 M->vec.fvec.xyz.y = 2.0f*b*c - 2.0f*s*a;
1503 M->vec.fvec.xyz.z = 1.0f - 2.0f*a*a - 2.0f*b*b;
1504 }
1505
1506 // --------------------------------------------------------------------------------------
1507
1508 //void vm_matrix_to_rot_axis_and_angle(matrix *m, float *theta, vec3d *rot_axis)
1509 // Converts a matrix into a rotation axis and an angle around that axis
1510 // Note for angle is very near 0, returns 0 with axis of (1,0,0)
1511 // For angles near PI, returns PI with correct axis
1512 //
1513 // rot_axis - the resultant axis of rotation
1514 // theta - the resultatn rotation around the axis
1515 // m - the initial matrix
vm_matrix_to_rot_axis_and_angle(const matrix * m,float * theta,vec3d * rot_axis)1516 void vm_matrix_to_rot_axis_and_angle(const matrix *m, float *theta, vec3d *rot_axis)
1517 {
1518 float trace = m->a2d[0][0] + m->a2d[1][1] + m->a2d[2][2];
1519 float cos_theta = 0.5f * (trace - 1.0f);
1520
1521 if (cos_theta > 0.999999875f) { // angle is less than 1 milirad (0.057 degrees)
1522 *theta = 0.0f;
1523
1524 vm_vec_make(rot_axis, 1.0f, 0.0f, 0.0f);
1525 } else if (cos_theta > -0.999999875f) { // angle is within limits between 0 and PI
1526 *theta = acosf_safe(cos_theta);
1527 Assert( !fl_is_nan(*theta) );
1528
1529 rot_axis->xyz.x = (m->vec.uvec.xyz.z - m->vec.fvec.xyz.y);
1530 rot_axis->xyz.y = (m->vec.fvec.xyz.x - m->vec.rvec.xyz.z);
1531 rot_axis->xyz.z = (m->vec.rvec.xyz.y - m->vec.uvec.xyz.x);
1532 if (IS_VEC_NULL_SQ_SAFE(rot_axis)) {
1533 vm_vec_make(rot_axis, 1.0f, 0.0f, 0.0f);
1534 } else {
1535 vm_vec_normalize(rot_axis);
1536 }
1537 } else { // angle is PI within limits
1538 *theta = PI;
1539
1540 // find index of largest diagonal term
1541 int largest_diagonal_index = 0;
1542
1543 if (m->a2d[1][1] > m->a2d[0][0]) {
1544 largest_diagonal_index = 1;
1545 }
1546 if (m->a2d[2][2] > m->a2d[largest_diagonal_index][largest_diagonal_index]) {
1547 largest_diagonal_index = 2;
1548 }
1549
1550 switch (largest_diagonal_index) {
1551 case 0:
1552 float ix;
1553
1554 rot_axis->xyz.x = fl_sqrt(m->a2d[0][0] + 1.0f);
1555 ix = 1.0f / rot_axis->xyz.x;
1556 rot_axis->xyz.y = m->a2d[0][1] * ix;
1557 rot_axis->xyz.z = m->a2d[0][2] * ix;
1558 break;
1559
1560 case 1:
1561 float iy;
1562
1563 rot_axis->xyz.y = fl_sqrt(m->a2d[1][1] + 1.0f);
1564 iy = 1.0f / rot_axis->xyz.y;
1565 rot_axis->xyz.x = m->a2d[1][0] * iy;
1566 rot_axis->xyz.z = m->a2d[1][2] * iy;
1567 break;
1568
1569 case 2:
1570 float iz;
1571
1572 rot_axis->xyz.z = fl_sqrt(m->a2d[2][2] + 1.0f);
1573 iz = 1.0f / rot_axis->xyz.z;
1574 rot_axis->xyz.x = m->a2d[2][0] * iz;
1575 rot_axis->xyz.y = m->a2d[2][1] * iz;
1576 break;
1577
1578 default:
1579 Int3(); // this should never happen
1580 break;
1581 }
1582
1583 // normalize rotation axis
1584 vm_vec_normalize(rot_axis);
1585 }
1586 }
1587
1588 // --------------------------------------------------------------------------------------
1589
1590
get_camera_limits(const matrix * start_camera,const matrix * end_camera,float time,vec3d * acc_max,vec3d * w_max)1591 void get_camera_limits(const matrix *start_camera, const matrix *end_camera, float time, vec3d *acc_max, vec3d *w_max)
1592 {
1593 matrix temp, rot_matrix;
1594 float theta;
1595 vec3d rot_axis;
1596 vec3d angle;
1597
1598 // determine the necessary rotation matrix
1599 vm_copy_transpose(&temp, start_camera);
1600 vm_matrix_x_matrix(&rot_matrix, &temp, end_camera);
1601 vm_orthogonalize_matrix(&rot_matrix);
1602
1603 // determine the rotation axis and angle
1604 vm_matrix_to_rot_axis_and_angle(&rot_matrix, &theta, &rot_axis);
1605
1606 // find the rotation about each axis
1607 angle.xyz.x = theta * rot_axis.xyz.x;
1608 angle.xyz.y = theta * rot_axis.xyz.y;
1609 angle.xyz.z = theta * rot_axis.xyz.z;
1610
1611 // allow for 0 time input
1612 if (time <= 1e-5f) {
1613 vm_vec_make(acc_max, 0.0f, 0.0f, 0.0f);
1614 vm_vec_make(w_max, 0.0f, 0.0f, 0.0f);
1615 } else {
1616
1617 // find acceleration limit using (theta/2) takes (time/2)
1618 // and using const accel theta = 1/2 acc * time^2
1619 acc_max->xyz.x = 4.0f * fl_abs(angle.xyz.x) / (time * time);
1620 acc_max->xyz.y = 4.0f * fl_abs(angle.xyz.y) / (time * time);
1621 acc_max->xyz.z = 4.0f * fl_abs(angle.xyz.z) / (time * time);
1622
1623 // find angular velocity limits
1624 // w_max = acc_max * time / 2
1625 w_max->xyz.x = acc_max->xyz.x * time / 2.0f;
1626 w_max->xyz.y = acc_max->xyz.y * time / 2.0f;
1627 w_max->xyz.z = acc_max->xyz.z * time / 2.0f;
1628 }
1629 }
1630
1631 #define OVERSHOOT_PREVENTION_PADDING 0.98f
1632 // physically models within the frame the physical behavior to get to a goal position
1633 // given an arbitrary initial velocity
vm_angular_move_1dimension_calc(float goal,float * vel,float delta_t,float * dist,float vel_limit,float acc_limit)1634 void vm_angular_move_1dimension_calc(float goal, float* vel, float delta_t,
1635 float* dist, float vel_limit, float acc_limit)
1636 {
1637 // These diagrams depict two common situations, and although they both start with negative velocity
1638 // for illustrative purposes, it is not necessary and the apex may be at the present or even in the past.
1639 //
1640 // t1 < t2 means there is a straight segment, so we coast for some time
1641 //
1642 // _..--- goal
1643 // now .''
1644 // | .' |
1645 // v .' |
1646 // . .' t_straight
1647 // ''-.-''|
1648 // apex t1 (= t_up)
1649 //
1650 // t1 > t2, no straight segment, accelerate then deccelerate, no coasting
1651 //
1652 // now _..--- goal
1653 // | .'
1654 // v .
1655 // . .|
1656 // ''-.-'' |
1657 // apex t2 (= t_up)
1658 //
1659
1660 float t1 = (vel_limit - *vel) / acc_limit; // time to accelerate from the current velocity (possibly negative) to +vel_limit
1661 float apex_t = -*vel / acc_limit; // the time when we had / will have velocity zero, assuming acceleration at +acc_limit
1662 float apex = *vel * apex_t / 2; // the position we had / will have at the apex
1663 float switchover_point = OVERSHOOT_PREVENTION_PADDING / (OVERSHOOT_PREVENTION_PADDING + 1.f); // when on the path
1664 // we switch from accelerating to deccelerating (very close to 1/2)
1665 float half_dist = (goal - *dist - apex) * switchover_point; // half the distance from apex to goal (where we hit peak velocity)
1666 float t2 = apex_t + fl_sqrt(2 * half_dist / acc_limit); // The time at which we reach half_dist, assuming we never hit vel_limit
1667 float t_up = fmin(delta_t, fmin(t1, t2)); // We exit the initial upward curve when we either hit vel_limit (t1)
1668 // or we start the approach to the goal (t2), so t_up is the min
1669
1670 // add distance and vel for t_up
1671 *dist += *vel * t_up + acc_limit * t_up * t_up / 2;
1672 *vel += acc_limit * t_up;
1673 // If we have run out of time in the frame, break, else advance by t_up
1674 if (delta_t <= t_up) return;
1675 delta_t -= t_up;
1676
1677 // If t1 <= t2 then we have a straight segment (cruising at +vel_limit)
1678 if (t1 <= t2) {
1679 // time it takes to reach the approach
1680 float t_straight = fmin(delta_t, (goal - 0.5f * vel_limit * vel_limit / acc_limit - *dist) / vel_limit);
1681 // add distance and vel
1682 *dist += vel_limit * t_straight;
1683 // If we have run out of time in the frame, break, else advance by t_straight
1684 if (delta_t <= t_straight) return;
1685 delta_t -= t_straight;
1686 }
1687
1688 // On approach to the goal, with acceleration -acc_limit
1689 // Our current velocity is either vel_limit if we had a straight segment, or the peak velocity at half_dist
1690 // slow down our acc very slightly to avoid possible time costly overshoot
1691 acc_limit *= OVERSHOOT_PREVENTION_PADDING;
1692 // t_down is the time to slow to a stop
1693 float t_down = fmin(delta_t, *vel / acc_limit);
1694 // add distance and vel for t_down
1695 *dist += *vel * t_down - acc_limit * t_down * t_down / 2;
1696 *vel -= acc_limit * t_down;
1697 // If we have run out of time in the frame, break, else advance by t_down
1698 if (delta_t <= t_down) return;
1699
1700 // We've arrived
1701 *dist = goal;
1702 *vel = 0;
1703 }
1704
1705 // physically models within the frame the physical behavior to get to a one-dimensional goal position
1706 // given an arbitrary initial velocity
vm_angular_move_1dimension(float goal,float delta_t,float * vel,float vel_limit,float acc_limit,float slowdown_factor,bool force_no_overshoot)1707 float vm_angular_move_1dimension(float goal, float delta_t, float* vel, float vel_limit, float acc_limit, float slowdown_factor, bool force_no_overshoot)
1708 {
1709 float effective_vel_limit = slowdown_factor == 0 ? 0 : slowdown_factor * vel_limit;
1710 float effective_acc_limit = slowdown_factor == 0 ? 0 : slowdown_factor * acc_limit;
1711 if (acc_limit <= 0) return *vel * delta_t; // Can't accelerate? No point in continuing!
1712 float dist = 0;
1713 float t_slow = fmin(delta_t, (fabs(*vel) - effective_vel_limit) / acc_limit); // Time until we get down to our max speed
1714 if (t_slow > 0) { // If that's zero (were at max) or negative (below max)
1715 float acc = *vel >= 0 ? -acc_limit : acc_limit; // excellent, but otherwise, there's no choices to make
1716 dist += *vel * t_slow + acc * t_slow * t_slow / 2; // slam on the brakes and continue only if there was enough
1717 *vel += acc * t_slow; // time in the frame to get down to max
1718 if (delta_t <= t_slow) return dist;
1719 delta_t -= t_slow;
1720 }
1721 if (effective_vel_limit <= 0 || effective_acc_limit <= 0) return dist + *vel * delta_t; // Can't move (from slowdown factor or otherwise)? Also no point in continuing!
1722
1723
1724 float goal_trajectory_speed = fl_sqrt(2.0f * acc_limit * fabsf(goal));
1725 bool should_acc_upwards = goal >= 0 ? *vel < goal_trajectory_speed : *vel < -goal_trajectory_speed;
1726 // This makes sure that the initial acceleration is always positive
1727 // If the goal is above, or if it's below but our vel will put it above us before we can slow down enough, we're good
1728 if (should_acc_upwards) {
1729 if (goal < 0 && force_no_overshoot)
1730 *vel = -goal_trajectory_speed; // With no overshoot our input velocity is always to set to the perfect trajectory
1731 vm_angular_move_1dimension_calc(goal, vel, delta_t, &dist, effective_vel_limit, effective_acc_limit);
1732
1733 }
1734 else { // else flip it so our goal is above and again we get initial positive accel
1735 if (goal > 0 && force_no_overshoot)
1736 *vel = goal_trajectory_speed; // With no overshoot our input velocity is always to set to the perfect trajectory
1737 *vel = -*vel, dist = -dist;
1738 vm_angular_move_1dimension_calc(-goal, vel, delta_t, &dist, effective_vel_limit, effective_acc_limit);
1739 *vel = -*vel, dist = -dist;
1740 }
1741 return dist;
1742 }
1743
time_to_arrival_calc(float goal,float vel,float vel_limit,float acc_limit)1744 float time_to_arrival_calc(float goal, float vel, float vel_limit, float acc_limit) {
1745 float t1 = (vel_limit - vel) / acc_limit; // time to accelerate from the current velocity (possibly negative) to +vel_limit
1746 float apex_t = -vel / acc_limit; // the time when we had / will have velocity zero, assuming acceleration at +acc_limit
1747 float apex = vel * apex_t / 2; // the position we had / will have at the apex
1748 float half_dist = (goal - apex) / 2; // half the distance from apex to goal (where we hit peak velocity)
1749 float t2 = apex_t + fl_sqrt(2 * half_dist / acc_limit); // The time at which we reach half_dist, assuming we never hit vel_limit
1750
1751 float time; // accumulated time to arrival
1752
1753 // If t1 <= t2 then we have a straight segment (cruising at +vel_limit)
1754 if (t1 <= t2) {
1755 float dist = vel * t1 + acc_limit * t1 * t1 / 2; // at the end of the upward bend we are at dist
1756 vel = vel_limit; // and we reach velocity vel_limit
1757 // and the time at the start of the approach is t1 + t_straight
1758 time = t1 + (goal - 0.5f * vel_limit * vel_limit / acc_limit - dist) / vel_limit;
1759 }
1760 else {
1761 // If t2 < t1 then there is no straight segment, we just accelerate until the approach
1762 // so time = t2 and vel is however much we can accelerate in that time
1763 time = t2;
1764 vel += acc_limit * t2;
1765 }
1766 // The total time is the time to the approach + the deceleration time
1767 return time + vel / acc_limit;
1768 }
1769
1770 // called by vm_angular_move to compute a slowing factor
1771 // pared down versions of the 1dimension functions
time_to_arrival(float goal,float vel,float vel_limit,float acc_limit)1772 float time_to_arrival(float goal, float vel, float vel_limit, float acc_limit) {
1773 // We won't consider speeds above our max, the time estimate gets complicated and the result won't be a straight line anyway
1774 if (fabs(vel) > vel_limit) {
1775 vel = vel > 0 ? vel_limit : -vel_limit;
1776 }
1777 return (vel < (goal >= 0 ? fl_sqrt(2.0f * acc_limit * goal) : -fl_sqrt(2.0f * acc_limit * -goal))) // same thing as scalar interpolate
1778 ? time_to_arrival_calc(goal, vel, vel_limit, acc_limit)
1779 : time_to_arrival_calc(-goal, -vel, vel_limit, acc_limit);
1780 }
1781
1782 // splits up the accelerating/deccelerating/go to position function for each component
1783 // and also scales their speed to make a nice straight line
1784 // note that this is now treated as a movement in linear space, despite the name
vm_angular_move(const vec3d * goal,float delta_t,vec3d * vel,const vec3d * vel_limit,const vec3d * acc_limit,bool aggressive_bank,bool force_no_overshoot,bool no_directional_bias)1785 vec3d vm_angular_move(const vec3d* goal, float delta_t,
1786 vec3d* vel, const vec3d* vel_limit, const vec3d* acc_limit, bool aggressive_bank, bool force_no_overshoot, bool no_directional_bias)
1787 {
1788 vec3d ret, slow;
1789 vm_vec_make(&slow, 1.f, 1.f, 1.f);
1790 if (no_directional_bias) {
1791 // first, the estimated time to arrive at the goal angular position is calculated for each component
1792 slow.xyz.x = time_to_arrival(goal->xyz.x, vel->xyz.x, vel_limit->xyz.x, acc_limit->xyz.x);
1793 slow.xyz.y = time_to_arrival(goal->xyz.y, vel->xyz.y, vel_limit->xyz.y, acc_limit->xyz.y);
1794 slow.xyz.z = time_to_arrival(goal->xyz.z, vel->xyz.z, vel_limit->xyz.z, acc_limit->xyz.z);
1795
1796 // then, compute a slowing factor for the 1 or 2 faster-to-arrive-at-their-destination components
1797 // so they arrive at approximately the same time as the slowest component, so the path there is nice and straight
1798 float max = fmax(slow.xyz.x, fmax(slow.xyz.y, slow.xyz.z));
1799 if (max != 0) vm_vec_scale(&slow, 1 / max);
1800 if (aggressive_bank) slow.xyz.z = 1.f;
1801 }
1802
1803 ret.xyz.x = vm_angular_move_1dimension(goal->xyz.x, delta_t, &vel->xyz.x, vel_limit->xyz.x, acc_limit->xyz.x, slow.xyz.x, force_no_overshoot);
1804 ret.xyz.y = vm_angular_move_1dimension(goal->xyz.y, delta_t, &vel->xyz.y, vel_limit->xyz.y, acc_limit->xyz.y, slow.xyz.y, force_no_overshoot);
1805 ret.xyz.z = vm_angular_move_1dimension(goal->xyz.z, delta_t, &vel->xyz.z, vel_limit->xyz.z, acc_limit->xyz.z, slow.xyz.z, force_no_overshoot);
1806 return ret;
1807 }
1808
1809 // ---------------------------------------------------------------------------------------------
1810 //
1811 // inputs: goal_orient => goal orientation matrix
1812 // curr_orient => current orientation matrix
1813 // w_in => current input angular velocity
1814 // delta_t => time to move toward goal
1815 // next_orient => the orientation matrix at time delta_t (with current forward vector)
1816 // w_out => the angular velocity of the ship at delta_t
1817 // vel_limit => maximum rotational speed
1818 // acc_limit => maximum rotational acceleration
1819 // no_directional_bias => will cause the angular path generated to be as straight as possible, rather than greedily
1820 // turning at maximum on all axes (and thus possibly produce a 'crooked' path)
1821 // force_no_overshoot => forces the interpolation to not overshoot, if it is approaching its goal too fast
1822 // it will always arrive with 0 velocity, even if its acceleration would not normally
1823 // allow it slow down in time
1824 //
1825 // Asteroth - this replaced retail's "vm_matrix_interpolate" in PR 2668.
1826 // The produced behavior is on average 0.52% slower (std dev 0.74%) than the retail function
1827 // Roughly twice that if framerate_independent_turning is enabled.
1828 //
1829 // The function attempts to rotate the input matrix into the goal matrix taking account of anglular
1830 // momentum (velocity)
vm_angular_move_matrix(const matrix * goal_orient,const matrix * curr_orient,const vec3d * w_in,float delta_t,matrix * next_orient,vec3d * w_out,const vec3d * vel_limit,const vec3d * acc_limit,bool no_directional_bias,bool force_no_overshoot)1831 void vm_angular_move_matrix(const matrix* goal_orient, const matrix* curr_orient, const vec3d* w_in, float delta_t,
1832 matrix* next_orient, vec3d* w_out, const vec3d* vel_limit, const vec3d* acc_limit, bool no_directional_bias, bool force_no_overshoot)
1833 {
1834 // Find rotation needed for goal
1835 // goal_orient = R curr_orient, so R = goal_orient curr_orient^-1
1836 matrix Mtemp1;
1837 vm_copy_transpose(&Mtemp1, curr_orient); // Mtemp1 = curr ^-1
1838 matrix rot_matrix; // rotation matrix from curr_orient to goal_orient
1839 vm_matrix_x_matrix(&rot_matrix, &Mtemp1, goal_orient); // R = goal * Mtemp1
1840 vm_orthogonalize_matrix(&rot_matrix);
1841 vec3d rot_axis; // vector indicating direction of rotation axis
1842 float theta; // magnitude of rotation about the rotation axis
1843 vm_matrix_to_rot_axis_and_angle(&rot_matrix, &theta, &rot_axis); // determines angle and rotation axis from curr to goal
1844
1845 // find theta to goal
1846 vec3d theta_goal; // desired angular position at the end of the time interval
1847 vm_vec_copy_scale(&theta_goal, &rot_axis, theta);
1848
1849 // continue to interpolate, unless we are at the goal with no velocity, in which case we have arrived
1850 if (theta < SMALL_NUM && vm_vec_mag_squared(w_in) < SMALL_NUM * SMALL_NUM) {
1851 *next_orient = *goal_orient;
1852 vm_vec_zero(w_out);
1853 return;
1854 }
1855
1856 // calculate best approach in linear space (returns velocity in w_out and position difference in rot_axis)
1857 *w_out = *w_in;
1858 rot_axis = vm_angular_move(&theta_goal, delta_t, w_out, vel_limit, acc_limit, false, force_no_overshoot, no_directional_bias);
1859
1860 // arrived at goal? (equality comparison is okay here because vm_vector_interpolate returns theta_goal on arrival)
1861 if (rot_axis == theta_goal) {
1862 *next_orient = *goal_orient;
1863 // rotate velocity out to reflect new local frame
1864 vec3d vtemp = *w_out;
1865 vm_vec_rotate(w_out, &vtemp, &rot_matrix);
1866 return;
1867 }
1868
1869 // normalize rotation axis and determine total rotation angle
1870 theta = vm_vec_mag(&rot_axis);
1871 if (theta > SMALL_NUM)
1872 vm_vec_scale(&rot_axis, 1 / theta);
1873
1874 // if the positional change is small, reuse orient (and return because velocity is already set)
1875 if (theta < SMALL_NUM) {
1876 *next_orient = *curr_orient;
1877 return;
1878 }
1879
1880 // otherwise rotate orient by theta along rot_axis
1881 vm_quaternion_rotate(&Mtemp1, theta, &rot_axis);
1882 Assert(is_valid_matrix(&Mtemp1));
1883 vm_matrix_x_matrix(next_orient, curr_orient, &Mtemp1);
1884 vm_orthogonalize_matrix(next_orient);
1885 // and rotate velocity out to reflect new local frame
1886 vec3d vtemp = *w_out;
1887 vm_vec_rotate(w_out, &vtemp, &Mtemp1);
1888 }
1889
1890
1891 // ---------------------------------------------------------------------------------------------
1892 //
1893 // inputs: goal_f => goal forward vector
1894 // orient => current orientation matrix (with current forward vector)
1895 // w_in => current input angular velocity
1896 // delta_t => this frametime
1897 // delta_bank => desired change in bank in degrees
1898 // next_orient => the orientation matrix at time delta_t (with current forward vector)
1899 // w_out => the angular velocity of the ship at delta_t
1900 // vel_limit => maximum rotational speed
1901 // acc_limit => maximum rotational acceleration
1902 // no_directional_bias => will cause the angular path generated to be as straight as possible, rather than greedily
1903 // turning at maximum on all axes (and thus possibly produce a 'crooked' path)
1904 //
1905 // Asteroth - this replaced retail's "vm_forward_interpolate" in PR 2668.
1906 // The produced behavior is on average 0.06% slower (std dev 0.32%) than the retail function
1907 // Roughly twice that if framerate_independent_turning is enabled, or if the object is a missile.
1908 //
1909 // function attempts to rotate the forward vector toward the goal forward vector taking account of anglular
1910 // momentum (velocity) Attempt to try to move bank by goal delta_bank.
1911 // called "vm_forward_interpolate" in retail
vm_angular_move_forward_vec(const vec3d * goal_f,const matrix * orient,const vec3d * w_in,float delta_t,float delta_bank,matrix * next_orient,vec3d * w_out,const vec3d * vel_limit,const vec3d * acc_limit,bool no_directional_bias)1912 void vm_angular_move_forward_vec(const vec3d* goal_f, const matrix* orient, const vec3d* w_in, float delta_t, float delta_bank,
1913 matrix* next_orient, vec3d* w_out, const vec3d* vel_limit, const vec3d* acc_limit, bool no_directional_bias)
1914 {
1915 vec3d rot_axis;
1916 vm_vec_cross(&rot_axis, &orient->vec.fvec, goal_f); // Get the direction to rotate to the goal
1917 float cos_theta = vm_vec_dot(&orient->vec.fvec, goal_f); // Get cos(theta) where theta is the amount to rotate
1918 float sin_theta = fmin(vm_vec_mag(&rot_axis), 1.0f); // Get sin(theta) (cap at 1 for floating point errors)
1919 vec3d theta_goal;
1920 vm_vec_make(&theta_goal, 0, 0, delta_bank); // theta_goal will contain the radians to rotate (in the same direction as rot_axis but in local coords)
1921
1922 if (sin_theta <= SMALL_NUM) { // sin(theta) is small so we are either very close or very far
1923 if (cos_theta < 0) { // cos(theta) < 0, sin(theta) ~ 0 means we are pointed exactly the opposite way
1924 float w_mag_sq = w_in->xyz.x * w_in->xyz.x + w_in->xyz.y * w_in->xyz.y;
1925 if (w_mag_sq <= SMALL_NUM * SMALL_NUM) { // if we have ~ no angular velocity
1926 theta_goal.xyz.x = PI; // Rotate in x direction (arbitrarily)
1927 }
1928 else { // otherwise prefer to rotate in the direction of angular velocity
1929 float d = PI / fl_sqrt(w_mag_sq);
1930 theta_goal.xyz.x = w_in->xyz.x * d;
1931 theta_goal.xyz.y = w_in->xyz.y * d;
1932 }
1933 }
1934 // continue to interpolate, unless we also have no velocity, in which case we have arrived
1935 else if (vm_vec_mag_squared(w_in) < SMALL_NUM * SMALL_NUM) {
1936 *next_orient = *orient;
1937 vm_vec_zero(w_out);
1938 return;
1939 }
1940 }
1941 else {
1942 vec3d local_rot_axis;
1943 // rotate rot_axis into ship reference frame
1944 vm_vec_rotate(&local_rot_axis, &rot_axis, orient);
1945
1946 // derive theta from sin(theta) for better accuracy
1947 vm_vec_copy_scale(&theta_goal, &local_rot_axis, (cos_theta > 0 ? asinf_safe(sin_theta) : PI - asinf_safe(sin_theta)) / sin_theta);
1948
1949 // reset z to delta_bank, because it just got cleared
1950 theta_goal.xyz.z = delta_bank;
1951 }
1952
1953 // calculate best approach in linear space (returns velocity in w_out and position difference in rot_axis)
1954 *w_out = *w_in;
1955 rot_axis = vm_angular_move(&theta_goal, delta_t, w_out, vel_limit, acc_limit, true, false, no_directional_bias);
1956
1957 // normalize rotation axis and determine total rotation angle
1958 float theta = vm_vec_mag(&rot_axis);
1959 if (theta > SMALL_NUM)
1960 vm_vec_scale(&rot_axis, 1 / theta);
1961
1962 // if the positional change is small, reuse orient (and return because velocity is already set)
1963 if (theta < SMALL_NUM) {
1964 *next_orient = *orient;
1965 return;
1966 }
1967
1968 // otherwise rotate orient by theta along rot_axis
1969 matrix Mtemp1;
1970 vm_quaternion_rotate(&Mtemp1, theta, &rot_axis);
1971 vm_matrix_x_matrix(next_orient, orient, &Mtemp1);
1972 Assert(is_valid_matrix(next_orient));
1973 // and rotate velocity out to reflect new local frame
1974 vec3d vtemp = *w_out;
1975 vm_vec_rotate(w_out, &vtemp, &Mtemp1);
1976 }
1977
1978
1979
1980
1981 // ------------------------------------------------------------------------------------
1982 // vm_find_bounding_sphere()
1983 //
1984 // Calculate a bounding sphere for a set of points.
1985 //
1986 // input: pnts => array of world positions
1987 // num_pnts => number of points inside pnts array
1988 // center => OUTPUT PARAMETER: contains world pos of bounding sphere center
1989 // radius => OUTPUT PARAMETER: continas radius of bounding sphere
1990 //
1991 #define BIGNUMBER 100000000.0f
vm_find_bounding_sphere(const vec3d * pnts,int num_pnts,vec3d * center,float * radius)1992 void vm_find_bounding_sphere(const vec3d *pnts, int num_pnts, vec3d *center, float *radius)
1993 {
1994 int i;
1995 float rad, rad_sq, xspan, yspan, zspan, maxspan;
1996 float old_to_p, old_to_p_sq, old_to_new;
1997 vec3d diff, xmin, xmax, ymin, ymax, zmin, zmax, dia1, dia2;
1998 const vec3d *p;
1999
2000 xmin = vmd_zero_vector;
2001 ymin = vmd_zero_vector;
2002 zmin = vmd_zero_vector;
2003 xmax = vmd_zero_vector;
2004 ymax = vmd_zero_vector;
2005 zmax = vmd_zero_vector;
2006 xmin.xyz.x = ymin.xyz.y = zmin.xyz.z = BIGNUMBER;
2007 xmax.xyz.x = ymax.xyz.y = zmax.xyz.z = -BIGNUMBER;
2008
2009 for ( i = 0; i < num_pnts; i++ ) {
2010 p = &pnts[i];
2011 if ( p->xyz.x < xmin.xyz.x )
2012 xmin = *p;
2013 if ( p->xyz.x > xmax.xyz.x )
2014 xmax = *p;
2015 if ( p->xyz.y < ymin.xyz.y )
2016 ymin = *p;
2017 if ( p->xyz.y > ymax.xyz.y )
2018 ymax = *p;
2019 if ( p->xyz.z < zmin.xyz.z )
2020 zmin = *p;
2021 if ( p->xyz.z > zmax.xyz.z )
2022 zmax = *p;
2023 }
2024
2025 // find distance between two min and max points (squared)
2026 vm_vec_sub(&diff, &xmax, &xmin);
2027 xspan = vm_vec_mag_squared(&diff);
2028
2029 vm_vec_sub(&diff, &ymax, &ymin);
2030 yspan = vm_vec_mag_squared(&diff);
2031
2032 vm_vec_sub(&diff, &zmax, &zmin);
2033 zspan = vm_vec_mag_squared(&diff);
2034
2035 dia1 = xmin;
2036 dia2 = xmax;
2037 maxspan = xspan;
2038 if ( yspan > maxspan ) {
2039 maxspan = yspan;
2040 dia1 = ymin;
2041 dia2 = ymax;
2042 }
2043 if ( zspan > maxspan ) {
2044 maxspan = zspan;
2045 dia1 = zmin;
2046 dia2 = zmax;
2047 }
2048
2049 // calc initial center
2050 vm_vec_add(center, &dia1, &dia2);
2051 vm_vec_scale(center, 0.5f);
2052
2053 vm_vec_sub(&diff, &dia2, center);
2054 rad_sq = vm_vec_mag_squared(&diff);
2055 rad = fl_sqrt(rad_sq);
2056 Assert( !fl_is_nan(rad) );
2057
2058 // second pass
2059 for ( i = 0; i < num_pnts; i++ ) {
2060 p = &pnts[i];
2061 vm_vec_sub(&diff, p, center);
2062 old_to_p_sq = vm_vec_mag_squared(&diff);
2063 if ( old_to_p_sq > rad_sq ) {
2064 old_to_p = fl_sqrt(old_to_p_sq);
2065 // calc radius of new sphere
2066 rad = (rad + old_to_p) / 2.0f;
2067 rad_sq = rad * rad;
2068 old_to_new = old_to_p - rad;
2069 // calc new center of sphere
2070 center->xyz.x = (rad*center->xyz.x + old_to_new*p->xyz.x) / old_to_p;
2071 center->xyz.y = (rad*center->xyz.y + old_to_new*p->xyz.y) / old_to_p;
2072 center->xyz.z = (rad*center->xyz.z + old_to_new*p->xyz.z) / old_to_p;
2073 nprintf(("Alan", "New sphere: cen,rad = %f %f %f %f\n", center->xyz.x, center->xyz.y, center->xyz.z, rad));
2074 }
2075 }
2076
2077 *radius = rad;
2078 }
2079
2080 // ----------------------------------------------------------------------------
2081 // vm_rotate_vec_to_body()
2082 //
2083 // rotates a vector from world coordinates to body coordinates
2084 //
2085 // inputs: body_vec => vector in body coordinates
2086 // world_vec => vector in world coordinates
2087 // orient => orientation matrix
2088 //
vm_rotate_vec_to_body(vec3d * body_vec,const vec3d * world_vec,const matrix * orient)2089 vec3d* vm_rotate_vec_to_body(vec3d *body_vec, const vec3d *world_vec, const matrix *orient)
2090 {
2091 return vm_vec_unrotate(body_vec, world_vec, orient);
2092 }
2093
2094
2095 // ----------------------------------------------------------------------------
2096 // vm_rotate_vec_to_world()
2097 //
2098 // rotates a vector from body coordinates to world coordinates
2099 //
2100 // inputs world_vec => vector in world coordinates
2101 // body_vec => vector in body coordinates
2102 // orient => orientation matrix
2103 //
vm_rotate_vec_to_world(vec3d * world_vec,const vec3d * body_vec,const matrix * orient)2104 vec3d* vm_rotate_vec_to_world(vec3d *world_vec, const vec3d *body_vec, const matrix *orient)
2105 {
2106 return vm_vec_rotate(world_vec, body_vec, orient);
2107 }
2108
2109
2110 // ----------------------------------------------------------------------------
2111 // vm_estimate_next_orientation()
2112 //
2113 // given a last orientation and current orientation, estimate the next orientation
2114 //
2115 // inputs: last_orient => last orientation matrix
2116 // current_orient => current orientation matrix
2117 // next_orient => next orientation matrix [the result]
2118 //
vm_estimate_next_orientation(const matrix * last_orient,const matrix * current_orient,matrix * next_orient)2119 void vm_estimate_next_orientation(const matrix *last_orient, const matrix *current_orient, matrix *next_orient)
2120 {
2121 // R L = C => R = C (L)^-1
2122 // N = R C => N = C (L)^-1 C
2123
2124 matrix Mtemp;
2125 matrix Rot_matrix;
2126 vm_copy_transpose(&Mtemp, last_orient); // Mtemp = (L)^-1
2127 vm_matrix_x_matrix(&Rot_matrix, &Mtemp, current_orient); // R = C Mtemp1
2128 vm_matrix_x_matrix(next_orient, current_orient, &Rot_matrix);
2129 }
2130
2131 // Return true if all elements of *vec are legal, that is, not NaN or infinity.
is_valid_vec(const vec3d * vec)2132 bool is_valid_vec(const vec3d *vec)
2133 {
2134 return !std::isnan(vec->xyz.x) && !std::isnan(vec->xyz.y) && !std::isnan(vec->xyz.z)
2135 && !std::isinf(vec->xyz.x) && !std::isinf(vec->xyz.y) && !std::isinf(vec->xyz.z);
2136 }
2137
2138 // Return true if all elements of *m are legal, that is, not a NAN.
is_valid_matrix(const matrix * m)2139 bool is_valid_matrix(const matrix *m)
2140 {
2141 return is_valid_vec(&m->vec.fvec) && is_valid_vec(&m->vec.uvec) && is_valid_vec(&m->vec.rvec);
2142 }
2143
2144 // interpolate between 2 vectors. t goes from 0.0 to 1.0. at
vm_vec_interp_constant(vec3d * out,const vec3d * v0,const vec3d * v1,float t)2145 void vm_vec_interp_constant(vec3d *out, const vec3d *v0, const vec3d *v1, float t)
2146 {
2147 vec3d cross;
2148 float total_ang;
2149
2150 // get the cross-product of the 2 vectors
2151 vm_vec_cross(&cross, v0, v1);
2152 vm_vec_normalize(&cross);
2153
2154 // get the total angle between the 2 vectors
2155 total_ang = -(acosf_safe(vm_vec_dot(v0, v1)));
2156
2157 // rotate around the cross product vector by the appropriate angle
2158 vm_rot_point_around_line(out, v0, t * total_ang, &vmd_zero_vector, &cross);
2159 }
2160
2161 // randomly perturb a vector around a given (normalized vector) or optional orientation matrix
vm_vec_random_cone(vec3d * out,const vec3d * in,float max_angle,const matrix * orient)2162 void vm_vec_random_cone(vec3d *out, const vec3d *in, float max_angle, const matrix *orient)
2163 {
2164 vec3d temp;
2165 const matrix *rot;
2166 matrix m;
2167
2168 // get an orientation matrix
2169 if(orient != NULL){
2170 rot = orient;
2171 } else {
2172 vm_vector_2_matrix(&m, in, NULL, NULL);
2173 rot = &m;
2174 }
2175
2176 // Get properly distributed spherical coordinates (DahBlount)
2177 float z = util::UniformFloatRange(cosf(fl_radians(max_angle)), 1.0f).next(); // Take a 2-sphere slice
2178 float phi = util::UniformFloatRange(0.0f, PI2).next();
2179 vm_vec_make( &temp, sqrtf(1.0f - z*z)*cosf(phi), sqrtf(1.0f - z*z)*sinf(phi), z ); // Using the z-vec as the starting point
2180
2181 vm_vec_unrotate(out, &temp, rot); // We find the final vector by rotating temp to the correct orientation
2182 }
2183
vm_vec_random_cone(vec3d * out,const vec3d * in,float min_angle,float max_angle,const matrix * orient)2184 void vm_vec_random_cone(vec3d *out, const vec3d *in, float min_angle, float max_angle, const matrix *orient){
2185 vec3d temp;
2186 const matrix *rot;
2187 matrix m;
2188
2189 if (max_angle < min_angle) {
2190 auto tmp = min_angle;
2191 min_angle = max_angle;
2192 max_angle = tmp;
2193 }
2194
2195 // get an orientation matrix
2196 if(orient != NULL){
2197 rot = orient;
2198 } else {
2199 vm_vector_2_matrix(&m, in, NULL, NULL);
2200 rot = &m;
2201 }
2202
2203 // Get properly distributed spherical coordinates (DahBlount)
2204 // This might not seem intuitive, but the min_angle is the angle that will have a larger z coordinate
2205 float z = util::UniformFloatRange(cosf(fl_radians(max_angle)), cosf(fl_radians(min_angle))).next(); // Take a 2-sphere slice
2206 float phi = util::UniformFloatRange(0.0f, PI2).next();
2207 vm_vec_make( &temp, sqrtf(1.0f - z*z)*cosf(phi), sqrtf(1.0f - z*z)*sinf(phi), z ); // Using the z-vec as the starting point
2208
2209 vm_vec_unrotate(out, &temp, rot); // We find the final vector by rotating temp to the correct orientation
2210 }
2211
2212
2213 // given a start vector, an orientation, and a radius, generate a point on the plane of the circle
2214 // if on_edge is true, the point will be on the edge of the circle
vm_vec_random_in_circle(vec3d * out,const vec3d * in,const matrix * orient,float radius,bool on_edge,bool bias_towards_center)2215 void vm_vec_random_in_circle(vec3d *out, const vec3d *in, const matrix *orient, float radius, bool on_edge, bool bias_towards_center)
2216 {
2217 vec3d temp;
2218 float scalar = frand();
2219
2220 // sqrt because scaling inward increases the probability density by the square of its proximity towards the center
2221 if (!bias_towards_center)
2222 scalar = sqrtf(scalar);
2223
2224 // point somewhere in the plane, maybe scaled inward
2225 vm_vec_scale_add(&temp, in, &orient->vec.rvec, on_edge ? radius : scalar * radius);
2226
2227 // rotate to a random point on the circle
2228 vm_rot_point_around_line(out, &temp, fl_radians(frand_range(0.0f, 360.0f)), in, &orient->vec.fvec);
2229 }
2230
2231 // given a start vector and a radius, generate a point in a spherical volume
2232 // if on_surface is true, the point will be on the surface of the sphere
vm_vec_random_in_sphere(vec3d * out,const vec3d * in,float radius,bool on_surface,bool bias_towards_center)2233 void vm_vec_random_in_sphere(vec3d *out, const vec3d *in, float radius, bool on_surface, bool bias_towards_center)
2234 {
2235 vec3d temp;
2236
2237 float z = util::UniformFloatRange(-1, 1).next(); // Take a 2-sphere slice
2238 float phi = util::UniformFloatRange(0.0f, PI2).next();
2239 vm_vec_make(&temp, sqrtf(1.0f - z * z) * cosf(phi), sqrtf(1.0f - z * z) * sinf(phi), z); // Using the z-vec as the starting point
2240
2241 float scalar = util::UniformFloatRange(0.0f, 1.0f).next();
2242
2243 // cube root because scaling inward increases the probability density by the cube of its proximity towards the center
2244 if (!bias_towards_center)
2245 scalar = powf(scalar, 0.333f);
2246
2247 vm_vec_scale_add(out, in, &temp, on_surface ? radius : scalar * radius);
2248 }
2249
2250 // find the nearest point on the line to p. if dist is non-NULL, it is filled in
2251 // returns 0 if the point is inside the line segment, -1 if "before" the line segment and 1 ir "after" the line segment
vm_vec_dist_to_line(const vec3d * p,const vec3d * l0,const vec3d * l1,vec3d * nearest,float * dist)2252 int vm_vec_dist_to_line(const vec3d *p, const vec3d *l0, const vec3d *l1, vec3d *nearest, float *dist)
2253 {
2254 vec3d a, b, c;
2255 float b_mag, comp;
2256
2257 #ifndef NDEBUG
2258 if(vm_vec_same(l0, l1)){
2259 *nearest = vmd_zero_vector;
2260 return -1;
2261 }
2262 #endif
2263
2264 // compb_a == a dot b / len(b)
2265 vm_vec_sub(&a, p, l0);
2266 vm_vec_sub(&b, l1, l0);
2267 b_mag = vm_vec_copy_normalize(&c, &b);
2268
2269 // calculate component
2270 comp = vm_vec_dot(&a, &b) / b_mag;
2271
2272 // stuff nearest
2273 vm_vec_scale_add(nearest, l0, &c, comp);
2274
2275 // maybe get the distance
2276 if(dist != NULL){
2277 *dist = vm_vec_dist(nearest, p);
2278 }
2279
2280 // return the proper value
2281 if(comp < 0.0f){
2282 return -1; // before the line
2283 } else if(comp > b_mag){
2284 return 1; // after the line
2285 }
2286 return 0; // on the line
2287 }
2288
2289 // Goober5000
2290 // Finds the distance squared to a line. Same as above, except it uses vm_vec_dist_squared, which is faster;
2291 // and it doesn't check whether the nearest point is on the line segment.
vm_vec_dist_squared_to_line(const vec3d * p,const vec3d * l0,const vec3d * l1,vec3d * nearest,float * dist_squared)2292 void vm_vec_dist_squared_to_line(const vec3d *p, const vec3d *l0, const vec3d *l1, vec3d *nearest, float *dist_squared)
2293 {
2294 vec3d a, b, c;
2295 float b_mag, comp;
2296
2297 #ifndef NDEBUG
2298 if(vm_vec_same(l0, l1)){
2299 *nearest = vmd_zero_vector;
2300 return;
2301 }
2302 #endif
2303
2304 // compb_a == a dot b / len(b)
2305 vm_vec_sub(&a, p, l0);
2306 vm_vec_sub(&b, l1, l0);
2307 b_mag = vm_vec_copy_normalize(&c, &b);
2308
2309 // calculate component
2310 comp = vm_vec_dot(&a, &b) / b_mag;
2311
2312 // stuff nearest
2313 vm_vec_scale_add(nearest, l0, &c, comp);
2314
2315 // get the distance
2316 *dist_squared = vm_vec_dist_squared(nearest, p);
2317 }
2318
2319 //SUSHI: 2D vector "box" scaling
2320 //Scales the vector in-place so that the longest dimension = scale
vm_vec_boxscale(vec2d * vec,float)2321 void vm_vec_boxscale(vec2d *vec, float /*scale*/)
2322 {
2323 float ratio = 1.0f / MAX(fl_abs(vec->x), fl_abs(vec->y));
2324 vec->x *= ratio;
2325 vec->y *= ratio;
2326 }
2327
2328 // adds two matrices, fills in dest, returns ptr to dest
2329 // ok for dest to equal either source, but should use vm_matrix_add2() if so
2330 // dest = src0 + src1
vm_matrix_add(matrix * dest,const matrix * src0,const matrix * src1)2331 void vm_matrix_add(matrix* dest, const matrix* src0, const matrix* src1)
2332 {
2333 dest->vec.fvec = src0->vec.fvec + src1->vec.fvec;
2334 dest->vec.rvec = src0->vec.rvec + src1->vec.rvec;
2335 dest->vec.uvec = src0->vec.uvec + src1->vec.uvec;
2336 }
2337
2338 // subs two matrices, fills in dest, returns ptr to dest
2339 // ok for dest to equal either source, but should use vm_matrix_sub2() if so
2340 // dest = src0 - src1
vm_matrix_sub(matrix * dest,const matrix * src0,const matrix * src1)2341 void vm_matrix_sub(matrix* dest, const matrix* src0, const matrix* src1)
2342 {
2343 dest->vec.fvec = src0->vec.fvec - src1->vec.fvec;
2344 dest->vec.rvec = src0->vec.rvec - src1->vec.rvec;
2345 dest->vec.uvec = src0->vec.uvec - src1->vec.uvec;
2346 }
2347
2348 // adds one matrix to another.
2349 // dest can equal source
2350 // dest += src
vm_matrix_add2(matrix * dest,const matrix * src)2351 void vm_matrix_add2(matrix* dest, const matrix* src)
2352 {
2353 dest->vec.fvec += src->vec.fvec;
2354 dest->vec.rvec += src->vec.rvec;
2355 dest->vec.uvec += src->vec.uvec;
2356 }
2357
2358 // subs one matrix from another, returns ptr to dest
2359 // dest can equal source
2360 // dest -= src
vm_matrix_sub2(matrix * dest,const matrix * src)2361 void vm_matrix_sub2(matrix* dest, const matrix* src)
2362 {
2363 dest->vec.fvec -= src->vec.fvec;
2364 dest->vec.rvec -= src->vec.rvec;
2365 dest->vec.uvec -= src->vec.uvec;
2366 }
2367
2368 // TODO Remove this function if we ever move to a math library like glm
2369 /**
2370 * @brief Attempts to invert a 3x3 matrix
2371 * @param[inout] dest The inverted matrix, or 0 if inversion is impossible
2372 * @param[in] m Pointer to the matrix we want to invert
2373 *
2374 * @returns Whether or not the matrix is invertible
2375 */
vm_inverse_matrix(matrix * dest,const matrix * m)2376 bool vm_inverse_matrix(matrix* dest, const matrix* m)
2377 {
2378 // Use doubles here because this is used for ship inv_mois and we could be dealing with extremely small numbers
2379 double inv[3][3]; // create a temp matrix so we can avoid getting a determinant that is 0
2380
2381 // Use a2d so it's easier for people to read
2382 inv[0][0] = -(double)m->a2d[1][2] * (double)m->a2d[2][1] + (double)m->a2d[1][1] * (double)m->a2d[2][2];
2383 inv[0][1] = (double)m->a2d[0][2] * (double)m->a2d[2][1] - (double)m->a2d[0][1] * (double)m->a2d[2][2];
2384 inv[0][2] = -(double)m->a2d[0][2] * (double)m->a2d[1][1] + (double)m->a2d[0][1] * (double)m->a2d[1][2];
2385 inv[1][0] = (double)m->a2d[1][2] * (double)m->a2d[2][0] - (double)m->a2d[1][0] * (double)m->a2d[2][2];
2386 inv[1][1] = -(double)m->a2d[0][2] * (double)m->a2d[2][0] + (double)m->a2d[0][0] * (double)m->a2d[2][2];
2387 inv[1][2] = (double)m->a2d[0][2] * (double)m->a2d[1][0] - (double)m->a2d[0][0] * (double)m->a2d[1][2];
2388 inv[2][0] = -(double)m->a2d[1][1] * (double)m->a2d[2][0] + (double)m->a2d[1][0] * (double)m->a2d[2][1];
2389 inv[2][1] = (double)m->a2d[0][1] * (double)m->a2d[2][0] - (double)m->a2d[0][0] * (double)m->a2d[2][1];
2390 inv[2][2] = -(double)m->a2d[0][1] * (double)m->a2d[1][0] + (double)m->a2d[0][0] * (double)m->a2d[1][1];
2391
2392 double det = (double)m->a2d[0][0] * inv[0][0] + (double)m->a2d[0][1] * inv[1][0] + (double)m->a2d[0][2] * inv[2][0];
2393 if (det == 0) {
2394 *dest = vmd_zero_matrix;
2395 return false;
2396 }
2397
2398 det = 1.0f / det;
2399
2400 for (int i = 0; i < 3; i++) {
2401 for (int j = 0; j < 3; j++) {
2402 dest->a2d[i][j] = (float)(inv[i][j] * det);
2403 }
2404 }
2405
2406 return true;
2407 }
2408
2409 // TODO Remove this function if we ever move to a math library like glm
2410 /**
2411 * @brief Attempts to invert a 4x4 matrix
2412 * @param[inout] dest The inverted matrix, or 0 if inversion is impossible
2413 * @param[in] m Pointer to the matrix we want to invert
2414 *
2415 * @returns Whether or not the matrix is invertible
2416 */
vm_inverse_matrix4(matrix4 * dest,const matrix4 * m)2417 bool vm_inverse_matrix4(matrix4* dest, const matrix4* m)
2418 {
2419 matrix4 inv; // create a temp matrix so we can avoid getting a determinant that is 0
2420
2421 // Use a2d so it's easier for people to read
2422 inv.a2d[0][0] = m->a2d[1][1] * m->a2d[2][2] * m->a2d[3][3] -
2423 m->a2d[1][1] * m->a2d[2][3] * m->a2d[3][2] -
2424 m->a2d[2][1] * m->a2d[1][2] * m->a2d[3][3] +
2425 m->a2d[2][1] * m->a2d[1][3] * m->a2d[3][2] +
2426 m->a2d[3][1] * m->a2d[1][2] * m->a2d[2][3] -
2427 m->a2d[3][1] * m->a2d[1][3] * m->a2d[2][2];
2428
2429 inv.a2d[1][0] = -m->a2d[1][0] * m->a2d[2][2] * m->a2d[3][3] +
2430 m->a2d[1][0] * m->a2d[2][3] * m->a2d[3][2] +
2431 m->a2d[2][0] * m->a2d[1][2] * m->a2d[3][3] -
2432 m->a2d[2][0] * m->a2d[1][3] * m->a2d[3][2] -
2433 m->a2d[3][0] * m->a2d[1][2] * m->a2d[2][3] +
2434 m->a2d[3][0] * m->a2d[1][3] * m->a2d[2][2];
2435
2436 inv.a2d[2][0] = m->a2d[1][0] * m->a2d[2][1] * m->a2d[3][3] -
2437 m->a2d[1][0] * m->a2d[2][3] * m->a2d[3][1] -
2438 m->a2d[2][0] * m->a2d[1][1] * m->a2d[3][3] +
2439 m->a2d[2][0] * m->a2d[1][3] * m->a2d[3][1] +
2440 m->a2d[3][0] * m->a2d[1][1] * m->a2d[2][3] -
2441 m->a2d[3][0] * m->a2d[1][3] * m->a2d[2][1];
2442
2443 inv.a2d[3][0] = -m->a2d[1][0] * m->a2d[2][1] * m->a2d[3][2] +
2444 m->a2d[1][0] * m->a2d[2][2] * m->a2d[3][1] +
2445 m->a2d[2][0] * m->a2d[1][1] * m->a2d[3][2] -
2446 m->a2d[2][0] * m->a2d[1][2] * m->a2d[3][1] -
2447 m->a2d[3][0] * m->a2d[1][1] * m->a2d[2][2] +
2448 m->a2d[3][0] * m->a2d[1][2] * m->a2d[2][1];
2449
2450 inv.a2d[0][1] = -m->a2d[0][1] * m->a2d[2][2] * m->a2d[3][3] +
2451 m->a2d[0][1] * m->a2d[2][3] * m->a2d[3][2] +
2452 m->a2d[2][1] * m->a2d[0][2] * m->a2d[3][3] -
2453 m->a2d[2][1] * m->a2d[0][3] * m->a2d[3][2] -
2454 m->a2d[3][1] * m->a2d[0][2] * m->a2d[2][3] +
2455 m->a2d[3][1] * m->a2d[0][3] * m->a2d[2][2];
2456
2457 inv.a2d[1][1] = m->a2d[0][0] * m->a2d[2][2] * m->a2d[3][3] -
2458 m->a2d[0][0] * m->a2d[2][3] * m->a2d[3][2] -
2459 m->a2d[2][0] * m->a2d[0][2] * m->a2d[3][3] +
2460 m->a2d[2][0] * m->a2d[0][3] * m->a2d[3][2] +
2461 m->a2d[3][0] * m->a2d[0][2] * m->a2d[2][3] -
2462 m->a2d[3][0] * m->a2d[0][3] * m->a2d[2][2];
2463
2464 inv.a2d[2][1] = -m->a2d[0][0] * m->a2d[2][1] * m->a2d[3][3] +
2465 m->a2d[0][0] * m->a2d[2][3] * m->a2d[3][1] +
2466 m->a2d[2][0] * m->a2d[0][1] * m->a2d[3][3] -
2467 m->a2d[2][0] * m->a2d[0][3] * m->a2d[3][1] -
2468 m->a2d[3][0] * m->a2d[0][1] * m->a2d[2][3] +
2469 m->a2d[3][0] * m->a2d[0][3] * m->a2d[2][1];
2470
2471 inv.a2d[3][1] = m->a2d[0][0] * m->a2d[2][1] * m->a2d[3][2] -
2472 m->a2d[0][0] * m->a2d[2][2] * m->a2d[3][1] -
2473 m->a2d[2][0] * m->a2d[0][1] * m->a2d[3][2] +
2474 m->a2d[2][0] * m->a2d[0][2] * m->a2d[3][1] +
2475 m->a2d[3][0] * m->a2d[0][1] * m->a2d[2][2] -
2476 m->a2d[3][0] * m->a2d[0][2] * m->a2d[2][1];
2477
2478 inv.a2d[0][2] = m->a2d[0][1] * m->a2d[1][2] * m->a2d[3][3] -
2479 m->a2d[0][1] * m->a2d[1][3] * m->a2d[3][2] -
2480 m->a2d[1][1] * m->a2d[0][2] * m->a2d[3][3] +
2481 m->a2d[1][1] * m->a2d[0][3] * m->a2d[3][2] +
2482 m->a2d[3][1] * m->a2d[0][2] * m->a2d[1][3] -
2483 m->a2d[3][1] * m->a2d[0][3] * m->a2d[1][2];
2484
2485 inv.a2d[1][2] = -m->a2d[0][0] * m->a2d[1][2] * m->a2d[3][3] +
2486 m->a2d[0][0] * m->a2d[1][3] * m->a2d[3][2] +
2487 m->a2d[1][0] * m->a2d[0][2] * m->a2d[3][3] -
2488 m->a2d[1][0] * m->a2d[0][3] * m->a2d[3][2] -
2489 m->a2d[3][0] * m->a2d[0][2] * m->a2d[1][3] +
2490 m->a2d[3][0] * m->a2d[0][3] * m->a2d[1][2];
2491
2492 inv.a2d[2][2] = m->a2d[0][0] * m->a2d[1][1] * m->a2d[3][3] -
2493 m->a2d[0][0] * m->a2d[1][3] * m->a2d[3][1] -
2494 m->a2d[1][0] * m->a2d[0][1] * m->a2d[3][3] +
2495 m->a2d[1][0] * m->a2d[0][3] * m->a2d[3][1] +
2496 m->a2d[3][0] * m->a2d[0][1] * m->a2d[1][3] -
2497 m->a2d[3][0] * m->a2d[0][3] * m->a2d[1][1];
2498
2499 inv.a2d[3][2] = -m->a2d[0][0] * m->a2d[1][1] * m->a2d[3][2] +
2500 m->a2d[0][0] * m->a2d[1][2] * m->a2d[3][1] +
2501 m->a2d[1][0] * m->a2d[0][1] * m->a2d[3][2] -
2502 m->a2d[1][0] * m->a2d[0][2] * m->a2d[3][1] -
2503 m->a2d[3][0] * m->a2d[0][1] * m->a2d[1][2] +
2504 m->a2d[3][0] * m->a2d[0][2] * m->a2d[1][1];
2505
2506 inv.a2d[0][3] = -m->a2d[0][1] * m->a2d[1][2] * m->a2d[2][3] +
2507 m->a2d[0][1] * m->a2d[1][3] * m->a2d[2][2] +
2508 m->a2d[1][1] * m->a2d[0][2] * m->a2d[2][3] -
2509 m->a2d[1][1] * m->a2d[0][3] * m->a2d[2][2] -
2510 m->a2d[2][1] * m->a2d[0][2] * m->a2d[1][3] +
2511 m->a2d[2][1] * m->a2d[0][3] * m->a2d[1][2];
2512
2513 inv.a2d[1][3] = m->a2d[0][0] * m->a2d[1][2] * m->a2d[2][3] -
2514 m->a2d[0][0] * m->a2d[1][3] * m->a2d[2][2] -
2515 m->a2d[1][0] * m->a2d[0][2] * m->a2d[2][3] +
2516 m->a2d[1][0] * m->a2d[0][3] * m->a2d[2][2] +
2517 m->a2d[2][0] * m->a2d[0][2] * m->a2d[1][3] -
2518 m->a2d[2][0] * m->a2d[0][3] * m->a2d[1][2];
2519
2520 inv.a2d[2][3] = -m->a2d[0][0] * m->a2d[1][1] * m->a2d[2][3] +
2521 m->a2d[0][0] * m->a2d[1][3] * m->a2d[2][1] +
2522 m->a2d[1][0] * m->a2d[0][1] * m->a2d[2][3] -
2523 m->a2d[1][0] * m->a2d[0][3] * m->a2d[2][1] -
2524 m->a2d[2][0] * m->a2d[0][1] * m->a2d[1][3] +
2525 m->a2d[2][0] * m->a2d[0][3] * m->a2d[1][1];
2526
2527 inv.a2d[3][3] = m->a2d[0][0] * m->a2d[1][1] * m->a2d[2][2] -
2528 m->a2d[0][0] * m->a2d[1][2] * m->a2d[2][1] -
2529 m->a2d[1][0] * m->a2d[0][1] * m->a2d[2][2] +
2530 m->a2d[1][0] * m->a2d[0][2] * m->a2d[2][1] +
2531 m->a2d[2][0] * m->a2d[0][1] * m->a2d[1][2] -
2532 m->a2d[2][0] * m->a2d[0][2] * m->a2d[1][1];
2533
2534 float det = m->a2d[0][0] * inv.a2d[0][0] + m->a2d[0][1] * inv.a2d[1][0] + m->a2d[0][2] * inv.a2d[2][0] + m->a2d[0][3] * inv.a2d[3][0];
2535
2536 if (det == 0) {
2537 *dest = vmd_zero_matrix4;
2538 return false;
2539 }
2540
2541 det = 1.0f / det;
2542
2543 for (int i = 0; i < 16; i++) {
2544 dest->a1d[i] = inv.a1d[i] * det;
2545 }
2546
2547 return true;
2548 }
2549
vm_matrix4_set_orthographic(matrix4 * out,vec3d * max,vec3d * min)2550 void vm_matrix4_set_orthographic(matrix4* out, vec3d *max, vec3d *min)
2551 {
2552 memset(out, 0, sizeof(matrix4));
2553
2554 out->a1d[0] = 2.0f / (max->xyz.x - min->xyz.x);
2555 out->a1d[5] = 2.0f / (max->xyz.y - min->xyz.y);
2556 out->a1d[10] = -2.0f / (max->xyz.z - min->xyz.z);
2557 out->a1d[12] = -(max->xyz.x + min->xyz.x) / (max->xyz.x - min->xyz.x);
2558 out->a1d[13] = -(max->xyz.y + min->xyz.y) / (max->xyz.y - min->xyz.y);
2559 out->a1d[14] = -(max->xyz.z + min->xyz.z) / (max->xyz.z - min->xyz.z);
2560 out->a1d[15] = 1.0f;
2561 }
2562
vm_matrix4_set_inverse_transform(matrix4 * out,matrix * m,vec3d * v)2563 void vm_matrix4_set_inverse_transform(matrix4 *out, matrix *m, vec3d *v)
2564 {
2565 // this is basically the same function as the opengl view matrix construction
2566 // except we don't invert the Z-axis
2567 vec3d scaled_pos;
2568 vec3d inv_pos;
2569 matrix inv_orient;
2570
2571 vm_vec_copy_scale(&scaled_pos, v, -1.0f);
2572
2573 vm_copy_transpose(&inv_orient, m);
2574 vm_vec_rotate(&inv_pos, &scaled_pos, m);
2575
2576 vm_matrix4_set_transform(out, &inv_orient, &inv_pos);
2577 }
2578
vm_matrix4_set_identity(matrix4 * out)2579 void vm_matrix4_set_identity(matrix4 *out)
2580 {
2581 out->a2d[0][0] = 1.0f;
2582 out->a2d[0][1] = 0.0f;
2583 out->a2d[0][2] = 0.0f;
2584 out->a2d[0][3] = 0.0f;
2585
2586 out->a2d[1][0] = 0.0f;
2587 out->a2d[1][1] = 1.0f;
2588 out->a2d[1][2] = 0.0f;
2589 out->a2d[1][3] = 0.0f;
2590
2591 out->a2d[2][0] = 0.0f;
2592 out->a2d[2][1] = 0.0f;
2593 out->a2d[2][2] = 1.0f;
2594 out->a2d[2][3] = 0.0f;
2595
2596 out->a2d[3][0] = 0.0f;
2597 out->a2d[3][1] = 0.0f;
2598 out->a2d[3][2] = 0.0f;
2599 out->a2d[3][3] = 1.0f;
2600 }
2601
vm_matrix4_set_transform(matrix4 * out,matrix * m,vec3d * v)2602 void vm_matrix4_set_transform(matrix4 *out, matrix *m, vec3d *v)
2603 {
2604 vm_matrix4_set_identity(out);
2605
2606 out->a2d[0][0] = m->a2d[0][0];
2607 out->a2d[0][1] = m->a2d[0][1];
2608 out->a2d[0][2] = m->a2d[0][2];
2609
2610 out->a2d[1][0] = m->a2d[1][0];
2611 out->a2d[1][1] = m->a2d[1][1];
2612 out->a2d[1][2] = m->a2d[1][2];
2613
2614 out->a2d[2][0] = m->a2d[2][0];
2615 out->a2d[2][1] = m->a2d[2][1];
2616 out->a2d[2][2] = m->a2d[2][2];
2617
2618 out->a2d[3][0] = v->a1d[0];
2619 out->a2d[3][1] = v->a1d[1];
2620 out->a2d[3][2] = v->a1d[2];
2621 }
2622
vm_matrix4_get_orientation(matrix * out,const matrix4 * m)2623 void vm_matrix4_get_orientation(matrix *out, const matrix4 *m)
2624 {
2625 out->a2d[0][0] = m->a2d[0][0];
2626 out->a2d[0][1] = m->a2d[0][1];
2627 out->a2d[0][2] = m->a2d[0][2];
2628
2629 out->a2d[1][0] = m->a2d[1][0];
2630 out->a2d[1][1] = m->a2d[1][1];
2631 out->a2d[1][2] = m->a2d[1][2];
2632
2633 out->a2d[2][0] = m->a2d[2][0];
2634 out->a2d[2][1] = m->a2d[2][1];
2635 out->a2d[2][2] = m->a2d[2][2];
2636 }
2637
vm_matrix4_get_offset(vec3d * out,matrix4 * m)2638 void vm_matrix4_get_offset(vec3d *out, matrix4 *m)
2639 {
2640 out->xyz.x = m->vec.pos.xyzw.x;
2641 out->xyz.y = m->vec.pos.xyzw.y;
2642 out->xyz.z = m->vec.pos.xyzw.z;
2643 }
2644
vm_matrix4_x_matrix4(matrix4 * dest,const matrix4 * src0,const matrix4 * src1)2645 void vm_matrix4_x_matrix4(matrix4 *dest, const matrix4 *src0, const matrix4 *src1)
2646 {
2647 dest->vec.rvec.xyzw.x = vm_vec4_dot4(src0->vec.rvec.xyzw.x, src0->vec.uvec.xyzw.x, src0->vec.fvec.xyzw.x, src0->vec.pos.xyzw.x, &src1->vec.rvec);
2648 dest->vec.uvec.xyzw.x = vm_vec4_dot4(src0->vec.rvec.xyzw.x, src0->vec.uvec.xyzw.x, src0->vec.fvec.xyzw.x, src0->vec.pos.xyzw.x, &src1->vec.uvec);
2649 dest->vec.fvec.xyzw.x = vm_vec4_dot4(src0->vec.rvec.xyzw.x, src0->vec.uvec.xyzw.x, src0->vec.fvec.xyzw.x, src0->vec.pos.xyzw.x, &src1->vec.fvec);
2650 dest->vec.pos.xyzw.x = vm_vec4_dot4(src0->vec.rvec.xyzw.x, src0->vec.uvec.xyzw.x, src0->vec.fvec.xyzw.x, src0->vec.pos.xyzw.x, &src1->vec.pos);
2651
2652 dest->vec.rvec.xyzw.y = vm_vec4_dot4(src0->vec.rvec.xyzw.y, src0->vec.uvec.xyzw.y, src0->vec.fvec.xyzw.y, src0->vec.pos.xyzw.y, &src1->vec.rvec);
2653 dest->vec.uvec.xyzw.y = vm_vec4_dot4(src0->vec.rvec.xyzw.y, src0->vec.uvec.xyzw.y, src0->vec.fvec.xyzw.y, src0->vec.pos.xyzw.y, &src1->vec.uvec);
2654 dest->vec.fvec.xyzw.y = vm_vec4_dot4(src0->vec.rvec.xyzw.y, src0->vec.uvec.xyzw.y, src0->vec.fvec.xyzw.y, src0->vec.pos.xyzw.y, &src1->vec.fvec);
2655 dest->vec.pos.xyzw.y = vm_vec4_dot4(src0->vec.rvec.xyzw.y, src0->vec.uvec.xyzw.y, src0->vec.fvec.xyzw.y, src0->vec.pos.xyzw.y, &src1->vec.pos);
2656
2657 dest->vec.rvec.xyzw.z = vm_vec4_dot4(src0->vec.rvec.xyzw.z, src0->vec.uvec.xyzw.z, src0->vec.fvec.xyzw.z, src0->vec.pos.xyzw.z, &src1->vec.rvec);
2658 dest->vec.uvec.xyzw.z = vm_vec4_dot4(src0->vec.rvec.xyzw.z, src0->vec.uvec.xyzw.z, src0->vec.fvec.xyzw.z, src0->vec.pos.xyzw.z, &src1->vec.uvec);
2659 dest->vec.fvec.xyzw.z = vm_vec4_dot4(src0->vec.rvec.xyzw.z, src0->vec.uvec.xyzw.z, src0->vec.fvec.xyzw.z, src0->vec.pos.xyzw.z, &src1->vec.fvec);
2660 dest->vec.pos.xyzw.z = vm_vec4_dot4(src0->vec.rvec.xyzw.z, src0->vec.uvec.xyzw.z, src0->vec.fvec.xyzw.z, src0->vec.pos.xyzw.z, &src1->vec.pos);
2661
2662 dest->vec.rvec.xyzw.w = vm_vec4_dot4(src0->vec.rvec.xyzw.w, src0->vec.uvec.xyzw.w, src0->vec.fvec.xyzw.w, src0->vec.pos.xyzw.w, &src1->vec.rvec);
2663 dest->vec.uvec.xyzw.w = vm_vec4_dot4(src0->vec.rvec.xyzw.w, src0->vec.uvec.xyzw.w, src0->vec.fvec.xyzw.w, src0->vec.pos.xyzw.w, &src1->vec.uvec);
2664 dest->vec.fvec.xyzw.w = vm_vec4_dot4(src0->vec.rvec.xyzw.w, src0->vec.uvec.xyzw.w, src0->vec.fvec.xyzw.w, src0->vec.pos.xyzw.w, &src1->vec.fvec);
2665 dest->vec.pos.xyzw.w = vm_vec4_dot4(src0->vec.rvec.xyzw.w, src0->vec.uvec.xyzw.w, src0->vec.fvec.xyzw.w, src0->vec.pos.xyzw.w, &src1->vec.pos);
2666 }
2667
vm_vec4_dot4(float x,float y,float z,float w,const vec4 * v)2668 float vm_vec4_dot4(float x, float y, float z, float w, const vec4 *v)
2669 {
2670 return (x * v->xyzw.x) + (y * v->xyzw.y) + (z * v->xyzw.z) + (w * v->xyzw.w);
2671 }
2672
vm_vec_transform(vec4 * dest,const vec4 * src,const matrix4 * m)2673 void vm_vec_transform(vec4 *dest, const vec4 *src, const matrix4 *m)
2674 {
2675 dest->xyzw.x = (m->vec.rvec.xyzw.x * src->xyzw.x) + (m->vec.uvec.xyzw.x * src->xyzw.y) + (m->vec.fvec.xyzw.x * src->xyzw.z) + (m->vec.pos.xyzw.x * src->xyzw.w);
2676 dest->xyzw.y = (m->vec.rvec.xyzw.y * src->xyzw.x) + (m->vec.uvec.xyzw.y * src->xyzw.y) + (m->vec.fvec.xyzw.y * src->xyzw.z) + (m->vec.pos.xyzw.y * src->xyzw.w);
2677 dest->xyzw.z = (m->vec.rvec.xyzw.z * src->xyzw.x) + (m->vec.uvec.xyzw.z * src->xyzw.y) + (m->vec.fvec.xyzw.z * src->xyzw.z) + (m->vec.pos.xyzw.z * src->xyzw.w);
2678 dest->xyzw.w = (m->vec.rvec.xyzw.w * src->xyzw.x) + (m->vec.uvec.xyzw.w * src->xyzw.y) + (m->vec.fvec.xyzw.w * src->xyzw.z) + (m->vec.pos.xyzw.w * src->xyzw.w);
2679 }
2680
vm_vec_transform(vec3d * dest,const vec3d * src,const matrix4 * m,bool pos)2681 void vm_vec_transform(vec3d *dest, const vec3d *src, const matrix4 *m, bool pos)
2682 {
2683 vec4 temp_src, temp_dest;
2684
2685 temp_src.xyzw.x = src->xyz.x;
2686 temp_src.xyzw.y = src->xyz.y;
2687 temp_src.xyzw.z = src->xyz.z;
2688
2689 // whether to treat vec3d src as a position or a vector.
2690 // 0.0f will prevent matrix4 m's offset from being added. 1.0f will add the offset.
2691 temp_src.xyzw.w = pos ? 1.0f : 0.0f;
2692
2693 vm_vec_transform(&temp_dest, &temp_src, m);
2694
2695 dest->xyz.x = temp_dest.xyzw.x;
2696 dest->xyz.y = temp_dest.xyzw.y;
2697 dest->xyz.z = temp_dest.xyzw.z;
2698 }
vm_vec4_to_vec3(const vec4 & vec)2699 vec3d vm_vec4_to_vec3(const vec4& vec) {
2700 vec3d out;
2701
2702 out.xyz.x = vec.xyzw.x;
2703 out.xyz.y = vec.xyzw.y;
2704 out.xyz.z = vec.xyzw.z;
2705
2706 return out;
2707 }
vm_vec3_to_ve4(const vec3d & vec,float w)2708 vec4 vm_vec3_to_ve4(const vec3d& vec, float w) {
2709 vec4 out;
2710
2711 out.xyzw.x = vec.xyz.x;
2712 out.xyzw.y = vec.xyz.y;
2713 out.xyzw.z = vec.xyz.z;
2714 out.xyzw.w = w;
2715
2716 return out;
2717 }
2718
2719 // This function is used when we want to "match orientation" to a target, here match_orient,
2720 // while still pointing our forward vector in a certain direction, here goal_fvec.
2721 // out_rvec is the best matching right vector to match_orient.rvec
vm_match_bank(vec3d * out_rvec,const vec3d * goal_fvec,const matrix * match_orient)2722 void vm_match_bank(vec3d* out_rvec, const vec3d* goal_fvec, const matrix* match_orient) {
2723 // We want to calculate out_rvec as a frame transformation, translating match_orient.rvec
2724 // from source_frame to dest_frame.
2725 //
2726 // We set up the frames such that:
2727 // * source fvec = match_orient.fvec
2728 // * dest fvec = goal_fvec
2729 // * source uvec = dest uvec
2730 // This uniquely determines both frames, and the rvecs go along for the ride.
2731 // Once we have these frames, we just rotate match_orient.rvec from one frame to the other.
2732
2733 // Calculate the source frame. The common uvec has to be perpendicular to match_orient.fvec
2734 // and goal_fvec so we cross to get it. The rvec is left as 0 to be set by vm_orthogonalize_matrix
2735 matrix source_frame = vmd_zero_matrix;
2736 source_frame.vec.fvec = match_orient->vec.fvec;
2737 vm_vec_cross(&source_frame.vec.uvec, &source_frame.vec.fvec, goal_fvec);
2738 vm_orthogonalize_matrix(&source_frame);
2739
2740 // Calculate the destination frame, using goal_fvec and the common uvec.
2741 // These are already orthogonal and normalized so we can just cross to get the rvec rather than
2742 // calling vm_orthogonalize_matrix
2743 matrix dest_frame;
2744 dest_frame.vec.fvec = *goal_fvec;
2745 dest_frame.vec.uvec = source_frame.vec.uvec;
2746 vm_vec_cross(&dest_frame.vec.rvec, &dest_frame.vec.uvec, &dest_frame.vec.fvec);
2747
2748 // Apply the transformation to match_orient.rvec, returning the result in out_rvec
2749 vec3d temp;
2750 vm_vec_rotate(&temp, &match_orient->vec.rvec, &source_frame);
2751 vm_vec_unrotate(out_rvec, &temp, &dest_frame);
2752 }
2753
2754 // Cyborg17 - Rotational interpolation between two angle structs in radians. Assumes that the rotation direction is the smaller arc difference.
2755 // src0 is the starting angle struct, src1 is the ending angle struct, interp_perc must be a float between 0.0f and 1.0f inclusive.
2756 // rot_vel is only used to determine the rotation direction. This functions assumes a <= 2PI rotation in any axis.
2757 // You will get inaccurate results otherwise.
vm_interpolate_angles_quick(angles * dest0,angles * src0,angles * src1,float interp_perc)2758 void vm_interpolate_angles_quick(angles *dest0, angles *src0, angles *src1, float interp_perc) {
2759
2760 Assertion((interp_perc >= 0.0f) && (interp_perc <= 1.0f), "Interpolation percentage, %f, sent to vm_interpolate_angles is invalid. The valid range is [0,1], go find a coder!", interp_perc);
2761
2762 angles arc_measures;
2763
2764 arc_measures.p = src1->p - src0->p;
2765 arc_measures.h = src1->h - src0->h;
2766 arc_measures.b = src1->b - src0->b;
2767
2768 // pitch
2769 // if start and end are basically the same, assume we can basically jump to the end.
2770 if ( (fabs(arc_measures.p) < 0.00001f) ) {
2771 arc_measures.p = 0.0f;
2772
2773 } // Test if we actually need to go in the other direction
2774 else if (arc_measures.p > (PI*1.5f)) {
2775 arc_measures.p = PI2 - arc_measures.p;
2776
2777 } // Test if we actually need to go in the other direction for negative values
2778 else if (arc_measures.p < -PI_2) {
2779 arc_measures.p = -PI2 - arc_measures.p;
2780 }
2781
2782 // heading
2783 // if start and end are basically the same, assume we can basically jump to the end.
2784 if ( (fabs(arc_measures.h) < 0.00001f) ) {
2785 arc_measures.h = 0.0f;
2786
2787 } // Test if we actually need to go in the other direction
2788 else if (arc_measures.h > (PI*1.5f)) {
2789 arc_measures.h = PI2 - arc_measures.h;
2790
2791 } // Test if we actually need to go in the other direction for negative values
2792 else if (arc_measures.h < -PI_2) {
2793 arc_measures.h = -PI2 - arc_measures.h;
2794 }
2795
2796 // bank
2797 // if start and end are basically the same, assume we can basically jump to the end.
2798 if ( (fabs(arc_measures.b) < 0.00001f) ) {
2799 arc_measures.b = 0.0f;
2800
2801 } // Test if we actually need to go in the other direction
2802 else if (arc_measures.b > (PI*1.5f)) {
2803 arc_measures.b = PI2 - arc_measures.b;
2804
2805 } // Test if we actually need to go in the other direction for negative values
2806 else if (arc_measures.b < -PI_2) {
2807 arc_measures.b = -PI2 - arc_measures.b;
2808 }
2809
2810 // Now just multiply the difference in angles by the given percentage, and then subtract it from the destination angles.
2811 // If arc_measures is 0.0f, then we are basically bashing to the ending orientation without worrying about the inbetween.
2812 dest0->p = src0->p + (arc_measures.p * interp_perc);
2813 dest0->h = src0->h + (arc_measures.h * interp_perc);
2814 dest0->b = src0->b + (arc_measures.b * interp_perc);
2815 }
2816
operator <<(std::ostream & os,const vec3d & vec)2817 std::ostream& operator<<(std::ostream& os, const vec3d& vec)
2818 {
2819 os << "vec3d<" << vec.xyz.x << ", " << vec.xyz.y << ", " << vec.xyz.z << ">";
2820 return os;
2821 }
2822
vm_stretch_matrix(const vec3d * stretch_dir,float stretch)2823 matrix vm_stretch_matrix(const vec3d* stretch_dir, float stretch) {
2824 matrix outer_prod;
2825 vm_vec_outer_product(&outer_prod, stretch_dir);
2826
2827 for (float& i : outer_prod.a1d)
2828 i *= stretch - 1.f;
2829
2830
2831 return vmd_identity_matrix + outer_prod;
2832 }
2833
2834 // generates a well distributed quasi-random position in a -1 to 1 cube
2835 // the caller must provide and increment the seed for each call for proper results
2836 // algorithm taken from http://extremelearning.com.au/unreasonable-effectiveness-of-quasirandom-sequences/
2837 const float phi3 = 1.220744084f;
vm_well_distributed_rand_vec(int seed,vec3d * offset)2838 vec3d vm_well_distributed_rand_vec(int seed, vec3d* offset) {
2839 vec3d out;
2840 if (offset != nullptr) {
2841 out.xyz.x = fmod(-fmod(offset->xyz.x, 1.f) + ((1.f / phi3) * seed), 1.f) * 2 - 1;
2842 out.xyz.y = fmod(-fmod(offset->xyz.y, 1.f) + ((1.f / (phi3 * phi3)) * seed), 1.f) * 2 - 1;
2843 out.xyz.z = fmod(-fmod(offset->xyz.z, 1.f) + ((1.f / (phi3 * phi3 * phi3)) * seed), 1.f) * 2 - 1;
2844 }
2845 else {
2846 out.xyz.x = fmod((1.f / phi3) * seed, 1.f) * 2 - 1;
2847 out.xyz.y = fmod((1.f / (phi3 * phi3)) * seed, 1.f) * 2 - 1;
2848 out.xyz.z = fmod((1.f / (phi3 * phi3 * phi3)) * seed, 1.f) * 2 - 1;
2849 }
2850 return out;
2851 }
2852