2 * Copyright (C) Volition, Inc. 1999. All rights reserved.
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
10 * $Logfile: /Freespace2/code/Math/VecMat.cpp $
15 * C module containg functions for manipulating vectors and matricies
18 * Revision 1.7 2005/10/01 21:45:56 taylor
19 * fix an old Volition math bug, causes various SDL_assert()'s and physics strangeness in some missions
21 * Revision 1.6 2004/09/20 01:31:44 theoddone33
24 * Revision 1.5 2002/09/04 01:12:11 relnev
25 * changes to screen backup/mouse drawing code. removed a few warnings.
27 * Revision 1.4 2002/06/17 06:33:09 relnev
28 * ryan's struct patch for gcc 2.95
30 * Revision 1.3 2002/06/09 04:41:22 relnev
31 * added copyright header
33 * Revision 1.2 2002/05/07 03:16:46 theoddone33
34 * The Great Newline Fix
36 * Revision 1.1.1.1 2002/05/03 03:28:09 root
40 * 10 9/08/99 3:36p Jefff
41 * Make sure argument of sqrt is positive in approach.
43 * 9 6/22/99 1:51p Danw
44 * Some sanity for vm_vec_dist_to_line(...)
46 * 8 6/18/99 5:16p Dave
47 * Added real beam weapon lighting. Fixed beam weapon sounds. Added MOTD
48 * dialog to PXO screen.
50 * 7 4/28/99 11:13p Dave
51 * Temporary checkin of artillery code.
53 * 6 1/24/99 11:37p Dave
54 * First full rev of beam weapons. Very customizable. Removed some bogus
55 * Int3()'s in low level net code.
57 * 5 1/12/99 12:53a Dave
58 * More work on beam weapons - made collision detection very efficient -
59 * collide against all object types properly - made 3 movement types
60 * smooth. Put in test code to check for possible non-darkening pixels on
63 * 4 1/06/99 2:24p Dave
64 * Stubs and release build fixes.
66 * 3 11/18/98 4:10p Johnson
67 * Add assert in vm_interpolate_matrix
69 * 2 10/07/98 10:53a Dave
72 * 1 10/07/98 10:49a Dave
74 * 72 9/11/98 10:10a Andsager
75 * Optimize and rename matrix_decomp to vm_matrix_to_rot_axis_and_angle,
76 * rename quatern_rot to vm_quaternion_rotate
78 * 71 5/01/98 2:25p Andsager
79 * Fix bug in matrix interpolate (approach) when in rotvel is above limit.
81 * 70 4/07/98 3:10p Andsager
82 * Make vm_test_parallel based on absolute difference. Optimize matrix
83 * decomp. Fix potential bug in get_camera_limits with time = 0.
84 * Optimize vm_forward_interpolate.
86 * 69 4/06/98 8:54a Andsager
87 * Fix bug where matrix interpolate gets accel of 0
89 * 68 4/03/98 5:34p Andsager
90 * Optimized approach and away (used in matrix interpolation)
92 * 67 4/01/98 9:21p John
93 * Made NDEBUG, optimized build with no warnings or errors.
95 * 66 3/23/98 1:12p Andsager
96 * Reformat matrix inerpolation code
98 * 65 3/23/98 12:53p Andsager
100 * 63 3/09/98 3:51p Mike
101 * More error checking.
103 * 62 2/26/98 3:28p John
104 * Changed all sqrt's to use fl_sqrt. Took out isqrt function
106 * 61 2/02/98 5:12p Mike
107 * Make vm_vec_rand_vec_quick() detect potential null vector condition and
110 * 60 1/20/98 9:47a Mike
111 * Suppress optimized compiler warnings.
112 * Some secondary weapon work.
114 * 59 12/17/97 5:44p Andsager
115 * Change vm_matrix_interpolate so that it does not overshoot if optional
116 * last parameter is 1
118 * 58 9/30/97 5:04p Andsager
119 * add vm_estimate_next_orientation
121 * 57 9/28/97 2:17p Andsager
122 * added vm_project_point_onto_plane
124 * 56 9/09/97 10:15p Andsager
125 * added vm_rotate_vec_to_body() and vm_rotate_vec_to_world()
127 * 55 8/20/97 5:33p Andsager
128 * added vm_vec_projection_parallel and vm_vec_projection_onto_surface
130 * 54 8/20/97 9:51a Lawrance
131 * swap x and y parameters in atan2_safe() to be consistent with library
134 * 53 8/20/97 9:40a Lawrance
135 * modified special case values in atan2_safe()
137 * 52 8/19/97 11:41p Lawrance
138 * use atan2_safe() instead of atan2()
140 * 51 8/18/97 4:46p Hoffoss
141 * Added global default axis vector constants.
143 * 50 8/03/97 3:54p Lawrance
144 * added vm_find_bounding_sphere()
146 * 49 7/28/97 3:40p Andsager
147 * remove duplicate vm_forwarad_interpolate
149 * 48 7/28/97 2:21p John
150 * changed vecmat functions to not return src. Started putting in code
151 * for inline vector math. Fixed some bugs with optimizer.
153 * 47 7/28/97 3:24p Andsager
155 * 46 7/28/97 2:41p Mike
156 * Replace vm_forward_interpolate().
158 * 45 7/28/97 1:18p Andsager
159 * implement vm_fvec_matrix_interpolate(), which interpolates matrices on
162 * 44 7/28/97 10:28a Mike
163 * Use forward_interpolate() to prevent weird banking behavior.
165 * Suppress a couple annoying mprints and clarify another.
167 * 43 7/24/97 5:24p Andsager
168 * implement forward vector interpolation
170 * 42 7/10/97 8:52a Andsager
171 * optimization and clarification of matrix_decomp()
173 * 41 7/09/97 2:54p Mike
174 * More matrix_decomp optimization.
176 * 40 7/09/97 2:52p Mike
177 * Optimize and error-prevent matrix_decomp().
179 * 39 7/09/97 12:05a Mike
180 * Error prevention in matrix_interpolate().
182 * 38 7/07/97 11:58p Lawrance
183 * add get_camera_limits()
185 * 37 7/03/97 11:22a Mike
186 * Fix bug in matrix_interpolate. Was doing result = goal instead of
189 * 36 7/03/97 9:27a Mike
190 * Hook in Dave's latest version of matrix_interpolate which doesn't jerk.
192 * 35 7/02/97 4:25p Mike
193 * Add matrix_interpolate(), but don't call it.
195 * 34 7/01/97 3:27p Mike
196 * Improve skill level support.
198 * 33 6/25/97 12:27p Hoffoss
199 * Added some functions I needed for Fred.
201 * 32 5/21/97 8:49a Lawrance
202 * added vm_vec_same()
204 * 31 4/15/97 4:00p Mike
205 * Intermediate checkin caused by getting other files. Working on camera
208 * 30 4/10/97 3:20p Mike
209 * Change hull damage to be like shields.
211 * 29 3/17/97 1:55p Hoffoss
212 * Added function for error checking matrices.
214 * 28 3/11/97 10:46p Mike
215 * Fix make_rand_vec_quick. Was generating values in -0.5..1.5 instead of
218 * 27 3/06/97 5:36p Mike
219 * Change vec_normalize_safe() back to vec_normalize().
220 * Spruce up docking a bit.
222 * 26 3/06/97 10:56a Mike
223 * Write error checking version of vm_vec_normalize().
224 * Fix resultant problems.
226 * 25 3/04/97 3:30p John
227 * added function to interpolate an angle.
229 * 24 2/26/97 10:32a John
230 * changed debris collision to use vm_vec_dist_squared. Changed
231 * vm_vec_dist_squared to not int3 on bad values.
233 * 23 2/25/97 5:54p Hoffoss
234 * Improved vector and matrix compare functions.
236 * 22 2/25/97 5:28p Hoffoss
237 * added some commented out test code.
239 * 21 2/25/97 5:12p John
240 * Added functions to see if two matrices or vectors are close.
250 #include "floating.h"
252 #define SMALL_NUM 1e-7
253 #define SMALLER_NUM 1e-20
254 #define CONVERT_RADIANS 0.017453 // conversion factor from degrees to radians
255 int index_largest (float a, float b, float c); // returns index of largest, NO_LARGEST if all less than SMALL_NUM
258 vector vmd_zero_vector = ZERO_VECTOR;
259 vector vmd_x_vector = { { { 1.0f, 0.0f, 0.0f } } };
260 vector vmd_y_vector = { { { 0.0f, 1.0f, 0.0f } } };
261 vector vmd_z_vector = { { { 0.0f, 0.0f, 1.0f } } };
262 matrix vmd_identity_matrix = IDENTITY_MATRIX;
264 #define UNINITIALIZED_VALUE -12345678.9f
266 // -----------------------------------------------------------
269 // Wrapper around atan2() that used atan() to calculate angle. Safe
270 // for optimized builds. Handles special cases when x == 0.
272 float atan2_safe(float y, float x)
276 // special case, x == 0
288 ang = (float)atan(y/x);
296 // ---------------------------------------------------------------------
297 // vm_vec_component()
299 // finds projection of a vector along a unit (normalized) vector
301 float vm_vec_projection_parallel(vector *component, vector *src, vector *unit_vec)
304 SDL_assert( vm_vec_mag(unit_vec) > 0.999f && vm_vec_mag(unit_vec) < 1.001f );
306 mag = vm_vec_dotprod(src, unit_vec);
307 vm_vec_copy_scale(component, unit_vec, mag);
311 // ---------------------------------------------------------------------
312 // vm_vec_projection_onto_plane()
314 // finds projection of a vector onto a plane specified by a unit normal vector
316 void vm_vec_projection_onto_plane(vector *projection, vector *src, vector *unit_normal)
319 SDL_assert( vm_vec_mag(unit_normal) > 0.999f && vm_vec_mag(unit_normal) < 1.001f );
321 mag = vm_vec_dotprod(src, unit_normal);
323 vm_vec_scale_add2(projection, unit_normal, -mag);
326 // ---------------------------------------------------------------------
327 // vm_vec_project_point_onto_plane()
329 // finds the point on a plane closest to a given point
330 // moves the point in the direction of the plane normal until it is on the plane
332 void vm_project_point_onto_plane(vector *new_point, vector *point, vector *plane_normal, vector *plane_point)
334 float D; // plane constant in Ax+By+Cz+D = 0 or dot(X,n) - dot(Xp,n) = 0, so D = -dot(Xp,n)
336 SDL_assert( vm_vec_mag(plane_normal) > 0.999f && vm_vec_mag(plane_normal) < 1.001f );
338 D = -vm_vec_dotprod(plane_point, plane_normal);
339 dist = vm_vec_dotprod(point, plane_normal) + D;
342 vm_vec_scale_add2(new_point, plane_normal, -dist);
345 // Take abs(x), then sqrt. Could insert warning message if desired.
354 void vm_set_identity(matrix *m)
356 m->v.rvec.xyz.x = 1.0f; m->v.rvec.xyz.y = 0.0f; m->v.rvec.xyz.z = 0.0f;
357 m->v.uvec.xyz.x = 0.0f; m->v.uvec.xyz.y = 1.0f; m->v.uvec.xyz.z = 0.0f;
358 m->v.fvec.xyz.x = 0.0f; m->v.fvec.xyz.y = 0.0f; m->v.fvec.xyz.z = 1.0f;
361 //adds two vectors, fills in dest, returns ptr to dest
362 //ok for dest to equal either source, but should use vm_vec_add2() if so
363 #ifndef _INLINE_VECMAT
364 void vm_vec_add(vector *dest,vector *src0,vector *src1)
366 dest->xyz.x = src0->xyz.x + src1->xyz.x;
367 dest->xyz.y = src0->xyz.y + src1->xyz.y;
368 dest->xyz.z = src0->xyz.z + src1->xyz.z;
372 //subs two vectors, fills in dest, returns ptr to dest
373 //ok for dest to equal either source, but should use vm_vec_sub2() if so
374 #ifndef _INLINE_VECMAT
375 void vm_vec_sub(vector *dest,vector *src0,vector *src1)
377 dest->xyz.x = src0->xyz.x - src1->xyz.x;
378 dest->xyz.y = src0->xyz.y - src1->xyz.y;
379 dest->xyz.z = src0->xyz.z - src1->xyz.z;
384 //adds one vector to another. returns ptr to dest
385 //dest can equal source
386 #ifndef _INLINE_VECMAT
387 void vm_vec_add2(vector *dest,vector *src)
389 dest->xyz.x += src->xyz.x;
390 dest->xyz.y += src->xyz.y;
391 dest->xyz.z += src->xyz.z;
395 //subs one vector from another, returns ptr to dest
396 //dest can equal source
397 #ifndef _INLINE_VECMAT
398 void vm_vec_sub2(vector *dest,vector *src)
400 dest->xyz.x -= src->xyz.x;
401 dest->xyz.y -= src->xyz.y;
402 dest->xyz.z -= src->xyz.z;
406 //averages two vectors. returns ptr to dest
407 //dest can equal either source
408 vector *vm_vec_avg(vector *dest,vector *src0,vector *src1)
410 dest->xyz.x = (src0->xyz.x + src1->xyz.x) * 0.5f;
411 dest->xyz.y = (src0->xyz.y + src1->xyz.y) * 0.5f;
412 dest->xyz.z = (src0->xyz.z + src1->xyz.z) * 0.5f;
418 //averages four vectors. returns ptr to dest
419 //dest can equal any source
420 vector *vm_vec_avg4(vector *dest,vector *src0,vector *src1,vector *src2,vector *src3)
422 dest->xyz.x = (src0->xyz.x + src1->xyz.x + src2->xyz.x + src3->xyz.x) * 0.25f;
423 dest->xyz.y = (src0->xyz.y + src1->xyz.y + src2->xyz.y + src3->xyz.y) * 0.25f;
424 dest->xyz.z = (src0->xyz.z + src1->xyz.z + src2->xyz.z + src3->xyz.z) * 0.25f;
429 //scales a vector in place. returns ptr to vector
430 #ifndef _INLINE_VECMAT
431 void vm_vec_scale(vector *dest,float s)
433 dest->xyz.x = dest->xyz.x * s;
434 dest->xyz.y = dest->xyz.y * s;
435 dest->xyz.z = dest->xyz.z * s;
440 //scales and copies a vector. returns ptr to dest
441 #ifndef _INLINE_VECMAT
442 void vm_vec_copy_scale(vector *dest,vector *src,float s)
444 dest->xyz.x = src->xyz.x*s;
445 dest->xyz.y = src->xyz.y*s;
446 dest->xyz.z = src->xyz.z*s;
450 //scales a vector, adds it to another, and stores in a 3rd vector
451 //dest = src1 + k * src2
452 #ifndef _INLINE_VECMAT
453 void vm_vec_scale_add(vector *dest,vector *src1,vector *src2,float k)
455 dest->xyz.x = src1->xyz.x + src2->xyz.x*k;
456 dest->xyz.y = src1->xyz.y + src2->xyz.y*k;
457 dest->xyz.z = src1->xyz.z + src2->xyz.z*k;
461 //scales a vector and adds it to another
463 #ifndef _INLINE_VECMAT
464 void vm_vec_scale_add2(vector *dest,vector *src,float k)
466 dest->xyz.x += src->xyz.x*k;
467 dest->xyz.y += src->xyz.y*k;
468 dest->xyz.z += src->xyz.z*k;
472 //scales a vector and adds it to another
474 #ifndef _INLINE_VECMAT
475 void vm_vec_scale_sub2(vector *dest,vector *src,float k)
477 dest->xyz.x -= src->xyz.x*k;
478 dest->xyz.y -= src->xyz.y*k;
479 dest->xyz.z -= src->xyz.z*k;
483 //scales a vector in place, taking n/d for scale. returns ptr to vector
485 #ifndef _INLINE_VECMAT
486 void vm_vec_scale2(vector *dest,float n,float d)
490 dest->xyz.x = dest->xyz.x* n * d;
491 dest->xyz.y = dest->xyz.y* n * d;
492 dest->xyz.z = dest->xyz.z* n * d;
496 //returns dot product of 2 vectors
497 #ifndef _INLINE_VECMAT
498 float vm_vec_dotprod(vector *v0,vector *v1)
500 return (v1->xyz.x*v0->xyz.x)+(v1->xyz.y*v0->xyz.y)+(v1->xyz.z*v0->xyz.z);
505 //returns dot product of <x,y,z> and vector
506 #ifndef _INLINE_VECMAT
507 float vm_vec_dot3(float x,float y,float z,vector *v)
509 return (x*v->xyz.x)+(y*v->xyz.y)+(z*v->xyz.z);
513 //returns magnitude of a vector
514 float vm_vec_mag(vector *v)
516 float x,y,z,mag1, mag2;
517 x = v->xyz.x*v->xyz.x;
518 y = v->xyz.y*v->xyz.y;
519 z = v->xyz.z*v->xyz.z;
523 mag2 = fl_sqrt(mag1);
529 //returns squared magnitude of a vector, useful if you want to compare distances
530 float vm_vec_mag_squared(vector *v)
533 x = v->xyz.x*v->xyz.x;
534 y = v->xyz.y*v->xyz.y;
535 z = v->xyz.z*v->xyz.z;
540 float vm_vec_dist_squared(vector *v0, vector *v1)
544 dx = v0->xyz.x - v1->xyz.x;
545 dy = v0->xyz.y - v1->xyz.y;
546 dz = v0->xyz.z - v1->xyz.z;
547 return dx*dx + dy*dy + dz*dz;
550 //computes the distance between two points. (does sub and mag)
551 float vm_vec_dist(vector *v0,vector *v1)
556 vm_vec_sub(&t,v0,v1);
565 //computes an approximation of the magnitude of the vector
566 //uses dist = largest + next_largest*3/8 + smallest*3/16
567 float vm_vec_mag_quick(vector *v)
571 if ( v->xyz.x < 0.0 )
576 if ( v->xyz.y < 0.0 )
581 if ( v->xyz.z < 0.0 )
598 bc = (b * 0.25f) + (c * 0.125f);
600 t = a + bc + (bc * 0.5f);
605 //computes an approximation of the distance between two points.
606 //uses dist = largest + next_largest*3/8 + smallest*3/16
607 float vm_vec_dist_quick(vector *v0,vector *v1)
611 vm_vec_sub(&t,v0,v1);
613 return vm_vec_mag_quick(&t);
616 //normalize a vector. returns mag of source vec
617 float vm_vec_copy_normalize(vector *dest,vector *src)
623 // Mainly here to trap attempts to normalize a null vector.
625 Warning(LOCATION, "Null vector in vector normalize.\n"
626 "Trace out of vecmat.cpp and find offending code.\n");
636 dest->xyz.x = src->xyz.x * im;
637 dest->xyz.y = src->xyz.y * im;
638 dest->xyz.z = src->xyz.z * im;
643 //normalize a vector. returns mag of source vec
644 float vm_vec_normalize(vector *v)
647 t = vm_vec_copy_normalize(v,v);
651 // Normalize a vector.
652 // If vector is 0,0,0, return 1,0,0.
653 // Don't generate a Warning().
654 // returns mag of source vec
655 float vm_vec_normalize_safe(vector *v)
661 // Mainly here to trap attempts to normalize a null vector.
680 //returns approximation of 1/magnitude of a vector
681 float vm_vec_imag(vector *v)
683 // return 1.0f / sqrt( (v->xyz.x*v->xyz.x)+(v->xyz.y*v->xyz.y)+(v->xyz.z*v->xyz.z) );
684 return fl_isqrt( (v->xyz.x*v->xyz.x)+(v->xyz.y*v->xyz.y)+(v->xyz.z*v->xyz.z) );
687 //normalize a vector. returns 1/mag of source vec. uses approx 1/mag
688 float vm_vec_copy_normalize_quick(vector *dest,vector *src)
690 // return vm_vec_copy_normalize(dest, src);
693 im = vm_vec_imag(src);
695 SDL_assert(im > 0.0f);
697 dest->xyz.x = src->xyz.x*im;
698 dest->xyz.y = src->xyz.y*im;
699 dest->xyz.z = src->xyz.z*im;
704 //normalize a vector. returns mag of source vec. uses approx mag
705 float vm_vec_normalize_quick(vector *src)
707 // return vm_vec_normalize(src);
711 im = vm_vec_imag(src);
713 SDL_assert(im > 0.0f);
715 src->xyz.x = src->xyz.x*im;
716 src->xyz.y = src->xyz.y*im;
717 src->xyz.z = src->xyz.z*im;
723 //normalize a vector. returns mag of source vec. uses approx mag
724 float vm_vec_copy_normalize_quick_mag(vector *dest,vector *src)
726 // return vm_vec_copy_normalize(dest, src);
730 m = vm_vec_mag_quick(src);
732 SDL_assert(m > 0.0f);
736 dest->xyz.x = src->xyz.x * im;
737 dest->xyz.y = src->xyz.y * im;
738 dest->xyz.z = src->xyz.z * im;
744 //normalize a vector. returns mag of source vec. uses approx mag
745 float vm_vec_normalize_quick_mag(vector *v)
747 // return vm_vec_normalize(v);
750 m = vm_vec_mag_quick(v);
752 SDL_assert(m > 0.0f);
754 v->xyz.x = v->xyz.x*m;
755 v->xyz.y = v->xyz.y*m;
756 v->xyz.z = v->xyz.z*m;
764 //return the normalized direction vector between two points
765 //dest = normalized(end - start). Returns mag of direction vector
766 //NOTE: the order of the parameters matches the vector subtraction
767 float vm_vec_normalized_dir(vector *dest,vector *end,vector *start)
771 vm_vec_sub(dest,end,start);
772 t = vm_vec_normalize(dest);
776 //return the normalized direction vector between two points
777 //dest = normalized(end - start). Returns mag of direction vector
778 //NOTE: the order of the parameters matches the vector subtraction
779 float vm_vec_normalized_dir_quick(vector *dest,vector *end,vector *start)
781 vm_vec_sub(dest,end,start);
783 return vm_vec_normalize_quick(dest);
786 //return the normalized direction vector between two points
787 //dest = normalized(end - start). Returns mag of direction vector
788 //NOTE: the order of the parameters matches the vector subtraction
789 float vm_vec_normalized_dir_quick_mag(vector *dest,vector *end,vector *start)
792 vm_vec_sub(dest,end,start);
794 t = vm_vec_normalize_quick_mag(dest);
798 //computes surface normal from three points. result is normalized
799 //returns ptr to dest
800 //dest CANNOT equal either source
801 vector *vm_vec_normal(vector *dest,vector *p0,vector *p1,vector *p2)
803 vm_vec_perp(dest,p0,p1,p2);
805 vm_vec_normalize(dest);
811 //computes cross product of two vectors.
812 //Note: this magnitude of the resultant vector is the
813 //product of the magnitudes of the two source vectors. This means it is
814 //quite easy for this routine to overflow and underflow. Be careful that
815 //your inputs are ok.
816 vector *vm_vec_crossprod(vector *dest,vector *src0,vector *src1)
818 dest->xyz.x = (src0->xyz.y * src1->xyz.z) - (src0->xyz.z * src1->xyz.y);
819 dest->xyz.y = (src0->xyz.z * src1->xyz.x) - (src0->xyz.x * src1->xyz.z);
820 dest->xyz.z = (src0->xyz.x * src1->xyz.y) - (src0->xyz.y * src1->xyz.x);
825 // test if 2 vectors are parallel or not.
826 int vm_test_parallel(vector *src0, vector *src1)
828 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) ) {
835 //computes non-normalized surface normal from three points.
836 //returns ptr to dest
837 //dest CANNOT equal either source
838 vector *vm_vec_perp(vector *dest,vector *p0,vector *p1,vector *p2)
842 vm_vec_sub(&t0,p1,p0);
843 vm_vec_sub(&t1,p2,p1);
845 return vm_vec_crossprod(dest,&t0,&t1);
849 //computes the delta angle between two vectors.
850 //vectors need not be normalized. if they are, call vm_vec_delta_ang_norm()
851 //the forward vector (third parameter) can be NULL, in which case the absolute
852 //value of the angle in returned. Otherwise the angle around that vector is
854 float vm_vec_delta_ang(vector *v0,vector *v1,vector *fvec)
859 vm_vec_copy_normalize(&t0,v0);
860 vm_vec_copy_normalize(&t1,v1);
861 vm_vec_copy_normalize(&t2,fvec);
863 t = vm_vec_delta_ang_norm(&t0,&t1,&t2);
868 //computes the delta angle between two normalized vectors.
869 float vm_vec_delta_ang_norm(vector *v0,vector *v1,vector *fvec)
874 a = (float)acos(vm_vec_dot(v0,v1));
877 vm_vec_cross(&t,v0,v1);
878 if ( vm_vec_dotprod(&t,fvec) < 0.0 ) {
886 matrix *sincos_2_matrix(matrix *m,float sinp,float cosp,float sinb,float cosb,float sinh,float cosh)
888 float sbsh,cbch,cbsh,sbch;
896 m->v.rvec.xyz.x = cbch + sinp*sbsh; //m1
897 m->v.uvec.xyz.z = sbsh + sinp*cbch; //m8
899 m->v.uvec.xyz.x = sinp*cbsh - sbch; //m2
900 m->v.rvec.xyz.z = sinp*sbch - cbsh; //m7
902 m->v.fvec.xyz.x = sinh*cosp; //m3
903 m->v.rvec.xyz.y = sinb*cosp; //m4
904 m->v.uvec.xyz.y = cosb*cosp; //m5
905 m->v.fvec.xyz.z = cosh*cosp; //m9
907 m->v.fvec.xyz.y = -sinp; //m6
914 //computes a matrix from a set of three angles. returns ptr to matrix
915 matrix *vm_angles_2_matrix(matrix *m,angles *a)
918 float sinp,cosp,sinb,cosb,sinh,cosh;
920 sinp = (float)sin(a->p); cosp = (float)cos(a->p);
921 sinb = (float)sin(a->b); cosb = (float)cos(a->b);
922 sinh = (float)sin(a->h); cosh = (float)cos(a->h);
924 t = sincos_2_matrix(m,sinp,cosp,sinb,cosb,sinh,cosh);
929 //computes a matrix from one angle.
930 // angle_index = 0,1,2 for p,b,h
931 matrix *vm_angle_2_matrix(matrix *m, float a, int angle_index)
934 float sinp,cosp,sinb,cosb,sinh,cosh;
936 sinp = (float)sin(0.0f); cosp = (float)cos(0.0f);
937 sinb = (float)sin(0.0f); cosb = (float)cos(0.0f);
938 sinh = (float)sin(0.0f); cosh = (float)cos(0.0f);
940 switch (angle_index) {
942 sinp = (float)sin(a); cosp = (float)cos(a);
945 sinb = (float)sin(a); cosb = (float)cos(a);
948 sinh = (float)sin(a); cosh = (float)cos(a);
952 t = sincos_2_matrix(m,sinp,cosp,sinb,cosb,sinh,cosh);
958 //computes a matrix from a forward vector and an angle
959 matrix *vm_vec_ang_2_matrix(matrix *m,vector *v,float a)
962 float sinb,cosb,sinp,cosp,sinh,cosh;
964 sinb = (float)sin(a); cosb = (float)cos(a);
967 cosp = fl_sqrt(1.0 - sinp*sinp);
969 sinh = v->xyz.x / cosp;
970 cosh = v->xyz.z / cosp;
972 t = sincos_2_matrix(m,sinp,cosp,sinb,cosb,sinh,cosh);
978 //computes a matrix from one or more vectors. The forward vector is required,
979 //with the other two being optional. If both up & right vectors are passed,
980 //the up vector is used. If only the forward vector is passed, a bank of
982 //returns ptr to matrix
983 matrix *vm_vector_2_matrix(matrix *m,vector *fvec,vector *uvec,vector *rvec)
985 vector *xvec=&m->v.rvec,*yvec=&m->v.uvec,*zvec=&m->v.fvec;
988 SDL_assert(fvec != NULL);
990 // This had been commented out, but that's bogus. Code below relies on a valid zvec.
991 if (vm_vec_copy_normalize(zvec,fvec) == 0.0) {
998 if (rvec == NULL) { //just forward vec
1003 if ((zvec->xyz.x==0.0) && (zvec->xyz.z==0.0)) { //forward vec is straight up or down
1005 m->v.rvec.xyz.x = (float)1.0;
1006 m->v.uvec.xyz.z = (zvec->xyz.y<0.0)?(float)1.0:(float)-1.0;
1008 m->v.rvec.xyz.y = m->v.rvec.xyz.z = m->v.uvec.xyz.x = m->v.uvec.xyz.y = (float)0.0;
1010 else { //not straight up or down
1012 xvec->xyz.x = zvec->xyz.z;
1013 xvec->xyz.y = (float)0.0;
1014 xvec->xyz.z = -zvec->xyz.x;
1016 vm_vec_normalize(xvec);
1018 vm_vec_crossprod(yvec,zvec,xvec);
1023 else { //use right vec
1025 if (vm_vec_copy_normalize(xvec,rvec) == 0.0)
1028 vm_vec_crossprod(yvec,zvec,xvec);
1030 //normalize new perpendicular vector
1031 if (vm_vec_normalize(yvec) == 0.0)
1034 //now recompute right vector, in case it wasn't entirely perpendiclar
1035 vm_vec_crossprod(xvec,yvec,zvec);
1041 if (vm_vec_copy_normalize(yvec,uvec) == 0.0f)
1044 vm_vec_crossprod(xvec,yvec,zvec);
1046 //normalize new perpendicular vector
1047 if (vm_vec_normalize(xvec) == 0.0)
1050 //now recompute up vector, in case it wasn't entirely perpendiclar
1051 vm_vec_crossprod(yvec,zvec,xvec);
1057 //quicker version of vm_vector_2_matrix() that takes normalized vectors
1058 matrix *vm_vector_2_matrix_norm(matrix *m,vector *fvec,vector *uvec,vector *rvec)
1060 vector *xvec=&m->v.rvec,*yvec=&m->v.uvec,*zvec=&m->v.fvec;
1063 SDL_assert(fvec != NULL);
1069 if (rvec == NULL) { //just forward vec
1074 if ((zvec->xyz.x==0.0) && (zvec->xyz.z==0.0)) { //forward vec is straight up or down
1076 m->v.rvec.xyz.x = (float)1.0;
1077 m->v.uvec.xyz.z = (zvec->xyz.y<0.0)?(float)1.0:(float)-1.0;
1079 m->v.rvec.xyz.y = m->v.rvec.xyz.z = m->v.uvec.xyz.x = m->v.uvec.xyz.y = (float)0.0;
1081 else { //not straight up or down
1083 xvec->xyz.x = zvec->xyz.z;
1084 xvec->xyz.y = (float)0.0;
1085 xvec->xyz.z = -zvec->xyz.x;
1087 vm_vec_normalize(xvec);
1089 vm_vec_crossprod(yvec,zvec,xvec);
1094 else { //use right vec
1096 vm_vec_crossprod(yvec,zvec,xvec);
1098 //normalize new perpendicular vector
1099 if (vm_vec_normalize(yvec) == 0.0)
1102 //now recompute right vector, in case it wasn't entirely perpendiclar
1103 vm_vec_crossprod(xvec,yvec,zvec);
1109 vm_vec_crossprod(xvec,yvec,zvec);
1111 //normalize new perpendicular vector
1112 if (vm_vec_normalize(xvec) == 0.0)
1115 //now recompute up vector, in case it wasn't entirely perpendiclar
1116 vm_vec_crossprod(yvec,zvec,xvec);
1125 //rotates a vector through a matrix. returns ptr to dest vector
1126 //dest CANNOT equal source
1127 vector *vm_vec_rotate(vector *dest,vector *src,matrix *m)
1129 dest->xyz.x = (src->xyz.x*m->v.rvec.xyz.x)+(src->xyz.y*m->v.rvec.xyz.y)+(src->xyz.z*m->v.rvec.xyz.z);
1130 dest->xyz.y = (src->xyz.x*m->v.uvec.xyz.x)+(src->xyz.y*m->v.uvec.xyz.y)+(src->xyz.z*m->v.uvec.xyz.z);
1131 dest->xyz.z = (src->xyz.x*m->v.fvec.xyz.x)+(src->xyz.y*m->v.fvec.xyz.y)+(src->xyz.z*m->v.fvec.xyz.z);
1136 //rotates a vector through the transpose of the given matrix.
1137 //returns ptr to dest vector
1138 //dest CANNOT equal source
1139 // This is a faster replacement for this common code sequence:
1140 // vm_copy_transpose_matrix(&tempm,src_matrix);
1141 // vm_vec_rotate(dst_vec,src_vect,&tempm);
1143 // vm_vec_unrotate(dst_vec,src_vect, src_matrix)
1145 // THIS DOES NOT ACTUALLY TRANSPOSE THE SOURCE MATRIX!!! So if
1146 // you need it transposed later on, you should use the
1147 // vm_vec_transpose() / vm_vec_rotate() technique.
1149 vector *vm_vec_unrotate(vector *dest,vector *src,matrix *m)
1151 dest->xyz.x = (src->xyz.x*m->v.rvec.xyz.x)+(src->xyz.y*m->v.uvec.xyz.x)+(src->xyz.z*m->v.fvec.xyz.x);
1152 dest->xyz.y = (src->xyz.x*m->v.rvec.xyz.y)+(src->xyz.y*m->v.uvec.xyz.y)+(src->xyz.z*m->v.fvec.xyz.y);
1153 dest->xyz.z = (src->xyz.x*m->v.rvec.xyz.z)+(src->xyz.y*m->v.uvec.xyz.z)+(src->xyz.z*m->v.fvec.xyz.z);
1158 //transpose a matrix in place. returns ptr to matrix
1159 matrix *vm_transpose_matrix(matrix *m)
1163 t = m->v.uvec.xyz.x; m->v.uvec.xyz.x = m->v.rvec.xyz.y; m->v.rvec.xyz.y = t;
1164 t = m->v.fvec.xyz.x; m->v.fvec.xyz.x = m->v.rvec.xyz.z; m->v.rvec.xyz.z = t;
1165 t = m->v.fvec.xyz.y; m->v.fvec.xyz.y = m->v.uvec.xyz.z; m->v.uvec.xyz.z = t;
1170 //copy and transpose a matrix. returns ptr to matrix
1171 //dest CANNOT equal source. use vm_transpose_matrix() if this is the case
1172 matrix *vm_copy_transpose_matrix(matrix *dest,matrix *src)
1175 SDL_assert(dest != src);
1177 dest->v.rvec.xyz.x = src->v.rvec.xyz.x;
1178 dest->v.rvec.xyz.y = src->v.uvec.xyz.x;
1179 dest->v.rvec.xyz.z = src->v.fvec.xyz.x;
1181 dest->v.uvec.xyz.x = src->v.rvec.xyz.y;
1182 dest->v.uvec.xyz.y = src->v.uvec.xyz.y;
1183 dest->v.uvec.xyz.z = src->v.fvec.xyz.y;
1185 dest->v.fvec.xyz.x = src->v.rvec.xyz.z;
1186 dest->v.fvec.xyz.y = src->v.uvec.xyz.z;
1187 dest->v.fvec.xyz.z = src->v.fvec.xyz.z;
1193 //mulitply 2 matrices, fill in dest. returns ptr to dest
1194 //dest CANNOT equal either source
1195 matrix *vm_matrix_x_matrix(matrix *dest,matrix *src0,matrix *src1)
1198 SDL_assert(dest!=src0 && dest!=src1);
1200 dest->v.rvec.xyz.x = vm_vec_dot3(src0->v.rvec.xyz.x,src0->v.uvec.xyz.x,src0->v.fvec.xyz.x, &src1->v.rvec);
1201 dest->v.uvec.xyz.x = vm_vec_dot3(src0->v.rvec.xyz.x,src0->v.uvec.xyz.x,src0->v.fvec.xyz.x, &src1->v.uvec);
1202 dest->v.fvec.xyz.x = vm_vec_dot3(src0->v.rvec.xyz.x,src0->v.uvec.xyz.x,src0->v.fvec.xyz.x, &src1->v.fvec);
1204 dest->v.rvec.xyz.y = vm_vec_dot3(src0->v.rvec.xyz.y,src0->v.uvec.xyz.y,src0->v.fvec.xyz.y, &src1->v.rvec);
1205 dest->v.uvec.xyz.y = vm_vec_dot3(src0->v.rvec.xyz.y,src0->v.uvec.xyz.y,src0->v.fvec.xyz.y, &src1->v.uvec);
1206 dest->v.fvec.xyz.y = vm_vec_dot3(src0->v.rvec.xyz.y,src0->v.uvec.xyz.y,src0->v.fvec.xyz.y, &src1->v.fvec);
1208 dest->v.rvec.xyz.z = vm_vec_dot3(src0->v.rvec.xyz.z,src0->v.uvec.xyz.z,src0->v.fvec.xyz.z, &src1->v.rvec);
1209 dest->v.uvec.xyz.z = vm_vec_dot3(src0->v.rvec.xyz.z,src0->v.uvec.xyz.z,src0->v.fvec.xyz.z, &src1->v.uvec);
1210 dest->v.fvec.xyz.z = vm_vec_dot3(src0->v.rvec.xyz.z,src0->v.uvec.xyz.z,src0->v.fvec.xyz.z, &src1->v.fvec);
1217 //extract angles from a matrix
1218 angles *vm_extract_angles_matrix(angles *a,matrix *m)
1220 float sinh,cosh,cosp;
1222 if (m->v.fvec.xyz.x==0.0 && m->v.fvec.xyz.z==0.0) //zero head
1225 // a->h = (float)atan2(m->v.fvec.xyz.z,m->v.fvec.xyz.x);
1226 a->h = (float)atan2_safe(m->v.fvec.xyz.x,m->v.fvec.xyz.z);
1228 sinh = (float)sin(a->h); cosh = (float)cos(a->h);
1230 if (fl_abs(sinh) > fl_abs(cosh)) //sine is larger, so use it
1231 cosp = m->v.fvec.xyz.x*sinh;
1232 else //cosine is larger, so use it
1233 cosp = m->v.fvec.xyz.z*cosh;
1235 if (cosp==0.0 && m->v.fvec.xyz.y==0.0)
1238 // a->p = (float)atan2(cosp,-m->v.fvec.xyz.y);
1239 a->p = (float)atan2_safe(-m->v.fvec.xyz.y, cosp);
1242 if (cosp == 0.0) //the cosine of pitch is zero. we're pitched straight up. say no bank
1249 sinb = m->v.rvec.xyz.y/cosp;
1250 cosb = m->v.uvec.xyz.y/cosp;
1252 if (sinb==0.0 && cosb==0.0)
1255 // a->b = (float)atan2(cosb,sinb);
1256 a->b = (float)atan2_safe(sinb,cosb);
1264 //extract heading and pitch from a vector, assuming bank==0
1265 angles *vm_extract_angles_vector_normalized(angles *a,vector *v)
1268 a->b = 0.0f; //always zero bank
1270 a->p = (float)asin(-v->xyz.y);
1272 if (v->xyz.x==0.0f && v->xyz.z==0.0f)
1275 a->h = (float)atan2_safe(v->xyz.z,v->xyz.x);
1280 //extract heading and pitch from a vector, assuming bank==0
1281 angles *vm_extract_angles_vector(angles *a,vector *v)
1285 if (vm_vec_copy_normalize(&t,v) != 0.0)
1286 vm_extract_angles_vector_normalized(a,&t);
1291 //compute the distance from a point to a plane. takes the normalized normal
1292 //of the plane (ebx), a point on the plane (edi), and the point to check (esi).
1293 //returns distance in eax
1294 //distance is signed, so negative dist is on the back of the plane
1295 float vm_dist_to_plane(vector *checkp,vector *norm,vector *planep)
1300 vm_vec_sub(&t,checkp,planep);
1302 t1 = vm_vec_dot(&t,norm);
1308 // Given mouse movement in dx, dy, returns a 3x3 rotation matrix in RotMat.
1309 // Taken from Graphics Gems III, page 51, "The Rolling Ball"
1311 //if ( (Mouse.dx!=0) || (Mouse.dy!=0) ) {
1312 // GetMouseRotation( Mouse.dx, Mouse.dy, &MouseRotMat );
1313 // vm_matrix_x_matrix(&tempm,&LargeView.ev_matrix,&MouseRotMat);
1314 // LargeView.ev_matrix = tempm;
1318 void vm_trackball( int idx, int idy, matrix * RotMat )
1320 float dr, cos_theta, sin_theta, denom, cos_theta1;
1321 float Radius = 100.0f;
1327 dx = (float)idx; dy = (float)idy;
1329 dr = fl_sqrt(dx*dx+dy*dy);
1331 denom = fl_sqrt(Radius*Radius+dr*dr);
1333 cos_theta = Radius/denom;
1334 sin_theta = dr/denom;
1336 cos_theta1 = 1.0f - cos_theta;
1341 RotMat->v.rvec.xyz.x = cos_theta + (dydr*dydr)*cos_theta1;
1342 RotMat->v.uvec.xyz.x = - ((dxdr*dydr)*cos_theta1);
1343 RotMat->v.fvec.xyz.x = (dxdr*sin_theta);
1345 RotMat->v.rvec.xyz.y = RotMat->v.uvec.xyz.x;
1346 RotMat->v.uvec.xyz.y = cos_theta + ((dxdr*dxdr)*cos_theta1);
1347 RotMat->v.fvec.xyz.y = (dydr*sin_theta);
1349 RotMat->v.rvec.xyz.z = -RotMat->v.fvec.xyz.x;
1350 RotMat->v.uvec.xyz.z = -RotMat->v.fvec.xyz.y;
1351 RotMat->v.fvec.xyz.z = cos_theta;
1354 // Compute the outer product of A = A * transpose(A). 1x3 vector becomes 3x3 matrix.
1355 void vm_vec_outer_product(matrix *mat, vector *vec)
1357 mat->v.rvec.xyz.x = vec->xyz.x * vec->xyz.x;
1358 mat->v.rvec.xyz.y = vec->xyz.x * vec->xyz.y;
1359 mat->v.rvec.xyz.z = vec->xyz.x * vec->xyz.z;
1361 mat->v.uvec.xyz.x = vec->xyz.y * vec->xyz.x;
1362 mat->v.uvec.xyz.y = vec->xyz.y * vec->xyz.y;
1363 mat->v.uvec.xyz.z = vec->xyz.y * vec->xyz.z;
1365 mat->v.fvec.xyz.x = vec->xyz.z * vec->xyz.x;
1366 mat->v.fvec.xyz.y = vec->xyz.z * vec->xyz.y;
1367 mat->v.fvec.xyz.z = vec->xyz.z * vec->xyz.z;
1370 // Find the point on the line between p0 and p1 that is nearest to int_pnt.
1371 // Stuff result in nearest_point.
1372 // Uses algorithm from page 148 of Strang, Linear Algebra and Its Applications.
1373 // Returns value indicating whether *nearest_point is between *p0 and *p1.
1374 // 0.0f means *nearest_point is *p0, 1.0f means it's *p1. 2.0f means it's beyond p1 by 2x.
1375 // -1.0f means it's "before" *p0 by 1x.
1376 float find_nearest_point_on_line(vector *nearest_point, vector *p0, vector *p1, vector *int_pnt)
1378 vector norm, xlated_int_pnt, projected_point;
1382 vm_vec_sub(&norm, p1, p0);
1383 vm_vec_sub(&xlated_int_pnt, int_pnt, p0);
1385 if (IS_VEC_NULL(&norm)) {
1386 *nearest_point = *int_pnt;
1390 mag = vm_vec_normalize(&norm); // Normalize vector so we don't have to divide by dot product.
1393 *nearest_point = *int_pnt;
1395 // Warning(LOCATION, "Very small magnitude in find_nearest_point_on_line.\n");
1398 vm_vec_outer_product(&mat, &norm);
1400 vm_vec_rotate(&projected_point, &xlated_int_pnt, &mat);
1401 vm_vec_add(nearest_point, &projected_point, p0);
1403 dot = vm_vec_dot(&norm, &projected_point);
1408 //make sure matrix is orthogonal
1409 //computes a matrix from one or more vectors. The forward vector is required,
1410 //with the other two being optional. If both up & right vectors are passed,
1411 //the up vector is used. If only the forward vector is passed, a bank of
1413 //returns ptr to matrix
1414 void vm_orthogonalize_matrix(matrix *m_src)
1418 matrix * m = &tempm;
1420 if (vm_vec_copy_normalize(&m->v.fvec,&m_src->v.fvec) == 0.0f) {
1421 Error( LOCATION, "forward vec should not be zero-length" );
1424 umag = vm_vec_mag(&m_src->v.uvec);
1425 rmag = vm_vec_mag(&m_src->v.rvec);
1426 if (umag <= 0.0f) { // no up vector to use..
1427 if (rmag <= 0.0f) { // no right vector either, so make something up
1428 if (!m->v.fvec.xyz.x && !m->v.fvec.xyz.z && m->v.fvec.xyz.y) // vertical vector
1429 (void) vm_vec_make(&m->v.uvec, 0.0f, 0.0f, 1.0f);
1431 (void) vm_vec_make(&m->v.uvec, 0.0f, 1.0f, 0.0f);
1433 } else { // use the right vector to figure up vector
1434 vm_vec_crossprod(&m->v.uvec, &m->v.fvec, &m_src->v.rvec);
1435 if (vm_vec_normalize(&m->v.uvec) == 0.0f)
1436 Error( LOCATION, "Bad vector!" );
1439 } else { // use source up vector
1440 vm_vec_copy_normalize(&m->v.uvec, &m_src->v.uvec);
1443 // use forward and up vectors as good vectors to calculate right vector
1444 vm_vec_crossprod(&m->v.rvec, &m->v.uvec, &m->v.fvec);
1446 //normalize new perpendicular vector
1447 if (vm_vec_normalize(&m->v.rvec) == 0.0f)
1448 Error( LOCATION, "Bad vector!" );
1450 //now recompute up vector, in case it wasn't entirely perpendiclar
1451 vm_vec_crossprod(&m->v.uvec, &m->v.fvec, &m->v.rvec);
1455 // like vm_orthogonalize_matrix(), except that zero vectors can exist within the
1456 // matrix without causing problems. Valid vectors will be created where needed.
1457 void vm_fix_matrix(matrix *m)
1459 float fmag, umag, rmag;
1461 fmag = vm_vec_mag(&m->v.fvec);
1462 umag = vm_vec_mag(&m->v.uvec);
1463 rmag = vm_vec_mag(&m->v.rvec);
1465 if ((umag > 0.0f) && (rmag > 0.0f) && !vm_test_parallel(&m->v.uvec, &m->v.rvec)) {
1466 vm_vec_crossprod(&m->v.fvec, &m->v.uvec, &m->v.rvec);
1467 vm_vec_normalize(&m->v.fvec);
1469 } else if (umag > 0.0f) {
1470 if (!m->v.uvec.xyz.x && !m->v.uvec.xyz.y && m->v.uvec.xyz.z) // z vector
1471 (void) vm_vec_make(&m->v.fvec, 1.0f, 0.0f, 0.0f);
1473 (void) vm_vec_make(&m->v.fvec, 0.0f, 0.0f, 1.0f);
1477 vm_vec_normalize(&m->v.fvec);
1479 // we now have a valid and normalized forward vector
1481 if ((umag <= 0.0f) || vm_test_parallel(&m->v.fvec, &m->v.uvec)) { // no up vector to use..
1482 if ((rmag <= 0.0f) || vm_test_parallel(&m->v.fvec, &m->v.rvec)) { // no right vector either, so make something up
1483 if (!m->v.fvec.xyz.x && m->v.fvec.xyz.y && !m->v.fvec.xyz.z) // vertical vector
1484 (void) vm_vec_make(&m->v.uvec, 0.0f, 0.0f, -1.0f);
1486 (void) vm_vec_make(&m->v.uvec, 0.0f, 1.0f, 0.0f);
1488 } else { // use the right vector to figure up vector
1489 vm_vec_crossprod(&m->v.uvec, &m->v.fvec, &m->v.rvec);
1490 vm_vec_normalize(&m->v.uvec);
1494 vm_vec_normalize(&m->v.uvec);
1496 // we now have both valid and normalized forward and up vectors
1498 vm_vec_crossprod(&m->v.rvec, &m->v.uvec, &m->v.fvec);
1500 //normalize new perpendicular vector
1501 vm_vec_normalize(&m->v.rvec);
1503 //now recompute up vector, in case it wasn't entirely perpendiclar
1504 vm_vec_crossprod(&m->v.uvec, &m->v.fvec, &m->v.rvec);
1507 //Rotates the orient matrix by the angles in tangles and then
1508 //makes sure that the matrix is orthogonal.
1509 void vm_rotate_matrix_by_angles( matrix *orient, angles *tangles )
1511 matrix rotmat,new_orient;
1512 vm_angles_2_matrix(&rotmat,tangles);
1513 vm_matrix_x_matrix(&new_orient,orient,&rotmat);
1514 *orient = new_orient;
1515 vm_orthogonalize_matrix(orient);
1518 // dir must be normalized!
1519 float vm_vec_dot_to_point(vector *dir, vector *p1, vector *p2)
1523 vm_vec_sub(&tvec, p2, p1);
1524 vm_vec_normalize(&tvec);
1526 return vm_vec_dot(dir, &tvec);
1530 /////////////////////////////////////////////////////////
1531 // Given a plane and a point, return the point on the plane closest the the point.
1532 // Result returned in q.
1533 void compute_point_on_plane(vector *q, plane *planep, vector *p)
1538 normal.xyz.x = planep->A;
1539 normal.xyz.y = planep->B;
1540 normal.xyz.z = planep->C;
1542 k = (planep->D + vm_vec_dot(&normal, p)) / vm_vec_dot(&normal, &normal);
1544 vm_vec_scale_add(q, p, &normal, -k);
1548 // Generate a fairly random vector that's fairly near normalized.
1549 void vm_vec_rand_vec_quick(vector *rvec)
1551 rvec->xyz.x = (frand() - 0.5f) * 2;
1552 rvec->xyz.y = (frand() - 0.5f) * 2;
1553 rvec->xyz.z = (frand() - 0.5f) * 2;
1555 if (IS_VEC_NULL(rvec))
1558 vm_vec_normalize_quick(rvec);
1561 // Given an point "in" rotate it by "angle" around an
1562 // arbritary line defined by a point on the line "line_point"
1563 // and the normalized line direction, "line_dir"
1564 // Returns the rotated point in "out".
1565 void vm_rot_point_around_line(vector *out, vector *in, float angle, vector *line_point, vector *line_dir)
1571 vm_vector_2_matrix_norm(&m, line_dir, NULL, NULL );
1572 vm_copy_transpose_matrix(&im,&m);
1576 vm_angles_2_matrix(&r,&ta);
1578 vm_vec_sub( &tmp, in, line_point ); // move relative to a point on line
1579 vm_vec_rotate( &tmp1, &tmp, &m); // rotate into line's base
1580 vm_vec_rotate( &tmp, &tmp1, &r); // rotate around Z
1581 vm_vec_rotate( &tmp1, &tmp, &im); // unrotate out of line's base
1582 vm_vec_add( out, &tmp1, line_point ); // move back to world coordinates
1585 // Given two position vectors, return 0 if the same, else non-zero.
1586 int vm_vec_cmp( vector * a, vector * b )
1588 float diff = vm_vec_dist(a,b);
1589 //mprintf(( "Diff=%.32f\n", diff ));
1590 if ( diff > 0.005f )
1596 // Given two orientation matrices, return 0 if the same, else non-zero.
1597 int vm_matrix_cmp( matrix * a, matrix * b )
1599 float tmp1,tmp2,tmp3;
1600 tmp1 = (float)fl_abs(vm_vec_dot( &a->v.uvec, &b->v.uvec ) - 1.0f);
1601 tmp2 = (float)fl_abs(vm_vec_dot( &a->v.fvec, &b->v.fvec ) - 1.0f);
1602 tmp3 = (float)fl_abs(vm_vec_dot( &a->v.rvec, &b->v.rvec ) - 1.0f);
1603 // mprintf(( "Mat=%.16f, %.16f, %.16f\n", tmp1, tmp2, tmp3 ));
1605 if ( tmp1 > 0.0000005f ) return 1;
1606 if ( tmp2 > 0.0000005f ) return 1;
1607 if ( tmp3 > 0.0000005f ) return 1;
1612 // Moves angle 'h' towards 'desired_angle', taking the shortest
1613 // route possible. It will move a maximum of 'step_size' radians
1614 // each call. All angles in radians.
1615 void vm_interp_angle( float *h, float desired_angle, float step_size )
1619 if ( desired_angle < 0.0f ) desired_angle += PI2;
1620 if ( desired_angle > PI2 ) desired_angle -= PI2;
1622 delta = desired_angle - *h;
1624 if ( fl_abs(delta) > PI ) {
1625 // Go the other way, since it will be shorter.
1626 if ( delta > 0.0f ) {
1627 delta = delta - PI2;
1629 delta = PI2 - delta;
1633 if ( delta > step_size )
1635 else if ( delta < -step_size )
1640 // If we wrap outside of 0 to 2*PI, then put the
1641 // angle back in the range 0 to 2*PI.
1642 if ( *h > PI2 ) *h -= PI2;
1643 if ( *h < 0.0f ) *h += PI2;
1646 // check a matrix for zero rows and columns
1647 int vm_check_matrix_for_zeros(matrix *m)
1649 if (!m->v.fvec.xyz.x && !m->v.fvec.xyz.y && !m->v.fvec.xyz.z)
1651 if (!m->v.rvec.xyz.x && !m->v.rvec.xyz.y && !m->v.rvec.xyz.z)
1653 if (!m->v.uvec.xyz.x && !m->v.uvec.xyz.y && !m->v.uvec.xyz.z)
1656 if (!m->v.fvec.xyz.x && !m->v.rvec.xyz.x && !m->v.uvec.xyz.x)
1658 if (!m->v.fvec.xyz.y && !m->v.rvec.xyz.y && !m->v.uvec.xyz.y)
1660 if (!m->v.fvec.xyz.z && !m->v.rvec.xyz.z && !m->v.uvec.xyz.z)
1666 // see if two vectors are the same
1667 int vm_vec_same(vector *v1, vector *v2)
1669 if ( v1->xyz.x == v2->xyz.x && v1->xyz.y == v2->xyz.y && v1->xyz.z == v2->xyz.z )
1676 // --------------------------------------------------------------------------------------
1678 void vm_quaternion_rotate(matrix *M, float theta, vector *u)
1679 // given an arbitrary rotation axis and rotation angle, function generates the
1680 // corresponding rotation matrix
1682 // M is the return rotation matrix theta is the angle of rotation
1683 // u is the direction of the axis.
1684 // this is adapted from Computer Graphics (Hearn and Bker 2nd ed.) p. 420
1690 a = (float) (u->xyz.x * sin(theta * 0.5f));
1691 b = (float) (u->xyz.y * sin(theta * 0.5f));
1692 c = (float) (u->xyz.z * sin(theta * 0.5f));
1693 s = (float) cos(theta/2.0);
1696 M->v.rvec.xyz.x = 1.0f - 2.0f*b*b - 2.0f*c*c;
1697 M->v.rvec.xyz.y = 2.0f*a*b + 2.0f*s*c;
1698 M->v.rvec.xyz.z = 2.0f*a*c - 2.0f*s*b;
1700 M->v.uvec.xyz.x = 2.0f*a*b - 2.0f*s*c;
1701 M->v.uvec.xyz.y = 1.0f - 2.0f*a*a - 2.0f*c*c;
1702 M->v.uvec.xyz.z = 2.0f*b*c + 2.0f*s*a;
1704 M->v.fvec.xyz.x = 2.0f*a*c + 2.0f*s*b;
1705 M->v.fvec.xyz.y = 2.0f*b*c - 2.0f*s*a;
1706 M->v.fvec.xyz.z = 1.0f - 2.0f*a*a - 2.0f*b*b;
1709 // --------------------------------------------------------------------------------------
1710 // function finds the rotation matrix about the z axis for a given rotation angle (in radians)
1711 // this is an optimized version vm_quaternion_rotate
1713 // inputs: m => point to resultant rotation matrix
1714 // angle => rotation angle about z axis (in radians)
1716 void rotate_z ( matrix *m, float theta )
1718 m->v.rvec.xyz.x = (float) cos (theta);
1719 m->v.rvec.xyz.y = (float) sin (theta);
1720 m->v.rvec.xyz.z = 0.0f;
1722 m->v.uvec.xyz.x = -m->v.rvec.xyz.y;
1723 m->v.uvec.xyz.y = m->v.rvec.xyz.x;
1724 m->v.uvec.xyz.z = 0.0f;
1726 m->v.fvec.xyz.x = 0.0f;
1727 m->v.fvec.xyz.y = 0.0f;
1728 m->v.fvec.xyz.z = 1.0f;
1732 // --------------------------------------------------------------------------------------
1734 //void vm_matrix_to_rot_axis_and_angle(matrix *m, float *theta, vector *rot_axis)
1735 // Converts a matrix into a rotation axis and an angle around that axis
1736 // Note for angle is very near 0, returns 0 with axis of (1,0,0)
1737 // For angles near PI, returns PI with correct axis
1739 // rot_axis - the resultant axis of rotation
1740 // theta - the resultatn rotation around the axis
1741 // m - the initial matrix
1742 void vm_matrix_to_rot_axis_and_angle(matrix *m, float *theta, vector *rot_axis)
1744 float trace = m->a2d[0][0] + m->a2d[1][1] + m->a2d[2][2];
1745 float cos_theta = 0.5f * (trace - 1.0f);
1747 if (cos_theta > 0.999999875f) { // angle is less than 1 milirad (0.057 degrees)
1750 (void) vm_vec_make(rot_axis, 1.0f, 0.0f, 0.0f);
1751 } else if (cos_theta > -0.999999875f) { // angle is within limits between 0 and PI
1752 *theta = float(acos(cos_theta));
1753 SDL_assert(!isnan(*theta));
1755 rot_axis->xyz.x = (m->v.uvec.xyz.z - m->v.fvec.xyz.y);
1756 rot_axis->xyz.y = (m->v.fvec.xyz.x - m->v.rvec.xyz.z);
1757 rot_axis->xyz.z = (m->v.rvec.xyz.y - m->v.uvec.xyz.x);
1758 vm_vec_normalize(rot_axis);
1759 } else { // angle is PI within limits
1762 // find index of largest diagonal term
1763 int largest_diagonal_index = 0;
1765 if (m->a2d[1][1] > m->a2d[0][0]) {
1766 largest_diagonal_index = 1;
1768 if (m->a2d[2][2] > m->a2d[largest_diagonal_index][largest_diagonal_index]) {
1769 largest_diagonal_index = 2;
1772 switch (largest_diagonal_index) {
1776 rot_axis->xyz.x = fl_sqrt(m->a2d[0][0] + 1.0f);
1777 ix = 1.0f / rot_axis->xyz.x;
1778 rot_axis->xyz.y = m->a2d[0][1] * ix;
1779 rot_axis->xyz.z = m->a2d[0][2] * ix;
1780 vm_vec_normalize(rot_axis);
1786 rot_axis->xyz.y = fl_sqrt(m->a2d[1][1] + 1.0f);
1787 iy = 1.0f / rot_axis->xyz.y;
1788 rot_axis->xyz.x = m->a2d[1][0] * iy;
1789 rot_axis->xyz.z = m->a2d[1][2] * iy;
1790 vm_vec_normalize(rot_axis);
1796 rot_axis->xyz.z = fl_sqrt(m->a2d[2][2] + 1.0f);
1797 iz = 1.0f / rot_axis->xyz.z;
1798 rot_axis->xyz.x = m->a2d[2][0] * iz;
1799 rot_axis->xyz.y = m->a2d[2][1] * iz;
1803 Int3(); // this should never happen
1807 // normalize rotation axis
1808 vm_vec_normalize(rot_axis);
1813 // --------------------------------------------------------------------------------------
1814 // This routine determines the resultant angular displacement and angular velocity in trying to reach a goal
1815 // given an angular velocity APPROACHing a goal. It uses maximal acceleration to a point (called peak), then maximal
1816 // deceleration to arrive at the goal with zero angular velocity. This can occasionally cause overshoot.
1821 // returns delta_theta
1822 float away(float w_in, float w_max, float theta_goal, float aa, float delta_t, float *w_out, int no_overshoot);
1823 float approach(float w_in, float w_max, float theta_goal, float aa, float delta_t, float *w_out, int no_overshoot)
1825 float delta_theta; // amount rotated during time delta_t
1826 SDL_assert(w_in >= 0);
1827 SDL_assert(theta_goal > 0);
1832 delta_theta = w_in*delta_t;
1836 if (no_overshoot && (w_in*w_in > 2.0f*1.05f*aa*theta_goal)) {
1837 w_in = fl_sqrt(2.0f*aa*theta_goal);
1840 if (w_in*w_in > 2.0f*1.05f*aa*theta_goal) { // overshoot condition
1841 effective_aa = 1.05f*aa;
1842 delta_theta = w_in*delta_t - 0.5f*effective_aa*delta_t*delta_t;
1844 if (delta_theta > theta_goal) { // pass goal during this frame
1845 float t_goal = (-w_in + fl_sqrt(w_in*w_in +2.0f*effective_aa*theta_goal)) / effective_aa;
1846 // get time to theta_goal and away
1847 SDL_assert(t_goal < delta_t);
1848 w_in -= effective_aa*t_goal;
1849 delta_theta = w_in*t_goal + 0.5f*effective_aa*t_goal*t_goal;
1850 delta_theta -= away(-w_in, w_max, 0.0f, aa, delta_t - t_goal, w_out, no_overshoot);
1854 if (delta_theta < 0) {
1855 // pass goal and return this frame
1859 // do not pass goal this frame
1860 *w_out = w_in - effective_aa*delta_t;
1864 } else if (w_in*w_in < 2.0f*0.95f*aa*theta_goal) { // undershoot condition
1865 // find peak angular velocity
1866 float wp_sqr = fl_abs(aa*theta_goal + 0.5f*w_in*w_in);
1867 SDL_assert(wp_sqr >= 0);
1869 if (wp_sqr > w_max*w_max) {
1870 float time_to_w_max = (w_max - w_in) / aa;
1871 if (time_to_w_max < 0) {
1872 // speed already too high
1873 // TODO: consider possible ramp down to below w_max
1874 *w_out = w_in - aa*delta_t;
1879 delta_theta = 0.5f*(w_in + *w_out)*delta_t;
1881 } else if (time_to_w_max > delta_t) {
1882 // does not reach w_max this frame
1883 *w_out = w_in + aa*delta_t;
1884 delta_theta = 0.5f*(w_in + *w_out)*delta_t;
1887 // reaches w_max this frame
1888 // TODO: consider when to ramp down from w_max
1890 delta_theta = 0.5f*(w_in + *w_out)*delta_t;
1893 } else { // wp < w_max
1894 if (wp_sqr > (w_in + aa*delta_t)*(w_in + aa*delta_t)) {
1895 // does not reach wp this frame
1896 *w_out = w_in + aa*delta_t;
1897 delta_theta = 0.5f*(w_in + *w_out)*delta_t;
1900 // reaches wp this frame
1901 float wp = fl_sqrt(wp_sqr);
1902 float time_to_wp = (wp - w_in) / aa;
1903 SDL_assert(time_to_wp > 0);
1907 delta_theta = 0.5f*(w_in + *w_out)*time_to_wp;
1910 float time_remaining = delta_t - time_to_wp;
1911 *w_out -= aa*time_remaining;
1912 if (*w_out < 0) { // reached goal
1914 delta_theta = theta_goal;
1917 delta_theta += 0.5f*(wp + *w_out)*time_remaining;
1921 } else { // on target
1922 // reach goal this frame
1923 if (w_in - aa*delta_t < 0) {
1924 // reach goal this frame
1929 *w_out = w_in - aa*delta_t;
1930 SDL_assert(*w_out >= 0);
1931 delta_theta = 0.5f*(w_in + *w_out)*delta_t;
1938 // --------------------------------------------------------------------------------------
1940 // This routine determines the resultant angular displacement and angular velocity in trying to reach a goal
1941 // given an angular velocity AWAY from a goal. It uses maximal acceleration to a point (called peak), then maximal
1942 // deceleration to arrive at the goal with zero angular acceleration.
1947 // returns angle rotated this frame
1948 float away(float w_in, float w_max, float theta_goal, float aa, float delta_t, float *w_out, int no_overshoot)
1951 float delta_theta;// amount rotated during time
1952 float t0; // time to velocity is 0
1953 float t_excess; // time remaining in interval after velocity is 0
1955 SDL_assert(theta_goal >=0);
1956 SDL_assert(w_in <= 0);
1958 if ((-w_in < 1e-5) && (theta_goal < 1e-5)) {
1965 delta_theta = w_in*delta_t;
1971 if (t0 > delta_t) { // no reversal in this time interval
1972 *w_out = w_in + aa * delta_t;
1973 delta_theta = (w_in + *w_out) / 2.0f * delta_t;
1977 // use time remaining after v = 0
1978 delta_theta = 0.5f*w_in*t0;
1979 theta_goal -= delta_theta; // delta_theta is *negative*
1980 t_excess = delta_t - t0;
1981 delta_theta += approach(0.0f, w_max, theta_goal, aa, t_excess, w_out, no_overshoot);
1985 // --------------------------------------------------------------------------------------
1987 void vm_matrix_interpolate(matrix *goal_orient, matrix *curr_orient, vector *w_in, float delta_t,
1988 matrix *next_orient, vector *w_out, vector *vel_limit, vector *acc_limit, int no_overshoot)
1990 matrix rot_matrix; // rotation matrix from curr_orient to goal_orient
1991 matrix Mtemp1; // temp matrix
1992 vector rot_axis; // vector indicating direction of rotation axis
1993 vector theta_goal; // desired angular position at the end of the time interval
1994 vector theta_end; // actual angular position at the end of the time interval
1995 float theta; // magnitude of rotation about the rotation axis
1997 // FIND ROTATION NEEDED FOR GOAL
1998 // goal_orient = R curr_orient, so R = goal_orient curr_orient^-1
1999 vm_copy_transpose_matrix(&Mtemp1, curr_orient); // Mtemp1 = curr ^-1
2000 vm_matrix_x_matrix(&rot_matrix, &Mtemp1, goal_orient); // R = goal * Mtemp1
2001 vm_orthogonalize_matrix(&rot_matrix);
2002 vm_matrix_to_rot_axis_and_angle(&rot_matrix, &theta, &rot_axis); // determines angle and rotation axis from curr to goal
2004 // find theta to goal
2005 vm_vec_copy_scale(&theta_goal, &rot_axis, theta);
2007 if (theta < SMALL_NUM) {
2008 *next_orient = *goal_orient;
2013 theta_end = vmd_zero_vector;
2016 // find rotation about x
2017 if (theta_goal.xyz.x > 0) {
2018 if (w_in->xyz.x >= 0) {
2019 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);
2020 theta_end.xyz.x = delta_theta;
2021 } else { // w_in->xyz.x < 0
2022 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);
2023 theta_end.xyz.x = delta_theta;
2025 } else if (theta_goal.xyz.x < 0) {
2026 if (w_in->xyz.x <= 0) {
2027 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);
2028 theta_end.xyz.x = -delta_theta;
2029 w_out->xyz.x = -w_out->xyz.x;
2030 } else { // w_in->xyz.x > 0
2031 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);
2032 theta_end.xyz.x = -delta_theta;
2033 w_out->xyz.x = -w_out->xyz.x;
2035 } else { // theta_goal == 0
2036 if (w_in->xyz.x < 0) {
2037 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);
2038 theta_end.xyz.x = delta_theta;
2040 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);
2041 theta_end.xyz.x = -delta_theta;
2042 w_out->xyz.x = -w_out->xyz.x;
2047 // find rotation about y
2048 if (theta_goal.xyz.y > 0) {
2049 if (w_in->xyz.y >= 0) {
2050 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);
2051 theta_end.xyz.y = delta_theta;
2052 } else { // w_in->xyz.y < 0
2053 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);
2054 theta_end.xyz.y = delta_theta;
2056 } else if (theta_goal.xyz.y < 0) {
2057 if (w_in->xyz.y <= 0) {
2058 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);
2059 theta_end.xyz.y = -delta_theta;
2060 w_out->xyz.y = -w_out->xyz.y;
2061 } else { // w_in->xyz.y > 0
2062 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);
2063 theta_end.xyz.y = -delta_theta;
2064 w_out->xyz.y = -w_out->xyz.y;
2066 } else { // theta_goal == 0
2067 if (w_in->xyz.y < 0) {
2068 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);
2069 theta_end.xyz.y = delta_theta;
2071 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);
2072 theta_end.xyz.y = -delta_theta;
2073 w_out->xyz.y = -w_out->xyz.y;
2077 // find rotation about z
2078 if (theta_goal.xyz.z > 0) {
2079 if (w_in->xyz.z >= 0) {
2080 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);
2081 theta_end.xyz.z = delta_theta;
2082 } else { // w_in->xyz.z < 0
2083 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);
2084 theta_end.xyz.z = delta_theta;
2086 } else if (theta_goal.xyz.z < 0) {
2087 if (w_in->xyz.z <= 0) {
2088 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);
2089 theta_end.xyz.z = -delta_theta;
2090 w_out->xyz.z = -w_out->xyz.z;
2091 } else { // w_in->xyz.z > 0
2092 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);
2093 theta_end.xyz.z = -delta_theta;
2094 w_out->xyz.z = -w_out->xyz.z;
2096 } else { // theta_goal == 0
2097 if (w_in->xyz.z < 0) {
2098 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);
2099 theta_end.xyz.z = delta_theta;
2101 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);
2102 theta_end.xyz.z = -delta_theta;
2103 w_out->xyz.z = -w_out->xyz.z;
2107 // the amount of rotation about each axis is determined in
2108 // functions approach and away. first find the magnitude
2109 // of the rotation and then normalize the axis
2110 rot_axis = theta_end;
2111 SDL_assert(is_valid_vec(&rot_axis));
2112 SDL_assert(vm_vec_mag(&rot_axis) > 0);
2114 // normalize rotation axis and determine total rotation angle
2115 theta = vm_vec_normalize(&rot_axis);
2118 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) {
2119 *next_orient = *goal_orient;
2121 // otherwise rotate to better position
2122 vm_quaternion_rotate(&Mtemp1, theta, &rot_axis);
2123 SDL_assert(is_valid_matrix(&Mtemp1));
2124 vm_matrix_x_matrix(next_orient, curr_orient, &Mtemp1);
2125 vm_orthogonalize_matrix(next_orient);
2127 } // end matrix_interpolate
2130 // --------------------------------------------------------------------------------------
2133 void get_camera_limits(matrix *start_camera, matrix *end_camera, float time, vector *acc_max, vector *w_max)
2135 matrix temp, rot_matrix;
2140 // determine the necessary rotation matrix
2141 vm_copy_transpose(&temp, start_camera);
2142 vm_matrix_x_matrix(&rot_matrix, &temp, end_camera);
2143 vm_orthogonalize_matrix(&rot_matrix);
2145 // determine the rotation axis and angle
2146 vm_matrix_to_rot_axis_and_angle(&rot_matrix, &theta, &rot_axis);
2148 // find the rotation about each axis
2149 angle.xyz.x = theta * rot_axis.xyz.x;
2150 angle.xyz.y = theta * rot_axis.xyz.y;
2151 angle.xyz.z = theta * rot_axis.xyz.z;
2153 // allow for 0 time input
2154 if (time <= 1e-5f) {
2155 (void) vm_vec_make(acc_max, 0.0f, 0.0f, 0.0f);
2156 (void) vm_vec_make(w_max, 0.0f, 0.0f, 0.0f);
2159 // find acceleration limit using (theta/2) takes (time/2)
2160 // and using const accel theta = 1/2 acc * time^2
2161 acc_max->xyz.x = 4.0f * (float)fl_abs(angle.xyz.x) / (time * time);
2162 acc_max->xyz.y = 4.0f * (float)fl_abs(angle.xyz.y) / (time * time);
2163 acc_max->xyz.z = 4.0f * (float)fl_abs(angle.xyz.z) / (time * time);
2165 // find angular velocity limits
2166 // w_max = acc_max * time / 2
2167 w_max->xyz.x = acc_max->xyz.x * time / 2.0f;
2168 w_max->xyz.y = acc_max->xyz.y * time / 2.0f;
2169 w_max->xyz.z = acc_max->xyz.z * time / 2.0f;
2173 // ---------------------------------------------------------------------------------------------
2175 // inputs: goal_orient => goal orientation matrix
2176 // orient => current orientation matrix (with current forward vector)
2177 // w_in => current input angular velocity
2178 // delta_t => time to move toward goal
2179 // next_orient => the orientation matrix at time delta_t (with current forward vector)
2180 // NOTE: this does not include any rotation about z (bank)
2181 // w_out => the angular velocity of the ship at delta_t
2182 // vel_limit => maximum rotational speed
2183 // acc_limit => maximum rotational speed
2185 // function moves the forward vector toward the goal forward vector taking account of anglular
2186 // momentum (velocity) Attempt to try to move bank by goal delta_bank. Rotational velocity
2187 // on x/y is rotated with bank, giving smoother motion.
2188 void vm_fvec_matrix_interpolate(matrix *goal_orient, matrix *orient, vector *w_in, float delta_t, matrix *next_orient,
2189 vector *w_out, vector *vel_limit, vector *acc_limit, int no_overshoot)
2191 matrix Mtemp1; // temporary matrix
2192 matrix M_intermed; // intermediate matrix after xy rotation
2193 vector local_rot_axis; // vector indicating direction of rotation axis (local coords)
2194 vector rot_axis; // vector indicating direction of rotation axis (world coords)
2195 vector theta_goal; // desired angular position at the end of the time interval
2196 vector theta_end; // actual angular position at the end of the time interval
2197 float theta; // magnitude of rotation about the rotation axis
2198 float bank; // magnitude of rotation about the forward axis
2199 int no_bank; // flag set if there is no bank for the object
2200 vector vtemp; // temp angular velocity before rotation about z
2201 float z_dotprod; // dotprod of orient->v.fvec and goal_orient->v.fvec
2202 float r_dotprod; // dotprod of orient->v.rvec and goal_orient->v.rvec
2205 // FIND XY ROTATION NEEDED FOR GOAL
2206 // rotation vector is (current fvec) orient->v.fvec x goal_f
2207 // magnitude = asin ( magnitude of crossprod )
2208 vm_vec_crossprod ( &rot_axis, &orient->v.fvec, &goal_orient->v.fvec );
2210 float t = vm_vec_mag(&rot_axis);
2214 z_dotprod = vm_vec_dotprod ( &orient->v.fvec, &goal_orient->v.fvec );
2216 if ( t < SMALLER_NUM ) {
2217 if ( z_dotprod > 0.0f )
2219 else { // the forward vector is pointing exactly opposite of goal
2220 // arbitrarily choose the x axis to rotate around until t becomes large enough
2222 rot_axis = orient->v.rvec;
2225 theta = (float) asin ( t );
2226 vm_vec_scale ( &rot_axis, 1/t );
2227 if ( z_dotprod < 0.0f )
2231 // rotate rot_axis into ship reference frame
2232 vm_vec_rotate ( &local_rot_axis, &rot_axis, orient );
2234 // find theta to goal
2235 vm_vec_copy_scale(&theta_goal, &local_rot_axis, theta);
2236 SDL_assert ( fl_abs (theta_goal.xyz.z) < 0.001f ); // check for proper rotation
2238 theta_end = vmd_zero_vector;
2241 // find rotation about x
2242 if (theta_goal.xyz.x > 0) {
2243 if (w_in->xyz.x >= 0) {
2244 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);
2245 theta_end.xyz.x = delta_theta;
2246 } else { // w_in->xyz.x < 0
2247 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);
2248 theta_end.xyz.x = delta_theta;
2250 } else if (theta_goal.xyz.x < 0) {
2251 if (w_in->xyz.x <= 0) {
2252 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);
2253 theta_end.xyz.x = -delta_theta;
2254 w_out->xyz.x = -w_out->xyz.x;
2255 } else { // w_in->xyz.x > 0
2256 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);
2257 theta_end.xyz.x = -delta_theta;
2258 w_out->xyz.x = -w_out->xyz.x;
2260 } else { // theta_goal == 0
2261 if (w_in->xyz.x < 0) {
2262 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);
2263 theta_end.xyz.x = delta_theta;
2265 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);
2266 theta_end.xyz.x = -delta_theta;
2267 w_out->xyz.x = -w_out->xyz.x;
2271 // find rotation about y
2272 if (theta_goal.xyz.y > 0) {
2273 if (w_in->xyz.y >= 0) {
2274 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);
2275 theta_end.xyz.y = delta_theta;
2276 } else { // w_in->xyz.y < 0
2277 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);
2278 theta_end.xyz.y = delta_theta;
2280 } else if (theta_goal.xyz.y < 0) {
2281 if (w_in->xyz.y <= 0) {
2282 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);
2283 theta_end.xyz.y = -delta_theta;
2284 w_out->xyz.y = -w_out->xyz.y;
2285 } else { // w_in->xyz.y > 0
2286 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);
2287 theta_end.xyz.y = -delta_theta;
2288 w_out->xyz.y = -w_out->xyz.y;
2290 } else { // theta_goal == 0
2291 if (w_in->xyz.y < 0) {
2292 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);
2293 theta_end.xyz.y = delta_theta;
2295 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);
2296 theta_end.xyz.y = -delta_theta;
2297 w_out->xyz.y = -w_out->xyz.y;
2301 // FIND Z ROTATON MATRIX
2302 theta_end.xyz.z = 0.0f;
2303 rot_axis = theta_end;
2304 SDL_assert(is_valid_vec(&rot_axis));
2306 // normalize rotation axis and determine total rotation angle
2307 theta = vm_vec_mag(&rot_axis);
2308 if (theta < SMALL_NUM) {
2309 M_intermed = *orient;
2311 vm_vec_scale ( &rot_axis, 1/theta );
2312 vm_quaternion_rotate ( &Mtemp1, theta, &rot_axis );
2313 SDL_assert(is_valid_matrix(&Mtemp1));
2314 vm_matrix_x_matrix ( &M_intermed, orient, &Mtemp1 );
2315 SDL_assert(is_valid_matrix(&M_intermed));
2319 // FIND ROTATION ABOUT Z (IF ANY)
2320 // no rotation if delta_bank and w_in both 0 or rotational acc in forward is 0
2321 no_bank = ( acc_limit->xyz.z == 0.0f && vel_limit->xyz.z == 0.0f );
2323 if ( no_bank ) { // no rotation on z, so we're done (no rotation of w)
2324 *next_orient = M_intermed;
2325 vm_orthogonalize_matrix ( next_orient );
2328 // calculate delta_bank using orient->v.rvec, goal_orient->v.rvec
2330 vm_vec_crossprod ( &rot_axis, &orient->v.rvec, &goal_orient->v.rvec );
2332 t = vm_vec_mag(&rot_axis);
2336 r_dotprod = vm_vec_dotprod ( &orient->v.rvec, &goal_orient->v.rvec );
2338 if ( t < SMALLER_NUM ) {
2339 if ( r_dotprod > 0.0f )
2341 else { // the right vector is pointing exactly opposite of goal, so rotate 180 on z
2343 rot_axis = orient->v.fvec;
2346 theta = (float) asin ( t );
2347 vm_vec_scale ( &rot_axis, 1/t );
2348 if ( z_dotprod < 0.0f )
2352 // rotate rot_axis into ship reference frame
2353 vm_vec_rotate ( &local_rot_axis, &rot_axis, orient );
2355 // find theta.xyz.z to goal
2356 delta_bank = local_rot_axis.xyz.z * theta;
2357 SDL_assert( fl_abs (local_rot_axis.xyz.x) < 0.001f ); // check for proper rotation
2359 // end calculate delta_bank
2360 // find rotation about z
2361 if (delta_bank > 0) {
2362 if (w_in->xyz.z >= 0) {
2363 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);
2365 } else { // w_in->xyz.z < 0
2366 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);
2369 } else if (delta_bank < 0) {
2370 if (w_in->xyz.z <= 0) {
2371 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);
2372 bank = -delta_theta;
2373 w_out->xyz.z = -w_out->xyz.z;
2374 } else { // w_in->xyz.z > 0
2375 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);
2376 bank = -delta_theta;
2377 w_out->xyz.z = -w_out->xyz.z;
2379 } else { // theta_goal == 0
2380 if (w_in->xyz.z < 0) {
2381 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);
2384 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);
2385 bank = -delta_theta;
2386 w_out->xyz.z = -w_out->xyz.z;
2390 if ( fl_abs (bank) < SMALL_NUM )
2392 *next_orient = M_intermed;
2393 vm_orthogonalize_matrix ( next_orient );
2396 rotate_z ( &Mtemp1, bank );
2398 vm_vec_rotate ( w_out, &vtemp, &Mtemp1 );
2399 vm_matrix_x_matrix ( next_orient, &M_intermed, &Mtemp1 );
2400 SDL_assert(is_valid_matrix(next_orient));
2401 vm_orthogonalize_matrix ( next_orient );
2404 } // end vm_fvec_matrix_interpolate
2407 // ---------------------------------------------------------------------------------------------
2409 // inputs: goal_f => goal forward vector
2410 // orient => current orientation matrix (with current forward vector)
2411 // w_in => current input angular velocity
2412 // delta_t => time to move toward goal
2413 // delta_bank => desired change in bank in degrees
2414 // next_orient => the orientation matrix at time delta_t (with current forward vector)
2415 // NOTE: this does not include any rotation about z (bank)
2416 // w_out => the angular velocity of the ship at delta_t
2417 // vel_limit => maximum rotational speed
2418 // acc_limit => maximum rotational speed
2420 // function moves the forward vector toward the goal forward vector taking account of anglular
2421 // momentum (velocity) Attempt to try to move bank by goal delta_bank. Rotational velocity
2422 // on x/y is rotated with bank, giving smoother motion.
2423 void vm_forward_interpolate(vector *goal_f, matrix *orient, vector *w_in, float delta_t, float delta_bank,
2424 matrix *next_orient, vector *w_out, vector *vel_limit, vector *acc_limit, int no_overshoot)
2426 matrix Mtemp1; // temporary matrix
2427 vector local_rot_axis; // vector indicating direction of rotation axis (local coords)
2428 vector rot_axis; // vector indicating direction of rotation axis (world coords)
2429 vector theta_goal; // desired angular position at the end of the time interval
2430 vector theta_end; // actual angular position at the end of the time interval
2431 float theta; // magnitude of rotation about the rotation axis
2432 float bank; // magnitude of rotation about the forward axis
2433 int no_bank; // flag set if there is no bank for the object
2437 // FIND ROTATION NEEDED FOR GOAL
2438 // rotation vector is (current fvec) orient->v.fvec x goal_f
2439 // magnitude = asin ( magnitude of crossprod )
2440 vm_vec_crossprod( &rot_axis, &orient->v.fvec, goal_f );
2442 float t = vm_vec_mag(&rot_axis);
2446 z_dotprod = vm_vec_dotprod( &orient->v.fvec, goal_f );
2448 if ( t < SMALLER_NUM ) {
2449 if ( z_dotprod > 0.0f )
2451 else { // the forward vector is pointing exactly opposite of goal
2452 // arbitrarily choose the x axis to rotate around until t becomes large enough
2454 rot_axis = orient->v.rvec;
2457 theta = (float) asin( t );
2458 vm_vec_scale ( &rot_axis, 1/t );
2459 if ( z_dotprod < 0.0f )
2463 // rotate rot_axis into ship reference frame
2464 vm_vec_rotate( &local_rot_axis, &rot_axis, orient );
2466 // find theta to goal
2467 vm_vec_copy_scale(&theta_goal, &local_rot_axis, theta);
2468 SDL_assert(fl_abs(theta_goal.xyz.z) < 0.001f); // check for proper rotation
2470 theta_end = vmd_zero_vector;
2473 // find rotation about x
2474 if (theta_goal.xyz.x > 0) {
2475 if (w_in->xyz.x >= 0) {
2476 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);
2477 theta_end.xyz.x = delta_theta;
2478 } else { // w_in->xyz.x < 0
2479 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);
2480 theta_end.xyz.x = delta_theta;
2482 } else if (theta_goal.xyz.x < 0) {
2483 if (w_in->xyz.x <= 0) {
2484 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);
2485 theta_end.xyz.x = -delta_theta;
2486 w_out->xyz.x = -w_out->xyz.x;
2487 } else { // w_in->xyz.x > 0
2488 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);
2489 theta_end.xyz.x = -delta_theta;
2490 w_out->xyz.x = -w_out->xyz.x;
2492 } else { // theta_goal == 0
2493 if (w_in->xyz.x < 0) {
2494 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);
2495 theta_end.xyz.x = delta_theta;
2497 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);
2498 theta_end.xyz.x = -delta_theta;
2499 w_out->xyz.x = -w_out->xyz.x;
2503 // find rotation about y
2504 if (theta_goal.xyz.y > 0) {
2505 if (w_in->xyz.y >= 0) {
2506 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);
2507 theta_end.xyz.y = delta_theta;
2508 } else { // w_in->xyz.y < 0
2509 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);
2510 theta_end.xyz.y = delta_theta;
2512 } else if (theta_goal.xyz.y < 0) {
2513 if (w_in->xyz.y <= 0) {
2514 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);
2515 theta_end.xyz.y = -delta_theta;
2516 w_out->xyz.y = -w_out->xyz.y;
2517 } else { // w_in->xyz.y > 0
2518 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);
2519 theta_end.xyz.y = -delta_theta;
2520 w_out->xyz.y = -w_out->xyz.y;
2522 } else { // theta_goal == 0
2523 if (w_in->xyz.y < 0) {
2524 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);
2525 theta_end.xyz.y = delta_theta;
2527 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);
2528 theta_end.xyz.y = -delta_theta;
2529 w_out->xyz.y = -w_out->xyz.y;
2533 // no rotation if delta_bank and w_in both 0 or rotational acc in forward is 0
2534 no_bank = ( delta_bank == 0.0f && vel_limit->xyz.z == 0.0f && acc_limit->xyz.z == 0.0f );
2536 // do rotation about z
2539 // convert delta_bank to radians
2540 delta_bank *= (float) CONVERT_RADIANS;
2542 // find rotation about z
2543 if (delta_bank > 0) {
2544 if (w_in->xyz.z >= 0) {
2545 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);
2547 } else { // w_in->xyz.z < 0
2548 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);
2551 } else if (delta_bank < 0) {
2552 if (w_in->xyz.z <= 0) {
2553 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);
2554 bank = -delta_theta;
2555 w_out->xyz.z = -w_out->xyz.z;
2556 } else { // w_in->xyz.z > 0
2557 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);
2558 bank = -delta_theta;
2559 w_out->xyz.z = -w_out->xyz.z;
2561 } else { // theta_goal == 0
2562 if (w_in->xyz.z < 0) {
2563 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);
2566 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);
2567 bank = -delta_theta;
2568 w_out->xyz.z = -w_out->xyz.z;
2573 // the amount of rotation about each axis is determined in
2574 // functions approach and away. first find the magnitude
2575 // of the rotation and then normalize the axis (ship coords)
2576 theta_end.xyz.z = bank;
2577 rot_axis = theta_end;
2579 // normalize rotation axis and determine total rotation angle
2580 theta = vm_vec_mag(&rot_axis);
2581 if ( theta > SMALL_NUM )
2582 vm_vec_scale( &rot_axis, 1/theta );
2584 if ( theta < SMALL_NUM ) {
2585 *next_orient = *orient;
2588 vm_quaternion_rotate( &Mtemp1, theta, &rot_axis );
2589 vm_matrix_x_matrix( next_orient, orient, &Mtemp1 );
2590 SDL_assert(is_valid_matrix(next_orient));
2592 vm_vec_rotate( w_out, &vtemp, &Mtemp1 );
2594 } // end vm_forward_interpolate
2596 // ------------------------------------------------------------------------------------
2597 // vm_find_bounding_sphere()
2599 // Calculate a bounding sphere for a set of points.
2601 // input: pnts => array of world positions
2602 // num_pnts => number of points inside pnts array
2603 // center => OUTPUT PARAMETER: contains world pos of bounding sphere center
2604 // radius => OUTPUT PARAMETER: continas radius of bounding sphere
2606 #define BIGNUMBER 100000000.0f
2607 void vm_find_bounding_sphere(vector *pnts, int num_pnts, vector *center, float *radius)
2610 float rad, rad_sq, xspan, yspan, zspan, maxspan;
2611 float old_to_p, old_to_p_sq, old_to_new;
2612 vector diff, xmin, xmax, ymin, ymax, zmin, zmax, dia1, dia2, *p;
2614 xmin = vmd_zero_vector;
2615 ymin = vmd_zero_vector;
2616 zmin = vmd_zero_vector;
2617 xmax = vmd_zero_vector;
2618 ymax = vmd_zero_vector;
2619 zmax = vmd_zero_vector;
2620 xmin.xyz.x = ymin.xyz.y = zmin.xyz.z = BIGNUMBER;
2621 xmax.xyz.x = ymax.xyz.y = zmax.xyz.z = -BIGNUMBER;
2623 for ( i = 0; i < num_pnts; i++ ) {
2625 if ( p->xyz.x < xmin.xyz.x )
2627 if ( p->xyz.x > xmax.xyz.x )
2629 if ( p->xyz.y < ymin.xyz.y )
2631 if ( p->xyz.y > ymax.xyz.y )
2633 if ( p->xyz.z < zmin.xyz.z )
2635 if ( p->xyz.z > zmax.xyz.z )
2639 // find distance between two min and max points (squared)
2640 vm_vec_sub(&diff, &xmax, &xmin);
2641 xspan = vm_vec_mag_squared(&diff);
2643 vm_vec_sub(&diff, &ymax, &ymin);
2644 yspan = vm_vec_mag_squared(&diff);
2646 vm_vec_sub(&diff, &zmax, &zmin);
2647 zspan = vm_vec_mag_squared(&diff);
2652 if ( yspan > maxspan ) {
2657 if ( zspan > maxspan ) {
2662 // calc inital center
2663 vm_vec_add(center, &dia1, &dia2);
2664 vm_vec_scale(center, 0.5f);
2666 vm_vec_sub(&diff, &dia2, center);
2667 rad_sq = vm_vec_mag_squared(&diff);
2668 rad = fl_sqrt(rad_sq);
2669 SDL_assert( !isnan(rad) );
2672 for ( i = 0; i < num_pnts; i++ ) {
2674 vm_vec_sub(&diff, p, center);
2675 old_to_p_sq = vm_vec_mag_squared(&diff);
2676 if ( old_to_p_sq > rad_sq ) {
2677 old_to_p = fl_sqrt(old_to_p_sq);
2678 // calc radius of new sphere
2679 rad = (rad + old_to_p) / 2.0f;
2681 old_to_new = old_to_p - rad;
2682 // calc new center of sphere
2683 center->xyz.x = (rad*center->xyz.x + old_to_new*p->xyz.x) / old_to_p;
2684 center->xyz.y = (rad*center->xyz.y + old_to_new*p->xyz.y) / old_to_p;
2685 center->xyz.z = (rad*center->xyz.z + old_to_new*p->xyz.z) / old_to_p;
2686 nprintf(("Alan", "New sphere: cen,rad = %f %f %f %f\n", center->xyz.x, center->xyz.y, center->xyz.z, rad));
2693 // ----------------------------------------------------------------------------
2694 // vm_rotate_vec_to_body()
2696 // rotates a vector from world coordinates to body coordinates
2698 // inputs: body_vec => vector in body coordinates
2699 // world_vec => vector in world coordinates
2700 // orient => orientation matrix
2702 vector* vm_rotate_vec_to_body(vector *body_vec, vector *world_vec, matrix *orient)
2704 return vm_vec_unrotate(body_vec, world_vec, orient);
2708 // ----------------------------------------------------------------------------
2709 // vm_rotate_vec_to_world()
2711 // rotates a vector from body coordinates to world coordinates
2713 // inputs world_vec => vector in world coordinates
2714 // body_vec => vector in body coordinates
2715 // orient => orientation matrix
2717 vector* vm_rotate_vec_to_world(vector *world_vec, vector *body_vec, matrix *orient)
2719 return vm_vec_rotate(world_vec, body_vec, orient);
2723 // ----------------------------------------------------------------------------
2724 // vm_estimate_next_orientation()
2726 // given a last orientation and current orientation, estimate the next orientation
2728 // inputs: last_orient => last orientation matrix
2729 // current_orient => current orientation matrix
2730 // next_orient => next orientation matrix [the result]
2732 void vm_estimate_next_orientation(matrix *last_orient, matrix *current_orient, matrix *next_orient)
2734 // R L = C => R = C (L)^-1
2735 // N = R C => N = C (L)^-1 C
2739 vm_copy_transpose_matrix(&Mtemp, last_orient); // Mtemp = (L)^-1
2740 vm_matrix_x_matrix(&Rot_matrix, &Mtemp, current_orient); // R = C Mtemp1
2741 vm_matrix_x_matrix(next_orient, current_orient, &Rot_matrix);
2744 // Return true if all elements of *vec are legal, that is, not a NAN.
2745 int is_valid_vec(vector *vec)
2747 return !isnan(vec->xyz.x) && !isnan(vec->xyz.y) && !isnan(vec->xyz.z);
2750 // Return true if all elements of *m are legal, that is, not a NAN.
2751 int is_valid_matrix(matrix *m)
2753 return is_valid_vec(&m->v.fvec) && is_valid_vec(&m->v.uvec) && is_valid_vec(&m->v.rvec);
2756 // interpolate between 2 vectors. t goes from 0.0 to 1.0. at
2757 void vm_vec_interp_constant(vector *out, vector *v0, vector *v1, float t)
2762 // get the cross-product of the 2 vectors
2763 vm_vec_crossprod(&cross, v0, v1);
2764 vm_vec_normalize(&cross);
2766 // get the total angle between the 2 vectors
2767 total_ang = -(float)acos(vm_vec_dot(v0, v1));
2769 // rotate around the cross product vector by the appropriate angle
2770 vm_rot_point_around_line(out, v0, t * total_ang, &vmd_zero_vector, &cross);
2773 // randomly perturb a vector around a given (normalized vector) or optional orientation matrix
2774 void vm_vec_random_cone(vector *out, vector *in, float max_angle, matrix *orient)
2780 // get an orientation matrix
2784 vm_vector_2_matrix(&m, in, NULL, NULL);
2789 vm_rot_point_around_line(&t1, in, fl_radian(frand_range(-max_angle, max_angle)), &vmd_zero_vector, &rot->v.fvec);
2792 vm_rot_point_around_line(&t2, &t1, fl_radian(frand_range(-max_angle, max_angle)), &vmd_zero_vector, &rot->v.rvec);
2795 vm_rot_point_around_line(out, &t2, fl_radian(frand_range(-max_angle, max_angle)), &vmd_zero_vector, &rot->v.uvec);
2798 // given a start vector, an orientation and a radius, give a point on the plane of the circle
2799 // if on_edge is 1, the point is on the very edge of the circle
2800 void vm_vec_random_in_circle(vector *out, vector *in, matrix *orient, float radius, int on_edge)
2804 // point somewhere in the plane
2805 vm_vec_scale_add(&temp, in, &orient->v.rvec, on_edge ? radius : frand_range(0.0f, radius));
2807 // rotate to a random point on the circle
2808 vm_rot_point_around_line(out, &temp, fl_radian(frand_range(0.0f, 359.0f)), in, &orient->v.fvec);
2811 // find the nearest point on the line to p. if dist is non-NULL, it is filled in
2812 // returns 0 if the point is inside the line segment, -1 if "before" the line segment and 1 ir "after" the line segment
2813 int vm_vec_dist_to_line(vector *p, vector *l0, vector *l1, vector *nearest, float *dist)
2819 if(vm_vec_same(l0, l1)){
2820 *nearest = vmd_zero_vector;
2825 // compb_a == a dot b / len(b)
2826 vm_vec_sub(&a, p, l0);
2827 vm_vec_sub(&b, l1, l0);
2828 b_mag = vm_vec_copy_normalize(&c, &b);
2830 // calculate component
2831 comp = vm_vec_dotprod(&a, &b) / b_mag;
2834 vm_vec_scale_add(nearest, l0, &c, comp);
2836 // maybe get the distance
2838 *dist = vm_vec_dist(nearest, p);
2841 // return the proper value
2843 return -1; // before the line
2844 } else if(comp > b_mag){
2845 return 1; // after the line
2847 return 0; // on the line