From 04b545ee298c3c6421f0bfc7a8a1527b55c7cef7 Mon Sep 17 00:00:00 2001 From: Kp Date: Sun, 3 Mar 2013 01:03:33 +0000 Subject: [PATCH] Move */maths/vecmat.c -> common/maths/vecmat.c --- SConstruct | 3 +- {d1x-rebirth => common}/maths/vecmat.c | 0 d2x-rebirth/maths/vecmat.c | 966 ------------------------- 3 files changed, 1 insertion(+), 968 deletions(-) rename {d1x-rebirth => common}/maths/vecmat.c (100%) delete mode 100644 d2x-rebirth/maths/vecmat.c diff --git a/SConstruct b/SConstruct index 3a7b8cf85..815e1f83f 100644 --- a/SConstruct +++ b/SConstruct @@ -203,6 +203,7 @@ class DXXArchive(DXXCommon): 'maths/fixc.c', 'maths/rand.c', 'maths/tables.c', +'maths/vecmat.c', ] ] editor_sources = [] @@ -455,7 +456,6 @@ class D1XProgram(DXXProgram): 'main/vclip.c', 'main/wall.c', 'main/weapon.c', -'maths/vecmat.c', 'mem/mem.c', 'misc/args.c', 'misc/dl_list.c', @@ -708,7 +708,6 @@ class D2XProgram(DXXProgram): 'main/vclip.c', 'main/wall.c', 'main/weapon.c', -'maths/vecmat.c', 'mem/mem.c', 'misc/args.c', 'misc/dl_list.c', diff --git a/d1x-rebirth/maths/vecmat.c b/common/maths/vecmat.c similarity index 100% rename from d1x-rebirth/maths/vecmat.c rename to common/maths/vecmat.c diff --git a/d2x-rebirth/maths/vecmat.c b/d2x-rebirth/maths/vecmat.c deleted file mode 100644 index af48060c3..000000000 --- a/d2x-rebirth/maths/vecmat.c +++ /dev/null @@ -1,966 +0,0 @@ -/* -THE COMPUTER CODE CONTAINED HEREIN IS THE SOLE PROPERTY OF PARALLAX -SOFTWARE CORPORATION ("PARALLAX"). PARALLAX, IN DISTRIBUTING THE CODE TO -END-USERS, AND SUBJECT TO ALL OF THE TERMS AND CONDITIONS HEREIN, GRANTS A -ROYALTY-FREE, PERPETUAL LICENSE TO SUCH END-USERS FOR USE BY SUCH END-USERS -IN USING, DISPLAYING, AND CREATING DERIVATIVE WORKS THEREOF, SO LONG AS -SUCH USE, DISPLAY OR CREATION IS FOR NON-COMMERCIAL, ROYALTY OR REVENUE -FREE PURPOSES. IN NO EVENT SHALL THE END-USER USE THE COMPUTER CODE -CONTAINED HEREIN FOR REVENUE-BEARING PURPOSES. THE END-USER UNDERSTANDS -AND AGREES TO THE TERMS HEREIN AND ACCEPTS THE SAME BY USE OF THIS FILE. -COPYRIGHT 1993-1998 PARALLAX SOFTWARE CORPORATION. ALL RIGHTS RESERVED. -*/ - -/* - * - * C version of vecmat library - * - */ - -#include -#include // for sqrt - -#include "maths.h" -#include "vecmat.h" -#include "dxxerror.h" - -//#define USE_ISQRT 1 - -vms_vector vmd_zero_vector = {0, 0, 0}; -vms_matrix vmd_identity_matrix = { { f1_0, 0, 0 }, - { 0, f1_0, 0 }, - { 0, 0, f1_0 } }; - -//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 -vms_vector *vm_vec_add(vms_vector *dest,const vms_vector *src0,const vms_vector *src1) -{ - dest->x = src0->x + src1->x; - dest->y = src0->y + src1->y; - dest->z = src0->z + src1->z; - - return dest; -} - - -//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 -vms_vector *vm_vec_sub(vms_vector *dest,const vms_vector *src0,const vms_vector *src1) -{ - dest->x = src0->x - src1->x; - dest->y = src0->y - src1->y; - dest->z = src0->z - src1->z; - - return dest; -} - -//adds one vector to another. returns ptr to dest -//dest can equal source -vms_vector *vm_vec_add2(vms_vector *dest,const vms_vector *src) -{ - dest->x += src->x; - dest->y += src->y; - dest->z += src->z; - - return dest; -} - -//subs one vector from another, returns ptr to dest -//dest can equal source -vms_vector *vm_vec_sub2(vms_vector *dest,const vms_vector *src) -{ - dest->x -= src->x; - dest->y -= src->y; - dest->z -= src->z; - - return dest; -} - -//averages two vectors. returns ptr to dest -//dest can equal either source -vms_vector *vm_vec_avg(vms_vector *dest,const vms_vector *src0,const vms_vector *src1) -{ - dest->x = (src0->x + src1->x)/2; - dest->y = (src0->y + src1->y)/2; - dest->z = (src0->z + src1->z)/2; - - return dest; -} - - -//averages four vectors. returns ptr to dest -//dest can equal any source -vms_vector *vm_vec_avg4(vms_vector *dest,const vms_vector *src0,const vms_vector *src1,const vms_vector *src2,const vms_vector *src3) -{ - dest->x = (src0->x + src1->x + src2->x + src3->x)/4; - dest->y = (src0->y + src1->y + src2->y + src3->y)/4; - dest->z = (src0->z + src1->z + src2->z + src3->z)/4; - - return dest; -} - - -//scales a vector in place. returns ptr to vector -vms_vector *vm_vec_scale(vms_vector *dest,fix s) -{ - dest->x = fixmul(dest->x,s); - dest->y = fixmul(dest->y,s); - dest->z = fixmul(dest->z,s); - - return dest; -} - -//scales and copies a vector. returns ptr to dest -vms_vector *vm_vec_copy_scale(vms_vector *dest,const vms_vector *src,fix s) -{ - dest->x = fixmul(src->x,s); - dest->y = fixmul(src->y,s); - dest->z = fixmul(src->z,s); - - return dest; -} - -//scales a vector, adds it to another, and stores in a 3rd vector -//dest = src1 + k * src2 -vms_vector *vm_vec_scale_add(vms_vector *dest,const vms_vector *src1,const vms_vector *src2,fix k) -{ - dest->x = src1->x + fixmul(src2->x,k); - dest->y = src1->y + fixmul(src2->y,k); - dest->z = src1->z + fixmul(src2->z,k); - - return dest; -} - -//scales a vector and adds it to another -//dest += k * src -vms_vector *vm_vec_scale_add2(vms_vector *dest,const vms_vector *src,fix k) -{ - dest->x += fixmul(src->x,k); - dest->y += fixmul(src->y,k); - dest->z += fixmul(src->z,k); - - return dest; -} - -//scales a vector in place, taking n/d for scale. returns ptr to vector -//dest *= n/d -vms_vector *vm_vec_scale2(vms_vector *dest,fix n,fix d) -{ -#if 1 // DPH: Kludge: this was overflowing a lot, so I made it use the FPU. - float nd; - nd = f2fl(n) / f2fl(d); - dest->x = fl2f( f2fl(dest->x) * nd); - dest->y = fl2f( f2fl(dest->y) * nd); - dest->z = fl2f( f2fl(dest->z) * nd); -#else - dest->x = fixmuldiv(dest->x,n,d); - dest->y = fixmuldiv(dest->y,n,d); - dest->z = fixmuldiv(dest->z,n,d); -#endif - - return dest; -} - -fix vm_vec_dotprod(const vms_vector *v0,const vms_vector *v1) -{ -#if 0 - quadint q; - - q.low = q.high = 0; - - fixmulaccum(&q,v0->x,v1->x); - fixmulaccum(&q,v0->y,v1->y); - fixmulaccum(&q,v0->z,v1->z); - - return fixquadadjust(&q); -#else - long long p = - (long long) v0->x * v1->x - + (long long) v0->y * v1->y - + (long long) v0->z * v1->z; - /* Convert back to fix and return. */ - return p >> 16; -#endif -} - -fix vm_vec_dot3(fix x,fix y,fix z,vms_vector *v) -{ -#if 0 - quadint q; - - q.low = q.high = 0; - - fixmulaccum(&q,x,v->x); - fixmulaccum(&q,y,v->y); - fixmulaccum(&q,z,v->z); - - return fixquadadjust(&q); -#else - long long p = - (long long) x * v->x - + (long long) y * v->y - + (long long) z * v->z; - /* Convert back to fix and return. */ - return p >> 16; -#endif -} - -//returns magnitude of a vector -fix vm_vec_mag(vms_vector *v) -{ - quadint q; - - q.low = q.high = 0; - - fixmulaccum(&q,v->x,v->x); - fixmulaccum(&q,v->y,v->y); - fixmulaccum(&q,v->z,v->z); - - return quad_sqrt(q.low,q.high); -} - -//computes the distance between two points. (does sub and mag) -fix vm_vec_dist(const vms_vector *v0,const vms_vector *v1) -{ - vms_vector t; - - vm_vec_sub(&t,v0,v1); - - return vm_vec_mag(&t); -} - - -//computes an approximation of the magnitude of the vector -//uses dist = largest + next_largest*3/8 + smallest*3/16 -fix vm_vec_mag_quick(vms_vector *v) -{ - fix a,b,c,bc; - - a = labs(v->x); - b = labs(v->y); - c = labs(v->z); - - if (a < b) { - fix t=a; a=b; b=t; - } - - if (b < c) { - fix t=b; b=c; c=t; - - if (a < b) { - fix t=a; a=b; b=t; - } - } - - bc = (b>>2) + (c>>3); - - return a + bc + (bc>>1); -} - - -//computes an approximation of the distance between two points. -//uses dist = largest + next_largest*3/8 + smallest*3/16 -fix vm_vec_dist_quick(vms_vector *v0,vms_vector *v1) -{ - vms_vector t; - - vm_vec_sub(&t,v0,v1); - - return vm_vec_mag_quick(&t); -} - -//normalize a vector. returns mag of source vec -fix vm_vec_copy_normalize(vms_vector *dest,vms_vector *src) -{ - fix m; - - m = vm_vec_mag(src); - - if (m > 0) { - dest->x = fixdiv(src->x,m); - dest->y = fixdiv(src->y,m); - dest->z = fixdiv(src->z,m); - } - - return m; -} - -//normalize a vector. returns mag of source vec -fix vm_vec_normalize(vms_vector *v) -{ - return vm_vec_copy_normalize(v,v); -} - -#ifndef USE_ISQRT -//normalize a vector. returns mag of source vec. uses approx mag -fix vm_vec_copy_normalize_quick(vms_vector *dest,vms_vector *src) -{ - fix m; - - m = vm_vec_mag_quick(src); - - if (m > 0) { - dest->x = fixdiv(src->x,m); - dest->y = fixdiv(src->y,m); - dest->z = fixdiv(src->z,m); - } - - return m; -} - -#else -//these routines use an approximation for 1/sqrt - -//returns approximation of 1/magnitude of a vector -fix vm_vec_imag(vms_vector *v) -{ - quadint q; - - q.low = q.high = 0; - - fixmulaccum(&q,v->x,v->x); - fixmulaccum(&q,v->y,v->y); - fixmulaccum(&q,v->z,v->z); - - if (q.high==0) - return fix_isqrt(fixquadadjust(&q)); - else if (q.high >= 0x800000) { - return (fix_isqrt(q.high) >> 8); - } - else - return (fix_isqrt((q.high<<8) + (q.low>>24)) >> 4); -} - -//normalize a vector. returns 1/mag of source vec. uses approx 1/mag -fix vm_vec_copy_normalize_quick(vms_vector *dest,vms_vector *src) -{ - fix im; - - im = vm_vec_imag(src); - - dest->x = fixmul(src->x,im); - dest->y = fixmul(src->y,im); - dest->z = fixmul(src->z,im); - - return im; -} - -#endif - -//normalize a vector. returns 1/mag of source vec. uses approx 1/mag -fix vm_vec_normalize_quick(vms_vector *v) -{ - return vm_vec_copy_normalize_quick(v,v); -} - -//return the normalized direction vector between two points -//dest = normalized(end - start). Returns 1/mag of direction vector -//NOTE: the order of the parameters matches the vector subtraction -fix vm_vec_normalized_dir_quick(vms_vector *dest,vms_vector *end,vms_vector *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 -fix vm_vec_normalized_dir(vms_vector *dest,vms_vector *end,vms_vector *start) -{ - vm_vec_sub(dest,end,start); - - return vm_vec_normalize(dest); -} - -//computes surface normal from three points. result is normalized -//returns ptr to dest -//dest CANNOT equal either source -vms_vector *vm_vec_normal(vms_vector *dest,vms_vector *p0,vms_vector *p1,vms_vector *p2) -{ - vm_vec_perp(dest,p0,p1,p2); - - vm_vec_normalize(dest); - - return dest; -} - -//make sure a vector is reasonably sized to go into a cross product -void check_vec(vms_vector *v) -{ - fix check; - int cnt = 0; - - check = labs(v->x) | labs(v->y) | labs(v->z); - - if (check == 0) - return; - - if (check & 0xfffc0000) { //too big - - while (check & 0xfff00000) { - cnt += 4; - check >>= 4; - } - - while (check & 0xfffc0000) { - cnt += 2; - check >>= 2; - } - - v->x >>= cnt; - v->y >>= cnt; - v->z >>= cnt; - } - else //maybe too small - if ((check & 0xffff8000) == 0) { //yep, too small - - while ((check & 0xfffff000) == 0) { - cnt += 4; - check <<= 4; - } - - while ((check & 0xffff8000) == 0) { - cnt += 2; - check <<= 2; - } - - v->x >>= cnt; - v->y >>= cnt; - v->z >>= cnt; - } -} - -//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. -//#ifndef __powerc -#if 0 -vms_vector *vm_vec_crossprod(vms_vector *dest,vms_vector *src0,vms_vector *src1) -{ - double d; - Assert(dest!=src0 && dest!=src1); - - d = (double)(src0->y) * (double)(src1->z); - d += (double)-(src0->z) * (double)(src1->y); - d /= 65536.0; - if (d < 0.0) - d = d - 1.0; - dest->x = (fix)d; - - d = (double)(src0->z) * (double)(src1->x); - d += (double)-(src0->x) * (double)(src1->z); - d /= 65536.0; - if (d < 0.0) - d = d - 1.0; - dest->y = (fix)d; - - d = (double)(src0->x) * (double)(src1->y); - d += (double)-(src0->y) * (double)(src1->x); - d /= 65536.0; - if (d < 0.0) - d = d - 1.0; - dest->z = (fix)d; - - return dest; -} -#else - -vms_vector *vm_vec_crossprod(vms_vector *dest,vms_vector *src0,vms_vector *src1) -{ - quadint q; - - Assert(dest!=src0 && dest!=src1); - - q.low = q.high = 0; - fixmulaccum(&q,src0->y,src1->z); - fixmulaccum(&q,-src0->z,src1->y); - dest->x = fixquadadjust(&q); - - q.low = q.high = 0; - fixmulaccum(&q,src0->z,src1->x); - fixmulaccum(&q,-src0->x,src1->z); - dest->y = fixquadadjust(&q); - - q.low = q.high = 0; - fixmulaccum(&q,src0->x,src1->y); - fixmulaccum(&q,-src0->y,src1->x); - dest->z = fixquadadjust(&q); - - return dest; -} - -#endif - - -//computes non-normalized surface normal from three points. -//returns ptr to dest -//dest CANNOT equal either source -vms_vector *vm_vec_perp(vms_vector *dest,vms_vector *p0,vms_vector *p1,vms_vector *p2) -{ - vms_vector t0,t1; - - vm_vec_sub(&t0,p1,p0); - vm_vec_sub(&t1,p2,p1); - - check_vec(&t0); - check_vec(&t1); - - 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. -fixang vm_vec_delta_ang(vms_vector *v0,vms_vector *v1,vms_vector *fvec) -{ - vms_vector t0,t1; - - vm_vec_copy_normalize(&t0,v0); - vm_vec_copy_normalize(&t1,v1); - - return vm_vec_delta_ang_norm(&t0,&t1,fvec); -} - -//computes the delta angle between two normalized vectors. -fixang vm_vec_delta_ang_norm(vms_vector *v0,vms_vector *v1,vms_vector *fvec) -{ - fixang a; - - a = fix_acos(vm_vec_dot(v0,v1)); - - if (fvec) { - vms_vector t; - - vm_vec_cross(&t,v0,v1); - - if (vm_vec_dot(&t,fvec) < 0) - a = -a; - } - - return a; -} - -vms_matrix *sincos_2_matrix(vms_matrix *m,fix sinp,fix cosp,fix sinb,fix cosb,fix sinh,fix cosh) -{ - fix sbsh,cbch,cbsh,sbch; - - sbsh = fixmul(sinb,sinh); - cbch = fixmul(cosb,cosh); - cbsh = fixmul(cosb,sinh); - sbch = fixmul(sinb,cosh); - - m->rvec.x = cbch + fixmul(sinp,sbsh); //m1 - m->uvec.z = sbsh + fixmul(sinp,cbch); //m8 - - m->uvec.x = fixmul(sinp,cbsh) - sbch; //m2 - m->rvec.z = fixmul(sinp,sbch) - cbsh; //m7 - - m->fvec.x = fixmul(sinh,cosp); //m3 - m->rvec.y = fixmul(sinb,cosp); //m4 - m->uvec.y = fixmul(cosb,cosp); //m5 - m->fvec.z = fixmul(cosh,cosp); //m9 - - m->fvec.y = -sinp; //m6 - - return m; - -} - -//computes a matrix from a set of three angles. returns ptr to matrix -vms_matrix *vm_angles_2_matrix(vms_matrix *m,vms_angvec *a) -{ - fix sinp,cosp,sinb,cosb,sinh,cosh; - - fix_sincos(a->p,&sinp,&cosp); - fix_sincos(a->b,&sinb,&cosb); - fix_sincos(a->h,&sinh,&cosh); - - return sincos_2_matrix(m,sinp,cosp,sinb,cosb,sinh,cosh); - -} - -//computes a matrix from a forward vector and an angle -vms_matrix *vm_vec_ang_2_matrix(vms_matrix *m,vms_vector *v,fixang a) -{ - fix sinb,cosb,sinp,cosp,sinh,cosh; - - fix_sincos(a,&sinb,&cosb); - - sinp = -v->y; - cosp = fix_sqrt(f1_0 - fixmul(sinp,sinp)); - - sinh = fixdiv(v->x,cosp); - cosh = fixdiv(v->z,cosp); - - return sincos_2_matrix(m,sinp,cosp,sinb,cosb,sinh,cosh); -} - - -//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 -vms_matrix *vm_vector_2_matrix(vms_matrix *m,vms_vector *fvec,vms_vector *uvec,vms_vector *rvec) -{ - vms_vector *xvec=&m->rvec,*yvec=&m->uvec,*zvec=&m->fvec; - - Assert(fvec != NULL); - - if (vm_vec_copy_normalize(zvec,fvec) == 0) { - Int3(); //forward vec should not be zero-length - return m; - } - - if (uvec == NULL) { - - if (rvec == NULL) { //just forward vec - -bad_vector2: - ; - - if (zvec->x==0 && zvec->z==0) { //forward vec is straight up or down - - m->rvec.x = f1_0; - m->uvec.z = (zvec->y<0)?f1_0:-f1_0; - - m->rvec.y = m->rvec.z = m->uvec.x = m->uvec.y = 0; - } - else { //not straight up or down - - xvec->x = zvec->z; - xvec->y = 0; - xvec->z = -zvec->x; - - vm_vec_normalize(xvec); - - vm_vec_crossprod(yvec,zvec,xvec); - - } - - } - else { //use right vec - - if (vm_vec_copy_normalize(xvec,rvec) == 0) - goto bad_vector2; - - vm_vec_crossprod(yvec,zvec,xvec); - - //normalize new perpendicular vector - if (vm_vec_normalize(yvec) == 0) - 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) - goto bad_vector2; - - vm_vec_crossprod(xvec,yvec,zvec); - - //normalize new perpendicular vector - if (vm_vec_normalize(xvec) == 0) - 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 -vms_matrix *vm_vector_2_matrix_norm(vms_matrix *m,vms_vector *fvec,vms_vector *uvec,vms_vector *rvec) -{ - vms_vector *xvec=&m->rvec,*yvec=&m->uvec,*zvec=&m->fvec; - - Assert(fvec != NULL); - - if (uvec == NULL) { - - if (rvec == NULL) { //just forward vec - -bad_vector2: - ; - - if (zvec->x==0 && zvec->z==0) { //forward vec is straight up or down - - m->rvec.x = f1_0; - m->uvec.z = (zvec->y<0)?f1_0:-f1_0; - - m->rvec.y = m->rvec.z = m->uvec.x = m->uvec.y = 0; - } - else { //not straight up or down - - xvec->x = zvec->z; - xvec->y = 0; - xvec->z = -zvec->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) - 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) - 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 -vms_vector *vm_vec_rotate(vms_vector *dest,const vms_vector *src,const vms_matrix *m) -{ - Assert(dest != src); - - dest->x = vm_vec_dot(src,&m->rvec); - dest->y = vm_vec_dot(src,&m->uvec); - dest->z = vm_vec_dot(src,&m->fvec); - - return dest; -} - - -//transpose a matrix in place. returns ptr to matrix -vms_matrix *vm_transpose_matrix(vms_matrix *m) -{ - fix t; - - t = m->uvec.x; m->uvec.x = m->rvec.y; m->rvec.y = t; - t = m->fvec.x; m->fvec.x = m->rvec.z; m->rvec.z = t; - t = m->fvec.y; m->fvec.y = m->uvec.z; m->uvec.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 -vms_matrix *vm_copy_transpose_matrix(vms_matrix *dest,vms_matrix *src) -{ - Assert(dest != src); - - dest->rvec.x = src->rvec.x; - dest->rvec.y = src->uvec.x; - dest->rvec.z = src->fvec.x; - - dest->uvec.x = src->rvec.y; - dest->uvec.y = src->uvec.y; - dest->uvec.z = src->fvec.y; - - dest->fvec.x = src->rvec.z; - dest->fvec.y = src->uvec.z; - dest->fvec.z = src->fvec.z; - - return dest; -} - -//mulitply 2 matrices, fill in dest. returns ptr to dest -//dest CANNOT equal either source -vms_matrix *vm_matrix_x_matrix(vms_matrix *dest,vms_matrix *src0,vms_matrix *src1) -{ - Assert(dest!=src0 && dest!=src1); - - dest->rvec.x = vm_vec_dot3(src0->rvec.x,src0->uvec.x,src0->fvec.x, &src1->rvec); - dest->uvec.x = vm_vec_dot3(src0->rvec.x,src0->uvec.x,src0->fvec.x, &src1->uvec); - dest->fvec.x = vm_vec_dot3(src0->rvec.x,src0->uvec.x,src0->fvec.x, &src1->fvec); - - dest->rvec.y = vm_vec_dot3(src0->rvec.y,src0->uvec.y,src0->fvec.y, &src1->rvec); - dest->uvec.y = vm_vec_dot3(src0->rvec.y,src0->uvec.y,src0->fvec.y, &src1->uvec); - dest->fvec.y = vm_vec_dot3(src0->rvec.y,src0->uvec.y,src0->fvec.y, &src1->fvec); - - dest->rvec.z = vm_vec_dot3(src0->rvec.z,src0->uvec.z,src0->fvec.z, &src1->rvec); - dest->uvec.z = vm_vec_dot3(src0->rvec.z,src0->uvec.z,src0->fvec.z, &src1->uvec); - dest->fvec.z = vm_vec_dot3(src0->rvec.z,src0->uvec.z,src0->fvec.z, &src1->fvec); - - return dest; -} - -//extract angles from a matrix -vms_angvec *vm_extract_angles_matrix(vms_angvec *a,vms_matrix *m) -{ - fix sinh,cosh,cosp; - - if (m->fvec.x==0 && m->fvec.z==0) //zero head - a->h = 0; - else - a->h = fix_atan2(m->fvec.z,m->fvec.x); - - fix_sincos(a->h,&sinh,&cosh); - - if (abs(sinh) > abs(cosh)) //sine is larger, so use it - cosp = fixdiv(m->fvec.x,sinh); - else //cosine is larger, so use it - cosp = fixdiv(m->fvec.z,cosh); - - if (cosp==0 && m->fvec.y==0) - a->p = 0; - else - a->p = fix_atan2(cosp,-m->fvec.y); - - - if (cosp == 0) //the cosine of pitch is zero. we're pitched straight up. say no bank - - a->b = 0; - - else { - fix sinb,cosb; - - sinb = fixdiv(m->rvec.y,cosp); - cosb = fixdiv(m->uvec.y,cosp); - - if (sinb==0 && cosb==0) - a->b = 0; - else - a->b = fix_atan2(cosb,sinb); - - } - - return a; -} - - -//extract heading and pitch from a vector, assuming bank==0 -vms_angvec *vm_extract_angles_vector_normalized(vms_angvec *a,vms_vector *v) -{ - a->b = 0; //always zero bank - - a->p = fix_asin(-v->y); - - if (v->x==0 && v->z==0) - a->h = 0; - else - a->h = fix_atan2(v->z,v->x); - - return a; -} - -//extract heading and pitch from a vector, assuming bank==0 -vms_angvec *vm_extract_angles_vector(vms_angvec *a,vms_vector *v) -{ - vms_vector t; - - if (vm_vec_copy_normalize(&t,v) != 0) - 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 -fix vm_dist_to_plane(const vms_vector *checkp,const vms_vector *norm,const vms_vector *planep) -{ - vms_vector t; - - vm_vec_sub(&t,checkp,planep); - - return vm_vec_dot(&t,norm); - -} - -vms_vector *vm_vec_make(vms_vector *v,fix x,fix y,fix z) -{ - v->x=x; v->y=y; v->z=z; - return v; -} - -// convert vms_matrix to vms_quaternion -void vms_quaternion_from_matrix(vms_quaternion * q, const vms_matrix * m) -{ - fix tr = m->rvec.x + m->uvec.y + m->fvec.z; - if (tr > 0) { - fix s = fixmul(fix_sqrt(tr + fl2f(1.0)), fl2f(2.0)); - q->w = fixmul(fl2f(0.25), s) * .5; - q->x = fixdiv(m->fvec.y - m->uvec.z, s) * .5; - q->y = fixdiv(m->rvec.z - m->fvec.x, s) * .5; - q->z = fixdiv(m->uvec.x - m->rvec.y, s) * .5; - } else if ((m->rvec.x > m->uvec.y)&(m->rvec.x > m->fvec.z)) { - fix s = fixmul(fix_sqrt(fl2f(1.0) + m->rvec.x - m->uvec.y - m->fvec.z), fl2f(2.0)); - q->w = fixdiv(m->fvec.y - m->uvec.z, s) * .5; - q->x = fixmul(fl2f(0.25), s) * .5; - q->y = fixdiv(m->rvec.y + m->uvec.x, s) * .5; - q->z = fixdiv(m->rvec.z + m->fvec.x, s) * .5; - } else if (m->uvec.y > m->fvec.z) { - fix s = fixmul(fix_sqrt(fl2f(1.0) + m->uvec.y - m->rvec.x - m->fvec.z), fl2f(2.0)); - q->w = fixdiv(m->rvec.z - m->fvec.x, s) * .5; - q->x = fixdiv(m->rvec.y + m->uvec.x ,s) * .5; - q->y = fixmul(fl2f(0.25), s) * .5; - q->z = fixdiv(m->uvec.z + m->fvec.y , s) * .5; - } else { - fix s = fixmul(fix_sqrt(fl2f(1.0) + m->fvec.z - m->rvec.x - m->uvec.y), fl2f(2.0)); - q->w = fixdiv(m->uvec.x - m->rvec.y , s) * .5; - q->x = fixdiv(m->rvec.z + m->fvec.x , s) * .5; - q->y = fixdiv(m->uvec.z + m->fvec.y, s) * .5; - q->z = fixmul(fl2f(0.25), s) * .5; - } -} - -// convert vms_quaternion to vms_matrix -void vms_matrix_from_quaternion(vms_matrix * m, const vms_quaternion * q) -{ - fix sqw = fixmul(q->w * 2, q->w * 2); - fix sqx = fixmul(q->x * 2, q->x * 2); - fix sqy = fixmul(q->y * 2, q->y * 2); - fix sqz = fixmul(q->z * 2, q->z * 2); - fix invs = fixdiv(fl2f(1.0), (sqw + sqx + sqy + sqz)); - fix tmp1, tmp2; - - m->rvec.x = fixmul(sqx - sqy - sqz + sqw, invs); - m->uvec.y = fixmul(-sqx + sqy - sqz + sqw, invs); - m->fvec.z = fixmul(-sqx - sqy + sqz + sqw, invs); - - tmp1 = fixmul(q->x * 2, q->y * 2); - tmp2 = fixmul(q->z * 2, q->w * 2); - m->uvec.x = fixmul(fixmul(fl2f(2.0), (tmp1 + tmp2)), invs); - m->rvec.y = fixmul(fixmul(fl2f(2.0), (tmp1 - tmp2)), invs); - - tmp1 = fixmul(q->x * 2, q->z * 2); - tmp2 = fixmul(q->y * 2, q->w * 2); - m->fvec.x = fixmul(fixmul(fl2f(2.0), (tmp1 - tmp2)), invs); - m->rvec.z = fixmul(fixmul(fl2f(2.0), (tmp1 + tmp2)), invs); - - tmp1 = fixmul(q->y * 2, q->z * 2); - tmp2 = fixmul(q->x * 2, q->w * 2); - m->fvec.y = fixmul(fixmul(fl2f(2.0), (tmp1 + tmp2)), invs); - m->uvec.z = fixmul(fixmul(fl2f(2.0), (tmp1 - tmp2)), invs); -}