You can not select more than 25 topics Topics must start with a letter or number, can include dashes ('-') and can be up to 35 characters long.

mathfuncs.cpp 20KB

123456789101112131415161718192021222324252627282930313233343536373839404142434445464748495051525354555657585960616263646566676869707172737475767778798081828384858687888990919293949596979899100101102103104105106107108109110111112113114115116117118119120121122123124125126127128129130131132133134135136137138139140141142143144145146147148149150151152153154155156157158159160161162163164165166167168169170171172173174175176177178179180181182183184185186187188189190191192193194195196197198199200201202203204205206207208209210211212213214215216217218219220221222223224225226227228229230231232233234235236237238239240241242243244245246247248249250251252253254255256257258259260261262263264265266267268269270271272273274275276277278279280281282283284285286287288289290291292293294295296297298299300301302303304305306307308309310311312313314315316317318319320321322323324325326327328329330331332333334335336337338339340341342343344345346347348349350351352353354355356357358359360361362363364365366367368369370371372373374375376377378379380381382383384385386387388389390391392393394395396397398399400401402403404405406407408409410411412413414415416417418419420421422423424425426427428429430431432433434435436437438439440441442443444445446447448449450451452453454455456457458459460461462463464465466467468469470471472473474475476477478479480481482483484485486487488489490491492493494495496497498499500501502503504505506507508509510511512513514515516517518519520521522523524525526527528529530531532533534535536537538539540541542543544545546547548549550551552553554555556557558559560561562563564565566567568569570571572573574575576577578579580581582583584585586587588589590591592593594595596597598599600601602603604605606607608609610611612613614615616617618619620621622623624625626627628629630631632633634635636637638639640641642643644645646647648649650651652653654655656657658659660661662663664665666667668669670671672673674675676677678679680681682683684685686687688689690691692693694695696697698699700701702703704705706707708709710711712713714715716717718719720721722723724725726727728729730731732733734735736737738739740741742743744745746747748749750751752753754755756757
  1. /******************************************************************************\
  2. | OpenGL 4 Example Code. |
  3. | Accompanies written series "Anton's OpenGL 4 Tutorials" |
  4. | Email: anton at antongerdelan dot net |
  5. | First version 27 Jan 2014 |
  6. | Copyright Dr Anton Gerdelan, Trinity College Dublin, Ireland. |
  7. | See individual libraries' separate legal notices |
  8. |******************************************************************************|
  9. | Commonly-used maths structures and functions |
  10. | Simple-as-possible. No disgusting templates. |
  11. | Structs vec3, mat4, versor. just hold arrays of floats called "v","m","q", |
  12. | respectively. So, for example, to get values from a mat4 do: my_mat.m |
  13. | A versor is the proper name for a unit quaternion. |
  14. \******************************************************************************/
  15. #include "mathfuncs.h"
  16. #include <stdio.h>
  17. #define _USE_MATH_DEFINES
  18. #include <math.h>
  19. /*--------------------------------CONSTRUCTORS--------------------------------*/
  20. vec2::vec2 () {}
  21. vec2::vec2 (float x, float y) {
  22. v[0] = x;
  23. v[1] = y;
  24. }
  25. vec3::vec3 () {}
  26. vec3::vec3 (float x, float y, float z) {
  27. v[0] = x;
  28. v[1] = y;
  29. v[2] = z;
  30. }
  31. vec3::vec3 (const vec2& vv, float z) {
  32. v[0] = vv.v[0];
  33. v[1] = vv.v[1];
  34. v[2] = z;
  35. }
  36. vec3::vec3 (const vec4& vv) {
  37. v[0] = vv.v[0];
  38. v[1] = vv.v[1];
  39. v[2] = vv.v[2];
  40. }
  41. vec4::vec4 () {}
  42. vec4::vec4 (float x, float y, float z, float w) {
  43. v[0] = x;
  44. v[1] = y;
  45. v[2] = z;
  46. v[3] = w;
  47. }
  48. vec4::vec4 (const vec2& vv, float z, float w) {
  49. v[0] = vv.v[0];
  50. v[1] = vv.v[1];
  51. v[2] = z;
  52. v[3] = w;
  53. }
  54. vec4::vec4 (const vec3& vv, float w) {
  55. v[0] = vv.v[0];
  56. v[1] = vv.v[1];
  57. v[2] = vv.v[2];
  58. v[3] = w;
  59. }
  60. mat3::mat3 () {}
  61. /* note: entered in COLUMNS */
  62. mat3::mat3 (float a, float b, float c,
  63. float d, float e, float f,
  64. float g, float h, float i) {
  65. m[0] = a;
  66. m[1] = b;
  67. m[2] = c;
  68. m[3] = d;
  69. m[4] = e;
  70. m[5] = f;
  71. m[6] = g;
  72. m[7] = h;
  73. m[8] = i;
  74. }
  75. mat4::mat4 () {}
  76. /* note: entered in COLUMNS */
  77. mat4::mat4 (float a, float b, float c, float d,
  78. float e, float f, float g, float h,
  79. float i, float j, float k, float l,
  80. float mm, float n, float o, float p) {
  81. m[0] = a;
  82. m[1] = b;
  83. m[2] = c;
  84. m[3] = d;
  85. m[4] = e;
  86. m[5] = f;
  87. m[6] = g;
  88. m[7] = h;
  89. m[8] = i;
  90. m[9] = j;
  91. m[10] = k;
  92. m[11] = l;
  93. m[12] = mm;
  94. m[13] = n;
  95. m[14] = o;
  96. m[15] = p;
  97. }
  98. /*-----------------------------PRINT FUNCTIONS--------------------------------*/
  99. void print (const vec2& v) {
  100. printf ("[%.2f, %.2f]\n", v.v[0], v.v[1]);
  101. }
  102. void print (const vec3& v) {
  103. printf ("[%.2f, %.2f, %.2f]\n", v.v[0], v.v[1], v.v[2]);
  104. }
  105. void print (const vec4& v) {
  106. printf ("[%.2f, %.2f, %.2f, %.2f]\n", v.v[0], v.v[1], v.v[2], v.v[3]);
  107. }
  108. void print (const mat3& m) {
  109. printf("\n");
  110. printf ("[%.2f][%.2f][%.2f]\n", m.m[0], m.m[3], m.m[6]);
  111. printf ("[%.2f][%.2f][%.2f]\n", m.m[1], m.m[4], m.m[7]);
  112. printf ("[%.2f][%.2f][%.2f]\n", m.m[2], m.m[5], m.m[8]);
  113. }
  114. void print (const mat4& m) {
  115. printf("\n");
  116. printf ("[%.2f][%.2f][%.2f][%.2f]\n", m.m[0], m.m[4], m.m[8], m.m[12]);
  117. printf ("[%.2f][%.2f][%.2f][%.2f]\n", m.m[1], m.m[5], m.m[9], m.m[13]);
  118. printf ("[%.2f][%.2f][%.2f][%.2f]\n", m.m[2], m.m[6], m.m[10], m.m[14]);
  119. printf ("[%.2f][%.2f][%.2f][%.2f]\n", m.m[3], m.m[7], m.m[11], m.m[15]);
  120. }
  121. /*--------------------------FLOATING POINT FUNCTIONS--------------------------*/
  122. int signum(const float& f) {
  123. return (f > 0.0f) ? 1 : (f < 0.0f) ? -1 : 0;
  124. }
  125. float intbound(const float& s, const float& ds) {
  126. if (ds < 0) {
  127. return intbound(-s, -ds);
  128. } else {
  129. float ss = mod(s, 1);
  130. return (1-ss)/ds;
  131. }
  132. }
  133. float mod(const float& value, const float& modulus) {
  134. return fmod((fmod(value, modulus) + modulus), modulus);
  135. }
  136. /*------------------------------VECTOR FUNCTIONS------------------------------*/
  137. float length (const vec3& v) {
  138. return sqrt (v.v[0] * v.v[0] + v.v[1] * v.v[1] + v.v[2] * v.v[2]);
  139. }
  140. // squared length
  141. float length2 (const vec3& v) {
  142. return v.v[0] * v.v[0] + v.v[1] * v.v[1] + v.v[2] * v.v[2];
  143. }
  144. // note: proper spelling (hehe)
  145. vec3 normalise (const vec3& v) {
  146. vec3 vb;
  147. float l = length (v);
  148. if (0.0f == l) {
  149. return vec3 (0.0f, 0.0f, 0.0f);
  150. }
  151. vb.v[0] = v.v[0] / l;
  152. vb.v[1] = v.v[1] / l;
  153. vb.v[2] = v.v[2] / l;
  154. return vb;
  155. }
  156. vec3 vec3::operator+ (const vec3& rhs) {
  157. vec3 vc;
  158. vc.v[0] = v[0] + rhs.v[0];
  159. vc.v[1] = v[1] + rhs.v[1];
  160. vc.v[2] = v[2] + rhs.v[2];
  161. return vc;
  162. }
  163. vec3& vec3::operator+= (const vec3& rhs) {
  164. v[0] += rhs.v[0];
  165. v[1] += rhs.v[1];
  166. v[2] += rhs.v[2];
  167. return *this; // return self
  168. }
  169. vec3 vec3::operator- (const vec3& rhs) {
  170. vec3 vc;
  171. vc.v[0] = v[0] - rhs.v[0];
  172. vc.v[1] = v[1] - rhs.v[1];
  173. vc.v[2] = v[2] - rhs.v[2];
  174. return vc;
  175. }
  176. vec3& vec3::operator-= (const vec3& rhs) {
  177. v[0] -= rhs.v[0];
  178. v[1] -= rhs.v[1];
  179. v[2] -= rhs.v[2];
  180. return *this;
  181. }
  182. vec3 vec3::operator+ (float rhs) {
  183. vec3 vc;
  184. vc.v[0] = v[0] + rhs;
  185. vc.v[1] = v[1] + rhs;
  186. vc.v[2] = v[2] + rhs;
  187. return vc;
  188. }
  189. vec3 vec3::operator- (float rhs) {
  190. vec3 vc;
  191. vc.v[0] = v[0] - rhs;
  192. vc.v[1] = v[1] - rhs;
  193. vc.v[2] = v[2] - rhs;
  194. return vc;
  195. }
  196. vec3 vec3::operator* (float rhs) {
  197. vec3 vc;
  198. vc.v[0] = v[0] * rhs;
  199. vc.v[1] = v[1] * rhs;
  200. vc.v[2] = v[2] * rhs;
  201. return vc;
  202. }
  203. vec3 vec3::operator/ (float rhs) {
  204. vec3 vc;
  205. vc.v[0] = v[0] / rhs;
  206. vc.v[1] = v[1] / rhs;
  207. vc.v[2] = v[2] / rhs;
  208. return vc;
  209. }
  210. vec3& vec3::operator*= (float rhs) {
  211. v[0] = v[0] * rhs;
  212. v[1] = v[1] * rhs;
  213. v[2] = v[2] * rhs;
  214. return *this;
  215. }
  216. vec3& vec3::operator= (const vec3& rhs) {
  217. v[0] = rhs.v[0];
  218. v[1] = rhs.v[1];
  219. v[2] = rhs.v[2];
  220. return *this;
  221. }
  222. float dot (const vec3& a, const vec3& b) {
  223. return a.v[0] * b.v[0] + a.v[1] * b.v[1] + a.v[2] * b.v[2];
  224. }
  225. vec3 cross (const vec3& a, const vec3& b) {
  226. float x = a.v[1] * b.v[2] - a.v[2] * b.v[1];
  227. float y = a.v[2] * b.v[0] - a.v[0] * b.v[2];
  228. float z = a.v[0] * b.v[1] - a.v[1] * b.v[0];
  229. return vec3 (x, y, z);
  230. }
  231. float get_squared_dist (vec3 from, vec3 to) {
  232. float x = (to.v[0] - from.v[0]) * (to.v[0] - from.v[0]);
  233. float y = (to.v[1] - from.v[1]) * (to.v[1] - from.v[1]);
  234. float z = (to.v[2] - from.v[2]) * (to.v[2] - from.v[2]);
  235. return x + y + z;
  236. }
  237. /* converts an un-normalised direction into a heading in degrees
  238. NB i suspect that the z is backwards here but i've used in in
  239. several places like this. d'oh! */
  240. float direction_to_heading (vec3 d) {
  241. return atan2 (-d.v[0], -d.v[2]) * ONE_RAD_IN_DEG;
  242. }
  243. vec3 heading_to_direction (float degrees) {
  244. float rad = degrees * ONE_DEG_IN_RAD;
  245. return vec3 (-sinf (rad), 0.0f, -cosf (rad));
  246. }
  247. /*-----------------------------MATRIX FUNCTIONS-------------------------------*/
  248. mat3 zero_mat3 () {
  249. return mat3 (
  250. 0.0f, 0.0f, 0.0f,
  251. 0.0f, 0.0f, 0.0f,
  252. 0.0f, 0.0f, 0.0f
  253. );
  254. }
  255. mat3 identity_mat3 () {
  256. return mat3 (
  257. 1.0f, 0.0f, 0.0f,
  258. 0.0f, 1.0f, 0.0f,
  259. 0.0f, 0.0f, 1.0f
  260. );
  261. }
  262. mat4 zero_mat4 () {
  263. return mat4 (
  264. 0.0f, 0.0f, 0.0f, 0.0f,
  265. 0.0f, 0.0f, 0.0f, 0.0f,
  266. 0.0f, 0.0f, 0.0f, 0.0f,
  267. 0.0f, 0.0f, 0.0f, 0.0f
  268. );
  269. }
  270. mat4 identity_mat4 () {
  271. return mat4 (
  272. 1.0f, 0.0f, 0.0f, 0.0f,
  273. 0.0f, 1.0f, 0.0f, 0.0f,
  274. 0.0f, 0.0f, 1.0f, 0.0f,
  275. 0.0f, 0.0f, 0.0f, 1.0f
  276. );
  277. }
  278. /* mat4 array layout
  279. 0 4 8 12
  280. 1 5 9 13
  281. 2 6 10 14
  282. 3 7 11 15
  283. */
  284. vec4 mat4::operator* (const vec4& rhs) {
  285. // 0x + 4y + 8z + 12w
  286. float x =
  287. m[0] * rhs.v[0] +
  288. m[4] * rhs.v[1] +
  289. m[8] * rhs.v[2] +
  290. m[12] * rhs.v[3];
  291. // 1x + 5y + 9z + 13w
  292. float y = m[1] * rhs.v[0] +
  293. m[5] * rhs.v[1] +
  294. m[9] * rhs.v[2] +
  295. m[13] * rhs.v[3];
  296. // 2x + 6y + 10z + 14w
  297. float z = m[2] * rhs.v[0] +
  298. m[6] * rhs.v[1] +
  299. m[10] * rhs.v[2] +
  300. m[14] * rhs.v[3];
  301. // 3x + 7y + 11z + 15w
  302. float w = m[3] * rhs.v[0] +
  303. m[7] * rhs.v[1] +
  304. m[11] * rhs.v[2] +
  305. m[15] * rhs.v[3];
  306. return vec4 (x, y, z, w);
  307. }
  308. mat4 mat4::operator* (const mat4& rhs) {
  309. mat4 r = zero_mat4 ();
  310. int r_index = 0;
  311. for (int col = 0; col < 4; col++) {
  312. for (int row = 0; row < 4; row++) {
  313. float sum = 0.0f;
  314. for (int i = 0; i < 4; i++) {
  315. sum += rhs.m[i + col * 4] * m[row + i * 4];
  316. }
  317. r.m[r_index] = sum;
  318. r_index++;
  319. }
  320. }
  321. return r;
  322. }
  323. mat4& mat4::operator= (const mat4& rhs) {
  324. for (int i = 0; i < 16; i++) {
  325. m[i] = rhs.m[i];
  326. }
  327. return *this;
  328. }
  329. // returns a scalar value with the determinant for a 4x4 matrix
  330. // see http://www.euclideanspace.com/maths/algebra/matrix/functions/determinant/fourD/index.htm
  331. float determinant (const mat4& mm) {
  332. return
  333. mm.m[12] * mm.m[9] * mm.m[6] * mm.m[3] -
  334. mm.m[8] * mm.m[13] * mm.m[6] * mm.m[3] -
  335. mm.m[12] * mm.m[5] * mm.m[10] * mm.m[3] +
  336. mm.m[4] * mm.m[13] * mm.m[10] * mm.m[3] +
  337. mm.m[8] * mm.m[5] * mm.m[14] * mm.m[3] -
  338. mm.m[4] * mm.m[9] * mm.m[14] * mm.m[3] -
  339. mm.m[12] * mm.m[9] * mm.m[2] * mm.m[7] +
  340. mm.m[8] * mm.m[13] * mm.m[2] * mm.m[7] +
  341. mm.m[12] * mm.m[1] * mm.m[10] * mm.m[7] -
  342. mm.m[0] * mm.m[13] * mm.m[10] * mm.m[7] -
  343. mm.m[8] * mm.m[1] * mm.m[14] * mm.m[7] +
  344. mm.m[0] * mm.m[9] * mm.m[14] * mm.m[7] +
  345. mm.m[12] * mm.m[5] * mm.m[2] * mm.m[11] -
  346. mm.m[4] * mm.m[13] * mm.m[2] * mm.m[11] -
  347. mm.m[12] * mm.m[1] * mm.m[6] * mm.m[11] +
  348. mm.m[0] * mm.m[13] * mm.m[6] * mm.m[11] +
  349. mm.m[4] * mm.m[1] * mm.m[14] * mm.m[11] -
  350. mm.m[0] * mm.m[5] * mm.m[14] * mm.m[11] -
  351. mm.m[8] * mm.m[5] * mm.m[2] * mm.m[15] +
  352. mm.m[4] * mm.m[9] * mm.m[2] * mm.m[15] +
  353. mm.m[8] * mm.m[1] * mm.m[6] * mm.m[15] -
  354. mm.m[0] * mm.m[9] * mm.m[6] * mm.m[15] -
  355. mm.m[4] * mm.m[1] * mm.m[10] * mm.m[15] +
  356. mm.m[0] * mm.m[5] * mm.m[10] * mm.m[15];
  357. }
  358. /* returns a 16-element array that is the inverse of a 16-element array (4x4
  359. matrix). see http://www.euclideanspace.com/maths/algebra/matrix/functions/inverse/fourD/index.htm */
  360. mat4 inverse (const mat4& mm) {
  361. float det = determinant (mm);
  362. /* there is no inverse if determinant is zero (not likely unless scale is
  363. broken) */
  364. if (0.0f == det) {
  365. fprintf (stderr, "WARNING. matrix has no determinant. can not invert\n");
  366. return mm;
  367. }
  368. float inv_det = 1.0f / det;
  369. return mat4 (
  370. inv_det * (
  371. mm.m[9] * mm.m[14] * mm.m[7] - mm.m[13] * mm.m[10] * mm.m[7] +
  372. mm.m[13] * mm.m[6] * mm.m[11] - mm.m[5] * mm.m[14] * mm.m[11] -
  373. mm.m[9] * mm.m[6] * mm.m[15] + mm.m[5] * mm.m[10] * mm.m[15]
  374. ),
  375. inv_det * (
  376. mm.m[13] * mm.m[10] * mm.m[3] - mm.m[9] * mm.m[14] * mm.m[3] -
  377. mm.m[13] * mm.m[2] * mm.m[11] + mm.m[1] * mm.m[14] * mm.m[11] +
  378. mm.m[9] * mm.m[2] * mm.m[15] - mm.m[1] * mm.m[10] * mm.m[15]
  379. ),
  380. inv_det * (
  381. mm.m[5] * mm.m[14] * mm.m[3] - mm.m[13] * mm.m[6] * mm.m[3] +
  382. mm.m[13] * mm.m[2] * mm.m[7] - mm.m[1] * mm.m[14] * mm.m[7] -
  383. mm.m[5] * mm.m[2] * mm.m[15] + mm.m[1] * mm.m[6] * mm.m[15]
  384. ),
  385. inv_det * (
  386. mm.m[9] * mm.m[6] * mm.m[3] - mm.m[5] * mm.m[10] * mm.m[3] -
  387. mm.m[9] * mm.m[2] * mm.m[7] + mm.m[1] * mm.m[10] * mm.m[7] +
  388. mm.m[5] * mm.m[2] * mm.m[11] - mm.m[1] * mm.m[6] * mm.m[11]
  389. ),
  390. inv_det * (
  391. mm.m[12] * mm.m[10] * mm.m[7] - mm.m[8] * mm.m[14] * mm.m[7] -
  392. mm.m[12] * mm.m[6] * mm.m[11] + mm.m[4] * mm.m[14] * mm.m[11] +
  393. mm.m[8] * mm.m[6] * mm.m[15] - mm.m[4] * mm.m[10] * mm.m[15]
  394. ),
  395. inv_det * (
  396. mm.m[8] * mm.m[14] * mm.m[3] - mm.m[12] * mm.m[10] * mm.m[3] +
  397. mm.m[12] * mm.m[2] * mm.m[11] - mm.m[0] * mm.m[14] * mm.m[11] -
  398. mm.m[8] * mm.m[2] * mm.m[15] + mm.m[0] * mm.m[10] * mm.m[15]
  399. ),
  400. inv_det * (
  401. mm.m[12] * mm.m[6] * mm.m[3] - mm.m[4] * mm.m[14] * mm.m[3] -
  402. mm.m[12] * mm.m[2] * mm.m[7] + mm.m[0] * mm.m[14] * mm.m[7] +
  403. mm.m[4] * mm.m[2] * mm.m[15] - mm.m[0] * mm.m[6] * mm.m[15]
  404. ),
  405. inv_det * (
  406. mm.m[4] * mm.m[10] * mm.m[3] - mm.m[8] * mm.m[6] * mm.m[3] +
  407. mm.m[8] * mm.m[2] * mm.m[7] - mm.m[0] * mm.m[10] * mm.m[7] -
  408. mm.m[4] * mm.m[2] * mm.m[11] + mm.m[0] * mm.m[6] * mm.m[11]
  409. ),
  410. inv_det * (
  411. mm.m[8] * mm.m[13] * mm.m[7] - mm.m[12] * mm.m[9] * mm.m[7] +
  412. mm.m[12] * mm.m[5] * mm.m[11] - mm.m[4] * mm.m[13] * mm.m[11] -
  413. mm.m[8] * mm.m[5] * mm.m[15] + mm.m[4] * mm.m[9] * mm.m[15]
  414. ),
  415. inv_det * (
  416. mm.m[12] * mm.m[9] * mm.m[3] - mm.m[8] * mm.m[13] * mm.m[3] -
  417. mm.m[12] * mm.m[1] * mm.m[11] + mm.m[0] * mm.m[13] * mm.m[11] +
  418. mm.m[8] * mm.m[1] * mm.m[15] - mm.m[0] * mm.m[9] * mm.m[15]
  419. ),
  420. inv_det * (
  421. mm.m[4] * mm.m[13] * mm.m[3] - mm.m[12] * mm.m[5] * mm.m[3] +
  422. mm.m[12] * mm.m[1] * mm.m[7] - mm.m[0] * mm.m[13] * mm.m[7] -
  423. mm.m[4] * mm.m[1] * mm.m[15] + mm.m[0] * mm.m[5] * mm.m[15]
  424. ),
  425. inv_det * (
  426. mm.m[8] * mm.m[5] * mm.m[3] - mm.m[4] * mm.m[9] * mm.m[3] -
  427. mm.m[8] * mm.m[1] * mm.m[7] + mm.m[0] * mm.m[9] * mm.m[7] +
  428. mm.m[4] * mm.m[1] * mm.m[11] - mm.m[0] * mm.m[5] * mm.m[11]
  429. ),
  430. inv_det * (
  431. mm.m[12] * mm.m[9] * mm.m[6] - mm.m[8] * mm.m[13] * mm.m[6] -
  432. mm.m[12] * mm.m[5] * mm.m[10] + mm.m[4] * mm.m[13] * mm.m[10] +
  433. mm.m[8] * mm.m[5] * mm.m[14] - mm.m[4] * mm.m[9] * mm.m[14]
  434. ),
  435. inv_det * (
  436. mm.m[8] * mm.m[13] * mm.m[2] - mm.m[12] * mm.m[9] * mm.m[2] +
  437. mm.m[12] * mm.m[1] * mm.m[10] - mm.m[0] * mm.m[13] * mm.m[10] -
  438. mm.m[8] * mm.m[1] * mm.m[14] + mm.m[0] * mm.m[9] * mm.m[14]
  439. ),
  440. inv_det * (
  441. mm.m[12] * mm.m[5] * mm.m[2] - mm.m[4] * mm.m[13] * mm.m[2] -
  442. mm.m[12] * mm.m[1] * mm.m[6] + mm.m[0] * mm.m[13] * mm.m[6] +
  443. mm.m[4] * mm.m[1] * mm.m[14] - mm.m[0] * mm.m[5] * mm.m[14]
  444. ),
  445. inv_det * (
  446. mm.m[4] * mm.m[9] * mm.m[2] - mm.m[8] * mm.m[5] * mm.m[2] +
  447. mm.m[8] * mm.m[1] * mm.m[6] - mm.m[0] * mm.m[9] * mm.m[6] -
  448. mm.m[4] * mm.m[1] * mm.m[10] + mm.m[0] * mm.m[5] * mm.m[10]
  449. )
  450. );
  451. }
  452. // returns a 16-element array flipped on the main diagonal
  453. mat4 transpose (const mat4& mm) {
  454. return mat4 (
  455. mm.m[0], mm.m[4], mm.m[8], mm.m[12],
  456. mm.m[1], mm.m[5], mm.m[9], mm.m[13],
  457. mm.m[2], mm.m[6], mm.m[10], mm.m[14],
  458. mm.m[3], mm.m[7], mm.m[11], mm.m[15]
  459. );
  460. }
  461. /*--------------------------AFFINE MATRIX FUNCTIONS---------------------------*/
  462. // translate a 4d matrix with xyz array
  463. mat4 translate (const mat4& m, const vec3& v) {
  464. mat4 m_t = identity_mat4 ();
  465. m_t.m[12] = v.v[0];
  466. m_t.m[13] = v.v[1];
  467. m_t.m[14] = v.v[2];
  468. return m_t * m;
  469. }
  470. // rotate around x axis by an angle in degrees
  471. mat4 rotate_x_deg (const mat4& m, float deg) {
  472. // convert to radians
  473. float rad = deg * ONE_DEG_IN_RAD;
  474. mat4 m_r = identity_mat4 ();
  475. m_r.m[5] = cos (rad);
  476. m_r.m[9] = -sin (rad);
  477. m_r.m[6] = sin (rad);
  478. m_r.m[10] = cos (rad);
  479. return m_r * m;
  480. }
  481. // rotate around y axis by an angle in degrees
  482. mat4 rotate_y_deg (const mat4& m, float deg) {
  483. // convert to radians
  484. float rad = deg * ONE_DEG_IN_RAD;
  485. mat4 m_r = identity_mat4 ();
  486. m_r.m[0] = cos (rad);
  487. m_r.m[8] = sin (rad);
  488. m_r.m[2] = -sin (rad);
  489. m_r.m[10] = cos (rad);
  490. return m_r * m;
  491. }
  492. // rotate around z axis by an angle in degrees
  493. mat4 rotate_z_deg (const mat4& m, float deg) {
  494. // convert to radians
  495. float rad = deg * ONE_DEG_IN_RAD;
  496. mat4 m_r = identity_mat4 ();
  497. m_r.m[0] = cos (rad);
  498. m_r.m[4] = -sin (rad);
  499. m_r.m[1] = sin (rad);
  500. m_r.m[5] = cos (rad);
  501. return m_r * m;
  502. }
  503. // scale a matrix by [x, y, z]
  504. mat4 scale (const mat4& m, const vec3& v) {
  505. mat4 a = identity_mat4 ();
  506. a.m[0] = v.v[0];
  507. a.m[5] = v.v[1];
  508. a.m[10] = v.v[2];
  509. return a * m;
  510. }
  511. /*-----------------------VIRTUAL CAMERA MATRIX FUNCTIONS----------------------*/
  512. // returns a view matrix using the opengl lookAt style. COLUMN ORDER.
  513. mat4 look_at (const vec3& cam_pos, vec3 targ_pos, const vec3& up) {
  514. // inverse translation
  515. mat4 p = identity_mat4 ();
  516. p = translate (p, vec3 (-cam_pos.v[0], -cam_pos.v[1], -cam_pos.v[2]));
  517. // distance vector
  518. vec3 d = targ_pos - cam_pos;
  519. // forward vector
  520. vec3 f = normalise (d);
  521. // right vector
  522. vec3 r = normalise (cross (f, up));
  523. // real up vector
  524. vec3 u = normalise (cross (r, f));
  525. mat4 ori = identity_mat4 ();
  526. ori.m[0] = r.v[0];
  527. ori.m[4] = r.v[1];
  528. ori.m[8] = r.v[2];
  529. ori.m[1] = u.v[0];
  530. ori.m[5] = u.v[1];
  531. ori.m[9] = u.v[2];
  532. ori.m[2] = -f.v[0];
  533. ori.m[6] = -f.v[1];
  534. ori.m[10] = -f.v[2];
  535. return ori * p;//p * ori;
  536. }
  537. // returns a perspective function mimicking the opengl projection style.
  538. mat4 perspective (float fovy, float aspect, float near, float far) {
  539. float fov_rad = fovy * ONE_DEG_IN_RAD;
  540. float range = tan (fov_rad / 2.0f) * near;
  541. float sx = (2.0f * near) / (range * aspect + range * aspect);
  542. float sy = near / range;
  543. float sz = -(far + near) / (far - near);
  544. float pz = -(2.0f * far * near) / (far - near);
  545. mat4 m = zero_mat4 (); // make sure bottom-right corner is zero
  546. m.m[0] = sx;
  547. m.m[5] = sy;
  548. m.m[10] = sz;
  549. m.m[14] = pz;
  550. m.m[11] = -1.0f;
  551. return m;
  552. }
  553. /*----------------------------HAMILTON IN DA HOUSE!---------------------------*/
  554. versor::versor () { }
  555. versor versor::operator/ (float rhs) {
  556. versor result;
  557. result.q[0] = q[0] / rhs;
  558. result.q[1] = q[1] / rhs;
  559. result.q[2] = q[2] / rhs;
  560. result.q[3] = q[3] / rhs;
  561. return result;
  562. }
  563. versor versor::operator* (float rhs) {
  564. versor result;
  565. result.q[0] = q[0] * rhs;
  566. result.q[1] = q[1] * rhs;
  567. result.q[2] = q[2] * rhs;
  568. result.q[3] = q[3] * rhs;
  569. return result;
  570. }
  571. void print (const versor& q) {
  572. printf ("[%.2f ,%.2f, %.2f, %.2f]\n", q.q[0], q.q[1], q.q[2], q.q[3]);
  573. }
  574. versor versor::operator* (const versor& rhs) {
  575. versor result;
  576. result.q[0] = rhs.q[0] * q[0] - rhs.q[1] * q[1] -
  577. rhs.q[2] * q[2] - rhs.q[3] * q[3];
  578. result.q[1] = rhs.q[0] * q[1] + rhs.q[1] * q[0] -
  579. rhs.q[2] * q[3] + rhs.q[3] * q[2];
  580. result.q[2] = rhs.q[0] * q[2] + rhs.q[1] * q[3] +
  581. rhs.q[2] * q[0] - rhs.q[3] * q[1];
  582. result.q[3] = rhs.q[0] * q[3] - rhs.q[1] * q[2] +
  583. rhs.q[2] * q[1] + rhs.q[3] * q[0];
  584. // re-normalise in case of mangling
  585. return normalise (result);
  586. }
  587. versor versor::operator+ (const versor& rhs) {
  588. versor result;
  589. result.q[0] = rhs.q[0] + q[0];
  590. result.q[1] = rhs.q[1] + q[1];
  591. result.q[2] = rhs.q[2] + q[2];
  592. result.q[3] = rhs.q[3] + q[3];
  593. // re-normalise in case of mangling
  594. return normalise (result);
  595. }
  596. versor quat_from_axis_rad (float radians, float x, float y, float z) {
  597. versor result;
  598. result.q[0] = cos (radians / 2.0);
  599. result.q[1] = sin (radians / 2.0) * x;
  600. result.q[2] = sin (radians / 2.0) * y;
  601. result.q[3] = sin (radians / 2.0) * z;
  602. return result;
  603. }
  604. versor quat_from_axis_deg (float degrees, float x, float y, float z) {
  605. return quat_from_axis_rad (ONE_DEG_IN_RAD * degrees, x, y, z);
  606. }
  607. mat4 quat_to_mat4 (const versor& q) {
  608. float w = q.q[0];
  609. float x = q.q[1];
  610. float y = q.q[2];
  611. float z = q.q[3];
  612. return mat4 (
  613. 1.0f - 2.0f * y * y - 2.0f * z * z,
  614. 2.0f * x * y + 2.0f * w * z,
  615. 2.0f * x * z - 2.0f * w * y,
  616. 0.0f,
  617. 2.0f * x * y - 2.0f * w * z,
  618. 1.0f - 2.0f * x * x - 2.0f * z * z,
  619. 2.0f * y * z + 2.0f * w * x,
  620. 0.0f,
  621. 2.0f * x * z + 2.0f * w * y,
  622. 2.0f * y * z - 2.0f * w * x,
  623. 1.0f - 2.0f * x * x - 2.0f * y * y,
  624. 0.0f,
  625. 0.0f,
  626. 0.0f,
  627. 0.0f,
  628. 1.0f
  629. );
  630. }
  631. versor normalise (versor& q) {
  632. // norm(q) = q / magnitude (q)
  633. // magnitude (q) = sqrt (w*w + x*x...)
  634. // only compute sqrt if interior sum != 1.0
  635. float sum =
  636. q.q[0] * q.q[0] + q.q[1] * q.q[1] +
  637. q.q[2] * q.q[2] + q.q[3] * q.q[3];
  638. // NB: floats have min 6 digits of precision
  639. const float thresh = 0.0001f;
  640. if (fabs (1.0f - sum) < thresh) {
  641. return q;
  642. }
  643. float mag = sqrt (sum);
  644. return q / mag;
  645. }
  646. float dot (const versor& q, const versor& r) {
  647. return q.q[0] * r.q[0] + q.q[1] * r.q[1] + q.q[2] * r.q[2] + q.q[3] * r.q[3];
  648. }
  649. versor slerp (versor& q, versor& r, float t) {
  650. // angle between q0-q1
  651. float cos_half_theta = dot (q, r);
  652. // as found here http://stackoverflow.com/questions/2886606/flipping-issue-when-interpolating-rotations-using-quaternions
  653. // if dot product is negative then one quaternion should be negated, to make
  654. // it take the short way around, rather than the long way
  655. // yeah! and furthermore Susan, I had to recalculate the d.p. after this
  656. if (cos_half_theta < 0.0f) {
  657. for (int i = 0; i < 4; i++) {
  658. q.q[i] *= -1.0f;
  659. }
  660. cos_half_theta = dot (q, r);
  661. }
  662. // if qa=qb or qa=-qb then theta = 0 and we can return qa
  663. if (fabs (cos_half_theta) >= 1.0f) {
  664. return q;
  665. }
  666. // Calculate temporary values
  667. float sin_half_theta = sqrt (1.0f - cos_half_theta * cos_half_theta);
  668. // if theta = 180 degrees then result is not fully defined
  669. // we could rotate around any axis normal to qa or qb
  670. versor result;
  671. if (fabs (sin_half_theta) < 0.001f) {
  672. for (int i = 0; i < 4; i++) {
  673. result.q[i] = (1.0f - t) * q.q[i] + t * r.q[i];
  674. }
  675. return result;
  676. }
  677. float half_theta = acos (cos_half_theta);
  678. float a = sin ((1.0f - t) * half_theta) / sin_half_theta;
  679. float b = sin (t * half_theta) / sin_half_theta;
  680. for (int i = 0; i < 4; i++) {
  681. result.q[i] = q.q[i] * a + r.q[i] * b;
  682. }
  683. return result;
  684. }