Mat4.cpp 23 KB

123456789101112131415161718192021222324252627282930313233343536373839404142434445464748495051525354555657585960616263646566676869707172737475767778798081828384858687888990919293949596979899100101102103104105106107108109110111112113114115116117118119120121122123124125126127128129130131132133134135136137138139140141142143144145146147148149150151152153154155156157158159160161162163164165166167168169170171172173174175176177178179180181182183184185186187188189190191192193194195196197198199200201202203204205206207208209210211212213214215216217218219220221222223224225226227228229230231232233234235236237238239240241242243244245246247248249250251252253254255256257258259260261262263264265266267268269270271272273274275276277278279280281282283284285286287288289290291292293294295296297298299300301302303304305306307308309310311312313314315316317318319320321322323324325326327328329330331332333334335336337338339340341342343344345346347348349350351352353354355356357358359360361362363364365366367368369370371372373374375376377378379380381382383384385386387388389390391392393394395396397398399400401402403404405406407408409410411412413414415416417418419420421422423424425426427428429430431432433434435436437438439440441442443444445446447448449450451452453454455456457458459460461462463464465466467468469470471472473474475476477478479480481482483484485486487488489490491492493494495496497498499500501502503504505506507508509510511512513514515516517518519520521522523524525526527528529530531532533534535536537538539540541542543544545546547548549550551552553554555556557558559560561562563564565566567568569570571572573574575576577578579580581582583584585586587588589590591592593594595596597598599600601602603604605606607608609610611612613614615616617618619620621622623624625626627628629630631632633634635636637638639640641642643644645646647648649650651652653654655656657658659660661662663664665666667668669670671672673674675676677678679680681682683684685686687688689690691692693694695696697698699700701702703704705706707708709710711712713714715716717718719720721722723724725726727728729730731732733734735736737738739740741742743744745746747748749750751752753754755756757758759760761762763764765766767768769770771772773774775776777778779780781782783784785786787788789790791792793794795796797798799800801802803804805806807808809810811812813814815816817818819820821822823824825826827828829830831832833834835836837838839840841842843844845846847848849850851852853854855856857858859860861862863864865866867868869870871872873874875876877878879880881882883884885886887888889890891892893894895896897898899900901902903904905906907908909910911912913914915916917918919920921922923924925926927928929930931932933934935936937938939940941942943944945946947948949950951952953954955956957958959960961962963964965966967968969970971972973974975976977978979980981982983
  1. /**
  2. Copyright 2013 BlackBerry Inc.
  3. Copyright (c) 2017-2018 Xiamen Yaji Software Co., Ltd.
  4. Licensed under the Apache License, Version 2.0 (the "License");
  5. you may not use this file except in compliance with the License.
  6. You may obtain a copy of the License at
  7. http://www.apache.org/licenses/LICENSE-2.0
  8. Unless required by applicable law or agreed to in writing, software
  9. distributed under the License is distributed on an "AS IS" BASIS,
  10. WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
  11. See the License for the specific language governing permissions and
  12. limitations under the License.
  13. Original file from GamePlay3D: http://gameplay3d.org
  14. This file was modified to fit the cocos2d-x project
  15. */
  16. #include "math/Mat4.h"
  17. #include <cmath>
  18. #include "math/Quaternion.h"
  19. #include "math/MathUtil.h"
  20. #include "base/ccMacros.h"
  21. NS_CC_MATH_BEGIN
  22. Mat4::Mat4()
  23. {
  24. *this = IDENTITY;
  25. }
  26. Mat4::Mat4(float m11, float m12, float m13, float m14, float m21, float m22, float m23, float m24,
  27. float m31, float m32, float m33, float m34, float m41, float m42, float m43, float m44)
  28. {
  29. set(m11, m12, m13, m14, m21, m22, m23, m24, m31, m32, m33, m34, m41, m42, m43, m44);
  30. }
  31. Mat4::Mat4(const float* mat)
  32. {
  33. set(mat);
  34. }
  35. Mat4::Mat4(const Mat4& copy)
  36. {
  37. memcpy(m, copy.m, MATRIX_SIZE);
  38. }
  39. Mat4::~Mat4()
  40. {
  41. }
  42. void Mat4::createLookAt(const Vec3& eyePosition, const Vec3& targetPosition, const Vec3& up, Mat4* dst)
  43. {
  44. createLookAt(eyePosition.x, eyePosition.y, eyePosition.z, targetPosition.x, targetPosition.y, targetPosition.z,
  45. up.x, up.y, up.z, dst);
  46. }
  47. void Mat4::createLookAt(float eyePositionX, float eyePositionY, float eyePositionZ,
  48. float targetPositionX, float targetPositionY, float targetPositionZ,
  49. float upX, float upY, float upZ, Mat4* dst)
  50. {
  51. GP_ASSERT(dst);
  52. Vec3 eye(eyePositionX, eyePositionY, eyePositionZ);
  53. Vec3 target(targetPositionX, targetPositionY, targetPositionZ);
  54. Vec3 up(upX, upY, upZ);
  55. up.normalize();
  56. Vec3 zaxis;
  57. Vec3::subtract(eye, target, &zaxis);
  58. zaxis.normalize();
  59. Vec3 xaxis;
  60. Vec3::cross(up, zaxis, &xaxis);
  61. xaxis.normalize();
  62. Vec3 yaxis;
  63. Vec3::cross(zaxis, xaxis, &yaxis);
  64. yaxis.normalize();
  65. dst->m[0] = xaxis.x;
  66. dst->m[1] = yaxis.x;
  67. dst->m[2] = zaxis.x;
  68. dst->m[3] = 0.0f;
  69. dst->m[4] = xaxis.y;
  70. dst->m[5] = yaxis.y;
  71. dst->m[6] = zaxis.y;
  72. dst->m[7] = 0.0f;
  73. dst->m[8] = xaxis.z;
  74. dst->m[9] = yaxis.z;
  75. dst->m[10] = zaxis.z;
  76. dst->m[11] = 0.0f;
  77. dst->m[12] = -Vec3::dot(xaxis, eye);
  78. dst->m[13] = -Vec3::dot(yaxis, eye);
  79. dst->m[14] = -Vec3::dot(zaxis, eye);
  80. dst->m[15] = 1.0f;
  81. }
  82. void Mat4::createPerspective(float fieldOfView, float aspectRatio,
  83. float zNearPlane, float zFarPlane, Mat4* dst)
  84. {
  85. GP_ASSERT(dst);
  86. GP_ASSERT(zFarPlane != zNearPlane);
  87. float f_n = 1.0f / (zFarPlane - zNearPlane);
  88. float theta = MATH_DEG_TO_RAD(fieldOfView) * 0.5f;
  89. if (std::abs(std::fmod(theta, MATH_PIOVER2)) < MATH_EPSILON)
  90. {
  91. CCLOGERROR("Invalid field of view value (%f) causes attempted calculation tan(%f), which is undefined.", fieldOfView, theta);
  92. return;
  93. }
  94. float divisor = std::tan(theta);
  95. GP_ASSERT(divisor);
  96. float factor = 1.0f / divisor;
  97. memset(dst, 0, MATRIX_SIZE);
  98. GP_ASSERT(aspectRatio);
  99. dst->m[0] = (1.0f / aspectRatio) * factor;
  100. dst->m[5] = factor;
  101. dst->m[10] = (-(zFarPlane + zNearPlane)) * f_n;
  102. dst->m[11] = -1.0f;
  103. dst->m[14] = -2.0f * zFarPlane * zNearPlane * f_n;
  104. }
  105. void Mat4::createOrthographic(float width, float height, float zNearPlane, float zFarPlane, Mat4* dst)
  106. {
  107. float halfWidth = width / 2.0f;
  108. float halfHeight = height / 2.0f;
  109. createOrthographicOffCenter(-halfWidth, halfWidth, -halfHeight, halfHeight, zNearPlane, zFarPlane, dst);
  110. }
  111. void Mat4::createOrthographicOffCenter(float left, float right, float bottom, float top,
  112. float zNearPlane, float zFarPlane, Mat4* dst)
  113. {
  114. GP_ASSERT(dst);
  115. GP_ASSERT(right != left);
  116. GP_ASSERT(top != bottom);
  117. GP_ASSERT(zFarPlane != zNearPlane);
  118. memset(dst, 0, MATRIX_SIZE);
  119. dst->m[0] = 2 / (right - left);
  120. dst->m[5] = 2 / (top - bottom);
  121. dst->m[10] = 2 / (zNearPlane - zFarPlane);
  122. dst->m[12] = (left + right) / (left - right);
  123. dst->m[13] = (top + bottom) / (bottom - top);
  124. dst->m[14] = (zNearPlane + zFarPlane) / (zNearPlane - zFarPlane);
  125. dst->m[15] = 1;
  126. }
  127. void Mat4::createBillboard(const Vec3& objectPosition, const Vec3& cameraPosition,
  128. const Vec3& cameraUpVector, Mat4* dst)
  129. {
  130. createBillboardHelper(objectPosition, cameraPosition, cameraUpVector, nullptr, dst);
  131. }
  132. void Mat4::createBillboard(const Vec3& objectPosition, const Vec3& cameraPosition,
  133. const Vec3& cameraUpVector, const Vec3& cameraForwardVector,
  134. Mat4* dst)
  135. {
  136. createBillboardHelper(objectPosition, cameraPosition, cameraUpVector, &cameraForwardVector, dst);
  137. }
  138. void Mat4::createBillboardHelper(const Vec3& objectPosition, const Vec3& cameraPosition,
  139. const Vec3& cameraUpVector, const Vec3* cameraForwardVector,
  140. Mat4* dst)
  141. {
  142. Vec3 delta(objectPosition, cameraPosition);
  143. bool isSufficientDelta = delta.lengthSquared() > MATH_EPSILON;
  144. dst->setIdentity();
  145. dst->m[3] = objectPosition.x;
  146. dst->m[7] = objectPosition.y;
  147. dst->m[11] = objectPosition.z;
  148. // As per the contracts for the 2 variants of createBillboard, we need
  149. // either a safe default or a sufficient distance between object and camera.
  150. if (cameraForwardVector || isSufficientDelta)
  151. {
  152. Vec3 target = isSufficientDelta ? cameraPosition : (objectPosition - *cameraForwardVector);
  153. // A billboard is the inverse of a lookAt rotation
  154. Mat4 lookAt;
  155. createLookAt(objectPosition, target, cameraUpVector, &lookAt);
  156. dst->m[0] = lookAt.m[0];
  157. dst->m[1] = lookAt.m[4];
  158. dst->m[2] = lookAt.m[8];
  159. dst->m[4] = lookAt.m[1];
  160. dst->m[5] = lookAt.m[5];
  161. dst->m[6] = lookAt.m[9];
  162. dst->m[8] = lookAt.m[2];
  163. dst->m[9] = lookAt.m[6];
  164. dst->m[10] = lookAt.m[10];
  165. }
  166. }
  167. // void Mat4::createReflection(const Plane& plane, Mat4* dst)
  168. // {
  169. // Vec3 normal(plane.getNormal());
  170. // float k = -2.0f * plane.getDistance();
  171. // dst->setIdentity();
  172. // dst->m[0] -= 2.0f * normal.x * normal.x;
  173. // dst->m[5] -= 2.0f * normal.y * normal.y;
  174. // dst->m[10] -= 2.0f * normal.z * normal.z;
  175. // dst->m[1] = dst->m[4] = -2.0f * normal.x * normal.y;
  176. // dst->m[2] = dst->m[8] = -2.0f * normal.x * normal.z;
  177. // dst->m[6] = dst->m[9] = -2.0f * normal.y * normal.z;
  178. // dst->m[3] = k * normal.x;
  179. // dst->m[7] = k * normal.y;
  180. // dst->m[11] = k * normal.z;
  181. // }
  182. void Mat4::createScale(const Vec3& scale, Mat4* dst)
  183. {
  184. GP_ASSERT(dst);
  185. memcpy(dst, &IDENTITY, MATRIX_SIZE);
  186. dst->m[0] = scale.x;
  187. dst->m[5] = scale.y;
  188. dst->m[10] = scale.z;
  189. }
  190. void Mat4::createScale(float xScale, float yScale, float zScale, Mat4* dst)
  191. {
  192. GP_ASSERT(dst);
  193. memcpy(dst, &IDENTITY, MATRIX_SIZE);
  194. dst->m[0] = xScale;
  195. dst->m[5] = yScale;
  196. dst->m[10] = zScale;
  197. }
  198. void Mat4::createRotation(const Quaternion& q, Mat4* dst)
  199. {
  200. GP_ASSERT(dst);
  201. float x2 = q.x + q.x;
  202. float y2 = q.y + q.y;
  203. float z2 = q.z + q.z;
  204. float xx2 = q.x * x2;
  205. float yy2 = q.y * y2;
  206. float zz2 = q.z * z2;
  207. float xy2 = q.x * y2;
  208. float xz2 = q.x * z2;
  209. float yz2 = q.y * z2;
  210. float wx2 = q.w * x2;
  211. float wy2 = q.w * y2;
  212. float wz2 = q.w * z2;
  213. dst->m[0] = 1.0f - yy2 - zz2;
  214. dst->m[1] = xy2 + wz2;
  215. dst->m[2] = xz2 - wy2;
  216. dst->m[3] = 0.0f;
  217. dst->m[4] = xy2 - wz2;
  218. dst->m[5] = 1.0f - xx2 - zz2;
  219. dst->m[6] = yz2 + wx2;
  220. dst->m[7] = 0.0f;
  221. dst->m[8] = xz2 + wy2;
  222. dst->m[9] = yz2 - wx2;
  223. dst->m[10] = 1.0f - xx2 - yy2;
  224. dst->m[11] = 0.0f;
  225. dst->m[12] = 0.0f;
  226. dst->m[13] = 0.0f;
  227. dst->m[14] = 0.0f;
  228. dst->m[15] = 1.0f;
  229. }
  230. void Mat4::createRotation(const Vec3& axis, float angle, Mat4* dst)
  231. {
  232. GP_ASSERT(dst);
  233. float x = axis.x;
  234. float y = axis.y;
  235. float z = axis.z;
  236. // Make sure the input axis is normalized.
  237. float n = x*x + y*y + z*z;
  238. if (n != 1.0f)
  239. {
  240. // Not normalized.
  241. n = std::sqrt(n);
  242. // Prevent divide too close to zero.
  243. if (n > 0.000001f)
  244. {
  245. n = 1.0f / n;
  246. x *= n;
  247. y *= n;
  248. z *= n;
  249. }
  250. }
  251. float c = std::cos(angle);
  252. float s = std::sin(angle);
  253. float t = 1.0f - c;
  254. float tx = t * x;
  255. float ty = t * y;
  256. float tz = t * z;
  257. float txy = tx * y;
  258. float txz = tx * z;
  259. float tyz = ty * z;
  260. float sx = s * x;
  261. float sy = s * y;
  262. float sz = s * z;
  263. dst->m[0] = c + tx*x;
  264. dst->m[1] = txy + sz;
  265. dst->m[2] = txz - sy;
  266. dst->m[3] = 0.0f;
  267. dst->m[4] = txy - sz;
  268. dst->m[5] = c + ty*y;
  269. dst->m[6] = tyz + sx;
  270. dst->m[7] = 0.0f;
  271. dst->m[8] = txz + sy;
  272. dst->m[9] = tyz - sx;
  273. dst->m[10] = c + tz*z;
  274. dst->m[11] = 0.0f;
  275. dst->m[12] = 0.0f;
  276. dst->m[13] = 0.0f;
  277. dst->m[14] = 0.0f;
  278. dst->m[15] = 1.0f;
  279. }
  280. void Mat4::createRotationX(float angle, Mat4* dst)
  281. {
  282. GP_ASSERT(dst);
  283. memcpy(dst, &IDENTITY, MATRIX_SIZE);
  284. float c = std::cos(angle);
  285. float s = std::sin(angle);
  286. dst->m[5] = c;
  287. dst->m[6] = s;
  288. dst->m[9] = -s;
  289. dst->m[10] = c;
  290. }
  291. void Mat4::createRotationY(float angle, Mat4* dst)
  292. {
  293. GP_ASSERT(dst);
  294. memcpy(dst, &IDENTITY, MATRIX_SIZE);
  295. float c = std::cos(angle);
  296. float s = std::sin(angle);
  297. dst->m[0] = c;
  298. dst->m[2] = -s;
  299. dst->m[8] = s;
  300. dst->m[10] = c;
  301. }
  302. void Mat4::createRotationZ(float angle, Mat4* dst)
  303. {
  304. GP_ASSERT(dst);
  305. memcpy(dst, &IDENTITY, MATRIX_SIZE);
  306. float c = std::cos(angle);
  307. float s = std::sin(angle);
  308. dst->m[0] = c;
  309. dst->m[1] = s;
  310. dst->m[4] = -s;
  311. dst->m[5] = c;
  312. }
  313. void Mat4::createTranslation(const Vec3& translation, Mat4* dst)
  314. {
  315. GP_ASSERT(dst);
  316. memcpy(dst, &IDENTITY, MATRIX_SIZE);
  317. dst->m[12] = translation.x;
  318. dst->m[13] = translation.y;
  319. dst->m[14] = translation.z;
  320. }
  321. void Mat4::createTranslation(float xTranslation, float yTranslation, float zTranslation, Mat4* dst)
  322. {
  323. GP_ASSERT(dst);
  324. memcpy(dst, &IDENTITY, MATRIX_SIZE);
  325. dst->m[12] = xTranslation;
  326. dst->m[13] = yTranslation;
  327. dst->m[14] = zTranslation;
  328. }
  329. void Mat4::add(float scalar)
  330. {
  331. add(scalar, this);
  332. }
  333. void Mat4::add(float scalar, Mat4* dst)
  334. {
  335. GP_ASSERT(dst);
  336. #ifdef __SSE__
  337. MathUtil::addMatrix(col, scalar, dst->col);
  338. #else
  339. MathUtil::addMatrix(m, scalar, dst->m);
  340. #endif
  341. }
  342. void Mat4::add(const Mat4& mat)
  343. {
  344. add(*this, mat, this);
  345. }
  346. void Mat4::add(const Mat4& m1, const Mat4& m2, Mat4* dst)
  347. {
  348. GP_ASSERT(dst);
  349. #ifdef __SSE__
  350. MathUtil::addMatrix(m1.col, m2.col, dst->col);
  351. #else
  352. MathUtil::addMatrix(m1.m, m2.m, dst->m);
  353. #endif
  354. }
  355. bool Mat4::decompose(Vec3* scale, Quaternion* rotation, Vec3* translation) const
  356. {
  357. if (translation)
  358. {
  359. // Extract the translation.
  360. translation->x = m[12];
  361. translation->y = m[13];
  362. translation->z = m[14];
  363. }
  364. // Nothing left to do.
  365. if (scale == nullptr && rotation == nullptr)
  366. return true;
  367. // Extract the scale.
  368. // This is simply the length of each axis (row/column) in the matrix.
  369. Vec3 xaxis(m[0], m[1], m[2]);
  370. float scaleX = xaxis.length();
  371. Vec3 yaxis(m[4], m[5], m[6]);
  372. float scaleY = yaxis.length();
  373. Vec3 zaxis(m[8], m[9], m[10]);
  374. float scaleZ = zaxis.length();
  375. // Determine if we have a negative scale (true if determinant is less than zero).
  376. // In this case, we simply negate a single axis of the scale.
  377. float det = determinant();
  378. if (det < 0)
  379. scaleZ = -scaleZ;
  380. if (scale)
  381. {
  382. scale->x = scaleX;
  383. scale->y = scaleY;
  384. scale->z = scaleZ;
  385. }
  386. // Nothing left to do.
  387. if (rotation == nullptr)
  388. return true;
  389. // Scale too close to zero, can't decompose rotation.
  390. if (scaleX < MATH_TOLERANCE || scaleY < MATH_TOLERANCE || std::abs(scaleZ) < MATH_TOLERANCE)
  391. return false;
  392. float rn;
  393. // Factor the scale out of the matrix axes.
  394. rn = 1.0f / scaleX;
  395. xaxis.x *= rn;
  396. xaxis.y *= rn;
  397. xaxis.z *= rn;
  398. rn = 1.0f / scaleY;
  399. yaxis.x *= rn;
  400. yaxis.y *= rn;
  401. yaxis.z *= rn;
  402. rn = 1.0f / scaleZ;
  403. zaxis.x *= rn;
  404. zaxis.y *= rn;
  405. zaxis.z *= rn;
  406. // Now calculate the rotation from the resulting matrix (axes).
  407. float trace = xaxis.x + yaxis.y + zaxis.z + 1.0f;
  408. if (trace > MATH_EPSILON)
  409. {
  410. float s = 0.5f / std::sqrt(trace);
  411. rotation->w = 0.25f / s;
  412. rotation->x = (yaxis.z - zaxis.y) * s;
  413. rotation->y = (zaxis.x - xaxis.z) * s;
  414. rotation->z = (xaxis.y - yaxis.x) * s;
  415. }
  416. else
  417. {
  418. // Note: since xaxis, yaxis, and zaxis are normalized,
  419. // we will never divide by zero in the code below.
  420. if (xaxis.x > yaxis.y && xaxis.x > zaxis.z)
  421. {
  422. float s = 0.5f / std::sqrt(1.0f + xaxis.x - yaxis.y - zaxis.z);
  423. rotation->w = (yaxis.z - zaxis.y) * s;
  424. rotation->x = 0.25f / s;
  425. rotation->y = (yaxis.x + xaxis.y) * s;
  426. rotation->z = (zaxis.x + xaxis.z) * s;
  427. }
  428. else if (yaxis.y > zaxis.z)
  429. {
  430. float s = 0.5f / std::sqrt(1.0f + yaxis.y - xaxis.x - zaxis.z);
  431. rotation->w = (zaxis.x - xaxis.z) * s;
  432. rotation->x = (yaxis.x + xaxis.y) * s;
  433. rotation->y = 0.25f / s;
  434. rotation->z = (zaxis.y + yaxis.z) * s;
  435. }
  436. else
  437. {
  438. float s = 0.5f / std::sqrt(1.0f + zaxis.z - xaxis.x - yaxis.y);
  439. rotation->w = (xaxis.y - yaxis.x ) * s;
  440. rotation->x = (zaxis.x + xaxis.z ) * s;
  441. rotation->y = (zaxis.y + yaxis.z ) * s;
  442. rotation->z = 0.25f / s;
  443. }
  444. }
  445. return true;
  446. }
  447. float Mat4::determinant() const
  448. {
  449. float a0 = m[0] * m[5] - m[1] * m[4];
  450. float a1 = m[0] * m[6] - m[2] * m[4];
  451. float a2 = m[0] * m[7] - m[3] * m[4];
  452. float a3 = m[1] * m[6] - m[2] * m[5];
  453. float a4 = m[1] * m[7] - m[3] * m[5];
  454. float a5 = m[2] * m[7] - m[3] * m[6];
  455. float b0 = m[8] * m[13] - m[9] * m[12];
  456. float b1 = m[8] * m[14] - m[10] * m[12];
  457. float b2 = m[8] * m[15] - m[11] * m[12];
  458. float b3 = m[9] * m[14] - m[10] * m[13];
  459. float b4 = m[9] * m[15] - m[11] * m[13];
  460. float b5 = m[10] * m[15] - m[11] * m[14];
  461. // Calculate the determinant.
  462. return (a0 * b5 - a1 * b4 + a2 * b3 + a3 * b2 - a4 * b1 + a5 * b0);
  463. }
  464. void Mat4::getScale(Vec3* scale) const
  465. {
  466. decompose(scale, nullptr, nullptr);
  467. }
  468. bool Mat4::getRotation(Quaternion* rotation) const
  469. {
  470. return decompose(nullptr, rotation, nullptr);
  471. }
  472. void Mat4::getTranslation(Vec3* translation) const
  473. {
  474. decompose(nullptr, nullptr, translation);
  475. }
  476. void Mat4::getUpVector(Vec3* dst) const
  477. {
  478. GP_ASSERT(dst);
  479. dst->x = m[4];
  480. dst->y = m[5];
  481. dst->z = m[6];
  482. }
  483. void Mat4::getDownVector(Vec3* dst) const
  484. {
  485. GP_ASSERT(dst);
  486. dst->x = -m[4];
  487. dst->y = -m[5];
  488. dst->z = -m[6];
  489. }
  490. void Mat4::getLeftVector(Vec3* dst) const
  491. {
  492. GP_ASSERT(dst);
  493. dst->x = -m[0];
  494. dst->y = -m[1];
  495. dst->z = -m[2];
  496. }
  497. void Mat4::getRightVector(Vec3* dst) const
  498. {
  499. GP_ASSERT(dst);
  500. dst->x = m[0];
  501. dst->y = m[1];
  502. dst->z = m[2];
  503. }
  504. void Mat4::getForwardVector(Vec3* dst) const
  505. {
  506. GP_ASSERT(dst);
  507. dst->x = -m[8];
  508. dst->y = -m[9];
  509. dst->z = -m[10];
  510. }
  511. void Mat4::getBackVector(Vec3* dst) const
  512. {
  513. GP_ASSERT(dst);
  514. dst->x = m[8];
  515. dst->y = m[9];
  516. dst->z = m[10];
  517. }
  518. Mat4 Mat4::getInversed() const
  519. {
  520. Mat4 mat(*this);
  521. mat.inverse();
  522. return mat;
  523. }
  524. bool Mat4::inverse()
  525. {
  526. float a0 = m[0] * m[5] - m[1] * m[4];
  527. float a1 = m[0] * m[6] - m[2] * m[4];
  528. float a2 = m[0] * m[7] - m[3] * m[4];
  529. float a3 = m[1] * m[6] - m[2] * m[5];
  530. float a4 = m[1] * m[7] - m[3] * m[5];
  531. float a5 = m[2] * m[7] - m[3] * m[6];
  532. float b0 = m[8] * m[13] - m[9] * m[12];
  533. float b1 = m[8] * m[14] - m[10] * m[12];
  534. float b2 = m[8] * m[15] - m[11] * m[12];
  535. float b3 = m[9] * m[14] - m[10] * m[13];
  536. float b4 = m[9] * m[15] - m[11] * m[13];
  537. float b5 = m[10] * m[15] - m[11] * m[14];
  538. // Calculate the determinant.
  539. float det = a0 * b5 - a1 * b4 + a2 * b3 + a3 * b2 - a4 * b1 + a5 * b0;
  540. // Close to zero, can't invert.
  541. if (std::abs(det) <= MATH_TOLERANCE)
  542. return false;
  543. // Support the case where m == dst.
  544. Mat4 inverse;
  545. inverse.m[0] = m[5] * b5 - m[6] * b4 + m[7] * b3;
  546. inverse.m[1] = -m[1] * b5 + m[2] * b4 - m[3] * b3;
  547. inverse.m[2] = m[13] * a5 - m[14] * a4 + m[15] * a3;
  548. inverse.m[3] = -m[9] * a5 + m[10] * a4 - m[11] * a3;
  549. inverse.m[4] = -m[4] * b5 + m[6] * b2 - m[7] * b1;
  550. inverse.m[5] = m[0] * b5 - m[2] * b2 + m[3] * b1;
  551. inverse.m[6] = -m[12] * a5 + m[14] * a2 - m[15] * a1;
  552. inverse.m[7] = m[8] * a5 - m[10] * a2 + m[11] * a1;
  553. inverse.m[8] = m[4] * b4 - m[5] * b2 + m[7] * b0;
  554. inverse.m[9] = -m[0] * b4 + m[1] * b2 - m[3] * b0;
  555. inverse.m[10] = m[12] * a4 - m[13] * a2 + m[15] * a0;
  556. inverse.m[11] = -m[8] * a4 + m[9] * a2 - m[11] * a0;
  557. inverse.m[12] = -m[4] * b3 + m[5] * b1 - m[6] * b0;
  558. inverse.m[13] = m[0] * b3 - m[1] * b1 + m[2] * b0;
  559. inverse.m[14] = -m[12] * a3 + m[13] * a1 - m[14] * a0;
  560. inverse.m[15] = m[8] * a3 - m[9] * a1 + m[10] * a0;
  561. multiply(inverse, 1.0f / det, this);
  562. return true;
  563. }
  564. bool Mat4::isIdentity() const
  565. {
  566. return (memcmp(m, &IDENTITY, MATRIX_SIZE) == 0);
  567. }
  568. void Mat4::multiply(float scalar)
  569. {
  570. multiply(scalar, this);
  571. }
  572. void Mat4::multiply(float scalar, Mat4* dst) const
  573. {
  574. multiply(*this, scalar, dst);
  575. }
  576. void Mat4::multiply(const Mat4& m, float scalar, Mat4* dst)
  577. {
  578. GP_ASSERT(dst);
  579. #ifdef __SSE__
  580. MathUtil::multiplyMatrix(m.col, scalar, dst->col);
  581. #else
  582. MathUtil::multiplyMatrix(m.m, scalar, dst->m);
  583. #endif
  584. }
  585. void Mat4::multiply(const Mat4& mat)
  586. {
  587. multiply(*this, mat, this);
  588. }
  589. void Mat4::multiply(const Mat4& m1, const Mat4& m2, Mat4* dst)
  590. {
  591. GP_ASSERT(dst);
  592. #ifdef __SSE__
  593. MathUtil::multiplyMatrix(m1.col, m2.col, dst->col);
  594. #else
  595. MathUtil::multiplyMatrix(m1.m, m2.m, dst->m);
  596. #endif
  597. }
  598. void Mat4::negate()
  599. {
  600. #ifdef __SSE__
  601. MathUtil::negateMatrix(col, col);
  602. #else
  603. MathUtil::negateMatrix(m, m);
  604. #endif
  605. }
  606. Mat4 Mat4::getNegated() const
  607. {
  608. Mat4 mat(*this);
  609. mat.negate();
  610. return mat;
  611. }
  612. void Mat4::rotate(const Quaternion& q)
  613. {
  614. rotate(q, this);
  615. }
  616. void Mat4::rotate(const Quaternion& q, Mat4* dst) const
  617. {
  618. Mat4 r;
  619. createRotation(q, &r);
  620. multiply(*this, r, dst);
  621. }
  622. void Mat4::rotate(const Vec3& axis, float angle)
  623. {
  624. rotate(axis, angle, this);
  625. }
  626. void Mat4::rotate(const Vec3& axis, float angle, Mat4* dst) const
  627. {
  628. Mat4 r;
  629. createRotation(axis, angle, &r);
  630. multiply(*this, r, dst);
  631. }
  632. void Mat4::rotateX(float angle)
  633. {
  634. rotateX(angle, this);
  635. }
  636. void Mat4::rotateX(float angle, Mat4* dst) const
  637. {
  638. Mat4 r;
  639. createRotationX(angle, &r);
  640. multiply(*this, r, dst);
  641. }
  642. void Mat4::rotateY(float angle)
  643. {
  644. rotateY(angle, this);
  645. }
  646. void Mat4::rotateY(float angle, Mat4* dst) const
  647. {
  648. Mat4 r;
  649. createRotationY(angle, &r);
  650. multiply(*this, r, dst);
  651. }
  652. void Mat4::rotateZ(float angle)
  653. {
  654. rotateZ(angle, this);
  655. }
  656. void Mat4::rotateZ(float angle, Mat4* dst) const
  657. {
  658. Mat4 r;
  659. createRotationZ(angle, &r);
  660. multiply(*this, r, dst);
  661. }
  662. void Mat4::scale(float value)
  663. {
  664. scale(value, this);
  665. }
  666. void Mat4::scale(float value, Mat4* dst) const
  667. {
  668. scale(value, value, value, dst);
  669. }
  670. void Mat4::scale(float xScale, float yScale, float zScale)
  671. {
  672. scale(xScale, yScale, zScale, this);
  673. }
  674. void Mat4::scale(float xScale, float yScale, float zScale, Mat4* dst) const
  675. {
  676. Mat4 s;
  677. createScale(xScale, yScale, zScale, &s);
  678. multiply(*this, s, dst);
  679. }
  680. void Mat4::scale(const Vec3& s)
  681. {
  682. scale(s.x, s.y, s.z, this);
  683. }
  684. void Mat4::scale(const Vec3& s, Mat4* dst) const
  685. {
  686. scale(s.x, s.y, s.z, dst);
  687. }
  688. void Mat4::set(float m11, float m12, float m13, float m14, float m21, float m22, float m23, float m24,
  689. float m31, float m32, float m33, float m34, float m41, float m42, float m43, float m44)
  690. {
  691. m[0] = m11;
  692. m[1] = m21;
  693. m[2] = m31;
  694. m[3] = m41;
  695. m[4] = m12;
  696. m[5] = m22;
  697. m[6] = m32;
  698. m[7] = m42;
  699. m[8] = m13;
  700. m[9] = m23;
  701. m[10] = m33;
  702. m[11] = m43;
  703. m[12] = m14;
  704. m[13] = m24;
  705. m[14] = m34;
  706. m[15] = m44;
  707. }
  708. void Mat4::set(const float* mat)
  709. {
  710. GP_ASSERT(mat);
  711. memcpy(this->m, mat, MATRIX_SIZE);
  712. }
  713. void Mat4::set(const Mat4& mat)
  714. {
  715. memcpy(this->m, mat.m, MATRIX_SIZE);
  716. }
  717. void Mat4::setIdentity()
  718. {
  719. memcpy(m, &IDENTITY, MATRIX_SIZE);
  720. }
  721. void Mat4::setZero()
  722. {
  723. memset(m, 0, MATRIX_SIZE);
  724. }
  725. void Mat4::subtract(const Mat4& mat)
  726. {
  727. subtract(*this, mat, this);
  728. }
  729. void Mat4::subtract(const Mat4& m1, const Mat4& m2, Mat4* dst)
  730. {
  731. GP_ASSERT(dst);
  732. #ifdef __SSE__
  733. MathUtil::subtractMatrix(m1.col, m2.col, dst->col);
  734. #else
  735. MathUtil::subtractMatrix(m1.m, m2.m, dst->m);
  736. #endif
  737. }
  738. void Mat4::transformVector(Vec3* vector) const
  739. {
  740. GP_ASSERT(vector);
  741. transformVector(vector->x, vector->y, vector->z, 0.0f, vector);
  742. }
  743. void Mat4::transformVector(const Vec3& vector, Vec3* dst) const
  744. {
  745. transformVector(vector.x, vector.y, vector.z, 0.0f, dst);
  746. }
  747. void Mat4::transformVector(float x, float y, float z, float w, Vec3* dst) const
  748. {
  749. GP_ASSERT(dst);
  750. MathUtil::transformVec4(m, x, y, z, w, (float*)dst);
  751. }
  752. void Mat4::transformVector(Vec4* vector) const
  753. {
  754. GP_ASSERT(vector);
  755. transformVector(*vector, vector);
  756. }
  757. void Mat4::transformVector(const Vec4& vector, Vec4* dst) const
  758. {
  759. GP_ASSERT(dst);
  760. #ifdef __SSE__
  761. MathUtil::transformVec4(col, vector.v, dst->v);
  762. #else
  763. MathUtil::transformVec4(m, (const float*) &vector, (float*)dst);
  764. #endif
  765. }
  766. void Mat4::translate(float x, float y, float z)
  767. {
  768. translate(x, y, z, this);
  769. }
  770. void Mat4::translate(float x, float y, float z, Mat4* dst) const
  771. {
  772. Mat4 t;
  773. createTranslation(x, y, z, &t);
  774. multiply(*this, t, dst);
  775. }
  776. void Mat4::translate(const Vec3& t)
  777. {
  778. translate(t.x, t.y, t.z, this);
  779. }
  780. void Mat4::translate(const Vec3& t, Mat4* dst) const
  781. {
  782. translate(t.x, t.y, t.z, dst);
  783. }
  784. void Mat4::transpose()
  785. {
  786. #ifdef __SSE__
  787. MathUtil::transposeMatrix(col, col);
  788. #else
  789. MathUtil::transposeMatrix(m, m);
  790. #endif
  791. }
  792. Mat4 Mat4::getTransposed() const
  793. {
  794. Mat4 mat(*this);
  795. mat.transpose();
  796. return mat;
  797. }
  798. const Mat4 Mat4::IDENTITY = Mat4(
  799. 1.0f, 0.0f, 0.0f, 0.0f,
  800. 0.0f, 1.0f, 0.0f, 0.0f,
  801. 0.0f, 0.0f, 1.0f, 0.0f,
  802. 0.0f, 0.0f, 0.0f, 1.0f);
  803. const Mat4 Mat4::ZERO = Mat4(
  804. 0, 0, 0, 0,
  805. 0, 0, 0, 0,
  806. 0, 0, 0, 0,
  807. 0, 0, 0, 0 );
  808. NS_CC_MATH_END