home *** CD-ROM | disk | FTP | other *** search
/ PC PowerPlay 58 / pcpp58a.iso / extras / quake 3 source / Q3A_ToolSource.exe / Main / mathlib.c < prev    next >
Encoding:
C/C++ Source or Header  |  2001-01-02  |  8.5 KB  |  414 lines
  1. // mathlib.c -- math primitives
  2.  
  3. #include "cmdlib.h"
  4. #include "mathlib.h"
  5.  
  6. #ifdef _WIN32
  7. //Improve floating-point consistency.
  8. //without this option weird floating point issues occur
  9. #pragma optimize( "p", on )
  10. #endif
  11.  
  12.  
  13. vec3_t vec3_origin = {0,0,0};
  14.  
  15. /*
  16. ** NormalToLatLong
  17. **
  18. ** We use two byte encoded normals in some space critical applications.
  19. ** Lat = 0 at (1,0,0) to 360 (-1,0,0), encoded in 8-bit sine table format
  20. ** Lng = 0 at (0,0,1) to 180 (0,0,-1), encoded in 8-bit sine table format
  21. **
  22. */
  23. void NormalToLatLong( const vec3_t normal, byte bytes[2] ) {
  24.     // check for singularities
  25.     if ( normal[0] == 0 && normal[1] == 0 ) {
  26.         if ( normal[2] > 0 ) {
  27.             bytes[0] = 0;
  28.             bytes[1] = 0;        // lat = 0, long = 0
  29.         } else {
  30.             bytes[0] = 128;
  31.             bytes[1] = 0;        // lat = 0, long = 128
  32.         }
  33.     } else {
  34.         int    a, b;
  35.  
  36.         a = RAD2DEG( atan2( normal[1], normal[0] ) ) * (255.0f / 360.0f );
  37.         a &= 0xff;
  38.  
  39.         b = RAD2DEG( acos( normal[2] ) ) * ( 255.0f / 360.0f );
  40.         b &= 0xff;
  41.  
  42.         bytes[0] = b;    // longitude
  43.         bytes[1] = a;    // lattitude
  44.     }
  45. }
  46.  
  47. /*
  48. =====================
  49. PlaneFromPoints
  50.  
  51. Returns false if the triangle is degenrate.
  52. The normal will point out of the clock for clockwise ordered points
  53. =====================
  54. */
  55. qboolean PlaneFromPoints( vec4_t plane, const vec3_t a, const vec3_t b, const vec3_t c ) {
  56.     vec3_t    d1, d2;
  57.  
  58.     VectorSubtract( b, a, d1 );
  59.     VectorSubtract( c, a, d2 );
  60.     CrossProduct( d2, d1, plane );
  61.     if ( VectorNormalize( plane, plane ) == 0 ) {
  62.         return qfalse;
  63.     }
  64.  
  65.     plane[3] = DotProduct( a, plane );
  66.     return qtrue;
  67. }
  68.  
  69. /*
  70. ================
  71. MakeNormalVectors
  72.  
  73. Given a normalized forward vector, create two
  74. other perpendicular vectors
  75. ================
  76. */
  77. void MakeNormalVectors (vec3_t forward, vec3_t right, vec3_t up)
  78. {
  79.     float        d;
  80.  
  81.     // this rotate and negate guarantees a vector
  82.     // not colinear with the original
  83.     right[1] = -forward[0];
  84.     right[2] = forward[1];
  85.     right[0] = forward[2];
  86.  
  87.     d = DotProduct (right, forward);
  88.     VectorMA (right, -d, forward, right);
  89.     VectorNormalize (right, right);
  90.     CrossProduct (right, forward, up);
  91. }
  92.  
  93.  
  94. void Vec10Copy( vec_t *in, vec_t *out ) {
  95.     out[0] = in[0];
  96.     out[1] = in[1];
  97.     out[2] = in[2];
  98.     out[3] = in[3];
  99.     out[4] = in[4];
  100.     out[5] = in[5];
  101.     out[6] = in[6];
  102.     out[7] = in[7];
  103.     out[8] = in[8];
  104.     out[9] = in[9];
  105. }
  106.  
  107.  
  108. void VectorRotate3x3( vec3_t v, float r[3][3], vec3_t d )
  109. {
  110.     d[0] = v[0] * r[0][0] + v[1] * r[1][0] + v[2] * r[2][0];
  111.     d[1] = v[0] * r[0][1] + v[1] * r[1][1] + v[2] * r[2][1];
  112.     d[2] = v[0] * r[0][2] + v[1] * r[1][2] + v[2] * r[2][2];
  113. }
  114.  
  115. double VectorLength( const vec3_t v ) {
  116.     int        i;
  117.     double    length;
  118.     
  119.     length = 0;
  120.     for (i=0 ; i< 3 ; i++)
  121.         length += v[i]*v[i];
  122.     length = sqrt (length);        // FIXME
  123.  
  124.     return length;
  125. }
  126.  
  127. qboolean VectorCompare( const vec3_t v1, const vec3_t v2 ) {
  128.     int        i;
  129.     
  130.     for (i=0 ; i<3 ; i++)
  131.         if (fabs(v1[i]-v2[i]) > EQUAL_EPSILON)
  132.             return qfalse;
  133.             
  134.     return qtrue;
  135. }
  136.  
  137. vec_t Q_rint (vec_t in)
  138. {
  139.     return floor (in + 0.5);
  140. }
  141.  
  142. void VectorMA( const vec3_t va, double scale, const vec3_t vb, vec3_t vc ) {
  143.     vc[0] = va[0] + scale*vb[0];
  144.     vc[1] = va[1] + scale*vb[1];
  145.     vc[2] = va[2] + scale*vb[2];
  146. }
  147.  
  148. void CrossProduct( const vec3_t v1, const vec3_t v2, vec3_t cross ) {
  149.     cross[0] = v1[1]*v2[2] - v1[2]*v2[1];
  150.     cross[1] = v1[2]*v2[0] - v1[0]*v2[2];
  151.     cross[2] = v1[0]*v2[1] - v1[1]*v2[0];
  152. }
  153.  
  154. vec_t _DotProduct (vec3_t v1, vec3_t v2)
  155. {
  156.     return v1[0]*v2[0] + v1[1]*v2[1] + v1[2]*v2[2];
  157. }
  158.  
  159. void _VectorSubtract (vec3_t va, vec3_t vb, vec3_t out)
  160. {
  161.     out[0] = va[0]-vb[0];
  162.     out[1] = va[1]-vb[1];
  163.     out[2] = va[2]-vb[2];
  164. }
  165.  
  166. void _VectorAdd (vec3_t va, vec3_t vb, vec3_t out)
  167. {
  168.     out[0] = va[0]+vb[0];
  169.     out[1] = va[1]+vb[1];
  170.     out[2] = va[2]+vb[2];
  171. }
  172.  
  173. void _VectorCopy (vec3_t in, vec3_t out)
  174. {
  175.     out[0] = in[0];
  176.     out[1] = in[1];
  177.     out[2] = in[2];
  178. }
  179.  
  180. void _VectorScale (vec3_t v, vec_t scale, vec3_t out)
  181. {
  182.     out[0] = v[0] * scale;
  183.     out[1] = v[1] * scale;
  184.     out[2] = v[2] * scale;
  185. }
  186.  
  187. vec_t VectorNormalize( const vec3_t in, vec3_t out ) {
  188.     vec_t    length, ilength;
  189.  
  190.     length = sqrt (in[0]*in[0] + in[1]*in[1] + in[2]*in[2]);
  191.     if (length == 0)
  192.     {
  193.         VectorClear (out);
  194.         return 0;
  195.     }
  196.  
  197.     ilength = 1.0/length;
  198.     out[0] = in[0]*ilength;
  199.     out[1] = in[1]*ilength;
  200.     out[2] = in[2]*ilength;
  201.  
  202.     return length;
  203. }
  204.  
  205. vec_t ColorNormalize( const vec3_t in, vec3_t out ) {
  206.     float    max, scale;
  207.  
  208.     max = in[0];
  209.     if (in[1] > max)
  210.         max = in[1];
  211.     if (in[2] > max)
  212.         max = in[2];
  213.  
  214.     if (max == 0) {
  215.         out[0] = out[1] = out[2] = 1.0;
  216.         return 0;
  217.     }
  218.  
  219.     scale = 1.0 / max;
  220.  
  221.     VectorScale (in, scale, out);
  222.  
  223.     return max;
  224. }
  225.  
  226.  
  227.  
  228. void VectorInverse (vec3_t v)
  229. {
  230.     v[0] = -v[0];
  231.     v[1] = -v[1];
  232.     v[2] = -v[2];
  233. }
  234.  
  235. void ClearBounds (vec3_t mins, vec3_t maxs)
  236. {
  237.     mins[0] = mins[1] = mins[2] = 99999;
  238.     maxs[0] = maxs[1] = maxs[2] = -99999;
  239. }
  240.  
  241. void AddPointToBounds( const vec3_t v, vec3_t mins, vec3_t maxs ) {
  242.     int        i;
  243.     vec_t    val;
  244.  
  245.     for (i=0 ; i<3 ; i++)
  246.     {
  247.         val = v[i];
  248.         if (val < mins[i])
  249.             mins[i] = val;
  250.         if (val > maxs[i])
  251.             maxs[i] = val;
  252.     }
  253. }
  254.  
  255.  
  256. /*
  257. =================
  258. PlaneTypeForNormal
  259. =================
  260. */
  261. int    PlaneTypeForNormal (vec3_t normal) {
  262.     if (normal[0] == 1.0 || normal[0] == -1.0)
  263.         return PLANE_X;
  264.     if (normal[1] == 1.0 || normal[1] == -1.0)
  265.         return PLANE_Y;
  266.     if (normal[2] == 1.0 || normal[2] == -1.0)
  267.         return PLANE_Z;
  268.     
  269.     return PLANE_NON_AXIAL;
  270. }
  271.  
  272. /*
  273. ================
  274. MatrixMultiply
  275. ================
  276. */
  277. void MatrixMultiply(float in1[3][3], float in2[3][3], float out[3][3]) {
  278.     out[0][0] = in1[0][0] * in2[0][0] + in1[0][1] * in2[1][0] +
  279.                 in1[0][2] * in2[2][0];
  280.     out[0][1] = in1[0][0] * in2[0][1] + in1[0][1] * in2[1][1] +
  281.                 in1[0][2] * in2[2][1];
  282.     out[0][2] = in1[0][0] * in2[0][2] + in1[0][1] * in2[1][2] +
  283.                 in1[0][2] * in2[2][2];
  284.     out[1][0] = in1[1][0] * in2[0][0] + in1[1][1] * in2[1][0] +
  285.                 in1[1][2] * in2[2][0];
  286.     out[1][1] = in1[1][0] * in2[0][1] + in1[1][1] * in2[1][1] +
  287.                 in1[1][2] * in2[2][1];
  288.     out[1][2] = in1[1][0] * in2[0][2] + in1[1][1] * in2[1][2] +
  289.                 in1[1][2] * in2[2][2];
  290.     out[2][0] = in1[2][0] * in2[0][0] + in1[2][1] * in2[1][0] +
  291.                 in1[2][2] * in2[2][0];
  292.     out[2][1] = in1[2][0] * in2[0][1] + in1[2][1] * in2[1][1] +
  293.                 in1[2][2] * in2[2][1];
  294.     out[2][2] = in1[2][0] * in2[0][2] + in1[2][1] * in2[1][2] +
  295.                 in1[2][2] * in2[2][2];
  296. }
  297.  
  298. void ProjectPointOnPlane( vec3_t dst, const vec3_t p, const vec3_t normal )
  299. {
  300.     float d;
  301.     vec3_t n;
  302.     float inv_denom;
  303.  
  304.     inv_denom = 1.0F / DotProduct( normal, normal );
  305.  
  306.     d = DotProduct( normal, p ) * inv_denom;
  307.  
  308.     n[0] = normal[0] * inv_denom;
  309.     n[1] = normal[1] * inv_denom;
  310.     n[2] = normal[2] * inv_denom;
  311.  
  312.     dst[0] = p[0] - d * n[0];
  313.     dst[1] = p[1] - d * n[1];
  314.     dst[2] = p[2] - d * n[2];
  315. }
  316.  
  317. /*
  318. ** assumes "src" is normalized
  319. */
  320. void PerpendicularVector( vec3_t dst, const vec3_t src )
  321. {
  322.     int    pos;
  323.     int i;
  324.     float minelem = 1.0F;
  325.     vec3_t tempvec;
  326.  
  327.     /*
  328.     ** find the smallest magnitude axially aligned vector
  329.     */
  330.     for ( pos = 0, i = 0; i < 3; i++ )
  331.     {
  332.         if ( fabs( src[i] ) < minelem )
  333.         {
  334.             pos = i;
  335.             minelem = fabs( src[i] );
  336.         }
  337.     }
  338.     tempvec[0] = tempvec[1] = tempvec[2] = 0.0F;
  339.     tempvec[pos] = 1.0F;
  340.  
  341.     /*
  342.     ** project the point onto the plane defined by src
  343.     */
  344.     ProjectPointOnPlane( dst, tempvec, src );
  345.  
  346.     /*
  347.     ** normalize the result
  348.     */
  349.     VectorNormalize( dst, dst );
  350. }
  351.  
  352. /*
  353. ===============
  354. RotatePointAroundVector
  355.  
  356. This is not implemented very well...
  357. ===============
  358. */
  359. void RotatePointAroundVector( vec3_t dst, const vec3_t dir, const vec3_t point,
  360.                              float degrees ) {
  361.     float    m[3][3];
  362.     float    im[3][3];
  363.     float    zrot[3][3];
  364.     float    tmpmat[3][3];
  365.     float    rot[3][3];
  366.     int    i;
  367.     vec3_t vr, vup, vf;
  368.     float    rad;
  369.  
  370.     vf[0] = dir[0];
  371.     vf[1] = dir[1];
  372.     vf[2] = dir[2];
  373.  
  374.     PerpendicularVector( vr, dir );
  375.     CrossProduct( vr, vf, vup );
  376.  
  377.     m[0][0] = vr[0];
  378.     m[1][0] = vr[1];
  379.     m[2][0] = vr[2];
  380.  
  381.     m[0][1] = vup[0];
  382.     m[1][1] = vup[1];
  383.     m[2][1] = vup[2];
  384.  
  385.     m[0][2] = vf[0];
  386.     m[1][2] = vf[1];
  387.     m[2][2] = vf[2];
  388.  
  389.     memcpy( im, m, sizeof( im ) );
  390.  
  391.     im[0][1] = m[1][0];
  392.     im[0][2] = m[2][0];
  393.     im[1][0] = m[0][1];
  394.     im[1][2] = m[2][1];
  395.     im[2][0] = m[0][2];
  396.     im[2][1] = m[1][2];
  397.  
  398.     memset( zrot, 0, sizeof( zrot ) );
  399.     zrot[0][0] = zrot[1][1] = zrot[2][2] = 1.0F;
  400.  
  401.     rad = DEG2RAD( degrees );
  402.     zrot[0][0] = cos( rad );
  403.     zrot[0][1] = sin( rad );
  404.     zrot[1][0] = -sin( rad );
  405.     zrot[1][1] = cos( rad );
  406.  
  407.     MatrixMultiply( m, zrot, tmpmat );
  408.     MatrixMultiply( tmpmat, im, rot );
  409.  
  410.     for ( i = 0; i < 3; i++ ) {
  411.         dst[i] = rot[i][0] * point[0] + rot[i][1] * point[1] + rot[i][2] * point[2];
  412.     }
  413. }
  414.