Matrix3D.cpp 6.1 KB

123456789101112131415161718192021222324252627282930313233343536373839404142434445464748495051525354555657585960616263646566676869707172737475767778798081828384858687888990919293949596979899100101102103104105106107108109110111112113114115116117118119120121122123124125126127128129130131132133134135136137138139140141142143144145146147148149150151152153154155156157158159160161162163164165166167168169170171172173174175176177178179180181182183184185186187188189190191192193194195196197198199200201202203204205206207208209210211212213214215216217218219220221222223224225226227228229230
  1. #include "Matrix3D.h"
  2. #include <cstring>
  3. #include <cmath>
  4. Matrix3D::Matrix3D()
  5. {
  6. setToIdentity();
  7. }
  8. void Matrix3D::setToIdentity()
  9. {
  10. data[0] = 1.0f;
  11. data[1] = 0.0f;
  12. data[2] = 0.0f;
  13. data[3] = 0.0f;
  14. data[4] = 0.0f;
  15. data[5] = 1.0f;
  16. data[6] = 0.0f;
  17. data[7] = 0.0f;
  18. data[8] = 0.0f;
  19. data[9] = 0.0f;
  20. data[10] = 1.0f;
  21. data[11] = 0.0f;
  22. data[12] = 0.0f;
  23. data[13] = 0.0f;
  24. data[14] = 0.0f;
  25. data[15] = 1.0f;
  26. }
  27. void Matrix3D::set(const Matrix3D& m)
  28. {
  29. memcpy(data, m.data, sizeof(float) * 16);
  30. }
  31. void Matrix3D::set(int row, int col, float value)
  32. {
  33. data[row + (col << 2)] = value;
  34. }
  35. float Matrix3D::get(int row, int col) const
  36. {
  37. return data[row + (col << 2)];
  38. }
  39. const float* Matrix3D::getValues() const
  40. {
  41. return data;
  42. }
  43. void Matrix3D::mul(const Matrix3D& m)
  44. {
  45. float mNew[16];
  46. mNew[0] = data[0] * m.data[0] + data[4] * m.data[1] + data[8] * m.data[2] + data[12] * m.data[3];
  47. mNew[1] = data[1] * m.data[0] + data[5] * m.data[1] + data[9] * m.data[2] + data[13] * m.data[3];
  48. mNew[2] = data[2] * m.data[0] + data[6] * m.data[1] + data[10] * m.data[2] + data[14] * m.data[3];
  49. mNew[3] = data[3] * m.data[0] + data[7] * m.data[1] + data[11] * m.data[2] + data[15] * m.data[3];
  50. mNew[4] = data[0] * m.data[4] + data[4] * m.data[5] + data[8] * m.data[6] + data[12] * m.data[7];
  51. mNew[5] = data[1] * m.data[4] + data[5] * m.data[5] + data[9] * m.data[6] + data[13] * m.data[7];
  52. mNew[6] = data[2] * m.data[4] + data[6] * m.data[5] + data[10] * m.data[6] + data[14] * m.data[7];
  53. mNew[7] = data[3] * m.data[4] + data[7] * m.data[5] + data[11] * m.data[6] + data[15] * m.data[7];
  54. mNew[8] = data[0] * m.data[8] + data[4] * m.data[9] + data[8] * m.data[10] + data[12] * m.data[11];
  55. mNew[9] = data[1] * m.data[8] + data[5] * m.data[9] + data[9] * m.data[10] + data[13] * m.data[11];
  56. mNew[10] = data[2] * m.data[8] + data[6] * m.data[9] + data[10] * m.data[10] + data[14] * m.data[11];
  57. mNew[11] = data[3] * m.data[8] + data[7] * m.data[9] + data[11] * m.data[10] + data[15] * m.data[11];
  58. mNew[12] = data[0] * m.data[12] + data[4] * m.data[13] + data[8] * m.data[14] + data[12] * m.data[15];
  59. mNew[13] = data[1] * m.data[12] + data[5] * m.data[13] + data[9] * m.data[14] + data[13] * m.data[15];
  60. mNew[14] = data[2] * m.data[12] + data[6] * m.data[13] + data[10] * m.data[14] + data[14] * m.data[15];
  61. mNew[15] = data[3] * m.data[12] + data[7] * m.data[13] + data[11] * m.data[14] + data[15] * m.data[15];
  62. memcpy(data, mNew, sizeof(float) * 16);
  63. }
  64. void Matrix3D::scale(float sx, float sy, float sz)
  65. {
  66. data[0] *= sx;
  67. data[1] *= sx;
  68. data[2] *= sx;
  69. data[3] *= sx;
  70. data[4] *= sy;
  71. data[5] *= sy;
  72. data[6] *= sy;
  73. data[7] *= sy;
  74. data[8] *= sz;
  75. data[9] *= sz;
  76. data[10] *= sz;
  77. data[11] *= sz;
  78. }
  79. void Matrix3D::translate(float tx, float ty, float tz)
  80. {
  81. data[12] += data[0] * tx + data[4] * ty + data[8] * tz;
  82. data[13] += data[1] * tx + data[5] * ty + data[9] * tz;
  83. data[14] += data[2] * tx + data[6] * ty + data[10] * tz;
  84. data[15] += data[3] * tx + data[7] * ty + data[11] * tz;
  85. }
  86. void Matrix3D::translateX(float tx)
  87. {
  88. data[12] += data[0] * tx;
  89. data[13] += data[1] * tx;
  90. data[14] += data[2] * tx;
  91. data[15] += data[3] * tx;
  92. }
  93. void Matrix3D::translateY(float ty)
  94. {
  95. data[12] += data[4] * ty;
  96. data[13] += data[5] * ty;
  97. data[14] += data[6] * ty;
  98. data[15] += data[7] * ty;
  99. }
  100. void Matrix3D::translateZ(float tz)
  101. {
  102. data[12] += data[8] * tz;
  103. data[13] += data[9] * tz;
  104. data[14] += data[10] * tz;
  105. data[15] += data[11] * tz;
  106. }
  107. void Matrix3D::translateTo(float tx, float ty, float tz)
  108. {
  109. data[0] = 1.0f;
  110. data[1] = 0.0f;
  111. data[2] = 0.0f;
  112. data[3] = 0.0f;
  113. data[4] = 0.0f;
  114. data[5] = 1.0f;
  115. data[6] = 0.0f;
  116. data[7] = 0.0f;
  117. data[8] = 0.0f;
  118. data[9] = 0.0f;
  119. data[10] = 1.0f;
  120. data[11] = 0.0f;
  121. data[12] = tx;
  122. data[13] = ty;
  123. data[14] = tz;
  124. data[15] = 1.0f;
  125. }
  126. void Matrix3D::rotateX(float degrees)
  127. {
  128. degrees *= M_PI / 180.0f;
  129. float sin = sinf(degrees);
  130. float cos = cosf(degrees);
  131. float a = data[4];
  132. float b = data[8];
  133. data[4] = a * cos + b * sin;
  134. data[8] = a * -sin + b * cos;
  135. a = data[5];
  136. b = data[9];
  137. data[5] = a * cos + b * sin;
  138. data[9] = a * -sin + b * cos;
  139. a = data[6];
  140. b = data[10];
  141. data[6] = a * cos + b * sin;
  142. data[10] = a * -sin + b * cos;
  143. a = data[7];
  144. b = data[11];
  145. data[7] = a * cos + b * sin;
  146. data[11] = a * -sin + b * cos;
  147. }
  148. void Matrix3D::rotateY(float degrees)
  149. {
  150. degrees *= M_PI / 180.0f;
  151. float sin = sinf(degrees);
  152. float cos = cosf(degrees);
  153. float a = data[0];
  154. float b = data[8];
  155. data[0] = a * cos + b * -sin;
  156. data[8] = a * sin + b * cos;
  157. a = data[1];
  158. b = data[9];
  159. data[1] = a * cos + b * -sin;
  160. data[9] = a * sin + b * cos;
  161. a = data[2];
  162. b = data[10];
  163. data[2] = a * cos + b * -sin;
  164. data[10] = a * sin + b * cos;
  165. a = data[3];
  166. b = data[11];
  167. data[3] = a * cos + b * -sin;
  168. data[11] = a * sin + b * cos;
  169. }
  170. void Matrix3D::rotateZ(float degrees)
  171. {
  172. degrees *= M_PI / 180.0f;
  173. float sin = sinf(degrees);
  174. float cos = cosf(degrees);
  175. float a = data[0];
  176. float b = data[4];
  177. data[0] = a * cos + b * sin;
  178. data[4] = a * -sin + b * cos;
  179. a = data[1];
  180. b = data[5];
  181. data[1] = a * cos + b * sin;
  182. data[5] = a * -sin + b * cos;
  183. a = data[2];
  184. b = data[6];
  185. data[2] = a * cos + b * sin;
  186. data[6] = a * -sin + b * cos;
  187. a = data[3];
  188. b = data[7];
  189. data[3] = a * cos + b * sin;
  190. data[7] = a * -sin + b * cos;
  191. }
  192. std::ostream& operator<<(std::ostream& os, const Matrix3D& m)
  193. {
  194. os << "Matrix3D\n(\n";
  195. os << m.get(0, 0) << ", " << m.get(0, 1) << ", " << m.get(0, 2) << ", " << m.get(0, 3) << "\n";
  196. os << m.get(1, 0) << ", " << m.get(1, 1) << ", " << m.get(1, 2) << ", " << m.get(1, 3) << "\n";
  197. os << m.get(2, 0) << ", " << m.get(2, 1) << ", " << m.get(2, 2) << ", " << m.get(2, 3) << "\n";
  198. os << m.get(3, 0) << ", " << m.get(3, 1) << ", " << m.get(3, 2) << ", " << m.get(3, 3) << "\n";
  199. os << ")";
  200. return os;
  201. }