home *** CD-ROM | disk | FTP | other *** search
/ Dream 52 / Amiga_Dream_52.iso / Amiga / Jeux / demos / crystalPPC.lha / math3d.cpp < prev    next >
C/C++ Source or Header  |  1998-02-05  |  14KB  |  584 lines
  1. #include <math.h>
  2. #include <time.h>
  3. #include "system.h"
  4.  
  5. #ifndef DEF_H
  6. #include "def.h"
  7. #endif
  8.  
  9. #ifndef MATH3D_H
  10. #include "math3d.h"
  11. #endif
  12.  
  13. #ifndef TOKEN_H
  14. #include "token.h"
  15. #endif
  16.  
  17. //---------------------------------------------------------------------------
  18. /*
  19. Tables tables;
  20.  
  21. Tables::Tables ()
  22. {
  23.   printf ("Creating mathematical tables ... "); fflush (stdout);
  24.   printf ("DONE\n");
  25. }
  26. */
  27.  
  28. //---------------------------------------------------------------------------
  29.  
  30. int Box::in (float x, float y)
  31. {
  32.   if (x < minx || x > maxx) return FALSE;
  33.   if (y < miny || y > maxy) return FALSE;
  34.   return TRUE;
  35. }
  36.  
  37. int Box::overlap (Box* box)
  38. {
  39.   if (maxx < box->minx || minx > box->maxx) return FALSE;
  40.   if (maxy < box->miny || miny > box->maxy) return FALSE;
  41.   return TRUE;
  42. }
  43.  
  44. void Box::start_bounding_box ()
  45. {
  46.   minx = 10000.;
  47.   miny = 10000.;
  48.   maxx = -10000.;
  49.   maxy = -10000.;
  50. }
  51.  
  52. void Box::add_bounding_vertex (float x, float y)
  53. {
  54.   if (x < minx) minx = x;
  55.   if (x > maxx) maxx = x;
  56.   if (y < miny) miny = y;
  57.   if (y > maxy) maxy = y;
  58. }
  59.  
  60. void Box::dump ()
  61. {
  62.   MSG (("(%2.2f,%2.2f)-(%2.2f,%2.2f)", minx, miny, maxx, maxy));
  63. }
  64.  
  65. //---------------------------------------------------------------------------
  66.  
  67. Matrix3::Matrix3 ()
  68. {
  69. }
  70.  
  71. Matrix3::Matrix3 (float m11, float m12, float m13,
  72.                 float m21, float m22, float m23,
  73.                float m31, float m32, float m33)
  74. {
  75.   Matrix3::m11 = m11;
  76.   Matrix3::m12 = m12;
  77.   Matrix3::m13 = m13;
  78.   Matrix3::m21 = m21;
  79.   Matrix3::m22 = m22;
  80.   Matrix3::m23 = m23;
  81.   Matrix3::m31 = m31;
  82.   Matrix3::m32 = m32;
  83.   Matrix3::m33 = m33;
  84. }
  85.  
  86. Matrix3::~Matrix3 ()
  87. {
  88. }
  89.  
  90. Matrix3& Matrix3::operator+= (Matrix3& m)
  91. {
  92.   m11 += m.m11; m12 += m.m12; m13 += m.m13;
  93.   m21 += m.m21; m22 += m.m22; m23 += m.m23;
  94.   m31 += m.m31; m32 += m.m32; m33 += m.m33;
  95.   return *this;
  96. }
  97.  
  98. Matrix3& Matrix3::operator-= (Matrix3& m)
  99. {
  100.   m11 -= m.m11; m12 -= m.m12; m13 -= m.m13;
  101.   m21 -= m.m21; m22 -= m.m22; m23 -= m.m23;
  102.   m31 -= m.m31; m32 -= m.m32; m33 -= m.m33;
  103.   return *this;
  104. }
  105.  
  106. Matrix3& Matrix3::operator*= (Matrix3& m)
  107. {
  108.   Matrix3 r;
  109.   r.m11 = m11*m.m11 + m12*m.m21 + m13*m.m31;
  110.   r.m12 = m11*m.m12 + m12*m.m22 + m13*m.m32;
  111.   r.m13 = m11*m.m13 + m12*m.m23 + m13*m.m33;
  112.   r.m21 = m21*m.m11 + m22*m.m21 + m23*m.m31;
  113.   r.m22 = m21*m.m12 + m22*m.m22 + m23*m.m32;
  114.   r.m23 = m21*m.m13 + m22*m.m23 + m23*m.m33;
  115.   r.m31 = m31*m.m11 + m32*m.m21 + m33*m.m31;
  116.   r.m32 = m31*m.m12 + m32*m.m22 + m33*m.m32;
  117.   r.m33 = m31*m.m13 + m32*m.m23 + m33*m.m33;
  118.   *this = r;
  119.   return *this;
  120. }
  121.  
  122. Matrix3& Matrix3::operator*= (float s)
  123. {
  124.   m11 *= s; m12 *= s; m13 *= s;
  125.   m21 *= s; m22 *= s; m23 *= s;
  126.   m31 *= s; m32 *= s; m33 *= s;
  127.   return *this;
  128. }
  129.  
  130. void Matrix3::identity ()
  131. {
  132.   m12 = m13 = 0;
  133.   m21 = m23 = 0;
  134.   m31 = m32 = 0;
  135.   m11 = m22 = m33 = 1;
  136. }
  137.  
  138. void Matrix3::transpose ()
  139. {
  140.   float swap;
  141.   swap = m12; m12 = m21; m21 = swap;
  142.   swap = m13; m13 = m31; m31 = swap;
  143.   swap = m23; m23 = m32; m32 = swap;
  144. }
  145.  
  146. void Matrix3::inverse ()
  147. {
  148.   float s = 1./determinant ();
  149.   Matrix3 C;
  150.   C.m11 =  (m22*m33 - m23*m32);
  151.   C.m12 = -(m21*m33 - m23*m31);
  152.   C.m13 =  (m21*m32 - m22*m31);
  153.   C.m21 = -(m12*m33 - m13*m32);
  154.   C.m22 =  (m11*m33 - m13*m31);
  155.   C.m23 = -(m11*m32 - m12*m31);
  156.   C.m31 =  (m12*m23 - m13*m22);
  157.   C.m32 = -(m11*m23 - m13*m21);
  158.   C.m33 =  (m11*m22 - m12*m21);
  159.   C.transpose ();
  160.   *this = C;
  161.   (*this) *= s;
  162. }
  163.  
  164. float Matrix3::determinant ()
  165. {
  166.   return
  167.     m11 * (m22*m33 - m23*m32)
  168.    -m12 * (m21*m33 - m23*m31)
  169.    +m13 * (m21*m32 - m22*m31);
  170. }
  171.  
  172. void Matrix3::transform (float x, float y, float z, Vector3& t)
  173. {
  174.   t.x = m11*x+m12*y+m13*z;
  175.   t.y = m21*x+m22*y+m23*z;
  176.   t.z = m31*x+m32*y+m33*z;
  177. }
  178.  
  179. void Matrix3::transform (Vector3& f, Vector3& t)
  180. {
  181.   t.x = m11*f.x+m12*f.y+m13*f.z;
  182.   t.y = m21*f.x+m22*f.y+m23*f.z;
  183.   t.z = m31*f.x+m32*f.y+m33*f.z;
  184. }
  185.  
  186. void Matrix3::dump (char* name)
  187. {
  188.   MSG (("Matrix '%s':\n", name));
  189.   MSG (("/\n"));
  190.   MSG (("| %3.2f %3.2f %3.2f\n", m11, m12, m13));
  191.   MSG (("| %3.2f %3.2f %3.2f\n", m21, m22, m23));
  192.   MSG (("| %3.2f %3.2f %3.2f\n", m31, m32, m33));
  193.   MSG (("\\\n"));
  194. }
  195.  
  196. void Matrix3::save (FILE* fp, int indent)
  197. {
  198.   char sp[100]; strcpy (sp, spaces); sp[indent] = 0;
  199.   fprintf (fp, "%sMATRIX (", sp);
  200.   fprintf (fp, "%f,%f,%f,", m11, m12, m13);
  201.   fprintf (fp, "%f,%f,%f,", m21, m22, m23);
  202.   fprintf (fp, "%f,%f,%f", m31, m32, m33);
  203.   fprintf (fp, ")\n");
  204. }
  205.  
  206. void Matrix3::load (char** buf)
  207. {
  208.   char* t;
  209.   skip_token (buf, "MATRIX");
  210.  
  211.   t = get_token (buf);
  212.   if (!strcmp (t, "IDENTITY"))
  213.   {
  214.     identity ();
  215.     return;
  216.   }
  217.   else if (*t != '(')
  218.   {
  219.     float rc;
  220.     sscanf (t, "%f", &rc);
  221.     identity ();
  222.     *this *= rc;
  223.     return;
  224.   }
  225.  
  226.   m11 = get_token_float (buf);
  227.   skip_token (buf, ",", "Expected '%s' instead of '%s' for MATRIX statement!\n");
  228.   m12 = get_token_float (buf);
  229.   skip_token (buf, ",", "Expected '%s' instead of '%s' for MATRIX statement!\n");
  230.   m13 = get_token_float (buf);
  231.   skip_token (buf, ",", "Expected '%s' instead of '%s' for MATRIX statement!\n");
  232.   m21 = get_token_float (buf);
  233.   skip_token (buf, ",", "Expected '%s' instead of '%s' for MATRIX statement!\n");
  234.   m22 = get_token_float (buf);
  235.   skip_token (buf, ",", "Expected '%s' instead of '%s' for MATRIX statement!\n");
  236.   m23 = get_token_float (buf);
  237.   skip_token (buf, ",", "Expected '%s' instead of '%s' for MATRIX statement!\n");
  238.   m31 = get_token_float (buf);
  239.   skip_token (buf, ",", "Expected '%s' instead of '%s' for MATRIX statement!\n");
  240.   m32 = get_token_float (buf);
  241.   skip_token (buf, ",", "Expected '%s' instead of '%s' for MATRIX statement!\n");
  242.   m33 = get_token_float (buf);
  243.   skip_token (buf, ")", "Expected '%s' instead of '%s' to conclude MATRIX statement!\n");
  244.  
  245. }
  246.  
  247. void Matrix3::init ()
  248. {
  249. }
  250.  
  251. //---------------------------------------------------------------------------
  252.  
  253. int Vector2::which_side_2d (Vector2& v1, Vector2& v2)
  254. {
  255.   float s = (v1.y - y)*(v2.x - v1.x) - (v1.x - x)*(v2.y - v1.y);
  256.   if (s < 0) return -1;
  257.   else if (s > 0) return 1;
  258.   else return 0;
  259. }
  260.  
  261. // This algorithm assumes that the polygon is convex and that
  262. // the vertices of the polygon are oriented in clockwise ordering.
  263. // If this was not the case then the polygon should not be drawn (culled)
  264. // and this routine would not be called for it.
  265. int Vector2::in_poly_2d (Vector2* P, int n, Box* bounding_box)
  266. {
  267.   if (!bounding_box->in (x, y)) return POLY_OUT;
  268.   int i, i1;
  269.   int side;
  270.   i1 = n-1;
  271.   for (i = 0 ; i < n ; i++)
  272.   {
  273.     // If this vertex is left of the polygon edge we are outside the polygon.
  274.     side = which_side_2d (P[i1], P[i]);
  275.     if (side < 0) return POLY_OUT;
  276.     else if (side == 0) return POLY_ON;
  277.     i1 = i;
  278.   }
  279.   return POLY_IN;
  280. }
  281.  
  282. void Vector3::between (Vector3& v1, Vector3& v2, Vector3& v, float pct, float wid)
  283. {
  284.   if (pct != -1)
  285.     pct /= 100.;
  286.   else
  287.   {
  288.     float dist = sqrt ((v1.x-v2.x)*(v1.x-v2.x)+(v1.y-v2.y)*(v1.y-v2.y)+(v1.z-v2.z)*(v1.z-v2.z));
  289.     pct = wid/dist;
  290.   }
  291.   v.x = pct*(v2.x-v1.x)+v1.x;
  292.   v.y = pct*(v2.y-v1.y)+v1.y;
  293.   v.z = pct*(v2.z-v1.z)+v1.z;
  294. }
  295.  
  296. void Vector3::dump (char* name)
  297. {
  298.   MSG (("Vector '%s': (%2.2f,%2.2f,%2.2f)\n", name, x, y, z));
  299. }
  300.  
  301. void Vector3::save (FILE* fp, int indent)
  302. {
  303.   char sp[100]; strcpy (sp, spaces); sp[indent] = 0;
  304.   fprintf (fp, "%s(%f,%f,%f)\n", sp, x, y, z);
  305. }
  306.  
  307. void Vector3::load (char** buf)
  308. {
  309.   skip_token (buf, "(", "Expected '%s' instead of '%s' to begin vector!\n");
  310.   x = get_token_float (buf);
  311.   skip_token (buf, ",", "Expected '%s' instead of '%s' for vector!\n");
  312.   y = get_token_float (buf);
  313.   skip_token (buf, ",", "Expected '%s' instead of '%s' for vector!\n");
  314.   z = get_token_float (buf);
  315.   skip_token (buf, ")", "Expected '%s' instead of '%s' to conclude a vector!\n");
  316. }
  317.  
  318. void Vector2::dump (char* name)
  319. {
  320.   MSG (("Vector '%s': (%2.2f,%2.2f)\n", name, x, y));
  321. }
  322.  
  323. Vector3& Vector3::operator+= (Vector3& v)
  324. {
  325.   x += v.x;
  326.   y += v.y;
  327.   z += v.z;
  328.   return *this;
  329. }
  330.  
  331. Vector3& Vector3::operator-= (Vector3& v)
  332. {
  333.   x -= v.x;
  334.   y -= v.y;
  335.   z -= v.z;
  336.   return *this;
  337. }
  338.  
  339. float Vector2::Area2 (float ax, float ay, float bx, float by, float cx, float cy)
  340. {
  341.   float rc =
  342.     ax * by - ay * bx +
  343.     ay * cx - ax * cy +
  344.     bx * cy - cx * by;
  345.   if (ABS (rc) < SMALL_EPSILON) rc = 0;
  346.   return rc;
  347. }
  348.  
  349. int Vector2::Right (float ax, float ay, float bx, float by, float cx, float cy)
  350. {
  351.   return Area2 (ax, ay, bx, by, cx, cy) < 0;
  352. }
  353.  
  354. int Vector2::Left (float ax, float ay, float bx, float by, float cx, float cy)
  355. {
  356.   return Area2 (ax, ay, bx, by, cx, cy) > 0;
  357. }
  358.  
  359. //---------------------------------------------------------------------------
  360.  
  361. /*
  362.  * General function to test if two lines intersect.
  363.  * This function should not be used in speed-sensitive
  364.  * computations but it can be used to pre-compute stuff.
  365.  * The first line has its origin at (0,0).
  366.  * This function returns FALSE if the lines don't intersect
  367.  * (given that the second line is only a line-segment really
  368.  * and the first line is a vector starting at (0,0)).
  369.  * Note, that even in the case that lines don't intersect
  370.  * the intersection point is still correctly computed unless
  371.  * the slopes of the two lines is too close together.
  372.  * In the last case the intersection point is set to (0,0).
  373.  */
  374. int intersect (
  375.   float x1, float y1,   /* End-point of first line (vector really) */
  376.   float X1, float Y1,   /* Coordinates of second line (segment really) */
  377.   float X2, float Y2,
  378.   Vector2* isect)       /* Intersection vertex */
  379. {
  380.   float u;              /* Slope of first line */
  381.   float v;              /* Slope of second line */
  382.   float cx, cy;         /* Intersection point */
  383.   int ii;               /* Set to TRUE if really intersect */
  384.  
  385.   ii = TRUE;
  386.   cx = 0.;
  387.   cy = 0.;
  388.  
  389.   if (ABS (x1) > ABS (y1))
  390.   {
  391.     /*
  392.      * First line is more horizontal than vertical:
  393.      *      y1
  394.      * y = ---- x
  395.      *      x1
  396.      */
  397.     u = y1/x1;
  398.  
  399.     if (ABS (X2-X1) > ABS (Y2-Y1))
  400.     {
  401.       /*
  402.        * Second line is more horizontal than vertical.
  403.        */
  404.       v = (Y2-Y1) / (X2-X1);
  405.       if (ABS (u-v) < SMALL_EPSILON)
  406.         ii = FALSE;
  407.       else
  408.       {
  409.         cx = (Y1-v*X1)/(u-v);
  410.         cy = u*cx;
  411.  
  412.         /*
  413.          * Check if the line really cuts.
  414.          */
  415.         if ((cx < X1 && cx < X2) || (cx > X1 && cx > X2)) ii = FALSE;
  416.       }
  417.     }
  418.     else
  419.     {
  420.       /*
  421.        * Second line is more vertical than horizontal.
  422.        */
  423.       v = (X2-X1) / (Y2-Y1);
  424.       if (ABS (1-u*v) < SMALL_EPSILON)
  425.         ii = FALSE;
  426.       else
  427.       {
  428.         cx = (X1-v*Y1)/(1-u*v);
  429.         cy = u*cx;
  430.  
  431.         /*
  432.          * Check if the line really cuts.
  433.          */
  434.         if ((cy < Y1 && cy < Y2) || (cy > Y1 && cy > Y2)) ii = FALSE;
  435.       }
  436.     }
  437.  
  438.     /*
  439.      * Check if cutting point is in right direction of first vector.
  440.      */
  441.     if ((x1 < 0 || cx < 0) && (x1 > 0 || cx > 0)) ii = FALSE;
  442.   }
  443.   else
  444.   {
  445.     /*
  446.      * First line is more vertical than horizontal:
  447.      *      x1
  448.      * x = ---- y
  449.      *      y1
  450.      */
  451.     u = x1/y1;
  452.  
  453.     if (ABS (X2-X1) > ABS (Y2-Y1))
  454.     {
  455.       /*
  456.        * Second line is more horizontal than vertical.
  457.        */
  458.       v = (Y2-Y1) / (X2-X1);
  459.       if (ABS (1-u*v) < SMALL_EPSILON)
  460.         ii = FALSE;
  461.       else
  462.       {
  463.         cy = (Y1-v*X1)/(1-u*v);
  464.         cx = u*cy;
  465.  
  466.         /*
  467.          * Check if the line really cuts.
  468.          */
  469.         if ((cx < X1 && cx < X2) || (cx > X1 && cx > X2)) ii = FALSE;
  470.       }
  471.     }
  472.     else
  473.     {
  474.       /*
  475.        * Second line is more vertical than horizontal.
  476.        */
  477.       v = (X2-X1) / (Y2-Y1);
  478.       if (ABS (u-v) < SMALL_EPSILON)
  479.         ii = FALSE;
  480.       else
  481.       {
  482.         cy = (X1-v*Y1)/(u-v);
  483.         cx = u*cy;
  484.  
  485.         /*
  486.          * Check if the line really cuts.
  487.          */
  488.         if ((cy < Y1 && cy < Y2) || (cy > Y1 && cy > Y2)) ii = FALSE;
  489.       }
  490.     }
  491.  
  492.     /*
  493.      * Check if cutting point is in right direction of first vector.
  494.      */
  495.     if ((y1 < 0 || cy < 0) && (y1 > 0 || cy > 0)) ii = FALSE;
  496.   }
  497.  
  498.   if (isect)
  499.   {
  500.     isect->x = cx;
  501.     isect->y = cy;
  502.   }
  503.   return ii;
  504. }
  505.  
  506. /*
  507.  *           (YA-YC)(XD-XC)-(XA-XC)(YD-YC)
  508.  *       r = -----------------------------  (eqn 1)
  509.  *           (XB-XA)(YD-YC)-(YB-YA)(XD-XC)
  510.  *
  511.  *           (YA-YC)(XB-XA)-(XA-XC)(YB-YA)
  512.  *       s = -----------------------------  (eqn 2)
  513.  *           (XB-XA)(YD-YC)-(YB-YA)(XD-XC)
  514.  */
  515. int Segment::intersect_segments (
  516.   Vector2& a, Vector2& b, /* First segment */
  517.   Vector2& c, Vector2& d, /* Second segment */
  518.   Vector2& isect,         /* Intersection vertex */
  519.   float* rp)          /* Return of the 'r' value */
  520. {
  521.   float denom;
  522.   float r, s;
  523.  
  524.   denom = (b.x-a.x)*(d.y-c.y) - (b.y-a.y)*(d.x-c.x);
  525.   if (ABS (denom) < EPSILON) return FALSE;
  526.  
  527.   r = ((a.y-c.y)*(d.x-c.x) - (a.x-c.x)*(d.y-c.y)) / denom;
  528.   s = ((a.y-c.y)*(b.x-a.x) - (a.x-c.x)*(b.y-a.y)) / denom;
  529.  
  530.   //if (r < 0 || r > 1 || s < 0 || s > 1) return FALSE;
  531.   if (r < -SMALL_EPSILON || r > 1+SMALL_EPSILON || s < -SMALL_EPSILON || s > 1+SMALL_EPSILON) return FALSE;
  532.  
  533.   isect.x = a.x + r*(b.x-a.x);
  534.   isect.y = a.y + r*(b.y-a.y);
  535.   *rp = r;
  536.  
  537.   return TRUE;
  538. }
  539.  
  540. int Segment::intersect_segment_line (
  541.   Vector2& a, Vector2& b, /* First segment */
  542.   Vector2& c, Vector2& d, /* Second line */
  543.   Vector2& isect,         /* Intersection vertex */
  544.   float* rp)          /* Return of the 'r' value */
  545. {
  546.   float denom;
  547.   float r;
  548.  
  549.   denom = (b.x-a.x)*(d.y-c.y) - (b.y-a.y)*(d.x-c.x);
  550.   if (ABS (denom) < SMALL_EPSILON) return FALSE;
  551.  
  552.   r = ((a.y-c.y)*(d.x-c.x) - (a.x-c.x)*(d.y-c.y)) / denom;
  553.  
  554.   //if (r < 0 || r > 1) return FALSE;
  555.   if (r < -SMALL_EPSILON || r > 1+SMALL_EPSILON) return FALSE;
  556.  
  557.   isect.x = a.x + r*(b.x-a.x);
  558.   isect.y = a.y + r*(b.y-a.y);
  559.   *rp = r;
  560.  
  561.   return TRUE;
  562. }
  563.  
  564. /*
  565.  * x = r * (x2-x1) + x1;
  566.  * y = r * (y2-y1) + y1;
  567.  * z = r * (z2-z1) + z1;
  568.  */
  569. float Segment::intersect_z_plane_3d (
  570.   float z,            // Z plane coordinate
  571.   Vector3& a, Vector3& b,    // Segment
  572.   Vector3& i)            // Intersection vertex
  573. {
  574.   //     z = r * (z2-z1) + z1;
  575.   // --> r = (z-z1) / (z2-z1)
  576.   float r = (z-a.z) / (b.z-a.z);
  577.   i.x = r * (b.x-a.x) + a.x;
  578.   i.y = r * (b.y-a.y) + a.y;
  579.   i.z = z;
  580.   return r;
  581. }
  582.  
  583. //---------------------------------------------------------------------------
  584.