Mat4.h 16 KB

123456789101112131415161718192021222324252627282930313233343536373839404142434445464748495051525354555657585960616263646566676869707172737475767778798081828384858687888990919293949596979899100101102103104105106107108109110111112113114115116117118119120121122123124125126127128129130131132133134135136137138139140141142143144145146147148149150151152153154155156157158159160161162163164165166167168169170171172173174175176177178179180181182183184185186187188189190191192193194195196197198199200201202203204205206207208209210211212213214215216217218219220221222223224225226227228229230231232233234235236237238239240241242243244245246247248249250251252253254255256257258259260261262263264265266267268269270271272273274275276277278279280281282283284285286287288289290291292293294295296297298299300301302303304305306307308309310311312313314315316317318319320321322323324325326327328329330331332333334335336337338339340341342343344345346347348349350351352353354355356357358359360361362363364365366367368369370371372373374375376377378379380381382383384385386387388389390391392393394395396397398399400401402403404405406407408409410411412413414415416417418419420421422423424425426427428429430431432433434435436437438439440441442443444445446
  1. #pragma once
  2. #include <iostream>
  3. #include "Logging.h"
  4. #include "Mat3.h"
  5. #include "Vec3.h"
  6. namespace Framework
  7. {
  8. template<typename T>
  9. //! Eine 4x4 Matrix
  10. class Mat4
  11. {
  12. public:
  13. T elements[4][4]; //! Die Elemente der Matrix
  14. //! Kopiert alle Werte einer anderen Matrix
  15. //! \param r Die andere Matrix
  16. Mat4& operator=(const Mat4& r)
  17. {
  18. memcpy(elements, r.elements, sizeof(elements));
  19. return *this;
  20. }
  21. //! Skalliert die Matrix
  22. //! \param r der Faktor
  23. Mat4& operator*=(const T r)
  24. {
  25. for (T& e : elements)
  26. e *= r;
  27. return *this;
  28. }
  29. //! Multipliziert die MAtrix mit einer anderen
  30. //! \param r Die andere Matrix
  31. Mat4& operator*=(const Mat4& r)
  32. {
  33. return *this = *this * r;
  34. }
  35. //! Skalliert die Matrix ohne sie zu verändern
  36. //! \param r der Faktor
  37. Mat4 operator*(const T r) const
  38. {
  39. Mat4 result = *this;
  40. return result *= r;
  41. }
  42. //! Multipliziert zwei Matrizen
  43. //! \param r Die andere Matrix
  44. Mat4 operator*(const Mat4& r) const
  45. {
  46. Mat4 result;
  47. for (int j = 0; j < 4; j++)
  48. {
  49. for (int k = 0; k < 4; k++)
  50. {
  51. T sum = 0;
  52. for (int i = 0; i < 4; i++)
  53. sum += elements[j][i] * r.elements[i][k];
  54. result.elements[j][k] = sum;
  55. }
  56. }
  57. return result;
  58. }
  59. //! Multiplziert die Matrix mit einem Vektor
  60. //! \param r Der Vektor
  61. Vec3<T> operator*(const Vec3<T>& r) const
  62. {
  63. Vec3<T> result;
  64. result.x = elements[0][0] * r.x + elements[0][1] * r.y
  65. + elements[0][2] * r.z + elements[0][3];
  66. result.y = elements[1][0] * r.x + elements[1][1] * r.y
  67. + elements[1][2] * r.z + elements[1][3];
  68. result.z = elements[2][0] * r.x + elements[2][1] * r.y
  69. + elements[2][2] * r.z + elements[2][3];
  70. return result;
  71. }
  72. Vec3<T> mult0(const Vec3<T>& r) const
  73. {
  74. Vec3<T> result;
  75. result.x = elements[0][0] * r.x + elements[0][1] * r.y
  76. + elements[0][2] * r.z;
  77. result.y = elements[1][0] * r.x + elements[1][1] * r.y
  78. + elements[1][2] * r.z;
  79. result.z = elements[2][0] * r.x + elements[2][1] * r.y
  80. + elements[2][2] * r.z;
  81. return result;
  82. }
  83. //! Berechnet die inverse Matrix
  84. Mat4 getInverse() const
  85. {
  86. Mat4 ret;
  87. ret.elements[0][0]
  88. = elements[1][1] * elements[2][2] * elements[3][3]
  89. - elements[1][1] * elements[2][3] * elements[3][2]
  90. - elements[2][1] * elements[1][2] * elements[3][3]
  91. + elements[2][1] * elements[1][3] * elements[3][2]
  92. + elements[3][1] * elements[1][2] * elements[2][3]
  93. - elements[3][1] * elements[1][3] * elements[2][2];
  94. ret.elements[1][0]
  95. = -elements[1][0] * elements[2][2] * elements[3][3]
  96. + elements[1][0] * elements[2][3] * elements[3][2]
  97. + elements[2][0] * elements[1][2] * elements[3][3]
  98. - elements[2][0] * elements[1][3] * elements[3][2]
  99. - elements[3][0] * elements[1][2] * elements[2][3]
  100. + elements[3][0] * elements[1][3] * elements[2][2];
  101. ret.elements[2][0]
  102. = elements[1][0] * elements[2][1] * elements[3][3]
  103. - elements[1][0] * elements[2][3] * elements[3][1]
  104. - elements[2][0] * elements[1][1] * elements[3][3]
  105. + elements[2][0] * elements[1][3] * elements[3][1]
  106. + elements[3][0] * elements[1][1] * elements[2][3]
  107. - elements[3][0] * elements[1][3] * elements[2][1];
  108. ret.elements[3][0]
  109. = -elements[1][0] * elements[2][1] * elements[3][2]
  110. + elements[1][0] * elements[2][2] * elements[3][1]
  111. + elements[2][0] * elements[1][1] * elements[3][2]
  112. - elements[2][0] * elements[1][2] * elements[3][1]
  113. - elements[3][0] * elements[1][1] * elements[2][2]
  114. + elements[3][0] * elements[1][2] * elements[2][1];
  115. ret.elements[0][1]
  116. = -elements[0][1] * elements[2][2] * elements[3][3]
  117. + elements[0][1] * elements[2][3] * elements[3][2]
  118. + elements[2][1] * elements[0][2] * elements[3][3]
  119. - elements[2][1] * elements[0][3] * elements[3][2]
  120. - elements[3][1] * elements[0][2] * elements[2][3]
  121. + elements[3][1] * elements[0][3] * elements[2][2];
  122. ret.elements[1][1]
  123. = elements[0][0] * elements[2][2] * elements[3][3]
  124. - elements[0][0] * elements[2][3] * elements[3][2]
  125. - elements[2][0] * elements[0][2] * elements[3][3]
  126. + elements[2][0] * elements[0][3] * elements[3][2]
  127. + elements[3][0] * elements[0][2] * elements[2][3]
  128. - elements[3][0] * elements[0][3] * elements[2][2];
  129. ret.elements[2][1]
  130. = -elements[0][0] * elements[2][1] * elements[3][3]
  131. + elements[0][0] * elements[2][3] * elements[3][1]
  132. + elements[2][0] * elements[0][1] * elements[3][3]
  133. - elements[2][0] * elements[0][3] * elements[3][1]
  134. - elements[3][0] * elements[0][1] * elements[2][3]
  135. + elements[3][0] * elements[0][3] * elements[2][1];
  136. ret.elements[3][1]
  137. = elements[0][0] * elements[2][1] * elements[3][2]
  138. - elements[0][0] * elements[2][2] * elements[3][1]
  139. - elements[2][0] * elements[0][1] * elements[3][2]
  140. + elements[2][0] * elements[0][2] * elements[3][1]
  141. + elements[3][0] * elements[0][1] * elements[2][2]
  142. - elements[3][0] * elements[0][2] * elements[2][1];
  143. ret.elements[0][2]
  144. = elements[0][1] * elements[1][2] * elements[3][3]
  145. - elements[0][1] * elements[1][3] * elements[3][2]
  146. - elements[1][1] * elements[0][2] * elements[3][3]
  147. + elements[1][1] * elements[0][3] * elements[3][2]
  148. + elements[3][1] * elements[0][2] * elements[1][3]
  149. - elements[3][1] * elements[0][3] * elements[1][2];
  150. ret.elements[1][2]
  151. = -elements[0][0] * elements[1][2] * elements[3][3]
  152. + elements[0][0] * elements[1][3] * elements[3][2]
  153. + elements[1][0] * elements[0][2] * elements[3][3]
  154. - elements[1][0] * elements[0][3] * elements[3][2]
  155. - elements[3][0] * elements[0][2] * elements[1][3]
  156. + elements[3][0] * elements[0][3] * elements[1][2];
  157. ret.elements[2][2]
  158. = elements[0][0] * elements[1][1] * elements[3][3]
  159. - elements[0][0] * elements[1][3] * elements[3][1]
  160. - elements[1][0] * elements[0][1] * elements[3][3]
  161. + elements[1][0] * elements[0][3] * elements[3][1]
  162. + elements[3][0] * elements[0][1] * elements[1][3]
  163. - elements[3][0] * elements[0][3] * elements[1][1];
  164. ret.elements[3][2]
  165. = -elements[0][0] * elements[1][1] * elements[3][2]
  166. + elements[0][0] * elements[1][2] * elements[3][1]
  167. + elements[1][0] * elements[0][1] * elements[3][2]
  168. - elements[1][0] * elements[0][2] * elements[3][1]
  169. - elements[3][0] * elements[0][1] * elements[1][2]
  170. + elements[3][0] * elements[0][2] * elements[1][1];
  171. ret.elements[0][3]
  172. = -elements[0][1] * elements[1][2] * elements[2][3]
  173. + elements[0][1] * elements[1][3] * elements[2][2]
  174. + elements[1][1] * elements[0][2] * elements[2][3]
  175. - elements[1][1] * elements[0][3] * elements[2][2]
  176. - elements[2][1] * elements[0][2] * elements[1][3]
  177. + elements[2][1] * elements[0][3] * elements[1][2];
  178. ret.elements[1][3]
  179. = elements[0][0] * elements[1][2] * elements[2][3]
  180. - elements[0][0] * elements[1][3] * elements[2][2]
  181. - elements[1][0] * elements[0][2] * elements[2][3]
  182. + elements[1][0] * elements[0][3] * elements[2][2]
  183. + elements[2][0] * elements[0][2] * elements[1][3]
  184. - elements[2][0] * elements[0][3] * elements[1][2];
  185. ret.elements[2][3]
  186. = -elements[0][0] * elements[1][1] * elements[2][3]
  187. + elements[0][0] * elements[1][3] * elements[2][1]
  188. + elements[1][0] * elements[0][1] * elements[2][3]
  189. - elements[1][0] * elements[0][3] * elements[2][1]
  190. - elements[2][0] * elements[0][1] * elements[1][3]
  191. + elements[2][0] * elements[0][3] * elements[1][1];
  192. ret.elements[3][3]
  193. = elements[0][0] * elements[1][1] * elements[2][2]
  194. - elements[0][0] * elements[1][2] * elements[2][1]
  195. - elements[1][0] * elements[0][1] * elements[2][2]
  196. + elements[1][0] * elements[0][2] * elements[2][1]
  197. + elements[2][0] * elements[0][1] * elements[1][2]
  198. - elements[2][0] * elements[0][2] * elements[1][1];
  199. T det = elements[0][0] * ret.elements[0][0]
  200. + elements[0][1] * ret.elements[1][0]
  201. + elements[0][2] * ret.elements[2][0]
  202. + elements[0][3] * ret.elements[3][0];
  203. if (det == 0)
  204. {
  205. Logging::error() << "Fehler beim erstellen der Inversen Matrix";
  206. return ret;
  207. }
  208. det = 1.0f / det;
  209. for (int i = 0; i < 16; i++)
  210. ret.elements[i / 4][i % 4] = ret.elements[i / 4][i % 4] * det;
  211. return ret;
  212. }
  213. //! Erzeugt eine Matrix, die einen Vektor um die Z Achse dreht, wenn sie
  214. //! mit ihm multipliziert wird \param radian Der Winkel in Bogenmas
  215. static Mat4 rotationZ(T radian)
  216. {
  217. const T cosTheta = (T)lowPrecisionCos(radian);
  218. const T sinTheta = (T)lowPrecisionSin(radian);
  219. Mat4 r = {cosTheta,
  220. -sinTheta,
  221. 0,
  222. 0,
  223. sinTheta,
  224. cosTheta,
  225. 0,
  226. 0,
  227. 0,
  228. 0,
  229. 1,
  230. 0,
  231. 0,
  232. 0,
  233. 0,
  234. 1};
  235. return r;
  236. }
  237. //! Erzeugt eine Matrix, die einen Vektor um die X Achse dreht, wenn sie
  238. //! mit ihm multipliziert wird \param radian Der Winkel in Bogenmas
  239. static Mat4 rotationX(T radian)
  240. {
  241. const T cosTheta = (T)lowPrecisionCos(radian);
  242. const T sinTheta = (T)lowPrecisionSin(radian);
  243. Mat4 r = {1,
  244. 0,
  245. 0,
  246. 0,
  247. 0,
  248. cosTheta,
  249. -sinTheta,
  250. 0,
  251. 0,
  252. sinTheta,
  253. cosTheta,
  254. 0,
  255. 0,
  256. 0,
  257. 0,
  258. 1};
  259. return r;
  260. }
  261. //! Erzeugt eine Matrix, die einen Vektor um die Y Achse dreht, wenn sie
  262. //! mit ihm multipliziert wird \param radian Der Winkel in Bogenmas
  263. static Mat4 rotationY(T radian)
  264. {
  265. const T cosTheta = (T)lowPrecisionCos(radian);
  266. const T sinTheta = (T)lowPrecisionSin(radian);
  267. Mat4 r = {cosTheta,
  268. 0,
  269. sinTheta,
  270. 0,
  271. 0,
  272. 1,
  273. 0,
  274. 0,
  275. -sinTheta,
  276. 0,
  277. cosTheta,
  278. 0,
  279. 0,
  280. 0,
  281. 0,
  282. 1};
  283. return r;
  284. }
  285. //! Erzeugt eine Matrix, die einen Vektor Skalliert, wenn sie mit ihm
  286. //! multipliziert wird \param faktor Der Faktor
  287. static Mat4 scaling(T faktor)
  288. {
  289. Mat4 s = {
  290. faktor, 0, 0, 0, 0, faktor, 0, 0, 0, 0, faktor, 0, 0, 0, 0, 1};
  291. return s;
  292. }
  293. //! Erzeugt eine Matrix, die einen Vektor Skalliert, wenn sie mit ihm
  294. //! multipliziert wird \param faktorX Der Faktor für die X Komponente
  295. //! des Vektors \param faktorY Der Faktor für die Y Komponente des
  296. //! Vektors \param faktorZ Der Faktor für die Z Komponente des Vektors
  297. static Mat4 scaling(T faktorX, T faktorY, T faktorZ)
  298. {
  299. Mat4 s = {faktorX,
  300. 0,
  301. 0,
  302. 0,
  303. 0,
  304. faktorY,
  305. 0,
  306. 0,
  307. 0,
  308. 0,
  309. faktorZ,
  310. 0,
  311. 0,
  312. 0,
  313. 0,
  314. 1};
  315. return s;
  316. }
  317. //! Erzeugt eine Matrix, die einen Vektor verchiebt, wenn sie mit ihm
  318. //! multipliziert wird \param offset Die Koordinaten, um die der Vektor
  319. //! verschoben werden soll
  320. static Mat4 translation(const Vec3<T> offset)
  321. {
  322. Mat4 t = {1,
  323. 0,
  324. 0,
  325. offset.x,
  326. 0,
  327. 1,
  328. 0,
  329. offset.y,
  330. 0,
  331. 0,
  332. 1,
  333. offset.z,
  334. 0,
  335. 0,
  336. 0,
  337. 1};
  338. return t;
  339. }
  340. //! Erzeugt eine Matrix, die einen Vektor auf den Bildschirm Projeziert
  341. //! \param openingAngle Der Öffnungswinkel der Kamera im Bogenmas
  342. //! \param bildschirmXY Das Seitenverhältnis des Rechtecks auf dem
  343. //! Bildschirm, in dem gezeichnet werden soll. (Breite / Höhe) \param
  344. //! minz Der Mindestabstand zur Kamera, ab dem gezeichnet wird \param
  345. //! maxZ Der Maximalabstand zur Kamera, ab dem nicht mehr gezeichnet
  346. //! wird
  347. static Mat4 projektion(
  348. float openingAngle, float bildschirmXY, float minZ, float maxZ)
  349. {
  350. Mat4 p = {(float)(1 / tan(openingAngle / 2)) / bildschirmXY,
  351. 0,
  352. 0,
  353. 0,
  354. 0,
  355. (float)(1 / tan(openingAngle / 2)),
  356. 0,
  357. 0,
  358. 0,
  359. 0,
  360. maxZ / (maxZ - minZ),
  361. -(minZ * maxZ) / (maxZ - minZ),
  362. 0,
  363. 0,
  364. 1,
  365. 0};
  366. return p;
  367. }
  368. //! Erzeugt eine Matrix, die mit allem Multipliziert werden kann ohne es
  369. //! zu ändern
  370. static Mat4 identity()
  371. {
  372. Mat4 i = {1, 0, 0, 0, 0, 1, 0, 0, 0, 0, 1, 0, 0, 0, 0, 1};
  373. return i;
  374. }
  375. //! Gibt eine Rotationsmatrix zurück, so dass vector a wenn man ihn
  376. //! damit dreht in richtung des vectors b zeigt \param a der vector der
  377. //! gedreht werden soll \param b der vector zu dem gedreht werden soll
  378. static Mat4 rotationTo(Vec3<T>& a, Vec3<T>& b)
  379. {
  380. Vec3<T> aNorm = Vec3<T>(a).normalize();
  381. Vec3<T> bNorm = Vec3<T>(b).normalize();
  382. Vec3<T> v = aNorm.crossProduct(bNorm);
  383. T s = v.getLengthSq();
  384. T c = aNorm * bNorm;
  385. T m = (1 - c) / s;
  386. Mat3<T> cpm({0, -v.z, v.y, v.z, 0, -v.x, -v.y, v.x, 0});
  387. Mat3<T> cpm2 = cpm * cpm;
  388. Mat3<T> res = Mat3<T>::identity() + cpm + cpm2 * m;
  389. return Mat4({res.elements[0][0],
  390. res.elements[0][1],
  391. res.elements[0][2],
  392. 0,
  393. res.elements[1][0],
  394. res.elements[1][1],
  395. res.elements[1][2],
  396. 0,
  397. res.elements[2][0],
  398. res.elements[2][1],
  399. res.elements[2][2],
  400. 0,
  401. 0,
  402. 0,
  403. 0,
  404. 1});
  405. }
  406. };
  407. } // namespace Framework