Subversion Repositories Games.Descent

Rev

Blame | Last modification | View Log | Download | RSS feed

  1. /*
  2.  * This file is part of the DXX-Rebirth project <https://www.dxx-rebirth.com/>.
  3.  * It is copyright by its individual contributors, as recorded in the
  4.  * project's Git history.  See COPYING.txt at the top level for license
  5.  * terms and a link to the Git history.
  6.  */
  7.  
  8. /*
  9.  *
  10.  * C version of vecmat library
  11.  *
  12.  */
  13.  
  14. #include <stdlib.h>
  15. #include <stdint.h>
  16. #include <math.h>           // for sqrt
  17.  
  18. #include "maths.h"
  19. #include "vecmat.h"
  20. #include "dxxerror.h"
  21.  
  22. namespace dcx {
  23.  
  24. //#define USE_ISQRT 1
  25.  
  26. constexpr vms_matrix vmd_identity_matrix = IDENTITY_MATRIX;
  27.  
  28. //adds two vectors, fills in dest, returns ptr to dest
  29. //ok for dest to equal either source, but should use vm_vec_add2() if so
  30. vms_vector &vm_vec_add(vms_vector &dest,const vms_vector &src0,const vms_vector &src1)
  31. {
  32.         dest.x = src0.x + src1.x;
  33.         dest.y = src0.y + src1.y;
  34.         dest.z = src0.z + src1.z;
  35.         return dest;
  36. }
  37.  
  38.  
  39. //subs two vectors, fills in dest, returns ptr to dest
  40. //ok for dest to equal either source, but should use vm_vec_sub2() if so
  41. vms_vector &_vm_vec_sub(vms_vector &dest,const vms_vector &src0,const vms_vector &src1)
  42. {
  43.         dest.x = src0.x - src1.x;
  44.         dest.y = src0.y - src1.y;
  45.         dest.z = src0.z - src1.z;
  46.         return dest;
  47. }
  48.  
  49. //adds one vector to another. returns ptr to dest
  50. //dest can equal source
  51. void vm_vec_add2(vms_vector &dest,const vms_vector &src)
  52. {
  53.         dest.x += src.x;
  54.         dest.y += src.y;
  55.         dest.z += src.z;
  56. }
  57.  
  58. //subs one vector from another, returns ptr to dest
  59. //dest can equal source
  60. void vm_vec_sub2(vms_vector &dest,const vms_vector &src)
  61. {
  62.         dest.x -= src.x;
  63.         dest.y -= src.y;
  64.         dest.z -= src.z;
  65. }
  66.  
  67. static inline fix avg_fix(fix64 a, fix64 b)
  68. {
  69.         return (a + b) / 2;
  70. }
  71.  
  72. //averages two vectors. returns ptr to dest
  73. //dest can equal either source
  74. void vm_vec_avg(vms_vector &dest,const vms_vector &src0,const vms_vector &src1)
  75. {
  76.         dest.x = avg_fix(src0.x, src1.x);
  77.         dest.y = avg_fix(src0.y, src1.y);
  78.         dest.z = avg_fix(src0.z, src1.z);
  79. }
  80.  
  81. //scales a vector in place.  returns ptr to vector
  82. vms_vector &vm_vec_scale(vms_vector &dest,fix s)
  83. {
  84.         return vm_vec_copy_scale(dest, dest, s);
  85. }
  86.  
  87. //scales and copies a vector.  returns ptr to dest
  88. vms_vector &vm_vec_copy_scale(vms_vector &dest,const vms_vector &src,fix s)
  89. {
  90.         dest.x = fixmul(src.x,s);
  91.         dest.y = fixmul(src.y,s);
  92.         dest.z = fixmul(src.z,s);
  93.         return dest;
  94. }
  95.  
  96. //scales a vector, adds it to another, and stores in a 3rd vector
  97. //dest = src1 + k * src2
  98. void vm_vec_scale_add(vms_vector &dest,const vms_vector &src1,const vms_vector &src2,fix k)
  99. {
  100.         dest.x = src1.x + fixmul(src2.x,k);
  101.         dest.y = src1.y + fixmul(src2.y,k);
  102.         dest.z = src1.z + fixmul(src2.z,k);
  103. }
  104.  
  105. //scales a vector and adds it to another
  106. //dest += k * src
  107. void vm_vec_scale_add2(vms_vector &dest,const vms_vector &src,fix k)
  108. {
  109.         dest.x += fixmul(src.x,k);
  110.         dest.y += fixmul(src.y,k);
  111.         dest.z += fixmul(src.z,k);
  112. }
  113.  
  114. //scales a vector in place, taking n/d for scale.  returns ptr to vector
  115. //dest *= n/d
  116. void vm_vec_scale2(vms_vector &dest,fix n,fix d)
  117. {
  118. #if 0 // DPH: Kludge: this was overflowing a lot, so I made it use the FPU.
  119.         float nd;
  120.         nd = f2fl(n) / f2fl(d);
  121.         dest.x = fl2f( f2fl(dest.x) * nd);
  122.         dest.y = fl2f( f2fl(dest.y) * nd);
  123.         dest.z = fl2f( f2fl(dest.z) * nd);
  124. #else
  125.         dest.x = fixmuldiv(dest.x,n,d);
  126.         dest.y = fixmuldiv(dest.y,n,d);
  127.         dest.z = fixmuldiv(dest.z,n,d);
  128. #endif
  129. }
  130.  
  131. static fix vm_vec_dot3(fix x,fix y,fix z,const vms_vector &v) __attribute_warn_unused_result;
  132. static fix vm_vec_dot3(fix x,fix y,fix z,const vms_vector &v)
  133. {
  134. #if 0
  135.         quadint q;
  136.  
  137.         q.low = q.high = 0;
  138.  
  139.         fixmulaccum(&q,x,v->x);
  140.         fixmulaccum(&q,y,v->y);
  141.         fixmulaccum(&q,z,v->z);
  142.  
  143.         return fixquadadjust(&q);
  144. #else
  145.         int64_t x0 = x;
  146.         int64_t x1 = v.x;
  147.         int64_t y0 = y;
  148.         int64_t y1 = v.y;
  149.         int64_t z0 = z;
  150.         int64_t z1 = v.z;
  151.         int64_t p = (x0 * x1) + (y0 * y1) + (z0 * z1);
  152.         /* Convert back to fix and return. */
  153.         return p >> 16;
  154. #endif
  155. }
  156.  
  157. fix vm_vec_dot(const vms_vector &v0,const vms_vector &v1)
  158. {
  159.         return vm_vec_dot3(v0.x, v0.y, v0.z, v1);
  160. }
  161.  
  162. //returns magnitude of a vector
  163. vm_magnitude_squared vm_vec_mag2(const vms_vector &v)
  164. {
  165.         const int64_t x = v.x;
  166.         const int64_t y = v.y;
  167.         const int64_t z = v.z;
  168.         return vm_magnitude_squared{static_cast<uint64_t>((x * x) + (y * y) + (z * z))};
  169. }
  170.  
  171. vm_magnitude vm_vec_mag(const vms_vector &v)
  172. {
  173.         quadint q;
  174.         q.q = vm_vec_mag2(v).d2;
  175.         return vm_magnitude{quad_sqrt(q)};
  176. }
  177.  
  178. //computes the distance between two points. (does sub and mag)
  179. vm_distance vm_vec_dist(const vms_vector &v0,const vms_vector &v1)
  180. {
  181.         return vm_vec_mag(vm_vec_sub(v0,v1));
  182. }
  183.  
  184. vm_distance_squared vm_vec_dist2(const vms_vector &v0,const vms_vector &v1)
  185. {
  186.         return vm_vec_mag2(vm_vec_sub(v0,v1));
  187. }
  188.  
  189. //computes an approximation of the magnitude of the vector
  190. //uses dist = largest + next_largest*3/8 + smallest*3/16
  191. vm_magnitude vm_vec_mag_quick(const vms_vector &v)
  192. {
  193.         fix a,b,c,bc;
  194.  
  195.         a = labs(v.x);
  196.         b = labs(v.y);
  197.         c = labs(v.z);
  198.  
  199.         if (a < b) {
  200.                 std::swap(a, b);
  201.         }
  202.  
  203.         if (b < c) {
  204.                 std::swap(b, c);
  205.                 if (a < b) {
  206.                         std::swap(a, b);
  207.                 }
  208.         }
  209.  
  210.         bc = (b>>2) + (c>>3);
  211.  
  212.         return vm_magnitude{static_cast<uint32_t>(a + bc + (bc>>1))};
  213. }
  214.  
  215.  
  216. //computes an approximation of the distance between two points.
  217. //uses dist = largest + next_largest*3/8 + smallest*3/16
  218. vm_distance vm_vec_dist_quick(const vms_vector &v0,const vms_vector &v1)
  219. {
  220.         return vm_vec_mag_quick(vm_vec_sub(v0,v1));
  221. }
  222.  
  223. //normalize a vector. returns mag of source vec
  224. vm_magnitude vm_vec_copy_normalize(vms_vector &dest,const vms_vector &src)
  225. {
  226.         auto m = vm_vec_mag(src);
  227.         if (m) {
  228.                 vm_vec_divide(dest, src, m);
  229.         }
  230.         return m;
  231. }
  232.  
  233. void vm_vec_divide(vms_vector &dest,const vms_vector &src, fix m)
  234. {
  235.         dest.x = fixdiv(src.x,m);
  236.         dest.y = fixdiv(src.y,m);
  237.         dest.z = fixdiv(src.z,m);
  238. }
  239.  
  240. //normalize a vector. returns mag of source vec
  241. vm_magnitude vm_vec_normalize(vms_vector &v)
  242. {
  243.         return vm_vec_copy_normalize(v,v);
  244. }
  245.  
  246. //normalize a vector. returns mag of source vec. uses approx mag
  247. vm_magnitude vm_vec_copy_normalize_quick(vms_vector &dest,const vms_vector &src)
  248. {
  249.         auto m = vm_vec_mag_quick(src);
  250.         if (m) {
  251.                 vm_vec_divide(dest, src, m);
  252.         }
  253.         return m;
  254. }
  255.  
  256. //normalize a vector. returns 1/mag of source vec. uses approx 1/mag
  257. vm_magnitude vm_vec_normalize_quick(vms_vector &v)
  258. {
  259.         return vm_vec_copy_normalize_quick(v,v);
  260. }
  261.  
  262. //return the normalized direction vector between two points
  263. //dest = normalized(end - start).  Returns 1/mag of direction vector
  264. //NOTE: the order of the parameters matches the vector subtraction
  265. vm_magnitude vm_vec_normalized_dir_quick(vms_vector &dest,const vms_vector &end,const vms_vector &start)
  266. {
  267.         return vm_vec_normalize_quick(vm_vec_sub(dest,end,start));
  268. }
  269.  
  270. //return the normalized direction vector between two points
  271. //dest = normalized(end - start).  Returns mag of direction vector
  272. //NOTE: the order of the parameters matches the vector subtraction
  273. vm_magnitude vm_vec_normalized_dir(vms_vector &dest,const vms_vector &end,const vms_vector &start)
  274. {
  275.         return vm_vec_normalize(vm_vec_sub(dest,end,start));
  276. }
  277.  
  278. //computes surface normal from three points. result is normalized
  279. //returns ptr to dest
  280. //dest CANNOT equal either source
  281. void vm_vec_normal(vms_vector &dest,const vms_vector &p0,const vms_vector &p1,const vms_vector &p2)
  282. {
  283.         vm_vec_perp(dest,p0,p1,p2);
  284.         vm_vec_normalize(dest);
  285. }
  286.  
  287. //make sure a vector is reasonably sized to go into a cross product
  288. static void check_vec(vms_vector *v)
  289. {
  290.         fix check;
  291.         int cnt = 0;
  292.  
  293.         check = labs(v->x) | labs(v->y) | labs(v->z);
  294.        
  295.         if (check == 0)
  296.                 return;
  297.  
  298.         if (check & 0xfffc0000) {               //too big
  299.  
  300.                 while (check & 0xfff00000) {
  301.                         cnt += 4;
  302.                         check >>= 4;
  303.                 }
  304.  
  305.                 while (check & 0xfffc0000) {
  306.                         cnt += 2;
  307.                         check >>= 2;
  308.                 }
  309.  
  310.                 v->x >>= cnt;
  311.                 v->y >>= cnt;
  312.                 v->z >>= cnt;
  313.         }
  314.         else                                                                                            //maybe too small
  315.                 if ((check & 0xffff8000) == 0) {                //yep, too small
  316.  
  317.                         while ((check & 0xfffff000) == 0) {
  318.                                 cnt += 4;
  319.                                 check <<= 4;
  320.                         }
  321.  
  322.                         while ((check & 0xffff8000) == 0) {
  323.                                 cnt += 2;
  324.                                 check <<= 2;
  325.                         }
  326.  
  327.                         v->x >>= cnt;
  328.                         v->y >>= cnt;
  329.                         v->z >>= cnt;
  330.                 }
  331. }
  332.  
  333. //computes cross product of two vectors.
  334. //Note: this magnitude of the resultant vector is the
  335. //product of the magnitudes of the two source vectors.  This means it is
  336. //quite easy for this routine to overflow and underflow.  Be careful that
  337. //your inputs are ok.
  338. void vm_vec_cross(vms_vector &dest,const vms_vector &src0,const vms_vector &src1)
  339. {
  340.         quadint q;
  341.  
  342.         Assert(&dest!=&src0 && &dest!=&src1);
  343.  
  344.         q.q = 0;
  345.         fixmulaccum(&q,src0.y,src1.z);
  346.         fixmulaccum(&q,-src0.z,src1.y);
  347.         dest.x = fixquadadjust(&q);
  348.  
  349.         q.q = 0;
  350.         fixmulaccum(&q,src0.z,src1.x);
  351.         fixmulaccum(&q,-src0.x,src1.z);
  352.         dest.y = fixquadadjust(&q);
  353.  
  354.         q.q = 0;
  355.         fixmulaccum(&q,src0.x,src1.y);
  356.         fixmulaccum(&q,-src0.y,src1.x);
  357.         dest.z = fixquadadjust(&q);
  358. }
  359.  
  360. //computes non-normalized surface normal from three points.
  361. //returns ptr to dest
  362. //dest CANNOT equal either source
  363. void vm_vec_perp(vms_vector &dest,const vms_vector &p0,const vms_vector &p1,const vms_vector &p2)
  364. {
  365.         auto t0 = vm_vec_sub(p1,p0);
  366.         auto t1 = vm_vec_sub(p2,p1);
  367.         check_vec(&t0);
  368.         check_vec(&t1);
  369.         vm_vec_cross(dest,t0,t1);
  370. }
  371.  
  372.  
  373. //computes the delta angle between two vectors.
  374. //vectors need not be normalized. if they are, call vm_vec_delta_ang_norm()
  375. //the forward vector (third parameter) can be NULL, in which case the absolute
  376. //value of the angle in returned.  Otherwise the angle around that vector is
  377. //returned.
  378. fixang vm_vec_delta_ang(const vms_vector &v0,const vms_vector &v1,const vms_vector &fvec)
  379. {
  380.         vms_vector t0,t1;
  381.  
  382.         if (!vm_vec_copy_normalize(t0,v0) || !vm_vec_copy_normalize(t1,v1))
  383.                 return 0;
  384.  
  385.         return vm_vec_delta_ang_norm(t0,t1,fvec);
  386. }
  387.  
  388. //computes the delta angle between two normalized vectors.
  389. fixang vm_vec_delta_ang_norm(const vms_vector &v0,const vms_vector &v1,const vms_vector &fvec)
  390. {
  391.         fixang a;
  392.  
  393.         a = fix_acos(vm_vec_dot(v0,v1));
  394.                 if (vm_vec_dot(vm_vec_cross(v0,v1),fvec) < 0)
  395.                         a = -a;
  396.         return a;
  397. }
  398.  
  399. static void sincos_2_matrix(vms_matrix &m, const fixang bank, const fix_sincos_result p, const fix_sincos_result h)
  400. {
  401. #define DXX_S2M_DECL(V) \
  402.         const auto &sin##V = V.sin;     \
  403.         const auto &cos##V = V.cos
  404.         DXX_S2M_DECL(p);
  405.         DXX_S2M_DECL(h);
  406.         m.fvec.y = -sinp;                                                               //m6
  407.         m.fvec.x = fixmul(sinh,cosp);                           //m3
  408.         m.fvec.z = fixmul(cosh,cosp);                           //m9
  409.         const auto &&b = fix_sincos(bank);
  410.         DXX_S2M_DECL(b);
  411. #undef DXX_S2M_DECL
  412.         m.rvec.y = fixmul(sinb,cosp);                           //m4
  413.         m.uvec.y = fixmul(cosb,cosp);                           //m5
  414.  
  415.         const auto cbch = fixmul(cosb,cosh);
  416.         const auto sbsh = fixmul(sinb,sinh);
  417.         m.rvec.x = cbch + fixmul(sinp,sbsh);            //m1
  418.         m.uvec.z = sbsh + fixmul(sinp,cbch);            //m8
  419.  
  420.         const auto sbch = fixmul(sinb,cosh);
  421.         const auto cbsh = fixmul(cosb,sinh);
  422.         m.uvec.x = fixmul(sinp,cbsh) - sbch;            //m2
  423.         m.rvec.z = fixmul(sinp,sbch) - cbsh;            //m7
  424. }
  425.  
  426. //computes a matrix from a set of three angles.  returns ptr to matrix
  427. void vm_angles_2_matrix(vms_matrix &m,const vms_angvec &a)
  428. {
  429.         const auto al = a;
  430.         sincos_2_matrix(m, al.b, fix_sincos(al.p), fix_sincos(al.h));
  431. }
  432.  
  433. #if DXX_USE_EDITOR
  434. //computes a matrix from a forward vector and an angle
  435. void vm_vec_ang_2_matrix(vms_matrix &m,const vms_vector &v,fixang a)
  436. {
  437.         fix sinp,cosp,sinh,cosh;
  438.         sinp = -v.y;
  439.         cosp = fix_sqrt(f1_0 - fixmul(sinp,sinp));
  440.  
  441.         sinh = fixdiv(v.x,cosp);
  442.         cosh = fixdiv(v.z,cosp);
  443.         sincos_2_matrix(m, a, {sinp, cosp}, {sinh, cosh});
  444. }
  445. #endif
  446.  
  447. //computes a matrix from one or more vectors. The forward vector is required,
  448. //with the other two being optional.  If both up & right vectors are passed,
  449. //the up vector is used.  If only the forward vector is passed, a bank of
  450. //zero is assumed
  451. //returns ptr to matrix
  452. void vm_vector_2_matrix(vms_matrix &m,const vms_vector &fvec,const vms_vector *uvec,const vms_vector *rvec)
  453. {
  454.         vms_vector &xvec=m.rvec,&yvec=m.uvec,&zvec=m.fvec;
  455.         if (!vm_vec_copy_normalize(zvec,fvec)) {
  456.                 Int3();         //forward vec should not be zero-length
  457.                 return;
  458.         }
  459.         if (uvec == NULL) {
  460.                 if (rvec == NULL) {             //just forward vec
  461. bad_vector2:
  462.         ;
  463.                         if (zvec.x==0 && zvec.z==0) {           //forward vec is straight up or down
  464.  
  465.                                 m.rvec.x = f1_0;
  466.                                 m.uvec.z = (zvec.y < 0)?f1_0:-f1_0;
  467.  
  468.                                 m.rvec.y = m.rvec.z = m.uvec.x = m.uvec.y = 0;
  469.                         }
  470.                         else {          //not straight up or down
  471.  
  472.                                 xvec.x = zvec.z;
  473.                                 xvec.y = 0;
  474.                                 xvec.z = -zvec.x;
  475.  
  476.                                 vm_vec_normalize(xvec);
  477.                                 vm_vec_cross(yvec,zvec,xvec);
  478.                         }
  479.                 }
  480.                 else {                                          //use right vec
  481.  
  482.                         if (!vm_vec_copy_normalize(xvec,*rvec))
  483.                                 goto bad_vector2;
  484.  
  485.                         vm_vec_cross(yvec,zvec,xvec);
  486.  
  487.                         //normalize new perpendicular vector
  488.                         if (!vm_vec_normalize(yvec))
  489.                                 goto bad_vector2;
  490.  
  491.                         //now recompute right vector, in case it wasn't entirely perpendiclar
  492.                         vm_vec_cross(xvec,yvec,zvec);
  493.  
  494.                 }
  495.         }
  496.         else {          //use up vec
  497.  
  498.                 if (!vm_vec_copy_normalize(yvec,*uvec))
  499.                         goto bad_vector2;
  500.  
  501.                 vm_vec_cross(xvec,yvec,zvec);
  502.                
  503.                 //normalize new perpendicular vector
  504.                 if (!vm_vec_normalize(xvec))
  505.                         goto bad_vector2;
  506.  
  507.                 //now recompute up vector, in case it wasn't entirely perpendiclar
  508.                 vm_vec_cross(yvec,zvec,xvec);
  509.         }
  510. }
  511.  
  512.  
  513. //rotates a vector through a matrix. returns ptr to dest vector
  514. //dest CANNOT equal source
  515. void vm_vec_rotate(vms_vector &dest,const vms_vector &src,const vms_matrix &m)
  516. {
  517.         dest.x = vm_vec_dot(src,m.rvec);
  518.         dest.y = vm_vec_dot(src,m.uvec);
  519.         dest.z = vm_vec_dot(src,m.fvec);
  520. }
  521.  
  522. //mulitply 2 matrices, fill in dest.  returns ptr to dest
  523. //dest CANNOT equal either source
  524. void _vm_matrix_x_matrix(vms_matrix &dest,const vms_matrix &src0,const vms_matrix &src1)
  525. {
  526.         dest.rvec.x = vm_vec_dot3(src0.rvec.x,src0.uvec.x,src0.fvec.x, src1.rvec);
  527.         dest.uvec.x = vm_vec_dot3(src0.rvec.x,src0.uvec.x,src0.fvec.x, src1.uvec);
  528.         dest.fvec.x = vm_vec_dot3(src0.rvec.x,src0.uvec.x,src0.fvec.x, src1.fvec);
  529.  
  530.         dest.rvec.y = vm_vec_dot3(src0.rvec.y,src0.uvec.y,src0.fvec.y, src1.rvec);
  531.         dest.uvec.y = vm_vec_dot3(src0.rvec.y,src0.uvec.y,src0.fvec.y, src1.uvec);
  532.         dest.fvec.y = vm_vec_dot3(src0.rvec.y,src0.uvec.y,src0.fvec.y, src1.fvec);
  533.  
  534.         dest.rvec.z = vm_vec_dot3(src0.rvec.z,src0.uvec.z,src0.fvec.z, src1.rvec);
  535.         dest.uvec.z = vm_vec_dot3(src0.rvec.z,src0.uvec.z,src0.fvec.z, src1.uvec);
  536.         dest.fvec.z = vm_vec_dot3(src0.rvec.z,src0.uvec.z,src0.fvec.z, src1.fvec);
  537. }
  538.  
  539. //extract angles from a matrix
  540. void vm_extract_angles_matrix(vms_angvec &a,const vms_matrix &m)
  541. {
  542.         fix cosp;
  543.  
  544.         if (m.fvec.x==0 && m.fvec.z==0)         //zero head
  545.                 a.h = 0;
  546.         else
  547.                 a.h = fix_atan2(m.fvec.z,m.fvec.x);
  548.  
  549.         const auto &&ah = fix_sincos(a.h);
  550.         const auto &sinh = ah.sin;
  551.         const auto &cosh = ah.cos;
  552.  
  553.         if (abs(sinh) > abs(cosh))                              //sine is larger, so use it
  554.                 cosp = fixdiv(m.fvec.x,sinh);
  555.         else                                                                                    //cosine is larger, so use it
  556.                 cosp = fixdiv(m.fvec.z,cosh);
  557.  
  558.         if (cosp==0 && m.fvec.y==0)
  559.                 a.p = 0;
  560.         else
  561.                 a.p = fix_atan2(cosp,-m.fvec.y);
  562.  
  563.  
  564.         if (cosp == 0)  //the cosine of pitch is zero.  we're pitched straight up. say no bank
  565.  
  566.                 a.b = 0;
  567.  
  568.         else {
  569.                 fix sinb,cosb;
  570.  
  571.                 sinb = fixdiv(m.rvec.y,cosp);
  572.                 cosb = fixdiv(m.uvec.y,cosp);
  573.  
  574.                 if (sinb==0 && cosb==0)
  575.                         a.b = 0;
  576.                 else
  577.                         a.b = fix_atan2(cosb,sinb);
  578.  
  579.         }
  580. }
  581.  
  582.  
  583. //extract heading and pitch from a vector, assuming bank==0
  584. static vms_angvec &vm_extract_angles_vector_normalized(vms_angvec &a,const vms_vector &v)
  585. {
  586.         a.b = 0;                //always zero bank
  587.         a.p = fix_asin(-v.y);
  588.         if (v.x == 0 && v.z == 0)
  589.                 a.h = 0;
  590.         else
  591.                 a.h = fix_atan2(v.z,v.x);
  592.         return a;
  593. }
  594.  
  595. //extract heading and pitch from a vector, assuming bank==0
  596. void vm_extract_angles_vector(vms_angvec &a,const vms_vector &v)
  597. {
  598.         vms_vector t;
  599.         if (vm_vec_copy_normalize(t,v))
  600.                 vm_extract_angles_vector_normalized(a,t);
  601.         else
  602.                 a = {};
  603. }
  604.  
  605. //compute the distance from a point to a plane.  takes the normalized normal
  606. //of the plane (ebx), a point on the plane (edi), and the point to check (esi).
  607. //returns distance in eax
  608. //distance is signed, so negative dist is on the back of the plane
  609. fix vm_dist_to_plane(const vms_vector &checkp,const vms_vector &norm,const vms_vector &planep)
  610. {
  611.         return vm_vec_dot(vm_vec_sub(checkp,planep),norm);
  612. }
  613.  
  614. // convert vms_matrix to vms_quaternion
  615. void vms_quaternion_from_matrix(vms_quaternion &rq, const vms_matrix &m)
  616. {
  617.         const auto rx = m.rvec.x;
  618.         const auto ry = m.rvec.y;
  619.         const auto rz = m.rvec.z;
  620.         const auto ux = m.uvec.x;
  621.         const auto uy = m.uvec.y;
  622.         const auto uz = m.uvec.z;
  623.         const auto fx = m.fvec.x;
  624.         const auto fy = m.fvec.y;
  625.         const auto fz = m.fvec.z;
  626.         const fix tr = rx + uy + fz;
  627.         fix qw, qx, qy, qz;
  628.         if (tr > 0) {
  629.                 fix s = fixmul(fix_sqrt(tr + fl2f(1.0)), fl2f(2.0));
  630.                 qw = fixmul(fl2f(0.25), s);
  631.                 qx = fixdiv(fy - uz, s);
  632.                 qy = fixdiv(rz - fx, s);
  633.                 qz = fixdiv(ux - ry, s);
  634.         } else if ((rx > uy) & (rx > fz)) {
  635.                 fix s = fixmul(fix_sqrt(fl2f(1.0) + rx - uy - fz), fl2f(2.0));
  636.                 qw = fixdiv(fy - uz, s);
  637.                 qx = fixmul(fl2f(0.25), s);
  638.                 qy = fixdiv(ry + ux, s);
  639.                 qz = fixdiv(rz + fx, s);
  640.         } else if (uy > fz) {
  641.                 fix s = fixmul(fix_sqrt(fl2f(1.0) + uy - rx - fz), fl2f(2.0));
  642.                 qw = fixdiv(rz - fx, s);
  643.                 qx = fixdiv(ry + ux, s);
  644.                 qy = fixmul(fl2f(0.25), s);
  645.                 qz = fixdiv(uz + fy, s);
  646.         } else {
  647.                 fix s = fixmul(fix_sqrt(fl2f(1.0) + fz - rx - uy), fl2f(2.0));
  648.                 qw = fixdiv(ux - ry, s);
  649.                 qx = fixdiv(rz + fx, s);
  650.                 qy = fixdiv(uz + fy, s);
  651.                 qz = fixmul(fl2f(0.25), s);
  652.         }
  653.         rq.w = qw / 2;
  654.         rq.x = qx / 2;
  655.         rq.y = qy / 2;
  656.         rq.z = qz / 2;
  657. }
  658.  
  659. // convert vms_quaternion to vms_matrix
  660. void vms_matrix_from_quaternion(vms_matrix &m, const vms_quaternion &q)
  661. {
  662.         const fix qw2 = q.w * 2;
  663.         const fix qx2 = q.x * 2;
  664.         const fix qy2 = q.y * 2;
  665.         const fix qz2 = q.z * 2;
  666.         const fix sqw = fixmul(qw2, qw2);
  667.         const fix sqx = fixmul(qx2, qx2);
  668.         const fix sqy = fixmul(qy2, qy2);
  669.         const fix sqz = fixmul(qz2, qz2);
  670.         const fix invs = fixdiv(fl2f(1.0), (sqw + sqx + sqy + sqz));
  671.  
  672.         m.rvec.x = fixmul(sqx - sqy - sqz + sqw, invs);
  673.         m.uvec.y = fixmul(-sqx + sqy - sqz + sqw, invs);
  674.         m.fvec.z = fixmul(-sqx - sqy + sqz + sqw, invs);
  675.        
  676.         const fix qxy = fixmul(qx2, qy2);
  677.         const fix qzw = fixmul(qz2, qw2);
  678.         m.uvec.x = fixmul(fixmul(fl2f(2.0), (qxy + qzw)), invs);
  679.         m.rvec.y = fixmul(fixmul(fl2f(2.0), (qxy - qzw)), invs);
  680.        
  681.         const fix qxz = fixmul(qx2, qz2);
  682.         const fix qyw = fixmul(qy2, qw2);
  683.         m.fvec.x = fixmul(fixmul(fl2f(2.0), (qxz - qyw)), invs);
  684.         m.rvec.z = fixmul(fixmul(fl2f(2.0), (qxz + qyw)), invs);
  685.        
  686.         const fix qyz = fixmul(qy2, qz2);
  687.         const fix qxw = fixmul(qx2, qw2);
  688.         m.fvec.y = fixmul(fixmul(fl2f(2.0), (qyz + qxw)), invs);
  689.         m.uvec.z = fixmul(fixmul(fl2f(2.0), (qyz - qxw)), invs);
  690. }
  691.  
  692. }
  693.