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