2 * $Logfile: /Freespace2/code/Math/VecMat.cpp $
7 * C module containg functions for manipulating vectors and matricies
10 * Revision 1.1 2002/05/03 03:28:09 root
14 * 10 9/08/99 3:36p Jefff
15 * Make sure argument of sqrt is positive in approach.
17 * 9 6/22/99 1:51p Danw
18 * Some sanity for vm_vec_dist_to_line(...)
20 * 8 6/18/99 5:16p Dave
21 * Added real beam weapon lighting. Fixed beam weapon sounds. Added MOTD
22 * dialog to PXO screen.
24 * 7 4/28/99 11:13p Dave
25 * Temporary checkin of artillery code.
27 * 6 1/24/99 11:37p Dave
28 * First full rev of beam weapons. Very customizable. Removed some bogus
29 * Int3()'s in low level net code.
31 * 5 1/12/99 12:53a Dave
32 * More work on beam weapons - made collision detection very efficient -
33 * collide against all object types properly - made 3 movement types
34 * smooth. Put in test code to check for possible non-darkening pixels on
37 * 4 1/06/99 2:24p Dave
38 * Stubs and release build fixes.
40 * 3 11/18/98 4:10p Johnson
41 * Add assert in vm_interpolate_matrix
43 * 2 10/07/98 10:53a Dave
46 * 1 10/07/98 10:49a Dave
48 * 72 9/11/98 10:10a Andsager
49 * Optimize and rename matrix_decomp to vm_matrix_to_rot_axis_and_angle,
50 * rename quatern_rot to vm_quaternion_rotate
52 * 71 5/01/98 2:25p Andsager
53 * Fix bug in matrix interpolate (approach) when in rotvel is above limit.
55 * 70 4/07/98 3:10p Andsager
56 * Make vm_test_parallel based on absolute difference. Optimize matrix
57 * decomp. Fix potential bug in get_camera_limits with time = 0.
58 * Optimize vm_forward_interpolate.
60 * 69 4/06/98 8:54a Andsager
61 * Fix bug where matrix interpolate gets accel of 0
63 * 68 4/03/98 5:34p Andsager
64 * Optimized approach and away (used in matrix interpolation)
66 * 67 4/01/98 9:21p John
67 * Made NDEBUG, optimized build with no warnings or errors.
69 * 66 3/23/98 1:12p Andsager
70 * Reformat matrix inerpolation code
72 * 65 3/23/98 12:53p Andsager
74 * 63 3/09/98 3:51p Mike
75 * More error checking.
77 * 62 2/26/98 3:28p John
78 * Changed all sqrt's to use fl_sqrt. Took out isqrt function
80 * 61 2/02/98 5:12p Mike
81 * Make vm_vec_rand_vec_quick() detect potential null vector condition and
84 * 60 1/20/98 9:47a Mike
85 * Suppress optimized compiler warnings.
86 * Some secondary weapon work.
88 * 59 12/17/97 5:44p Andsager
89 * Change vm_matrix_interpolate so that it does not overshoot if optional
92 * 58 9/30/97 5:04p Andsager
93 * add vm_estimate_next_orientation
95 * 57 9/28/97 2:17p Andsager
96 * added vm_project_point_onto_plane
98 * 56 9/09/97 10:15p Andsager
99 * added vm_rotate_vec_to_body() and vm_rotate_vec_to_world()
101 * 55 8/20/97 5:33p Andsager
102 * added vm_vec_projection_parallel and vm_vec_projection_onto_surface
104 * 54 8/20/97 9:51a Lawrance
105 * swap x and y parameters in atan2_safe() to be consistent with library
108 * 53 8/20/97 9:40a Lawrance
109 * modified special case values in atan2_safe()
111 * 52 8/19/97 11:41p Lawrance
112 * use atan2_safe() instead of atan2()
114 * 51 8/18/97 4:46p Hoffoss
115 * Added global default axis vector constants.
117 * 50 8/03/97 3:54p Lawrance
118 * added vm_find_bounding_sphere()
120 * 49 7/28/97 3:40p Andsager
121 * remove duplicate vm_forwarad_interpolate
123 * 48 7/28/97 2:21p John
124 * changed vecmat functions to not return src. Started putting in code
125 * for inline vector math. Fixed some bugs with optimizer.
127 * 47 7/28/97 3:24p Andsager
129 * 46 7/28/97 2:41p Mike
130 * Replace vm_forward_interpolate().
132 * 45 7/28/97 1:18p Andsager
133 * implement vm_fvec_matrix_interpolate(), which interpolates matrices on
136 * 44 7/28/97 10:28a Mike
137 * Use forward_interpolate() to prevent weird banking behavior.
139 * Suppress a couple annoying mprints and clarify another.
141 * 43 7/24/97 5:24p Andsager
142 * implement forward vector interpolation
144 * 42 7/10/97 8:52a Andsager
145 * optimization and clarification of matrix_decomp()
147 * 41 7/09/97 2:54p Mike
148 * More matrix_decomp optimization.
150 * 40 7/09/97 2:52p Mike
151 * Optimize and error-prevent matrix_decomp().
153 * 39 7/09/97 12:05a Mike
154 * Error prevention in matrix_interpolate().
156 * 38 7/07/97 11:58p Lawrance
157 * add get_camera_limits()
159 * 37 7/03/97 11:22a Mike
160 * Fix bug in matrix_interpolate. Was doing result = goal instead of
163 * 36 7/03/97 9:27a Mike
164 * Hook in Dave's latest version of matrix_interpolate which doesn't jerk.
166 * 35 7/02/97 4:25p Mike
167 * Add matrix_interpolate(), but don't call it.
169 * 34 7/01/97 3:27p Mike
170 * Improve skill level support.
172 * 33 6/25/97 12:27p Hoffoss
173 * Added some functions I needed for Fred.
175 * 32 5/21/97 8:49a Lawrance
176 * added vm_vec_same()
178 * 31 4/15/97 4:00p Mike
179 * Intermediate checkin caused by getting other files. Working on camera
182 * 30 4/10/97 3:20p Mike
183 * Change hull damage to be like shields.
185 * 29 3/17/97 1:55p Hoffoss
186 * Added function for error checking matrices.
188 * 28 3/11/97 10:46p Mike
189 * Fix make_rand_vec_quick. Was generating values in -0.5..1.5 instead of
192 * 27 3/06/97 5:36p Mike
193 * Change vec_normalize_safe() back to vec_normalize().
194 * Spruce up docking a bit.
196 * 26 3/06/97 10:56a Mike
197 * Write error checking version of vm_vec_normalize().
198 * Fix resultant problems.
200 * 25 3/04/97 3:30p John
201 * added function to interpolate an angle.
203 * 24 2/26/97 10:32a John
204 * changed debris collision to use vm_vec_dist_squared. Changed
205 * vm_vec_dist_squared to not int3 on bad values.
207 * 23 2/25/97 5:54p Hoffoss
208 * Improved vector and matrix compare functions.
210 * 22 2/25/97 5:28p Hoffoss
211 * added some commented out test code.
213 * 21 2/25/97 5:12p John
214 * Added functions to see if two matrices or vectors are close.
224 #include "floating.h"
226 #define SMALL_NUM 1e-7
227 #define SMALLER_NUM 1e-20
228 #define CONVERT_RADIANS 0.017453 // conversion factor from degrees to radians
229 int index_largest (float a, float b, float c); // returns index of largest, NO_LARGEST if all less than SMALL_NUM
232 vector vmd_zero_vector = ZERO_VECTOR;
233 vector vmd_x_vector = { 1.0f, 0.0f, 0.0f };
234 vector vmd_y_vector = { 0.0f, 1.0f, 0.0f };
235 vector vmd_z_vector = { 0.0f, 0.0f, 1.0f };
236 matrix vmd_identity_matrix = IDENTITY_MATRIX;
238 #define UNINITIALIZED_VALUE -12345678.9f
240 // -----------------------------------------------------------
243 // Wrapper around atan2() that used atan() to calculate angle. Safe
244 // for optimized builds. Handles special cases when x == 0.
246 float atan2_safe(float y, float x)
250 // special case, x == 0
262 ang = (float)atan(y/x);
270 // ---------------------------------------------------------------------
271 // vm_vec_component()
273 // finds projection of a vector along a unit (normalized) vector
275 float vm_vec_projection_parallel(vector *component, vector *src, vector *unit_vec)
278 Assert( vm_vec_mag(unit_vec) > 0.999f && vm_vec_mag(unit_vec) < 1.001f );
280 mag = vm_vec_dotprod(src, unit_vec);
281 vm_vec_copy_scale(component, unit_vec, mag);
285 // ---------------------------------------------------------------------
286 // vm_vec_projection_onto_plane()
288 // finds projection of a vector onto a plane specified by a unit normal vector
290 void vm_vec_projection_onto_plane(vector *projection, vector *src, vector *unit_normal)
293 Assert( vm_vec_mag(unit_normal) > 0.999f && vm_vec_mag(unit_normal) < 1.001f );
295 mag = vm_vec_dotprod(src, unit_normal);
297 vm_vec_scale_add2(projection, unit_normal, -mag);
300 // ---------------------------------------------------------------------
301 // vm_vec_project_point_onto_plane()
303 // finds the point on a plane closest to a given point
304 // moves the point in the direction of the plane normal until it is on the plane
306 void vm_project_point_onto_plane(vector *new_point, vector *point, vector *plane_normal, vector *plane_point)
308 float D; // plane constant in Ax+By+Cz+D = 0 or dot(X,n) - dot(Xp,n) = 0, so D = -dot(Xp,n)
310 Assert( vm_vec_mag(plane_normal) > 0.999f && vm_vec_mag(plane_normal) < 1.001f );
312 D = -vm_vec_dotprod(plane_point, plane_normal);
313 dist = vm_vec_dotprod(point, plane_normal) + D;
316 vm_vec_scale_add2(new_point, plane_normal, -dist);
319 // Take abs(x), then sqrt. Could insert warning message if desired.
328 void vm_set_identity(matrix *m)
330 m->rvec.x = 1.0f; m->rvec.y = 0.0f; m->rvec.z = 0.0f;
331 m->uvec.x = 0.0f; m->uvec.y = 1.0f; m->uvec.z = 0.0f;
332 m->fvec.x = 0.0f; m->fvec.y = 0.0f; m->fvec.z = 1.0f;
335 //adds two vectors, fills in dest, returns ptr to dest
336 //ok for dest to equal either source, but should use vm_vec_add2() if so
337 #ifndef _INLINE_VECMAT
338 void vm_vec_add(vector *dest,vector *src0,vector *src1)
340 dest->x = src0->x + src1->x;
341 dest->y = src0->y + src1->y;
342 dest->z = src0->z + src1->z;
346 //subs two vectors, fills in dest, returns ptr to dest
347 //ok for dest to equal either source, but should use vm_vec_sub2() if so
348 #ifndef _INLINE_VECMAT
349 void vm_vec_sub(vector *dest,vector *src0,vector *src1)
351 dest->x = src0->x - src1->x;
352 dest->y = src0->y - src1->y;
353 dest->z = src0->z - src1->z;
358 //adds one vector to another. returns ptr to dest
359 //dest can equal source
360 #ifndef _INLINE_VECMAT
361 void vm_vec_add2(vector *dest,vector *src)
369 //subs one vector from another, returns ptr to dest
370 //dest can equal source
371 #ifndef _INLINE_VECMAT
372 void vm_vec_sub2(vector *dest,vector *src)
380 //averages two vectors. returns ptr to dest
381 //dest can equal either source
382 vector *vm_vec_avg(vector *dest,vector *src0,vector *src1)
384 dest->x = (src0->x + src1->x) * 0.5f;
385 dest->y = (src0->y + src1->y) * 0.5f;
386 dest->z = (src0->z + src1->z) * 0.5f;
392 //averages four vectors. returns ptr to dest
393 //dest can equal any source
394 vector *vm_vec_avg4(vector *dest,vector *src0,vector *src1,vector *src2,vector *src3)
396 dest->x = (src0->x + src1->x + src2->x + src3->x) * 0.25f;
397 dest->y = (src0->y + src1->y + src2->y + src3->y) * 0.25f;
398 dest->z = (src0->z + src1->z + src2->z + src3->z) * 0.25f;
403 //scales a vector in place. returns ptr to vector
404 #ifndef _INLINE_VECMAT
405 void vm_vec_scale(vector *dest,float s)
407 dest->x = dest->x * s;
408 dest->y = dest->y * s;
409 dest->z = dest->z * s;
414 //scales and copies a vector. returns ptr to dest
415 #ifndef _INLINE_VECMAT
416 void vm_vec_copy_scale(vector *dest,vector *src,float s)
424 //scales a vector, adds it to another, and stores in a 3rd vector
425 //dest = src1 + k * src2
426 #ifndef _INLINE_VECMAT
427 void vm_vec_scale_add(vector *dest,vector *src1,vector *src2,float k)
429 dest->x = src1->x + src2->x*k;
430 dest->y = src1->y + src2->y*k;
431 dest->z = src1->z + src2->z*k;
435 //scales a vector and adds it to another
437 #ifndef _INLINE_VECMAT
438 void vm_vec_scale_add2(vector *dest,vector *src,float k)
446 //scales a vector and adds it to another
448 #ifndef _INLINE_VECMAT
449 void vm_vec_scale_sub2(vector *dest,vector *src,float k)
457 //scales a vector in place, taking n/d for scale. returns ptr to vector
459 #ifndef _INLINE_VECMAT
460 void vm_vec_scale2(vector *dest,float n,float d)
464 dest->x = dest->x* n * d;
465 dest->y = dest->y* n * d;
466 dest->z = dest->z* n * d;
470 //returns dot product of 2 vectors
471 #ifndef _INLINE_VECMAT
472 float vm_vec_dotprod(vector *v0,vector *v1)
474 return (v1->x*v0->x)+(v1->y*v0->y)+(v1->z*v0->z);
479 //returns dot product of <x,y,z> and vector
480 #ifndef _INLINE_VECMAT
481 float vm_vec_dot3(float x,float y,float z,vector *v)
483 return (x*v->x)+(y*v->y)+(z*v->z);
487 //returns magnitude of a vector
488 float vm_vec_mag(vector *v)
490 float x,y,z,mag1, mag2;
497 mag2 = fl_sqrt(mag1);
503 //returns squared magnitude of a vector, useful if you want to compare distances
504 float vm_vec_mag_squared(vector *v)
514 float vm_vec_dist_squared(vector *v0, vector *v1)
521 return dx*dx + dy*dy + dz*dz;
524 //computes the distance between two points. (does sub and mag)
525 float vm_vec_dist(vector *v0,vector *v1)
530 vm_vec_sub(&t,v0,v1);
539 //computes an approximation of the magnitude of the vector
540 //uses dist = largest + next_largest*3/8 + smallest*3/16
541 float vm_vec_mag_quick(vector *v)
572 bc = (b * 0.25f) + (c * 0.125f);
574 t = a + bc + (bc * 0.5f);
579 //computes an approximation of the distance between two points.
580 //uses dist = largest + next_largest*3/8 + smallest*3/16
581 float vm_vec_dist_quick(vector *v0,vector *v1)
585 vm_vec_sub(&t,v0,v1);
587 return vm_vec_mag_quick(&t);
590 //normalize a vector. returns mag of source vec
591 float vm_vec_copy_normalize(vector *dest,vector *src)
597 // Mainly here to trap attempts to normalize a null vector.
599 Warning(LOCATION, "Null vector in vector normalize.\n"
600 "Trace out of vecmat.cpp and find offending code.\n");
610 dest->x = src->x * im;
611 dest->y = src->y * im;
612 dest->z = src->z * im;
617 //normalize a vector. returns mag of source vec
618 float vm_vec_normalize(vector *v)
621 t = vm_vec_copy_normalize(v,v);
625 // Normalize a vector.
626 // If vector is 0,0,0, return 1,0,0.
627 // Don't generate a Warning().
628 // returns mag of source vec
629 float vm_vec_normalize_safe(vector *v)
635 // Mainly here to trap attempts to normalize a null vector.
654 //returns approximation of 1/magnitude of a vector
655 float vm_vec_imag(vector *v)
657 // return 1.0f / sqrt( (v->x*v->x)+(v->y*v->y)+(v->z*v->z) );
658 return fl_isqrt( (v->x*v->x)+(v->y*v->y)+(v->z*v->z) );
661 //normalize a vector. returns 1/mag of source vec. uses approx 1/mag
662 float vm_vec_copy_normalize_quick(vector *dest,vector *src)
664 // return vm_vec_copy_normalize(dest, src);
667 im = vm_vec_imag(src);
678 //normalize a vector. returns mag of source vec. uses approx mag
679 float vm_vec_normalize_quick(vector *src)
681 // return vm_vec_normalize(src);
685 im = vm_vec_imag(src);
697 //normalize a vector. returns mag of source vec. uses approx mag
698 float vm_vec_copy_normalize_quick_mag(vector *dest,vector *src)
700 // return vm_vec_copy_normalize(dest, src);
704 m = vm_vec_mag_quick(src);
710 dest->x = src->x * im;
711 dest->y = src->y * im;
712 dest->z = src->z * im;
718 //normalize a vector. returns mag of source vec. uses approx mag
719 float vm_vec_normalize_quick_mag(vector *v)
721 // return vm_vec_normalize(v);
724 m = vm_vec_mag_quick(v);
738 //return the normalized direction vector between two points
739 //dest = normalized(end - start). Returns mag of direction vector
740 //NOTE: the order of the parameters matches the vector subtraction
741 float vm_vec_normalized_dir(vector *dest,vector *end,vector *start)
745 vm_vec_sub(dest,end,start);
746 t = vm_vec_normalize(dest);
750 //return the normalized direction vector between two points
751 //dest = normalized(end - start). Returns mag of direction vector
752 //NOTE: the order of the parameters matches the vector subtraction
753 float vm_vec_normalized_dir_quick(vector *dest,vector *end,vector *start)
755 vm_vec_sub(dest,end,start);
757 return vm_vec_normalize_quick(dest);
760 //return the normalized direction vector between two points
761 //dest = normalized(end - start). Returns mag of direction vector
762 //NOTE: the order of the parameters matches the vector subtraction
763 float vm_vec_normalized_dir_quick_mag(vector *dest,vector *end,vector *start)
766 vm_vec_sub(dest,end,start);
768 t = vm_vec_normalize_quick_mag(dest);
772 //computes surface normal from three points. result is normalized
773 //returns ptr to dest
774 //dest CANNOT equal either source
775 vector *vm_vec_normal(vector *dest,vector *p0,vector *p1,vector *p2)
777 vm_vec_perp(dest,p0,p1,p2);
779 vm_vec_normalize(dest);
785 //computes cross product of two vectors.
786 //Note: this magnitude of the resultant vector is the
787 //product of the magnitudes of the two source vectors. This means it is
788 //quite easy for this routine to overflow and underflow. Be careful that
789 //your inputs are ok.
790 vector *vm_vec_crossprod(vector *dest,vector *src0,vector *src1)
792 dest->x = (src0->y * src1->z) - (src0->z * src1->y);
793 dest->y = (src0->z * src1->x) - (src0->x * src1->z);
794 dest->z = (src0->x * src1->y) - (src0->y * src1->x);
799 // test if 2 vectors are parallel or not.
800 int vm_test_parallel(vector *src0, vector *src1)
802 if ( (fl_abs(src0->x - src1->x) < 1e-4) && (fl_abs(src0->y - src1->y) < 1e-4) && (fl_abs(src0->z - src1->z) < 1e-4) ) {
809 //computes non-normalized surface normal from three points.
810 //returns ptr to dest
811 //dest CANNOT equal either source
812 vector *vm_vec_perp(vector *dest,vector *p0,vector *p1,vector *p2)
816 vm_vec_sub(&t0,p1,p0);
817 vm_vec_sub(&t1,p2,p1);
819 return vm_vec_crossprod(dest,&t0,&t1);
823 //computes the delta angle between two vectors.
824 //vectors need not be normalized. if they are, call vm_vec_delta_ang_norm()
825 //the forward vector (third parameter) can be NULL, in which case the absolute
826 //value of the angle in returned. Otherwise the angle around that vector is
828 float vm_vec_delta_ang(vector *v0,vector *v1,vector *fvec)
833 vm_vec_copy_normalize(&t0,v0);
834 vm_vec_copy_normalize(&t1,v1);
835 vm_vec_copy_normalize(&t2,fvec);
837 t = vm_vec_delta_ang_norm(&t0,&t1,&t2);
842 //computes the delta angle between two normalized vectors.
843 float vm_vec_delta_ang_norm(vector *v0,vector *v1,vector *fvec)
848 a = (float)acos(vm_vec_dot(v0,v1));
851 vm_vec_cross(&t,v0,v1);
852 if ( vm_vec_dotprod(&t,fvec) < 0.0 ) {
860 matrix *sincos_2_matrix(matrix *m,float sinp,float cosp,float sinb,float cosb,float sinh,float cosh)
862 float sbsh,cbch,cbsh,sbch;
870 m->rvec.x = cbch + sinp*sbsh; //m1
871 m->uvec.z = sbsh + sinp*cbch; //m8
873 m->uvec.x = sinp*cbsh - sbch; //m2
874 m->rvec.z = sinp*sbch - cbsh; //m7
876 m->fvec.x = sinh*cosp; //m3
877 m->rvec.y = sinb*cosp; //m4
878 m->uvec.y = cosb*cosp; //m5
879 m->fvec.z = cosh*cosp; //m9
881 m->fvec.y = -sinp; //m6
888 //computes a matrix from a set of three angles. returns ptr to matrix
889 matrix *vm_angles_2_matrix(matrix *m,angles *a)
892 float sinp,cosp,sinb,cosb,sinh,cosh;
894 sinp = (float)sin(a->p); cosp = (float)cos(a->p);
895 sinb = (float)sin(a->b); cosb = (float)cos(a->b);
896 sinh = (float)sin(a->h); cosh = (float)cos(a->h);
898 t = sincos_2_matrix(m,sinp,cosp,sinb,cosb,sinh,cosh);
903 //computes a matrix from one angle.
904 // angle_index = 0,1,2 for p,b,h
905 matrix *vm_angle_2_matrix(matrix *m, float a, int angle_index)
908 float sinp,cosp,sinb,cosb,sinh,cosh;
910 sinp = (float)sin(0.0f); cosp = (float)cos(0.0f);
911 sinb = (float)sin(0.0f); cosb = (float)cos(0.0f);
912 sinh = (float)sin(0.0f); cosh = (float)cos(0.0f);
914 switch (angle_index) {
916 sinp = (float)sin(a); cosp = (float)cos(a);
919 sinb = (float)sin(a); cosb = (float)cos(a);
922 sinh = (float)sin(a); cosh = (float)cos(a);
926 t = sincos_2_matrix(m,sinp,cosp,sinb,cosb,sinh,cosh);
932 //computes a matrix from a forward vector and an angle
933 matrix *vm_vec_ang_2_matrix(matrix *m,vector *v,float a)
936 float sinb,cosb,sinp,cosp,sinh,cosh;
938 sinb = (float)sin(a); cosb = (float)cos(a);
941 cosp = fl_sqrt(1.0 - sinp*sinp);
946 t = sincos_2_matrix(m,sinp,cosp,sinb,cosb,sinh,cosh);
952 //computes a matrix from one or more vectors. The forward vector is required,
953 //with the other two being optional. If both up & right vectors are passed,
954 //the up vector is used. If only the forward vector is passed, a bank of
956 //returns ptr to matrix
957 matrix *vm_vector_2_matrix(matrix *m,vector *fvec,vector *uvec,vector *rvec)
959 vector *xvec=&m->rvec,*yvec=&m->uvec,*zvec=&m->fvec;
962 Assert(fvec != NULL);
964 // This had been commented out, but that's bogus. Code below relies on a valid zvec.
965 if (vm_vec_copy_normalize(zvec,fvec) == 0.0) {
972 if (rvec == NULL) { //just forward vec
977 if ((zvec->x==0.0) && (zvec->z==0.0)) { //forward vec is straight up or down
979 m->rvec.x = (float)1.0;
980 m->uvec.z = (zvec->y<0.0)?(float)1.0:(float)-1.0;
982 m->rvec.y = m->rvec.z = m->uvec.x = m->uvec.y = (float)0.0;
984 else { //not straight up or down
987 xvec->y = (float)0.0;
990 vm_vec_normalize(xvec);
992 vm_vec_crossprod(yvec,zvec,xvec);
997 else { //use right vec
999 if (vm_vec_copy_normalize(xvec,rvec) == 0.0)
1002 vm_vec_crossprod(yvec,zvec,xvec);
1004 //normalize new perpendicular vector
1005 if (vm_vec_normalize(yvec) == 0.0)
1008 //now recompute right vector, in case it wasn't entirely perpendiclar
1009 vm_vec_crossprod(xvec,yvec,zvec);
1015 if (vm_vec_copy_normalize(yvec,uvec) == 0.0f)
1018 vm_vec_crossprod(xvec,yvec,zvec);
1020 //normalize new perpendicular vector
1021 if (vm_vec_normalize(xvec) == 0.0)
1024 //now recompute up vector, in case it wasn't entirely perpendiclar
1025 vm_vec_crossprod(yvec,zvec,xvec);
1031 //quicker version of vm_vector_2_matrix() that takes normalized vectors
1032 matrix *vm_vector_2_matrix_norm(matrix *m,vector *fvec,vector *uvec,vector *rvec)
1034 vector *xvec=&m->rvec,*yvec=&m->uvec,*zvec=&m->fvec;
1037 Assert(fvec != NULL);
1043 if (rvec == NULL) { //just forward vec
1048 if ((zvec->x==0.0) && (zvec->z==0.0)) { //forward vec is straight up or down
1050 m->rvec.x = (float)1.0;
1051 m->uvec.z = (zvec->y<0.0)?(float)1.0:(float)-1.0;
1053 m->rvec.y = m->rvec.z = m->uvec.x = m->uvec.y = (float)0.0;
1055 else { //not straight up or down
1058 xvec->y = (float)0.0;
1061 vm_vec_normalize(xvec);
1063 vm_vec_crossprod(yvec,zvec,xvec);
1068 else { //use right vec
1070 vm_vec_crossprod(yvec,zvec,xvec);
1072 //normalize new perpendicular vector
1073 if (vm_vec_normalize(yvec) == 0.0)
1076 //now recompute right vector, in case it wasn't entirely perpendiclar
1077 vm_vec_crossprod(xvec,yvec,zvec);
1083 vm_vec_crossprod(xvec,yvec,zvec);
1085 //normalize new perpendicular vector
1086 if (vm_vec_normalize(xvec) == 0.0)
1089 //now recompute up vector, in case it wasn't entirely perpendiclar
1090 vm_vec_crossprod(yvec,zvec,xvec);
1099 //rotates a vector through a matrix. returns ptr to dest vector
1100 //dest CANNOT equal source
1101 vector *vm_vec_rotate(vector *dest,vector *src,matrix *m)
1103 dest->x = (src->x*m->rvec.x)+(src->y*m->rvec.y)+(src->z*m->rvec.z);
1104 dest->y = (src->x*m->uvec.x)+(src->y*m->uvec.y)+(src->z*m->uvec.z);
1105 dest->z = (src->x*m->fvec.x)+(src->y*m->fvec.y)+(src->z*m->fvec.z);
1110 //rotates a vector through the transpose of the given matrix.
1111 //returns ptr to dest vector
1112 //dest CANNOT equal source
1113 // This is a faster replacement for this common code sequence:
1114 // vm_copy_transpose_matrix(&tempm,src_matrix);
1115 // vm_vec_rotate(dst_vec,src_vect,&tempm);
1117 // vm_vec_unrotate(dst_vec,src_vect, src_matrix)
1119 // THIS DOES NOT ACTUALLY TRANSPOSE THE SOURCE MATRIX!!! So if
1120 // you need it transposed later on, you should use the
1121 // vm_vec_transpose() / vm_vec_rotate() technique.
1123 vector *vm_vec_unrotate(vector *dest,vector *src,matrix *m)
1125 dest->x = (src->x*m->rvec.x)+(src->y*m->uvec.x)+(src->z*m->fvec.x);
1126 dest->y = (src->x*m->rvec.y)+(src->y*m->uvec.y)+(src->z*m->fvec.y);
1127 dest->z = (src->x*m->rvec.z)+(src->y*m->uvec.z)+(src->z*m->fvec.z);
1132 //transpose a matrix in place. returns ptr to matrix
1133 matrix *vm_transpose_matrix(matrix *m)
1137 t = m->uvec.x; m->uvec.x = m->rvec.y; m->rvec.y = t;
1138 t = m->fvec.x; m->fvec.x = m->rvec.z; m->rvec.z = t;
1139 t = m->fvec.y; m->fvec.y = m->uvec.z; m->uvec.z = t;
1144 //copy and transpose a matrix. returns ptr to matrix
1145 //dest CANNOT equal source. use vm_transpose_matrix() if this is the case
1146 matrix *vm_copy_transpose_matrix(matrix *dest,matrix *src)
1149 Assert(dest != src);
1151 dest->rvec.x = src->rvec.x;
1152 dest->rvec.y = src->uvec.x;
1153 dest->rvec.z = src->fvec.x;
1155 dest->uvec.x = src->rvec.y;
1156 dest->uvec.y = src->uvec.y;
1157 dest->uvec.z = src->fvec.y;
1159 dest->fvec.x = src->rvec.z;
1160 dest->fvec.y = src->uvec.z;
1161 dest->fvec.z = src->fvec.z;
1167 //mulitply 2 matrices, fill in dest. returns ptr to dest
1168 //dest CANNOT equal either source
1169 matrix *vm_matrix_x_matrix(matrix *dest,matrix *src0,matrix *src1)
1172 Assert(dest!=src0 && dest!=src1);
1174 dest->rvec.x = vm_vec_dot3(src0->rvec.x,src0->uvec.x,src0->fvec.x, &src1->rvec);
1175 dest->uvec.x = vm_vec_dot3(src0->rvec.x,src0->uvec.x,src0->fvec.x, &src1->uvec);
1176 dest->fvec.x = vm_vec_dot3(src0->rvec.x,src0->uvec.x,src0->fvec.x, &src1->fvec);
1178 dest->rvec.y = vm_vec_dot3(src0->rvec.y,src0->uvec.y,src0->fvec.y, &src1->rvec);
1179 dest->uvec.y = vm_vec_dot3(src0->rvec.y,src0->uvec.y,src0->fvec.y, &src1->uvec);
1180 dest->fvec.y = vm_vec_dot3(src0->rvec.y,src0->uvec.y,src0->fvec.y, &src1->fvec);
1182 dest->rvec.z = vm_vec_dot3(src0->rvec.z,src0->uvec.z,src0->fvec.z, &src1->rvec);
1183 dest->uvec.z = vm_vec_dot3(src0->rvec.z,src0->uvec.z,src0->fvec.z, &src1->uvec);
1184 dest->fvec.z = vm_vec_dot3(src0->rvec.z,src0->uvec.z,src0->fvec.z, &src1->fvec);
1191 //extract angles from a matrix
1192 angles *vm_extract_angles_matrix(angles *a,matrix *m)
1194 float sinh,cosh,cosp;
1196 if (m->fvec.x==0.0 && m->fvec.z==0.0) //zero head
1199 // a->h = (float)atan2(m->fvec.z,m->fvec.x);
1200 a->h = (float)atan2_safe(m->fvec.x,m->fvec.z);
1202 sinh = (float)sin(a->h); cosh = (float)cos(a->h);
1204 if (fl_abs(sinh) > fl_abs(cosh)) //sine is larger, so use it
1205 cosp = m->fvec.x*sinh;
1206 else //cosine is larger, so use it
1207 cosp = m->fvec.z*cosh;
1209 if (cosp==0.0 && m->fvec.y==0.0)
1212 // a->p = (float)atan2(cosp,-m->fvec.y);
1213 a->p = (float)atan2_safe(-m->fvec.y, cosp);
1216 if (cosp == 0.0) //the cosine of pitch is zero. we're pitched straight up. say no bank
1223 sinb = m->rvec.y/cosp;
1224 cosb = m->uvec.y/cosp;
1226 if (sinb==0.0 && cosb==0.0)
1229 // a->b = (float)atan2(cosb,sinb);
1230 a->b = (float)atan2_safe(sinb,cosb);
1238 //extract heading and pitch from a vector, assuming bank==0
1239 angles *vm_extract_angles_vector_normalized(angles *a,vector *v)
1242 a->b = 0.0f; //always zero bank
1244 a->p = (float)asin(-v->y);
1246 if (v->x==0.0f && v->z==0.0f)
1249 a->h = (float)atan2_safe(v->z,v->x);
1254 //extract heading and pitch from a vector, assuming bank==0
1255 angles *vm_extract_angles_vector(angles *a,vector *v)
1259 if (vm_vec_copy_normalize(&t,v) != 0.0)
1260 vm_extract_angles_vector_normalized(a,&t);
1265 //compute the distance from a point to a plane. takes the normalized normal
1266 //of the plane (ebx), a point on the plane (edi), and the point to check (esi).
1267 //returns distance in eax
1268 //distance is signed, so negative dist is on the back of the plane
1269 float vm_dist_to_plane(vector *checkp,vector *norm,vector *planep)
1274 vm_vec_sub(&t,checkp,planep);
1276 t1 = vm_vec_dot(&t,norm);
1282 // Given mouse movement in dx, dy, returns a 3x3 rotation matrix in RotMat.
1283 // Taken from Graphics Gems III, page 51, "The Rolling Ball"
1285 //if ( (Mouse.dx!=0) || (Mouse.dy!=0) ) {
1286 // GetMouseRotation( Mouse.dx, Mouse.dy, &MouseRotMat );
1287 // vm_matrix_x_matrix(&tempm,&LargeView.ev_matrix,&MouseRotMat);
1288 // LargeView.ev_matrix = tempm;
1292 void vm_trackball( int idx, int idy, matrix * RotMat )
1294 float dr, cos_theta, sin_theta, denom, cos_theta1;
1295 float Radius = 100.0f;
1301 dx = (float)idx; dy = (float)idy;
1303 dr = fl_sqrt(dx*dx+dy*dy);
1305 denom = fl_sqrt(Radius*Radius+dr*dr);
1307 cos_theta = Radius/denom;
1308 sin_theta = dr/denom;
1310 cos_theta1 = 1.0f - cos_theta;
1315 RotMat->rvec.x = cos_theta + (dydr*dydr)*cos_theta1;
1316 RotMat->uvec.x = - ((dxdr*dydr)*cos_theta1);
1317 RotMat->fvec.x = (dxdr*sin_theta);
1319 RotMat->rvec.y = RotMat->uvec.x;
1320 RotMat->uvec.y = cos_theta + ((dxdr*dxdr)*cos_theta1);
1321 RotMat->fvec.y = (dydr*sin_theta);
1323 RotMat->rvec.z = -RotMat->fvec.x;
1324 RotMat->uvec.z = -RotMat->fvec.y;
1325 RotMat->fvec.z = cos_theta;
1328 // Compute the outer product of A = A * transpose(A). 1x3 vector becomes 3x3 matrix.
1329 void vm_vec_outer_product(matrix *mat, vector *vec)
1331 mat->rvec.x = vec->x * vec->x;
1332 mat->rvec.y = vec->x * vec->y;
1333 mat->rvec.z = vec->x * vec->z;
1335 mat->uvec.x = vec->y * vec->x;
1336 mat->uvec.y = vec->y * vec->y;
1337 mat->uvec.z = vec->y * vec->z;
1339 mat->fvec.x = vec->z * vec->x;
1340 mat->fvec.y = vec->z * vec->y;
1341 mat->fvec.z = vec->z * vec->z;
1344 // Find the point on the line between p0 and p1 that is nearest to int_pnt.
1345 // Stuff result in nearest_point.
1346 // Uses algorithm from page 148 of Strang, Linear Algebra and Its Applications.
1347 // Returns value indicating whether *nearest_point is between *p0 and *p1.
1348 // 0.0f means *nearest_point is *p0, 1.0f means it's *p1. 2.0f means it's beyond p1 by 2x.
1349 // -1.0f means it's "before" *p0 by 1x.
1350 float find_nearest_point_on_line(vector *nearest_point, vector *p0, vector *p1, vector *int_pnt)
1352 vector norm, xlated_int_pnt, projected_point;
1356 vm_vec_sub(&norm, p1, p0);
1357 vm_vec_sub(&xlated_int_pnt, int_pnt, p0);
1359 if (IS_VEC_NULL(&norm)) {
1360 *nearest_point = *int_pnt;
1364 mag = vm_vec_normalize(&norm); // Normalize vector so we don't have to divide by dot product.
1367 *nearest_point = *int_pnt;
1369 // Warning(LOCATION, "Very small magnitude in find_nearest_point_on_line.\n");
1372 vm_vec_outer_product(&mat, &norm);
1374 vm_vec_rotate(&projected_point, &xlated_int_pnt, &mat);
1375 vm_vec_add(nearest_point, &projected_point, p0);
1377 dot = vm_vec_dot(&norm, &projected_point);
1382 //make sure matrix is orthogonal
1383 //computes a matrix from one or more vectors. The forward vector is required,
1384 //with the other two being optional. If both up & right vectors are passed,
1385 //the up vector is used. If only the forward vector is passed, a bank of
1387 //returns ptr to matrix
1388 void vm_orthogonalize_matrix(matrix *m_src)
1392 matrix * m = &tempm;
1394 if (vm_vec_copy_normalize(&m->fvec,&m_src->fvec) == 0.0f) {
1395 Error( LOCATION, "forward vec should not be zero-length" );
1398 umag = vm_vec_mag(&m_src->uvec);
1399 rmag = vm_vec_mag(&m_src->rvec);
1400 if (umag <= 0.0f) { // no up vector to use..
1401 if (rmag <= 0.0f) { // no right vector either, so make something up
1402 if (!m->fvec.x && !m->fvec.z && m->fvec.y) // vertical vector
1403 vm_vec_make(&m->uvec, 0.0f, 0.0f, 1.0f);
1405 vm_vec_make(&m->uvec, 0.0f, 1.0f, 0.0f);
1407 } else { // use the right vector to figure up vector
1408 vm_vec_crossprod(&m->uvec, &m->fvec, &m_src->rvec);
1409 if (vm_vec_normalize(&m->uvec) == 0.0f)
1410 Error( LOCATION, "Bad vector!" );
1413 } else { // use source up vector
1414 vm_vec_copy_normalize(&m->uvec, &m_src->uvec);
1417 // use forward and up vectors as good vectors to calculate right vector
1418 vm_vec_crossprod(&m->rvec, &m->uvec, &m->fvec);
1420 //normalize new perpendicular vector
1421 if (vm_vec_normalize(&m->rvec) == 0.0f)
1422 Error( LOCATION, "Bad vector!" );
1424 //now recompute up vector, in case it wasn't entirely perpendiclar
1425 vm_vec_crossprod(&m->uvec, &m->fvec, &m->rvec);
1429 // like vm_orthogonalize_matrix(), except that zero vectors can exist within the
1430 // matrix without causing problems. Valid vectors will be created where needed.
1431 void vm_fix_matrix(matrix *m)
1433 float fmag, umag, rmag;
1435 fmag = vm_vec_mag(&m->fvec);
1436 umag = vm_vec_mag(&m->uvec);
1437 rmag = vm_vec_mag(&m->rvec);
1439 if ((umag > 0.0f) && (rmag > 0.0f) && !vm_test_parallel(&m->uvec, &m->rvec)) {
1440 vm_vec_crossprod(&m->fvec, &m->uvec, &m->rvec);
1441 vm_vec_normalize(&m->fvec);
1443 } else if (umag > 0.0f) {
1444 if (!m->uvec.x && !m->uvec.y && m->uvec.z) // z vector
1445 vm_vec_make(&m->fvec, 1.0f, 0.0f, 0.0f);
1447 vm_vec_make(&m->fvec, 0.0f, 0.0f, 1.0f);
1451 vm_vec_normalize(&m->fvec);
1453 // we now have a valid and normalized forward vector
1455 if ((umag <= 0.0f) || vm_test_parallel(&m->fvec, &m->uvec)) { // no up vector to use..
1456 if ((rmag <= 0.0f) || vm_test_parallel(&m->fvec, &m->rvec)) { // no right vector either, so make something up
1457 if (!m->fvec.x && m->fvec.y && !m->fvec.z) // vertical vector
1458 vm_vec_make(&m->uvec, 0.0f, 0.0f, -1.0f);
1460 vm_vec_make(&m->uvec, 0.0f, 1.0f, 0.0f);
1462 } else { // use the right vector to figure up vector
1463 vm_vec_crossprod(&m->uvec, &m->fvec, &m->rvec);
1464 vm_vec_normalize(&m->uvec);
1468 vm_vec_normalize(&m->uvec);
1470 // we now have both valid and normalized forward and up vectors
1472 vm_vec_crossprod(&m->rvec, &m->uvec, &m->fvec);
1474 //normalize new perpendicular vector
1475 vm_vec_normalize(&m->rvec);
1477 //now recompute up vector, in case it wasn't entirely perpendiclar
1478 vm_vec_crossprod(&m->uvec, &m->fvec, &m->rvec);
1481 //Rotates the orient matrix by the angles in tangles and then
1482 //makes sure that the matrix is orthogonal.
1483 void vm_rotate_matrix_by_angles( matrix *orient, angles *tangles )
1485 matrix rotmat,new_orient;
1486 vm_angles_2_matrix(&rotmat,tangles);
1487 vm_matrix_x_matrix(&new_orient,orient,&rotmat);
1488 *orient = new_orient;
1489 vm_orthogonalize_matrix(orient);
1492 // dir must be normalized!
1493 float vm_vec_dot_to_point(vector *dir, vector *p1, vector *p2)
1497 vm_vec_sub(&tvec, p2, p1);
1498 vm_vec_normalize(&tvec);
1500 return vm_vec_dot(dir, &tvec);
1504 /////////////////////////////////////////////////////////
1505 // Given a plane and a point, return the point on the plane closest the the point.
1506 // Result returned in q.
1507 void compute_point_on_plane(vector *q, plane *planep, vector *p)
1512 normal.x = planep->A;
1513 normal.y = planep->B;
1514 normal.z = planep->C;
1516 k = (planep->D + vm_vec_dot(&normal, p)) / vm_vec_dot(&normal, &normal);
1518 vm_vec_scale_add(q, p, &normal, -k);
1520 tv = planep->A * q->x + planep->B * q->y + planep->C * q->z + planep->D;
1524 // Generate a fairly random vector that's fairly near normalized.
1525 void vm_vec_rand_vec_quick(vector *rvec)
1527 rvec->x = (frand() - 0.5f) * 2;
1528 rvec->y = (frand() - 0.5f) * 2;
1529 rvec->z = (frand() - 0.5f) * 2;
1531 if (IS_VEC_NULL(rvec))
1534 vm_vec_normalize_quick(rvec);
1537 // Given an point "in" rotate it by "angle" around an
1538 // arbritary line defined by a point on the line "line_point"
1539 // and the normalized line direction, "line_dir"
1540 // Returns the rotated point in "out".
1541 void vm_rot_point_around_line(vector *out, vector *in, float angle, vector *line_point, vector *line_dir)
1547 vm_vector_2_matrix_norm(&m, line_dir, NULL, NULL );
1548 vm_copy_transpose_matrix(&im,&m);
1552 vm_angles_2_matrix(&r,&ta);
1554 vm_vec_sub( &tmp, in, line_point ); // move relative to a point on line
1555 vm_vec_rotate( &tmp1, &tmp, &m); // rotate into line's base
1556 vm_vec_rotate( &tmp, &tmp1, &r); // rotate around Z
1557 vm_vec_rotate( &tmp1, &tmp, &im); // unrotate out of line's base
1558 vm_vec_add( out, &tmp1, line_point ); // move back to world coordinates
1561 // Given two position vectors, return 0 if the same, else non-zero.
1562 int vm_vec_cmp( vector * a, vector * b )
1564 float diff = vm_vec_dist(a,b);
1565 //mprintf(( "Diff=%.32f\n", diff ));
1566 if ( diff > 0.005f )
1572 // Given two orientation matrices, return 0 if the same, else non-zero.
1573 int vm_matrix_cmp( matrix * a, matrix * b )
1575 float tmp1,tmp2,tmp3;
1576 tmp1 = (float)fl_abs(vm_vec_dot( &a->uvec, &b->uvec ) - 1.0f);
1577 tmp2 = (float)fl_abs(vm_vec_dot( &a->fvec, &b->fvec ) - 1.0f);
1578 tmp3 = (float)fl_abs(vm_vec_dot( &a->rvec, &b->rvec ) - 1.0f);
1579 // mprintf(( "Mat=%.16f, %.16f, %.16f\n", tmp1, tmp2, tmp3 ));
1581 if ( tmp1 > 0.0000005f ) return 1;
1582 if ( tmp2 > 0.0000005f ) return 1;
1583 if ( tmp3 > 0.0000005f ) return 1;
1588 // Moves angle 'h' towards 'desired_angle', taking the shortest
1589 // route possible. It will move a maximum of 'step_size' radians
1590 // each call. All angles in radians.
1591 void vm_interp_angle( float *h, float desired_angle, float step_size )
1595 if ( desired_angle < 0.0f ) desired_angle += PI2;
1596 if ( desired_angle > PI2 ) desired_angle -= PI2;
1598 delta = desired_angle - *h;
1600 if ( fl_abs(delta) > PI ) {
1601 // Go the other way, since it will be shorter.
1602 if ( delta > 0.0f ) {
1603 delta = delta - PI2;
1605 delta = PI2 - delta;
1609 if ( delta > step_size )
1611 else if ( delta < -step_size )
1616 // If we wrap outside of 0 to 2*PI, then put the
1617 // angle back in the range 0 to 2*PI.
1618 if ( *h > PI2 ) *h -= PI2;
1619 if ( *h < 0.0f ) *h += PI2;
1622 // check a matrix for zero rows and columns
1623 int vm_check_matrix_for_zeros(matrix *m)
1625 if (!m->fvec.x && !m->fvec.y && !m->fvec.z)
1627 if (!m->rvec.x && !m->rvec.y && !m->rvec.z)
1629 if (!m->uvec.x && !m->uvec.y && !m->uvec.z)
1632 if (!m->fvec.x && !m->rvec.x && !m->uvec.x)
1634 if (!m->fvec.y && !m->rvec.y && !m->uvec.y)
1636 if (!m->fvec.z && !m->rvec.z && !m->uvec.z)
1642 // see if two vectors are the same
1643 int vm_vec_same(vector *v1, vector *v2)
1645 if ( v1->x == v2->x && v1->y == v2->y && v1->z == v2->z )
1652 // --------------------------------------------------------------------------------------
1654 void vm_quaternion_rotate(matrix *M, float theta, vector *u)
1655 // given an arbitrary rotation axis and rotation angle, function generates the
1656 // corresponding rotation matrix
1658 // M is the return rotation matrix theta is the angle of rotation
1659 // u is the direction of the axis.
1660 // this is adapted from Computer Graphics (Hearn and Bker 2nd ed.) p. 420
1666 a = (float) (u->x * sin(theta * 0.5f));
1667 b = (float) (u->y * sin(theta * 0.5f));
1668 c = (float) (u->z * sin(theta * 0.5f));
1669 s = (float) cos(theta/2.0);
1672 M->rvec.x = 1.0f - 2.0f*b*b - 2.0f*c*c;
1673 M->rvec.y = 2.0f*a*b + 2.0f*s*c;
1674 M->rvec.z = 2.0f*a*c - 2.0f*s*b;
1676 M->uvec.x = 2.0f*a*b - 2.0f*s*c;
1677 M->uvec.y = 1.0f - 2.0f*a*a - 2.0f*c*c;
1678 M->uvec.z = 2.0f*b*c + 2.0f*s*a;
1680 M->fvec.x = 2.0f*a*c + 2.0f*s*b;
1681 M->fvec.y = 2.0f*b*c - 2.0f*s*a;
1682 M->fvec.z = 1.0f - 2.0f*a*a - 2.0f*b*b;
1685 // --------------------------------------------------------------------------------------
1686 // function finds the rotation matrix about the z axis for a given rotation angle (in radians)
1687 // this is an optimized version vm_quaternion_rotate
1689 // inputs: m => point to resultant rotation matrix
1690 // angle => rotation angle about z axis (in radians)
1692 void rotate_z ( matrix *m, float theta )
1694 m->rvec.x = (float) cos (theta);
1695 m->rvec.y = (float) sin (theta);
1698 m->uvec.x = -m->rvec.y;
1699 m->uvec.y = m->rvec.x;
1708 // --------------------------------------------------------------------------------------
1710 //void vm_matrix_to_rot_axis_and_angle(matrix *m, float *theta, vector *rot_axis)
1711 // Converts a matrix into a rotation axis and an angle around that axis
1712 // Note for angle is very near 0, returns 0 with axis of (1,0,0)
1713 // For angles near PI, returns PI with correct axis
1715 // rot_axis - the resultant axis of rotation
1716 // theta - the resultatn rotation around the axis
1717 // m - the initial matrix
1718 void vm_matrix_to_rot_axis_and_angle(matrix *m, float *theta, vector *rot_axis)
1720 float trace = m->a2d[0][0] + m->a2d[1][1] + m->a2d[2][2];
1721 float cos_theta = 0.5f * (trace - 1.0f);
1723 if (cos_theta > 0.999999875f) { // angle is less than 1 milirad (0.057 degrees)
1726 vm_vec_make(rot_axis, 1.0f, 0.0f, 0.0f);
1727 } else if (cos_theta > -0.999999875f) { // angle is within limits between 0 and PI
1728 *theta = float(acos(cos_theta));
1729 Assert(!_isnan(*theta));
1731 rot_axis->x = (m->uvec.z - m->fvec.y);
1732 rot_axis->y = (m->fvec.x - m->rvec.z);
1733 rot_axis->z = (m->rvec.y - m->uvec.x);
1734 vm_vec_normalize(rot_axis);
1735 } else { // angle is PI within limits
1738 // find index of largest diagonal term
1739 int largest_diagonal_index = 0;
1741 if (m->a2d[1][1] > m->a2d[0][0]) {
1742 largest_diagonal_index = 1;
1744 if (m->a2d[2][2] > m->a2d[largest_diagonal_index][largest_diagonal_index]) {
1745 largest_diagonal_index = 2;
1748 switch (largest_diagonal_index) {
1751 ix = 1.0f / rot_axis->x;
1753 rot_axis->x = fl_sqrt(m->a2d[0][0] + 1.0f);
1754 rot_axis->y = m->a2d[0][1] * ix;
1755 rot_axis->z = m->a2d[0][2] * ix;
1756 vm_vec_normalize(rot_axis);
1761 iy = 1.0f / rot_axis->y;
1763 rot_axis->y = fl_sqrt(m->a2d[1][1] + 1.0f);
1764 rot_axis->x = m->a2d[1][0] * iy;
1765 rot_axis->z = m->a2d[1][2] * iy;
1766 vm_vec_normalize(rot_axis);
1771 iz = 1.0f / rot_axis->z;
1773 rot_axis->z = fl_sqrt(m->a2d[2][2] + 1.0f);
1774 rot_axis->x = m->a2d[2][0] * iz;
1775 rot_axis->y = m->a2d[2][1] * iz;
1779 Int3(); // this should never happen
1783 // normalize rotation axis
1784 vm_vec_normalize(rot_axis);
1789 // --------------------------------------------------------------------------------------
1790 // This routine determines the resultant angular displacement and angular velocity in trying to reach a goal
1791 // given an angular velocity APPROACHing a goal. It uses maximal acceleration to a point (called peak), then maximal
1792 // deceleration to arrive at the goal with zero angular velocity. This can occasionally cause overshoot.
1797 // returns delta_theta
1798 float away(float w_in, float w_max, float theta_goal, float aa, float delta_t, float *w_out, int no_overshoot);
1799 float approach(float w_in, float w_max, float theta_goal, float aa, float delta_t, float *w_out, int no_overshoot)
1801 float delta_theta; // amount rotated during time delta_t
1803 Assert(theta_goal > 0);
1808 delta_theta = w_in*delta_t;
1812 if (no_overshoot && (w_in*w_in > 2.0f*1.05f*aa*theta_goal)) {
1813 w_in = fl_sqrt(2.0f*aa*theta_goal);
1816 if (w_in*w_in > 2.0f*1.05f*aa*theta_goal) { // overshoot condition
1817 effective_aa = 1.05f*aa;
1818 delta_theta = w_in*delta_t - 0.5f*effective_aa*delta_t*delta_t;
1820 if (delta_theta > theta_goal) { // pass goal during this frame
1821 float t_goal = (-w_in + fl_sqrt(w_in*w_in +2.0f*effective_aa*theta_goal)) / effective_aa;
1822 // get time to theta_goal and away
1823 Assert(t_goal < delta_t);
1824 w_in -= effective_aa*t_goal;
1825 delta_theta = w_in*t_goal + 0.5f*effective_aa*t_goal*t_goal;
1826 delta_theta -= away(-w_in, w_max, 0.0f, aa, delta_t - t_goal, w_out, no_overshoot);
1830 if (delta_theta < 0) {
1831 // pass goal and return this frame
1835 // do not pass goal this frame
1836 *w_out = w_in - effective_aa*delta_t;
1840 } else if (w_in*w_in < 2.0f*0.95f*aa*theta_goal) { // undershoot condition
1841 // find peak angular velocity
1842 float wp_sqr = fl_abs(aa*theta_goal + 0.5f*w_in*w_in);
1843 Assert(wp_sqr >= 0);
1845 if (wp_sqr > w_max*w_max) {
1846 float time_to_w_max = (w_max - w_in) / aa;
1847 if (time_to_w_max < 0) {
1848 // speed already too high
1849 // TODO: consider possible ramp down to below w_max
1850 *w_out = w_in - aa*delta_t;
1855 delta_theta = 0.5f*(w_in + *w_out)*delta_t;
1857 } else if (time_to_w_max > delta_t) {
1858 // does not reach w_max this frame
1859 *w_out = w_in + aa*delta_t;
1860 delta_theta = 0.5f*(w_in + *w_out)*delta_t;
1863 // reaches w_max this frame
1864 // TODO: consider when to ramp down from w_max
1866 delta_theta = 0.5f*(w_in + *w_out)*delta_t;
1869 } else { // wp < w_max
1870 if (wp_sqr > (w_in + aa*delta_t)*(w_in + aa*delta_t)) {
1871 // does not reach wp this frame
1872 *w_out = w_in + aa*delta_t;
1873 delta_theta = 0.5f*(w_in + *w_out)*delta_t;
1876 // reaches wp this frame
1877 float wp = fl_sqrt(wp_sqr);
1878 float time_to_wp = (wp - w_in) / aa;
1879 Assert(time_to_wp > 0);
1883 delta_theta = 0.5f*(w_in + *w_out)*time_to_wp;
1886 float time_remaining = delta_t - time_to_wp;
1887 *w_out -= aa*time_remaining;
1888 if (*w_out < 0) { // reached goal
1890 delta_theta = theta_goal;
1893 delta_theta += 0.5f*(wp + *w_out)*time_remaining;
1897 } else { // on target
1898 // reach goal this frame
1899 if (w_in - aa*delta_t < 0) {
1900 // reach goal this frame
1905 *w_out = w_in - aa*delta_t;
1906 Assert(*w_out >= 0);
1907 delta_theta = 0.5f*(w_in + *w_out)*delta_t;
1914 // --------------------------------------------------------------------------------------
1916 // This routine determines the resultant angular displacement and angular velocity in trying to reach a goal
1917 // given an angular velocity AWAY from a goal. It uses maximal acceleration to a point (called peak), then maximal
1918 // deceleration to arrive at the goal with zero angular acceleration.
1923 // returns angle rotated this frame
1924 float away(float w_in, float w_max, float theta_goal, float aa, float delta_t, float *w_out, int no_overshoot)
1927 float delta_theta;// amount rotated during time
1928 float t0; // time to velocity is 0
1929 float t_excess; // time remaining in interval after velocity is 0
1931 Assert(theta_goal >=0);
1934 if ((-w_in < 1e-5) && (theta_goal < 1e-5)) {
1941 delta_theta = w_in*delta_t;
1947 if (t0 > delta_t) { // no reversal in this time interval
1948 *w_out = w_in + aa * delta_t;
1949 delta_theta = (w_in + *w_out) / 2.0f * delta_t;
1953 // use time remaining after v = 0
1954 delta_theta = 0.5f*w_in*t0;
1955 theta_goal -= delta_theta; // delta_theta is *negative*
1956 t_excess = delta_t - t0;
1957 delta_theta += approach(0.0f, w_max, theta_goal, aa, t_excess, w_out, no_overshoot);
1961 // --------------------------------------------------------------------------------------
1963 void vm_matrix_interpolate(matrix *goal_orient, matrix *curr_orient, vector *w_in, float delta_t,
1964 matrix *next_orient, vector *w_out, vector *vel_limit, vector *acc_limit, int no_overshoot)
1966 matrix rot_matrix; // rotation matrix from curr_orient to goal_orient
1967 matrix Mtemp1; // temp matrix
1968 vector rot_axis; // vector indicating direction of rotation axis
1969 vector theta_goal; // desired angular position at the end of the time interval
1970 vector theta_end; // actual angular position at the end of the time interval
1971 float theta; // magnitude of rotation about the rotation axis
1973 // FIND ROTATION NEEDED FOR GOAL
1974 // goal_orient = R curr_orient, so R = goal_orient curr_orient^-1
1975 vm_copy_transpose_matrix(&Mtemp1, curr_orient); // Mtemp1 = curr ^-1
1976 vm_matrix_x_matrix(&rot_matrix, &Mtemp1, goal_orient); // R = goal * Mtemp1
1977 vm_orthogonalize_matrix(&rot_matrix);
1978 vm_matrix_to_rot_axis_and_angle(&rot_matrix, &theta, &rot_axis); // determines angle and rotation axis from curr to goal
1980 // find theta to goal
1981 vm_vec_copy_scale(&theta_goal, &rot_axis, theta);
1983 if (theta < SMALL_NUM) {
1984 *next_orient = *goal_orient;
1989 theta_end = vmd_zero_vector;
1992 // find rotation about x
1993 if (theta_goal.x > 0) {
1995 delta_theta = approach(w_in->x, vel_limit->x, theta_goal.x, acc_limit->x, delta_t, &w_out->x, no_overshoot);
1996 theta_end.x = delta_theta;
1997 } else { // w_in->x < 0
1998 delta_theta = away(w_in->x, vel_limit->x, theta_goal.x, acc_limit->x, delta_t, &w_out->x, no_overshoot);
1999 theta_end.x = delta_theta;
2001 } else if (theta_goal.x < 0) {
2003 delta_theta = approach(-w_in->x, vel_limit->x, -theta_goal.x, acc_limit->x, delta_t, &w_out->x, no_overshoot);
2004 theta_end.x = -delta_theta;
2005 w_out->x = -w_out->x;
2006 } else { // w_in->x > 0
2007 delta_theta = away(-w_in->x, vel_limit->x, -theta_goal.x, acc_limit->x, delta_t, &w_out->x, no_overshoot);
2008 theta_end.x = -delta_theta;
2009 w_out->x = -w_out->x;
2011 } else { // theta_goal == 0
2013 delta_theta = away(w_in->x, vel_limit->x, theta_goal.x, acc_limit->x, delta_t, &w_out->x, no_overshoot);
2014 theta_end.x = delta_theta;
2016 delta_theta = away(-w_in->x, vel_limit->x, theta_goal.x, acc_limit->x, delta_t, &w_out->x, no_overshoot);
2017 theta_end.x = -delta_theta;
2018 w_out->x = -w_out->x;
2023 // find rotation about y
2024 if (theta_goal.y > 0) {
2026 delta_theta = approach(w_in->y, vel_limit->y, theta_goal.y, acc_limit->y, delta_t, &w_out->y, no_overshoot);
2027 theta_end.y = delta_theta;
2028 } else { // w_in->y < 0
2029 delta_theta = away(w_in->y, vel_limit->y, theta_goal.y, acc_limit->y, delta_t, &w_out->y, no_overshoot);
2030 theta_end.y = delta_theta;
2032 } else if (theta_goal.y < 0) {
2034 delta_theta = approach(-w_in->y, vel_limit->y, -theta_goal.y, acc_limit->y, delta_t, &w_out->y, no_overshoot);
2035 theta_end.y = -delta_theta;
2036 w_out->y = -w_out->y;
2037 } else { // w_in->y > 0
2038 delta_theta = away(-w_in->y, vel_limit->y, -theta_goal.y, acc_limit->y, delta_t, &w_out->y, no_overshoot);
2039 theta_end.y = -delta_theta;
2040 w_out->y = -w_out->y;
2042 } else { // theta_goal == 0
2044 delta_theta = away(w_in->y, vel_limit->y, theta_goal.y, acc_limit->y, delta_t, &w_out->y, no_overshoot);
2045 theta_end.y = delta_theta;
2047 delta_theta = away(-w_in->y, vel_limit->y, theta_goal.y, acc_limit->y, delta_t, &w_out->y, no_overshoot);
2048 theta_end.y = -delta_theta;
2049 w_out->y = -w_out->y;
2053 // find rotation about z
2054 if (theta_goal.z > 0) {
2056 delta_theta = approach(w_in->z, vel_limit->z, theta_goal.z, acc_limit->z, delta_t, &w_out->z, no_overshoot);
2057 theta_end.z = delta_theta;
2058 } else { // w_in->z < 0
2059 delta_theta = away(w_in->z, vel_limit->z, theta_goal.z, acc_limit->z, delta_t, &w_out->z, no_overshoot);
2060 theta_end.z = delta_theta;
2062 } else if (theta_goal.z < 0) {
2064 delta_theta = approach(-w_in->z, vel_limit->z, -theta_goal.z, acc_limit->z, delta_t, &w_out->z, no_overshoot);
2065 theta_end.z = -delta_theta;
2066 w_out->z = -w_out->z;
2067 } else { // w_in->z > 0
2068 delta_theta = away(-w_in->z, vel_limit->z, -theta_goal.z, acc_limit->z, delta_t, &w_out->z, no_overshoot);
2069 theta_end.z = -delta_theta;
2070 w_out->z = -w_out->z;
2072 } else { // theta_goal == 0
2074 delta_theta = away(w_in->z, vel_limit->z, theta_goal.z, acc_limit->z, delta_t, &w_out->z, no_overshoot);
2075 theta_end.z = delta_theta;
2077 delta_theta = away(-w_in->z, vel_limit->z, theta_goal.z, acc_limit->z, delta_t, &w_out->z, no_overshoot);
2078 theta_end.z = -delta_theta;
2079 w_out->z = -w_out->z;
2083 // the amount of rotation about each axis is determined in
2084 // functions approach and away. first find the magnitude
2085 // of the rotation and then normalize the axis
2086 rot_axis = theta_end;
2087 Assert(is_valid_vec(&rot_axis));
2088 Assert(vm_vec_mag(&rot_axis) > 0);
2090 // normalize rotation axis and determine total rotation angle
2091 theta = vm_vec_normalize(&rot_axis);
2094 if (theta_end.x == theta_goal.x && theta_end.y == theta_goal.y && theta_end.z == theta_goal.z) {
2095 *next_orient = *goal_orient;
2097 // otherwise rotate to better position
2098 vm_quaternion_rotate(&Mtemp1, theta, &rot_axis);
2099 Assert(is_valid_matrix(&Mtemp1));
2100 vm_matrix_x_matrix(next_orient, curr_orient, &Mtemp1);
2101 vm_orthogonalize_matrix(next_orient);
2103 } // end matrix_interpolate
2106 // --------------------------------------------------------------------------------------
2109 void get_camera_limits(matrix *start_camera, matrix *end_camera, float time, vector *acc_max, vector *w_max)
2111 matrix temp, rot_matrix;
2116 // determine the necessary rotation matrix
2117 vm_copy_transpose(&temp, start_camera);
2118 vm_matrix_x_matrix(&rot_matrix, &temp, end_camera);
2119 vm_orthogonalize_matrix(&rot_matrix);
2121 // determine the rotation axis and angle
2122 vm_matrix_to_rot_axis_and_angle(&rot_matrix, &theta, &rot_axis);
2124 // find the rotation about each axis
2125 angle.x = theta * rot_axis.x;
2126 angle.y = theta * rot_axis.y;
2127 angle.z = theta * rot_axis.z;
2129 // allow for 0 time input
2130 if (time <= 1e-5f) {
2131 vm_vec_make(acc_max, 0.0f, 0.0f, 0.0f);
2132 vm_vec_make(w_max, 0.0f, 0.0f, 0.0f);
2135 // find acceleration limit using (theta/2) takes (time/2)
2136 // and using const accel theta = 1/2 acc * time^2
2137 acc_max->x = 4.0f * (float)fl_abs(angle.x) / (time * time);
2138 acc_max->y = 4.0f * (float)fl_abs(angle.y) / (time * time);
2139 acc_max->z = 4.0f * (float)fl_abs(angle.z) / (time * time);
2141 // find angular velocity limits
2142 // w_max = acc_max * time / 2
2143 w_max->x = acc_max->x * time / 2.0f;
2144 w_max->y = acc_max->y * time / 2.0f;
2145 w_max->z = acc_max->z * time / 2.0f;
2149 // ---------------------------------------------------------------------------------------------
2151 // inputs: goal_orient => goal orientation matrix
2152 // orient => current orientation matrix (with current forward vector)
2153 // w_in => current input angular velocity
2154 // delta_t => time to move toward goal
2155 // next_orient => the orientation matrix at time delta_t (with current forward vector)
2156 // NOTE: this does not include any rotation about z (bank)
2157 // w_out => the angular velocity of the ship at delta_t
2158 // vel_limit => maximum rotational speed
2159 // acc_limit => maximum rotational speed
2161 // function moves the forward vector toward the goal forward vector taking account of anglular
2162 // momentum (velocity) Attempt to try to move bank by goal delta_bank. Rotational velocity
2163 // on x/y is rotated with bank, giving smoother motion.
2164 void vm_fvec_matrix_interpolate(matrix *goal_orient, matrix *orient, vector *w_in, float delta_t, matrix *next_orient,
2165 vector *w_out, vector *vel_limit, vector *acc_limit, int no_overshoot)
2167 matrix Mtemp1; // temporary matrix
2168 matrix M_intermed; // intermediate matrix after xy rotation
2169 vector local_rot_axis; // vector indicating direction of rotation axis (local coords)
2170 vector rot_axis; // vector indicating direction of rotation axis (world coords)
2171 vector theta_goal; // desired angular position at the end of the time interval
2172 vector theta_end; // actual angular position at the end of the time interval
2173 float theta; // magnitude of rotation about the rotation axis
2174 float bank; // magnitude of rotation about the forward axis
2175 int no_bank; // flag set if there is no bank for the object
2176 vector vtemp; // temp angular velocity before rotation about z
2177 float z_dotprod; // dotprod of orient->fvec and goal_orient->fvec
2178 float r_dotprod; // dotprod of orient->rvec and goal_orient->rvec
2181 // FIND XY ROTATION NEEDED FOR GOAL
2182 // rotation vector is (current fvec) orient->fvec x goal_f
2183 // magnitude = asin ( magnitude of crossprod )
2184 vm_vec_crossprod ( &rot_axis, &orient->fvec, &goal_orient->fvec );
2186 float t = vm_vec_mag(&rot_axis);
2190 z_dotprod = vm_vec_dotprod ( &orient->fvec, &goal_orient->fvec );
2192 if ( t < SMALLER_NUM ) {
2193 if ( z_dotprod > 0.0f )
2195 else { // the forward vector is pointing exactly opposite of goal
2196 // arbitrarily choose the x axis to rotate around until t becomes large enough
2198 rot_axis = orient->rvec;
2201 theta = (float) asin ( t );
2202 vm_vec_scale ( &rot_axis, 1/t );
2203 if ( z_dotprod < 0.0f )
2207 // rotate rot_axis into ship reference frame
2208 vm_vec_rotate ( &local_rot_axis, &rot_axis, orient );
2210 // find theta to goal
2211 vm_vec_copy_scale(&theta_goal, &local_rot_axis, theta);
2212 Assert ( fl_abs (theta_goal.z) < 0.001f ); // check for proper rotation
2214 theta_end = vmd_zero_vector;
2217 // find rotation about x
2218 if (theta_goal.x > 0) {
2220 delta_theta = approach(w_in->x, vel_limit->x, theta_goal.x, acc_limit->x, delta_t, &w_out->x, no_overshoot);
2221 theta_end.x = delta_theta;
2222 } else { // w_in->x < 0
2223 delta_theta = away(w_in->x, vel_limit->x, theta_goal.x, acc_limit->x, delta_t, &w_out->x, no_overshoot);
2224 theta_end.x = delta_theta;
2226 } else if (theta_goal.x < 0) {
2228 delta_theta = approach(-w_in->x, vel_limit->x, -theta_goal.x, acc_limit->x, delta_t, &w_out->x, no_overshoot);
2229 theta_end.x = -delta_theta;
2230 w_out->x = -w_out->x;
2231 } else { // w_in->x > 0
2232 delta_theta = away(-w_in->x, vel_limit->x, -theta_goal.x, acc_limit->x, delta_t, &w_out->x, no_overshoot);
2233 theta_end.x = -delta_theta;
2234 w_out->x = -w_out->x;
2236 } else { // theta_goal == 0
2238 delta_theta = away(w_in->x, vel_limit->x, theta_goal.x, acc_limit->x, delta_t, &w_out->x, no_overshoot);
2239 theta_end.x = delta_theta;
2241 delta_theta = away(-w_in->x, vel_limit->x, theta_goal.x, acc_limit->x, delta_t, &w_out->x, no_overshoot);
2242 theta_end.x = -delta_theta;
2243 w_out->x = -w_out->x;
2247 // find rotation about y
2248 if (theta_goal.y > 0) {
2250 delta_theta = approach(w_in->y, vel_limit->y, theta_goal.y, acc_limit->y, delta_t, &w_out->y, no_overshoot);
2251 theta_end.y = delta_theta;
2252 } else { // w_in->y < 0
2253 delta_theta = away(w_in->y, vel_limit->y, theta_goal.y, acc_limit->y, delta_t, &w_out->y, no_overshoot);
2254 theta_end.y = delta_theta;
2256 } else if (theta_goal.y < 0) {
2258 delta_theta = approach(-w_in->y, vel_limit->y, -theta_goal.y, acc_limit->y, delta_t, &w_out->y, no_overshoot);
2259 theta_end.y = -delta_theta;
2260 w_out->y = -w_out->y;
2261 } else { // w_in->y > 0
2262 delta_theta = away(-w_in->y, vel_limit->y, -theta_goal.y, acc_limit->y, delta_t, &w_out->y, no_overshoot);
2263 theta_end.y = -delta_theta;
2264 w_out->y = -w_out->y;
2266 } else { // theta_goal == 0
2268 delta_theta = away(w_in->y, vel_limit->y, theta_goal.y, acc_limit->y, delta_t, &w_out->y, no_overshoot);
2269 theta_end.y = delta_theta;
2271 delta_theta = away(-w_in->y, vel_limit->y, theta_goal.y, acc_limit->y, delta_t, &w_out->y, no_overshoot);
2272 theta_end.y = -delta_theta;
2273 w_out->y = -w_out->y;
2277 // FIND Z ROTATON MATRIX
2279 rot_axis = theta_end;
2280 Assert(is_valid_vec(&rot_axis));
2282 // normalize rotation axis and determine total rotation angle
2283 theta = vm_vec_mag(&rot_axis);
2284 if (theta < SMALL_NUM) {
2286 M_intermed = *orient;
2288 vm_vec_scale ( &rot_axis, 1/theta );
2289 vm_quaternion_rotate ( &Mtemp1, theta, &rot_axis );
2290 Assert(is_valid_matrix(&Mtemp1));
2291 vm_matrix_x_matrix ( &M_intermed, orient, &Mtemp1 );
2292 Assert(is_valid_matrix(&M_intermed));
2296 // FIND ROTATION ABOUT Z (IF ANY)
2297 // no rotation if delta_bank and w_in both 0 or rotational acc in forward is 0
2298 no_bank = ( acc_limit->z == 0.0f && vel_limit->z == 0.0f );
2300 if ( no_bank ) { // no rotation on z, so we're done (no rotation of w)
2301 *next_orient = M_intermed;
2302 vm_orthogonalize_matrix ( next_orient );
2305 // calculate delta_bank using orient->rvec, goal_orient->rvec
2307 vm_vec_crossprod ( &rot_axis, &orient->rvec, &goal_orient->rvec );
2309 t = vm_vec_mag(&rot_axis);
2313 r_dotprod = vm_vec_dotprod ( &orient->rvec, &goal_orient->rvec );
2315 if ( t < SMALLER_NUM ) {
2316 if ( r_dotprod > 0.0f )
2318 else { // the right vector is pointing exactly opposite of goal, so rotate 180 on z
2320 rot_axis = orient->fvec;
2323 theta = (float) asin ( t );
2324 vm_vec_scale ( &rot_axis, 1/t );
2325 if ( z_dotprod < 0.0f )
2329 // rotate rot_axis into ship reference frame
2330 vm_vec_rotate ( &local_rot_axis, &rot_axis, orient );
2332 // find theta.z to goal
2333 delta_bank = local_rot_axis.z * theta;
2334 Assert( fl_abs (local_rot_axis.x) < 0.001f ); // check for proper rotation
2337 // end calculate delta_bank
2338 // find rotation about z
2339 if (delta_bank > 0) {
2341 delta_theta = approach(w_in->z, vel_limit->z, delta_bank, acc_limit->z, delta_t, &w_out->z, no_overshoot);
2343 } else { // w_in->z < 0
2344 delta_theta = away(w_in->z, vel_limit->z, delta_bank, acc_limit->z, delta_t, &w_out->z, no_overshoot);
2347 } else if (delta_bank < 0) {
2349 delta_theta = approach(-w_in->z, vel_limit->z, -delta_bank, acc_limit->z, delta_t, &w_out->z, no_overshoot);
2350 bank = -delta_theta;
2351 w_out->z = -w_out->z;
2352 } else { // w_in->z > 0
2353 delta_theta = away(-w_in->z, vel_limit->z, -delta_bank, acc_limit->z, delta_t, &w_out->z, no_overshoot);
2354 bank = -delta_theta;
2355 w_out->z = -w_out->z;
2357 } else { // theta_goal == 0
2359 delta_theta = away(w_in->z, vel_limit->z, delta_bank, acc_limit->z, delta_t, &w_out->z, no_overshoot);
2362 delta_theta = away(-w_in->z, vel_limit->z, delta_bank, acc_limit->z, delta_t, &w_out->z, no_overshoot);
2363 bank = -delta_theta;
2364 w_out->z = -w_out->z;
2368 if ( fl_abs (bank) < SMALL_NUM )
2370 *next_orient = M_intermed;
2371 vm_orthogonalize_matrix ( next_orient );
2374 rotate_z ( &Mtemp1, bank );
2376 vm_vec_rotate ( w_out, &vtemp, &Mtemp1 );
2377 vm_matrix_x_matrix ( next_orient, &M_intermed, &Mtemp1 );
2378 Assert(is_valid_matrix(next_orient));
2379 vm_orthogonalize_matrix ( next_orient );
2382 } // end vm_fvec_matrix_interpolate
2385 // ---------------------------------------------------------------------------------------------
2387 // inputs: goal_f => goal forward vector
2388 // orient => current orientation matrix (with current forward vector)
2389 // w_in => current input angular velocity
2390 // delta_t => time to move toward goal
2391 // delta_bank => desired change in bank in degrees
2392 // next_orient => the orientation matrix at time delta_t (with current forward vector)
2393 // NOTE: this does not include any rotation about z (bank)
2394 // w_out => the angular velocity of the ship at delta_t
2395 // vel_limit => maximum rotational speed
2396 // acc_limit => maximum rotational speed
2398 // function moves the forward vector toward the goal forward vector taking account of anglular
2399 // momentum (velocity) Attempt to try to move bank by goal delta_bank. Rotational velocity
2400 // on x/y is rotated with bank, giving smoother motion.
2401 void vm_forward_interpolate(vector *goal_f, matrix *orient, vector *w_in, float delta_t, float delta_bank,
2402 matrix *next_orient, vector *w_out, vector *vel_limit, vector *acc_limit, int no_overshoot)
2404 matrix Mtemp1; // temporary matrix
2405 vector local_rot_axis; // vector indicating direction of rotation axis (local coords)
2406 vector rot_axis; // vector indicating direction of rotation axis (world coords)
2407 vector theta_goal; // desired angular position at the end of the time interval
2408 vector theta_end; // actual angular position at the end of the time interval
2409 float theta; // magnitude of rotation about the rotation axis
2410 float bank; // magnitude of rotation about the forward axis
2411 int no_bank; // flag set if there is no bank for the object
2415 // FIND ROTATION NEEDED FOR GOAL
2416 // rotation vector is (current fvec) orient->fvec x goal_f
2417 // magnitude = asin ( magnitude of crossprod )
2418 vm_vec_crossprod( &rot_axis, &orient->fvec, goal_f );
2420 float t = vm_vec_mag(&rot_axis);
2424 z_dotprod = vm_vec_dotprod( &orient->fvec, goal_f );
2426 if ( t < SMALLER_NUM ) {
2427 if ( z_dotprod > 0.0f )
2429 else { // the forward vector is pointing exactly opposite of goal
2430 // arbitrarily choose the x axis to rotate around until t becomes large enough
2432 rot_axis = orient->rvec;
2435 theta = (float) asin( t );
2436 vm_vec_scale ( &rot_axis, 1/t );
2437 if ( z_dotprod < 0.0f )
2441 // rotate rot_axis into ship reference frame
2442 vm_vec_rotate( &local_rot_axis, &rot_axis, orient );
2444 // find theta to goal
2445 vm_vec_copy_scale(&theta_goal, &local_rot_axis, theta);
2446 Assert(fl_abs(theta_goal.z) < 0.001f); // check for proper rotation
2448 theta_end = vmd_zero_vector;
2451 // find rotation about x
2452 if (theta_goal.x > 0) {
2454 delta_theta = approach(w_in->x, vel_limit->x, theta_goal.x, acc_limit->x, delta_t, &w_out->x, no_overshoot);
2455 theta_end.x = delta_theta;
2456 } else { // w_in->x < 0
2457 delta_theta = away(w_in->x, vel_limit->x, theta_goal.x, acc_limit->x, delta_t, &w_out->x, no_overshoot);
2458 theta_end.x = delta_theta;
2460 } else if (theta_goal.x < 0) {
2462 delta_theta = approach(-w_in->x, vel_limit->x, -theta_goal.x, acc_limit->x, delta_t, &w_out->x, no_overshoot);
2463 theta_end.x = -delta_theta;
2464 w_out->x = -w_out->x;
2465 } else { // w_in->x > 0
2466 delta_theta = away(-w_in->x, vel_limit->x, -theta_goal.x, acc_limit->x, delta_t, &w_out->x, no_overshoot);
2467 theta_end.x = -delta_theta;
2468 w_out->x = -w_out->x;
2470 } else { // theta_goal == 0
2472 delta_theta = away(w_in->x, vel_limit->x, theta_goal.x, acc_limit->x, delta_t, &w_out->x, no_overshoot);
2473 theta_end.x = delta_theta;
2475 delta_theta = away(-w_in->x, vel_limit->x, theta_goal.x, acc_limit->x, delta_t, &w_out->x, no_overshoot);
2476 theta_end.x = -delta_theta;
2477 w_out->x = -w_out->x;
2481 // find rotation about y
2482 if (theta_goal.y > 0) {
2484 delta_theta = approach(w_in->y, vel_limit->y, theta_goal.y, acc_limit->y, delta_t, &w_out->y, no_overshoot);
2485 theta_end.y = delta_theta;
2486 } else { // w_in->y < 0
2487 delta_theta = away(w_in->y, vel_limit->y, theta_goal.y, acc_limit->y, delta_t, &w_out->y, no_overshoot);
2488 theta_end.y = delta_theta;
2490 } else if (theta_goal.y < 0) {
2492 delta_theta = approach(-w_in->y, vel_limit->y, -theta_goal.y, acc_limit->y, delta_t, &w_out->y, no_overshoot);
2493 theta_end.y = -delta_theta;
2494 w_out->y = -w_out->y;
2495 } else { // w_in->y > 0
2496 delta_theta = away(-w_in->y, vel_limit->y, -theta_goal.y, acc_limit->y, delta_t, &w_out->y, no_overshoot);
2497 theta_end.y = -delta_theta;
2498 w_out->y = -w_out->y;
2500 } else { // theta_goal == 0
2502 delta_theta = away(w_in->y, vel_limit->y, theta_goal.y, acc_limit->y, delta_t, &w_out->y, no_overshoot);
2503 theta_end.y = delta_theta;
2505 delta_theta = away(-w_in->y, vel_limit->y, theta_goal.y, acc_limit->y, delta_t, &w_out->y, no_overshoot);
2506 theta_end.y = -delta_theta;
2507 w_out->y = -w_out->y;
2511 // no rotation if delta_bank and w_in both 0 or rotational acc in forward is 0
2512 no_bank = ( delta_bank == 0.0f && vel_limit->z == 0.0f && acc_limit->z == 0.0f );
2514 // do rotation about z
2517 // convert delta_bank to radians
2518 delta_bank *= (float) CONVERT_RADIANS;
2520 // find rotation about z
2521 if (delta_bank > 0) {
2523 delta_theta = approach(w_in->z, vel_limit->z, delta_bank, acc_limit->z, delta_t, &w_out->z, no_overshoot);
2525 } else { // w_in->z < 0
2526 delta_theta = away(w_in->z, vel_limit->z, delta_bank, acc_limit->z, delta_t, &w_out->z, no_overshoot);
2529 } else if (delta_bank < 0) {
2531 delta_theta = approach(-w_in->z, vel_limit->z, -delta_bank, acc_limit->z, delta_t, &w_out->z, no_overshoot);
2532 bank = -delta_theta;
2533 w_out->z = -w_out->z;
2534 } else { // w_in->z > 0
2535 delta_theta = away(-w_in->z, vel_limit->z, -delta_bank, acc_limit->z, delta_t, &w_out->z, no_overshoot);
2536 bank = -delta_theta;
2537 w_out->z = -w_out->z;
2539 } else { // theta_goal == 0
2541 delta_theta = away(w_in->z, vel_limit->z, delta_bank, acc_limit->z, delta_t, &w_out->z, no_overshoot);
2544 delta_theta = away(-w_in->z, vel_limit->z, delta_bank, acc_limit->z, delta_t, &w_out->z, no_overshoot);
2545 bank = -delta_theta;
2546 w_out->z = -w_out->z;
2551 // the amount of rotation about each axis is determined in
2552 // functions approach and away. first find the magnitude
2553 // of the rotation and then normalize the axis (ship coords)
2555 rot_axis = theta_end;
2557 // normalize rotation axis and determine total rotation angle
2558 theta = vm_vec_mag(&rot_axis);
2559 if ( theta > SMALL_NUM )
2560 vm_vec_scale( &rot_axis, 1/theta );
2562 if ( theta < SMALL_NUM ) {
2563 *next_orient = *orient;
2566 vm_quaternion_rotate( &Mtemp1, theta, &rot_axis );
2567 vm_matrix_x_matrix( next_orient, orient, &Mtemp1 );
2568 Assert(is_valid_matrix(next_orient));
2570 vm_vec_rotate( w_out, &vtemp, &Mtemp1 );
2572 } // end vm_forward_interpolate
2574 // ------------------------------------------------------------------------------------
2575 // vm_find_bounding_sphere()
2577 // Calculate a bounding sphere for a set of points.
2579 // input: pnts => array of world positions
2580 // num_pnts => number of points inside pnts array
2581 // center => OUTPUT PARAMETER: contains world pos of bounding sphere center
2582 // radius => OUTPUT PARAMETER: continas radius of bounding sphere
2584 #define BIGNUMBER 100000000.0f
2585 void vm_find_bounding_sphere(vector *pnts, int num_pnts, vector *center, float *radius)
2588 float rad, rad_sq, xspan, yspan, zspan, maxspan;
2589 float old_to_p, old_to_p_sq, old_to_new;
2590 vector diff, xmin, xmax, ymin, ymax, zmin, zmax, dia1, dia2, *p;
2592 xmin = vmd_zero_vector;
2593 ymin = vmd_zero_vector;
2594 zmin = vmd_zero_vector;
2595 xmax = vmd_zero_vector;
2596 ymax = vmd_zero_vector;
2597 zmax = vmd_zero_vector;
2598 xmin.x = ymin.y = zmin.z = BIGNUMBER;
2599 xmax.x = ymax.y = zmax.z = -BIGNUMBER;
2601 for ( i = 0; i < num_pnts; i++ ) {
2603 if ( p->x < xmin.x )
2605 if ( p->x > xmax.x )
2607 if ( p->y < ymin.y )
2609 if ( p->y > ymax.y )
2611 if ( p->z < zmin.z )
2613 if ( p->z > zmax.z )
2617 // find distance between two min and max points (squared)
2618 vm_vec_sub(&diff, &xmax, &xmin);
2619 xspan = vm_vec_mag_squared(&diff);
2621 vm_vec_sub(&diff, &ymax, &ymin);
2622 yspan = vm_vec_mag_squared(&diff);
2624 vm_vec_sub(&diff, &zmax, &zmin);
2625 zspan = vm_vec_mag_squared(&diff);
2630 if ( yspan > maxspan ) {
2635 if ( zspan > maxspan ) {
2641 // calc inital center
2642 vm_vec_add(center, &dia1, &dia2);
2643 vm_vec_scale(center, 0.5f);
2645 vm_vec_sub(&diff, &dia2, center);
2646 rad_sq = vm_vec_mag_squared(&diff);
2647 rad = fl_sqrt(rad_sq);
2648 Assert( !_isnan(rad) );
2651 for ( i = 0; i < num_pnts; i++ ) {
2653 vm_vec_sub(&diff, p, center);
2654 old_to_p_sq = vm_vec_mag_squared(&diff);
2655 if ( old_to_p_sq > rad_sq ) {
2656 old_to_p = fl_sqrt(old_to_p_sq);
2657 // calc radius of new sphere
2658 rad = (rad + old_to_p) / 2.0f;
2660 old_to_new = old_to_p - rad;
2661 // calc new center of sphere
2662 center->x = (rad*center->x + old_to_new*p->x) / old_to_p;
2663 center->y = (rad*center->y + old_to_new*p->y) / old_to_p;
2664 center->z = (rad*center->z + old_to_new*p->z) / old_to_p;
2665 nprintf(("Alan", "New sphere: cen,rad = %f %f %f %f\n", center->x, center->y, center->z, rad));
2672 // ----------------------------------------------------------------------------
2673 // vm_rotate_vec_to_body()
2675 // rotates a vector from world coordinates to body coordinates
2677 // inputs: body_vec => vector in body coordinates
2678 // world_vec => vector in world coordinates
2679 // orient => orientation matrix
2681 vector* vm_rotate_vec_to_body(vector *body_vec, vector *world_vec, matrix *orient)
2683 return vm_vec_unrotate(body_vec, world_vec, orient);
2687 // ----------------------------------------------------------------------------
2688 // vm_rotate_vec_to_world()
2690 // rotates a vector from body coordinates to world coordinates
2692 // inputs world_vec => vector in world coordinates
2693 // body_vec => vector in body coordinates
2694 // orient => orientation matrix
2696 vector* vm_rotate_vec_to_world(vector *world_vec, vector *body_vec, matrix *orient)
2698 return vm_vec_rotate(world_vec, body_vec, orient);
2702 // ----------------------------------------------------------------------------
2703 // vm_estimate_next_orientation()
2705 // given a last orientation and current orientation, estimate the next orientation
2707 // inputs: last_orient => last orientation matrix
2708 // current_orient => current orientation matrix
2709 // next_orient => next orientation matrix [the result]
2711 void vm_estimate_next_orientation(matrix *last_orient, matrix *current_orient, matrix *next_orient)
2713 // R L = C => R = C (L)^-1
2714 // N = R C => N = C (L)^-1 C
2718 vm_copy_transpose_matrix(&Mtemp, last_orient); // Mtemp = (L)^-1
2719 vm_matrix_x_matrix(&Rot_matrix, &Mtemp, current_orient); // R = C Mtemp1
2720 vm_matrix_x_matrix(next_orient, current_orient, &Rot_matrix);
2723 // Return true if all elements of *vec are legal, that is, not a NAN.
2724 int is_valid_vec(vector *vec)
2726 return !_isnan(vec->x) && !_isnan(vec->y) && !_isnan(vec->z);
2729 // Return true if all elements of *m are legal, that is, not a NAN.
2730 int is_valid_matrix(matrix *m)
2732 return is_valid_vec(&m->fvec) && is_valid_vec(&m->uvec) && is_valid_vec(&m->rvec);
2735 // interpolate between 2 vectors. t goes from 0.0 to 1.0. at
2736 void vm_vec_interp_constant(vector *out, vector *v0, vector *v1, float t)
2741 // get the cross-product of the 2 vectors
2742 vm_vec_crossprod(&cross, v0, v1);
2743 vm_vec_normalize(&cross);
2745 // get the total angle between the 2 vectors
2746 total_ang = -(float)acos(vm_vec_dot(v0, v1));
2748 // rotate around the cross product vector by the appropriate angle
2749 vm_rot_point_around_line(out, v0, t * total_ang, &vmd_zero_vector, &cross);
2752 // randomly perturb a vector around a given (normalized vector) or optional orientation matrix
2753 void vm_vec_random_cone(vector *out, vector *in, float max_angle, matrix *orient)
2759 // get an orientation matrix
2763 vm_vector_2_matrix(&m, in, NULL, NULL);
2768 vm_rot_point_around_line(&t1, in, fl_radian(frand_range(-max_angle, max_angle)), &vmd_zero_vector, &rot->fvec);
2771 vm_rot_point_around_line(&t2, &t1, fl_radian(frand_range(-max_angle, max_angle)), &vmd_zero_vector, &rot->rvec);
2774 vm_rot_point_around_line(out, &t2, fl_radian(frand_range(-max_angle, max_angle)), &vmd_zero_vector, &rot->uvec);
2777 // given a start vector, an orientation and a radius, give a point on the plane of the circle
2778 // if on_edge is 1, the point is on the very edge of the circle
2779 void vm_vec_random_in_circle(vector *out, vector *in, matrix *orient, float radius, int on_edge)
2783 // point somewhere in the plane
2784 vm_vec_scale_add(&temp, in, &orient->rvec, on_edge ? radius : frand_range(0.0f, radius));
2786 // rotate to a random point on the circle
2787 vm_rot_point_around_line(out, &temp, fl_radian(frand_range(0.0f, 359.0f)), in, &orient->fvec);
2790 // find the nearest point on the line to p. if dist is non-NULL, it is filled in
2791 // returns 0 if the point is inside the line segment, -1 if "before" the line segment and 1 ir "after" the line segment
2792 int vm_vec_dist_to_line(vector *p, vector *l0, vector *l1, vector *nearest, float *dist)
2798 if(vm_vec_same(l0, l1)){
2799 *nearest = vmd_zero_vector;
2804 // compb_a == a dot b / len(b)
2805 vm_vec_sub(&a, p, l0);
2806 vm_vec_sub(&b, l1, l0);
2807 b_mag = vm_vec_copy_normalize(&c, &b);
2809 // calculate component
2810 comp = vm_vec_dotprod(&a, &b) / b_mag;
2813 vm_vec_scale_add(nearest, l0, &c, comp);
2815 // maybe get the distance
2817 *dist = vm_vec_dist(nearest, p);
2820 // return the proper value
2822 return -1; // before the line
2823 } else if(comp > b_mag){
2824 return 1; // after the line
2826 return 0; // on the line