Matrix.cpp 5.2 KB

123456789101112131415161718192021222324252627282930313233343536373839404142434445464748495051525354555657585960616263646566676869707172737475767778798081828384858687888990919293949596979899100101102103104105106107108109110111112113114115116117118119120121122123124125126127128129130131132133134135136137138139140141142143144145146147148149150151152153154155156157158159160161162163164165166167168169170171172173174175176177178179180181182183184
  1. #include <cmath>
  2. #include <iomanip>
  3. #include <cstring>
  4. #include <x86intrin.h>
  5. #include "client/math/Matrix.h"
  6. Matrix::Matrix() {
  7. setToIdentity();
  8. }
  9. Matrix& Matrix::set(const Matrix& other) {
  10. *this = other;
  11. return *this;
  12. }
  13. Matrix& Matrix::setToIdentity() {
  14. data[0] = 1.0f;
  15. data[1] = 0.0f;
  16. data[2] = 0.0f;
  17. data[3] = 0.0f;
  18. data[4] = 0.0f;
  19. data[5] = 1.0f;
  20. data[6] = 0.0f;
  21. data[7] = 0.0f;
  22. data[8] = 0.0f;
  23. data[9] = 0.0f;
  24. data[10] = 1.0f;
  25. data[11] = 0.0f;
  26. data[12] = 0.0f;
  27. data[13] = 0.0f;
  28. data[14] = 0.0f;
  29. data[15] = 1.0f;
  30. return *this;
  31. }
  32. Matrix& Matrix::set(uint index, float f) {
  33. data[index] = f;
  34. return *this;
  35. }
  36. const float* Matrix::getValues() const {
  37. return data;
  38. }
  39. Matrix& Matrix::mul(const Matrix& m) {
  40. float mNew[16];
  41. mNew[0] = data[0] * m.data[0] + data[4] * m.data[1] + data[8] * m.data[2] + data[12] * m.data[3];
  42. mNew[1] = data[1] * m.data[0] + data[5] * m.data[1] + data[9] * m.data[2] + data[13] * m.data[3];
  43. mNew[2] = data[2] * m.data[0] + data[6] * m.data[1] + data[10] * m.data[2] + data[14] * m.data[3];
  44. mNew[3] = data[3] * m.data[0] + data[7] * m.data[1] + data[11] * m.data[2] + data[15] * m.data[3];
  45. mNew[4] = data[0] * m.data[4] + data[4] * m.data[5] + data[8] * m.data[6] + data[12] * m.data[7];
  46. mNew[5] = data[1] * m.data[4] + data[5] * m.data[5] + data[9] * m.data[6] + data[13] * m.data[7];
  47. mNew[6] = data[2] * m.data[4] + data[6] * m.data[5] + data[10] * m.data[6] + data[14] * m.data[7];
  48. mNew[7] = data[3] * m.data[4] + data[7] * m.data[5] + data[11] * m.data[6] + data[15] * m.data[7];
  49. mNew[8] = data[0] * m.data[8] + data[4] * m.data[9] + data[8] * m.data[10] + data[12] * m.data[11];
  50. mNew[9] = data[1] * m.data[8] + data[5] * m.data[9] + data[9] * m.data[10] + data[13] * m.data[11];
  51. mNew[10] = data[2] * m.data[8] + data[6] * m.data[9] + data[10] * m.data[10] + data[14] * m.data[11];
  52. mNew[11] = data[3] * m.data[8] + data[7] * m.data[9] + data[11] * m.data[10] + data[15] * m.data[11];
  53. mNew[12] = data[0] * m.data[12] + data[4] * m.data[13] + data[8] * m.data[14] + data[12] * m.data[15];
  54. mNew[13] = data[1] * m.data[12] + data[5] * m.data[13] + data[9] * m.data[14] + data[13] * m.data[15];
  55. mNew[14] = data[2] * m.data[12] + data[6] * m.data[13] + data[10] * m.data[14] + data[14] * m.data[15];
  56. mNew[15] = data[3] * m.data[12] + data[7] * m.data[13] + data[11] * m.data[14] + data[15] * m.data[15];
  57. std::memcpy(data, mNew, sizeof (float) * 16);
  58. return *this;
  59. }
  60. Matrix& Matrix::scale(float sx, float sy, float sz) {
  61. data[0] *= sx;
  62. data[1] *= sx;
  63. data[2] *= sx;
  64. data[3] *= sx;
  65. data[4] *= sy;
  66. data[5] *= sy;
  67. data[6] *= sy;
  68. data[7] *= sy;
  69. data[8] *= sz;
  70. data[9] *= sz;
  71. data[10] *= sz;
  72. data[11] *= sz;
  73. return *this;
  74. }
  75. Matrix& Matrix::scale(float s) {
  76. return scale(s, s, s);
  77. }
  78. Matrix& Matrix::translate(float tx, float ty, float tz) {
  79. return translateX(tx).translateY(ty).translateZ(tz);
  80. }
  81. Matrix& Matrix::translateX(float tx) {
  82. data[12] += data[0] * tx;
  83. data[13] += data[1] * tx;
  84. data[14] += data[2] * tx;
  85. data[15] += data[3] * tx;
  86. return *this;
  87. }
  88. Matrix& Matrix::translateY(float ty) {
  89. data[12] += data[4] * ty;
  90. data[13] += data[5] * ty;
  91. data[14] += data[6] * ty;
  92. data[15] += data[7] * ty;
  93. return *this;
  94. }
  95. Matrix& Matrix::translateZ(float tz) {
  96. data[12] += data[8] * tz;
  97. data[13] += data[9] * tz;
  98. data[14] += data[10] * tz;
  99. data[15] += data[11] * tz;
  100. return *this;
  101. }
  102. Matrix& Matrix::translateTo(float tx, float ty, float tz) {
  103. data[0] = 1.0f;
  104. data[1] = 0.0f;
  105. data[2] = 0.0f;
  106. data[3] = 0.0f;
  107. data[4] = 0.0f;
  108. data[5] = 1.0f;
  109. data[6] = 0.0f;
  110. data[7] = 0.0f;
  111. data[8] = 0.0f;
  112. data[9] = 0.0f;
  113. data[10] = 1.0f;
  114. data[11] = 0.0f;
  115. data[12] = tx;
  116. data[13] = ty;
  117. data[14] = tz;
  118. data[15] = 1.0f;
  119. return *this;
  120. }
  121. Matrix& Matrix::rotate(float degrees, uint indexA, uint indexB) {
  122. degrees *= M_PIf32 / 180.0f;
  123. float sin;
  124. float cos;
  125. sincosf(degrees, &sin, &cos);
  126. __m128 va = _mm_load_ps(data + indexA);
  127. __m128 vb = _mm_load_ps(data + indexB);
  128. __m128 vcos = _mm_set1_ps(cos);
  129. __m128 vresult1 = _mm_add_ps(_mm_mul_ps(va, vcos), _mm_mul_ps(vb, _mm_set1_ps(sin)));
  130. __m128 vresult2 = _mm_add_ps(_mm_mul_ps(va, _mm_set1_ps(-sin)), _mm_mul_ps(vb, vcos));
  131. _mm_store_ps(data + indexA, vresult1);
  132. _mm_store_ps(data + indexB, vresult2);
  133. return *this;
  134. }
  135. Matrix& Matrix::rotateX(float degrees) {
  136. return rotate(degrees, 4, 8);
  137. }
  138. Matrix& Matrix::rotateY(float degrees) {
  139. return rotate(degrees, 0, 8);
  140. }
  141. Matrix& Matrix::rotateZ(float degrees) {
  142. return rotate(degrees, 0, 4);
  143. }
  144. std::ostream& operator<<(std::ostream& os, const Matrix& m) {
  145. const float* data = m.getValues();
  146. os << "Matrix\n(\n";
  147. os << std::fixed << std::setprecision(5);
  148. for(int i = 0; i < 4; i++) {
  149. os << std::setw(15);
  150. os << data[i] << ", ";
  151. os << std::setw(15);
  152. os << data[i + 4] << ", ";
  153. os << std::setw(15);
  154. os << data[i + 8] << ", ";
  155. os << std::setw(15);
  156. os << data[i + 12] << "\n";
  157. }
  158. os << std::defaultfloat;
  159. os << ")";
  160. return os;
  161. }