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