413 lines
11 KiB
C++
413 lines
11 KiB
C++
// This is an incomplete port of http://glmatrix.net/
|
|
//
|
|
// Copyright (c) 2013 Brandon Jones, Colin MacKenzie IV
|
|
//
|
|
// This software is provided 'as-is', without any express or implied warranty.
|
|
// In no event will the authors be held liable for any damages arising from the
|
|
// use of this software.
|
|
//
|
|
// Permission is granted to anyone to use this software for any purpose,
|
|
// including commercial applications, and to alter it and redistribute it
|
|
// freely, subject to the following restrictions:
|
|
//
|
|
// 1. The origin of this software must not be misrepresented; you must not claim
|
|
// that you wrote the original software. If you use this software in a
|
|
// product, an acknowledgment in the product documentation would be
|
|
// appreciated but is not required.
|
|
//
|
|
// 2. Altered source versions must be plainly marked as such, and must not be
|
|
// misrepresented as being the original software.
|
|
//
|
|
// 3. This notice may not be removed or altered from any source distribution.
|
|
|
|
#include <mbgl/util/mat4.hpp>
|
|
|
|
#include <cmath>
|
|
|
|
namespace mbgl {
|
|
|
|
namespace matrix {
|
|
|
|
void identity(mat4& out) {
|
|
out[0] = 1.0f;
|
|
out[1] = 0.0f;
|
|
out[2] = 0.0f;
|
|
out[3] = 0.0f;
|
|
out[4] = 0.0f;
|
|
out[5] = 1.0f;
|
|
out[6] = 0.0f;
|
|
out[7] = 0.0f;
|
|
out[8] = 0.0f;
|
|
out[9] = 0.0f;
|
|
out[10] = 1.0f;
|
|
out[11] = 0.0f;
|
|
out[12] = 0.0f;
|
|
out[13] = 0.0f;
|
|
out[14] = 0.0f;
|
|
out[15] = 1.0f;
|
|
}
|
|
|
|
bool invert(mat4& out, const mat4& a) {
|
|
double a00 = a[0];
|
|
double a01 = a[1];
|
|
double a02 = a[2];
|
|
double a03 = a[3];
|
|
double a10 = a[4];
|
|
double a11 = a[5];
|
|
double a12 = a[6];
|
|
double a13 = a[7];
|
|
double a20 = a[8];
|
|
double a21 = a[9];
|
|
double a22 = a[10];
|
|
double a23 = a[11];
|
|
double a30 = a[12];
|
|
double a31 = a[13];
|
|
double a32 = a[14];
|
|
double a33 = a[15];
|
|
double b00 = a00 * a11 - a01 * a10;
|
|
double b01 = a00 * a12 - a02 * a10;
|
|
double b02 = a00 * a13 - a03 * a10;
|
|
double b03 = a01 * a12 - a02 * a11;
|
|
double b04 = a01 * a13 - a03 * a11;
|
|
double b05 = a02 * a13 - a03 * a12;
|
|
double b06 = a20 * a31 - a21 * a30;
|
|
double b07 = a20 * a32 - a22 * a30;
|
|
double b08 = a20 * a33 - a23 * a30;
|
|
double b09 = a21 * a32 - a22 * a31;
|
|
double b10 = a21 * a33 - a23 * a31;
|
|
double b11 = a22 * a33 - a23 * a32;
|
|
double // Calculate the determinant
|
|
det = b00 * b11 - b01 * b10 + b02 * b09 + b03 * b08 - b04 * b07 + b05 * b06;
|
|
|
|
if (!det) {
|
|
return true;
|
|
}
|
|
det = 1.0 / det;
|
|
|
|
out[0] = (a11 * b11 - a12 * b10 + a13 * b09) * det;
|
|
out[1] = (a02 * b10 - a01 * b11 - a03 * b09) * det;
|
|
out[2] = (a31 * b05 - a32 * b04 + a33 * b03) * det;
|
|
out[3] = (a22 * b04 - a21 * b05 - a23 * b03) * det;
|
|
out[4] = (a12 * b08 - a10 * b11 - a13 * b07) * det;
|
|
out[5] = (a00 * b11 - a02 * b08 + a03 * b07) * det;
|
|
out[6] = (a32 * b02 - a30 * b05 - a33 * b01) * det;
|
|
out[7] = (a20 * b05 - a22 * b02 + a23 * b01) * det;
|
|
out[8] = (a10 * b10 - a11 * b08 + a13 * b06) * det;
|
|
out[9] = (a01 * b08 - a00 * b10 - a03 * b06) * det;
|
|
out[10] = (a30 * b04 - a31 * b02 + a33 * b00) * det;
|
|
out[11] = (a21 * b02 - a20 * b04 - a23 * b00) * det;
|
|
out[12] = (a11 * b07 - a10 * b09 - a12 * b06) * det;
|
|
out[13] = (a00 * b09 - a01 * b07 + a02 * b06) * det;
|
|
out[14] = (a31 * b01 - a30 * b03 - a32 * b00) * det;
|
|
out[15] = (a20 * b03 - a21 * b01 + a22 * b00) * det;
|
|
|
|
return false;
|
|
}
|
|
|
|
void ortho(mat4& out, double left, double right, double bottom, double top, double near, double far) {
|
|
double lr = 1.0f / (left - right);
|
|
double bt = 1.0f / (bottom - top);
|
|
double nf = 1.0f / (near - far);
|
|
out[0] = -2.0f * lr;
|
|
out[1] = 0.0f;
|
|
out[2] = 0.0f;
|
|
out[3] = 0.0f;
|
|
out[4] = 0.0f;
|
|
out[5] = -2.0f * bt;
|
|
out[6] = 0.0f;
|
|
out[7] = 0.0f;
|
|
out[8] = 0.0f;
|
|
out[9] = 0.0f;
|
|
out[10] = 2.0f * nf;
|
|
out[11] = 0.0f;
|
|
out[12] = (left + right) * lr;
|
|
out[13] = (top + bottom) * bt;
|
|
out[14] = (far + near) * nf;
|
|
out[15] = 1.0f;
|
|
}
|
|
|
|
void perspective(mat4& out, double fovy, double aspect, double near, double far) {
|
|
double f = 1.0f / std::tan(fovy / 2.0f);
|
|
double nf = 1.0f / (near - far);
|
|
out[0] = f / aspect;
|
|
out[1] = 0.0f;
|
|
out[2] = 0.0f;
|
|
out[3] = 0.0f;
|
|
out[4] = 0.0f;
|
|
out[5] = f;
|
|
out[6] = 0.0f;
|
|
out[7] = 0.0f;
|
|
out[8] = 0.0f;
|
|
out[9] = 0.0f;
|
|
out[10] = (far + near) * nf;
|
|
out[11] = -1.0f;
|
|
out[12] = 0.0f;
|
|
out[13] = 0.0f;
|
|
out[14] = (2.0f * far * near) * nf;
|
|
out[15] = 0.0f;
|
|
}
|
|
|
|
void copy(mat4& out, const mat4& a) {
|
|
out[0] = a[0];
|
|
out[1] = a[1];
|
|
out[2] = a[2];
|
|
out[3] = a[3];
|
|
out[4] = a[4];
|
|
out[5] = a[5];
|
|
out[6] = a[6];
|
|
out[7] = a[7];
|
|
out[8] = a[8];
|
|
out[9] = a[9];
|
|
out[10] = a[10];
|
|
out[11] = a[11];
|
|
out[12] = a[12];
|
|
out[13] = a[13];
|
|
out[14] = a[14];
|
|
out[15] = a[15];
|
|
}
|
|
|
|
void translate(mat4& out, const mat4& a, double x, double y, double z) {
|
|
if (&a == &out) {
|
|
out[12] = a[0] * x + a[4] * y + a[8] * z + a[12];
|
|
out[13] = a[1] * x + a[5] * y + a[9] * z + a[13];
|
|
out[14] = a[2] * x + a[6] * y + a[10] * z + a[14];
|
|
out[15] = a[3] * x + a[7] * y + a[11] * z + a[15];
|
|
} else {
|
|
double a00;
|
|
double a01;
|
|
double a02;
|
|
double a03;
|
|
double a10;
|
|
double a11;
|
|
double a12;
|
|
double a13;
|
|
double a20;
|
|
double a21;
|
|
double a22;
|
|
double a23;
|
|
|
|
a00 = a[0];
|
|
a01 = a[1];
|
|
a02 = a[2];
|
|
a03 = a[3];
|
|
a10 = a[4];
|
|
a11 = a[5];
|
|
a12 = a[6];
|
|
a13 = a[7];
|
|
a20 = a[8];
|
|
a21 = a[9];
|
|
a22 = a[10];
|
|
a23 = a[11];
|
|
|
|
out[0] = a00;
|
|
out[1] = a01;
|
|
out[2] = a02;
|
|
out[3] = a03;
|
|
out[4] = a10;
|
|
out[5] = a11;
|
|
out[6] = a12;
|
|
out[7] = a13;
|
|
out[8] = a20;
|
|
out[9] = a21;
|
|
out[10] = a22;
|
|
out[11] = a23;
|
|
|
|
out[12] = a00 * x + a10 * y + a20 * z + a[12];
|
|
out[13] = a01 * x + a11 * y + a21 * z + a[13];
|
|
out[14] = a02 * x + a12 * y + a22 * z + a[14];
|
|
out[15] = a03 * x + a13 * y + a23 * z + a[15];
|
|
}
|
|
}
|
|
|
|
void rotate_x(mat4& out, const mat4& a, double rad) {
|
|
double s = std::sin(rad);
|
|
double c = std::cos(rad);
|
|
double a10 = a[4];
|
|
double a11 = a[5];
|
|
double a12 = a[6];
|
|
double a13 = a[7];
|
|
double a20 = a[8];
|
|
double a21 = a[9];
|
|
double a22 = a[10];
|
|
double a23 = a[11];
|
|
|
|
if (&a != &out) { // If the source and destination differ, copy the unchanged rows
|
|
out[0] = a[0];
|
|
out[1] = a[1];
|
|
out[2] = a[2];
|
|
out[3] = a[3];
|
|
out[12] = a[12];
|
|
out[13] = a[13];
|
|
out[14] = a[14];
|
|
out[15] = a[15];
|
|
}
|
|
|
|
// Perform axis-specific matrix multiplication
|
|
out[4] = a10 * c + a20 * s;
|
|
out[5] = a11 * c + a21 * s;
|
|
out[6] = a12 * c + a22 * s;
|
|
out[7] = a13 * c + a23 * s;
|
|
out[8] = a20 * c - a10 * s;
|
|
out[9] = a21 * c - a11 * s;
|
|
out[10] = a22 * c - a12 * s;
|
|
out[11] = a23 * c - a13 * s;
|
|
}
|
|
|
|
void rotate_y(mat4& out, const mat4& a, double rad) {
|
|
double s = std::sin(rad);
|
|
double c = std::cos(rad);
|
|
double a00 = a[0];
|
|
double a01 = a[1];
|
|
double a02 = a[2];
|
|
double a03 = a[3];
|
|
double a20 = a[8];
|
|
double a21 = a[9];
|
|
double a22 = a[10];
|
|
double a23 = a[11];
|
|
|
|
if (&a != &out) { // If the source and destination differ, copy the unchanged rows
|
|
out[4] = a[4];
|
|
out[5] = a[5];
|
|
out[6] = a[6];
|
|
out[7] = a[7];
|
|
out[12] = a[12];
|
|
out[13] = a[13];
|
|
out[14] = a[14];
|
|
out[15] = a[15];
|
|
}
|
|
|
|
// Perform axis-specific matrix multiplication
|
|
out[0] = a00 * c - a20 * s;
|
|
out[1] = a01 * c - a21 * s;
|
|
out[2] = a02 * c - a22 * s;
|
|
out[3] = a03 * c - a23 * s;
|
|
out[8] = a00 * s + a20 * c;
|
|
out[9] = a01 * s + a21 * c;
|
|
out[10] = a02 * s + a22 * c;
|
|
out[11] = a03 * s + a23 * c;
|
|
}
|
|
|
|
void rotate_z(mat4& out, const mat4& a, double rad) {
|
|
double s = std::sin(rad);
|
|
double c = std::cos(rad);
|
|
double a00 = a[0];
|
|
double a01 = a[1];
|
|
double a02 = a[2];
|
|
double a03 = a[3];
|
|
double a10 = a[4];
|
|
double a11 = a[5];
|
|
double a12 = a[6];
|
|
double a13 = a[7];
|
|
|
|
if (&a != &out) { // If the source and destination differ, copy the unchanged last row
|
|
out[8] = a[8];
|
|
out[9] = a[9];
|
|
out[10] = a[10];
|
|
out[11] = a[11];
|
|
out[12] = a[12];
|
|
out[13] = a[13];
|
|
out[14] = a[14];
|
|
out[15] = a[15];
|
|
}
|
|
|
|
// Perform axis-specific matrix multiplication
|
|
out[0] = a00 * c + a10 * s;
|
|
out[1] = a01 * c + a11 * s;
|
|
out[2] = a02 * c + a12 * s;
|
|
out[3] = a03 * c + a13 * s;
|
|
out[4] = a10 * c - a00 * s;
|
|
out[5] = a11 * c - a01 * s;
|
|
out[6] = a12 * c - a02 * s;
|
|
out[7] = a13 * c - a03 * s;
|
|
}
|
|
|
|
void scale(mat4& out, const mat4& a, double x, double y, double z) {
|
|
out[0] = a[0] * x;
|
|
out[1] = a[1] * x;
|
|
out[2] = a[2] * x;
|
|
out[3] = a[3] * x;
|
|
out[4] = a[4] * y;
|
|
out[5] = a[5] * y;
|
|
out[6] = a[6] * y;
|
|
out[7] = a[7] * y;
|
|
out[8] = a[8] * z;
|
|
out[9] = a[9] * z;
|
|
out[10] = a[10] * z;
|
|
out[11] = a[11] * z;
|
|
out[12] = a[12];
|
|
out[13] = a[13];
|
|
out[14] = a[14];
|
|
out[15] = a[15];
|
|
}
|
|
|
|
void multiply(mat4& out, const mat4& a, const mat4& b) {
|
|
double a00 = a[0];
|
|
double a01 = a[1];
|
|
double a02 = a[2];
|
|
double a03 = a[3];
|
|
double a10 = a[4];
|
|
double a11 = a[5];
|
|
double a12 = a[6];
|
|
double a13 = a[7];
|
|
double a20 = a[8];
|
|
double a21 = a[9];
|
|
double a22 = a[10];
|
|
double a23 = a[11];
|
|
double a30 = a[12];
|
|
double a31 = a[13];
|
|
double a32 = a[14];
|
|
double a33 = a[15];
|
|
|
|
// Cache only the current line of the second matrix
|
|
double b0 = b[0];
|
|
double b1 = b[1];
|
|
double b2 = b[2];
|
|
double b3 = b[3];
|
|
out[0] = b0 * a00 + b1 * a10 + b2 * a20 + b3 * a30;
|
|
out[1] = b0 * a01 + b1 * a11 + b2 * a21 + b3 * a31;
|
|
out[2] = b0 * a02 + b1 * a12 + b2 * a22 + b3 * a32;
|
|
out[3] = b0 * a03 + b1 * a13 + b2 * a23 + b3 * a33;
|
|
|
|
b0 = b[4];
|
|
b1 = b[5];
|
|
b2 = b[6];
|
|
b3 = b[7];
|
|
out[4] = b0 * a00 + b1 * a10 + b2 * a20 + b3 * a30;
|
|
out[5] = b0 * a01 + b1 * a11 + b2 * a21 + b3 * a31;
|
|
out[6] = b0 * a02 + b1 * a12 + b2 * a22 + b3 * a32;
|
|
out[7] = b0 * a03 + b1 * a13 + b2 * a23 + b3 * a33;
|
|
|
|
b0 = b[8];
|
|
b1 = b[9];
|
|
b2 = b[10];
|
|
b3 = b[11];
|
|
out[8] = b0 * a00 + b1 * a10 + b2 * a20 + b3 * a30;
|
|
out[9] = b0 * a01 + b1 * a11 + b2 * a21 + b3 * a31;
|
|
out[10] = b0 * a02 + b1 * a12 + b2 * a22 + b3 * a32;
|
|
out[11] = b0 * a03 + b1 * a13 + b2 * a23 + b3 * a33;
|
|
|
|
b0 = b[12];
|
|
b1 = b[13];
|
|
b2 = b[14];
|
|
b3 = b[15];
|
|
out[12] = b0 * a00 + b1 * a10 + b2 * a20 + b3 * a30;
|
|
out[13] = b0 * a01 + b1 * a11 + b2 * a21 + b3 * a31;
|
|
out[14] = b0 * a02 + b1 * a12 + b2 * a22 + b3 * a32;
|
|
out[15] = b0 * a03 + b1 * a13 + b2 * a23 + b3 * a33;
|
|
}
|
|
|
|
void transformMat4(vec4& out, const vec4& a, const mat4& m) {
|
|
double x = a[0];
|
|
double y = a[1];
|
|
double z = a[2];
|
|
double w = a[3];
|
|
out[0] = m[0] * x + m[4] * y + m[8] * z + m[12] * w;
|
|
out[1] = m[1] * x + m[5] * y + m[9] * z + m[13] * w;
|
|
out[2] = m[2] * x + m[6] * y + m[10] * z + m[14] * w;
|
|
out[3] = m[3] * x + m[7] * y + m[11] * z + m[15] * w;
|
|
}
|
|
|
|
} // namespace matrix
|
|
|
|
} // namespace mbgl
|