home *** CD-ROM | disk | FTP | other *** search
/ DOS/V Power Report 1997 March / VPR9703A.ISO / VPR_DATA / DOGA / SOURCES / PASM.LZH / MATRIX.CPP < prev    next >
C/C++ Source or Header  |  1996-05-27  |  6KB  |  278 lines
  1. #include <math.h>
  2. #include "matrix.h"
  3.  
  4. #include "log.h"
  5.  
  6. Matrix::Matrix(int n)
  7. {
  8.     v[0] = v[1] = v[2] = v[3] = Vector(0.0,0.0,0.0);
  9.     if (n != 0) {
  10.         v[0].x = v[1].y = v[2].z = 1.0;
  11.     }
  12. }
  13.  
  14. Matrix::Matrix(const Matrix& m)
  15. {
  16.     v[0]=m.v[0];
  17.     v[1]=m.v[1];
  18.     v[2]=m.v[2];
  19.     v[3]=m.v[3];
  20. }
  21.  
  22. Matrix::Matrix(const Vector& v0, const Vector& v1, const Vector& v2, const Vector& v3)
  23. {
  24.     v[0] = v0;
  25.     v[1] = v1;
  26.     v[2] = v2;
  27.     v[3] = v3;
  28. }
  29.  
  30. Matrix::Matrix(const Vector& vx, const Vector& vz)
  31. {
  32.     *this = vx ^ vz;
  33. }
  34.  
  35. Matrix& Matrix::operator +=(const Matrix& m)
  36. {
  37.     v[0] += m.v[0];
  38.     v[1] += m.v[1];
  39.     v[2] += m.v[2];
  40.     v[3] += m.v[3];
  41.     return *this;
  42. }
  43.  
  44. Matrix& Matrix::operator -=(const Matrix& m)
  45. {
  46.     v[0] -= m.v[0];
  47.     v[1] -= m.v[1];
  48.     v[2] -= m.v[2];
  49.     v[3] -= m.v[3];
  50.     return *this;
  51. }
  52.  
  53. const int Matrix::operator ==(const Matrix& m2) const
  54. {
  55.     return v[0] == m2.v[0]
  56.         && v[1] == m2.v[1]
  57.         && v[2] == m2.v[2]
  58.         && v[3] == m2.v[3];
  59. }
  60. const int Matrix::operator !=(const Matrix& m2) const
  61. {
  62.     return !(*this == m2);
  63. }
  64.  
  65. const Matrix Matrix::operator +(const Matrix& m2) const
  66. {
  67.     Matrix temp(*this);
  68.     temp += m2;
  69.     return temp;
  70. }
  71.  
  72. const Matrix Matrix::operator -(const Matrix& m2) const
  73. {
  74.     Matrix temp(*this);
  75.     temp -= m2;
  76.     return temp;
  77. }
  78.  
  79. const Vector Matrix::operator *(const Vector& v1) const
  80. {
  81.     Vector temp;
  82.     temp[0] = v[0].x * v1.x + v[1].x * v1.y + v[2].x * v1.z + v[3].x;
  83.     temp[1] = v[0].y * v1.x + v[1].y * v1.y + v[2].y * v1.z + v[3].y;
  84.     temp[2] = v[0].z * v1.x + v[1].z * v1.y + v[2].z * v1.z + v[3].z;
  85.     return temp;
  86. }
  87.  
  88. const Vector Matrix::mul_notmove(const Vector &v1) const
  89. {
  90.     Vector temp;
  91.     temp.x = v[0].x * v1.x + v[1].x * v1.y + v[2].x * v1.z;
  92.     temp.y = v[0].y * v1.x + v[1].y * v1.y + v[2].y * v1.z;
  93.     temp.z = v[0].z * v1.x + v[1].z * v1.y + v[2].z * v1.z;
  94.     return temp;
  95. }
  96.  
  97. const Matrix Matrix::operator *(const Matrix& m2) const
  98. {
  99.     Matrix temp;
  100.     temp.v[0] = mul_notmove(m2.v[0]);
  101.     temp.v[1] = mul_notmove(m2.v[1]);
  102.     temp.v[2] = mul_notmove(m2.v[2]);
  103.     temp.v[3] = mul_notmove(m2.v[3]) + v[3];
  104.     return temp;
  105. }
  106.  
  107. const Matrix Matrix::m_move(const Vector& v1)
  108. {
  109.     Matrix temp(1);
  110.     temp.v[3] = v1;
  111.     return temp;
  112. }
  113.  
  114. const Matrix Matrix::m_scale(const Vector& v1)
  115. {
  116.     Matrix temp(0);
  117.     temp.v[0].x = v1.x;
  118.     temp.v[1].y = v1.y;
  119.     temp.v[2].z = v1.z;
  120.     return temp;
  121. }
  122.  
  123. const Matrix Matrix::m_rotx(double t)
  124. {
  125.     Matrix temp(1);
  126.     temp.v[1].y = cos(t);
  127.     temp.v[1].z = sin(t);
  128.     temp.v[2].y = -temp.v[1][2];
  129.     temp.v[2].z = temp.v[1][1];
  130.     return temp;
  131. }
  132.  
  133. const Matrix Matrix::m_roty(double t)
  134. {
  135.     Matrix temp(1);
  136.     temp.v[2].z = cos(t);
  137.     temp.v[2].x = sin(t);
  138.     temp.v[0].z = -temp.v[2].x;
  139.     temp.v[0].x =  temp.v[2].z;
  140.     return temp;
  141. }
  142.  
  143. const Matrix Matrix::m_rotz(double t)
  144. {
  145.     Matrix temp(1);
  146.     temp.v[0].x = cos(t);
  147.     temp.v[0].y = sin(t);
  148.     temp.v[1].x = -temp.v[0].y;
  149.     temp.v[1].y =  temp.v[0].x;
  150.     return temp;
  151. }
  152.  
  153. const double Matrix::value(void) const
  154. {
  155.     return
  156.     v[0].x*v[1].y*v[2].z+v[1].x*v[2].y*v[0].z+v[2].x*v[0].y*v[1].z
  157.    -v[0].x*v[2].y*v[1].z-v[1].x*v[0].y*v[2].z-v[2].x*v[1].y*v[0].z;
  158. }
  159.  
  160. static double value(const Vector& v0, const Vector& v1, const Vector& v2)
  161. {
  162.     return v0.x*v1.y*v2.z+v1.x*v2.y*v0.z+v2.x*v0.y*v1.z
  163.           -v0.x*v2.y*v1.z-v1.x*v0.y*v2.z-v2.x*v1.y*v0.z;
  164. }
  165.  
  166. const Matrix Matrix::inv(void) const
  167. {
  168.     Matrix dst(0);
  169.     double val = value();
  170.     if (-minimumdouble < val && val < minimumdouble) {
  171.         return Matrix(1);
  172.     }
  173.     dst.v[0].x =  (v[1].y*v[2].z-v[2].y*v[1].z)/val;
  174.     dst.v[0].y = -(v[0].y*v[2].z-v[2].y*v[0].z)/val;
  175.     dst.v[0].z =  (v[0].y*v[1].z-v[1].y*v[0].z)/val;
  176.  
  177.     dst.v[1].x = -(v[1].x*v[2].z-v[2].x*v[1].z)/val;
  178.     dst.v[1].y =  (v[0].x*v[2].z-v[2].x*v[0].z)/val;
  179.     dst.v[1].z = -(v[0].x*v[1].z-v[1].x*v[0].z)/val;
  180.  
  181.     dst.v[2].x =  (v[1].x*v[2].y-v[2].x*v[1].y)/val;
  182.     dst.v[2].y = -(v[0].x*v[2].y-v[2].x*v[0].y)/val;
  183.     dst.v[2].z =  (v[0].x*v[1].y-v[1].x*v[0].y)/val;
  184.  
  185.     if (v[3].x != 0.0 || v[3].y != 0.0 || v[3].z != 0.0) {
  186.         dst.v[3].x = -::value(v[1], v[2], v[3])/val;
  187.         dst.v[3].y =  ::value(v[0], v[2], v[3])/val;
  188.         dst.v[3].z = -::value(v[0], v[1], v[3])/val;
  189.     }
  190.     return dst;
  191. }
  192.  
  193. const Matrix Matrix::tra(void) const
  194. {
  195.     Matrix dst(0);
  196.     dst.v[0].x = v[0].x;
  197.     dst.v[0].y = v[1].x;
  198.     dst.v[0].z = v[2].x;
  199.  
  200.     dst.v[1].x = v[0].y;
  201.     dst.v[1].y = v[1].y;
  202.     dst.v[1].z = v[2].y;
  203.  
  204.     dst.v[2].x = v[0].z;
  205.     dst.v[2].y = v[1].z;
  206.     dst.v[2].z = v[2].z;
  207.  
  208.     return dst;
  209. }
  210.  
  211.  
  212. const Vector Matrix::GetRotation(void) const
  213. {
  214.     Vector rot;
  215.     if (v[0].z <= -1.0) {
  216.         rot.y = deg(90);
  217.     } else if (v[0].z >= 1.0) {
  218.         rot.y = deg(-90);
  219.     } else {
  220.         rot.y = asin(-v[0].z);
  221.     }
  222.     double s = 1.0 - v[0].z * v[0].z;
  223.     if (s <= minimumdouble) {
  224.         rot.x = 0.0;
  225.         if (-minimumdouble < v[1].x && v[1].x < minimumdouble
  226.          && -minimumdouble < v[1].y && v[1].y < minimumdouble) {
  227.             rot.z = 0.0;
  228.         } else {
  229.             rot.z = atan2(-v[1].x, v[1].y);
  230.         }
  231.     } else {
  232.         double cosy = sqrt(s);
  233.         if (-minimumdouble < v[1].z && v[1].z < minimumdouble
  234.          && -minimumdouble < v[2].z && v[2].z < minimumdouble) {
  235.             rot.x = 0.0;
  236.         } else {
  237.             rot.x = atan2(v[1].z/cosy, v[2].z/cosy);
  238.         }
  239.         if (-minimumdouble < v[0].y && v[0].y < minimumdouble
  240.          && -minimumdouble < v[0].x && v[0].x < minimumdouble) {
  241.             rot.z = 0.0;
  242.         } else {
  243.             rot.z = atan2(v[0].y/cosy, v[0].x/cosy);
  244.         }
  245.     }
  246.     return rot;
  247. }
  248.  
  249. const Matrix Vector::operator ^(const Vector& vz) const
  250. {
  251.     Matrix m(0);
  252.     double l = length();
  253.     if (l <= minimumdouble) {
  254.         return Matrix(1);
  255.     }
  256.     m.v[0] = (*this) * (1.0 / l);
  257.     Vector vy = vz * (*this);
  258.     double vyl = vy.length();
  259.     if (vyl < minimumdouble) {
  260.         vy = Vector(0.0, 0.0, 1.0) * (*this);
  261.         vyl= vy.length();
  262.         if (vyl < minimumdouble) {
  263.             vy = Vector(1.0, 0.0, 0.0) * (*this);
  264.             vyl= vy.length();
  265.             if (vyl < minimumdouble) {
  266.                 return Matrix(1);
  267.             }
  268.         }
  269.     }
  270.     m.v[1] = vy * (1.0 / vyl);
  271. //    Vector vz2 = (*this) * vy;
  272. //    m.v[2] = (1.0 / vz2.length()) * vz2;
  273.     m.v[2] = m.v[0] * m.v[1];
  274.  
  275.     return m;
  276. }
  277.  
  278.