/* * Copyright (C) Volition, Inc. 1999. All rights reserved. * * All source code herein is the property of Volition, Inc. You may not sell * or otherwise commercially exploit the source or things you created based on the * source. * */ /* * $Logfile: /Freespace2/code/Math/VecMat.cpp $ * $Revision: 2.30.2.3 $ * $Date: 2007-02-26 01:47:14 $ * $Author: Goober5000 $ * * C module containg functions for manipulating vectors and matricies * * $Log: not supported by cvs2svn $ * Revision 2.30.2.2 2006/09/13 03:12:35 taylor * might as well add this, just to be on the safe side (I haven't hit it anywhere, but it could be made into a warning if it starts getting hit) * * Revision 2.30.2.1 2006/08/19 04:38:46 taylor * maybe optimize the (PI/2), (PI*2) and (RAND_MAX/2) stuff a little bit * * Revision 2.30 2006/05/24 05:09:25 wmcoolmon * Fix for Mantis bug #0000922 * * Revision 2.29 2006/04/12 22:23:41 taylor * compiler warning fixes to make GCC 4.1 shut the hell up * * Revision 2.28 2006/02/25 21:47:00 Goober5000 * spelling * * Revision 2.27 2006/02/16 05:09:26 taylor * NULL vec warning fix (I don't really like this one that much so it may get replaced later) * * Revision 2.26 2005/10/22 06:26:30 taylor * couple of changes to NULL vec fixes which better match what Volition would have done (makes both Goober and myself happier :)) * keep track of what was an error (even if erroneous) and why with "VECMAT-ERROR" comment * * Revision 2.25 2005/10/21 11:32:15 taylor * fix main NULL vec errors, still at least one left but these were those that happened most often: * modelinterp.cpp:parse_tmap() -> first IS_VEC_NULL() check would fail, wasn't a second on reassign * aicode.cpp:ai_path() -> when (Pl_objp->pos == gcvp), fixed globally in vecmat.cpp:vm_vec_dot_to_point() * * Revision 2.24 2005/07/22 09:59:30 Goober5000 * unhide null vector bug * --Goober5000 * * Revision 2.23 2005/04/05 05:53:18 taylor * s/vector/vec3d/g, better support for different compilers (Jens Granseuer) * * Revision 2.22 2005/03/25 06:57:35 wmcoolmon * Big, massive, codebase commit. I have not removed the old ai files as the ones I uploaded aren't up-to-date (But should work with the rest of the codebase) * * Revision 2.21 2005/03/24 23:36:13 taylor * fix compiler warnings with mismatched types and unused variables * cleanup some debug messages so they can be turned off if needed * get rid of extra strstr() check for thrusters since it should never get that far anyway * page_in/page_out of model glows should be better now * removed a bunch of unneeded casts and get type specific math functions right * * Revision 2.20 2005/03/19 18:02:34 bobboau * added new graphic functions for state blocks * also added a class formanageing a new effect * * Revision 2.19 2005/03/08 03:50:25 Goober5000 * edited for language ;) * --Goober5000 * * Revision 2.18 2005/03/03 06:05:29 wmcoolmon * Merge of WMC's codebase. "Features and bugs, making Goober say "Grr!", as release would be stalled now for two months for sure" * * Revision 2.17 2005/01/28 11:06:23 Goober5000 * changed a bunch of transpose-rotate sequences to use unrotate instead * --Goober5000 * * Revision 2.16 2005/01/27 05:26:07 Goober5000 * added a useful bit of information to vm_vec_rotate and vm_vec_unrotate * --Goober5000 * * Revision 2.15 2005/01/06 00:39:34 Goober5000 * gah * --Goober5000 * * Revision 2.14 2005/01/06 00:37:32 Goober5000 * changed argument from dist to dist_squared * --Goober5000 * * Revision 2.13 2005/01/06 00:27:34 Goober5000 * added vm_vec_dist_squared_to_line * --Goober5000 * * Revision 2.12 2004/10/31 21:49:49 taylor * fix math error that was hitting Assert()'s and making OGL models flip around for no reason * * Revision 2.11 2004/07/26 20:47:36 Kazan * remove MCD complete * * Revision 2.10 2004/07/12 16:32:52 Kazan * MCD - define _MCD_CHECK to use memory tracking * * Revision 2.9 2004/05/03 09:27:16 randomtiger * fl_abs(theta_goal.xyz.z) < 0.001f is reporting using mprintf now instead of assert * * Revision 2.8 2004/02/14 00:18:33 randomtiger * Please note that from now on OGL will only run with a registry set by Launcher v4. See forum for details. * OK, these changes effect a lot of file, I suggest everyone updates ASAP: * Removal of many files from project. * Removal of meanless Gr_bitmap_poly variable. * Removal of glide, directdraw, software modules all links to them, and all code specific to those paths. * Removal of redundant Fred paths that arent needed for Fred OGL. * Have seriously tidied the graphics initialisation code and added generic non standard mode functionality. * Fixed many D3D non standard mode bugs and brought OGL up to the same level. * Removed texture section support for D3D8, voodoo 2 and 3 cards will no longer run under fs2_open in D3D, same goes for any card with a maximum texture size less than 1024. * * Revision 2.7 2004/02/13 04:17:13 randomtiger * Turned off fog in OGL for Fred. * Simulated speech doesnt say tags marked by $ now. * The following are fixes to issues that came up testing TBP in fs2_open and fred2_open: * Changed vm_vec_mag and parse_tmap to fail gracefully on bad data. * Error now given on missing briefing icon and bad ship normal data. * Solved more species divide by zero error. * Fixed neb cube crash. * * Revision 2.6 2003/10/14 16:27:01 Goober5000 * commented back in null vector warning, because any code that flags this * is buggy and should be fixed (and it only pops up in release builds anyway) * --Goober5000 * * Revision 2.5 2003/10/13 05:57:48 Kazan * Removed a bunch of Useless *_printf()s in the rendering pipeline that were just slowing stuff down * Commented out the "warning null vector in vector normalize" crap * Added "beam no whack" flag for beams - said beams NEVER whack * Some reliability updates in FS2NetD * * Revision 2.4 2003/03/02 07:39:34 penguin * Added type for static var * - penguin * * Revision 2.3 2003/02/25 06:22:48 bobboau * fixed a bunch of fighter beam bugs, * most notabley the sound now works corectly, * and they have limeted range with atenuated damage (table option) * added bank specific compatabilities * * Revision 2.2 2002/12/07 01:37:41 bobboau * initial decals code, if you are worried a bug is being caused by the decals code it's only references are in, * collideshipweapon.cpp line 262, beam.cpp line 2771, and modelinterp.cpp line 2949. * it needs a better renderer, but is in prety good shape for now, * I also (think) I squashed a bug in the warpmodel code * * Revision 2.1 2002/08/01 01:41:06 penguin * The big include file move * * Revision 2.0 2002/06/03 04:02:24 penguin * Warpcore CVS sync * * Revision 1.2 2002/05/03 22:07:08 mharris * got some stuff to compile * * Revision 1.1 2002/05/02 18:03:09 mharris * Initial checkin - converted filenames and includes to lower case * * * 10 9/08/99 3:36p Jefff * Make sure argument of sqrt is positive in approach. * * 9 6/22/99 1:51p Danw * Some sanity for vm_vec_dist_to_line(...) * * 8 6/18/99 5:16p Dave * Added real beam weapon lighting. Fixed beam weapon sounds. Added MOTD * dialog to PXO screen. * * 7 4/28/99 11:13p Dave * Temporary checkin of artillery code. * * 6 1/24/99 11:37p Dave * First full rev of beam weapons. Very customizable. Removed some bogus * Int3()'s in low level net code. * * 5 1/12/99 12:53a Dave * More work on beam weapons - made collision detection very efficient - * collide against all object types properly - made 3 movement types * smooth. Put in test code to check for possible non-darkening pixels on * object textures. * * 4 1/06/99 2:24p Dave * Stubs and release build fixes. * * 3 11/18/98 4:10p Johnson * Add assert in vm_interpolate_matrix * * 2 10/07/98 10:53a Dave * Initial checkin. * * 1 10/07/98 10:49a Dave * * 72 9/11/98 10:10a Andsager * Optimize and rename matrix_decomp to vm_matrix_to_rot_axis_and_angle, * rename quatern_rot to vm_quaternion_rotate * * 71 5/01/98 2:25p Andsager * Fix bug in matrix interpolate (approach) when in rotvel is above limit. * * 70 4/07/98 3:10p Andsager * Make vm_test_parallel based on absolute difference. Optimize matrix * decomp. Fix potential bug in get_camera_limits with time = 0. * Optimize vm_forward_interpolate. * * 69 4/06/98 8:54a Andsager * Fix bug where matrix interpolate gets accel of 0 * * 68 4/03/98 5:34p Andsager * Optimized approach and away (used in matrix interpolation) * * 67 4/01/98 9:21p John * Made NDEBUG, optimized build with no warnings or errors. * * 66 3/23/98 1:12p Andsager * Reformat matrix inerpolation code * * 65 3/23/98 12:53p Andsager * * 63 3/09/98 3:51p Mike * More error checking. * * 62 2/26/98 3:28p John * Changed all sqrt's to use fl_sqrt. Took out isqrt function * * 61 2/02/98 5:12p Mike * Make vm_vec_rand_vec_quick() detect potential null vector condition and * recover. * * 60 1/20/98 9:47a Mike * Suppress optimized compiler warnings. * Some secondary weapon work. * * 59 12/17/97 5:44p Andsager * Change vm_matrix_interpolate so that it does not overshoot if optional * last parameter is 1 * * 58 9/30/97 5:04p Andsager * add vm_estimate_next_orientation * * 57 9/28/97 2:17p Andsager * added vm_project_point_onto_plane * * 56 9/09/97 10:15p Andsager * added vm_rotate_vec_to_body() and vm_rotate_vec_to_world() * * 55 8/20/97 5:33p Andsager * added vm_vec_projection_parallel and vm_vec_projection_onto_surface * * 54 8/20/97 9:51a Lawrance * swap x and y parameters in atan2_safe() to be consistent with library * atan2() * * 53 8/20/97 9:40a Lawrance * modified special case values in atan2_safe() * * 52 8/19/97 11:41p Lawrance * use atan2_safe() instead of atan2() * * 51 8/18/97 4:46p Hoffoss * Added global default axis vector constants. * * 50 8/03/97 3:54p Lawrance * added vm_find_bounding_sphere() * * 49 7/28/97 3:40p Andsager * remove duplicate vm_forwarad_interpolate * * 48 7/28/97 2:21p John * changed vecmat functions to not return src. Started putting in code * for inline vector math. Fixed some bugs with optimizer. * * 47 7/28/97 3:24p Andsager * * 46 7/28/97 2:41p Mike * Replace vm_forward_interpolate(). * * 45 7/28/97 1:18p Andsager * implement vm_fvec_matrix_interpolate(), which interpolates matrices on * xy and then z * * 44 7/28/97 10:28a Mike * Use forward_interpolate() to prevent weird banking behavior. * * Suppress a couple annoying mprints and clarify another. * * 43 7/24/97 5:24p Andsager * implement forward vector interpolation * * 42 7/10/97 8:52a Andsager * optimization and clarification of matrix_decomp() * * 41 7/09/97 2:54p Mike * More matrix_decomp optimization. * * 40 7/09/97 2:52p Mike * Optimize and error-prevent matrix_decomp(). * * 39 7/09/97 12:05a Mike * Error prevention in matrix_interpolate(). * * 38 7/07/97 11:58p Lawrance * add get_camera_limits() * * 37 7/03/97 11:22a Mike * Fix bug in matrix_interpolate. Was doing result = goal instead of * *result = *goal. * * 36 7/03/97 9:27a Mike * Hook in Dave's latest version of matrix_interpolate which doesn't jerk. * * 35 7/02/97 4:25p Mike * Add matrix_interpolate(), but don't call it. * * 34 7/01/97 3:27p Mike * Improve skill level support. * * 33 6/25/97 12:27p Hoffoss * Added some functions I needed for Fred. * * 32 5/21/97 8:49a Lawrance * added vm_vec_same() * * 31 4/15/97 4:00p Mike * Intermediate checkin caused by getting other files. Working on camera * slewing system. * * 30 4/10/97 3:20p Mike * Change hull damage to be like shields. * * 29 3/17/97 1:55p Hoffoss * Added function for error checking matrices. * * 28 3/11/97 10:46p Mike * Fix make_rand_vec_quick. Was generating values in -0.5..1.5 instead of * -1.0..1.0. * * 27 3/06/97 5:36p Mike * Change vec_normalize_safe() back to vec_normalize(). * Spruce up docking a bit. * * 26 3/06/97 10:56a Mike * Write error checking version of vm_vec_normalize(). * Fix resultant problems. * * 25 3/04/97 3:30p John * added function to interpolate an angle. * * 24 2/26/97 10:32a John * changed debris collision to use vm_vec_dist_squared. Changed * vm_vec_dist_squared to not int3 on bad values. * * 23 2/25/97 5:54p Hoffoss * Improved vector and matrix compare functions. * * 22 2/25/97 5:28p Hoffoss * added some commented out test code. * * 21 2/25/97 5:12p John * Added functions to see if two matrices or vectors are close. * * $NoKeywords: $ * */ #include #include #include "math/vecmat.h" #include "math/floating.h" #define SMALL_NUM 1e-7 #define SMALLER_NUM 1e-20 #define CONVERT_RADIANS 0.017453 // conversion factor from degrees to radians int index_largest (float a, float b, float c); // returns index of largest, NO_LARGEST if all less than SMALL_NUM vec3d vmd_zero_vector = ZERO_VECTOR; vec3d vmd_x_vector = { { { 1.0f, 0.0f, 0.0f } } }; vec3d vmd_y_vector = { { { 0.0f, 1.0f, 0.0f } } }; vec3d vmd_z_vector = { { { 0.0f, 0.0f, 1.0f } } }; matrix vmd_identity_matrix = IDENTITY_MATRIX; #define UNINITIALIZED_VALUE -12345678.9f // ----------------------------------------------------------- // atan2_safe() // // Wrapper around atan2() that used atan() to calculate angle. Safe // for optimized builds. Handles special cases when x == 0. // float atan2_safe(float y, float x) { float ang; // special case, x == 0 if ( x == 0 ) { if ( y == 0 ) ang = 0.0f; else if ( y > 0 ) ang = PI_2; else ang = -PI_2; return ang; } ang = (float)atan(y/x); if ( x < 0 ){ ang += PI; } return ang; } // --------------------------------------------------------------------- // vm_vec_component() // // finds projection of a vector along a unit (normalized) vector // float vm_vec_projection_parallel(vec3d *component, vec3d *src, vec3d *unit_vec) { float mag; Assert( vm_vec_mag(unit_vec) > 0.999f && vm_vec_mag(unit_vec) < 1.001f ); mag = vm_vec_dotprod(src, unit_vec); vm_vec_copy_scale(component, unit_vec, mag); return mag; } // --------------------------------------------------------------------- // vm_vec_projection_onto_plane() // // finds projection of a vector onto a plane specified by a unit normal vector // void vm_vec_projection_onto_plane(vec3d *projection, vec3d *src, vec3d *unit_normal) { float mag; Assert( vm_vec_mag(unit_normal) > 0.999f && vm_vec_mag(unit_normal) < 1.001f ); mag = vm_vec_dotprod(src, unit_normal); *projection = *src; vm_vec_scale_add2(projection, unit_normal, -mag); } // --------------------------------------------------------------------- // vm_vec_project_point_onto_plane() // // finds the point on a plane closest to a given point // moves the point in the direction of the plane normal until it is on the plane // void vm_project_point_onto_plane(vec3d *new_point, vec3d *point, vec3d *plane_normal, vec3d *plane_point) { float D; // plane constant in Ax+By+Cz+D = 0 or dot(X,n) - dot(Xp,n) = 0, so D = -dot(Xp,n) float dist; Assert( vm_vec_mag(plane_normal) > 0.999f && vm_vec_mag(plane_normal) < 1.001f ); D = -vm_vec_dotprod(plane_point, plane_normal); dist = vm_vec_dotprod(point, plane_normal) + D; *new_point = *point; vm_vec_scale_add2(new_point, plane_normal, -dist); } // Take abs(x), then sqrt. Could insert warning message if desired. float asqrt(float x) { if (x < 0.0f) return fl_sqrt(-x); else return fl_sqrt(x); } void vm_set_identity(matrix *m) { m->vec.rvec.xyz.x = 1.0f; m->vec.rvec.xyz.y = 0.0f; m->vec.rvec.xyz.z = 0.0f; m->vec.uvec.xyz.x = 0.0f; m->vec.uvec.xyz.y = 1.0f; m->vec.uvec.xyz.z = 0.0f; m->vec.fvec.xyz.x = 0.0f; m->vec.fvec.xyz.y = 0.0f; m->vec.fvec.xyz.z = 1.0f; } //adds two vectors, fills in dest, returns ptr to dest //ok for dest to equal either source, but should use vm_vec_add2() if so #ifndef _INLINE_VECMAT void vm_vec_add(vec3d *dest,vec3d *src0,vec3d *src1) { dest->xyz.x = src0->xyz.x + src1->xyz.x; dest->xyz.y = src0->xyz.y + src1->xyz.y; dest->xyz.z = src0->xyz.z + src1->xyz.z; } #endif //subs two vectors, fills in dest, returns ptr to dest //ok for dest to equal either source, but should use vm_vec_sub2() if so #ifndef _INLINE_VECMAT void vm_vec_sub(vec3d *dest,vec3d *src0,vec3d *src1) { dest->xyz.x = src0->xyz.x - src1->xyz.x; dest->xyz.y = src0->xyz.y - src1->xyz.y; dest->xyz.z = src0->xyz.z - src1->xyz.z; } #endif //adds one vector to another. returns ptr to dest //dest can equal source #ifndef _INLINE_VECMAT void vm_vec_add2(vec3d *dest,vec3d *src) { dest->xyz.x += src->xyz.x; dest->xyz.y += src->xyz.y; dest->xyz.z += src->xyz.z; } #endif //subs one vector from another, returns ptr to dest //dest can equal source #ifndef _INLINE_VECMAT void vm_vec_sub2(vec3d *dest,vec3d *src) { dest->xyz.x -= src->xyz.x; dest->xyz.y -= src->xyz.y; dest->xyz.z -= src->xyz.z; } #endif void vm_vert2vec(vertex *vert, vec3d *vec) //takes a vertex and makes it into a vector-Bobboau { vec->xyz.x = vert->x; vec->xyz.y = vert->y; vec->xyz.z = vert->z; } void vm_vec2vert(vec3d *vec, vertex *vert) //takes a vector and makes it into a vertex-Bobboau { vert->x = vec->xyz.x; vert->y = vec->xyz.y; vert->z = vec->xyz.z; } //averages n vectors. returns ptr to dest //dest can equal either source vec3d *vm_vec_avg_n(vec3d *dest, int n, vec3d src[]) { float x=0,y=0,z=0; for(int i = 0; ixyz.x = x / n; dest->xyz.y = y / n; dest->xyz.z = z / n; return dest; } //averages two vectors. returns ptr to dest //dest can equal either source vec3d *vm_vec_avg(vec3d *dest,vec3d *src0,vec3d *src1) { dest->xyz.x = (src0->xyz.x + src1->xyz.x) * 0.5f; dest->xyz.y = (src0->xyz.y + src1->xyz.y) * 0.5f; dest->xyz.z = (src0->xyz.z + src1->xyz.z) * 0.5f; return dest; } //averages four vectors. returns ptr to dest //dest can equal any source vec3d *vm_vec_avg3(vec3d *dest,vec3d *src0,vec3d *src1,vec3d *src2) { dest->xyz.x = (src0->xyz.x + src1->xyz.x + src2->xyz.x) * 0.333333333f; dest->xyz.y = (src0->xyz.y + src1->xyz.y + src2->xyz.y) * 0.333333333f; dest->xyz.z = (src0->xyz.z + src1->xyz.z + src2->xyz.z) * 0.333333333f; return dest; } //averages four vectors. returns ptr to dest //dest can equal any source vec3d *vm_vec_avg4(vec3d *dest,vec3d *src0,vec3d *src1,vec3d *src2,vec3d *src3) { dest->xyz.x = (src0->xyz.x + src1->xyz.x + src2->xyz.x + src3->xyz.x) * 0.25f; dest->xyz.y = (src0->xyz.y + src1->xyz.y + src2->xyz.y + src3->xyz.y) * 0.25f; dest->xyz.z = (src0->xyz.z + src1->xyz.z + src2->xyz.z + src3->xyz.z) * 0.25f; return dest; } //scales a vector in place. returns ptr to vector #ifndef _INLINE_VECMAT void vm_vec_scale(vec3d *dest,float s) { dest->xyz.x = dest->xyz.x * s; dest->xyz.y = dest->xyz.y * s; dest->xyz.z = dest->xyz.z * s; } #endif //scales and copies a vector. returns ptr to dest #ifndef _INLINE_VECMAT void vm_vec_copy_scale(vec3d *dest,vec3d *src,float s) { dest->xyz.x = src->xyz.x*s; dest->xyz.y = src->xyz.y*s; dest->xyz.z = src->xyz.z*s; } #endif //scales a vector, adds it to another, and stores in a 3rd vector //dest = src1 + k * src2 #ifndef _INLINE_VECMAT void vm_vec_scale_add(vec3d *dest,vec3d *src1,vec3d *src2,float k) { dest->xyz.x = src1->xyz.x + src2->xyz.x*k; dest->xyz.y = src1->xyz.y + src2->xyz.y*k; dest->xyz.z = src1->xyz.z + src2->xyz.z*k; } #endif //scales a vector, subtracts it to another, and stores in a 3rd vector //dest = src1 - k * src2 void vm_vec_scale_sub(vec3d *dest, vec3d *src1, vec3d *src2, float k) { dest->xyz.x = src1->xyz.x - src2->xyz.x*k; dest->xyz.y = src1->xyz.y - src2->xyz.y*k; dest->xyz.z = src1->xyz.z - src2->xyz.z*k; } //scales a vector and adds it to another //dest += k * src #ifndef _INLINE_VECMAT void vm_vec_scale_add2(vec3d *dest,vec3d *src,float k) { dest->xyz.x += src->xyz.x*k; dest->xyz.y += src->xyz.y*k; dest->xyz.z += src->xyz.z*k; } #endif //scales a vector and adds it to another //dest += k * src #ifndef _INLINE_VECMAT void vm_vec_scale_sub2(vec3d *dest,vec3d *src,float k) { dest->xyz.x -= src->xyz.x*k; dest->xyz.y -= src->xyz.y*k; dest->xyz.z -= src->xyz.z*k; } #endif //scales a vector in place, taking n/d for scale. returns ptr to vector //dest *= n/d #ifndef _INLINE_VECMAT void vm_vec_scale2(vec3d *dest,float n,float d) { d = 1.0f/d; dest->xyz.x = dest->xyz.x* n * d; dest->xyz.y = dest->xyz.y* n * d; dest->xyz.z = dest->xyz.z* n * d; } #endif //returns dot product of 2 vectors #ifndef _INLINE_VECMAT float vm_vec_dotprod(vec3d *v0,vec3d *v1) { return (v1->xyz.x*v0->xyz.x)+(v1->xyz.y*v0->xyz.y)+(v1->xyz.z*v0->xyz.z); } #endif //returns dot product of and vector #ifndef _INLINE_VECMAT float vm_vec_dot3(float x,float y,float z,vec3d *v) { return (x*v->xyz.x)+(y*v->xyz.y)+(z*v->xyz.z); } #endif //returns magnitude of a vector float vm_vec_mag(vec3d *v) { float x,y,z,mag1, mag2; x = v->xyz.x*v->xyz.x; y = v->xyz.y*v->xyz.y; z = v->xyz.z*v->xyz.z; mag1 = x+y+z; if ( mag1 < 0.0 ) return 0; mag2 = fl_sqrt(mag1); if ( mag2 < 0.0 ) Int3(); return mag2; } //returns squared magnitude of a vector, useful if you want to compare distances float vm_vec_mag_squared(vec3d *v) { float x,y,z,mag1; x = v->xyz.x*v->xyz.x; y = v->xyz.y*v->xyz.y; z = v->xyz.z*v->xyz.z; mag1 = x+y+z; return mag1; } float vm_vec_dist_squared(vec3d *v0, vec3d *v1) { float dx, dy, dz; dx = v0->xyz.x - v1->xyz.x; dy = v0->xyz.y - v1->xyz.y; dz = v0->xyz.z - v1->xyz.z; return dx*dx + dy*dy + dz*dz; } //computes the distance between two points. (does sub and mag) float vm_vec_dist(vec3d *v0,vec3d *v1) { float t1; vec3d t; vm_vec_sub(&t,v0,v1); t1 = vm_vec_mag(&t); return t1; } //computes an approximation of the magnitude of the vector //uses dist = largest + next_largest*3/8 + smallest*3/16 float vm_vec_mag_quick(vec3d *v) { float a,b,c,bc, t; if ( v->xyz.x < 0.0 ) a = -v->xyz.x; else a = v->xyz.x; if ( v->xyz.y < 0.0 ) b = -v->xyz.y; else b = v->xyz.y; if ( v->xyz.z < 0.0 ) c = -v->xyz.z; else c = v->xyz.z; if (a < b) { float t=a; a=b; b=t; } if (b < c) { float t=b; b=c; c=t; if (a < b) { float t=a; a=b; b=t; } } bc = (b * 0.25f) + (c * 0.125f); t = a + bc + (bc * 0.5f); return t; } //computes an approximation of the distance between two points. //uses dist = largest + next_largest*3/8 + smallest*3/16 float vm_vec_dist_quick(vec3d *v0,vec3d *v1) { vec3d t; vm_vec_sub(&t,v0,v1); return vm_vec_mag_quick(&t); } //normalize a vector. returns mag of source vec float vm_vec_copy_normalize(vec3d *dest,vec3d *src) { float m; m = vm_vec_mag(src); // Mainly here to trap attempts to normalize a null vector. if (m <= 0.0f) { // static int been_warned2 = false;//added this so the warning could be sounded and you can still get on with playing-Bobboau // if(!been_warned2) { Warning(LOCATION, "Null vec3d in vec3d normalize.\n" "Trace out of vecmat.cpp and find offending code.\n"); // been_warned2 = true; } dest->xyz.x = 1.0f; dest->xyz.y = 0.0f; dest->xyz.z = 0.0f; return 1.0f; } float im = 1.0f / m; dest->xyz.x = src->xyz.x * im; dest->xyz.y = src->xyz.y * im; dest->xyz.z = src->xyz.z * im; return m; } //normalize a vector. returns mag of source vec float vm_vec_normalize(vec3d *v) { float t; t = vm_vec_copy_normalize(v,v); return t; } // Normalize a vector. // If vector is 0,0,0, return 1,0,0. // Don't generate a Warning(). // returns mag of source vec float vm_vec_normalize_safe(vec3d *v) { float m; m = vm_vec_mag(v); // Mainly here to trap attempts to normalize a null vector. if (m <= 0.0f) { v->xyz.x = 1.0f; v->xyz.y = 0.0f; v->xyz.z = 0.0f; return 1.0f; } float im = 1.0f / m; v->xyz.x *= im; v->xyz.y *= im; v->xyz.z *= im; return m; } //returns approximation of 1/magnitude of a vector float vm_vec_imag(vec3d *v) { // return 1.0f / sqrt( (v->xyz.x*v->xyz.x)+(v->xyz.y*v->xyz.y)+(v->xyz.z*v->xyz.z) ); return fl_isqrt( (v->xyz.x*v->xyz.x)+(v->xyz.y*v->xyz.y)+(v->xyz.z*v->xyz.z) ); } //normalize a vector. returns 1/mag of source vec. uses approx 1/mag float vm_vec_copy_normalize_quick(vec3d *dest,vec3d *src) { // return vm_vec_copy_normalize(dest, src); float im; im = vm_vec_imag(src); Assert(im > 0.0f); dest->xyz.x = src->xyz.x*im; dest->xyz.y = src->xyz.y*im; dest->xyz.z = src->xyz.z*im; return 1.0f/im; } //normalize a vector. returns mag of source vec. uses approx mag float vm_vec_normalize_quick(vec3d *src) { // return vm_vec_normalize(src); float im; im = vm_vec_imag(src); Assert(im > 0.0f); src->xyz.x = src->xyz.x*im; src->xyz.y = src->xyz.y*im; src->xyz.z = src->xyz.z*im; return 1.0f/im; } //normalize a vector. returns mag of source vec. uses approx mag float vm_vec_copy_normalize_quick_mag(vec3d *dest,vec3d *src) { // return vm_vec_copy_normalize(dest, src); float m; m = vm_vec_mag_quick(src); Assert(m > 0.0f); float im = 1.0f / m; dest->xyz.x = src->xyz.x * im; dest->xyz.y = src->xyz.y * im; dest->xyz.z = src->xyz.z * im; return m; } //normalize a vector. returns mag of source vec. uses approx mag float vm_vec_normalize_quick_mag(vec3d *v) { // return vm_vec_normalize(v); float m; m = vm_vec_mag_quick(v); Assert(m > 0.0f); v->xyz.x = v->xyz.x*m; v->xyz.y = v->xyz.y*m; v->xyz.z = v->xyz.z*m; return m; } //return the normalized direction vector between two points //dest = normalized(end - start). Returns mag of direction vector //NOTE: the order of the parameters matches the vector subtraction float vm_vec_normalized_dir(vec3d *dest,vec3d *end,vec3d *start) { float t; vm_vec_sub(dest,end,start); // VECMAT-ERROR: NULL VEC3D (end == start) t = vm_vec_normalize_safe(dest); return t; } //return the normalized direction vector between two points //dest = normalized(end - start). Returns mag of direction vector //NOTE: the order of the parameters matches the vector subtraction float vm_vec_normalized_dir_quick(vec3d *dest,vec3d *end,vec3d *start) { vm_vec_sub(dest,end,start); return vm_vec_normalize_quick(dest); } //return the normalized direction vector between two points //dest = normalized(end - start). Returns mag of direction vector //NOTE: the order of the parameters matches the vector subtraction float vm_vec_normalized_dir_quick_mag(vec3d *dest,vec3d *end,vec3d *start) { float t; vm_vec_sub(dest,end,start); t = vm_vec_normalize_quick_mag(dest); return t; } //computes surface normal from three points. result is normalized //returns ptr to dest //dest CANNOT equal either source vec3d *vm_vec_normal(vec3d *dest,vec3d *p0,vec3d *p1,vec3d *p2) { vm_vec_perp(dest,p0,p1,p2); vm_vec_normalize(dest); return dest; } //computes cross product of two vectors. //Note: this magnitude of the resultant vector is the //product of the magnitudes of the two source vectors. This means it is //quite easy for this routine to overflow and underflow. Be careful that //your inputs are ok. vec3d *vm_vec_crossprod(vec3d *dest,vec3d *src0,vec3d *src1) { dest->xyz.x = (src0->xyz.y * src1->xyz.z) - (src0->xyz.z * src1->xyz.y); dest->xyz.y = (src0->xyz.z * src1->xyz.x) - (src0->xyz.x * src1->xyz.z); dest->xyz.z = (src0->xyz.x * src1->xyz.y) - (src0->xyz.y * src1->xyz.x); return dest; } // test if 2 vectors are parallel or not. int vm_test_parallel(vec3d *src0, vec3d *src1) { 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) ) { return 1; } else { return 0; } } //computes non-normalized surface normal from three points. //returns ptr to dest //dest CANNOT equal either source vec3d *vm_vec_perp(vec3d *dest,vec3d *p0,vec3d *p1,vec3d *p2) { vec3d t0,t1; vm_vec_sub(&t0,p1,p0); vm_vec_sub(&t1,p2,p1); return vm_vec_crossprod(dest,&t0,&t1); } //computes the delta angle between two vectors. //vectors need not be normalized. if they are, call vm_vec_delta_ang_norm() //the forward vector (third parameter) can be NULL, in which case the absolute //value of the angle in returned. Otherwise the angle around that vector is //returned. float vm_vec_delta_ang(vec3d *v0,vec3d *v1,vec3d *fvec) { float t; vec3d t0,t1,t2; vm_vec_copy_normalize(&t0,v0); vm_vec_copy_normalize(&t1,v1); if (NULL == fvec) { t = vm_vec_delta_ang_norm(&t0, &t1, NULL); } else { vm_vec_copy_normalize(&t2,fvec); t = vm_vec_delta_ang_norm(&t0,&t1,&t2); } return t; } //computes the delta angle between two normalized vectors. float vm_vec_delta_ang_norm(vec3d *v0,vec3d *v1,vec3d *fvec) { float a; vec3d t; a = acosf(vm_vec_dot(v0,v1)); if (fvec) { vm_vec_cross(&t,v0,v1); if ( vm_vec_dotprod(&t,fvec) < 0.0 ) { a = -a; } } return a; } matrix *sincos_2_matrix(matrix *m,float sinp,float cosp,float sinb,float cosb,float sinh,float cosh) { float sbsh,cbch,cbsh,sbch; sbsh = sinb*sinh; cbch = cosb*cosh; cbsh = cosb*sinh; sbch = sinb*cosh; m->vec.rvec.xyz.x = cbch + sinp*sbsh; //m1 m->vec.uvec.xyz.z = sbsh + sinp*cbch; //m8 m->vec.uvec.xyz.x = sinp*cbsh - sbch; //m2 m->vec.rvec.xyz.z = sinp*sbch - cbsh; //m7 m->vec.fvec.xyz.x = sinh*cosp; //m3 m->vec.rvec.xyz.y = sinb*cosp; //m4 m->vec.uvec.xyz.y = cosb*cosp; //m5 m->vec.fvec.xyz.z = cosh*cosp; //m9 m->vec.fvec.xyz.y = -sinp; //m6 return m; } //computes a matrix from a set of three angles. returns ptr to matrix matrix *vm_angles_2_matrix(matrix *m,angles *a) { matrix * t; float sinp,cosp,sinb,cosb,sinh,cosh; sinp = sinf(a->p); cosp = cosf(a->p); sinb = sinf(a->b); cosb = cosf(a->b); sinh = sinf(a->h); cosh = cosf(a->h); t = sincos_2_matrix(m,sinp,cosp,sinb,cosb,sinh,cosh); return t; } //computes a matrix from one angle. // angle_index = 0,1,2 for p,b,h matrix *vm_angle_2_matrix(matrix *m, float a, int angle_index) { matrix * t; float sinp,cosp,sinb,cosb,sinh,cosh; sinp = sinf(0.0f); cosp = cosf(0.0f); sinb = sinf(0.0f); cosb = cosf(0.0f); sinh = sinf(0.0f); cosh = cosf(0.0f); switch (angle_index) { case 0: sinp = sinf(a); cosp = cosf(a); break; case 1: sinb = sinf(a); cosb = cosf(a); break; case 2: sinh = sinf(a); cosh = cosf(a); break; } t = sincos_2_matrix(m,sinp,cosp,sinb,cosb,sinh,cosh); return t; } //computes a matrix from a forward vector and an angle matrix *vm_vec_ang_2_matrix(matrix *m,vec3d *v,float a) { matrix * t; float sinb,cosb,sinp,cosp,sinh,cosh; sinb = sinf(a); cosb = cosf(a); sinp = -v->xyz.y; cosp = fl_sqrt(1.0f - sinp*sinp); sinh = v->xyz.x / cosp; cosh = v->xyz.z / cosp; t = sincos_2_matrix(m,sinp,cosp,sinb,cosb,sinh,cosh); return t; } //computes a matrix from one or more vectors. The forward vector is required, //with the other two being optional. If both up & right vectors are passed, //the up vector is used. If only the forward vector is passed, a bank of //zero is assumed //returns ptr to matrix matrix *vm_vector_2_matrix(matrix *m,vec3d *fvec,vec3d *uvec,vec3d *rvec) { vec3d *xvec=&m->vec.rvec,*yvec=&m->vec.uvec,*zvec=&m->vec.fvec; Assert(fvec != NULL); // This had been commented out, but that's bogus. Code below relies on a valid zvec. if (vm_vec_copy_normalize(zvec,fvec) == 0.0f) { Assert(0); return m; } if (uvec == NULL) { if (rvec == NULL) { //just forward vec bad_vector2: ; if ((zvec->xyz.x==0.0f) && (zvec->xyz.z==0.0f)) { //forward vec is straight up or down m->vec.rvec.xyz.x = 1.0f; m->vec.uvec.xyz.z = (zvec->xyz.y<0.0)?1.0f:-1.0f; m->vec.rvec.xyz.y = m->vec.rvec.xyz.z = m->vec.uvec.xyz.x = m->vec.uvec.xyz.y = 0.0f; } else { //not straight up or down xvec->xyz.x = zvec->xyz.z; xvec->xyz.y = 0.0f; xvec->xyz.z = -zvec->xyz.x; vm_vec_normalize(xvec); vm_vec_crossprod(yvec,zvec,xvec); } } else { //use right vec if (vm_vec_copy_normalize(xvec,rvec) == 0.0f) goto bad_vector2; vm_vec_crossprod(yvec,zvec,xvec); //normalize new perpendicular vector if (vm_vec_normalize(yvec) == 0.0f) goto bad_vector2; //now recompute right vector, in case it wasn't entirely perpendiclar vm_vec_crossprod(xvec,yvec,zvec); } } else { //use up vec if (vm_vec_copy_normalize(yvec,uvec) == 0.0f) goto bad_vector2; vm_vec_crossprod(xvec,yvec,zvec); //normalize new perpendicular vector if (vm_vec_normalize(xvec) == 0.0f) goto bad_vector2; //now recompute up vector, in case it wasn't entirely perpendiclar vm_vec_crossprod(yvec,zvec,xvec); } return m; } //quicker version of vm_vector_2_matrix() that takes normalized vectors matrix *vm_vector_2_matrix_norm(matrix *m,vec3d *fvec,vec3d *uvec,vec3d *rvec) { vec3d *xvec=&m->vec.rvec,*yvec=&m->vec.uvec,*zvec=&m->vec.fvec; Assert(fvec != NULL); *zvec = *fvec; if (uvec == NULL) { if (rvec == NULL) { //just forward vec bad_vector2: ; if ((zvec->xyz.x==0.0f) && (zvec->xyz.z==0.0f)) { //forward vec is straight up or down m->vec.rvec.xyz.x = 1.0f; m->vec.uvec.xyz.z = (zvec->xyz.y<0.0f)?1.0f:-1.0f; m->vec.rvec.xyz.y = m->vec.rvec.xyz.z = m->vec.uvec.xyz.x = m->vec.uvec.xyz.y = 0.0f; } else { //not straight up or down xvec->xyz.x = zvec->xyz.z; xvec->xyz.y = 0.0f; xvec->xyz.z = -zvec->xyz.x; vm_vec_normalize(xvec); vm_vec_crossprod(yvec,zvec,xvec); } } else { //use right vec vm_vec_crossprod(yvec,zvec,xvec); //normalize new perpendicular vector if (vm_vec_normalize(yvec) == 0.0f) goto bad_vector2; //now recompute right vector, in case it wasn't entirely perpendiclar vm_vec_crossprod(xvec,yvec,zvec); } } else { //use up vec vm_vec_crossprod(xvec,yvec,zvec); //normalize new perpendicular vector if (vm_vec_normalize(xvec) == 0.0f) goto bad_vector2; //now recompute up vector, in case it wasn't entirely perpendiclar vm_vec_crossprod(yvec,zvec,xvec); } return m; } //rotates a vector through a matrix. returns ptr to dest vector //dest CANNOT equal source // // Goober5000: FYI, the result of rotating a normalized vector through a rotation matrix will // also be a normalized vector. It took me awhile to verify online that this was true. ;) vec3d *vm_vec_rotate(vec3d *dest,vec3d *src,matrix *m) { dest->xyz.x = (src->xyz.x*m->vec.rvec.xyz.x)+(src->xyz.y*m->vec.rvec.xyz.y)+(src->xyz.z*m->vec.rvec.xyz.z); dest->xyz.y = (src->xyz.x*m->vec.uvec.xyz.x)+(src->xyz.y*m->vec.uvec.xyz.y)+(src->xyz.z*m->vec.uvec.xyz.z); dest->xyz.z = (src->xyz.x*m->vec.fvec.xyz.x)+(src->xyz.y*m->vec.fvec.xyz.y)+(src->xyz.z*m->vec.fvec.xyz.z); return dest; } //rotates a vector through the transpose of the given matrix. //returns ptr to dest vector //dest CANNOT equal source // This is a faster replacement for this common code sequence: // vm_copy_transpose_matrix(&tempm,src_matrix); // vm_vec_rotate(dst_vec,src_vect,&tempm); // Replace with: // vm_vec_unrotate(dst_vec,src_vect, src_matrix) // // THIS DOES NOT ACTUALLY TRANSPOSE THE SOURCE MATRIX!!! So if // you need it transposed later on, you should use the // vm_vec_transpose() / vm_vec_rotate() technique. // // Goober5000: FYI, the result of rotating a normalized vector through a rotation matrix will // also be a normalized vector. It took me awhile to verify online that this was true. ;) vec3d *vm_vec_unrotate(vec3d *dest,vec3d *src,matrix *m) { dest->xyz.x = (src->xyz.x*m->vec.rvec.xyz.x)+(src->xyz.y*m->vec.uvec.xyz.x)+(src->xyz.z*m->vec.fvec.xyz.x); dest->xyz.y = (src->xyz.x*m->vec.rvec.xyz.y)+(src->xyz.y*m->vec.uvec.xyz.y)+(src->xyz.z*m->vec.fvec.xyz.y); dest->xyz.z = (src->xyz.x*m->vec.rvec.xyz.z)+(src->xyz.y*m->vec.uvec.xyz.z)+(src->xyz.z*m->vec.fvec.xyz.z); return dest; } //transpose a matrix in place. returns ptr to matrix matrix *vm_transpose_matrix(matrix *m) { float t; t = m->vec.uvec.xyz.x; m->vec.uvec.xyz.x = m->vec.rvec.xyz.y; m->vec.rvec.xyz.y = t; t = m->vec.fvec.xyz.x; m->vec.fvec.xyz.x = m->vec.rvec.xyz.z; m->vec.rvec.xyz.z = t; t = m->vec.fvec.xyz.y; m->vec.fvec.xyz.y = m->vec.uvec.xyz.z; m->vec.uvec.xyz.z = t; return m; } //copy and transpose a matrix. returns ptr to matrix //dest CANNOT equal source. use vm_transpose_matrix() if this is the case matrix *vm_copy_transpose_matrix(matrix *dest,matrix *src) { Assert(dest != src); dest->vec.rvec.xyz.x = src->vec.rvec.xyz.x; dest->vec.rvec.xyz.y = src->vec.uvec.xyz.x; dest->vec.rvec.xyz.z = src->vec.fvec.xyz.x; dest->vec.uvec.xyz.x = src->vec.rvec.xyz.y; dest->vec.uvec.xyz.y = src->vec.uvec.xyz.y; dest->vec.uvec.xyz.z = src->vec.fvec.xyz.y; dest->vec.fvec.xyz.x = src->vec.rvec.xyz.z; dest->vec.fvec.xyz.y = src->vec.uvec.xyz.z; dest->vec.fvec.xyz.z = src->vec.fvec.xyz.z; return dest; } //mulitply 2 matrices, fill in dest. returns ptr to dest //dest CANNOT equal either source matrix *vm_matrix_x_matrix(matrix *dest,matrix *src0,matrix *src1) { Assert(dest!=src0 && dest!=src1); dest->vec.rvec.xyz.x = vm_vec_dot3(src0->vec.rvec.xyz.x,src0->vec.uvec.xyz.x,src0->vec.fvec.xyz.x, &src1->vec.rvec); dest->vec.uvec.xyz.x = vm_vec_dot3(src0->vec.rvec.xyz.x,src0->vec.uvec.xyz.x,src0->vec.fvec.xyz.x, &src1->vec.uvec); dest->vec.fvec.xyz.x = vm_vec_dot3(src0->vec.rvec.xyz.x,src0->vec.uvec.xyz.x,src0->vec.fvec.xyz.x, &src1->vec.fvec); dest->vec.rvec.xyz.y = vm_vec_dot3(src0->vec.rvec.xyz.y,src0->vec.uvec.xyz.y,src0->vec.fvec.xyz.y, &src1->vec.rvec); dest->vec.uvec.xyz.y = vm_vec_dot3(src0->vec.rvec.xyz.y,src0->vec.uvec.xyz.y,src0->vec.fvec.xyz.y, &src1->vec.uvec); dest->vec.fvec.xyz.y = vm_vec_dot3(src0->vec.rvec.xyz.y,src0->vec.uvec.xyz.y,src0->vec.fvec.xyz.y, &src1->vec.fvec); dest->vec.rvec.xyz.z = vm_vec_dot3(src0->vec.rvec.xyz.z,src0->vec.uvec.xyz.z,src0->vec.fvec.xyz.z, &src1->vec.rvec); dest->vec.uvec.xyz.z = vm_vec_dot3(src0->vec.rvec.xyz.z,src0->vec.uvec.xyz.z,src0->vec.fvec.xyz.z, &src1->vec.uvec); dest->vec.fvec.xyz.z = vm_vec_dot3(src0->vec.rvec.xyz.z,src0->vec.uvec.xyz.z,src0->vec.fvec.xyz.z, &src1->vec.fvec); return dest; } //extract angles from a matrix angles *vm_extract_angles_matrix(angles *a,matrix *m) { float sinh,cosh,cosp; if (m->vec.fvec.xyz.x==0.0f && m->vec.fvec.xyz.z==0.0f) //zero head a->h = 0.0f; else // a->h = (float)atan2(m->vec.fvec.xyz.z,m->vec.fvec.xyz.x); a->h = atan2_safe(m->vec.fvec.xyz.x,m->vec.fvec.xyz.z); sinh = sinf(a->h); cosh = cosf(a->h); if (fl_abs(sinh) > fl_abs(cosh)) //sine is larger, so use it cosp = m->vec.fvec.xyz.x*sinh; else //cosine is larger, so use it cosp = m->vec.fvec.xyz.z*cosh; if (cosp==0.0f && m->vec.fvec.xyz.y==0.0f) a->p = 0.0f; else // a->p = (float)atan2(cosp,-m->vec.fvec.xyz.y); a->p = atan2_safe(-m->vec.fvec.xyz.y, cosp); if (cosp == 0.0f) //the cosine of pitch is zero. we're pitched straight up. say no bank a->b = 0.0f; else { float sinb,cosb; sinb = m->vec.rvec.xyz.y/cosp; cosb = m->vec.uvec.xyz.y/cosp; if (sinb==0.0f && cosb==0.0f) a->b = 0.0f; else // a->b = (float)atan2(cosb,sinb); a->b = atan2_safe(sinb,cosb); } return a; } //extract heading and pitch from a vector, assuming bank==0 angles *vm_extract_angles_vector_normalized(angles *a,vec3d *v) { a->b = 0.0f; //always zero bank a->p = asinf(-v->xyz.y); if (v->xyz.x==0.0f && v->xyz.z==0.0f) a->h = 0.0f; else a->h = atan2_safe(v->xyz.z,v->xyz.x); return a; } //extract heading and pitch from a vector, assuming bank==0 angles *vm_extract_angles_vector(angles *a,vec3d *v) { vec3d t; if (vm_vec_copy_normalize(&t,v) != 0.0f) vm_extract_angles_vector_normalized(a,&t); return a; } //compute the distance from a point to a plane. takes the normalized normal //of the plane (ebx), a point on the plane (edi), and the point to check (esi). //returns distance in eax //distance is signed, so negative dist is on the back of the plane float vm_dist_to_plane(vec3d *checkp,vec3d *norm,vec3d *planep) { float t1; vec3d t; vm_vec_sub(&t,checkp,planep); t1 = vm_vec_dot(&t,norm); return t1; } // Given mouse movement in dx, dy, returns a 3x3 rotation matrix in RotMat. // Taken from Graphics Gems III, page 51, "The Rolling Ball" // Example: //if ( (Mouse.dx!=0) || (Mouse.dy!=0) ) { // GetMouseRotation( Mouse.dx, Mouse.dy, &MouseRotMat ); // vm_matrix_x_matrix(&tempm,&LargeView.ev_matrix,&MouseRotMat); // LargeView.ev_matrix = tempm; //} void vm_trackball( int idx, int idy, matrix * RotMat ) { float dr, cos_theta, sin_theta, denom, cos_theta1; float Radius = 100.0f; float dx,dy; float dxdr,dydr; idy *= -1; dx = (float)idx; dy = (float)idy; dr = fl_sqrt(dx*dx+dy*dy); denom = fl_sqrt(Radius*Radius+dr*dr); cos_theta = Radius/denom; sin_theta = dr/denom; cos_theta1 = 1.0f - cos_theta; dxdr = dx/dr; dydr = dy/dr; RotMat->vec.rvec.xyz.x = cos_theta + (dydr*dydr)*cos_theta1; RotMat->vec.uvec.xyz.x = - ((dxdr*dydr)*cos_theta1); RotMat->vec.fvec.xyz.x = (dxdr*sin_theta); RotMat->vec.rvec.xyz.y = RotMat->vec.uvec.xyz.x; RotMat->vec.uvec.xyz.y = cos_theta + ((dxdr*dxdr)*cos_theta1); RotMat->vec.fvec.xyz.y = (dydr*sin_theta); RotMat->vec.rvec.xyz.z = -RotMat->vec.fvec.xyz.x; RotMat->vec.uvec.xyz.z = -RotMat->vec.fvec.xyz.y; RotMat->vec.fvec.xyz.z = cos_theta; } // Compute the outer product of A = A * transpose(A). 1x3 vector becomes 3x3 matrix. void vm_vec_outer_product(matrix *mat, vec3d *vec) { mat->vec.rvec.xyz.x = vec->xyz.x * vec->xyz.x; mat->vec.rvec.xyz.y = vec->xyz.x * vec->xyz.y; mat->vec.rvec.xyz.z = vec->xyz.x * vec->xyz.z; mat->vec.uvec.xyz.x = vec->xyz.y * vec->xyz.x; mat->vec.uvec.xyz.y = vec->xyz.y * vec->xyz.y; mat->vec.uvec.xyz.z = vec->xyz.y * vec->xyz.z; mat->vec.fvec.xyz.x = vec->xyz.z * vec->xyz.x; mat->vec.fvec.xyz.y = vec->xyz.z * vec->xyz.y; mat->vec.fvec.xyz.z = vec->xyz.z * vec->xyz.z; } // Find the point on the line between p0 and p1 that is nearest to int_pnt. // Stuff result in nearest_point. // Uses algorithm from page 148 of Strang, Linear Algebra and Its Applications. // Returns value indicating whether *nearest_point is between *p0 and *p1. // 0.0f means *nearest_point is *p0, 1.0f means it's *p1. 2.0f means it's beyond p1 by 2x. // -1.0f means it's "before" *p0 by 1x. float find_nearest_point_on_line(vec3d *nearest_point, vec3d *p0, vec3d *p1, vec3d *int_pnt) { vec3d norm, xlated_int_pnt, projected_point; matrix mat; float mag, dot; vm_vec_sub(&norm, p1, p0); vm_vec_sub(&xlated_int_pnt, int_pnt, p0); if (IS_VEC_NULL(&norm)) { *nearest_point = *int_pnt; return 9999.9f; } mag = vm_vec_normalize(&norm); // Normalize vector so we don't have to divide by dot product. if (mag < 0.01f) { *nearest_point = *int_pnt; return 9999.9f; // Warning(LOCATION, "Very small magnitude in find_nearest_point_on_line.\n"); } vm_vec_outer_product(&mat, &norm); vm_vec_rotate(&projected_point, &xlated_int_pnt, &mat); vm_vec_add(nearest_point, &projected_point, p0); dot = vm_vec_dot(&norm, &projected_point); return dot/mag; } //make sure matrix is orthogonal //computes a matrix from one or more vectors. The forward vector is required, //with the other two being optional. If both up & right vectors are passed, //the up vector is used. If only the forward vector is passed, a bank of //zero is assumed //returns ptr to matrix void vm_orthogonalize_matrix(matrix *m_src) { float umag, rmag; matrix tempm; matrix * m = &tempm; if (vm_vec_copy_normalize(&m->vec.fvec,&m_src->vec.fvec) == 0.0f) { Error( LOCATION, "forward vec should not be zero-length" ); } umag = vm_vec_mag(&m_src->vec.uvec); rmag = vm_vec_mag(&m_src->vec.rvec); if (umag <= 0.0f) { // no up vector to use.. if (rmag <= 0.0f) { // no right vector either, so make something up if (!m->vec.fvec.xyz.x && !m->vec.fvec.xyz.z && m->vec.fvec.xyz.y) // vertical vector vm_vec_make(&m->vec.uvec, 0.0f, 0.0f, 1.0f); else vm_vec_make(&m->vec.uvec, 0.0f, 1.0f, 0.0f); } else { // use the right vector to figure up vector vm_vec_crossprod(&m->vec.uvec, &m->vec.fvec, &m_src->vec.rvec); if (vm_vec_normalize(&m->vec.uvec) == 0.0f) Error( LOCATION, "Bad vector!" ); } } else { // use source up vector vm_vec_copy_normalize(&m->vec.uvec, &m_src->vec.uvec); } // use forward and up vectors as good vectors to calculate right vector vm_vec_crossprod(&m->vec.rvec, &m->vec.uvec, &m->vec.fvec); //normalize new perpendicular vector if (vm_vec_normalize(&m->vec.rvec) == 0.0f) Error( LOCATION, "Bad vector!" ); //now recompute up vector, in case it wasn't entirely perpendiclar vm_vec_crossprod(&m->vec.uvec, &m->vec.fvec, &m->vec.rvec); *m_src = tempm; } // like vm_orthogonalize_matrix(), except that zero vectors can exist within the // matrix without causing problems. Valid vectors will be created where needed. void vm_fix_matrix(matrix *m) { float fmag, umag, rmag; fmag = vm_vec_mag(&m->vec.fvec); umag = vm_vec_mag(&m->vec.uvec); rmag = vm_vec_mag(&m->vec.rvec); if (fmag <= 0.0f) { if ((umag > 0.0f) && (rmag > 0.0f) && !vm_test_parallel(&m->vec.uvec, &m->vec.rvec)) { vm_vec_crossprod(&m->vec.fvec, &m->vec.uvec, &m->vec.rvec); vm_vec_normalize(&m->vec.fvec); } else if (umag > 0.0f) { if (!m->vec.uvec.xyz.x && !m->vec.uvec.xyz.y && m->vec.uvec.xyz.z) // z vector vm_vec_make(&m->vec.fvec, 1.0f, 0.0f, 0.0f); else vm_vec_make(&m->vec.fvec, 0.0f, 0.0f, 1.0f); } } else vm_vec_normalize(&m->vec.fvec); // we now have a valid and normalized forward vector if ((umag <= 0.0f) || vm_test_parallel(&m->vec.fvec, &m->vec.uvec)) { // no up vector to use.. if ((rmag <= 0.0f) || vm_test_parallel(&m->vec.fvec, &m->vec.rvec)) { // no right vector either, so make something up if (!m->vec.fvec.xyz.x && m->vec.fvec.xyz.y && !m->vec.fvec.xyz.z) // vertical vector vm_vec_make(&m->vec.uvec, 0.0f, 0.0f, -1.0f); else vm_vec_make(&m->vec.uvec, 0.0f, 1.0f, 0.0f); } else { // use the right vector to figure up vector vm_vec_crossprod(&m->vec.uvec, &m->vec.fvec, &m->vec.rvec); vm_vec_normalize(&m->vec.uvec); } } else vm_vec_normalize(&m->vec.uvec); // we now have both valid and normalized forward and up vectors vm_vec_crossprod(&m->vec.rvec, &m->vec.uvec, &m->vec.fvec); //normalize new perpendicular vector vm_vec_normalize(&m->vec.rvec); //now recompute up vector, in case it wasn't entirely perpendiclar vm_vec_crossprod(&m->vec.uvec, &m->vec.fvec, &m->vec.rvec); } //Rotates the orient matrix by the angles in tangles and then //makes sure that the matrix is orthogonal. void vm_rotate_matrix_by_angles( matrix *orient, angles *tangles ) { matrix rotmat,new_orient; vm_angles_2_matrix(&rotmat,tangles); vm_matrix_x_matrix(&new_orient,orient,&rotmat); *orient = new_orient; vm_orthogonalize_matrix(orient); } // dir must be normalized! float vm_vec_dot_to_point(vec3d *dir, vec3d *p1, vec3d *p2) { vec3d tvec; vm_vec_sub(&tvec, p2, p1); // VECMAT-ERROR: NULL VEC3D (p1 == p2) vm_vec_normalize_safe(&tvec); return vm_vec_dot(dir, &tvec); } ///////////////////////////////////////////////////////// // Given a plane and a point, return the point on the plane closest the the point. // Result returned in q. void compute_point_on_plane(vec3d *q, plane *planep, vec3d *p) { float k, tv; vec3d normal; normal.xyz.x = planep->A; normal.xyz.y = planep->B; normal.xyz.z = planep->C; k = (planep->D + vm_vec_dot(&normal, p)) / vm_vec_dot(&normal, &normal); vm_vec_scale_add(q, p, &normal, -k); tv = planep->A * q->xyz.x + planep->B * q->xyz.y + planep->C * q->xyz.z + planep->D; } // Generate a fairly random vector that's fairly near normalized. void vm_vec_rand_vec_quick(vec3d *rvec) { rvec->xyz.x = (frand() - 0.5f) * 2; rvec->xyz.y = (frand() - 0.5f) * 2; rvec->xyz.z = (frand() - 0.5f) * 2; if (IS_VEC_NULL(rvec)) rvec->xyz.x = 1.0f; vm_vec_normalize_quick(rvec); } // Given an point "in" rotate it by "angle" around an // arbritary line defined by a point on the line "line_point" // and the normalized line direction, "line_dir" // Returns the rotated point in "out". void vm_rot_point_around_line(vec3d *out, vec3d *in, float angle, vec3d *line_point, vec3d *line_dir) { vec3d tmp, tmp1; matrix m, r; angles ta; vm_vector_2_matrix_norm(&m, line_dir, NULL, NULL ); ta.p = ta.h = 0.0f; ta.b = angle; vm_angles_2_matrix(&r,&ta); vm_vec_sub( &tmp, in, line_point ); // move relative to a point on line vm_vec_rotate( &tmp1, &tmp, &m); // rotate into line's base vm_vec_rotate( &tmp, &tmp1, &r); // rotate around Z vm_vec_unrotate( &tmp1, &tmp, &m); // unrotate out of line's base vm_vec_add( out, &tmp1, line_point ); // move back to world coordinates } // Given two position vectors, return 0 if the same, else non-zero. int vm_vec_cmp( vec3d * a, vec3d * b ) { float diff = vm_vec_dist(a,b); //mprintf(( "Diff=%.32f\n", diff )); if ( diff > 0.005f ) return 1; else return 0; } // Given two orientation matrices, return 0 if the same, else non-zero. int vm_matrix_cmp( matrix * a, matrix * b ) { float tmp1,tmp2,tmp3; tmp1 = fl_abs(vm_vec_dot( &a->vec.uvec, &b->vec.uvec ) - 1.0f); tmp2 = fl_abs(vm_vec_dot( &a->vec.fvec, &b->vec.fvec ) - 1.0f); tmp3 = fl_abs(vm_vec_dot( &a->vec.rvec, &b->vec.rvec ) - 1.0f); // mprintf(( "Mat=%.16f, %.16f, %.16f\n", tmp1, tmp2, tmp3 )); if ( tmp1 > 0.0000005f ) return 1; if ( tmp2 > 0.0000005f ) return 1; if ( tmp3 > 0.0000005f ) return 1; return 0; } // Moves angle 'h' towards 'desired_angle', taking the shortest // route possible. It will move a maximum of 'step_size' radians // each call. All angles in radians. void vm_interp_angle( float *h, float desired_angle, float step_size ) { float delta; if ( desired_angle < 0.0f ) desired_angle += PI2; if ( desired_angle > PI2 ) desired_angle -= PI2; delta = desired_angle - *h; if ( fl_abs(delta) > PI ) { // Go the other way, since it will be shorter. if ( delta > 0.0f ) { delta = delta - PI2; } else { delta = PI2 - delta; } } if ( delta > step_size ) *h += step_size; else if ( delta < -step_size ) *h -= step_size; else *h = desired_angle; // If we wrap outside of 0 to 2*PI, then put the // angle back in the range 0 to 2*PI. if ( *h > PI2 ) *h -= PI2; if ( *h < 0.0f ) *h += PI2; } // check a matrix for zero rows and columns int vm_check_matrix_for_zeros(matrix *m) { if (!m->vec.fvec.xyz.x && !m->vec.fvec.xyz.y && !m->vec.fvec.xyz.z) return 1; if (!m->vec.rvec.xyz.x && !m->vec.rvec.xyz.y && !m->vec.rvec.xyz.z) return 1; if (!m->vec.uvec.xyz.x && !m->vec.uvec.xyz.y && !m->vec.uvec.xyz.z) return 1; if (!m->vec.fvec.xyz.x && !m->vec.rvec.xyz.x && !m->vec.uvec.xyz.x) return 1; if (!m->vec.fvec.xyz.y && !m->vec.rvec.xyz.y && !m->vec.uvec.xyz.y) return 1; if (!m->vec.fvec.xyz.z && !m->vec.rvec.xyz.z && !m->vec.uvec.xyz.z) return 1; return 0; } // see if two vectors are the same int vm_vec_same(vec3d *v1, vec3d *v2) { if ( v1->xyz.x == v2->xyz.x && v1->xyz.y == v2->xyz.y && v1->xyz.z == v2->xyz.z ) return 1; return 0; } // see if two matrices are the same int vm_matrix_same(matrix *m1, matrix *m2) { int i; for (i = 0; i < 9; i++) if (m1->a1d[i] != m2->a1d[i]) return 0; return 1; } // -------------------------------------------------------------------------------------- void vm_quaternion_rotate(matrix *M, float theta, vec3d *u) // given an arbitrary rotation axis and rotation angle, function generates the // corresponding rotation matrix // // M is the return rotation matrix theta is the angle of rotation // u is the direction of the axis. // this is adapted from Computer Graphics (Hearn and Bker 2nd ed.) p. 420 // { float a,b,c, s; a = (u->xyz.x * sinf(theta * 0.5f)); b = (u->xyz.y * sinf(theta * 0.5f)); c = (u->xyz.z * sinf(theta * 0.5f)); s = cosf(theta/2.0f); // 1st ROW vector M->vec.rvec.xyz.x = 1.0f - 2.0f*b*b - 2.0f*c*c; M->vec.rvec.xyz.y = 2.0f*a*b + 2.0f*s*c; M->vec.rvec.xyz.z = 2.0f*a*c - 2.0f*s*b; // 2nd ROW vector M->vec.uvec.xyz.x = 2.0f*a*b - 2.0f*s*c; M->vec.uvec.xyz.y = 1.0f - 2.0f*a*a - 2.0f*c*c; M->vec.uvec.xyz.z = 2.0f*b*c + 2.0f*s*a; // 3rd ROW vector M->vec.fvec.xyz.x = 2.0f*a*c + 2.0f*s*b; M->vec.fvec.xyz.y = 2.0f*b*c - 2.0f*s*a; M->vec.fvec.xyz.z = 1.0f - 2.0f*a*a - 2.0f*b*b; } // -------------------------------------------------------------------------------------- // function finds the rotation matrix about the z axis for a given rotation angle (in radians) // this is an optimized version vm_quaternion_rotate // // inputs: m => point to resultant rotation matrix // angle => rotation angle about z axis (in radians) // void rotate_z ( matrix *m, float theta ) { m->vec.rvec.xyz.x = cosf (theta); m->vec.rvec.xyz.y = sinf (theta); m->vec.rvec.xyz.z = 0.0f; m->vec.uvec.xyz.x = -m->vec.rvec.xyz.y; m->vec.uvec.xyz.y = m->vec.rvec.xyz.x; m->vec.uvec.xyz.z = 0.0f; m->vec.fvec.xyz.x = 0.0f; m->vec.fvec.xyz.y = 0.0f; m->vec.fvec.xyz.z = 1.0f; } // -------------------------------------------------------------------------------------- //void vm_matrix_to_rot_axis_and_angle(matrix *m, float *theta, vec3d *rot_axis) // Converts a matrix into a rotation axis and an angle around that axis // Note for angle is very near 0, returns 0 with axis of (1,0,0) // For angles near PI, returns PI with correct axis // // rot_axis - the resultant axis of rotation // theta - the resultatn rotation around the axis // m - the initial matrix void vm_matrix_to_rot_axis_and_angle(matrix *m, float *theta, vec3d *rot_axis) { float trace = m->a2d[0][0] + m->a2d[1][1] + m->a2d[2][2]; float cos_theta = 0.5f * (trace - 1.0f); if (cos_theta > 0.999999875f) { // angle is less than 1 milirad (0.057 degrees) *theta = 0.0f; vm_vec_make(rot_axis, 1.0f, 0.0f, 0.0f); } else if (cos_theta > -0.999999875f) { // angle is within limits between 0 and PI *theta = acosf(cos_theta); Assert( !fl_is_nan(*theta) ); rot_axis->xyz.x = (m->vec.uvec.xyz.z - m->vec.fvec.xyz.y); rot_axis->xyz.y = (m->vec.fvec.xyz.x - m->vec.rvec.xyz.z); rot_axis->xyz.z = (m->vec.rvec.xyz.y - m->vec.uvec.xyz.x); vm_vec_normalize(rot_axis); } else { // angle is PI within limits *theta = PI; // find index of largest diagonal term int largest_diagonal_index = 0; if (m->a2d[1][1] > m->a2d[0][0]) { largest_diagonal_index = 1; } if (m->a2d[2][2] > m->a2d[largest_diagonal_index][largest_diagonal_index]) { largest_diagonal_index = 2; } switch (largest_diagonal_index) { case 0: float ix; rot_axis->xyz.x = fl_sqrt(m->a2d[0][0] + 1.0f); ix = 1.0f / rot_axis->xyz.x; rot_axis->xyz.y = m->a2d[0][1] * ix; rot_axis->xyz.z = m->a2d[0][2] * ix; break; case 1: float iy; rot_axis->xyz.y = fl_sqrt(m->a2d[1][1] + 1.0f); iy = 1.0f / rot_axis->xyz.y; rot_axis->xyz.x = m->a2d[1][0] * iy; rot_axis->xyz.z = m->a2d[1][2] * iy; break; case 2: float iz; rot_axis->xyz.z = fl_sqrt(m->a2d[2][2] + 1.0f); iz = 1.0f / rot_axis->xyz.z; rot_axis->xyz.x = m->a2d[2][0] * iz; rot_axis->xyz.y = m->a2d[2][1] * iz; break; default: Int3(); // this should never happen break; } // normalize rotation axis vm_vec_normalize(rot_axis); } } // -------------------------------------------------------------------------------------- // This routine determines the resultant angular displacement and angular velocity in trying to reach a goal // given an angular velocity APPROACHing a goal. It uses maximal acceleration to a point (called peak), then maximal // deceleration to arrive at the goal with zero angular velocity. This can occasionally cause overshoot. // w_in > 0 // w_max > 0 // theta_goal > 0 // aa > 0 // returns delta_theta float away(float w_in, float w_max, float theta_goal, float aa, float delta_t, float *w_out, int no_overshoot); float approach(float w_in, float w_max, float theta_goal, float aa, float delta_t, float *w_out, int no_overshoot) { float delta_theta; // amount rotated during time delta_t Assert(w_in >= 0); Assert(theta_goal > 0); float effective_aa; if (aa == 0) { *w_out = w_in; delta_theta = w_in*delta_t; return delta_theta; } if (no_overshoot && (w_in*w_in > 2.0f*1.05f*aa*theta_goal)) { w_in = fl_sqrt(2.0f*aa*theta_goal); } if (w_in*w_in > 2.0f*1.05f*aa*theta_goal) { // overshoot condition effective_aa = 1.05f*aa; delta_theta = w_in*delta_t - 0.5f*effective_aa*delta_t*delta_t; if (delta_theta > theta_goal) { // pass goal during this frame float t_goal = (-w_in + fl_sqrt(w_in*w_in +2.0f*effective_aa*theta_goal)) / effective_aa; // get time to theta_goal and away Assert(t_goal < delta_t); w_in -= effective_aa*t_goal; delta_theta = w_in*t_goal + 0.5f*effective_aa*t_goal*t_goal; delta_theta -= away(-w_in, w_max, 0.0f, aa, delta_t - t_goal, w_out, no_overshoot); *w_out = -*w_out; return delta_theta; } else { if (delta_theta < 0) { // pass goal and return this frame *w_out = 0.0f; return theta_goal; } else { // do not pass goal this frame *w_out = w_in - effective_aa*delta_t; return delta_theta; } } } else if (w_in*w_in < 2.0f*0.95f*aa*theta_goal) { // undershoot condition // find peak angular velocity float wp_sqr = fl_abs(aa*theta_goal + 0.5f*w_in*w_in); Assert(wp_sqr >= 0); if (wp_sqr > w_max*w_max) { float time_to_w_max = (w_max - w_in) / aa; if (time_to_w_max < 0) { // speed already too high // TODO: consider possible ramp down to below w_max *w_out = w_in - aa*delta_t; if (*w_out < 0) { *w_out = 0.0f; } delta_theta = 0.5f*(w_in + *w_out)*delta_t; return delta_theta; } else if (time_to_w_max > delta_t) { // does not reach w_max this frame *w_out = w_in + aa*delta_t; delta_theta = 0.5f*(w_in + *w_out)*delta_t; return delta_theta; } else { // reaches w_max this frame // TODO: consider when to ramp down from w_max *w_out = w_max; delta_theta = 0.5f*(w_in + *w_out)*delta_t; return delta_theta; } } else { // wp < w_max if (wp_sqr > (w_in + aa*delta_t)*(w_in + aa*delta_t)) { // does not reach wp this frame *w_out = w_in + aa*delta_t; delta_theta = 0.5f*(w_in + *w_out)*delta_t; return delta_theta; } else { // reaches wp this frame float wp = fl_sqrt(wp_sqr); float time_to_wp = (wp - w_in) / aa; //Assert(time_to_wp > 0); //WMC - this is not needed, right? // accel *w_out = wp; delta_theta = 0.5f*(w_in + *w_out)*time_to_wp; // decel float time_remaining = delta_t - time_to_wp; *w_out -= aa*time_remaining; if (*w_out < 0) { // reached goal *w_out = 0.0f; delta_theta = theta_goal; return delta_theta; } delta_theta += 0.5f*(wp + *w_out)*time_remaining; return delta_theta; } } } else { // on target // reach goal this frame if (w_in - aa*delta_t < 0) { // reach goal this frame *w_out = 0.0f; return theta_goal; } else { // move toward goal *w_out = w_in - aa*delta_t; Assert(*w_out >= 0); delta_theta = 0.5f*(w_in + *w_out)*delta_t; return delta_theta; } } } // -------------------------------------------------------------------------------------- // This routine determines the resultant angular displacement and angular velocity in trying to reach a goal // given an angular velocity AWAY from a goal. It uses maximal acceleration to a point (called peak), then maximal // deceleration to arrive at the goal with zero angular acceleration. // w_in < 0 // w_max > 0 // theta_goal > 0 // aa > 0 // returns angle rotated this frame float away(float w_in, float w_max, float theta_goal, float aa, float delta_t, float *w_out, int no_overshoot) { float delta_theta;// amount rotated during time float t0; // time to velocity is 0 float t_excess; // time remaining in interval after velocity is 0 Assert(theta_goal >=0); Assert(w_in <= 0); if ((-w_in < 1e-5) && (theta_goal < 1e-5)) { *w_out = 0.0f; return theta_goal; } if (aa == 0) { *w_out = w_in; delta_theta = w_in*delta_t; return delta_theta; } t0 = -w_in / aa; if (t0 > delta_t) { // no reversal in this time interval *w_out = w_in + aa * delta_t; delta_theta = (w_in + *w_out) / 2.0f * delta_t; return delta_theta; } // use time remaining after v = 0 delta_theta = 0.5f*w_in*t0; theta_goal -= delta_theta; // delta_theta is *negative* t_excess = delta_t - t0; delta_theta += approach(0.0f, w_max, theta_goal, aa, t_excess, w_out, no_overshoot); return delta_theta; } // -------------------------------------------------------------------------------------- void vm_matrix_interpolate(matrix *goal_orient, matrix *curr_orient, vec3d *w_in, float delta_t, matrix *next_orient, vec3d *w_out, vec3d *vel_limit, vec3d *acc_limit, int no_overshoot) { matrix rot_matrix; // rotation matrix from curr_orient to goal_orient matrix Mtemp1; // temp matrix vec3d rot_axis; // vector indicating direction of rotation axis vec3d theta_goal; // desired angular position at the end of the time interval vec3d theta_end; // actual angular position at the end of the time interval float theta; // magnitude of rotation about the rotation axis // FIND ROTATION NEEDED FOR GOAL // goal_orient = R curr_orient, so R = goal_orient curr_orient^-1 vm_copy_transpose_matrix(&Mtemp1, curr_orient); // Mtemp1 = curr ^-1 vm_matrix_x_matrix(&rot_matrix, &Mtemp1, goal_orient); // R = goal * Mtemp1 vm_orthogonalize_matrix(&rot_matrix); vm_matrix_to_rot_axis_and_angle(&rot_matrix, &theta, &rot_axis); // determines angle and rotation axis from curr to goal // find theta to goal vm_vec_copy_scale(&theta_goal, &rot_axis, theta); if (theta < SMALL_NUM) { *next_orient = *goal_orient; vm_vec_zero(w_out); return; } theta_end = vmd_zero_vector; float delta_theta; // find rotation about x if (theta_goal.xyz.x > 0) { if (w_in->xyz.x >= 0) { 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); theta_end.xyz.x = delta_theta; } else { // w_in->xyz.x < 0 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); theta_end.xyz.x = delta_theta; } } else if (theta_goal.xyz.x < 0) { if (w_in->xyz.x <= 0) { 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); theta_end.xyz.x = -delta_theta; w_out->xyz.x = -w_out->xyz.x; } else { // w_in->xyz.x > 0 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); theta_end.xyz.x = -delta_theta; w_out->xyz.x = -w_out->xyz.x; } } else { // theta_goal == 0 if (w_in->xyz.x < 0) { 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); theta_end.xyz.x = delta_theta; } else { 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); theta_end.xyz.x = -delta_theta; w_out->xyz.x = -w_out->xyz.x; } } // find rotation about y if (theta_goal.xyz.y > 0) { if (w_in->xyz.y >= 0) { 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); theta_end.xyz.y = delta_theta; } else { // w_in->xyz.y < 0 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); theta_end.xyz.y = delta_theta; } } else if (theta_goal.xyz.y < 0) { if (w_in->xyz.y <= 0) { 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); theta_end.xyz.y = -delta_theta; w_out->xyz.y = -w_out->xyz.y; } else { // w_in->xyz.y > 0 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); theta_end.xyz.y = -delta_theta; w_out->xyz.y = -w_out->xyz.y; } } else { // theta_goal == 0 if (w_in->xyz.y < 0) { 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); theta_end.xyz.y = delta_theta; } else { 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); theta_end.xyz.y = -delta_theta; w_out->xyz.y = -w_out->xyz.y; } } // find rotation about z if (theta_goal.xyz.z > 0) { if (w_in->xyz.z >= 0) { 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); theta_end.xyz.z = delta_theta; } else { // w_in->xyz.z < 0 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); theta_end.xyz.z = delta_theta; } } else if (theta_goal.xyz.z < 0) { if (w_in->xyz.z <= 0) { 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); theta_end.xyz.z = -delta_theta; w_out->xyz.z = -w_out->xyz.z; } else { // w_in->xyz.z > 0 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); theta_end.xyz.z = -delta_theta; w_out->xyz.z = -w_out->xyz.z; } } else { // theta_goal == 0 if (w_in->xyz.z < 0) { 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); theta_end.xyz.z = delta_theta; } else { 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); theta_end.xyz.z = -delta_theta; w_out->xyz.z = -w_out->xyz.z; } } // the amount of rotation about each axis is determined in // functions approach and away. first find the magnitude // of the rotation and then normalize the axis rot_axis = theta_end; Assert(is_valid_vec(&rot_axis)); Assert(vm_vec_mag(&rot_axis) > 0); // normalize rotation axis and determine total rotation angle theta = vm_vec_normalize(&rot_axis); // arrived at goal? 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) { *next_orient = *goal_orient; } else { // otherwise rotate to better position vm_quaternion_rotate(&Mtemp1, theta, &rot_axis); Assert(is_valid_matrix(&Mtemp1)); vm_matrix_x_matrix(next_orient, curr_orient, &Mtemp1); vm_orthogonalize_matrix(next_orient); } } // end matrix_interpolate // -------------------------------------------------------------------------------------- void get_camera_limits(matrix *start_camera, matrix *end_camera, float time, vec3d *acc_max, vec3d *w_max) { matrix temp, rot_matrix; float theta; vec3d rot_axis; vec3d angle; // determine the necessary rotation matrix vm_copy_transpose_matrix(&temp, start_camera); vm_matrix_x_matrix(&rot_matrix, &temp, end_camera); vm_orthogonalize_matrix(&rot_matrix); // determine the rotation axis and angle vm_matrix_to_rot_axis_and_angle(&rot_matrix, &theta, &rot_axis); // find the rotation about each axis angle.xyz.x = theta * rot_axis.xyz.x; angle.xyz.y = theta * rot_axis.xyz.y; angle.xyz.z = theta * rot_axis.xyz.z; // allow for 0 time input if (time <= 1e-5f) { vm_vec_make(acc_max, 0.0f, 0.0f, 0.0f); vm_vec_make(w_max, 0.0f, 0.0f, 0.0f); } else { // find acceleration limit using (theta/2) takes (time/2) // and using const accel theta = 1/2 acc * time^2 acc_max->xyz.x = 4.0f * fl_abs(angle.xyz.x) / (time * time); acc_max->xyz.y = 4.0f * fl_abs(angle.xyz.y) / (time * time); acc_max->xyz.z = 4.0f * fl_abs(angle.xyz.z) / (time * time); // find angular velocity limits // w_max = acc_max * time / 2 w_max->xyz.x = acc_max->xyz.x * time / 2.0f; w_max->xyz.y = acc_max->xyz.y * time / 2.0f; w_max->xyz.z = acc_max->xyz.z * time / 2.0f; } } // --------------------------------------------------------------------------------------------- // // inputs: goal_orient => goal orientation matrix // orient => current orientation matrix (with current forward vector) // w_in => current input angular velocity // delta_t => time to move toward goal // next_orient => the orientation matrix at time delta_t (with current forward vector) // NOTE: this does not include any rotation about z (bank) // w_out => the angular velocity of the ship at delta_t // vel_limit => maximum rotational speed // acc_limit => maximum rotational speed // // function moves the forward vector toward the goal forward vector taking account of anglular // momentum (velocity) Attempt to try to move bank by goal delta_bank. Rotational velocity // on x/y is rotated with bank, giving smoother motion. void vm_fvec_matrix_interpolate(matrix *goal_orient, matrix *orient, vec3d *w_in, float delta_t, matrix *next_orient, vec3d *w_out, vec3d *vel_limit, vec3d *acc_limit, int no_overshoot) { matrix Mtemp1; // temporary matrix matrix M_intermed; // intermediate matrix after xy rotation vec3d local_rot_axis; // vector indicating direction of rotation axis (local coords) vec3d rot_axis; // vector indicating direction of rotation axis (world coords) vec3d theta_goal; // desired angular position at the end of the time interval vec3d theta_end; // actual angular position at the end of the time interval float theta; // magnitude of rotation about the rotation axis float bank; // magnitude of rotation about the forward axis int no_bank; // flag set if there is no bank for the object vec3d vtemp; // temp angular velocity before rotation about z float z_dotprod; // dotprod of orient->vec.fvec and goal_orient->vec.fvec float r_dotprod; // dotprod of orient->vec.rvec and goal_orient->vec.rvec float delta_bank; // FIND XY ROTATION NEEDED FOR GOAL // rotation vector is (current fvec) orient->vec.fvec x goal_f // magnitude = asin ( magnitude of crossprod ) vm_vec_crossprod ( &rot_axis, &orient->vec.fvec, &goal_orient->vec.fvec ); float t = vm_vec_mag(&rot_axis); if (t > 1.0f) t = 1.0f; z_dotprod = vm_vec_dotprod ( &orient->vec.fvec, &goal_orient->vec.fvec ); if ( t < SMALLER_NUM ) { if ( z_dotprod > 0.0f ) theta = 0.0f; else { // the forward vector is pointing exactly opposite of goal // arbitrarily choose the x axis to rotate around until t becomes large enough theta = PI; rot_axis = orient->vec.rvec; } } else { theta = asinf ( t ); vm_vec_scale ( &rot_axis, 1/t ); if ( z_dotprod < 0.0f ) theta = PI - theta; } // rotate rot_axis into ship reference frame vm_vec_rotate ( &local_rot_axis, &rot_axis, orient ); // find theta to goal vm_vec_copy_scale(&theta_goal, &local_rot_axis, theta); Assert ( fl_abs (theta_goal.xyz.z) < 0.001f ); // check for proper rotation theta_end = vmd_zero_vector; float delta_theta; // find rotation about x if (theta_goal.xyz.x > 0) { if (w_in->xyz.x >= 0) { 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); theta_end.xyz.x = delta_theta; } else { // w_in->xyz.x < 0 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); theta_end.xyz.x = delta_theta; } } else if (theta_goal.xyz.x < 0) { if (w_in->xyz.x <= 0) { 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); theta_end.xyz.x = -delta_theta; w_out->xyz.x = -w_out->xyz.x; } else { // w_in->xyz.x > 0 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); theta_end.xyz.x = -delta_theta; w_out->xyz.x = -w_out->xyz.x; } } else { // theta_goal == 0 if (w_in->xyz.x < 0) { 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); theta_end.xyz.x = delta_theta; } else { 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); theta_end.xyz.x = -delta_theta; w_out->xyz.x = -w_out->xyz.x; } } // find rotation about y if (theta_goal.xyz.y > 0) { if (w_in->xyz.y >= 0) { 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); theta_end.xyz.y = delta_theta; } else { // w_in->xyz.y < 0 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); theta_end.xyz.y = delta_theta; } } else if (theta_goal.xyz.y < 0) { if (w_in->xyz.y <= 0) { 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); theta_end.xyz.y = -delta_theta; w_out->xyz.y = -w_out->xyz.y; } else { // w_in->xyz.y > 0 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); theta_end.xyz.y = -delta_theta; w_out->xyz.y = -w_out->xyz.y; } } else { // theta_goal == 0 if (w_in->xyz.y < 0) { 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); theta_end.xyz.y = delta_theta; } else { 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); theta_end.xyz.y = -delta_theta; w_out->xyz.y = -w_out->xyz.y; } } // FIND Z ROTATON MATRIX theta_end.xyz.z = 0.0f; rot_axis = theta_end; Assert(is_valid_vec(&rot_axis)); // normalize rotation axis and determine total rotation angle theta = vm_vec_mag(&rot_axis); if (theta < SMALL_NUM) { theta = 0.0f; M_intermed = *orient; } else { vm_vec_scale ( &rot_axis, 1/theta ); vm_quaternion_rotate ( &Mtemp1, theta, &rot_axis ); Assert(is_valid_matrix(&Mtemp1)); vm_matrix_x_matrix ( &M_intermed, orient, &Mtemp1 ); Assert(is_valid_matrix(&M_intermed)); } // FIND ROTATION ABOUT Z (IF ANY) // no rotation if delta_bank and w_in both 0 or rotational acc in forward is 0 no_bank = ( acc_limit->xyz.z == 0.0f && vel_limit->xyz.z == 0.0f ); if ( no_bank ) { // no rotation on z, so we're done (no rotation of w) *next_orient = M_intermed; vm_orthogonalize_matrix ( next_orient ); return; } else { // calculate delta_bank using orient->vec.rvec, goal_orient->vec.rvec // vm_vec_crossprod ( &rot_axis, &orient->vec.rvec, &goal_orient->vec.rvec ); t = vm_vec_mag(&rot_axis); if (t > 1.0f) t = 1.0f; r_dotprod = vm_vec_dotprod ( &orient->vec.rvec, &goal_orient->vec.rvec ); if ( t < SMALLER_NUM ) { if ( r_dotprod > 0.0f ) theta = 0.0f; else { // the right vector is pointing exactly opposite of goal, so rotate 180 on z theta = PI; rot_axis = orient->vec.fvec; } } else { theta = asinf ( t ); vm_vec_scale ( &rot_axis, 1/t ); if ( z_dotprod < 0.0f ) theta = PI - theta; } // rotate rot_axis into ship reference frame vm_vec_rotate ( &local_rot_axis, &rot_axis, orient ); // find theta.xyz.z to goal delta_bank = local_rot_axis.xyz.z * theta; Assert( fl_abs (local_rot_axis.xyz.x) < 0.001f ); // check for proper rotation bank = 0.0f; // end calculate delta_bank // find rotation about z if (delta_bank > 0) { if (w_in->xyz.z >= 0) { 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); bank = delta_theta; } else { // w_in->xyz.z < 0 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); bank = delta_theta; } } else if (delta_bank < 0) { if (w_in->xyz.z <= 0) { 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); bank = -delta_theta; w_out->xyz.z = -w_out->xyz.z; } else { // w_in->xyz.z > 0 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); bank = -delta_theta; w_out->xyz.z = -w_out->xyz.z; } } else { // theta_goal == 0 if (w_in->xyz.z < 0) { 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); bank = delta_theta; } else { 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); bank = -delta_theta; w_out->xyz.z = -w_out->xyz.z; } } if ( fl_abs (bank) < SMALL_NUM ) { *next_orient = M_intermed; vm_orthogonalize_matrix ( next_orient ); } else { rotate_z ( &Mtemp1, bank ); vtemp = *w_out; vm_vec_rotate ( w_out, &vtemp, &Mtemp1 ); vm_matrix_x_matrix ( next_orient, &M_intermed, &Mtemp1 ); Assert(is_valid_matrix(next_orient)); vm_orthogonalize_matrix ( next_orient ); } } } // end vm_fvec_matrix_interpolate // --------------------------------------------------------------------------------------------- // // inputs: goal_f => goal forward vector // orient => current orientation matrix (with current forward vector) // w_in => current input angular velocity // delta_t => time to move toward goal // delta_bank => desired change in bank in degrees // next_orient => the orientation matrix at time delta_t (with current forward vector) // NOTE: this does not include any rotation about z (bank) // w_out => the angular velocity of the ship at delta_t // vel_limit => maximum rotational speed // acc_limit => maximum rotational speed // // function moves the forward vector toward the goal forward vector taking account of anglular // momentum (velocity) Attempt to try to move bank by goal delta_bank. Rotational velocity // on x/y is rotated with bank, giving smoother motion. void vm_forward_interpolate(vec3d *goal_f, matrix *orient, vec3d *w_in, float delta_t, float delta_bank, matrix *next_orient, vec3d *w_out, vec3d *vel_limit, vec3d *acc_limit, int no_overshoot) { matrix Mtemp1; // temporary matrix vec3d local_rot_axis; // vector indicating direction of rotation axis (local coords) vec3d rot_axis; // vector indicating direction of rotation axis (world coords) vec3d theta_goal; // desired angular position at the end of the time interval vec3d theta_end; // actual angular position at the end of the time interval float theta; // magnitude of rotation about the rotation axis float bank; // magnitude of rotation about the forward axis int no_bank; // flag set if there is no bank for the object vec3d vtemp; // float z_dotprod; // FIND ROTATION NEEDED FOR GOAL // rotation vector is (current fvec) orient->vec.fvec x goal_f // magnitude = asin ( magnitude of crossprod ) vm_vec_crossprod( &rot_axis, &orient->vec.fvec, goal_f ); float t = vm_vec_mag(&rot_axis); if (t > 1.0f) t = 1.0f; z_dotprod = vm_vec_dotprod( &orient->vec.fvec, goal_f ); if ( t < SMALLER_NUM ) { if ( z_dotprod > 0.0f ) theta = 0.0f; else { // the forward vector is pointing exactly opposite of goal // arbitrarily choose the x axis to rotate around until t becomes large enough theta = PI; rot_axis = orient->vec.rvec; } } else { theta = asinf( t ); vm_vec_scale ( &rot_axis, 1/t ); if ( z_dotprod < 0.0f ) theta = PI - theta; } // rotate rot_axis into ship reference frame vm_vec_rotate( &local_rot_axis, &rot_axis, orient ); // find theta to goal vm_vec_copy_scale(&theta_goal, &local_rot_axis, theta); // DO NOT COMMENT THIS OUT!! if(!(fl_abs(theta_goal.xyz.z) < 0.001f)) // check for proper rotation mprintf(("vm_forward_interpolate: Bad rotation\n")); theta_end = vmd_zero_vector; float delta_theta; // find rotation about x if (theta_goal.xyz.x > 0) { if (w_in->xyz.x >= 0) { 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); theta_end.xyz.x = delta_theta; } else { // w_in->xyz.x < 0 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); theta_end.xyz.x = delta_theta; } } else if (theta_goal.xyz.x < 0) { if (w_in->xyz.x <= 0) { 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); theta_end.xyz.x = -delta_theta; w_out->xyz.x = -w_out->xyz.x; } else { // w_in->xyz.x > 0 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); theta_end.xyz.x = -delta_theta; w_out->xyz.x = -w_out->xyz.x; } } else { // theta_goal == 0 if (w_in->xyz.x < 0) { 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); theta_end.xyz.x = delta_theta; } else { 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); theta_end.xyz.x = -delta_theta; w_out->xyz.x = -w_out->xyz.x; } } // find rotation about y if (theta_goal.xyz.y > 0) { if (w_in->xyz.y >= 0) { 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); theta_end.xyz.y = delta_theta; } else { // w_in->xyz.y < 0 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); theta_end.xyz.y = delta_theta; } } else if (theta_goal.xyz.y < 0) { if (w_in->xyz.y <= 0) { 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); theta_end.xyz.y = -delta_theta; w_out->xyz.y = -w_out->xyz.y; } else { // w_in->xyz.y > 0 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); theta_end.xyz.y = -delta_theta; w_out->xyz.y = -w_out->xyz.y; } } else { // theta_goal == 0 if (w_in->xyz.y < 0) { 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); theta_end.xyz.y = delta_theta; } else { 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); theta_end.xyz.y = -delta_theta; w_out->xyz.y = -w_out->xyz.y; } } // no rotation if delta_bank and w_in both 0 or rotational acc in forward is 0 no_bank = ( delta_bank == 0.0f && vel_limit->xyz.z == 0.0f && acc_limit->xyz.z == 0.0f ); // do rotation about z bank = 0.0f; if ( !no_bank ) { // convert delta_bank to radians delta_bank *= (float) CONVERT_RADIANS; // find rotation about z if (delta_bank > 0) { if (w_in->xyz.z >= 0) { 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); bank = delta_theta; } else { // w_in->xyz.z < 0 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); bank = delta_theta; } } else if (delta_bank < 0) { if (w_in->xyz.z <= 0) { 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); bank = -delta_theta; w_out->xyz.z = -w_out->xyz.z; } else { // w_in->xyz.z > 0 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); bank = -delta_theta; w_out->xyz.z = -w_out->xyz.z; } } else { // theta_goal == 0 if (w_in->xyz.z < 0) { 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); bank = delta_theta; } else { 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); bank = -delta_theta; w_out->xyz.z = -w_out->xyz.z; } } } // the amount of rotation about each axis is determined in // functions approach and away. first find the magnitude // of the rotation and then normalize the axis (ship coords) theta_end.xyz.z = bank; rot_axis = theta_end; // normalize rotation axis and determine total rotation angle theta = vm_vec_mag(&rot_axis); if ( theta > SMALL_NUM ) vm_vec_scale( &rot_axis, 1/theta ); if ( theta < SMALL_NUM ) { *next_orient = *orient; return; } else { vm_quaternion_rotate( &Mtemp1, theta, &rot_axis ); vm_matrix_x_matrix( next_orient, orient, &Mtemp1 ); Assert(is_valid_matrix(next_orient)); vtemp = *w_out; vm_vec_rotate( w_out, &vtemp, &Mtemp1 ); } } // end vm_forward_interpolate // ------------------------------------------------------------------------------------ // vm_find_bounding_sphere() // // Calculate a bounding sphere for a set of points. // // input: pnts => array of world positions // num_pnts => number of points inside pnts array // center => OUTPUT PARAMETER: contains world pos of bounding sphere center // radius => OUTPUT PARAMETER: continas radius of bounding sphere // #define BIGNUMBER 100000000.0f void vm_find_bounding_sphere(vec3d *pnts, int num_pnts, vec3d *center, float *radius) { int i; float rad, rad_sq, xspan, yspan, zspan, maxspan; float old_to_p, old_to_p_sq, old_to_new; vec3d diff, xmin, xmax, ymin, ymax, zmin, zmax, dia1, dia2, *p; xmin = vmd_zero_vector; ymin = vmd_zero_vector; zmin = vmd_zero_vector; xmax = vmd_zero_vector; ymax = vmd_zero_vector; zmax = vmd_zero_vector; xmin.xyz.x = ymin.xyz.y = zmin.xyz.z = BIGNUMBER; xmax.xyz.x = ymax.xyz.y = zmax.xyz.z = -BIGNUMBER; for ( i = 0; i < num_pnts; i++ ) { p = &pnts[i]; if ( p->xyz.x < xmin.xyz.x ) xmin = *p; if ( p->xyz.x > xmax.xyz.x ) xmax = *p; if ( p->xyz.y < ymin.xyz.y ) ymin = *p; if ( p->xyz.y > ymax.xyz.y ) ymax = *p; if ( p->xyz.z < zmin.xyz.z ) zmin = *p; if ( p->xyz.z > zmax.xyz.z ) zmax = *p; } // find distance between two min and max points (squared) vm_vec_sub(&diff, &xmax, &xmin); xspan = vm_vec_mag_squared(&diff); vm_vec_sub(&diff, &ymax, &ymin); yspan = vm_vec_mag_squared(&diff); vm_vec_sub(&diff, &zmax, &zmin); zspan = vm_vec_mag_squared(&diff); dia1 = xmin; dia2 = xmax; maxspan = xspan; if ( yspan > maxspan ) { maxspan = yspan; dia1 = ymin; dia2 = ymax; } if ( zspan > maxspan ) { maxspan = yspan; dia1 = zmin; dia2 = zmax; } // calc initial center vm_vec_add(center, &dia1, &dia2); vm_vec_scale(center, 0.5f); vm_vec_sub(&diff, &dia2, center); rad_sq = vm_vec_mag_squared(&diff); rad = fl_sqrt(rad_sq); Assert( !_isnan(rad) ); // second pass for ( i = 0; i < num_pnts; i++ ) { p = &pnts[i]; vm_vec_sub(&diff, p, center); old_to_p_sq = vm_vec_mag_squared(&diff); if ( old_to_p_sq > rad_sq ) { old_to_p = fl_sqrt(old_to_p_sq); // calc radius of new sphere rad = (rad + old_to_p) / 2.0f; rad_sq = rad * rad; old_to_new = old_to_p - rad; // calc new center of sphere center->xyz.x = (rad*center->xyz.x + old_to_new*p->xyz.x) / old_to_p; center->xyz.y = (rad*center->xyz.y + old_to_new*p->xyz.y) / old_to_p; center->xyz.z = (rad*center->xyz.z + old_to_new*p->xyz.z) / old_to_p; nprintf(("Alan", "New sphere: cen,rad = %f %f %f %f\n", center->xyz.x, center->xyz.y, center->xyz.z, rad)); } } *radius = rad; } // ---------------------------------------------------------------------------- // vm_rotate_vec_to_body() // // rotates a vector from world coordinates to body coordinates // // inputs: body_vec => vector in body coordinates // world_vec => vector in world coordinates // orient => orientation matrix // vec3d* vm_rotate_vec_to_body(vec3d *body_vec, vec3d *world_vec, matrix *orient) { return vm_vec_unrotate(body_vec, world_vec, orient); } // ---------------------------------------------------------------------------- // vm_rotate_vec_to_world() // // rotates a vector from body coordinates to world coordinates // // inputs world_vec => vector in world coordinates // body_vec => vector in body coordinates // orient => orientation matrix // vec3d* vm_rotate_vec_to_world(vec3d *world_vec, vec3d *body_vec, matrix *orient) { return vm_vec_rotate(world_vec, body_vec, orient); } // ---------------------------------------------------------------------------- // vm_estimate_next_orientation() // // given a last orientation and current orientation, estimate the next orientation // // inputs: last_orient => last orientation matrix // current_orient => current orientation matrix // next_orient => next orientation matrix [the result] // void vm_estimate_next_orientation(matrix *last_orient, matrix *current_orient, matrix *next_orient) { // R L = C => R = C (L)^-1 // N = R C => N = C (L)^-1 C matrix Mtemp; matrix Rot_matrix; vm_copy_transpose_matrix(&Mtemp, last_orient); // Mtemp = (L)^-1 vm_matrix_x_matrix(&Rot_matrix, &Mtemp, current_orient); // R = C Mtemp1 vm_matrix_x_matrix(next_orient, current_orient, &Rot_matrix); } // Return true if all elements of *vec are legal, that is, not a NAN. int is_valid_vec(vec3d *vec) { return !_isnan(vec->xyz.x) && !_isnan(vec->xyz.y) && !_isnan(vec->xyz.z); } // Return true if all elements of *m are legal, that is, not a NAN. int is_valid_matrix(matrix *m) { return is_valid_vec(&m->vec.fvec) && is_valid_vec(&m->vec.uvec) && is_valid_vec(&m->vec.rvec); } // interpolate between 2 vectors. t goes from 0.0 to 1.0. at void vm_vec_interp_constant(vec3d *out, vec3d *v0, vec3d *v1, float t) { vec3d cross; float total_ang; // get the cross-product of the 2 vectors vm_vec_crossprod(&cross, v0, v1); vm_vec_normalize(&cross); // get the total angle between the 2 vectors total_ang = -(acosf(vm_vec_dot(v0, v1))); // rotate around the cross product vector by the appropriate angle vm_rot_point_around_line(out, v0, t * total_ang, &vmd_zero_vector, &cross); } // randomly perturb a vector around a given (normalized vector) or optional orientation matrix void vm_vec_random_cone(vec3d *out, vec3d *in, float max_angle, matrix *orient) { vec3d t1, t2; matrix *rot; matrix m; // get an orientation matrix if(orient != NULL){ rot = orient; } else { vm_vector_2_matrix(&m, in, NULL, NULL); rot = &m; } // axis 1 vm_rot_point_around_line(&t1, in, fl_radian(frand_range(-max_angle, max_angle)), &vmd_zero_vector, &rot->vec.fvec); // axis 2 vm_rot_point_around_line(&t2, &t1, fl_radian(frand_range(-max_angle, max_angle)), &vmd_zero_vector, &rot->vec.rvec); // axis 3 vm_rot_point_around_line(out, &t2, fl_radian(frand_range(-max_angle, max_angle)), &vmd_zero_vector, &rot->vec.uvec); } void vm_vec_random_cone(vec3d *out, vec3d *in, float min_angle, float max_angle, matrix *orient){ vec3d t1, t2; matrix *rot; matrix m; // get an orientation matrix if(orient != NULL){ rot = orient; } else { vm_vector_2_matrix(&m, in, NULL, NULL); rot = &m; } float dif_angle = max_angle - min_angle; // axis 1 float temp_ang = (frand_range(-dif_angle, dif_angle)); if(temp_ang < 0)temp_ang -= (min_angle); else temp_ang += (min_angle); vm_rot_point_around_line(&t1, in, fl_radian(temp_ang), &vmd_zero_vector, &rot->vec.fvec); // axis 2 temp_ang = (frand_range(-dif_angle, dif_angle)); if(temp_ang < 0)temp_ang -= (min_angle); else temp_ang += (min_angle); vm_rot_point_around_line(&t2, &t1, fl_radian(temp_ang), &vmd_zero_vector, &rot->vec.rvec); // axis 3 temp_ang = (frand_range(-dif_angle, dif_angle)); if(temp_ang < 0)temp_ang -= (min_angle); else temp_ang += (min_angle); vm_rot_point_around_line(out, &t2, fl_radian(temp_ang), &vmd_zero_vector, &rot->vec.uvec); } // given a start vector, an orientation and a radius, give a point on the plane of the circle // if on_edge is 1, the point is on the very edge of the circle void vm_vec_random_in_circle(vec3d *out, vec3d *in, matrix *orient, float radius, int on_edge) { vec3d temp; // point somewhere in the plane vm_vec_scale_add(&temp, in, &orient->vec.rvec, on_edge ? radius : frand_range(0.0f, radius)); // rotate to a random point on the circle vm_rot_point_around_line(out, &temp, fl_radian(frand_range(0.0f, 359.0f)), in, &orient->vec.fvec); } // find the nearest point on the line to p. if dist is non-NULL, it is filled in // returns 0 if the point is inside the line segment, -1 if "before" the line segment and 1 ir "after" the line segment int vm_vec_dist_to_line(vec3d *p, vec3d *l0, vec3d *l1, vec3d *nearest, float *dist) { vec3d a, b, c; float b_mag, comp; #ifndef NDEBUG if(vm_vec_same(l0, l1)){ *nearest = vmd_zero_vector; return -1; } #endif // compb_a == a dot b / len(b) vm_vec_sub(&a, p, l0); vm_vec_sub(&b, l1, l0); b_mag = vm_vec_copy_normalize(&c, &b); // calculate component comp = vm_vec_dotprod(&a, &b) / b_mag; // stuff nearest vm_vec_scale_add(nearest, l0, &c, comp); // maybe get the distance if(dist != NULL){ *dist = vm_vec_dist(nearest, p); } // return the proper value if(comp < 0.0f){ return -1; // before the line } else if(comp > b_mag){ return 1; // after the line } return 0; // on the line } // Goober5000 // Finds the distance squared to a line. Same as above, except it uses vm_vec_dist_squared, which is faster; // and it doesn't check whether the nearest point is on the line segment. void vm_vec_dist_squared_to_line(vec3d *p, vec3d *l0, vec3d *l1, vec3d *nearest, float *dist_squared) { vec3d a, b, c; float b_mag, comp; #ifndef NDEBUG if(vm_vec_same(l0, l1)){ *nearest = vmd_zero_vector; return; } #endif // compb_a == a dot b / len(b) vm_vec_sub(&a, p, l0); vm_vec_sub(&b, l1, l0); b_mag = vm_vec_copy_normalize(&c, &b); // calculate component comp = vm_vec_dotprod(&a, &b) / b_mag; // stuff nearest vm_vec_scale_add(nearest, l0, &c, comp); // get the distance *dist_squared = vm_vec_dist_squared(nearest, p); }